public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

change ba loop

Browse Source
This commit is contained in:
gfuse
2021-10-16 10:34:03 +02:00
parent cd209204bf
commit c69cb17521
6 changed files with 65 additions and 355 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
View File
GPU_version/vio/frame_handler_log.txt
-298
View File
@@ -1,298 +0,0 @@
[17:50:31.411] Init: frame zero: 0.000000, 0.000000 0.000000
[17:50:31.412] Init: Selected first frame.
[17:50:31.412] frame process finished the id is: 0 the obs is:0
[17:50:31.433] Init: px average disparity is:0.035702 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:31.433] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:31.433] frame process finished the id is: 1 the obs is:0
[17:50:31.536] Init: px average disparity is:0.035710 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:31.536] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:31.536] frame process finished the id is: 2 the obs is:0
[17:50:31.633] Init: px average disparity is:0.027114 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:31.633] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:31.633] frame process finished the id is: 3 the obs is:0
[17:50:31.731] Init: px average disparity is:0.028933 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:31.731] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:31.731] frame process finished the id is: 4 the obs is:0
[17:50:31.830] Init: px average disparity is:0.032903 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:31.830] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:31.830] frame process finished the id is: 5 the obs is:0
[17:50:31.921] Init: px average disparity is:0.029436 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:31.921] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:31.921] frame process finished the id is: 6 the obs is:0
[17:50:32.017] Init: px average disparity is:0.030897 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:32.017] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:32.017] frame process finished the id is: 7 the obs is:0
[17:50:32.115] Init: px average disparity is:0.029863 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:32.115] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:32.115] frame process finished the id is: 8 the obs is:0
[17:50:32.214] Init: px average disparity is:0.033033 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:32.214] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:32.214] frame process finished the id is: 9 the obs is:0
[17:50:32.318] Init: px average disparity is:0.032262 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:32.318] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:32.318] frame process finished the id is: 10 the obs is:0
[17:50:32.423] Init: px average disparity is:0.028435 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:32.423] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:32.423] frame process finished the id is: 11 the obs is:0
[17:50:32.521] Init: px average disparity is:0.038753 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:32.521] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:32.521] frame process finished the id is: 12 the obs is:0
[17:50:32.583] Odometry x=0.000000, y=0.000000, theta=-0.000000
[17:50:32.644] Init: px average disparity is:0.033534 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:32.644] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:32.644] frame process finished the id is: 13 the obs is:0
[17:50:32.735] Init: px average disparity is:0.028411 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:32.735] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:32.735] frame process finished the id is: 14 the obs is:0
[17:50:32.810] Odometry x=0.000000, y=0.019168, theta=0.000000
[17:50:32.842] Init: px average disparity is:0.146053 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:32.842] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:32.842] frame process finished the id is: 15 the obs is:0
[17:50:32.918] Init: px average disparity is:6.453716 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:32.918] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:32.918] frame process finished the id is: 16 the obs is:0
[17:50:33.020] Init: px average disparity is:4.606047 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:33.020] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:33.020] frame process finished the id is: 17 the obs is:0
[17:50:33.074] Odometry x=0.000245, y=0.051530, theta=0.002321
[17:50:33.123] Init: px average disparity is:4.914490 ,While minimum is: 10.000000 KLT tracked : 895
[17:50:33.123] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:33.123] frame process finished the id is: 18 the obs is:0
[17:50:33.223] Init: px average disparity is:9.779386 ,While minimum is: 10.000000 KLT tracked : 894
[17:50:33.223] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:33.223] frame process finished the id is: 19 the obs is:0
[17:50:33.352] Odometry x=0.000199, y=0.082338, theta=-0.000122
[17:50:33.458] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required. px average disparity is:12.724287 ,While minimum is: 10.000000 KLT tracked: 894
[17:50:33.458] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:33.458] frame process finished the id is: 20 the obs is:0
[17:50:33.559] Odometry x=0.000727, y=0.112621, theta=0.001152
[17:50:33.587] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required. px average disparity is:15.784901 ,While minimum is: 10.000000 KLT tracked: 886
[17:50:33.587] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:33.587] frame process finished the id is: 21 the obs is:0
[17:50:33.715] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required. px average disparity is:19.932699 ,While minimum is: 10.000000 KLT tracked: 886
[17:50:33.715] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:33.715] frame process finished the id is: 22 the obs is:0
[17:50:33.844] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required. px average disparity is:27.322473 ,While minimum is: 10.000000 KLT tracked: 886
[17:50:33.844] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:33.844] frame process finished the id is: 23 the obs is:0
[17:50:33.864] Odometry x=0.000558, y=0.141449, theta=0.010821
[17:50:33.973] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required. px average disparity is:28.563517 ,While minimum is: 10.000000 KLT tracked: 885
[17:50:33.973] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:33.973] frame process finished the id is: 24 the obs is:0
[17:50:34.091] Odometry x=0.000199, y=0.170487, theta=0.015299
[17:50:34.093] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required. px average disparity is:34.025067 ,While minimum is: 10.000000 KLT tracked: 885
[17:50:34.094] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:34.094] frame process finished the id is: 25 the obs is:0
[17:50:34.214] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required. px average disparity is:41.010976 ,While minimum is: 10.000000 KLT tracked: 881
[17:50:34.214] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:34.214] frame process finished the id is: 26 the obs is:0
[17:50:34.328] Odometry x=-0.000118, y=0.199585, theta=0.015206
[17:50:34.340] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:47.159182 ,While minimum is: 10.000000 KLT tracked: 843
[17:50:34.340] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:34.340] frame process finished the id is: 27 the obs is:0
[17:50:34.462] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:42.626608 ,While minimum is: 10.000000 KLT tracked: 782
[17:50:34.462] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:34.462] frame process finished the id is: 28 the obs is:0
[17:50:34.577] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:45.790779 ,While minimum is: 10.000000 KLT tracked: 752
[17:50:34.577] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:34.577] frame process finished the id is: 29 the obs is:0
[17:50:34.609] Odometry x=-0.000599, y=0.228582, theta=0.016890
[17:50:34.741] Init: Homography RANSAC (inlier) is:1 ,While 40 inliers minimum required. px average disparity is:43.837220 ,While minimum is: 10.000000 KLT tracked: 747
[17:50:34.741] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:34.741] frame process finished the id is: 30 the obs is:0
[17:50:34.813] Odometry x=-0.000827, y=0.256863, theta=0.016911
[17:50:34.852] Init: Homography RANSAC (inlier) is:1 ,While 40 inliers minimum required. px average disparity is:53.269346 ,While minimum is: 10.000000 KLT tracked: 738
[17:50:34.852] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:34.852] frame process finished the id is: 31 the obs is:0
[17:50:34.976] Init: Homography RANSAC (inlier) is:1 ,While 40 inliers minimum required. px average disparity is:49.698939 ,While minimum is: 10.000000 KLT tracked: 729
[17:50:34.976] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:34.976] frame process finished the id is: 32 the obs is:0
[17:50:35.047] Odometry x=-0.001150, y=0.284645, theta=0.003733
[17:50:35.120] Init: Homography RANSAC (inlier) is:2 ,While 40 inliers minimum required. px average disparity is:43.891760 ,While minimum is: 10.000000 KLT tracked: 729
[17:50:35.120] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:35.120] frame process finished the id is: 33 the obs is:0
[17:50:35.301] Odometry x=-0.000790, y=0.310881, theta=0.007198
[17:50:35.333] Init: Homography RANSAC (inlier) is:2 ,While 40 inliers minimum required. px average disparity is:83.937581 ,While minimum is: 10.000000 KLT tracked: 716
[17:50:35.333] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:35.333] frame process finished the id is: 34 the obs is:0
[17:50:35.456] Init: Homography RANSAC (inlier) is:2 ,While 40 inliers minimum required. px average disparity is:41.798021 ,While minimum is: 10.000000 KLT tracked: 700
[17:50:35.456] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:35.456] frame process finished the id is: 35 the obs is:0
[17:50:35.556] Odometry x=-0.001214, y=0.337069, theta=0.033919
[17:50:35.564] Init: Homography RANSAC (inlier) is:2 ,While 40 inliers minimum required. px average disparity is:41.888346 ,While minimum is: 10.000000 KLT tracked: 695
[17:50:35.564] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:35.564] frame process finished the id is: 36 the obs is:0
[17:50:35.675] Init: Homography RANSAC (inlier) is:1 ,While 40 inliers minimum required. px average disparity is:49.870518 ,While minimum is: 10.000000 KLT tracked: 695
[17:50:35.675] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:35.675] frame process finished the id is: 37 the obs is:0
[17:50:35.783] Init: Homography RANSAC (inlier) is:2 ,While 40 inliers minimum required. px average disparity is:50.665883 ,While minimum is: 10.000000 KLT tracked: 695
[17:50:35.783] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:35.783] frame process finished the id is: 38 the obs is:0
[17:50:35.890] Init: Homography RANSAC (inlier) is:2 ,While 40 inliers minimum required. px average disparity is:56.009125 ,While minimum is: 10.000000 KLT tracked: 695
[17:50:35.890] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:35.890] frame process finished the id is: 39 the obs is:0
[17:50:35.893] Odometry x=-0.002172, y=0.361497, theta=0.047289
[17:50:35.997] Init: Homography RANSAC (inlier) is:2 ,While 40 inliers minimum required. px average disparity is:55.714194 ,While minimum is: 10.000000 KLT tracked: 694
[17:50:35.997] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:35.997] frame process finished the id is: 40 the obs is:0
[17:50:36.059] Odometry x=-0.003696, y=0.385156, theta=0.063176
[17:50:36.105] Init: Homography RANSAC (inlier) is:2 ,While 40 inliers minimum required. px average disparity is:54.680002 ,While minimum is: 10.000000 KLT tracked: 690
[17:50:36.105] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:36.105] frame process finished the id is: 41 the obs is:0
[17:50:36.211] Init: Homography RANSAC (inlier) is:7 ,While 40 inliers minimum required. px average disparity is:59.079536 ,While minimum is: 10.000000 KLT tracked: 685
[17:50:36.211] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:36.211] frame process finished the id is: 42 the obs is:0
[17:50:36.308] Odometry x=-0.005270, y=0.410339, theta=0.062916
[17:50:36.316] Init: Homography RANSAC (inlier) is:10 ,While 40 inliers minimum required. px average disparity is:63.253156 ,While minimum is: 10.000000 KLT tracked: 683
[17:50:36.316] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:36.316] frame process finished the id is: 43 the obs is:0
[17:50:36.420] Init: Homography RANSAC (inlier) is:10 ,While 40 inliers minimum required. px average disparity is:64.019856 ,While minimum is: 10.000000 KLT tracked: 681
[17:50:36.420] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:36.420] frame process finished the id is: 44 the obs is:0
[17:50:36.525] Init: Homography RANSAC (inlier) is:11 ,While 40 inliers minimum required. px average disparity is:61.805289 ,While minimum is: 10.000000 KLT tracked: 680
[17:50:36.525] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:36.525] frame process finished the id is: 45 the obs is:0
[17:50:36.550] Odometry x=-0.008561, y=0.437785, theta=0.059562
[17:50:36.632] Init: Homography RANSAC (inlier) is:11 ,While 40 inliers minimum required. px average disparity is:62.020099 ,While minimum is: 10.000000 KLT tracked: 680
[17:50:36.632] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:36.632] frame process finished the id is: 46 the obs is:0
[17:50:36.743] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:64.020284 ,While minimum is: 10.000000 KLT tracked: 679
[17:50:36.743] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:36.743] frame process finished the id is: 47 the obs is:0
[17:50:36.827] Odometry x=-0.007797, y=0.462194, theta=0.054948
[17:50:36.852] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:64.197038 ,While minimum is: 10.000000 KLT tracked: 677
[17:50:36.852] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:36.852] frame process finished the id is: 48 the obs is:0
[17:50:36.958] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:61.479662 ,While minimum is: 10.000000 KLT tracked: 574
[17:50:36.958] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:36.958] frame process finished the id is: 49 the obs is:0
[17:50:37.059] Odometry x=-0.008775, y=0.478765, theta=0.068109
[17:50:37.059] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:62.428546 ,While minimum is: 10.000000 KLT tracked: 550
[17:50:37.059] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:37.059] frame process finished the id is: 50 the obs is:0
[17:50:37.160] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:66.159833 ,While minimum is: 10.000000 KLT tracked: 539
[17:50:37.160] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:37.160] frame process finished the id is: 51 the obs is:0
[17:50:37.264] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:66.867114 ,While minimum is: 10.000000 KLT tracked: 539
[17:50:37.264] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:37.264] frame process finished the id is: 52 the obs is:0
[17:50:37.314] Odometry x=-0.007862, y=0.494711, theta=0.061638
[17:50:37.364] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:61.918039 ,While minimum is: 10.000000 KLT tracked: 539
[17:50:37.364] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:37.364] frame process finished the id is: 53 the obs is:0
[17:50:37.464] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:59.145431 ,While minimum is: 10.000000 KLT tracked: 539
[17:50:37.464] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:37.464] frame process finished the id is: 54 the obs is:0
[17:50:37.565] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:57.093263 ,While minimum is: 10.000000 KLT tracked: 539
[17:50:37.565] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:37.565] frame process finished the id is: 55 the obs is:0
[17:50:37.567] Odometry x=-0.008503, y=0.512210, theta=0.057686
[17:50:37.667] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:56.741348 ,While minimum is: 10.000000 KLT tracked: 539
[17:50:37.667] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:37.667] frame process finished the id is: 56 the obs is:0
[17:50:37.771] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:57.555356 ,While minimum is: 10.000000 KLT tracked: 539
[17:50:37.771] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:37.771] frame process finished the id is: 57 the obs is:0
[17:50:37.870] Odometry x=-0.009642, y=0.532168, theta=0.057057
[17:50:37.872] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:58.978449 ,While minimum is: 10.000000 KLT tracked: 539
[17:50:37.872] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:37.872] frame process finished the id is: 58 the obs is:0
[17:50:37.974] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:60.911267 ,While minimum is: 10.000000 KLT tracked: 539
[17:50:37.974] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:37.974] frame process finished the id is: 59 the obs is:0
[17:50:38.044] Odometry x=-0.010007, y=0.554133, theta=0.065516
[17:50:38.075] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:66.148182 ,While minimum is: 10.000000 KLT tracked: 535
[17:50:38.075] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:38.075] frame process finished the id is: 60 the obs is:0
[17:50:38.177] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:66.284359 ,While minimum is: 10.000000 KLT tracked: 535
[17:50:38.177] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:38.177] frame process finished the id is: 61 the obs is:0
[17:50:38.281] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:59.361306 ,While minimum is: 10.000000 KLT tracked: 535
[17:50:38.281] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:38.281] frame process finished the id is: 62 the obs is:0
[17:50:38.331] Odometry x=-0.011163, y=0.573468, theta=0.049597
[17:50:38.382] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:57.549421 ,While minimum is: 10.000000 KLT tracked: 535
[17:50:38.382] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:38.382] frame process finished the id is: 63 the obs is:0
[17:50:38.484] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:55.672159 ,While minimum is: 10.000000 KLT tracked: 534
[17:50:38.484] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:38.484] frame process finished the id is: 64 the obs is:0
[17:50:38.543] Odometry x=-0.012779, y=0.592351, theta=0.053998
[17:50:38.586] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:56.877505 ,While minimum is: 10.000000 KLT tracked: 532
[17:50:38.586] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:38.586] frame process finished the id is: 65 the obs is:0
[17:50:38.691] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:59.757591 ,While minimum is: 10.000000 KLT tracked: 532
[17:50:38.691] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:38.691] frame process finished the id is: 66 the obs is:0
[17:50:38.793] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:60.977541 ,While minimum is: 10.000000 KLT tracked: 532
[17:50:38.793] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:38.793] frame process finished the id is: 67 the obs is:0
[17:50:38.801] Odometry x=-0.013783, y=0.610537, theta=0.056385
[17:50:38.895] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:62.845157 ,While minimum is: 10.000000 KLT tracked: 532
[17:50:38.895] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:38.895] frame process finished the id is: 68 the obs is:0
[17:50:38.995] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:66.353970 ,While minimum is: 10.000000 KLT tracked: 530
[17:50:38.995] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:38.995] frame process finished the id is: 69 the obs is:0
[17:50:39.065] Odometry x=-0.014543, y=0.628569, theta=0.063463
[17:50:39.096] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:68.514352 ,While minimum is: 10.000000 KLT tracked: 530
[17:50:39.096] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:39.096] frame process finished the id is: 70 the obs is:0
[17:50:39.202] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:66.615560 ,While minimum is: 10.000000 KLT tracked: 530
[17:50:39.202] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:39.202] frame process finished the id is: 71 the obs is:0
[17:50:39.296] Odometry x=-0.015858, y=0.646928, theta=0.083319
[17:50:39.303] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:67.226885 ,While minimum is: 10.000000 KLT tracked: 529
[17:50:39.303] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:39.303] frame process finished the id is: 72 the obs is:0
[17:50:39.402] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:68.437670 ,While minimum is: 10.000000 KLT tracked: 528
[17:50:39.402] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:39.402] frame process finished the id is: 73 the obs is:0
[17:50:39.504] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:69.508952 ,While minimum is: 10.000000 KLT tracked: 528
[17:50:39.504] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:39.504] frame process finished the id is: 74 the obs is:0
[17:50:39.547] Odometry x=-0.017335, y=0.665474, theta=0.086044
[17:50:39.604] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:68.999405 ,While minimum is: 10.000000 KLT tracked: 528
[17:50:39.604] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:39.604] frame process finished the id is: 75 the obs is:0
[17:50:39.712] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:74.253572 ,While minimum is: 10.000000 KLT tracked: 528
[17:50:39.712] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:39.712] frame process finished the id is: 76 the obs is:0
[17:50:39.812] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:71.703765 ,While minimum is: 10.000000 KLT tracked: 528
[17:50:39.812] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:39.812] frame process finished the id is: 77 the obs is:0
[17:50:39.878] Odometry x=-0.018968, y=0.684542, theta=0.090161
[17:50:39.911] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:72.506681 ,While minimum is: 10.000000 KLT tracked: 528
[17:50:39.912] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:39.912] frame process finished the id is: 78 the obs is:0
[17:50:40.011] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:69.933815 ,While minimum is: 10.000000 KLT tracked: 528
[17:50:40.011] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:40.011] frame process finished the id is: 79 the obs is:0
[17:50:40.042] Odometry x=-0.020779, y=0.703455, theta=0.103384
[17:50:40.111] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:73.374817 ,While minimum is: 10.000000 KLT tracked: 527
[17:50:40.111] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:40.111] frame process finished the id is: 80 the obs is:0
[17:50:40.218] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:77.413818 ,While minimum is: 10.000000 KLT tracked: 526
[17:50:40.218] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:40.218] frame process finished the id is: 81 the obs is:0
[17:50:40.318] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:78.690793 ,While minimum is: 10.000000 KLT tracked: 521
[17:50:40.318] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:40.318] frame process finished the id is: 82 the obs is:0
[17:50:40.347] Odometry x=-0.023292, y=0.721982, theta=0.111924
[17:50:40.417] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:87.487700 ,While minimum is: 10.000000 KLT tracked: 508
[17:50:40.417] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:40.417] frame process finished the id is: 83 the obs is:0
[17:50:40.516] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:89.148491 ,While minimum is: 10.000000 KLT tracked: 504
[17:50:40.516] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:40.516] frame process finished the id is: 84 the obs is:0
[17:50:40.547] Odometry x=-0.025504, y=0.740643, theta=0.105352
[17:50:40.618] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:91.776485 ,While minimum is: 10.000000 KLT tracked: 502
[17:50:40.618] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:40.618] frame process finished the id is: 85 the obs is:0
[17:50:40.718] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:86.958165 ,While minimum is: 10.000000 KLT tracked: 495
[17:50:40.718] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:40.719] frame process finished the id is: 86 the obs is:0
[17:50:40.799] Odometry x=-0.027781, y=0.760877, theta=0.100422
[17:50:40.817] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:85.094985 ,While minimum is: 10.000000 KLT tracked: 495
[17:50:40.817] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:50:40.817] frame process finished the id is: 87 th
GPU_version/vio/include/vio/global_optimizer.h
+3 -6
View File
@@ -76,6 +76,7 @@ protected:
boost::mutex mtx_; boost::mutex mtx_;
boost::thread* thread_; boost::thread* thread_;
Map& map_; Map& map_;
size_t v_id_ = 0;
#if VIO_DEBUG #if VIO_DEBUG
FILE* log_=nullptr; FILE* log_=nullptr;
@@ -89,14 +90,10 @@ protected:
/// Create a g2o vertice from a keyframe object. /// Create a g2o vertice from a keyframe object.
g2o::VertexSE3Expmap* createG2oFrameSE3( g2o::VertexSE3Expmap* createG2oFrameSE3(
FramePtr kf, FramePtr kf);
size_t id,
bool fixed);
/// Creates a g2o vertice from a mappoint object. /// Creates a g2o vertice from a mappoint object.
g2o::VertexSBAPointXYZ* createG2oPoint( g2o::VertexSBAPointXYZ* createG2oPoint(
Vector3d pos, Vector3d pos);
size_t id,
bool fixed);
/// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement. /// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement.
g2o::EdgeProjectXYZ2UV* createG2oEdgeSE3( g2o::EdgeProjectXYZ2UV* createG2oEdgeSE3(
g2o::VertexSE3Expmap* v_kf, g2o::VertexSE3Expmap* v_kf,
GPU_version/vio/param/vo_fast.yaml
+1 -1
View File
@@ -8,7 +8,7 @@ vio:
init_min_disparity: 10 init_min_disparity: 10
structureoptim_max_pts: 50 structureoptim_max_pts: 50
kfselect_mindist: 0.01 kfselect_mindist: 0.01
poseoptim_thresh: 1.25 poseoptim_thresh: 1.3
ACC_ekf: 0.2 #acc white noise in continuous in EKF ACC_ekf: 0.2 #acc white noise in continuous in EKF
GYO_ekf: 0.1 #gyro white noise in continuous in EKF GYO_ekf: 0.1 #gyro white noise in continuous in EKF
VO_ekf_translation: 1.0 #VO noise in EKF VO_ekf_translation: 1.0 #VO noise in EKF
GPU_version/vio/src/global_optimizer.cpp
+60 -49
View File
@@ -78,6 +78,32 @@ namespace vio {
vio::time_in_HH_MM_SS_MMM().c_str()); vio::time_in_HH_MM_SS_MMM().c_str());
#endif #endif
new_keyframe_=false; new_keyframe_=false;
ba_mux_.lock();
// Go through all Keyframes
v_id_ = 0;
for(auto it_kf=map_.keyframes_.begin();it_kf!=map_.keyframes_.end();++it_kf)
{
// New Keyframe Vertex
(*it_kf)->v_kf_ = createG2oFrameSE3(*it_kf);
for(auto&& it_ftr:(*it_kf)->fts_)
{
if(it_ftr->point==NULL)continue;
if(it_ftr->point->type_ != vio::Point::TYPE_GOOD)continue;
// for each keyframe add edges to all observed mapoints
if(it_ftr->point->pos_.hasNaN())continue;
if(it_ftr->point->pos_.norm()==0.)continue;
if(it_ftr->point->v_pt_.first == NULL)
{
// mappoint-vertex doesn't exist yet. create a new one:
it_ftr->point->v_pt_.first = createG2oPoint(it_ftr->point->pos_);
}
it_ftr->point->v_pt_.second.push_back(createG2oEdgeSE3((*it_kf)->v_kf_, it_ftr->point->v_pt_.first, vk::project2d(it_ftr->f),
true,
Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth()));
}
}
ba_mux_.unlock();
g2o::SparseOptimizer optimizer; g2o::SparseOptimizer optimizer;
optimizer.setVerbose(false); optimizer.setVerbose(false);
@@ -97,44 +123,35 @@ namespace vio {
} }
// init g2o // init g2o
list< pair<FramePtr,std::shared_ptr<Feature>> > incorrect_edges; list< pair<FramePtr,std::shared_ptr<Feature>> > incorrect_edges;
ba_mux_.lock();
g2o::OptimizableGraph::VertexContainer points; g2o::OptimizableGraph::VertexContainer points;
// Go through all Keyframes auto end=map_.keyframes_.end();
size_t v_id = 0; auto end_1=end--;
for(auto&& it_kf : map_.keyframes_) for(auto it=map_.keyframes_.begin(); it!=map_.keyframes_.end();++it){
{ optimizer.addVertex((*it)->v_kf_);
// New Keyframe Vertex for(auto&& p:(*it)->fts_){
it_kf->v_kf_ = createG2oFrameSE3(it_kf, v_id++, false); if(p->point == NULL)continue;
for(auto&& it_ftr:it_kf->fts_) if(p->point->v_pt_.first == NULL)continue;
{ if(!p->point->v_pt_.second.empty()){
if(it_ftr->point==NULL)continue; if(p->point->v_pt_.second.size()>1){
if(it_ftr->point->type_ != vio::Point::TYPE_GOOD)continue; optimizer.addVertex(p->point->v_pt_.first);///TODO
// for each keyframe add edges to all observed mapoints if(p->point->v_pt_.first->dimension() == 3)points.push_back(p->point->v_pt_.first);
if(it_ftr->point->pos_.hasNaN())continue; for(auto&& e:p->point->v_pt_.second)optimizer.addEdge(e);
if(it_ftr->point->pos_.norm()==0.)continue; p->point->v_pt_.second.clear();
if(it_ftr->point->v_pt_.first == NULL) }
{
// mappoint-vertex doesn't exist yet. create a new one:
it_ftr->point->v_pt_.first = createG2oPoint(it_ftr->point->pos_, v_id++, true);
} }
it_ftr->point->v_pt_.second.push_back(createG2oEdgeSE3(it_kf->v_kf_, it_ftr->point->v_pt_.first, vk::project2d(it_ftr->f),
true,
Config::poseOptimThresh()/it_kf->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth()));
} }
} if(!(*it)->v_kf_->edges().empty()){
for(auto&& it:map_.keyframes_){ if((*it)->v_kf_->edges().size()>1 && (*it)->v_kf_->dimension()==3)points.push_back((*it)->v_kf_);
optimizer.addVertex(it->v_kf_); if(it == end_1-- || it ==end || it==map_.keyframes_.end()) (*it)->v_kf_->setFixed(false);
for(auto&& p:it->fts_)if(p->point->v_pt_.second.size()>1){ }else{
optimizer.addVertex(p->point->v_pt_.first); optimizer.removeVertex((*it)->v_kf_);
for(auto&& e:p->point->v_pt_.second)optimizer.addEdge(e);
} }
if(it->v_kf_->edges().size()>1)points.push_back(it->v_kf_);
}
if(optimizer.vertices().empty() || optimizer.vertices().size()<1){
ba_mux_.unlock();
continue;
} }
// Optimization // Optimization
if(points.empty()){
optimizer.clear();
continue;
}
optimizer.initializeOptimization(); optimizer.initializeOptimization();
optimizer.computeActiveErrors(); optimizer.computeActiveErrors();
g2o::StructureOnlySolver<3> structure_only_ba; g2o::StructureOnlySolver<3> structure_only_ba;
@@ -144,18 +161,19 @@ namespace vio {
vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2()); vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
#endif #endif
if(optimizer.optimize(vio::Config::lobaNumIter())<1){ if(optimizer.optimize(vio::Config::lobaNumIter())<1){
ba_mux_.unlock(); optimizer.clear();
continue; continue;
} }
#if VIO_DEBUG #if VIO_DEBUG
fprintf(log_,"[%s] end error: %f \n", fprintf(log_,"[%s] end error: %f \n",
vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2()); vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
#endif #endif
ba_mux_.lock();
// Update Keyframe and MapPoint Positions // Update Keyframe and MapPoint Positions
for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin(); for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
it_kf != map_.keyframes_.end();++it_kf) it_kf != map_.keyframes_.end();++it_kf)
{ {
(*it_kf)->T_f_w_ = SE2_5(SE3((*it_kf)->v_kf_->estimate().rotation(), (*it_kf)->T_f_w_ = SE2_5(SE3((*it_kf)->v_kf_->estimate().rotation().toRotationMatrix(),
(*it_kf)->v_kf_->estimate().translation())); (*it_kf)->v_kf_->estimate().translation()));
(*it_kf)->v_kf_ = NULL; (*it_kf)->v_kf_ = NULL;
for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr) for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
@@ -170,34 +188,27 @@ namespace vio {
} }
} }
ba_mux_.unlock(); ba_mux_.unlock();
optimizer.clear();
} }
} }
g2o::VertexSE3Expmap* g2o::VertexSE3Expmap*
BA_Glob::createG2oFrameSE3(FramePtr frame, size_t id, bool fixed) BA_Glob::createG2oFrameSE3(FramePtr frame)
{ {
/* if(frame->id_==0 || frame->id_==1){
g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap();
v->setId(id);
v->setToOrigin();
v->setFixed(true);
v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation()));
}*/
/*std::cerr<<frame->se3().unit_quaternion().matrix()<<'\n'<<frame->se3().translation()<<'\n';*/
g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap(); g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap();
v->setId(id); ++v_id_;
v->setFixed(false); v->setId(v_id_);
v->setFixed(true);
v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation())); v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation()));
return v; return v;
} }
g2o::VertexSBAPointXYZ* g2o::VertexSBAPointXYZ*
BA_Glob::createG2oPoint(Vector3d pos, BA_Glob::createG2oPoint(Vector3d pos)
size_t id,
bool fixed)
{ {
++v_id_;
g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ(); g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ();
v->setId(id); v->setId(v_id_);
v->setFixed(false); v->setFixed(false);
v->setMarginalized(true); v->setMarginalized(true);
v->setEstimate(pos); v->setEstimate(pos);
GPU_version/vio/src/initialization.cpp
+1 -1
View File
@@ -75,7 +75,7 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
} }
computeHomography(frame_cur, computeHomography(frame_cur,
features_ref_, px_cur_, features_ref_, px_cur_,
frame_ref_->cam_->errorMultiplier2(), 5.0, frame_ref_->cam_->errorMultiplier2(), 6.0,
inliers_, xyz_in_cur_, T_cur_from_ref_); inliers_, xyz_in_cur_, T_cur_from_ref_);
if(inliers_.size() < Config::initMinInliers()){ if(inliers_.size() < Config::initMinInliers()){
#if VIO_DEBUG #if VIO_DEBUG