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

correct gpu segfault

Browse Source
This commit is contained in:
gfuse
2021-10-06 11:09:20 +02:00
parent 259e91352a
commit 17ad651f02
7 changed files with 927 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
Executable
+123
View File
@@ -0,0 +1,123 @@
[11:06:58.541] New key frame
[11:06:58.541] BA loop run
[11:06:58.542] init error: 111.169209
[11:06:58.543] end error: 0.522741
[11:06:58.674] New key frame
[11:06:58.674] BA loop run
[11:06:58.674] init error: 1535.405047
[11:06:58.675] end error: 0.000000
[11:06:58.782] New key frame
[11:06:58.782] BA loop run
[11:06:58.783] init error: 2166.366615
[11:06:58.783] end error: 0.000000
[11:06:58.895] New key frame
[11:06:58.895] BA loop run
[11:06:58.895] init error: 1969008.499946
[11:06:58.896] end error: 13.098446
[11:06:59.011] New key frame
[11:06:59.011] BA loop run
[11:06:59.011] init error: 48.044026
[11:06:59.012] end error: 0.000311
[11:06:59.127] New key frame
[11:06:59.127] BA loop run
[11:06:59.128] init error: 104.907537
[11:06:59.129] end error: 0.029171
[11:06:59.244] New key frame
[11:06:59.244] BA loop run
[11:06:59.244] init error: 29.231466
[11:06:59.246] end error: 0.185819
[11:06:59.359] New key frame
[11:06:59.359] BA loop run
[11:06:59.360] init error: 94.753494
[11:06:59.361] end error: 0.154880
[11:06:59.467] New key frame
[11:06:59.467] BA loop run
[11:06:59.468] init error: 19.887575
[11:06:59.470] end error: 0.184039
[11:06:59.687] New key frame
[11:06:59.687] BA loop run
[11:06:59.688] init error: 749.082843
[11:06:59.689] end error: 0.267130
[11:06:59.922] New key frame
[11:06:59.923] BA loop run
[11:06:59.923] init error: 68.437401
[11:06:59.927] end error: 0.030856
[11:07:00.046] New key frame
[11:07:00.046] BA loop run
[11:07:00.047] init error: 168.228993
[11:07:00.050] end error: 0.680629
[11:07:00.177] New key frame
[11:07:00.177] BA loop run
[11:07:00.178] init error: 226.456550
[11:07:00.186] end error: 1.624141
[11:07:00.322] New key frame
[11:07:00.322] BA loop run
[11:07:00.324] init error: 364.297420
[11:07:00.331] end error: 6.241724
[11:07:00.592] New key frame
[11:07:00.592] BA loop run
[11:07:00.595] init error: 2947.304509
[11:07:00.605] end error: 14.801503
[11:07:00.750] New key frame
[11:07:00.750] BA loop run
[11:07:00.753] init error: 173.564319
[11:07:00.765] end error: 14.556827
[11:07:00.904] New key frame
[11:07:00.904] BA loop run
[11:07:00.906] init error: 53039.896271
[11:07:00.914] end error: 25.636613
[11:07:01.073] New key frame
[11:07:01.073] BA loop run
[11:07:01.076] init error: 99896.971378
[11:07:01.087] end error: 46.032739
[11:07:01.568] New key frame
[11:07:01.568] BA loop run
[11:07:01.572] init error: 9966032.579275
[11:07:01.587] end error: 95.730038
[11:07:03.035] New key frame
[11:07:03.035] BA loop run
[11:07:03.040] init error: 606.022870
[11:07:03.054] end error: 22.251060
[11:07:03.288] New key frame
[11:07:03.288] BA loop run
[11:07:03.294] init error: 10166.284486
[11:07:03.310] end error: 14.188260
[11:07:03.800] New key frame
[11:07:03.801] BA loop run
[11:07:03.808] init error: 3525.805163
[11:07:03.829] end error: 20.503755
[11:07:04.084] New key frame
[11:07:04.084] BA loop run
[11:07:04.088] init error: 14189.806864
[11:07:04.102] end error: 216.837482
[11:07:04.342] New key frame
[11:07:04.342] BA loop run
[11:07:04.348] init error: 4400.882280
[11:07:04.367] end error: 37.015205
[11:07:04.683] New key frame
[11:07:04.683] BA loop run
[11:07:04.689] init error: 19475.042082
[11:07:04.707] end error: 61.342958
[11:07:06.465] New key frame
[11:07:06.465] BA loop run
[11:07:06.471] init error: 6078.668617
[11:07:06.487] end error: 88.529552
[11:07:06.641] New key frame
[11:07:06.641] BA loop run
[11:07:06.649] init error: 2713.465438
[11:07:06.664] end error: 56.754345
[11:07:06.781] New key frame
[11:07:06.781] BA loop run
[11:07:06.787] init error: 8196.651295
[11:07:06.803] end error: 20.070581
[11:07:06.921] New key frame
[11:07:06.922] BA loop run
[11:07:06.928] init error: 42486.824606
[11:07:06.943] end error: 13.026887
[11:07:07.048] New key frame
[11:07:07.048] BA loop run
[11:07:07.054] init error: 15360.510680
[11:07:07.068] end error: 20.866226
[11:07:07.179] New key frame
[11:07:07.179] BA loop run
[11:0
GPU_version/vio/frame_handler_log.txt
Executable
+728
View File
@@ -0,0 +1,728 @@
[11:06:56.451] frame process finished the id is: 0 the obs is:0
[11:06:56.536] frame process finished the id is: 1 the obs is:0
[11:06:56.640] Init: frame zero: 0.000000, 0.000000 0.000000
[11:06:56.640] Init: Selected first frame.
[11:06:56.640] frame process finished the id is: 2 the obs is:0
[11:06:56.668] Init: px average disparity is:0.152753 ,While minimum is: 10.000000 KLT tracked : 109
[11:06:56.668] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:56.668] frame process finished the id is: 3 the obs is:0
[11:06:56.776] Init: px average disparity is:0.221920 ,While minimum is: 10.000000 KLT tracked : 109
[11:06:56.776] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:56.776] frame process finished the id is: 4 the obs is:0
[11:06:56.865] Init: px average disparity is:0.152235 ,While minimum is: 10.000000 KLT tracked : 109
[11:06:56.865] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:56.865] frame process finished the id is: 5 the obs is:0
[11:06:56.973] Init: px average disparity is:0.197958 ,While minimum is: 10.000000 KLT tracked : 109
[11:06:56.973] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:56.974] frame process finished the id is: 6 the obs is:0
[11:06:57.068] Init: px average disparity is:0.221111 ,While minimum is: 10.000000 KLT tracked : 109
[11:06:57.068] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:57.068] frame process finished the id is: 7 the obs is:0
[11:06:57.167] Init: px average disparity is:0.224726 ,While minimum is: 10.000000 KLT tracked : 109
[11:06:57.167] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:57.167] frame process finished the id is: 8 the obs is:0
[11:06:57.274] Init: px average disparity is:0.158653 ,While minimum is: 10.000000 KLT tracked : 109
[11:06:57.274] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:57.274] frame process finished the id is: 9 the obs is:0
[11:06:57.363] Odometry x=0.000000, y=0.000000, theta=-0.000000
[11:06:57.379] Init: px average disparity is:0.179287 ,While minimum is: 10.000000 KLT tracked : 109
[11:06:57.379] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:57.379] frame process finished the id is: 10 the obs is:0
[11:06:57.456] Odometry x=0.000001, y=0.000003, theta=-0.032076
[11:06:57.466] Init: px average disparity is:0.148421 ,While minimum is: 10.000000 KLT tracked : 109
[11:06:57.466] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:57.466] frame process finished the id is: 11 the obs is:0
[11:06:57.554] Odometry x=0.000002, y=0.000008, theta=-0.056012
[11:06:57.580] Init: px average disparity is:0.137330 ,While minimum is: 10.000000 KLT tracked : 109
[11:06:57.580] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:57.580] frame process finished the id is: 12 the obs is:0
[11:06:57.656] Odometry x=0.000006, y=0.000005, theta=-0.079177
[11:06:57.667] Init: px average disparity is:4.945143 ,While minimum is: 10.000000 KLT tracked : 109
[11:06:57.667] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:57.667] frame process finished the id is: 13 the obs is:0
[11:06:57.755] Odometry x=0.000001, y=0.000022, theta=-0.092287
[11:06:57.844] Init: Homography RANSAC (inlier) is:0 ,While 50 inliers minimum required. px average disparity is:11.912397 ,While minimum is: 10.000000 KLT tracked: 109
[11:06:57.844] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:57.844] frame process finished the id is: 14 the obs is:0
[11:06:57.857] Odometry x=0.000010, y=0.000035, theta=-0.115961
[11:06:57.935] Init: Homography RANSAC (inlier) is:0 ,While 50 inliers minimum required. px average disparity is:19.121424 ,While minimum is: 10.000000 KLT tracked: 109
[11:06:57.935] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:57.935] frame process finished the id is: 15 the obs is:0
[11:06:57.954] Odometry x=0.000015, y=0.000040, theta=-0.148211
[11:06:58.043] Init: Homography RANSAC (inlier) is:1 ,While 50 inliers minimum required. px average disparity is:29.212896 ,While minimum is: 10.000000 KLT tracked: 109
[11:06:58.043] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:58.043] frame process finished the id is: 16 the obs is:0
[11:06:58.057] Odometry x=0.000019, y=0.000044, theta=-0.179404
[11:06:58.136] Init: Homography RANSAC (inlier) is:12 ,While 50 inliers minimum required. px average disparity is:40.866547 ,While minimum is: 10.000000 KLT tracked: 109
[11:06:58.136] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:58.136] frame process finished the id is: 17 the obs is:0
[11:06:58.155] Odometry x=0.000012, y=0.000052, theta=-0.209564
[11:06:58.238] Init: Homography RANSAC (inlier) is:12 ,While 50 inliers minimum required. px average disparity is:51.065260 ,While minimum is: 10.000000 KLT tracked: 109
[11:06:58.238] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:58.238] frame process finished the id is: 18 the obs is:0
[11:06:58.255] Odometry x=0.000033, y=0.000051, theta=-0.233318
[11:06:58.335] Init: Homography RANSAC (inlier) is:18 ,While 50 inliers minimum required. px average disparity is:58.238817 ,While minimum is: 10.000000 KLT tracked: 108
[11:06:58.335] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:58.335] frame process finished the id is: 19 the obs is:0
[11:06:58.355] Odometry x=0.000041, y=0.000053, theta=-0.260805
[11:06:58.447] Init: Homography RANSAC (inlier) is:37 ,While 50 inliers minimum required. px average disparity is:68.778966 ,While minimum is: 10.000000 KLT tracked: 106
[11:06:58.447] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:06:58.447] frame process finished the id is: 20 the obs is:0
[11:06:58.457] Odometry x=0.000056, y=0.000052, theta=-0.290973
[11:06:58.541] Init finished: Homography RANSAC (inlier) is:51 ,While 50 inliers minimum required. px average disparity is:79.322258 ,While minimum is: 10.000000 KLT tracked: 105
[11:06:58.541] Init: distance between the first and current frame is x:0.000065 ,z=0.000061,angle between two frames: -0.320842
[11:06:58.541] Init: Selected Second frame. The number of features: 51 depth mean:0.017606 min:-0.079090
[11:06:58.541] frame process finished the id is: 21 the obs is:51
[11:06:58.554] Odometry x=0.000065, y=0.000061, theta=-0.320842
[11:06:58.653] Odometry x=0.000073, y=0.000060, theta=-0.348643
[11:06:58.674] After Reprojection Map nMatches:50 ,distance between ekf and vo x:-0.000035 ,z=0.000057,angle between two frames:0.001333
[11:06:58.674] n deleted obs = 22 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =28 error init =0.145479 error end=0.145479
[11:06:58.674] After pose optimization, distance between ekf and vo x:-0.000035 ,z=0.000057,angle between two frames:0.001333
[11:06:58.674] Update EKF and 3D points the number of feature in the new frame: 28 and number of obs: 28
[11:06:58.674] need key frame pitch dis: 0.350212 translation dif:0.000104
[11:06:58.674] Choose frame as a key frame Scene Depth mean:-0.016632 ,depth min:-0.053989
[11:06:58.674] frame process finished the id is: 22 the obs is:28
[11:06:58.757] Odometry x=0.000065, y=0.000079, theta=-0.375535
[11:06:58.782] After Reprojection Map nMatches:26 ,distance between ekf and vo x:-0.000005 ,z=0.000014,angle between two frames:0.000548
[11:06:58.782] n deleted obs = 8 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =18 error init =0.013782 error end=0.013782
[11:06:58.782] After pose optimization, distance between ekf and vo x:-0.000005 ,z=0.000014,angle between two frames:0.000548
[11:06:58.782] Update EKF and 3D points the number of feature in the new frame: 18 and number of obs: 18
[11:06:58.782] need key frame pitch dis: 0.366246 translation dif:0.000114
[11:06:58.782] Choose frame as a key frame Scene Depth mean:-0.017656 ,depth min:-0.060820
[11:06:58.782] frame process finished the id is: 23 the obs is:18
[11:06:58.854] Odometry x=0.000053, y=0.000101, theta=-0.366246
[11:06:58.895] After Reprojection Map nMatches:17 ,distance between ekf and vo x:-0.000008 ,z=0.000004,angle between two frames:0.000916
[11:06:58.895] n deleted obs = 5 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =12 error init =0.001871 error end=0.001871
[11:06:58.895] After pose optimization, distance between ekf and vo x:-0.000008 ,z=0.000004,angle between two frames:0.000916
[11:06:58.895] Update EKF and 3D points the number of feature in the new frame: 12 and number of obs: 12
[11:06:58.895] need key frame pitch dis: 0.375436 translation dif:0.000122
[11:06:58.895] Choose frame as a key frame Scene Depth mean:0.099809 ,depth min:-0.052875
[11:06:58.895] frame process finished the id is: 24 the obs is:12
[11:06:58.954] Odometry x=0.000059, y=0.000107, theta=-0.375436
[11:06:59.010] After Reprojection Map nMatches:49 ,distance between ekf and vo x:0.000014 ,z=0.000067,angle between two frames:0.000102
[11:06:59.010] n deleted obs = 13 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =36 error init =0.073159 error end=0.021675
[11:06:59.011] After pose optimization, distance between ekf and vo x:-0.000981 ,z=-0.000308,angle between two frames:0.142173
[11:06:59.011] Update EKF and 3D points the number of feature in the new frame: 36 and number of obs: 36
[11:06:59.011] need key frame pitch dis: 0.212000 translation dif:0.019731
[11:06:59.011] Choose frame as a key frame Scene Depth mean:0.002210 ,depth min:-0.202268
[11:06:59.011] frame process finished the id is: 25 the obs is:36
[11:06:59.056] Odometry x=-0.000660, y=-0.000106, theta=-0.279623
[11:06:59.126] After Reprojection Map nMatches:52 ,distance between ekf and vo x:0.000013 ,z=0.000078,angle between two frames:0.000085
[11:06:59.127] n deleted obs = 18 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =34 error init =0.024394 error end=0.023554
[11:06:59.127] After pose optimization, distance between ekf and vo x:0.000274 ,z=0.000200,angle between two frames:0.018753
[11:06:59.127] Update EKF and 3D points the number of feature in the new frame: 34 and number of obs: 34
[11:06:59.127] need key frame pitch dis: 0.206542 translation dif:0.019663
[11:06:59.127] Choose frame as a key frame Scene Depth mean:-0.020615 ,depth min:-0.245136
[11:06:59.127] frame process finished the id is: 26 the obs is:34
[11:06:59.154] Odometry x=-0.000452, y=0.000043, theta=-0.274164
[11:06:59.243] After Reprojection Map nMatches:31 ,distance between ekf and vo x:-0.000026 ,z=0.000069,angle between two frames:0.000615
[11:06:59.243] n deleted obs = 9 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =22 error init =0.009221 error end=0.009221
[11:06:59.243] After pose optimization, distance between ekf and vo x:-0.000026 ,z=0.000069,angle between two frames:0.000615
[11:06:59.243] Update EKF and 3D points the number of feature in the new frame: 22 and number of obs: 22
[11:06:59.243] need key frame pitch dis: 0.283177 translation dif:0.000483
[11:06:59.243] Choose frame as a key frame Scene Depth mean:-0.054119 ,depth min:-0.267462
[11:06:59.244] frame process finished the id is: 27 the obs is:22
[11:06:59.256] Odometry x=-0.000470, y=0.000110, theta=-0.283177
[11:06:59.354] Odometry x=-0.000469, y=0.000120, theta=-0.314187
[11:06:59.359] After Reprojection Map nMatches:21 ,distance between ekf and vo x:-0.000041 ,z=0.000120,angle between two frames:0.000575
[11:06:59.359] n deleted obs = 2 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =19 error init =0.008569 error end=0.008472
[11:06:59.359] After pose optimization, distance between ekf and vo x:-0.000409 ,z=-0.000311,angle between two frames:0.010902
[11:06:59.359] Update EKF and 3D points the number of feature in the new frame: 19 and number of obs: 19
[11:06:59.359] need key frame pitch dis: 0.306483 translation dif:0.000700
[11:06:59.359] Choose frame as a key frame Scene Depth mean:-0.068819 ,depth min:-0.269408
[11:06:59.359] frame process finished the id is: 28 the obs is:19
[11:06:59.456] Odometry x=-0.000534, y=0.000043, theta=-0.334597
[11:06:59.467] After Reprojection Map nMatches:15 ,distance between ekf and vo x:0.000013 ,z=0.000048,angle between two frames:0.000211
[11:06:59.467] n deleted obs = 0 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =15 error init =0.000040 error end=0.000040
[11:06:59.467] After pose optimization, distance between ekf and vo x:0.000013 ,z=0.000048,angle between two frames:0.000211
[11:06:59.467] Update EKF and 3D points the number of feature in the new frame: 15 and number of obs: 15
[11:06:59.467] need key frame pitch dis: 0.321323 translation dif:0.000641
[11:06:59.467] Choose frame as a key frame Scene Depth mean:-0.079894 ,depth min:-0.353232
[11:06:59.467] frame process finished the id is: 29 the obs is:15
[11:06:59.554] Odometry x=-0.000641, y=0.000006, theta=-0.321323
[11:06:59.582] After Reprojection Map nMatches:15 ,distance between ekf and vo x:-0.000038 ,z=0.000139,angle between two frames:0.000748
[11:06:59.582] n deleted obs = 10 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =5 error init =0.018059 error end=0.018059
[11:06:59.582] After pose optimization, distance between ekf and vo x:-0.000038 ,z=0.000139,angle between two frames:0.000748
[11:06:59.582] frame process finished the id is: 29 the obs is:15
[11:06:59.656] Odometry x=-0.000633, y=0.000015, theta=-0.353447
[11:06:59.687] After Reprojection Map nMatches:15 ,distance between ekf and vo x:0.000141 ,z=0.000068,angle between two frames:0.000815
[11:06:59.687] n deleted obs = 3 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =12 error init =0.027294 error end=0.025064
[11:06:59.687] After pose optimization, distance between ekf and vo x:0.001606 ,z=-0.003935,angle between two frames:0.019132
[11:06:59.687] Update EKF and 3D points the number of feature in the new frame: 12 and number of obs: 12
[11:06:59.687] need key frame pitch dis: 0.348134 translation dif:0.002922
[11:06:59.687] Choose frame as a key frame Scene Depth mean:-0.264867 ,depth min:-0.449125
[11:06:59.687] frame process finished the id is: 31 the obs is:12
[11:06:59.754] Odometry x=0.000551, y=-0.002869, theta=-0.348134
[11:06:59.803] After Reprojection Map nMatches:12 ,distance between ekf and vo x:-0.000484 ,z=0.000780,angle between two frames:0.001203
[11:06:59.803] n deleted obs = 9 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =3 error init =0.007110 error end=0.007056
[11:06:59.803] After pose optimization, distance between ekf and vo x:-0.000585 ,z=0.000471,angle between two frames:0.001484
[11:06:59.803] frame process finished the id is: 31 the obs is:12
[11:06:59.856] Odometry x=0.000559, y=-0.002863, theta=-0.378627
[11:06:59.922] After Reprojection Map nMatches:44 ,distance between ekf and vo x:0.003857 ,z=0.000058,angle between two frames:0.009751
[11:06:59.922] n deleted obs = 6 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =38 error init =0.090212 error end=0.079102
[11:06:59.922] After pose optimization, distance between ekf and vo x:0.008459 ,z=-0.002547,angle between two frames:0.008796
[11:06:59.922] Update EKF and 3D points the number of feature in the new frame: 38 and number of obs: 38
[11:06:59.922] need key frame pitch dis: 0.312393 translation dif:0.027152
[11:06:59.922] Choose frame as a key frame Scene Depth mean:-0.006309 ,depth min:-0.587591
[11:06:59.922] frame process finished the id is: 33 the obs is:38
[11:06:59.954] Odometry x=0.006777, y=-0.004721, theta=-0.380015
[11:07:00.045] After Reprojection Map nMatches:78 ,distance between ekf and vo x:-0.000003 ,z=0.000046,angle between two frames:0.000210
[11:07:00.045] n deleted obs = 13 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =65 error init =0.058348 error end=0.058348
[11:07:00.045] After pose optimization, distance between ekf and vo x:-0.000003 ,z=0.000046,angle between two frames:0.000210
[11:07:00.045] Update EKF and 3D points the number of feature in the new frame: 65 and number of obs: 65
[11:07:00.046] need key frame pitch dis: 0.320395 translation dif:0.027116
[11:07:00.046] Choose frame as a key frame Scene Depth mean:-0.032305 ,depth min:-0.510465
[11:07:00.046] frame process finished the id is: 34 the obs is:65
[11:07:00.056] Odometry x=0.006779, y=-0.004679, theta=-0.388018
[11:07:00.158] Odometry x=0.006786, y=-0.004675, theta=-0.411228
[11:07:00.174] After Reprojection Map nMatches:126 ,distance between ekf and vo x:-0.000020 ,z=0.000017,angle between two frames:0.000568
[11:07:00.175] n deleted obs = 23 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =103 error init =0.034350 error end=0.027547
[11:07:00.175] After pose optimization, distance between ekf and vo x:0.001955 ,z=-0.002258,angle between two frames:0.046675
[11:07:00.175] Update EKF and 3D points the number of feature in the new frame: 103 and number of obs: 103
[11:07:00.177] need key frame pitch dis: 0.319628 translation dif:0.028763
[11:07:00.177] Choose frame as a key frame Scene Depth mean:-0.011347 ,depth min:-0.527633
[11:07:00.177] frame process finished the id is: 35 the obs is:103
[11:07:00.255] Odometry x=0.007216, y=-0.005158, theta=-0.423667
[11:07:00.320] After Reprojection Map nMatches:176 ,distance between ekf and vo x:-0.000001 ,z=0.000007,angle between two frames:0.000440
[11:07:00.321] n deleted obs = 29 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =147 error init =0.121094 error end=0.088411
[11:07:00.321] After pose optimization, distance between ekf and vo x:-0.003289 ,z=0.007344,angle between two frames:0.137225
[11:07:00.321] Update EKF and 3D points the number of feature in the new frame: 147 and number of obs: 147
[11:07:00.322] need key frame pitch dis: 0.234847 translation dif:0.022651
[11:07:00.322] Choose frame as a key frame Scene Depth mean:-0.159656 ,depth min:-7.115685
[11:07:00.322] frame process finished the id is: 36 the obs is:147
[11:07:00.354] Odometry x=0.005287, y=-0.000330, theta=-0.302469
[11:07:00.456] Odometry x=0.005279, y=-0.000318, theta=-0.327099
[11:07:00.458] After Reprojection Map nMatches:199 ,distance between ekf and vo x:0.000005 ,z=0.000010,angle between two frames:0.000008
[11:07:00.459] n deleted obs = 37 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =162 error init =0.316894 error end=0.044814
[11:07:00.459] After pose optimization, distance between ekf and vo x:-0.001593 ,z=0.000076,angle between two frames:0.483855
[11:07:00.459] frame process finished the id is: 36 the obs is:147
[11:07:00.554] Odometry x=0.005285, y=-0.000310, theta=-0.351682
[11:07:00.590] After Reprojection Map nMatches:173 ,distance between ekf and vo x:0.000006 ,z=0.000013,angle between two frames:0.000003
[11:07:00.590] n deleted obs = 21 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =152 error init =0.047439 error end=0.047439
[11:07:00.590] After pose optimization, distance between ekf and vo x:0.000006 ,z=0.000013,angle between two frames:0.000003
[11:07:00.590] Update EKF and 3D points the number of feature in the new frame: 152 and number of obs: 152
[11:07:00.592] need key frame pitch dis: 0.310821 translation dif:0.039172
[11:07:00.592] Choose frame as a key frame Scene Depth mean:-0.272442 ,depth min:-7.160390
[11:07:00.592] frame process finished the id is: 38 the obs is:152
[11:07:00.654] Odometry x=0.005301, y=-0.000298, theta=-0.342279
[11:07:00.748] After Reprojection Map nMatches:236 ,distance between ekf and vo x:0.000011 ,z=0.000016,angle between two frames:0.000010
[11:07:00.749] n deleted obs = 51 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =185 error init =0.051339 error end=0.041388
[11:07:00.749] After pose optimization, distance between ekf and vo x:0.003056 ,z=-0.001891,angle between two frames:0.101580
[11:07:00.749] Update EKF and 3D points the number of feature in the new frame: 185 and number of obs: 185
[11:07:00.750] need key frame pitch dis: 0.319387 translation dif:0.046637
[11:07:00.750] Choose frame as a key frame Scene Depth mean:-0.030572 ,depth min:-7.075789
[11:07:00.750] frame process finished the id is: 39 the obs is:185
[11:07:00.754] Odometry x=0.007550, y=-0.001715, theta=-0.274779
[11:07:00.856] Odometry x=0.007553, y=-0.001715, theta=-0.301751
[11:07:00.901] After Reprojection Map nMatches:282 ,distance between ekf and vo x:0.000006 ,z=0.000002,angle between two frames:0.000008
[11:07:00.902] n deleted obs = 73 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =209 error init =0.024603 error end=0.021303
[11:07:00.902] After pose optimization, distance between ekf and vo x:0.000642 ,z=0.001513,angle between two frames:0.098744
[11:07:00.902] Update EKF and 3D points the number of feature in the new frame: 209 and number of obs: 209
[11:07:00.904] need key frame pitch dis: 0.261505 translation dif:0.047818
[11:07:00.904] Choose frame as a key frame Scene Depth mean:0.165577 ,depth min:-0.876819
[11:07:00.904] frame process finished the id is: 40 the obs is:209
[11:07:00.949] Odometry x=0.008028, y=-0.000595, theta=-0.216396
[11:07:01.046] Odometry x=0.008024, y=-0.000597, theta=-0.251493
[11:07:01.070] After Reprojection Map nMatches:279 ,distance between ekf and vo x:-0.000005 ,z=0.000053,angle between two frames:0.000114
[11:07:01.070] n deleted obs = 35 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =244 error init =0.006849 error end=0.006849
[11:07:01.070] After pose optimization, distance between ekf and vo x:-0.000005 ,z=0.000053,angle between two frames:0.000114
[11:07:01.070] Update EKF and 3D points the number of feature in the new frame: 244 and number of obs: 244
[11:07:01.073] need key frame pitch dis: 0.240359 translation dif:0.022744
[11:07:01.073] Choose frame as a key frame Scene Depth mean:-0.114338 ,depth min:-7.010557
[11:07:01.073] frame process finished the id is: 41 the obs is:244
[11:07:01.149] Odometry x=0.008027, y=-0.000546, theta=-0.233459
[11:07:01.239] After Reprojection Map nMatches:301 ,distance between ekf and vo x:0.000020 ,z=0.000023,angle between two frames:0.000014
[11:07:01.241] n deleted obs = 59 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =242 error init =0.044666 error end=0.030511
[11:07:01.241] After pose optimization, distance between ekf and vo x:-0.000262 ,z=0.002987,angle between two frames:0.198635
[11:07:01.241] Update EKF and 3D points the number of feature in the new frame: 242 and number of obs: 242
[11:07:01.242] need key frame pitch dis: 0.077845 translation dif:0.014837
[11:07:01.242] frame process finished the id is: 42 the obs is:242
[11:07:01.248] Odometry x=0.007837, y=0.001659, theta=-0.095298
[11:07:01.356] Odometry x=0.007834, y=0.001671, theta=-0.126635
[11:07:01.409] After Reprojection Map nMatches:307 ,distance between ekf and vo x:0.000079 ,z=0.000054,angle between two frames:0.000167
[11:07:01.409] n deleted obs = 58 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =249 error init =0.053145 error end=0.053145
[11:07:01.409] After pose optimization, distance between ekf and vo x:0.000079 ,z=0.000054,angle between two frames:0.000167
[11:07:01.409] Update EKF and 3D points the number of feature in the new frame: 249 and number of obs: 249
[11:07:01.411] need key frame pitch dis: 0.030538 translation dif:0.016200
[11:07:01.411] frame process finished the id is: 43 the obs is:249
[11:07:01.449] Odometry x=0.007913, y=0.001710, theta=-0.112066
[11:07:01.547] Odometry x=0.007928, y=0.001719, theta=-0.142425
[11:07:01.566] After Reprojection Map nMatches:311 ,distance between ekf and vo x:0.000023 ,z=0.000028,angle between two frames:0.000003
[11:07:01.566] n deleted obs = 34 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =277 error init =0.021781 error end=0.021781
[11:07:01.566] After pose optimization, distance between ekf and vo x:0.000023 ,z=0.000028,angle between two frames:0.000003
[11:07:01.566] Update EKF and 3D points the number of feature in the new frame: 277 and number of obs: 277
[11:07:01.568] need key frame pitch dis: 0.124062 translation dif:0.014836
[11:07:01.568] Choose frame as a key frame Scene Depth mean:0.040162 ,depth min:-0.947652
[11:07:01.568] frame process finished the id is: 44 the obs is:277
[11:07:01.649] Odometry x=0.007936, y=0.001729, theta=-0.171015
[11:07:01.737] After Reprojection Map nMatches:376 ,distance between ekf and vo x:0.000014 ,z=0.000011,angle between two frames:0.000033
[11:07:01.738] n deleted obs = 118 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =258 error init =0.078687 error end=0.074192
[11:07:01.738] After pose optimization, distance between ekf and vo x:0.006743 ,z=-0.001360,angle between two frames:0.111099
[11:07:01.738] Update EKF and 3D points the number of feature in the new frame: 258 and number of obs: 258
[11:07:01.739] need key frame pitch dis: 0.059787 translation dif:0.019017
[11:07:01.739] frame process finished the id is: 45 the obs is:258
[11:07:01.756] Odometry x=0.012901, y=0.000738, theta=-0.077173
[11:07:01.849] Odometry x=0.012911, y=0.000748, theta=-0.104212
[11:07:01.904] After Reprojection Map nMatches:338 ,distance between ekf and vo x:0.000022 ,z=0.000010,angle between two frames:0.000013
[11:07:01.905] n deleted obs = 96 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =242 error init =0.029130 error end=0.029130
[11:07:01.905] After pose optimization, distance between ekf and vo x:0.000022 ,z=0.000010,angle between two frames:0.000013
[11:07:01.905] Update EKF and 3D points the number of feature in the new frame: 242 and number of obs: 242
[11:07:01.906] need key frame pitch dis: 0.047458 translation dif:0.013838
[11:07:01.906] frame process finished the id is: 46 the obs is:242
[11:07:01.947] Odometry x=0.012906, y=0.000772, theta=-0.091776
[11:07:02.048] Odometry x=0.012904, y=0.000783, theta=-0.121549
[11:07:02.081] After Reprojection Map nMatches:394 ,distance between ekf and vo x:0.000017 ,z=0.000036,angle between two frames:0.000022
[11:07:02.082] n deleted obs = 141 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =253 error init =0.067085 error end=0.067085
[11:07:02.082] After pose optimization, distance between ekf and vo x:0.000017 ,z=0.000036,angle between two frames:0.000022
[11:07:02.082] Update EKF and 3D points the number of feature in the new frame: 253 and number of obs: 253
[11:07:02.083] need key frame pitch dis: 0.031416 translation dif:0.013872
[11:07:02.083] frame process finished the id is: 47 the obs is:253
[11:07:02.147] Odometry x=0.012927, y=0.000806, theta=-0.107818
[11:07:02.249] Odometry x=0.012925, y=0.000806, theta=-0.142574
[11:07:02.270] After Reprojection Map nMatches:367 ,distance between ekf and vo x:0.000012 ,z=0.000049,angle between two frames:0.000028
[11:07:02.271] n deleted obs = 99 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =268 error init =0.032349 error end=0.032349
[11:07:02.271] After pose optimization, distance between ekf and vo x:0.000012 ,z=0.000049,angle between two frames:0.000028
[11:07:02.271] Update EKF and 3D points the number of feature in the new frame: 268 and number of obs: 268
[11:07:02.272] need key frame pitch dis: 0.014126 translation dif:0.013907
[11:07:02.272] frame process finished the id is: 48 the obs is:268
[11:07:02.347] Odometry x=0.012944, y=0.000850, theta=-0.125109
[11:07:02.449] Odometry x=0.012949, y=0.000855, theta=-0.144046
[11:07:02.450] After Reprojection Map nMatches:444 ,distance between ekf and vo x:0.000016 ,z=0.000024,angle between two frames:0.000004
[11:07:02.451] n deleted obs = 74 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =370 error init =0.014139 error end=0.014139
[11:07:02.451] After pose optimization, distance between ekf and vo x:0.000016 ,z=0.000024,angle between two frames:0.000004
[11:07:02.451] Update EKF and 3D points the number of feature in the new frame: 370 and number of obs: 370
[11:07:02.453] need key frame pitch dis: 0.010550 translation dif:0.013924
[11:07:02.453] frame process finished the id is: 49 the obs is:370
[11:07:02.551] Odometry x=0.012955, y=0.000864, theta=-0.169187
[11:07:02.647] Odometry x=0.012968, y=0.000872, theta=-0.193294
[11:07:02.650] After Reprojection Map nMatches:425 ,distance between ekf and vo x:0.000003 ,z=0.000005,angle between two frames:0.000093
[11:07:02.650] n deleted obs = 95 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =330 error init =0.068766 error end=0.068766
[11:07:02.650] After pose optimization, distance between ekf and vo x:0.000003 ,z=0.000005,angle between two frames:0.000093
[11:07:02.650] Update EKF and 3D points the number of feature in the new frame: 330 and number of obs: 330
[11:07:02.652] need key frame pitch dis: 0.045650 translation dif:0.013933
[11:07:02.652] frame process finished the id is: 50 the obs is:330
[11:07:02.749] Odometry x=0.012961, y=0.000882, theta=-0.221557
[11:07:02.847] Odometry x=0.012962, y=0.000887, theta=-0.252179
[11:07:02.850] After Reprojection Map nMatches:390 ,distance between ekf and vo x:0.000023 ,z=0.000039,angle between two frames:0.000003
[11:07:02.850] n deleted obs = 106 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =284 error init =0.066814 error end=0.066814
[11:07:02.850] After pose optimization, distance between ekf and vo x:0.000023 ,z=0.000039,angle between two frames:0.000003
[11:07:02.850] Update EKF and 3D points the number of feature in the new frame: 284 and number of obs: 284
[11:07:02.852] need key frame pitch dis: 0.090843 translation dif:0.013957
[11:07:02.852] frame process finished the id is: 51 the obs is:284
[11:07:02.948] Odometry x=0.012984, y=0.000896, theta=-0.272707
[11:07:03.033] After Reprojection Map nMatches:411 ,distance between ekf and vo x:0.000007 ,z=0.000010,angle between two frames:0.000040
[11:07:03.034] n deleted obs = 135 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =276 error init =0.039324 error end=0.039324
[11:07:03.034] After pose optimization, distance between ekf and vo x:0.000007 ,z=0.000010,angle between two frames:0.000040
[11:07:03.034] Update EKF and 3D points the number of feature in the new frame: 276 and number of obs: 276
[11:07:03.035] need key frame pitch dis: 0.110728 translation dif:0.013971
[11:07:03.035] Choose frame as a key frame Scene Depth mean:0.029549 ,depth min:-1.484752
[11:07:03.035] frame process finished the id is: 52 the obs is:276
[11:07:03.050] Odometry x=0.012987, y=0.000907, theta=-0.249962
[11:07:03.147] Odometry x=0.012987, y=0.000917, theta=-0.276727
[11:07:03.250] Odometry x=0.012998, y=0.000922, theta=-0.309878
[11:07:03.282] After Reprojection Map nMatches:481 ,distance between ekf and vo x:0.000011 ,z=0.000011,angle between two frames:0.000126
[11:07:03.285] n deleted obs = 154 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =327 error init =0.236551 error end=0.186599
[11:07:03.285] After pose optimization, distance between ekf and vo x:-0.003142 ,z=0.000728,angle between two frames:0.007295
[11:07:03.285] Update EKF and 3D points the number of feature in the new frame: 327 and number of obs: 327
[11:07:03.288] need key frame pitch dis: 3.414761 translation dif:0.001851
[11:07:03.288] Choose frame as a key frame Scene Depth mean:-0.047442 ,depth min:-7.149089
[11:07:03.288] frame process finished the id is: 53 the obs is:327
[11:07:03.352] Odometry x=0.010687, y=0.001449, theta=-0.278639
[11:07:03.450] Odometry x=0.010687, y=0.001453, theta=-0.304854
[11:07:03.550] Odometry x=0.010691, y=0.001455, theta=-0.334154
[11:07:03.573] After Reprojection Map nMatches:569 ,distance between ekf and vo x:0.000003 ,z=0.000002,angle between two frames:0.000011
[11:07:03.576] n deleted obs = 226 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =343 error init =0.789283 error end=0.098799
[11:07:03.576] After pose optimization, distance between ekf and vo x:0.099806 ,z=-0.053768,angle between two frames:0.845572
[11:07:03.576] frame process finished the id is: 53 the obs is:327
[11:07:03.654] Odometry x=0.010694, y=0.001455, theta=-0.365605
[11:07:03.749] Odometry x=0.010696, y=0.001463, theta=-0.393637
[11:07:03.796] After Reprojection Map nMatches:524 ,distance between ekf and vo x:0.000000 ,z=0.000000,angle between two frames:0.000002
[11:07:03.797] n deleted obs = 168 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =356 error init =0.135030 error end=0.135030
[11:07:03.797] After pose optimization, distance between ekf and vo x:0.000000 ,z=0.000000,angle between two frames:0.000002
[11:07:03.797] Update EKF and 3D points the number of feature in the new frame: 356 and number of obs: 356
[11:07:03.800] need key frame pitch dis: 0.227808 translation dif:0.009277
[11:07:03.800] Choose frame as a key frame Scene Depth mean:-0.008250 ,depth min:-2.140007
[11:07:03.801] frame process finished the id is: 55 the obs is:356
[11:07:03.852] Odometry x=0.010701, y=0.001458, theta=-0.378576
[11:07:03.956] Odometry x=0.010697, y=0.001457, theta=-0.391380
[11:07:04.049] Odometry x=0.010697, y=0.001448, theta=-0.425112
[11:07:04.080] After Reprojection Map nMatches:447 ,distance between ekf and vo x:0.000007 ,z=0.000004,angle between two frames:0.000009
[11:07:04.081] n deleted obs = 133 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =314 error init =0.292479 error end=0.130981
[11:07:04.081] After pose optimization, distance between ekf and vo x:0.008157 ,z=-0.003021,angle between two frames:0.067508
[11:07:04.081] Update EKF and 3D points the number of feature in the new frame: 314 and number of obs: 314
[11:07:04.083] need key frame pitch dis: 2.645695 translation dif:0.011271
[11:07:04.083] Choose frame as a key frame Scene Depth mean:0.030855 ,depth min:-2.070079
[11:07:04.084] frame process finished the id is: 56 the obs is:314
[11:07:04.154] Odometry x=0.012495, y=0.000798, theta=-0.433045
[11:07:04.251] Odometry x=0.012505, y=0.000810, theta=-0.452732
[11:07:04.337] After Reprojection Map nMatches:436 ,distance between ekf and vo x:0.000004 ,z=-0.000001,angle between two frames:0.000048
[11:07:04.339] n deleted obs = 125 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =311 error init =0.171241 error end=0.156063
[11:07:04.339] After pose optimization, distance between ekf and vo x:0.002805 ,z=-0.001105,angle between two frames:0.005197
[11:07:04.339] Update EKF and 3D points the number of feature in the new frame: 311 and number of obs: 311
[11:07:04.341] need key frame pitch dis: 3.526419 translation dif:0.023242
[11:07:04.342] Choose frame as a key frame Scene Depth mean:-0.010122 ,depth min:-12.353056
[11:07:04.342] frame process finished the id is: 57 the obs is:311
[11:07:04.350] Odometry x=0.014559, y=-0.000015, theta=-0.451097
[11:07:04.447] Odometry x=0.014563, y=-0.000032, theta=-0.486222
[11:07:04.547] Odometry x=0.014564, y=-0.000029, theta=-0.520927
[11:07:04.647] Odometry x=0.014568, y=-0.000023, theta=-0.549400
[11:07:04.679] After Reprojection Map nMatches:467 ,distance between ekf and vo x:0.000001 ,z=0.000000,angle between two frames:0.000005
[11:07:04.680] n deleted obs = 110 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =357 error init =0.092912 error end=0.078799
[11:07:04.680] After pose optimization, distance between ekf and vo x:0.001829 ,z=-0.000715,angle between two frames:0.070725
[11:07:04.680] Update EKF and 3D points the number of feature in the new frame: 357 and number of obs: 357
[11:07:04.683] need key frame pitch dis: 2.373300 translation dif:0.015132
[11:07:04.683] Choose frame as a key frame Scene Depth mean:0.030048 ,depth min:-0.951150
[11:07:04.683] frame process finished the id is: 58 the obs is:357
[11:07:04.753] Odometry x=0.015913, y=-0.000547, theta=-0.431800
[11:07:04.847] Odometry x=0.015913, y=-0.000549, theta=-0.457348
[11:07:04.942] After Reprojection Map nMatches:441 ,distance between ekf and vo x:0.000006 ,z=-0.000003,angle between two frames:0.000088
[11:07:04.945] n deleted obs = 118 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =323 error init =11.611596 error end=0.039294
[11:07:04.945] After pose optimization, distance between ekf and vo x:-19.025652 ,z=-74.676610,angle between two frames:1.350734
[11:07:04.945] frame process finished the id is: 58 the obs is:357
[11:07:04.947] Odometry x=0.015931, y=-0.000554, theta=-0.468235
[11:07:05.053] Odometry x=0.015936, y=-0.000549, theta=-0.499693
[11:07:05.148] Odometry x=0.015946, y=-0.000544, theta=-0.528317
[11:07:05.248] Odometry x=0.015955, y=-0.000542, theta=-0.558156
[11:07:05.257] After Reprojection Map nMatches:441 ,distance between ekf and vo x:-0.000004 ,z=0.000000,angle between two frames:0.000263
[11:07:05.259] n deleted obs = 197 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =244 error init =1.122930 error end=0.246340
[11:07:05.259] After pose optimization, distance between ekf and vo x:0.175359 ,z=-0.060430,angle between two frames:1.184909
[11:07:05.259] frame process finished the id is: 58 the obs is:357
[11:07:05.353] Odometry x=0.015959, y=-0.000549, theta=-0.585232
[11:07:05.447] Odometry x=0.015969, y=-0.000549, theta=-0.612663
[11:07:05.502] After Reprojection Map nMatches:511 ,distance between ekf and vo x:-0.000002 ,z=0.000001,angle between two frames:0.000177
[11:07:05.504] n deleted obs = 178 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =333 error init =2.391029 error end=0.209378
[11:07:05.504] After pose optimization, distance between ekf and vo x:-0.114263 ,z=0.049805,angle between two frames:2.098815
[11:07:05.504] frame process finished the id is: 58 the obs is:357
[11:07:05.553] Odometry x=0.015984, y=-0.000550, theta=-0.643388
[11:07:05.647] Odometry x=0.015997, y=-0.000551, theta=-0.658456
[11:07:05.741] After Reprojection Map nMatches:457 ,distance between ekf and vo x:-0.000005 ,z=0.000001,angle between two frames:0.000382
[11:07:05.742] n deleted obs = 100 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =357 error init =0.258330 error end=0.231143
[11:07:05.742] After pose optimization, distance between ekf and vo x:-0.006307 ,z=0.000147,angle between two frames:0.635085
[11:07:05.742] frame process finished the id is: 58 the obs is:357
[11:07:05.753] Odometry x=0.016011, y=-0.000549, theta=-0.679460
[11:07:05.848] Odometry x=0.016011, y=-0.000549, theta=-0.708275
[11:07:05.948] Odometry x=0.016017, y=-0.000547, theta=-0.736179
[11:07:06.017] After Reprojection Map nMatches:449 ,distance between ekf and vo x:-0.000004 ,z=0.000000,angle between two frames:0.000311
[11:07:06.019] n deleted obs = 73 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =376 error init =0.277366 error end=0.050855
[11:07:06.019] After pose optimization, distance between ekf and vo x:-0.018463 ,z=0.000760,angle between two frames:0.977088
[11:07:06.019] frame process finished the id is: 58 the obs is:357
[11:07:06.053] Odometry x=0.016018, y=-0.000538, theta=-0.767162
[11:07:06.148] Odometry x=0.016023, y=-0.000531, theta=-0.798118
[11:07:06.244] After Reprojection Map nMatches:365 ,distance between ekf and vo x:-0.000003 ,z=-0.000000,angle between two frames:0.000422
[11:07:06.246] n deleted obs = 128 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =237 error init =0.181203 error end=0.051818
[11:07:06.246] After pose optimization, distance between ekf and vo x:-0.000031 ,z=0.001262,angle between two frames:0.328805
[11:07:06.246] frame process finished the id is: 58 the obs is:357
[11:07:06.249] Odometry x=0.016037, y=-0.000532, theta=-0.825640
[11:07:06.357] Odometry x=0.016050, y=-0.000530, theta=-0.851662
[11:07:06.450] Odometry x=0.016059, y=-0.000533, theta=-0.876131
[11:07:06.461] After Reprojection Map nMatches:380 ,distance between ekf and vo x:-0.000003 ,z=0.000001,angle between two frames:0.000319
[11:07:06.463] n deleted obs = 86 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =294 error init =0.249988 error end=0.083116
[11:07:06.463] After pose optimization, distance between ekf and vo x:-0.007377 ,z=0.007322,angle between two frames:0.114627
[11:07:06.463] Update EKF and 3D points the number of feature in the new frame: 294 and number of obs: 294
[11:07:06.464] need key frame pitch dis: 0.742504 translation dif:0.015707
[11:07:06.465] Choose frame as a key frame Scene Depth mean:-0.005774 ,depth min:-6.443161
[11:07:06.465] frame process finished the id is: 65 the obs is:294
[11:07:06.550] Odometry x=0.014438, y=0.001076, theta=-0.871516
[11:07:06.639] After Reprojection Map nMatches:284 ,distance between ekf and vo x:-0.000003 ,z=0.000003,angle between two frames:0.000013
[11:07:06.640] n deleted obs = 105 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =179 error init =0.156698 error end=0.156698
[11:07:06.640] After pose optimization, distance between ekf and vo x:-0.000003 ,z=0.000003,angle between two frames:0.000013
[11:07:06.640] Update EKF and 3D points the number of feature in the new frame: 179 and number of obs: 179
[11:07:06.641] need key frame pitch dis: 3.914251 translation dif:0.007215
[11:07:06.641] Choose frame as a key frame Scene Depth mean:0.034777 ,depth min:-1.351853
[11:07:06.641] frame process finished the id is: 66 the obs is:179
[11:07:06.654] Odometry x=0.012584, y=0.002919, theta=-0.825449
[11:07:06.756] Odometry x=0.012586, y=0.002915, theta=-0.855006
[11:07:06.778] After Reprojection Map nMatches:277 ,distance between ekf and vo x:-0.000001 ,z=0.000003,angle between two frames:0.000042
[11:07:06.780] n deleted obs = 111 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =166 error init =0.158959 error end=0.155657
[11:07:06.780] After pose optimization, distance between ekf and vo x:0.000058 ,z=-0.000262,angle between two frames:0.017089
[11:07:06.780] Update EKF and 3D points the number of feature in the new frame: 166 and number of obs: 166
[11:07:06.781] need key frame pitch dis: 3.910915 translation dif:0.005977
[11:07:06.781] Choose frame as a key frame Scene Depth mean:-0.001381 ,depth min:-0.921823
[11:07:06.781] frame process finished the id is: 67 the obs is:166
[11:07:06.849] Odometry x=0.012639, y=0.002724, theta=-0.830577
[11:07:06.919] After Reprojection Map nMatches:246 ,distance between ekf and vo x:0.000003 ,z=0.000004,angle between two frames:0.000363
[11:07:06.919] n deleted obs = 37 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =209 error init =0.032289 error end=0.032289
[11:07:06.919] After pose optimization, distance between ekf and vo x:0.000003 ,z=0.000004,angle between two frames:0.000363
[11:07:06.920] Update EKF and 3D points the number of feature in the new frame: 209 and number of obs: 209
[11:07:06.921] need key frame pitch dis: 2.264916 translation dif:0.014286
[11:07:06.921] Choose frame as a key frame Scene Depth mean:0.024923 ,depth min:-0.276906
[11:07:06.921] frame process finished the id is: 68 the obs is:209
[11:07:06.952] Odometry x=0.012647, y=0.002726, theta=-0.837456
[11:07:07.046] After Reprojection Map nMatches:268 ,distance between ekf and vo x:0.000000 ,z=0.000000,angle between two frames:0.000034
[11:07:07.046] n deleted obs = 67 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =201 error init =0.397837 error end=0.397837
[11:07:07.046] After pose optimization, distance between ekf and vo x:0.000000 ,z=0.000000,angle between two frames:0.000034
[11:07:07.046] Update EKF and 3D points the number of feature in the new frame: 201 and number of obs: 201
[11:07:07.048] need key frame pitch dis: 2.202327 translation dif:0.006333
[11:07:07.048] Choose frame as a key frame Scene Depth mean:0.012490 ,depth min:-0.382688
[11:07:07.048] frame process finished the id is: 69 the obs is:201
[11:07:07.049] Odometry x=0.012643, y=0.002732, theta=-0.845838
[11:07:07.151] Odometry x=0.012649, y=0.002730, theta=-0.856080
[11:07:07.177] After Reprojection Map nMatches:229 ,distance between ekf and vo x:0.000000 ,z=0.000001,angle between two frames:0.000048
[11:07:07.178] n deleted obs = 89 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =140 error init =0.222171 error end=0.179126
[11:07:07.178] After pose optimization, distance between ekf and vo x:0.000223 ,z=0.000801,angle between two frames:0.191788
[11:07:07.178] Update EKF and 3D points the number of feature in the new frame: 140 and number of obs: 140
[11:07:07.179] need key frame pitch dis: 3.984537 translation dif:0.006548
[11:07:07.179] Choose frame as a key frame Scene Depth mean:0.013431 ,depth min:-0.279727
[11:07:07.179] frame process finished the id is: 70 the obs is:140
[11:07:07.249] Odometry x=0.012825, y=0.003320, theta=-1.000166
[11:07:07.300] After Reprojection Map nMatches:182 ,distance between ekf and vo x:-0.000001 ,z=0.000002,angle between two frames:0.000100
[11:07:07.301] n deleted obs = 35 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =147 error init =0.016488 error end=0.012840
[11:07:07.301] After pose optimization, distance between ekf and vo x:0.000075 ,z=-0.000277,angle between two frames:0.032459
[11:07:07.301] Update EKF and 3D points the number of feature in the new frame: 147 and number of obs: 147
[11:07:07.303] need key frame pitch dis: 4.122239 translation dif:0.027812
[11:07:07.303] Choose frame as a key frame Scene Depth mean:0.074168 ,depth min:-0.406359
[11:07:07.304] frame process finished the id is: 71 the obs is:147
[11:07:07.349] Odometry x=0.012887, y=0.003117, theta=-1.033620
[11:07:07.448] After Reprojection Map nMatches:204 ,distance between ekf and vo x:0.000003 ,z=0.000002,angle between two frames:0.000066
[11:07:07.449] n deleted obs = 64 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =140 error init =0.123807 error end=0.097588
[11:07:07.449] After pose optimization, distance between ekf and vo x:-0.001381 ,z=-0.000729,angle between two frames:0.022264
[11:07:07.449] Update EKF and 3D points the number of feature in the new frame: 140 and number of obs: 140
[11:07:07.450] need key frame pitch dis: 1.421671 translation dif:0.024268
[11:07:07.450] Choose frame as a key frame Scene Depth mean:0.085112 ,depth min:-0.467078
[11:07:07.450] frame process finished the id is: 72 the obs is:140
[11:07:07.452] Odometry x=0.011879, y=0.002575, theta=-1.059555
[11:07:07.549] Odometry x=0.011883, y=0.002576, theta=-1.098707
[11:07:07.592] After Reprojection Map nMatches:213 ,distance between ekf and vo x:0.000000 ,z=0.000002,angle between two frames:0.000168
[11:07:07.593] n deleted obs = 84 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =129 error init =0.353963 error end=0.353963
[11:07:07.593] After pose optimization, distance between ekf and vo x:0.000000 ,z=0.000002,angle between two frames:0.000168
[11:07:07.593] Update EKF and 3D points the number of feature in the new frame: 129 and number of obs: 129
[11:07:07.595] need key frame pitch dis: 1.441848 translation dif:0.024312
[11:07:07.595] Choose frame as a key frame Scene Depth mean:0.113952 ,depth min:-0.081213
[11:07:07.595] frame process finished the id is: 73 the obs is:129
[11:07:07.653] Odometry x=0.011892, y=0.002570, theta=-1.077846
[11:07:07.736] After Reprojection Map nMatches:191 ,distance between ekf and vo x:0.000000 ,z=-0.000000,angle between two frames:0.000097
[11:07:07.736] n deleted obs = 68 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =123 error init =0.068377 error end=0.068377
[11:07:07.736] After pose optimization, distance between ekf and vo x:0.000000 ,z=-0.000000,angle between two frames:0.000097
[11:07:07.736] Update EKF and 3D points the number of feature in the new frame: 123 and number of obs: 123
[11:07:07.737] need key frame pitch dis: 1.027010 translation dif:0.022094
[11:07:07.737] Choose frame as a key frame Scene Depth mean:0.094349 ,depth min:-0.685110
[11:07:07.737] frame process finished the id is: 74 the obs is:123
[11:07:07.750] Odometry x=0.011904, y=0.002578, theta=-1.084870
[11:07:07.852] Odometry x=0.011901, y=0.002568, theta=-1.116177
[11:07:07.880] After Reprojection Map nMatches:208 ,distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000046
[11:07:07.881] n deleted obs = 61 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =147 error init =0.240389 error end=0.240389
[11:07:07.881] After pose optimization, distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000046
[11:07:07.881] Update EKF and 3D points the number of feature in the new frame: 147 and number of obs: 147
[11:07:07.882] need key frame pitch dis: 1.271374 translation dif:0.035507
[11:07:07.883] Choose frame as a key frame Scene Depth mean:0.054283 ,depth min:-1.977791
[11:07:07.883] frame process finished the id is: 75 the obs is:147
[11:07:07.950] Odometry x=0.011926, y=0.002566, theta=-1.102505
[11:07:08.000] After Reprojection Map nMatches:152 ,distance between ekf and vo x:0.000000 ,z=-0.000000,angle between two frames:0.000001
[11:07:08.001] n deleted obs = 36 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =116 error init =0.033481 error end=0.032619
[11:07:08.001] After pose optimization, distance between ekf and vo x:-0.000068 ,z=0.000121,angle between two frames:0.000232
[11:07:08.001] Update EKF and 3D points the number of feature in the new frame: 116 and number of obs: 116
[11:07:08.002] need key frame pitch dis: 0.745318 translation dif:0.038130
[11:07:08.002] Choose frame as a key frame Scene Depth mean:0.079111 ,depth min:-0.530107
[11:07:08.002] frame process finished the id is: 76 the obs is:116
[11:07:08.053] Odometry x=0.011879, y=0.002655, theta=-1.110722
[11:07:08.105] After Reprojection Map nMatches:133 ,distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000039
[11:07:08.105] n deleted obs = 33 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =100 error init =0.395833 error end=0.295037
[11:07:08.105] After pose optimization, distance between ekf and vo x:0.002330 ,z=-0.002826,angle between two frames:0.002687
[11:07:08.105] Update EKF and 3D points the number of feature in the new frame: 100 and number of obs: 100
[11:07:08.106] need key frame pitch dis: 4.118604 translation dif:0.038917
[11:07:08.106] Choose frame as a key frame Scene Depth mean:-0.037843 ,depth min:-0.622781
[11:07:08.106] frame process finished the id is: 77 the obs is:100
[11:07:08.155] Odometry x=0.013603, y=0.000577, theta=-1.117475
[11:07:08.220] After Reprojection Map nMatches:130 ,distance between ekf and vo x:-0.000003 ,z=-0.000002,angle between two frames:0.000326
[11:07:08.220] n deleted obs = 33 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =97 error init =0.052341 error end=0.052341
[11:07:08.220] After pose optimization, distance between ekf and vo x:-0.000003 ,z=-0.000002,angle between two frames:0.000326
[11:07:08.220] Update EKF and 3D points the number of feature in the new frame: 97 and number of obs: 97
[11:07:08.221] need key frame pitch dis: 4.130085 translation dif:0.038302
[11:07:08.221] Choose frame as a key frame Scene Depth mean:-0.068028 ,depth min:-0.776305
[11:07:08.221] frame process finished the id is: 78 the obs is:97
[11:07:08.252] Odometry x=0.013600, y=0.000566, theta=-1.125867
[11:07:08.343] After Reprojection Map nMatches:150 ,distance between ekf and vo x:-0.000003 ,z=-0.000001,angle between two frames:0.000292
[11:07:08.344] n deleted obs = 12 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =138 error init =0.101025 error end=0.101025
[11:07:08.344] After pose optimization, distance between ekf and vo x:-0.000003 ,z=-0.000001,angle between two frames:0.000292
[11:07:08.344] Update EKF and 3D points the number of feature in the new frame: 138 and number of obs: 138
[11:07:08.345] need key frame pitch dis: 4.133893 translation dif:0.035146
[11:07:08.345] Choose frame as a key frame Scene Depth mean:-0.087976 ,depth min:-1.648025
[11:07:08.345] frame process finished the id is: 79 the obs is:138
[11:07:08.349] Odometry x=0.013608, y=0.000559, theta=-1.134831
[11:07:08.452] Odometry x=0.013634, y=0.000554, theta=-1.170567
[11:07:08.457] After Reprojection Map nMatches:141 ,distance between ekf and vo x:-0.000004 ,z=-0.000002,angle between two frames:0.000342
[11:07:08.458] n deleted obs = 57 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =84 error init =0.130741 error end=0.130741
[11:07:08.458] After pose optimization, distance between ekf and vo x:-0.000004 ,z=-0.000002,angle between two frames:0.000342
[11:07:08.458] Update EKF and 3D points the number of feature in the new frame: 84 and number of obs: 84
[11:07:08.458] need key frame pitch dis: 0.543366 translation dif:0.027576
[11:07:08.458] Choose frame as a key frame Scene Depth mean:-0.105817 ,depth min:-3.677933
[11:07:08.458] frame process finished the id is: 80 the obs is:84
[11:07:08.549] Odometry x=0.013629, y=0.000550, theta=-1.195068
[11:07:08.553] After Reprojection Map nMatches:144 ,distance between ekf and vo x:0.000001 ,z=-0.000001,angle between two frames:0.000005
[11:07:08.554] n deleted obs = 31 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =113 error init =0.017170 error end=0.017091
[11:07:08.554] After pose optimization, distance between ekf and vo x:-0.000069 ,z=0.000174,angle between two frames:0.000904
[11:07:08.554] Update EKF and 3D points the number of feature in the new frame: 113 and number of obs: 113
[11:07:08.555] need key frame pitch dis: 4.232610 translation dif:0.032484
[11:07:08.555] Choose frame as a key frame Scene Depth mean:0.065932 ,depth min:-0.137363
[11:07:08.555] frame process finished the id is: 81 the obs is:113
[11:07:08.653] Odometry x=0.013618, y=0.000585, theta=-1.222878
[11:07:08.655] After Reprojection Map nMatches:180 ,distance between ekf and vo x:0.000000 ,z=-0.000001,angle between two frames:0.000004
[11:07:08.655] n deleted obs = 51 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =129 error init =0.335414 error end=0.335414
[11:07:08.655] After pose optimization, distance between ekf and vo x:0.000000 ,z=-0.000001,angle between two frames:0.000004
[11:07:08.655] Update EKF and 3D points the number of feature in the new frame: 129 and number of obs: 129
[11:07:08.656] need key frame pitch dis: 0.600470 translation dif:0.016136
[11:07:08.656] Choose frame as a key frame Scene Depth mean:-0.046671 ,depth min:-1.632104
[11:07:08.656] frame process finished the id is: 82 the obs is:129
[11:07:08.752] Odometry x=0.013621, y=0.000582, theta=-1.256952
[11:07:08.773] After Reprojection Map nMatches:183 ,distance between ekf and vo x:-0.000003 ,z=-0.000005,angle between two frames:0.000397
[11:07:08.774] n deleted obs = 46 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =137 error init =0.055706 error end=0.033474
[11:07:08.774] After pose optimization, distance between ekf and vo x:-0.001837 ,z=0.010565,angle between two frames:0.116647
[11:07:08.774] Update EKF and 3D points the number of feature in the new frame: 137 and number of obs: 137
[11:07:08.775] need key frame pitch dis: 0.260660 translation dif:0.015056
[11:07:08.775] Choose frame as a key frame Scene Depth mean:-0.056510 ,depth min:-1.597598
[11:07:08.775] frame process finished the id is: 83 the obs is:137
[11:07:08.848] Odometry x=0.013223, y=0.002893, theta=-1.314508
[11:07:08.895] After Reprojection Map nMatches:202 ,distance between ekf and vo x:-0.000002 ,z=-0.000001,angle between two frames:0.000239
[11:07:08.896] n deleted obs = 68 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =134 error init =0.204926 error end=0.100290
[11:07:08.896] After pose optimization, distance between ekf and vo x:-0.000890 ,z=0.004903,angle between two frames:0.059950
[11:07:08.896] Update EKF and 3D points the number of feature in the new frame: 134 and number of obs: 134
[11:07:08.896] need key frame pitch dis: 0.401488 translation dif:0.007657
[11:07:08.896] Choose frame as a key frame Scene Depth mean:-0.031356 ,depth min:-1.591866
[11:07:08.897] frame process finished the id is: 84 the obs is:134
[11:07:08.952] Odometry x=0.012574, y=0.006491, theta=-1.366009
[11:07:09.020] After Reprojection Map nMatches:210 ,distance between ekf and vo x:-0.000004 ,z=-0.000006,angle between two frames:0.000436
[11:07:09.021] n deleted obs = 29 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =181 error init =0.011281 error end=0.011281
[11:07:09.021] After pose optimization, distance between ekf and vo x:-0.000002 ,z=-0.000010,angle between two frames:0.000537
[11:07:09.021] Update EKF and 3D points the number of feature in the new frame: 181 and number of obs: 181
[11:07:09.022] need key frame pitch dis: 0.379444 translation dif:0.006545
[11:07:09.022] Choose frame as a key frame Scene Depth mean:-0.043153 ,depth min:-1.580078
[11:07:09.022] frame process finished the id is: 85 the obs is:181
[11:07:09.055] Odometry x=0.012584, y=0.006488, theta=-1.375095
[11:07:09.129] After Reprojection Map nMatches:226 ,distance between ekf and vo x:-0.000001 ,z=-0.000001,angle between two frames:0.000127
[11:07:09.130] n deleted obs = 31 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =195 error init =0.108977 error end=0.103497
[11:07:09.130] After pose optimization, distance between ekf and vo x:0.000815 ,z=-0.002352,angle between two frames:0.012727
[11:07:09.130] Update EKF and 3D points the number of feature in the new frame: 195 and number of obs: 195
[11:07:09.132] need key frame pitch dis: 0.309493 translation dif:0.009592
[11:07:09.132] Choose frame as a key frame Scene Depth mean:0.394038 ,depth min:-1.539462
[11:07:09.132] frame process finished the id is: 86 the obs is:195
[11:07:09.157] Odometry x=0.013180, y=0.004761, theta=-1.392369
[11:07:09.244] After Reprojection Map nMatches:284 ,distance between ekf and vo x:-0.000001 ,z=-0.000001,angle between two frames:0.000122
[11:07:09.245] n deleted obs = 84 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =200 error init =0.032212 error end=0.032177
[11:07:09.245] After pose optimization, distance between ekf and vo x:-0.000013 ,z=0.000052,angle between two frames:0.000112
[11:07:09.245] Update EKF and 3D points the number of feature in the new frame: 200 and number of obs: 200
[11:07:09.247] need key frame pitch dis: 0.345219 translation dif:0.011473
[11:07:09.247] Choose frame as a key frame Scene Depth mean:-0.002032 ,depth min:-0.377516
[11:07:09.248] frame process finished the id is: 87 the obs is:200
[11:07:09.254] Odometry x=0.013175, y=0.004798, theta=-1.402033
[11:07:09.355] Odometry x=0.013181, y=0.004807, theta=-1.429318
[11:07:09.357] After Reprojection Map nMatches:208 ,distance between ekf and vo x:-0.000002 ,z=-0.000002,angle between two frames:0.000153
[11:07:09.358] n deleted obs = 47 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =161 error init =0.018976 error end=0.018891
[11:07:09.358] After pose optimization, distance between ekf and vo x:-0.000097 ,z=0.000032,angle between two frames:0.008783
[11:07:09.358] Update EKF and 3D points the number of feature in the new frame: 161 and number of obs: 161
[11:07:09.360] need key frame pitch dis: 1.703752 translation dif:0.017653
[11:07:09.360] Choose frame as a key frame Scene Depth mean:-0.021653 ,depth min:-1.493908
[11:07:09.360] frame process finished the id is: 88 the obs is:161
[11:07:09.457] Odometry x=0.013161, y=0.004801, theta=-1.458501
[11:07:09.489] After Reprojection Map nMatches:212 ,distance between ekf and vo x:-0.000001 ,z=-0.000003,angle between two frames:0.000182
[11:07:09.491] n deleted obs = 54 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =158 error init =0.067111 error end=0.065330
[11:07:09.491] After pose optimization, distance between ekf and vo x:-0.000098 ,z=0.000656,angle between two frames:0.000637
[11:07:09.491] Update EKF and 3D points the number of feature in the new frame: 158 and number of obs: 158
[11:07:09.492] need key frame pitch dis: 0.203474 translation dif:0.013395
[11:07:09.492] Choose frame as a key frame Scene Depth mean:-0.048193 ,depth min:-1.520407
[11:07:09.492] frame process finished the id is: 89 the obs is:158
[11:07:09.554] Odometry x=0.013133, y=0.004949, theta=-1.485270
[11:07:09.617] After Reprojection Map nMatches:296 ,distance between ekf and vo x:-0.000002 ,z=-0.000002,angle between two frames:0.000149
[11:07:09.618] n deleted obs = 51 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =245 error init =0.085523 error end=0.085523
[11:07:09.618] After pose optimization, distance between ekf and vo x:-0.000002 ,z=-0.000002,angle between two frames:0.000149
[11:07:09.618] Update EKF and 3D points the number of feature in the new frame: 245 and number of obs: 245
[11:07:09.620] need key frame pitch dis: 0.144032 translation dif:0.011249
[11:07:09.620] Choose frame as a key frame Scene Depth mean:-0.031465 ,depth min:-1.489282
[11:07:09.620] frame process finished the id is: 90 the obs is:245
[11:07:09.651] Odometry x=0.013141, y=0.004944, theta=-1.494291
[11:07:09.744] After Reprojection Map nMatches:248 ,distance between ekf and vo x:-0.000003 ,z=-0.000003,angle between two frames:0.000252
[11:07:09.745] n deleted obs = 52 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =196 error init =0.091317 error end=0.079368
[11:07:09.745] After pose optimization, distance between ekf and vo x:-0.000049 ,z=0.000405,angle between two frames:0.003851
[11:07:09.745] Update EKF and 3D points the number of feature in the new frame: 196 and number of obs: 196
[11:07:09.747] need key frame pitch dis: 0.109280 translation dif:0.008870
[11:07:09.747] Choose frame as a key frame Scene Depth mean:-0.033764 ,depth min:-1.438838
[11:07:09.747] frame process finished the id is: 91 the obs is:196
[11:07:09.755] Odometry x=0.013113, y=0.005233, theta=-1.498338
[11:07:09.847] Odometry x=0.013122, y=0.005228, theta=-1.528300
[11:07:09.856] After Reprojection Map nMatches:238 ,distance between ekf and vo x:-0.000001 ,z=-0.000001,angle between two frames:0.000114
[11:07:09.857] n deleted obs = 39 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =199 error init =0.012131 error end=0.012130
[11:07:09.857] After pose optimization, distance between ekf and vo x:-0.000001 ,z=0.000005,angle between two frames:0.000123
[11:07:09.857] Update EKF and 3D points the number of feature in the new frame: 199 and number of obs: 199
[11:07:09.859] need key frame pitch dis: 1.793856 translation dif:0.017499
[11:07:09.859] Choose frame as a key frame Scene Depth mean:-0.023104 ,depth min:-1.404713
[11:07:09.859] frame process finished the id is: 92 the obs is:199
[11:07:09.954] Odometry x=0.013116, y=0.005236, theta=-1.515117
[11:07:09.981] After Reprojection Map nMatches:240 ,distance between ekf and vo x:-0.000003 ,z=-0.000003,angle between two frames:0.000261
[11:07:09.982] n deleted obs = 76 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =164 error init =0.018174 error end=0.018042
[11:07:09.982] After pose optimization, distance between ekf and vo x:0.000004 ,z=-0.000029,angle between two frames:0.000207
[11:07:09.982] Update EKF and 3D points the number of feature in the new frame: 164 and number of obs: 164
[11:07:09.983] need key frame pitch dis: 0.136428 translation dif:0.004671
[11:07:09.983] Choose frame as a key frame Scene Depth mean:0.056638 ,depth min:-1.377374
[11:07:09.983] frame process finished the id is: 93 the obs is:164
[11:07:10.051] Odometry x=0.013124, y=0.005225, theta=-1.550542
[11:07:10.097] After Reprojection Map nMatches:245 ,distance between ekf and vo x:0.000001 ,z=-0.000002,angle between two frames:0.000078
[11:07:10.097] n deleted obs = 78 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =167 error init =0.057132 error end=0.057132
[11:07:10.097] After pose optimization, distance between ekf and vo x:0.000001 ,z=-0.000002,angle between two frames:0.000078
[11:07:10.097] Update EKF and 3D points the number of feature in the new frame: 167 and number of obs: 167
[11:07:10.099] need key frame pitch dis: 0.252436 translation dif:0.013428
[11:07:10.099] Choose frame as a key frame Scene Depth mean:-0.008845 ,depth min:-1.384353
[11:07:10.099] frame process finished the id is: 94 the obs is:167
[11:07:10.153] Odometry x=0.013124, y=0.005216, theta=-1.558817
[11:07:10.219] After Reprojection Map nMatches:291 ,distance between ekf and vo x:-0.000002 ,z=-0.000003,angle between two frames:0.000170
[11:07:10.219] n deleted obs = 41 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =250 error init =0.014755 error end=0.014755
[11:07:10.219] After pose optimization, distance between ekf and vo x:-0.000002 ,z=-0.000003,angle between two frames:0.000170
[11:07:10.219] Update EKF and 3D points the number of feature in the new frame: 250 and number of obs: 250
[11:07:10.221] need key frame pitch dis: 0.243648 translation dif:0.013424
[11:07:10.221] Choose frame as a key frame Scene Depth mean:-0.016520 ,depth min:-0.317090
[11:07:10.221] frame process finished the id is: 95 the obs is:250
[11:07:10.251] Odometry x=0.013139, y=0.005215, theta=-1.567605
[11:07:10.341] After Reprojection Map nMatches:269 ,distance between ekf and vo x:-0.000003 ,z=-0.000001,angle between two frames:0.000326
[11:07:10.341] n deleted obs = 63 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =206 error init =0.987960 error end=0.225815
[11:07:10.341] After pose optimization, distance between ekf and vo x:0.002130 ,z=0.002453,angle between two frames:0.210940
[11:07:10.341] frame process finished the id is: 95 the obs is:250
[11:07:10.348] Odometry x=0.013133, y=0.005205, theta=-1.593495
[11:07:10.457] Odometry x=0.013139, y=0.005203, theta=-1.617107
[11:07:10.457] After Reprojection Map nMatches:321 ,distance between ekf and vo x:-0.000003 ,z=-0.000000,angle between two frames:0.000276
[11:07:10.459] n deleted obs = 84 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =237 error init =0.011194 error end=0.011173
[11:07:10.459] After pose optimization, distance between ekf and vo x:-0.000005 ,z=-0.000052,angle between two frames:0.000134
[11:07:10.459] Update EKF and 3D points the number of feature in the new frame: 237 and number of obs: 237
[11:07:10.460] need key frame pitch dis: 0.095077 translation dif:0.008050
[11:07:10.460] frame process finished the id is: 97 the obs is:237
[11:07:10.548] Odometry x=0.013136, y=0.005191, theta=-1.639963
[11:07:10.580] After Reprojection Map nMatches:297 ,distance between ekf and vo x:-0.000004 ,z=-0.000001,angle between two frames:0.000325
[11:07:10.581] n deleted obs = 100 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =197 error init =0.085961 error end=0.082564
[11:07:10.581] After pose optimization, distance between ekf and vo x:-0.000123 ,z=-0.000354,angle between two frames:0.002422
[11:07:10.581] Update EKF and 3D points the number of feature in the new frame: 197 and number of obs: 197
[11:07:10.582] need key frame pitch dis: 0.068593 translation dif:0.008022
[11:07:10.582] frame process finished the id is: 98 the obs is:197
[11:07:10.651] Odometry x=0.013115, y=0.005111, theta=-1.662607
[11:07:10.701] After Reprojection Map nMatches:357 ,distance between ekf and vo x:0.000000 ,z=-0.000000,angle between two frames:0.000027
[11:07:10.702] n deleted obs = 95 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =262 error init =0.029260 error end=0.029260
[11:07:10.702] After pose optimization, distance between ekf and vo x:0.000000 ,z=-0.000000,angle between two frames:0.000027
[11:07:10.702] Update EKF and 3D points the number of feature in the new frame: 262 and number of obs: 262
[11:07:10.703] need key frame pitch dis: 0.045788 translation dif:0.008042
[11:07:10.703] frame process finished the id is: 99 the obs is:262
[11:07:10.751] Odometry x=0.013111, y=0.005105, theta=-1.669462
[11:07:10.825] After Reprojection Map nMatches:304 ,distance between ekf and vo x:-0.000002 ,z=-0.000001,angle between two frames:0.000190
[11:07:10.825] n deleted obs = 92 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =212 error init =0.041762 error end=0.041762
[11:07:10.826] After pose optimization, distance between ekf and vo x:-0.000002 ,z=-0.000001,angle between two frames:0.000190
[11:07:10.826] Update EKF and 3D points the number of feature in the new frame: 212 and number of obs: 212
[11:07:10.827] need key frame pitch dis: 0.071616 translation dif:0.009189
[11:07:10.827] frame process finished the id is: 100 the obs is:212
[11:07:10.854] Odometry x=0.013119, y=0.005106, theta=-1.675236
[11:07:10.931] After Reprojection Map nMatches:246 ,distance between ekf and vo x:-0.000000 ,z=0.000001,angle between two frames:0.000032
[11:07:10.932] n deleted obs = 78 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =168 error init =0.029848 error end=0.029810
[11:07:10.932] After pose optimization, distance between ekf and vo x:-0.000054 ,z=-0.000112,angle between two frames:0.000019
[11:07:10.932] Update EKF and 3D points the number of feature in the new frame: 168 and number of obs: 168
[11:07:10.933] need key frame pitch dis: 0.030077 translation dif:0.008036
[11:07:10.933] frame process finished the id is: 101 the obs is:168
[11:07:10.957] Odometry x=0.013088, y=0.005028, theta=-1.685174
[11:07:11.054] Odometry x=0.013098, y=0.005026, theta=-1.704063
[11:07:11.065] After Reprojection Map nMatches:303 ,distance between ekf and vo x:-0.000000 ,z=0.000000,angle between two frames:0.000064
[11:07:11.066] n deleted obs = 98 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =205 error init =0.034391 error end=0.028070
[11:07:11.066] After pose optimization, distance between ekf and vo x:-0.000621 ,z=-0.000233,angle between two frames:0.081613
[11:07:11.066] Update EKF and 3D points the number of feature in the new frame: 205 and number of obs: 205
[11:07:11.067] need key frame pitch dis: 0.053009 translation dif:0.007731
[11:07:11.067] frame process finished the id is: 102 the obs is:205
[11:07:11.157] Odometry x=0.012953, y=0.004975, theta=-1.714570
[11:07:11.255] Odometry x=0.012958, y=0.004978, theta=-1.745936
[11:07:11.352] Odometry x=0.012958, y=0.004975, theta=-1.773745
[11:07:11.456] Odometry x=0.012963, y=0.004967, theta=-1.801145
[11:07:11.554] Odometry x=0.012970, y=0.004973, theta=-1.827468
[11:07:11.652] Odometry x=0.012966, y=0.004966, theta=-1.854659
[11:07:11.751] Odometry x=0.012961, y=0.004965, theta=-1.884182
[11:07:11.852] Odometry x=0.012967, y=0.004970, theta=-1
GPU_version/vio/include/vio/cl_class.h
+20 -1
View File
@@ -51,6 +51,21 @@ public:
cl_int err=_kernel->setArg(id,*_images.back().first); cl_int err=_kernel->setArg(id,*_images.back().first);
assert(err == CL_SUCCESS); assert(err == CL_SUCCESS);
}; };
template<typename T>
void reload(size_t id,T* buf,cl::CommandQueue* queue,size_t size){
assert(buf);
cl_int error;
for(auto&& i:_buffers)if(i.second==id){
cl::Event event;
T* Map_buf=(T*)queue->enqueueMapBuffer(*i.first,CL_NON_BLOCKING,CL_MAP_WRITE,0,sizeof(T) * size,NULL,&event,&error);
event.wait();
memcpy(Map_buf,buf,sizeof(T) * size);
assert(queue->enqueueUnmapMemObject(*i.first,Map_buf,NULL,&event)==CL_SUCCESS);
event.wait();
}
assert(error == CL_SUCCESS);
};
void release(size_t id/*buffer ID*/){ void release(size_t id/*buffer ID*/){
for(auto it=_buffers.begin();it!=_buffers.end();++it){ for(auto it=_buffers.begin();it!=_buffers.end();++it){
if((*it).second==id){ if((*it).second==id){
@@ -87,7 +102,7 @@ private:
class read_cl{ class read_cl{
public: public:
read_cl(std::string path){ read_cl(std::string path){
src_str=(char *)malloc(0x100000); src_str=(char *)calloc(0x100000, sizeof(char));
FILE *fp; FILE *fp;
fp = fopen(path.c_str(), "r"); fp = fopen(path.c_str(), "r");
size = fread(src_str, 1, 0x100000, fp); size = fread(src_str, 1, 0x100000, fp);
@@ -115,6 +130,10 @@ public:
void load(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T&& value){ void load(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T&& value){
_kernels.at(id1)._kernel->setArg(id2,value); _kernels.at(id1)._kernel->setArg(id2,value);
}; };
template<typename T>
void reload(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, size_t size,T* buf){
_kernels.at(id1).reload(id2,buf,queue,size);
};
cl_int run(size_t id1/*kernal ID*/,std::size_t x=1,std::size_t y=1,std::size_t z=1) { cl_int run(size_t id1/*kernal ID*/,std::size_t x=1,std::size_t y=1,std::size_t z=1) {
cl_int err=0; cl_int err=0;
cl::Event event; cl::Event event;
GPU_version/vio/include/vio/nlls_solver_impl.hpp
+3 -3
View File
@@ -37,7 +37,7 @@ void vk::NLLSSolver<D, T>::optimizeGaussNewton()
if(!solve()) if(!solve())
{ {
// matrix was singular and could not be computed // matrix was singular and could not be computed
if(verbose_)std::cout << "Matrix is close to singular! Stop Optimizing." << std::endl; if(verbose_)std::cerr << "Matrix is close to singular! Stop Optimizing." << std::endl;
stop_ = true; stop_ = true;
} }
// check if error increased since last optimization // check if error increased since last optimization
@@ -45,7 +45,7 @@ void vk::NLLSSolver<D, T>::optimizeGaussNewton()
{ {
if(verbose_) if(verbose_)
{ {
std::cout << "It. " << iter_ std::cerr << "It. " << iter_
<< "\t Failure" << "\t Failure"
<< "\t new_chi2 = " << new_chi2 << "\t new_chi2 = " << new_chi2
<< "\t Error increased. Stop optimizing.\n"; << "\t Error increased. Stop optimizing.\n";
@@ -57,7 +57,7 @@ void vk::NLLSSolver<D, T>::optimizeGaussNewton()
chi2_ = new_chi2; chi2_ = new_chi2;
if(verbose_) if(verbose_)
{ {
std::cout << "It. " << iter_ std::cerr << "It. " << iter_
<< "\t Success" << "\t Success"
<< "\t new_chi2 = " << new_chi2 << "\t new_chi2 = " << new_chi2
<< "\t x_norm = " << vk::norm_max(x_)<<'\n'; << "\t x_norm = " << vk::norm_max(x_)<<'\n';
GPU_version/vio/src/feature_detection.cpp
+5 -4
View File
@@ -80,8 +80,8 @@ void FastDetector::detect(
int scale = (1<<L); int scale = (1<<L);
cv::Mat img=img_pyr.at(L); cv::Mat img=img_pyr.at(L);
gpu_fast_->load(0,0,img); gpu_fast_->load(0,0,img);
cl_int2 fast_corner[1000]={0}; cl_int2* fast_corner=(cl_int2*)calloc(2000, sizeof(cl_int2));
gpu_fast_->load(0,1,1000,fast_corner); gpu_fast_->load(0,1,2000,fast_corner);
cl_int icorner[1]={0}; cl_int icorner[1]={0};
gpu_fast_->load(0,2,1,icorner); gpu_fast_->load(0,2,1,icorner);
gpu_fast_->run(0,img.cols,img.rows); gpu_fast_->run(0,img.cols,img.rows);
@@ -92,7 +92,7 @@ void FastDetector::detect(
exit(0); exit(0);
} }
int size=count[0]; int size=count[0];
cl_int2 fast_corners[size]; cl_int2* fast_corners=(cl_int2*)calloc(size, sizeof(cl_int2));
gpu_fast_->read(0,1,size,fast_corners); gpu_fast_->read(0,1,size,fast_corners);
for(uint i=0;i<size;++i) for(uint i=0;i<size;++i)
{ {
@@ -105,12 +105,13 @@ void FastDetector::detect(
gpu_fast_->release(0,0); gpu_fast_->release(0,0);
gpu_fast_->release(0,1); gpu_fast_->release(0,1);
gpu_fast_->release(0,2); gpu_fast_->release(0,2);
free(fast_corner);
free(fast_corners);
} }
if(keypoints.size()<1){ if(keypoints.size()<1){
ROS_ERROR("GPU Driver crash try again!"); ROS_ERROR("GPU Driver crash try again!");
assert(0); assert(0);
} }
std::cerr<<keypoints.size()<<'\n';
cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4); cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
cv::Mat descriptor; cv::Mat descriptor;
extractor->compute(frame->img(), keypoints, descriptor); extractor->compute(frame->img(), keypoints, descriptor);
GPU_version/vio/src/frame_handler_mono.cpp
+2 -2
View File
@@ -146,8 +146,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
new_frame_->T_f_w_=init_f.second; new_frame_->T_f_w_=init_f.second;
new_frame_->Cov_ = init_f.first; new_frame_->Cov_ = init_f.first;
// sparse image align // sparse image align
SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_); std::unique_ptr<SparseImgAlignGpu> img_align=std::make_unique<SparseImgAlignGpu>(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
size_t err=img_align.run(last_frame_, new_frame_, log_); size_t err=img_align->run(last_frame_, new_frame_, log_);
if(err==-1){ if(err==-1){
#if VIO_DEBUG #if VIO_DEBUG
fprintf(log_,"[%s] Image Alignment failed -1\n",vio::time_in_HH_MM_SS_MMM().c_str()); fprintf(log_,"[%s] Image Alignment failed -1\n",vio::time_in_HH_MM_SS_MMM().c_str());
GPU_version/vio/src/sparse_img_align_gpu.cpp
+46 -25
View File
@@ -38,13 +38,10 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log)
reset(); reset();
cl_float3 ref_pos[1]={(float)ref_frame->pos()(0),(float)ref_frame->pos()(1),(float)ref_frame->T_f_w_.pitch()}; cl_float3 ref_pos[1]={(float)ref_frame->pos()(0),(float)ref_frame->pos()(1),(float)ref_frame->T_f_w_.pitch()};
cl_float3 cur_pos[1]={(float)cur_frame->pos()(0),(float)cur_frame->pos()(1),(float)cur_frame->T_f_w_.pitch()}; cl_float3 cur_pos[1]={(float)cur_frame->pos()(0),(float)cur_frame->pos()(1),(float)cur_frame->T_f_w_.pitch()};
residual_->load(1,2,1,cur_pos);
residual_->load(1,3,1,ref_pos);
cl_float3 features[ref_frame->fts_.size()]; cl_float3 features[ref_frame->fts_.size()];
cl_float2 featue_px[ref_frame->fts_.size()]; cl_float2 featue_px[ref_frame->fts_.size()];
feature_counter_ = 0; // is used to compute the index of the cached jacobian feature_counter_ = 0; // is used to compute the index of the cached jacobian
for(auto it:_for(ref_frame->fts_)){ for(auto it:_for(ref_frame->fts_)){
++feature_counter_;
if(it.item->point == nullptr){ if(it.item->point == nullptr){
features[it.index].x=0.f; features[it.index].x=0.f;
features[it.index].y=0.f; features[it.index].y=0.f;
@@ -59,19 +56,25 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log)
features[it.index].z=xyz_ref(2); features[it.index].z=xyz_ref(2);
featue_px[it.index].x=it.item->px(0); featue_px[it.index].x=it.item->px(0);
featue_px[it.index].y=it.item->px(1); featue_px[it.index].y=it.item->px(1);
++feature_counter_;
} }
if(!feature_counter_) // more than 10 if(!feature_counter_) // more than 10
{ {
return 0; return 0;
} }
feature_counter_=ref_frame->fts_.size(); feature_counter_=ref_frame->fts_.size();
cl_float error[feature_counter_]; residual_->load(1,2,1,cur_pos);
cl_float chi[feature_counter_]; residual_->load(1,3,1,ref_pos);
residual_->load(1,10,feature_counter_,chi);
residual_->load(1,7,feature_counter_,error);
residual_->load(1,4,feature_counter_,features); residual_->load(1,4,feature_counter_,features);
residual_->load(1,5,feature_counter_,featue_px); residual_->load(1,5,feature_counter_,featue_px);
cl_float* error_=(cl_float*)calloc(feature_counter_, sizeof(cl_float));
cl_float* H=(cl_float*)calloc(9*feature_counter_, sizeof(cl_float));
cl_float3* J=(cl_float3*)calloc(feature_counter_, sizeof(cl_float3));
cl_float* chi2_=(cl_float*)calloc(feature_counter_, sizeof(cl_float));
residual_->load(1,7,feature_counter_,error_);
residual_->load(1,8,9*feature_counter_,H);
residual_->load(1,9,feature_counter_,J);
residual_->load(1,10,feature_counter_,chi2_);
SE2 T_cur(cur_frame->T_f_w_.se2());///TODO temporary, we can remove it SE2 T_cur(cur_frame->T_f_w_.se2());///TODO temporary, we can remove it
for(level_=max_level_; level_>=min_level_; --level_) for(level_=max_level_; level_>=min_level_; --level_)
{ {
@@ -87,6 +90,18 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log)
} }
cl_float3 pos[1]={0}; cl_float3 pos[1]={0};
residual_->read(1,2,1,pos); residual_->read(1,2,1,pos);
residual_->release(1,2);
residual_->release(1,3);
residual_->release(1,4);
residual_->release(1,5);
residual_->release(1,7);
residual_->release(1,8);
residual_->release(1,9);
residual_->release(1,10);
free(error_);
free(H);
free(J);
free(chi2_);
if(isnan(pos[0].x) || isnan(pos[0].y) || isnan(pos[0].z))return 1; if(isnan(pos[0].x) || isnan(pos[0].y) || isnan(pos[0].z))return 1;
cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z); cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z);
return 1; return 1;
@@ -96,17 +111,19 @@ double SparseImgAlignGpu::computeResiduals(
bool linearize_system, bool linearize_system,
bool compute_weight_scale) bool compute_weight_scale)
{ {
cl_float H[9*feature_counter_]; cl_float* error_=(cl_float*)calloc(feature_counter_, sizeof(cl_float));
cl_float3 J[feature_counter_]; cl_float* H=(cl_float*)calloc(9*feature_counter_, sizeof(cl_float));
cl_float chi2_[feature_counter_]; cl_float3* J=(cl_float3*)calloc(feature_counter_, sizeof(cl_float3));
cl_float* chi2_=(cl_float*)calloc(feature_counter_, sizeof(cl_float));
cl_float scale=(float)scale_; cl_float scale=(float)scale_;
residual_->reload(1,7,feature_counter_,error_);
residual_->reload(1,8,9*feature_counter_,H);
residual_->reload(1,9,feature_counter_,J);
residual_->reload(1,10,feature_counter_,chi2_);
residual_->load(1,11,scale); residual_->load(1,11,scale);
residual_->load(1,10,feature_counter_,chi2_);
residual_->load(1,8,9*feature_counter_,H);
residual_->load(1,9,feature_counter_,J);
residual_->run(1,feature_counter_); residual_->run(1,feature_counter_);
cl_float error[feature_counter_]; cl_float* error=(cl_float*)calloc(feature_counter_, sizeof(cl_float));
cl_float chi[feature_counter_]; cl_float* chi=(cl_float*)calloc(feature_counter_, sizeof(cl_float));
residual_->read(1,10,feature_counter_,chi); residual_->read(1,10,feature_counter_,chi);
residual_->read(1,7,feature_counter_,error); residual_->read(1,7,feature_counter_,error);
for(int i = 1; i < feature_counter_; ++i){ for(int i = 1; i < feature_counter_; ++i){
@@ -114,13 +131,20 @@ double SparseImgAlignGpu::computeResiduals(
chi[0] += chi[i]; chi[0] += chi[i];
} }
scale_ = error[0]*(1.48f / feature_counter_); scale_ = error[0]*(1.48f / feature_counter_);
return chi[0]/(feature_counter_*8); double out=chi[0]/(feature_counter_*8);
free(error);
free(error_);
free(chi);
free(H);
free(J);
free(chi2_);
return out;
} }
bool SparseImgAlignGpu::solve() bool SparseImgAlignGpu::solve()
{ {
cl_float H[feature_counter_*9]; cl_float* H=(cl_float*)calloc(9*feature_counter_, sizeof(cl_float));
cl_float3 J[feature_counter_]; cl_float3* J=(cl_float3*)calloc(feature_counter_, sizeof(cl_float3));
residual_->read(1,8,feature_counter_*9,H); residual_->read(1,8,feature_counter_*9,H);
residual_->read(1,9,feature_counter_,J); residual_->read(1,9,feature_counter_,J);
for(int i = 1; i < feature_counter_; ++i){ for(int i = 1; i < feature_counter_; ++i){
@@ -143,10 +167,8 @@ bool SparseImgAlignGpu::solve()
x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d((double)J[0].x,(double)J[0].y,(double)J[0].z)); x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d((double)J[0].x,(double)J[0].y,(double)J[0].z));
double norm=x_.norm(); double norm=x_.norm();
if(norm<=0 ||norm > 1.0)x_=Eigen::Vector3d(0.1,0.1,0.1); if(norm<=0 ||norm > 1.0)x_=Eigen::Vector3d(0.1,0.1,0.1);
residual_->release(1,10); free(H);
residual_->release(1,11); free(J);
residual_->release(1,8);
residual_->release(1,9);
return true; return true;
} }
void SparseImgAlignGpu::update() void SparseImgAlignGpu::update()
@@ -157,8 +179,7 @@ void SparseImgAlignGpu::update()
pos[0].x=(float)update.translation()(0); pos[0].x=(float)update.translation()(0);
pos[0].y=(float)update.translation()(1); pos[0].y=(float)update.translation()(1);
pos[0].z=(float)atan2(update.so2().unit_complex().imag(),update.so2().unit_complex().real()); pos[0].z=(float)atan2(update.so2().unit_complex().imag(),update.so2().unit_complex().real());
residual_->release(1,2); residual_->reload(1,2,1,pos);
residual_->load(1,2,1,pos);
} }
void SparseImgAlignGpu::startIteration() void SparseImgAlignGpu::startIteration()