From 2b6a1461bbb2992204f9517058d459fdde77be59 Mon Sep 17 00:00:00 2001 From: gfuse Date: Sat, 2 Oct 2021 20:15:40 +0200 Subject: [PATCH] change the triangulation --- GPU_version/vio/depth_filter_log.txt | 168 ---- GPU_version/vio/frame_handler_log.txt | 1105 ++------------------- GPU_version/vio/include/vio/matcher.h | 5 +- GPU_version/vio/src/feature_alignment.cpp | 6 +- GPU_version/vio/src/matcher.cpp | 7 +- GPU_version/vio/src/math_utils.cpp | 24 +- GPU_version/vio/src/reprojector.cpp | 1 + 7 files changed, 113 insertions(+), 1203 deletions(-) diff --git a/GPU_version/vio/depth_filter_log.txt b/GPU_version/vio/depth_filter_log.txt index 01814b6..e69de29 100755 --- a/GPU_version/vio/depth_filter_log.txt +++ b/GPU_version/vio/depth_filter_log.txt @@ -1,168 +0,0 @@ -[14:54:18.504] New key frame -[14:54:18.504] BA loop run -[14:54:18.505] init error: 24.763610 -[14:54:18.506] end error: 0.000054 -[14:54:19.426] New key frame -[14:54:19.426] BA loop run -[14:54:19.426] init error: 12.320394 -[14:54:19.428] end error: 0.002815 -[14:54:19.527] New key frame -[14:54:19.527] BA loop run -[14:54:19.528] init error: 323.515875 -[14:54:19.529] end error: 0.003837 -[14:54:19.645] New key frame -[14:54:19.645] BA loop run -[14:54:19.645] init error: 2.917523 -[14:54:19.647] end error: 0.000002 -[14:54:19.756] New key frame -[14:54:19.756] BA loop run -[14:54:19.756] init error: 2.562036 -[14:54:19.758] end error: 0.041513 -[14:54:19.866] New key frame -[14:54:19.866] BA loop run -[14:54:19.867] init error: 1910.325804 -[14:54:19.867] end error: 0.000167 -[14:54:19.963] New key frame -[14:54:19.963] BA loop run -[14:54:19.963] init error: 289.670025 -[14:54:19.964] end error: 0.000167 -[14:54:20.055] New key frame -[14:54:20.055] BA loop run -[14:54:20.055] init error: 194.073707 -[14:54:20.060] end error: 0.074585 -[14:54:20.501] New key frame -[14:54:20.501] BA loop run -[14:54:20.501] init error: 4052.719150 -[14:54:20.504] end error: 0.478617 -[14:54:21.096] New key frame -[14:54:21.096] BA loop run -[14:54:21.097] init error: 597.219624 -[14:54:21.101] end error: 0.836308 -[14:54:21.315] New key frame -[14:54:21.315] BA loop run -[14:54:21.316] init error: 6935.758919 -[14:54:21.322] end error: 3.265738 -[14:54:21.763] New key frame -[14:54:21.763] BA loop run -[14:54:21.764] init error: 161.973034 -[14:54:21.975] end error: 2.271218 -[14:54:22.267] New key frame -[14:54:22.267] BA loop run -[14:54:22.268] init error: 95.065037 -[14:54:22.391] end error: 2.446271 -[14:54:22.941] New key frame -[14:54:22.941] BA loop run -[14:54:22.943] init error: 4603.510437 -[14:54:22.951] end error: 2.320479 -[14:54:24.537] New key frame -[14:54:24.537] BA loop run -[14:54:24.540] init error: 2029.462857 -[14:54:24.722] end error: 4.087532 -[14:54:25.356] New key frame -[14:54:25.356] BA loop run -[14:54:25.358] init error: 75690.392815 -[14:54:25.518] end error: 5.770364 -[14:54:25.673] New key frame -[14:54:25.673] BA loop run -[14:54:25.674] init error: 15240.984229 -[14:54:26.122] end error: 4.056428 -[14:54:26.123] New key frame -[14:54:26.123] BA loop run -[14:54:26.127] init error: 386.422521 -[14:54:26.710] end error: 3.468697 -[14:54:27.018] New key frame -[14:54:27.018] BA loop run -[14:54:27.020] init error: 2927.060707 -[14:54:27.234] end error: 3.722009 -[14:54:27.501] New key frame -[14:54:27.501] BA loop run -[14:54:27.503] init error: 68846.950097 -[14:54:27.510] end error: 3.166685 -[14:54:27.930] New key frame -[14:54:27.930] BA loop run -[14:54:27.932] init error: 13881.061617 -[14:54:27.997] end error: 8.823857 -[14:54:28.144] New key frame -[14:54:28.144] BA loop run -[14:54:28.147] init error: 3859.481635 -[14:54:28.329] end error: 8.667055 -[14:54:28.590] New key frame -[14:54:28.590] BA loop run -[14:54:28.592] init error: 4412.399485 -[14:54:28.992] end error: 7.702882 -[14:54:28.994] New key frame -[14:54:28.994] BA loop run -[14:54:28.998] init error: 2083.696145 -[14:54:29.013] end error: 11.568292 -[14:54:29.168] New key frame -[14:54:29.168] BA loop run -[14:54:29.171] init error: 5318.544608 -[14:54:29.355] end error: 80.936368 -[14:54:29.366] New key frame -[14:54:29.366] BA loop run -[14:54:29.370] init error: 518178.847730 -[14:54:29.381] end error: 234.161606 -[14:54:29.759] New key frame -[14:54:29.759] BA loop run -[14:54:29.761] init error: 122235.358731 -[14:54:29.769] end error: 62.169895 -[14:54:30.253] New key frame -[14:54:30.253] BA loop run -[14:54:30.255] init error: 1847824.989597 -[14:54:30.262] end error: 75.517423 -[14:54:31.363] New key frame -[14:54:31.363] BA loop run -[14:54:31.365] init error: 43049.124812 -[14:54:31.372] end error: 45.707574 -[14:54:33.254] New key frame -[14:54:33.254] BA loop run -[14:54:33.256] init error: 332144.508586 -[14:54:33.262] end error: 24.205326 -[14:54:40.425] New key frame -[14:54:40.425] BA loop run -[14:54:40.427] init error: 8015.415519 -[14:54:40.435] end error: 32.395425 -[14:54:40.540] New key frame -[14:54:40.540] BA loop run -[14:54:40.542] init error: 12811.599311 -[14:54:40.552] end error: 21.742364 -[14:54:41.772] New key frame -[14:54:41.772] BA loop run -[14:54:41.775] init error: 7821.177608 -[14:54:41.786] end error: 21.977430 -[14:54:41.903] New key frame -[14:54:41.903] BA loop run -[14:54:41.905] init error: 78.574760 -[14:54:42.003] end error: 9.756280 -[14:54:42.173] New key frame -[14:54:42.173] BA loop run -[14:54:42.176] init error: 955.246709 -[14:54:42.717] end error: 16.657278 -[14:54:42.722] New key frame -[14:54:42.727] BA loop run -[14:54:42.733] init error: 41010.712911 -[14:54:43.336] end error: 85.405648 -[14:54:43.340] New key frame -[14:54:43.340] BA loop run -[14:54:43.345] init error: 75196195.162744 -[14:54:43.675] end error: 462.211381 -[14:54:43.692] New key frame -[14:54:43.692] BA loop run -[14:54:43.698] init error: 803.385900 -[14:54:44.528] end error: 40.007984 -[14:54:44.740] New key frame -[14:54:44.740] BA loop run -[14:54:44.744] init error: 508388.132806 -[14:54:45.066] end error: 85.428830 -[14:54:45.068] New key frame -[14:54:45.068] BA loop run -[14:54:45.075] init error: 50137.336823 -[14:54:45.344] end error: 39.905726 -[14:54:45.346] New key frame -[14:54:45.349] BA loop run -[14:54:45.366] init error: 21223.109285 -[14:54:46.055] end error: 52.832861 -[14:54:46.058] New key frame -[14:54:46.058] BA loop run -[14:54:46.064] init error: 907829.961200 -[14:54:46.734] end error: 86.036304 diff --git a/GPU_version/vio/frame_handler_log.txt b/GPU_version/vio/frame_handler_log.txt index 89b95f0..dc690e1 100755 --- a/GPU_version/vio/frame_handler_log.txt +++ b/GPU_version/vio/frame_handler_log.txt @@ -1,1019 +1,86 @@ -[14:54:17.017] frame process finished the id is: 0 the obs is:0 -[14:54:17.105] frame process finished the id is: 1 the obs is:0 -[14:54:17.205] Init: frame zero: 0.000000, 0.000000 0.000000 -[14:54:17.205] Init: Selected first frame. -[14:54:17.205] frame process finished the id is: 2 the obs is:0 -[14:54:17.240] Init: px average disparity is:0.152753 ,While minimum is: 10.000000 KLT tracked : 109 -[14:54:17.240] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[14:54:17.240] frame process finished the id is: 3 the obs is:0 -[14:54:17.344] Init: px average disparity is:0.221920 ,While minimum is: 10.000000 KLT tracked : 109 -[14:54:17.344] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[14:54:17.344] frame process finished the id is: 4 the obs is:0 -[14:54:17.438] Init: px average disparity is:0.152235 ,While minimum is: 10.000000 KLT tracked : 109 -[14:54:17.438] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[14:54:17.438] frame process finished the id is: 5 the obs is:0 -[14:54:17.545] Init: px average disparity is:0.197958 ,While minimum is: 10.000000 KLT tracked : 109 -[14:54:17.545] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[14:54:17.545] frame process finished the id is: 6 the obs is:0 -[14:54:17.638] Init: px average disparity is:0.221111 ,While minimum is: 10.000000 KLT tracked : 109 -[14:54:17.638] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[14:54:17.638] frame process finished the id is: 7 the obs is:0 -[14:54:17.746] Init: px average disparity is:0.224726 ,While minimum is: 10.000000 KLT tracked : 109 -[14:54:17.746] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[14:54:17.746] frame process finished the id is: 8 the obs is:0 -[14:54:17.840] Init: px average disparity is:0.158653 ,While minimum is: 10.000000 KLT tracked : 109 -[14:54:17.840] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[14:54:17.840] frame process finished the id is: 9 the obs is:0 -[14:54:17.936] Odometry x=0.000000, y=0.000000, theta=-0.000000 -[14:54:17.950] Init: px average disparity is:0.179287 ,While minimum is: 10.000000 KLT tracked : 109 -[14:54:17.951] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[14:54:17.951] frame process finished the id is: 10 the obs is:0 -[14:54:18.028] Odometry x=0.000001, y=0.000003, theta=-0.033384 -[14:54:18.039] Init: px average disparity is:0.148421 ,While minimum is: 10.000000 KLT tracked : 109 -[14:54:18.039] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[14:54:18.039] frame process finished the id is: 11 the obs is:0 -[14:54:18.126] Odometry x=0.000002, y=0.000007, theta=-0.059169 -[14:54:18.146] Init: px average disparity is:0.137330 ,While minimum is: 10.000000 KLT tracked : 109 -[14:54:18.146] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[14:54:18.146] frame process finished the id is: 12 the obs is:0 -[14:54:18.229] Odometry x=0.000009, y=0.000010, theta=-0.083266 -[14:54:18.239] Init: px average disparity is:4.945143 ,While minimum is: 10.000000 KLT tracked : 109 -[14:54:18.239] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[14:54:18.239] frame process finished the id is: 13 the obs is:0 -[14:54:18.327] Odometry x=0.000004, y=0.000024, theta=-0.090806 -[14:54:18.420] Init: Homography RANSAC (inlier) is:0 ,While 30 inliers minimum required. px average disparity is:11.912397 ,While minimum is: 10.000000 KLT tracked: 109 -[14:54:18.420] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[14:54:18.420] frame process finished the id is: 14 the obs is:0 -[14:54:18.430] Odometry x=0.000013, y=0.000033, theta=-0.115987 -[14:54:18.504] Init finished: Homography RANSAC (inlier) is:46 ,While 30 inliers minimum required. px average disparity is:19.121424 ,While minimum is: 10.000000 KLT tracked: 109 -[14:54:18.504] Init: distance between the first and current frame is x:0.000053 ,z=0.047140,angle between two frames: -0.019482 -[14:54:18.504] Init: Selected Second frame. The number of features: 46 depth mean:0.000000 min:179769313486231570814527423731704356798070567525844996598917476803157260780028538760589558632766878171540458953514382464234321326889464182768467546703537516986049910576551282076245490090389328944075868508455133942304583236903222948165808559332123348274797826204144723168738177180919299881250404026184124858368.000000 -[14:54:18.504] frame process finished the id is: 15 the obs is:46 -[14:54:18.527] Odometry x=0.000053, y=0.047140, theta=-0.019482 -[14:54:18.614] After Reprojection Map nPoint:44 ,nCell:25 ,nMatches:12 ,distance between ekf and vo x:0.000222 ,z=-0.000068,angle between two frames:0.010889 -[14:54:18.614] n deleted obs = 22 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =22 error init =0.026117 error end=0.026117 -[14:54:18.614] After pose optimization, distance between ekf and vo x:0.000222 ,z=-0.000068,angle between two frames:0.010889 -[14:54:18.614] Update EKF and 3D points the number of feature in the new frame: 22 and number of obs: 22 -[14:54:18.614] need key frame pitch dis: 0.034566 translation dif:0.047084 -[14:54:18.614] frame process finished the id is: 16 the obs is:22 -[14:54:18.629] Odometry x=0.000247, y=0.047084, theta=-0.034566 -[14:54:18.712] After Reprojection Map nPoint:21 ,nCell:12 ,nMatches:6 ,distance between ekf and vo x:-0.000021 ,z=0.000026,angle between two frames:0.000047 -[14:54:18.712] n deleted obs = 9 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =12 error init =0.007593 error end=0.007593 -[14:54:18.712] After pose optimization, distance between ekf and vo x:-0.000021 ,z=0.000026,angle between two frames:0.000047 -[14:54:18.712] Update EKF and 3D points the number of feature in the new frame: 12 and number of obs: 12 -[14:54:18.712] need key frame pitch dis: 0.040527 translation dif:0.047113 -[14:54:18.712] frame process finished the id is: 17 the obs is:12 -[14:54:18.732] Odometry x=0.000227, y=0.047113, theta=-0.040527 -[14:54:18.823] After Reprojection Map nPoint:12 ,nCell:8 ,nMatches:4 ,distance between ekf and vo x:-0.000020 ,z=0.000050,angle between two frames:0.001460 -[14:54:18.823] n deleted obs = 10 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =2 error init =0.078066 error end=0.078066 -[14:54:18.823] After pose optimization, distance between ekf and vo x:-0.000020 ,z=0.000050,angle between two frames:0.001460 -[14:54:18.823] frame process finished the id is: 17 the obs is:12 -[14:54:18.828] Odometry x=0.000236, y=0.047120, theta=-0.076638 -[14:54:18.927] Odometry x=0.000240, y=0.047123, theta=-0.106932 -[14:54:18.929] After Reprojection Map nPoint:12 ,nCell:10 ,nMatches:4 ,distance between ekf and vo x:-0.000007 ,z=0.000075,angle between two frames:0.001121 -[14:54:18.929] n deleted obs = 1 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =11 error init =0.107885 error end=0.107885 -[14:54:18.929] After pose optimization, distance between ekf and vo x:-0.000007 ,z=0.000075,angle between two frames:0.001121 -[14:54:18.929] Update EKF and 3D points the number of feature in the new frame: 11 and number of obs: 11 -[14:54:18.929] need key frame pitch dis: 0.095284 translation dif:0.047175 -[14:54:18.929] frame process finished the id is: 19 the obs is:11 -[14:54:19.015] After Reprojection Map nPoint:11 ,nCell:8 ,nMatches:4 ,distance between ekf and vo x:0.000001 ,z=0.000017,angle between two frames:0.000460 -[14:54:19.015] n deleted obs = 2 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =9 error init =0.024415 error end=0.024415 -[14:54:19.015] After pose optimization, distance between ekf and vo x:0.000001 ,z=0.000017,angle between two frames:0.000460 -[14:54:19.015] frame process finished the id is: 19 the obs is:11 -[14:54:19.029] Odometry x=0.000249, y=0.047148, theta=-0.136488 -[14:54:19.113] After Reprojection Map nPoint:11 ,nCell:8 ,nMatches:4 ,distance between ekf and vo x:0.000002 ,z=0.000016,angle between two frames:0.000438 -[14:54:19.113] n deleted obs = 4 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =7 error init =0.003593 error end=0.003593 -[14:54:19.113] After pose optimization, distance between ekf and vo x:0.000002 ,z=0.000016,angle between two frames:0.000438 -[14:54:19.113] frame process finished the id is: 19 the obs is:11 -[14:54:19.126] Odometry x=0.000256, y=0.047155, theta=-0.170117 -[14:54:19.219] After Reprojection Map nPoint:11 ,nCell:7 ,nMatches:5 ,distance between ekf and vo x:0.000001 ,z=0.000005,angle between two frames:0.000158 -[14:54:19.219] n deleted obs = 1 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =10 error init =0.005338 error end=0.005338 -[14:54:19.219] After pose optimization, distance between ekf and vo x:0.000001 ,z=0.000005,angle between two frames:0.000158 -[14:54:19.219] Update EKF and 3D points the number of feature in the new frame: 10 and number of obs: 10 -[14:54:19.219] need key frame pitch dis: 0.199333 translation dif:0.047164 -[14:54:19.219] frame process finished the id is: 22 the obs is:10 -[14:54:19.226] Odometry x=0.000263, y=0.047163, theta=-0.199333 -[14:54:19.330] Odometry x=0.000265, y=0.047166, theta=-0.232529 -[14:54:19.332] After Reprojection Map nPoint:10 ,nCell:6 ,nMatches:4 ,distance between ekf and vo x:0.000002 ,z=0.000024,angle between two frames:0.000384 -[14:54:19.332] n deleted obs = 2 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =8 error init =0.006127 error end=0.006127 -[14:54:19.332] After pose optimization, distance between ekf and vo x:0.000002 ,z=0.000024,angle between two frames:0.000384 -[14:54:19.332] frame process finished the id is: 22 the obs is:10 -[14:54:19.426] After Reprojection Map nPoint:10 ,nCell:7 ,nMatches:4 ,distance between ekf and vo x:0.000005 ,z=0.000025,angle between two frames:0.000398 -[14:54:19.426] n deleted obs = 0 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =10 error init =0.000120 error end=0.000120 -[14:54:19.426] After pose optimization, distance between ekf and vo x:0.000005 ,z=0.000025,angle between two frames:0.000398 -[14:54:19.426] Update EKF and 3D points the number of feature in the new frame: 10 and number of obs: 10 -[14:54:19.426] need key frame pitch dis: 0.270087 translation dif:0.047193 -[14:54:19.426] Choose frame as a key frame Scene Depth mean:0.175471 ,depth min:-0.045412 -[14:54:19.426] frame process finished the id is: 24 the obs is:10 -[14:54:19.427] Odometry x=0.000270, y=0.047192, theta=-0.270087 -[14:54:19.526] Odometry x=0.000276, y=0.047200, theta=-0.306402 -[14:54:19.527] After Reprojection Map nPoint:20 ,nCell:9 ,nMatches:3 ,distance between ekf and vo x:-0.000071 ,z=-0.000069,angle between two frames:0.000250 -[14:54:19.527] n deleted obs = 10 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =10 error init =0.316362 error end=0.316362 -[14:54:19.527] After pose optimization, distance between ekf and vo x:-0.000071 ,z=-0.000069,angle between two frames:0.000250 -[14:54:19.527] Update EKF and 3D points the number of feature in the new frame: 10 and number of obs: 10 -[14:54:19.527] need key frame pitch dis: 0.290474 translation dif:0.047147 -[14:54:19.527] Choose frame as a key frame Scene Depth mean:0.002830 ,depth min:-0.063514 -[14:54:19.527] frame process finished the id is: 25 the obs is:10 -[14:54:19.628] Odometry x=0.000258, y=0.047192, theta=-0.333688 -[14:54:19.644] After Reprojection Map nPoint:20 ,nCell:12 ,nMatches:4 ,distance between ekf and vo x:0.000288 ,z=0.000477,angle between two frames:0.000594 -[14:54:19.644] n deleted obs = 7 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =13 error init =0.011332 error end=0.011332 -[14:54:19.644] After pose optimization, distance between ekf and vo x:0.000288 ,z=0.000477,angle between two frames:0.000594 -[14:54:19.644] Update EKF and 3D points the number of feature in the new frame: 13 and number of obs: 13 -[14:54:19.645] need key frame pitch dis: 2.681055 translation dif:0.136111 -[14:54:19.645] Choose frame as a key frame Scene Depth mean:0.006587 ,depth min:-0.039641 -[14:54:19.645] frame process finished the id is: 26 the obs is:13 -[14:54:19.726] Odometry x=0.000326, y=0.047301, theta=-0.354805 -[14:54:19.755] After Reprojection Map nPoint:23 ,nCell:12 ,nMatches:4 ,distance between ekf and vo x:0.000167 ,z=0.000261,angle between two frames:0.001750 -[14:54:19.755] n deleted obs = 4 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =19 error init =0.060890 error end=0.060780 -[14:54:19.755] After pose optimization, distance between ekf and vo x:0.000881 ,z=0.000036,angle between two frames:0.001847 -[14:54:19.755] Update EKF and 3D points the number of feature in the new frame: 19 and number of obs: 19 -[14:54:19.756] need key frame pitch dis: 3.466740 translation dif:0.115690 -[14:54:19.756] Choose frame as a key frame Scene Depth mean:-0.018072 ,depth min:-0.044756 -[14:54:19.756] frame process finished the id is: 27 the obs is:19 -[14:54:19.828] Odometry x=0.000585, y=0.047371, theta=-0.375975 -[14:54:19.866] After Reprojection Map nPoint:27 ,nCell:7 ,nMatches:5 ,distance between ekf and vo x:0.000170 ,z=0.000197,angle between two frames:0.001295 -[14:54:19.866] n deleted obs = 11 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =16 error init =0.018018 error end=0.018018 -[14:54:19.866] After pose optimization, distance between ekf and vo x:0.000170 ,z=0.000197,angle between two frames:0.001295 -[14:54:19.866] Update EKF and 3D points the number of feature in the new frame: 16 and number of obs: 16 -[14:54:19.866] need key frame pitch dis: 3.502776 translation dif:0.067176 -[14:54:19.866] Choose frame as a key frame Scene Depth mean:0.147456 ,depth min:-0.037518 -[14:54:19.866] frame process finished the id is: 28 the obs is:16 -[14:54:19.926] Odometry x=0.000734, y=0.047546, theta=-0.378829 -[14:54:19.962] After Reprojection Map nPoint:34 ,nCell:6 ,nMatches:5 ,distance between ekf and vo x:0.000064 ,z=0.000028,angle between two frames:0.000338 -[14:54:19.963] n deleted obs = 9 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =25 error init =0.258612 error end=0.258612 -[14:54:19.963] After pose optimization, distance between ekf and vo x:0.000064 ,z=0.000028,angle between two frames:0.000338 -[14:54:19.963] Update EKF and 3D points the number of feature in the new frame: 25 and number of obs: 25 -[14:54:19.963] need key frame pitch dis: 3.428077 translation dif:0.103991 -[14:54:19.963] Choose frame as a key frame Scene Depth mean:0.144530 ,depth min:-0.451551 -[14:54:19.963] frame process finished the id is: 29 the obs is:25 -[14:54:20.028] Odometry x=0.000798, y=0.047567, theta=-0.382598 -[14:54:20.052] After Reprojection Map nPoint:47 ,nCell:9 ,nMatches:8 ,distance between ekf and vo x:0.000004 ,z=0.000005,angle between two frames:0.000011 -[14:54:20.054] n deleted obs = 20 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =27 error init =0.038628 error end=0.038239 -[14:54:20.054] After pose optimization, distance between ekf and vo x:-0.000145 ,z=-0.000112,angle between two frames:0.004012 -[14:54:20.054] Update EKF and 3D points the number of feature in the new frame: 27 and number of obs: 27 -[14:54:20.055] need key frame pitch dis: 0.287961 translation dif:0.274969 -[14:54:20.055] Choose frame as a key frame Scene Depth mean:-0.012413 ,depth min:-0.403853 -[14:54:20.055] frame process finished the id is: 30 the obs is:27 -[14:54:20.126] Odometry x=0.000679, y=0.047475, theta=-0.384137 -[14:54:20.162] After Reprojection Map nPoint:71 ,nCell:20 ,nMatches:9 ,distance between ekf and vo x:0.000013 ,z=0.000010,angle between two frames:0.000212 -[14:54:20.162] n deleted obs = 34 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =37 error init =0.157086 error end=0.057609 -[14:54:20.162] After pose optimization, distance between ekf and vo x:0.017944 ,z=-0.033712,angle between two frames:0.771400 -[14:54:20.162] frame process finished the id is: 30 the obs is:27 -[14:54:20.228] Odometry x=0.000682, y=0.047481, theta=-0.422430 -[14:54:20.275] After Reprojection Map nPoint:53 ,nCell:18 ,nMatches:7 ,distance between ekf and vo x:0.000032 ,z=0.000039,angle between two frames:0.000041 -[14:54:20.275] n deleted obs = 18 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =35 error init =0.118154 error end=0.064805 -[14:54:20.275] After pose optimization, distance between ekf and vo x:-0.013412 ,z=-0.017342,angle between two frames:0.398810 -[14:54:20.275] frame process finished the id is: 30 the obs is:27 -[14:54:20.326] Odometry x=0.000684, y=0.047484, theta=-0.459092 -[14:54:20.389] After Reprojection Map nPoint:56 ,nCell:22 ,nMatches:7 ,distance between ekf and vo x:0.000016 ,z=0.000012,angle between two frames:0.000147 -[14:54:20.389] n deleted obs = 24 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =32 error init =0.169727 error end=0.065500 -[14:54:20.389] After pose optimization, distance between ekf and vo x:-0.027245 ,z=-0.014687,angle between two frames:0.498360 -[14:54:20.389] frame process finished the id is: 30 the obs is:27 -[14:54:20.428] Odometry x=0.000699, y=0.047487, theta=-0.489686 -[14:54:20.500] After Reprojection Map nPoint:54 ,nCell:22 ,nMatches:7 ,distance between ekf and vo x:0.000015 ,z=0.000010,angle between two frames:0.000135 -[14:54:20.500] n deleted obs = 7 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =47 error init =0.003153 error end=0.003153 -[14:54:20.500] After pose optimization, distance between ekf and vo x:0.000015 ,z=0.000010,angle between two frames:0.000135 -[14:54:20.500] Update EKF and 3D points the number of feature in the new frame: 47 and number of obs: 47 -[14:54:20.501] need key frame pitch dis: 3.542908 translation dif:0.215977 -[14:54:20.501] Choose frame as a key frame Scene Depth mean:-0.000590 ,depth min:-0.422220 -[14:54:20.501] frame process finished the id is: 34 the obs is:47 -[14:54:20.526] Odometry x=0.000714, y=0.047500, theta=-0.493649 -[14:54:20.595] After Reprojection Map nPoint:82 ,nCell:13 ,nMatches:12 ,distance between ekf and vo x:0.000008 ,z=0.000003,angle between two frames:0.000023 -[14:54:20.595] n deleted obs = 47 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =35 error init =0.439152 error end=0.209551 -[14:54:20.595] After pose optimization, distance between ekf and vo x:-0.003518 ,z=-0.005790,angle between two frames:0.026336 -[14:54:20.595] frame process finished the id is: 34 the obs is:47 -[14:54:20.628] Odometry x=0.000722, y=0.047502, theta=-0.524612 -[14:54:20.692] After Reprojection Map nPoint:61 ,nCell:14 ,nMatches:9 ,distance between ekf and vo x:0.000007 ,z=0.000002,angle between two frames:0.000010 -[14:54:20.695] n deleted obs = 23 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =38 error init =0.012770 error end=0.012115 -[14:54:20.695] After pose optimization, distance between ekf and vo x:-0.001106 ,z=-0.000850,angle between two frames:0.023408 -[14:54:20.695] frame process finished the id is: 34 the obs is:47 -[14:54:20.730] Odometry x=0.000732, y=0.047503, theta=-0.546693 -[14:54:20.792] After Reprojection Map nPoint:88 ,nCell:18 ,nMatches:15 ,distance between ekf and vo x:0.000010 ,z=0.000003,angle between two frames:0.000016 -[14:54:20.793] n deleted obs = 22 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =66 error init =4.818979 error end=0.258196 -[14:54:20.793] After pose optimization, distance between ekf and vo x:2.184906 ,z=1.737040,angle between two frames:2.938534 -[14:54:20.793] frame process finished the id is: 34 the obs is:47 -[14:54:20.828] Odometry x=0.000739, y=0.047507, theta=-0.570371 -[14:54:20.901] After Reprojection Map nPoint:87 ,nCell:13 ,nMatches:12 ,distance between ekf and vo x:0.000006 ,z=0.000001,angle between two frames:0.000009 -[14:54:20.902] n deleted obs = 37 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =50 error init =0.057842 error end=0.055419 -[14:54:20.902] After pose optimization, distance between ekf and vo x:-0.000988 ,z=-0.002408,angle between two frames:0.046339 -[14:54:20.902] frame process finished the id is: 34 the obs is:47 -[14:54:20.926] Odometry x=0.000745, y=0.047510, theta=-0.595427 -[14:54:20.998] After Reprojection Map nPoint:77 ,nCell:16 ,nMatches:13 ,distance between ekf and vo x:0.000006 ,z=0.000001,angle between two frames:0.000005 -[14:54:20.998] n deleted obs = 30 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =47 error init =0.235329 error end=0.057904 -[14:54:20.998] After pose optimization, distance between ekf and vo x:-0.019773 ,z=-0.028402,angle between two frames:0.634446 -[14:54:20.998] frame process finished the id is: 34 the obs is:47 -[14:54:21.028] Odometry x=0.000753, y=0.047511, theta=-0.620309 -[14:54:21.093] After Reprojection Map nPoint:63 ,nCell:12 ,nMatches:9 ,distance between ekf and vo x:0.000005 ,z=0.000001,angle between two frames:0.000002 -[14:54:21.096] n deleted obs = 0 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =63 error init =0.000096 error end=0.000095 -[14:54:21.096] After pose optimization, distance between ekf and vo x:-0.000056 ,z=0.000094,angle between two frames:0.000760 -[14:54:21.096] Update EKF and 3D points the number of feature in the new frame: 63 and number of obs: 63 -[14:54:21.096] need key frame pitch dis: 0.555964 translation dif:0.096206 -[14:54:21.096] Choose frame as a key frame Scene Depth mean:0.052905 ,depth min:-0.410202 -[14:54:21.096] frame process finished the id is: 40 the obs is:63 -[14:54:21.127] Odometry x=0.000708, y=0.047596, theta=-0.624484 -[14:54:21.218] After Reprojection Map nPoint:76 ,nCell:12 ,nMatches:10 ,distance between ekf and vo x:0.000014 ,z=-0.000007,angle between two frames:0.000295 -[14:54:21.219] n deleted obs = 18 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =58 error init =0.073928 error end=0.060493 -[14:54:21.219] After pose optimization, distance between ekf and vo x:-0.006177 ,z=-0.002089,angle between two frames:0.126436 -[14:54:21.219] frame process finished the id is: 40 the obs is:63 -[14:54:21.226] Odometry x=0.000720, y=0.047599, theta=-0.662872 -[14:54:21.314] After Reprojection Map nPoint:86 ,nCell:19 ,nMatches:11 ,distance between ekf and vo x:0.000002 ,z=0.000000,angle between two frames:0.000010 -[14:54:21.315] n deleted obs = 38 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =48 error init =0.058936 error end=0.058936 -[14:54:21.315] After pose optimization, distance between ekf and vo x:0.000002 ,z=0.000000,angle between two frames:0.000010 -[14:54:21.315] Update EKF and 3D points the number of feature in the new frame: 48 and number of obs: 48 -[14:54:21.315] need key frame pitch dis: 0.651414 translation dif:0.039945 -[14:54:21.315] Choose frame as a key frame Scene Depth mean:0.040038 ,depth min:-0.419961 -[14:54:21.315] frame process finished the id is: 42 the obs is:48 -[14:54:21.326] Odometry x=0.000723, y=0.047601, theta=-0.666553 -[14:54:21.423] After Reprojection Map nPoint:108 ,nCell:19 ,nMatches:15 ,distance between ekf and vo x:0.000005 ,z=0.000003,angle between two frames:0.000102 -[14:54:21.425] n deleted obs = 41 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =67 error init =0.288397 error end=0.079066 -[14:54:21.425] After pose optimization, distance between ekf and vo x:-0.005754 ,z=0.007815,angle between two frames:0.421562 -[14:54:21.425] frame process finished the id is: 42 the obs is:48 -[14:54:21.429] Odometry x=0.000724, y=0.047601, theta=-0.698738 -[14:54:21.522] Odometry x=0.000728, y=0.047607, theta=-0.727005 -[14:54:21.540] After Reprojection Map nPoint:116 ,nCell:22 ,nMatches:15 ,distance between ekf and vo x:0.000003 ,z=0.000000,angle between two frames:0.000002 -[14:54:21.545] n deleted obs = 4 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =112 error init =0.000916 error end=0.000613 -[14:54:21.545] After pose optimization, distance between ekf and vo x:-0.000086 ,z=0.000084,angle between two frames:0.050877 -[14:54:21.545] frame process finished the id is: 42 the obs is:48 -[14:54:21.618] Odometry x=0.000728, y=0.047607, theta=-0.762099 -[14:54:21.646] After Reprojection Map nPoint:83 ,nCell:17 ,nMatches:10 ,distance between ekf and vo x:0.000003 ,z=-0.000000,angle between two frames:0.000008 -[14:54:21.646] n deleted obs = 20 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =63 error init =0.213214 error end=0.113779 -[14:54:21.646] After pose optimization, distance between ekf and vo x:-0.020201 ,z=0.018617,angle between two frames:0.551873 -[14:54:21.646] frame process finished the id is: 42 the obs is:48 -[14:54:21.722] Odometry x=0.000739, y=0.047619, theta=-0.791353 -[14:54:21.761] After Reprojection Map nPoint:88 ,nCell:14 ,nMatches:12 ,distance between ekf and vo x:0.000012 ,z=-0.000002,angle between two frames:0.000006 -[14:54:21.762] n deleted obs = 24 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =64 error init =0.003894 error end=0.003894 -[14:54:21.762] After pose optimization, distance between ekf and vo x:0.000012 ,z=-0.000002,angle between two frames:0.000006 -[14:54:21.762] Update EKF and 3D points the number of feature in the new frame: 64 and number of obs: 64 -[14:54:21.763] need key frame pitch dis: 0.860215 translation dif:0.019216 -[14:54:21.763] Choose frame as a key frame Scene Depth mean:0.008543 ,depth min:-0.327669 -[14:54:21.763] frame process finished the id is: 46 the obs is:64 -[14:54:21.820] Odometry x=0.000756, y=0.047622, theta=-0.795728 -[14:54:21.928] Odometry x=0.000759, y=0.047629, theta=-0.833463 -[14:54:21.939] After Reprojection Map nPoint:140 ,nCell:28 ,nMatches:18 ,distance between ekf and vo x:0.000000 ,z=-0.000000,angle between two frames:0.000011 -[14:54:21.977] n deleted obs = 48 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =92 error init =0.069463 error end=0.061306 -[14:54:21.977] After pose optimization, distance between ekf and vo x:-0.004162 ,z=-0.007763,angle between two frames:0.042063 -[14:54:21.977] frame process finished the id is: 46 the obs is:64 -[14:54:22.021] Odometry x=0.000772, y=0.047620, theta=-0.866023 -[14:54:22.119] Odometry x=0.000782, y=0.047613, theta=-0.891817 -[14:54:22.146] After Reprojection Map nPoint:83 ,nCell:16 ,nMatches:11 ,distance between ekf and vo x:0.000002 ,z=-0.000001,angle between two frames:0.000006 -[14:54:22.147] n deleted obs = 3 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =80 error init =0.012436 error end=0.012376 -[14:54:22.147] After pose optimization, distance between ekf and vo x:-0.001511 ,z=-0.000497,angle between two frames:0.043132 -[14:54:22.147] frame process finished the id is: 46 the obs is:64 -[14:54:22.221] Odometry x=0.000790, y=0.047613, theta=-0.921535 -[14:54:22.265] After Reprojection Map nPoint:88 ,nCell:18 ,nMatches:11 ,distance between ekf and vo x:0.000037 ,z=-0.000024,angle between two frames:0.000173 -[14:54:22.265] n deleted obs = 20 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =68 error init =0.007688 error end=0.007688 -[14:54:22.265] After pose optimization, distance between ekf and vo x:0.000037 ,z=-0.000024,angle between two frames:0.000173 -[14:54:22.265] Update EKF and 3D points the number of feature in the new frame: 68 and number of obs: 68 -[14:54:22.267] need key frame pitch dis: 0.959650 translation dif:0.019134 -[14:54:22.267] Choose frame as a key frame Scene Depth mean:0.026508 ,depth min:-0.342568 -[14:54:22.267] frame process finished the id is: 49 the obs is:68 -[14:54:22.320] Odometry x=0.000829, y=0.047592, theta=-0.926551 -[14:54:22.430] Odometry x=0.000832, y=0.047592, theta=-0.966014 -[14:54:22.439] After Reprojection Map nPoint:65 ,nCell:20 ,nMatches:8 ,distance between ekf and vo x:0.000005 ,z=-0.000003,angle between two frames:0.000052 -[14:54:22.439] n deleted obs = 27 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =38 error init =0.243308 error end=0.196810 -[14:54:22.439] After pose optimization, distance between ekf and vo x:-0.004321 ,z=-0.002820,angle between two frames:0.143039 -[14:54:22.439] frame process finished the id is: 49 the obs is:68 -[14:54:22.520] Odometry x=0.000853, y=0.047581, theta=-1.001766 -[14:54:22.593] After Reprojection Map nPoint:76 ,nCell:12 ,nMatches:10 ,distance between ekf and vo x:0.000105 ,z=0.000042,angle between two frames:0.000507 -[14:54:22.593] n deleted obs = 12 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =64 error init =0.047560 error end=0.045176 -[14:54:22.593] After pose optimization, distance between ekf and vo x:-0.003557 ,z=-0.001418,angle between two frames:0.089974 -[14:54:22.593] frame process finished the id is: 49 the obs is:68 -[14:54:22.622] Odometry x=0.000862, y=0.047577, theta=-1.028350 -[14:54:22.711] After Reprojection Map nPoint:70 ,nCell:12 ,nMatches:10 ,distance between ekf and vo x:0.000104 ,z=0.000035,angle between two frames:0.000519 -[14:54:22.712] n deleted obs = 18 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =52 error init =0.086349 error end=0.058874 -[14:54:22.712] After pose optimization, distance between ekf and vo x:-0.037146 ,z=-0.015374,angle between two frames:0.301735 -[14:54:22.712] frame process finished the id is: 49 the obs is:68 -[14:54:22.719] Odometry x=0.000864, y=0.047567, theta=-1.053823 -[14:54:22.821] Odometry x=0.000866, y=0.047563, theta=-1.083592 -[14:54:22.828] After Reprojection Map nPoint:66 ,nCell:16 ,nMatches:8 ,distance between ekf and vo x:0.000094 ,z=0.000027,angle between two frames:0.000499 -[14:54:22.828] n deleted obs = 36 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =30 error init =0.245527 error end=0.169957 -[14:54:22.828] After pose optimization, distance between ekf and vo x:-0.031977 ,z=-0.014198,angle between two frames:0.214066 -[14:54:22.828] frame process finished the id is: 49 the obs is:68 -[14:54:22.919] Odometry x=0.000885, y=0.047564, theta=-1.110055 -[14:54:22.940] After Reprojection Map nPoint:78 ,nCell:17 ,nMatches:9 ,distance between ekf and vo x:0.000094 ,z=0.000071,angle between two frames:0.000646 -[14:54:22.940] n deleted obs = 49 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =29 error init =0.422378 error end=0.422378 -[14:54:22.940] After pose optimization, distance between ekf and vo x:0.000094 ,z=0.000071,angle between two frames:0.000646 -[14:54:22.940] Update EKF and 3D points the number of feature in the new frame: 29 and number of obs: 29 -[14:54:22.941] need key frame pitch dis: 1.004893 translation dif:0.055486 -[14:54:22.941] Choose frame as a key frame Scene Depth mean:0.014962 ,depth min:-0.163052 -[14:54:22.941] frame process finished the id is: 54 the obs is:29 -[14:54:23.021] Odometry x=0.000954, y=0.047624, theta=-1.090550 -[14:54:23.036] After Reprojection Map nPoint:88 ,nCell:18 ,nMatches:14 ,distance between ekf and vo x:0.000001 ,z=-0.000001,angle between two frames:0.000023 -[14:54:23.036] n deleted obs = 46 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =42 error init =0.127944 error end=0.119840 -[14:54:23.036] After pose optimization, distance between ekf and vo x:-0.002627 ,z=-0.001822,angle between two frames:0.059391 -[14:54:23.036] frame process finished the id is: 54 the obs is:29 -[14:54:23.123] Odometry x=0.000955, y=0.047620, theta=-1.118022 -[14:54:23.153] After Reprojection Map nPoint:88 ,nCell:21 ,nMatches:11 ,distance between ekf and vo x:0.000100 ,z=0.000010,angle between two frames:0.000390 -[14:54:23.156] n deleted obs = 33 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =55 error init =0.099820 error end=0.084622 -[14:54:23.156] After pose optimization, distance between ekf and vo x:-0.016136 ,z=0.003825,angle between two frames:0.163446 -[14:54:23.156] frame process finished the id is: 54 the obs is:29 -[14:54:23.219] Odometry x=0.000962, y=0.047619, theta=-1.142759 -[14:54:23.266] After Reprojection Map nPoint:80 ,nCell:17 ,nMatches:9 ,distance between ekf and vo x:0.000092 ,z=-0.000065,angle between two frames:0.000147 -[14:54:23.267] n deleted obs = 25 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =55 error init =0.184607 error end=0.118810 -[14:54:23.267] After pose optimization, distance between ekf and vo x:-0.012197 ,z=-0.001537,angle between two frames:0.164144 -[14:54:23.267] frame process finished the id is: 54 the obs is:29 -[14:54:23.321] Odometry x=0.000966, y=0.047620, theta=-1.171830 -[14:54:23.381] After Reprojection Map nPoint:67 ,nCell:24 ,nMatches:8 ,distance between ekf and vo x:0.000072 ,z=-0.000047,angle between two frames:0.000236 -[14:54:23.384] n deleted obs = 10 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =57 error init =0.008247 error end=0.003512 -[14:54:23.384] After pose optimization, distance between ekf and vo x:0.003000 ,z=-0.003124,angle between two frames:0.090817 -[14:54:23.384] frame process finished the id is: 54 the obs is:29 -[14:54:23.423] Odometry x=0.000966, y=0.047619, theta=-1.206984 -[14:54:23.494] After Reprojection Map nPoint:65 ,nCell:24 ,nMatches:8 ,distance between ekf and vo x:0.000037 ,z=-0.000018,angle between two frames:0.000122 -[14:54:23.496] n deleted obs = 14 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =51 error init =0.002696 error end=0.001924 -[14:54:23.496] After pose optimization, distance between ekf and vo x:-0.001454 ,z=-0.001288,angle between two frames:0.032309 -[14:54:23.496] frame process finished the id is: 54 the obs is:29 -[14:54:23.520] Odometry x=0.000978, y=0.047619, theta=-1.233297 -[14:54:23.599] After Reprojection Map nPoint:70 ,nCell:21 ,nMatches:10 ,distance between ekf and vo x:0.000018 ,z=-0.000007,angle between two frames:0.000105 -[14:54:23.599] n deleted obs = 21 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =49 error init =0.355966 error end=0.027159 -[14:54:23.599] After pose optimization, distance between ekf and vo x:-0.013455 ,z=-0.007557,angle between two frames:0.341697 -[14:54:23.599] frame process finished the id is: 54 the obs is:29 -[14:54:23.623] Odometry x=0.000983, y=0.047612, theta=-1.264552 -[14:54:23.700] After Reprojection Map nPoint:66 ,nCell:19 ,nMatches:9 ,distance between ekf and vo x:0.000017 ,z=0.000001,angle between two frames:0.000075 -[14:54:23.700] n deleted obs = 28 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =38 error init =0.188956 error end=0.092442 -[14:54:23.700] After pose optimization, distance between ekf and vo x:-0.000826 ,z=-0.001610,angle between two frames:0.074292 -[14:54:23.700] frame process finished the id is: 54 the obs is:29 -[14:54:23.720] Odometry x=0.000981, y=0.047610, theta=-1.295400 -[14:54:23.808] After Reprojection Map nPoint:54 ,nCell:10 ,nMatches:9 ,distance between ekf and vo x:0.000044 ,z=-0.000029,angle between two frames:0.000174 -[14:54:23.808] n deleted obs = 8 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =46 error init =0.457182 error end=0.016283 -[14:54:23.808] After pose optimization, distance between ekf and vo x:0.018933 ,z=-0.008310,angle between two frames:0.609622 -[14:54:23.808] frame process finished the id is: 54 the obs is:29 -[14:54:23.822] Odometry x=0.000989, y=0.047599, theta=-1.329081 -[14:54:23.923] Odometry x=0.000995, y=0.047600, theta=-1.355867 -[14:54:23.924] After Reprojection Map nPoint:74 ,nCell:27 ,nMatches:10 ,distance between ekf and vo x:0.000052 ,z=-0.000064,angle between two frames:0.000184 -[14:54:23.927] n deleted obs = 22 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =52 error init =0.353205 error end=0.082966 -[14:54:23.927] After pose optimization, distance between ekf and vo x:-0.039029 ,z=-0.014205,angle between two frames:0.615245 -[14:54:23.927] frame process finished the id is: 54 the obs is:29 -[14:54:24.020] Odometry x=0.001002, y=0.047597, theta=-1.383019 -[14:54:24.039] After Reprojection Map nPoint:75 ,nCell:28 ,nMatches:8 ,distance between ekf and vo x:0.000038 ,z=-0.000049,angle between two frames:0.000169 -[14:54:24.039] n deleted obs = 14 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =61 error init =0.181115 error end=0.025538 -[14:54:24.039] After pose optimization, distance between ekf and vo x:-0.050850 ,z=0.017569,angle between two frames:0.364917 -[14:54:24.039] frame process finished the id is: 54 the obs is:29 -[14:54:24.123] Odometry x=0.001009, y=0.047590, theta=-1.411627 -[14:54:24.151] After Reprojection Map nPoint:72 ,nCell:24 ,nMatches:10 ,distance between ekf and vo x:0.000023 ,z=-0.000036,angle between two frames:0.000183 -[14:54:24.151] n deleted obs = 4 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =68 error init =0.507586 error end=0.072033 -[14:54:24.151] After pose optimization, distance between ekf and vo x:0.305686 ,z=7.517312,angle between two frames:0.111137 -[14:54:24.151] frame process finished the id is: 54 the obs is:29 -[14:54:24.227] Odometry x=0.001014, y=0.047588, theta=-1.444109 -[14:54:24.247] After Reprojection Map nPoint:80 ,nCell:29 ,nMatches:11 ,distance between ekf and vo x:0.000001 ,z=0.000000,angle between two frames:0.000023 -[14:54:24.250] n deleted obs = 8 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =72 error init =2.083615 error end=0.013318 -[14:54:24.250] After pose optimization, distance between ekf and vo x:-1.448216 ,z=1.636250,angle between two frames:1.933260 -[14:54:24.250] frame process finished the id is: 54 the obs is:29 -[14:54:24.323] Odometry x=0.001022, y=0.047589, theta=-1.475826 -[14:54:24.345] After Reprojection Map nPoint:60 ,nCell:27 ,nMatches:7 ,distance between ekf and vo x:0.000001 ,z=-0.000003,angle between two frames:0.000020 -[14:54:24.345] n deleted obs = 9 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =51 error init =0.537776 error end=0.073181 -[14:54:24.345] After pose optimization, distance between ekf and vo x:-0.027975 ,z=0.032087,angle between two frames:3.746553 -[14:54:24.345] frame process finished the id is: 54 the obs is:29 -[14:54:24.426] Odometry x=0.001025, y=0.047592, theta=-1.497506 -[14:54:24.441] After Reprojection Map nPoint:76 ,nCell:31 ,nMatches:10 ,distance between ekf and vo x:0.000001 ,z=-0.000002,angle between two frames:0.000017 -[14:54:24.442] n deleted obs = 18 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =58 error init =2.111368 error end=0.083466 -[14:54:24.442] After pose optimization, distance between ekf and vo x:0.033614 ,z=0.010277,angle between two frames:0.932374 -[14:54:24.442] frame process finished the id is: 54 the obs is:29 -[14:54:24.529] Odometry x=0.001023, y=0.047595, theta=-1.514643 -[14:54:24.536] After Reprojection Map nPoint:56 ,nCell:16 ,nMatches:11 ,distance between ekf and vo x:0.000001 ,z=-0.000002,angle between two frames:0.000017 -[14:54:24.536] n deleted obs = 8 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =48 error init =0.019648 error end=0.019648 -[14:54:24.536] After pose optimization, distance between ekf and vo x:0.000001 ,z=-0.000002,angle between two frames:0.000017 -[14:54:24.536] Update EKF and 3D points the number of feature in the new frame: 48 and number of obs: 48 -[14:54:24.537] need key frame pitch dis: 1.416341 translation dif:0.057128 -[14:54:24.537] Choose frame as a key frame Scene Depth mean:-0.011206 ,depth min:-1.373119 -[14:54:24.537] frame process finished the id is: 69 the obs is:48 -[14:54:24.636] Odometry x=0.001022, y=0.047591, theta=-1.547561 -[14:54:24.707] After Reprojection Map nPoint:78 ,nCell:19 ,nMatches:14 ,distance between ekf and vo x:0.000011 ,z=-0.000041,angle between two frames:0.000960 -[14:54:24.725] Odometry x=0.001031, y=0.047589, theta=-1.578472 -[14:54:24.728] n deleted obs = 4 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =74 error init =0.010865 error end=0.002905 -[14:54:24.728] After pose optimization, distance between ekf and vo x:0.059602 ,z=0.004596,angle between two frames:0.287674 -[14:54:24.728] frame process finished the id is: 69 the obs is:48 -[14:54:24.834] Odometry x=0.001033, y=0.047595, theta=-1.600480 -[14:54:24.922] After Reprojection Map nPoint:83 ,nCell:27 ,nMatches:11 ,distance between ekf and vo x:-0.000001 ,z=-0.000032,angle between two frames:0.000754 -[14:54:24.922] n deleted obs = 24 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =59 error init =0.738940 error end=0.063394 -[14:54:24.922] After pose optimization, distance between ekf and vo x:-48.407113 ,z=150.861675,angle between two frames:0.098758 -[14:54:24.922] frame process finished the id is: 69 the obs is:48 -[14:54:24.923] Odometry x=0.001041, y=0.047575, theta=-1.625701 -[14:54:25.020] Odometry x=0.001027, y=0.047566, theta=-1.658397 -[14:54:25.033] After Reprojection Map nPoint:84 ,nCell:27 ,nMatches:12 ,distance between ekf and vo x:-0.000002 ,z=-0.000016,angle between two frames:0.000397 -[14:54:25.033] n deleted obs = 20 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =64 error init =0.471302 error end=0.062858 -[14:54:25.033] After pose optimization, distance between ekf and vo x:-0.014328 ,z=-0.007226,angle between two frames:0.977569 -[14:54:25.033] frame process finished the id is: 69 the obs is:48 -[14:54:25.120] Odometry x=0.001045, y=0.047551, theta=-1.691794 -[14:54:25.140] After Reprojection Map nPoint:48 ,nCell:20 ,nMatches:11 ,distance between ekf and vo x:-0.000002 ,z=-0.000013,angle between two frames:0.000322 -[14:54:25.140] n deleted obs = 4 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =44 error init =0.006118 error end=0.002280 -[14:54:25.140] After pose optimization, distance between ekf and vo x:0.006023 ,z=0.027901,angle between two frames:0.646537 -[14:54:25.140] frame process finished the id is: 69 the obs is:48 -[14:54:25.222] Odometry x=0.001047, y=0.047543, theta=-1.719184 -[14:54:25.246] After Reprojection Map nPoint:43 ,nCell:17 ,nMatches:8 ,distance between ekf and vo x:-0.000005 ,z=-0.000029,angle between two frames:0.000670 -[14:54:25.246] n deleted obs = 8 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =35 error init =0.001410 error end=0.001294 -[14:54:25.246] After pose optimization, distance between ekf and vo x:0.008548 ,z=-0.004678,angle between two frames:0.025762 -[14:54:25.246] frame process finished the id is: 69 the obs is:48 -[14:54:25.326] Odometry x=0.001049, y=0.047548, theta=-1.747682 -[14:54:25.353] After Reprojection Map nPoint:47 ,nCell:16 ,nMatches:9 ,distance between ekf and vo x:-0.000005 ,z=-0.000027,angle between two frames:0.000652 -[14:54:25.355] n deleted obs = 1 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =46 error init =0.000238 error end=0.000238 -[14:54:25.355] After pose optimization, distance between ekf and vo x:0.000126 ,z=0.000168,angle between two frames:0.003305 -[14:54:25.355] Update EKF and 3D points the number of feature in the new frame: 46 and number of obs: 46 -[14:54:25.356] need key frame pitch dis: 1.723633 translation dif:0.047702 -[14:54:25.356] Choose frame as a key frame Scene Depth mean:-0.021977 ,depth min:-1.512767 -[14:54:25.356] frame process finished the id is: 75 the obs is:46 -[14:54:25.420] Odometry x=0.001159, y=0.047688, theta=-1.723633 -[14:54:25.522] Odometry x=0.001160, y=0.047679, theta=-1.737890 -[14:54:25.628] Odometry x=0.001161, y=0.047677, theta=-1.777664 -[14:54:25.672] After Reprojection Map nPoint:46 ,nCell:19 ,nMatches:11 ,distance between ekf and vo x:0.000002 ,z=-0.000007,angle between two frames:0.000142 -[14:54:25.672] n deleted obs = 18 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =28 error init =0.048629 error end=0.048629 -[14:54:25.672] After pose optimization, distance between ekf and vo x:0.000002 ,z=-0.000007,angle between two frames:0.000142 -[14:54:25.672] Update EKF and 3D points the number of feature in the new frame: 28 and number of obs: 28 -[14:54:25.673] need key frame pitch dis: 1.734669 translation dif:0.047687 -[14:54:25.673] Choose frame as a key frame Scene Depth mean:0.081418 ,depth min:-0.122368 -[14:54:25.673] frame process finished the id is: 76 the obs is:28 -[14:54:25.720] Odometry x=0.001165, y=0.047673, theta=-1.734669 -[14:54:25.821] Odometry x=0.001170, y=0.047666, theta=-1.771591 -[14:54:25.844] After Reprojection Map nPoint:28 ,nCell:11 ,nMatches:7 ,distance between ekf and vo x:0.000000 ,z=0.000000,angle between two frames:0.000002 -[14:54:25.927] Odometry x=0.001176, y=0.047651, theta=-1.799563 -[14:54:26.020] Odometry x=0.001174, y=0.047643, theta=-1.830122 -[14:54:26.123] n deleted obs = 11 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =17 error init =0.020427 error end=0.020427 -[14:54:26.123] After pose optimization, distance between ekf and vo x:0.000000 ,z=0.000000,angle between two frames:0.000002 -[14:54:26.123] Update EKF and 3D points the number of feature in the new frame: 17 and number of obs: 17 -[14:54:26.123] need key frame pitch dis: 1.750828 translation dif:0.047674 -[14:54:26.123] Choose frame as a key frame Scene Depth mean:0.124507 ,depth min:-0.120230 -[14:54:26.123] frame process finished the id is: 77 the obs is:17 -[14:54:26.127] Odometry x=0.001172, y=0.047659, theta=-1.750828 -[14:54:26.219] Odometry x=0.001175, y=0.047651, theta=-1.762124 -[14:54:26.326] Odometry x=0.001173, y=0.047661, theta=-1.779055 -[14:54:26.420] Odometry x=0.001166, y=0.047659, theta=-1.811895 -[14:54:26.521] Odometry x=0.001175, y=0.047667, theta=-1.844874 -[14:54:26.536] After Reprojection Map nPoint:17 ,nCell:10 ,nMatches:4 ,distance between ekf and vo x:-0.000007 ,z=-0.000007,angle between two frames:0.000021 -[14:54:26.626] Odometry x=0.001182, y=0.047669, theta=-1.878139 -[14:54:26.712] n deleted obs = 10 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =7 error init =0.329574 error end=0.193797 -[14:54:26.712] After pose optimization, distance between ekf and vo x:0.088125 ,z=0.020621,angle between two frames:0.081829 -[14:54:26.712] frame process finished the id is: 77 the obs is:17 -[14:54:26.722] Odometry x=0.001188, y=0.047671, theta=-1.909672 -[14:54:26.821] Odometry x=0.001193, y=0.047660, theta=-1.938541 -[14:54:26.906] After Reprojection Map nPoint:15 ,nCell:5 ,nMatches:3 ,distance between ekf and vo x:-0.000013 ,z=-0.000015,angle between two frames:0.000298 -[14:54:26.907] n deleted obs = 9 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =6 error init =0.332891 error end=0.332891 -[14:54:26.907] After pose optimization, distance between ekf and vo x:-0.000013 ,z=-0.000015,angle between two frames:0.000298 -[14:54:26.907] frame process finished the id is: 77 the obs is:17 -[14:54:26.930] Odometry x=0.001200, y=0.047649, theta=-1.965791 -[14:54:27.018] After Reprojection Map nPoint:17 ,nCell:9 ,nMatches:4 ,distance between ekf and vo x:-0.000013 ,z=-0.000011,angle between two frames:0.000246 -[14:54:27.018] n deleted obs = 7 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =10 error init =0.045446 error end=0.045446 -[14:54:27.018] After pose optimization, distance between ekf and vo x:-0.000013 ,z=-0.000011,angle between two frames:0.000246 -[14:54:27.018] Update EKF and 3D points the number of feature in the new frame: 10 and number of obs: 10 -[14:54:27.018] need key frame pitch dis: 1.969502 translation dif:0.047650 -[14:54:27.018] Choose frame as a key frame Scene Depth mean:0.001381 ,depth min:-0.117121 -[14:54:27.018] frame process finished the id is: 80 the obs is:10 -[14:54:27.023] Odometry x=0.001190, y=0.047635, theta=-1.969502 -[14:54:27.125] Odometry x=0.001190, y=0.047629, theta=-2.003146 -[14:54:27.171] After Reprojection Map nPoint:10 ,nCell:3 ,nMatches:2 ,distance between ekf and vo x:-0.000008 ,z=-0.000010,angle between two frames:0.000226 -[14:54:27.226] Odometry x=0.001201, y=0.047637, theta=-2.035881 -[14:54:27.234] n deleted obs = 3 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =7 error init =0.004489 error end=0.004489 -[14:54:27.234] After pose optimization, distance between ekf and vo x:-0.000008 ,z=-0.000010,angle between two frames:0.000226 -[14:54:27.234] frame process finished the id is: 80 the obs is:10 -[14:54:27.328] Odometry x=0.001198, y=0.047634, theta=-2.066205 -[14:54:27.403] After Reprojection Map nPoint:69 ,nCell:22 ,nMatches:10 ,distance between ekf and vo x:-0.000004 ,z=-0.000007,angle between two frames:0.000130 -[14:54:27.406] n deleted obs = 20 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =49 error init =6.733214 error end=0.056900 -[14:54:27.406] After pose optimization, distance between ekf and vo x:0.029059 ,z=0.067971,angle between two frames:1.642534 -[14:54:27.406] frame process finished the id is: 80 the obs is:10 -[14:54:27.421] Odometry x=0.001202, y=0.047620, theta=-2.101564 -[14:54:27.500] After Reprojection Map nPoint:88 ,nCell:27 ,nMatches:11 ,distance between ekf and vo x:-0.000005 ,z=-0.000005,angle between two frames:0.000103 -[14:54:27.500] n deleted obs = 23 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =65 error init =0.257617 error end=0.257617 -[14:54:27.500] After pose optimization, distance between ekf and vo x:-0.000005 ,z=-0.000005,angle between two frames:0.000103 -[14:54:27.500] Update EKF and 3D points the number of feature in the new frame: 65 and number of obs: 65 -[14:54:27.501] need key frame pitch dis: 2.308604 translation dif:0.058591 -[14:54:27.501] Choose frame as a key frame Scene Depth mean:-0.031922 ,depth min:-0.491207 -[14:54:27.501] frame process finished the id is: 83 the obs is:65 -[14:54:27.524] Odometry x=0.001200, y=0.047613, theta=-2.105302 -[14:54:27.606] After Reprojection Map nPoint:118 ,nCell:41 ,nMatches:14 ,distance between ekf and vo x:-0.000003 ,z=-0.000005,angle between two frames:0.000090 -[14:54:27.606] n deleted obs = 27 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =91 error init =0.408091 error end=0.383732 -[14:54:27.606] After pose optimization, distance between ekf and vo x:0.006456 ,z=0.003266,angle between two frames:0.064078 -[14:54:27.606] frame process finished the id is: 83 the obs is:65 -[14:54:27.622] Odometry x=0.001198, y=0.047610, theta=-2.139800 -[14:54:27.707] After Reprojection Map nPoint:88 ,nCell:20 ,nMatches:11 ,distance between ekf and vo x:-0.000006 ,z=-0.000009,angle between two frames:0.000101 -[14:54:27.708] n deleted obs = 28 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =60 error init =0.102527 error end=0.081359 -[14:54:27.708] After pose optimization, distance between ekf and vo x:-0.005232 ,z=0.029995,angle between two frames:0.049234 -[14:54:27.708] frame process finished the id is: 83 the obs is:65 -[14:54:27.724] Odometry x=0.001198, y=0.047608, theta=-2.152326 -[14:54:27.821] Odometry x=0.001199, y=0.047594, theta=-2.188937 -[14:54:27.828] After Reprojection Map nPoint:92 ,nCell:23 ,nMatches:12 ,distance between ekf and vo x:-0.000011 ,z=-0.000012,angle between two frames:0.000165 -[14:54:27.829] n deleted obs = 30 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =62 error init =0.071145 error end=0.069853 -[14:54:27.829] After pose optimization, distance between ekf and vo x:-0.000342 ,z=0.003271,angle between two frames:0.026545 -[14:54:27.829] frame process finished the id is: 83 the obs is:65 -[14:54:27.922] Odometry x=0.001206, y=0.047587, theta=-2.221495 -[14:54:27.929] After Reprojection Map nPoint:97 ,nCell:24 ,nMatches:12 ,distance between ekf and vo x:-0.000005 ,z=-0.000005,angle between two frames:0.000031 -[14:54:27.929] n deleted obs = 31 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =66 error init =0.067764 error end=0.067764 -[14:54:27.929] After pose optimization, distance between ekf and vo x:-0.000005 ,z=-0.000005,angle between two frames:0.000031 -[14:54:27.929] Update EKF and 3D points the number of feature in the new frame: 66 and number of obs: 66 -[14:54:27.930] need key frame pitch dis: 0.610108 translation dif:0.049565 -[14:54:27.930] Choose frame as a key frame Scene Depth mean:-0.105666 ,depth min:-0.612095 -[14:54:27.930] frame process finished the id is: 87 the obs is:66 -[14:54:28.024] Odometry x=0.001207, y=0.047583, theta=-2.250535 -[14:54:28.128] Odometry x=0.001210, y=0.047575, theta=-2.284633 -[14:54:28.143] After Reprojection Map nPoint:90 ,nCell:23 ,nMatches:13 ,distance between ekf and vo x:-0.000001 ,z=-0.000001,angle between two frames:0.000021 -[14:54:28.143] n deleted obs = 30 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =60 error init =0.143042 error end=0.143042 -[14:54:28.143] After pose optimization, distance between ekf and vo x:-0.000001 ,z=-0.000001,angle between two frames:0.000021 -[14:54:28.143] Update EKF and 3D points the number of feature in the new frame: 60 and number of obs: 60 -[14:54:28.144] need key frame pitch dis: 2.465840 translation dif:0.029065 -[14:54:28.144] Choose frame as a key frame Scene Depth mean:-0.098015 ,depth min:-0.598322 -[14:54:28.144] frame process finished the id is: 88 the obs is:60 -[14:54:28.223] Odometry x=0.001197, y=0.047584, theta=-2.220803 -[14:54:28.292] After Reprojection Map nPoint:88 ,nCell:29 ,nMatches:15 ,distance between ekf and vo x:-0.000001 ,z=-0.000001,angle between two frames:0.000024 -[14:54:28.322] Odometry x=0.001206, y=0.047579, theta=-2.249204 -[14:54:28.329] n deleted obs = 33 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =55 error init =0.321785 error end=0.147139 -[14:54:28.329] After pose optimization, distance between ekf and vo x:0.035901 ,z=0.426207,angle between two frames:0.216394 -[14:54:28.329] frame process finished the id is: 88 the obs is:60 -[14:54:28.441] Odometry x=0.001206, y=0.047572, theta=-2.281584 -[14:54:28.486] After Reprojection Map nPoint:102 ,nCell:23 ,nMatches:14 ,distance between ekf and vo x:-0.000001 ,z=-0.000000,angle between two frames:0.000004 -[14:54:28.489] n deleted obs = 12 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =90 error init =0.009540 error end=0.003061 -[14:54:28.489] After pose optimization, distance between ekf and vo x:0.035744 ,z=-0.009888,angle between two frames:0.074135 -[14:54:28.489] frame process finished the id is: 88 the obs is:60 -[14:54:28.523] Odometry x=0.001205, y=0.047570, theta=-2.312249 -[14:54:28.588] After Reprojection Map nPoint:96 ,nCell:28 ,nMatches:13 ,distance between ekf and vo x:-0.000001 ,z=-0.000000,angle between two frames:0.000004 -[14:54:28.589] n deleted obs = 24 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =72 error init =0.013000 error end=0.006232 -[14:54:28.589] After pose optimization, distance between ekf and vo x:0.043879 ,z=0.010250,angle between two frames:0.015146 -[14:54:28.589] Update EKF and 3D points the number of feature in the new frame: 72 and number of obs: 72 -[14:54:28.590] need key frame pitch dis: 2.591958 translation dif:0.025186 -[14:54:28.590] Choose frame as a key frame Scene Depth mean:-0.151692 ,depth min:-0.741338 -[14:54:28.590] frame process finished the id is: 91 the obs is:72 -[14:54:28.626] Odometry x=0.039429, y=0.056497, theta=-2.329471 -[14:54:28.728] Odometry x=0.039434, y=0.056492, theta=-2.363172 -[14:54:28.839] Odometry x=0.039431, y=0.056491, theta=-2.394171 -[14:54:28.850] After Reprojection Map nPoint:144 ,nCell:38 ,nMatches:18 ,distance between ekf and vo x:-0.000000 ,z=0.000000,angle between two frames:0.000005 -[14:54:28.921] Odometry x=0.039425, y=0.056489, theta=-2.427420 -[14:54:28.993] n deleted obs = 24 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =120 error init =0.064385 error end=0.064385 -[14:54:28.993] After pose optimization, distance between ekf and vo x:-0.000000 ,z=0.000000,angle between two frames:0.000005 -[14:54:28.993] Update EKF and 3D points the number of feature in the new frame: 120 and number of obs: 120 -[14:54:28.994] need key frame pitch dis: 2.499251 translation dif:0.045672 -[14:54:28.994] Choose frame as a key frame Scene Depth mean:-0.111675 ,depth min:-0.678569 -[14:54:28.994] frame process finished the id is: 92 the obs is:120 -[14:54:29.026] Odometry x=0.039429, y=0.056493, theta=-2.346619 -[14:54:29.122] Odometry x=0.039428, y=0.056488, theta=-2.378301 -[14:54:29.166] After Reprojection Map nPoint:152 ,nCell:23 ,nMatches:20 ,distance between ekf and vo x:-0.000001 ,z=-0.000000,angle between two frames:0.000009 -[14:54:29.167] n deleted obs = 35 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =117 error init =0.018359 error end=0.018359 -[14:54:29.167] After pose optimization, distance between ekf and vo x:-0.000001 ,z=-0.000000,angle between two frames:0.000009 -[14:54:29.167] Update EKF and 3D points the number of feature in the new frame: 117 and number of obs: 117 -[14:54:29.168] need key frame pitch dis: 0.552284 translation dif:0.027051 -[14:54:29.168] Choose frame as a key frame Scene Depth mean:-0.120506 ,depth min:-0.704536 -[14:54:29.168] frame process finished the id is: 93 the obs is:117 -[14:54:29.236] Odometry x=0.039427, y=0.056487, theta=-2.355042 -[14:54:29.323] After Reprojection Map nPoint:123 ,nCell:30 ,nMatches:16 ,distance between ekf and vo x:-0.000001 ,z=0.000000,angle between two frames:0.000002 -[14:54:29.325] Odometry x=0.039420, y=0.056484, theta=-2.389795 -[14:54:29.364] n deleted obs = 47 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =76 error init =0.044433 error end=0.044028 -[14:54:29.364] After pose optimization, distance between ekf and vo x:-0.000762 ,z=-0.000923,angle between two frames:0.004770 -[14:54:29.364] Update EKF and 3D points the number of feature in the new frame: 76 and number of obs: 76 -[14:54:29.365] need key frame pitch dis: 0.547434 translation dif:0.029394 -[14:54:29.365] Choose frame as a key frame Scene Depth mean:-0.186271 ,depth min:-0.703036 -[14:54:29.366] frame process finished the id is: 94 the obs is:76 -[14:54:29.440] Odometry x=0.038768, y=0.055686, theta=-2.359783 -[14:54:29.528] Odometry x=0.038773, y=0.055677, theta=-2.393344 -[14:54:29.545] After Reprojection Map nPoint:146 ,nCell:23 ,nMatches:20 ,distance between ekf and vo x:-0.000003 ,z=-0.000002,angle between two frames:0.000022 -[14:54:29.553] n deleted obs = 45 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =101 error init =0.083092 error end=0.069977 -[14:54:29.553] After pose optimization, distance between ekf and vo x:0.031521 ,z=-0.006957,angle between two frames:0.031697 -[14:54:29.553] frame process finished the id is: 94 the obs is:76 -[14:54:29.626] Odometry x=0.038782, y=0.055668, theta=-2.423746 -[14:54:29.658] After Reprojection Map nPoint:124 ,nCell:20 ,nMatches:15 ,distance between ekf and vo x:-0.000004 ,z=-0.000001,angle between two frames:0.000022 -[14:54:29.663] n deleted obs = 17 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =107 error init =0.057819 error end=0.051210 -[14:54:29.663] After pose optimization, distance between ekf and vo x:0.004866 ,z=-0.001243,angle between two frames:0.033009 -[14:54:29.663] frame process finished the id is: 94 the obs is:76 -[14:54:29.729] Odometry x=0.038782, y=0.055670, theta=-2.452167 -[14:54:29.753] After Reprojection Map nPoint:120 ,nCell:24 ,nMatches:15 ,distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000002 -[14:54:29.758] n deleted obs = 30 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =90 error init =0.042771 error end=0.035853 -[14:54:29.758] After pose optimization, distance between ekf and vo x:0.003829 ,z=-0.003887,angle between two frames:0.007378 -[14:54:29.759] Update EKF and 3D points the number of feature in the new frame: 90 and number of obs: 90 -[14:54:29.759] need key frame pitch dis: 0.761025 translation dif:0.041052 -[14:54:29.759] Choose frame as a key frame Scene Depth mean:-0.082854 ,depth min:-0.804885 -[14:54:29.759] frame process finished the id is: 97 the obs is:90 -[14:54:29.828] Odometry x=0.039759, y=0.054663, theta=-2.489122 -[14:54:29.850] After Reprojection Map nPoint:120 ,nCell:27 ,nMatches:14 ,distance between ekf and vo x:-0.000000 ,z=0.000000,angle between two frames:0.000000 -[14:54:29.854] n deleted obs = 23 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =97 error init =0.027514 error end=0.027487 -[14:54:29.854] After pose optimization, distance between ekf and vo x:0.000647 ,z=0.000144,angle between two frames:0.002521 -[14:54:29.854] Update EKF and 3D points the number of feature in the new frame: 97 and number of obs: 97 -[14:54:29.855] need key frame pitch dis: 0.116496 translation dif:0.005559 -[14:54:29.855] frame process finished the id is: 98 the obs is:97 -[14:54:29.927] Odometry x=0.041912, y=0.053192, theta=-2.476276 -[14:54:29.949] After Reprojection Map nPoint:168 ,nCell:35 ,nMatches:20 ,distance between ekf and vo x:-0.000000 ,z=0.000000,angle between two frames:0.000002 -[14:54:29.950] n deleted obs = 23 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =145 error init =0.033838 error end=0.033838 -[14:54:29.950] After pose optimization, distance between ekf and vo x:-0.000000 ,z=0.000000,angle between two frames:0.000002 -[14:54:29.950] Update EKF and 3D points the number of feature in the new frame: 145 and number of obs: 145 -[14:54:29.950] need key frame pitch dis: 0.122014 translation dif:0.005561 -[14:54:29.950] frame process finished the id is: 99 the obs is:145 -[14:54:30.030] Odometry x=0.041913, y=0.053190, theta=-2.481794 -[14:54:30.064] After Reprojection Map nPoint:144 ,nCell:41 ,nMatches:15 ,distance between ekf and vo x:0.000001 ,z=-0.000000,angle between two frames:0.000000 -[14:54:30.065] n deleted obs = 36 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =108 error init =0.036276 error end=0.036254 -[14:54:30.065] After pose optimization, distance between ekf and vo x:0.000848 ,z=-0.000710,angle between two frames:0.000003 -[14:54:30.065] Update EKF and 3D points the number of feature in the new frame: 108 and number of obs: 108 -[14:54:30.065] need key frame pitch dis: 0.126458 translation dif:0.006460 -[14:54:30.065] frame process finished the id is: 100 the obs is:108 -[14:54:30.121] Odometry x=0.042648, y=0.052576, theta=-2.486237 -[14:54:30.159] After Reprojection Map nPoint:151 ,nCell:49 ,nMatches:16 ,distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:30.159] n deleted obs = 21 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =130 error init =0.004226 error end=0.004226 -[14:54:30.159] After pose optimization, distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:30.159] Update EKF and 3D points the number of feature in the new frame: 130 and number of obs: 130 -[14:54:30.160] need key frame pitch dis: 0.131754 translation dif:0.006466 -[14:54:30.160] frame process finished the id is: 101 the obs is:130 -[14:54:30.223] Odometry x=0.042648, y=0.052570, theta=-2.491534 -[14:54:30.252] After Reprojection Map nPoint:112 ,nCell:47 ,nMatches:11 ,distance between ekf and vo x:-0.000000 ,z=0.000000,angle between two frames:0.000000 -[14:54:30.252] n deleted obs = 20 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =92 error init =0.039951 error end=0.039951 -[14:54:30.252] After pose optimization, distance between ekf and vo x:-0.000000 ,z=0.000000,angle between two frames:0.000000 -[14:54:30.252] Update EKF and 3D points the number of feature in the new frame: 92 and number of obs: 92 -[14:54:30.253] need key frame pitch dis: 0.789714 translation dif:0.041743 -[14:54:30.253] Choose frame as a key frame Scene Depth mean:9.573212 ,depth min:-9.772418 -[14:54:30.253] frame process finished the id is: 102 the obs is:92 -[14:54:30.328] Odometry x=0.042646, y=0.052566, theta=-2.496359 -[14:54:30.350] After Reprojection Map nPoint:120 ,nCell:22 ,nMatches:16 ,distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:30.350] n deleted obs = 27 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =93 error init =0.031294 error end=0.030825 -[14:54:30.350] After pose optimization, distance between ekf and vo x:-0.000700 ,z=0.000268,angle between two frames:0.012595 -[14:54:30.350] Update EKF and 3D points the number of feature in the new frame: 93 and number of obs: 93 -[14:54:30.350] need key frame pitch dis: 0.140315 translation dif:0.006142 -[14:54:30.350] frame process finished the id is: 103 the obs is:93 -[14:54:30.420] Odometry x=0.042524, y=0.052604, theta=-2.529614 -[14:54:30.444] After Reprojection Map nPoint:120 ,nCell:36 ,nMatches:13 ,distance between ekf and vo x:-0.000000 ,z=0.000000,angle between two frames:0.000000 -[14:54:30.444] n deleted obs = 17 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =103 error init =0.004021 error end=0.004021 -[14:54:30.444] After pose optimization, distance between ekf and vo x:-0.000000 ,z=0.000000,angle between two frames:0.000000 -[14:54:30.444] Update EKF and 3D points the number of feature in the new frame: 103 and number of obs: 103 -[14:54:30.445] need key frame pitch dis: 0.012918 translation dif:0.000873 -[14:54:30.445] frame process finished the id is: 104 the obs is:103 -[14:54:30.526] Odometry x=0.042227, y=0.052724, theta=-2.509309 -[14:54:30.554] After Reprojection Map nPoint:142 ,nCell:31 ,nMatches:17 ,distance between ekf and vo x:0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:30.559] n deleted obs = 34 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =108 error init =0.084833 error end=0.084736 -[14:54:30.559] After pose optimization, distance between ekf and vo x:0.000535 ,z=0.000360,angle between two frames:0.000025 -[14:54:30.559] Update EKF and 3D points the number of feature in the new frame: 108 and number of obs: 108 -[14:54:30.560] need key frame pitch dis: 0.025518 translation dif:0.000601 -[14:54:30.560] frame process finished the id is: 105 the obs is:108 -[14:54:30.623] Odometry x=0.042323, y=0.052781, theta=-2.546979 -[14:54:30.664] After Reprojection Map nPoint:140 ,nCell:37 ,nMatches:15 ,distance between ekf and vo x:0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:30.670] n deleted obs = 64 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =76 error init =0.123129 error end=0.121509 -[14:54:30.670] After pose optimization, distance between ekf and vo x:0.000735 ,z=0.000125,angle between two frames:0.000026 -[14:54:30.670] Update EKF and 3D points the number of feature in the new frame: 76 and number of obs: 76 -[14:54:30.670] need key frame pitch dis: 0.055201 translation dif:0.000813 -[14:54:30.670] frame process finished the id is: 106 the obs is:76 -[14:54:30.725] Odometry x=0.042960, y=0.052888, theta=-2.551592 -[14:54:30.764] After Reprojection Map nPoint:126 ,nCell:30 ,nMatches:14 ,distance between ekf and vo x:0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:30.769] n deleted obs = 35 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =91 error init =0.046375 error end=0.045275 -[14:54:30.769] After pose optimization, distance between ekf and vo x:0.002571 ,z=-0.001166,angle between two frames:0.033151 -[14:54:30.769] frame process finished the id is: 106 the obs is:76 -[14:54:30.824] Odometry x=0.042963, y=0.052879, theta=-2.589849 -[14:54:30.862] After Reprojection Map nPoint:137 ,nCell:48 ,nMatches:13 ,distance between ekf and vo x:0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:30.868] n deleted obs = 30 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =107 error init =0.012894 error end=0.012630 -[14:54:30.868] After pose optimization, distance between ekf and vo x:0.000604 ,z=-0.000355,angle between two frames:0.013822 -[14:54:30.868] Update EKF and 3D points the number of feature in the new frame: 107 and number of obs: 107 -[14:54:30.868] need key frame pitch dis: 0.109501 translation dif:0.001396 -[14:54:30.868] frame process finished the id is: 108 the obs is:107 -[14:54:30.921] Odometry x=0.043485, y=0.052569, theta=-2.605892 -[14:54:30.985] After Reprojection Map nPoint:144 ,nCell:43 ,nMatches:15 ,distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:30.992] n deleted obs = 30 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =114 error init =0.018214 error end=0.017846 -[14:54:30.992] After pose optimization, distance between ekf and vo x:0.001310 ,z=0.000570,angle between two frames:0.000130 -[14:54:30.992] Update EKF and 3D points the number of feature in the new frame: 114 and number of obs: 114 -[14:54:30.992] need key frame pitch dis: 0.113497 translation dif:0.002180 -[14:54:30.992] frame process finished the id is: 109 the obs is:114 -[14:54:31.029] Odometry x=0.044619, y=0.053059, theta=-2.609888 -[14:54:31.082] After Reprojection Map nPoint:100 ,nCell:40 ,nMatches:10 ,distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:31.082] n deleted obs = 20 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =80 error init =0.071792 error end=0.071792 -[14:54:31.082] After pose optimization, distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:31.082] Update EKF and 3D points the number of feature in the new frame: 80 and number of obs: 80 -[14:54:31.083] need key frame pitch dis: 0.116992 translation dif:0.002181 -[14:54:31.083] frame process finished the id is: 110 the obs is:80 -[14:54:31.121] Odometry x=0.044620, y=0.053057, theta=-2.613384 -[14:54:31.171] After Reprojection Map nPoint:143 ,nCell:58 ,nMatches:14 ,distance between ekf and vo x:-0.000000 ,z=0.000000,angle between two frames:0.000000 -[14:54:31.172] n deleted obs = 49 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =94 error init =0.006764 error end=0.006764 -[14:54:31.172] After pose optimization, distance between ekf and vo x:-0.000000 ,z=0.000000,angle between two frames:0.000000 -[14:54:31.172] Update EKF and 3D points the number of feature in the new frame: 94 and number of obs: 94 -[14:54:31.172] need key frame pitch dis: 0.119805 translation dif:0.002176 -[14:54:31.172] frame process finished the id is: 111 the obs is:94 -[14:54:31.224] Odometry x=0.044615, y=0.053058, theta=-2.616196 -[14:54:31.260] After Reprojection Map nPoint:166 ,nCell:52 ,nMatches:19 ,distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:31.267] n deleted obs = 58 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =108 error init =0.071006 error end=0.070544 -[14:54:31.267] After pose optimization, distance between ekf and vo x:0.000224 ,z=0.000138,angle between two frames:0.000043 -[14:54:31.267] Update EKF and 3D points the number of feature in the new frame: 108 and number of obs: 108 -[14:54:31.267] need key frame pitch dis: 0.123702 translation dif:0.002340 -[14:54:31.267] frame process finished the id is: 112 the obs is:108 -[14:54:31.324] Odometry x=0.044807, y=0.053178, theta=-2.620093 -[14:54:31.355] After Reprojection Map nPoint:168 ,nCell:60 ,nMatches:17 ,distance between ekf and vo x:-0.000000 ,z=0.000000,angle between two frames:0.000000 -[14:54:31.362] n deleted obs = 42 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =126 error init =0.014118 error end=0.014112 -[14:54:31.362] After pose optimization, distance between ekf and vo x:0.000109 ,z=0.000022,angle between two frames:0.000164 -[14:54:31.362] Update EKF and 3D points the number of feature in the new frame: 126 and number of obs: 126 -[14:54:31.363] need key frame pitch dis: 2.912127 translation dif:0.048203 -[14:54:31.363] Choose frame as a key frame Scene Depth mean:3.115899 ,depth min:-26.720061 -[14:54:31.363] frame process finished the id is: 113 the obs is:126 -[14:54:31.427] Odometry x=0.044905, y=0.053195, theta=-2.623429 -[14:54:31.453] After Reprojection Map nPoint:131 ,nCell:32 ,nMatches:16 ,distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:31.453] n deleted obs = 27 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =104 error init =0.105822 error end=0.105822 -[14:54:31.453] After pose optimization, distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:31.453] Update EKF and 3D points the number of feature in the new frame: 104 and number of obs: 104 -[14:54:31.454] need key frame pitch dis: 0.128106 translation dif:0.021590 -[14:54:31.454] frame process finished the id is: 114 the obs is:104 -[14:54:31.530] Odometry x=0.044909, y=0.053192, theta=-2.629306 -[14:54:31.620] After Reprojection Map nPoint:160 ,nCell:36 ,nMatches:19 ,distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:31.620] n deleted obs = 61 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =99 error init =0.153717 error end=0.153717 -[14:54:31.620] After pose optimization, distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:31.620] Update EKF and 3D points the number of feature in the new frame: 99 and number of obs: 99 -[14:54:31.621] need key frame pitch dis: 0.158431 translation dif:0.021606 -[14:54:31.621] frame process finished the id is: 115 the obs is:99 -[14:54:31.629] Odometry x=0.044907, y=0.053175, theta=-2.659631 -[14:54:31.728] Odometry x=0.044910, y=0.053176, theta=-2.693223 -[14:54:31.828] Odometry x=0.044907, y=0.053170, theta=-2.728989 -[14:54:31.925] Odometry x=0.044898, y=0.053173, theta=-2.760540 -[14:54:32.029] Odometry x=0.044896, y=0.053173, theta=-2.788379 -[14:54:32.126] Odometry x=0.044886, y=0.053171, theta=-2.815796 -[14:54:32.225] Odometry x=0.044885, y=0.053177, theta=-2.846107 -[14:54:32.323] Odometry x=0.044886, y=0.053174, theta=-2.878938 -[14:54:32.424] Odometry x=0.044881, y=0.053174, theta=-2.908021 -[14:54:32.522] Odometry x=0.044873, y=0.053170, theta=-2.935627 -[14:54:32.623] Odometry x=0.044874, y=0.053169, theta=-2.962270 -[14:54:32.719] Odometry x=0.044871, y=0.053166, theta=-2.993586 -[14:54:32.823] Odometry x=0.044868, y=0.053164, theta=-3.022995 -[14:54:32.929] Odometry x=0.044863, y=0.053163, theta=-3.049753 -[14:54:33.027] Odometry x=0.044850, y=0.053170, theta=-3.079482 -[14:54:33.126] Odometry x=0.044850, y=0.053165, theta=-3.110702 -[14:54:33.224] Odometry x=0.044849, y=0.053168, theta=-3.137238 -[14:54:33.250] After Reprojection Map nPoint:88 ,nCell:22 ,nMatches:10 ,distance between ekf and vo x:-0.000001 ,z=-0.000001,angle between two frames:0.000000 -[14:54:33.253] n deleted obs = 9 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =79 error init =0.002324 error end=0.002275 -[14:54:33.253] After pose optimization, distance between ekf and vo x:0.000795 ,z=0.000596,angle between two frames:0.000058 -[14:54:33.253] Update EKF and 3D points the number of feature in the new frame: 79 and number of obs: 79 -[14:54:33.253] need key frame pitch dis: 0.617029 translation dif:0.021236 -[14:54:33.253] Choose frame as a key frame Scene Depth mean:6.759176 ,depth min:-12.329273 -[14:54:33.254] frame process finished the id is: 116 the obs is:79 -[14:54:33.327] Odometry x=0.045544, y=0.053677, theta=-3.118229 -[14:54:33.425] Odometry x=0.045541, y=0.053675, theta=3.135431 -[14:54:33.527] Odometry x=0.045545, y=0.053670, theta=3.103300 -[14:54:33.624] Odometry x=0.045541, y=0.053671, theta=3.072320 -[14:54:33.727] Odometry x=0.045531, y=0.053672, theta=3.044164 -[14:54:33.827] Odometry x=0.045535, y=0.053668, theta=3.015239 -[14:54:33.924] Odometry x=0.045550, y=0.053662, theta=2.984615 -[14:54:34.022] Odometry x=0.045543, y=0.053673, theta=2.958680 -[14:54:34.119] Odometry x=0.045542, y=0.053677, theta=2.926798 -[14:54:34.222] Odometry x=0.045543, y=0.053678, theta=2.899560 -[14:54:34.319] Odometry x=0.045529, y=0.053674, theta=2.872440 -[14:54:34.421] Odometry x=0.045529, y=0.053679, theta=2.844254 -[14:54:34.521] Odometry x=0.045531, y=0.053673, theta=2.813928 -[14:54:34.630] Odometry x=0.045526, y=0.053671, theta=2.784182 -[14:54:34.723] Odometry x=0.045552, y=0.053660, theta=2.758087 -[14:54:34.821] Odometry x=0.045549, y=0.053654, theta=2.734240 -[14:54:34.833] After Reprojection Map nPoint:131 ,nCell:45 ,nMatches:13 ,distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:34.834] n deleted obs = 44 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =87 error init =0.145235 error end=0.108223 -[14:54:34.834] After pose optimization, distance between ekf and vo x:-0.007123 ,z=-0.001190,angle between two frames:0.261589 -[14:54:34.834] frame process finished the id is: 116 the obs is:79 -[14:54:34.923] Odometry x=0.045543, y=0.053654, theta=2.703563 -[14:54:35.022] Odometry x=0.045541, y=0.053655, theta=2.675640 -[14:54:35.122] Odometry x=0.045535, y=0.053649, theta=2.646597 -[14:54:35.220] Odometry x=0.045526, y=0.053645, theta=2.619376 -[14:54:35.320] Odometry x=0.045530, y=0.053642, theta=2.592857 -[14:54:35.425] Odometry x=0.045534, y=0.053642, theta=2.564528 -[14:54:35.523] Odometry x=0.045524, y=0.053641, theta=2.537695 -[14:54:35.623] Odometry x=0.045527, y=0.053632, theta=2.507523 -[14:54:35.723] Odometry x=0.045519, y=0.053635, theta=2.480846 -[14:54:35.735] After Reprojection Map nPoint:116 ,nCell:30 ,nMatches:15 ,distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:35.740] n deleted obs = 28 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =88 error init =0.011653 error end=0.008796 -[14:54:35.740] After pose optimization, distance between ekf and vo x:-0.006322 ,z=-0.000355,angle between two frames:0.110751 -[14:54:35.740] frame process finished the id is: 116 the obs is:79 -[14:54:35.823] Odometry x=0.045512, y=0.053627, theta=2.456107 -[14:54:35.830] After Reprojection Map nPoint:129 ,nCell:38 ,nMatches:14 ,distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:35.834] n deleted obs = 31 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =98 error init =0.180749 error end=0.088691 -[14:54:35.834] After pose optimization, distance between ekf and vo x:-0.006592 ,z=0.005913,angle between two frames:0.596261 -[14:54:35.834] frame process finished the id is: 116 the obs is:79 -[14:54:35.923] After Reprojection Map nPoint:117 ,nCell:30 ,nMatches:13 ,distance between ekf and vo x:-0.000000 ,z=-0.000000,angle between two frames:0.000000 -[14:54:35.924] n deleted obs = 32 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =85 error init =0.076491 error end=0.057805 -[14:54:35.924] After pose optimization, distance between ekf and vo x:0.008662 ,z=0.003556,angle between two frames:0.544563 -[14:54:35.924] frame process finished the id is: 116 the obs is:79 -[14:54:35.926] Odometry x=0.045507, y=0.053626, theta=2.432841 -[14:54:36.029] Odometry x=0.045514, y=0.053622, theta=2.405371 -[14:54:36.127] Odometry x=0.045513, y=0.053615, theta=2.376073 -[14:54:36.226] Odometry x=0.045504, y=0.053613, theta=2.346244 -[14:54:36.326] Odometry x=0.045503, y=0.053614, theta=2.319955 -[14:54:36.424] Odometry x=0.045500, y=0.053617, theta=2.291576 -[14:54:36.522] Odometry x=0.045491, y=0.053621, theta=2.265932 -[14:54:36.632] Odometry x=0.045486, y=0.053628, theta=2.236350 -[14:54:36.727] Odometry x=0.045471, y=0.053620, theta=2.207567 -[14:54:36.822] Odometry x=0.045472, y=0.053613, theta=2.176500 -[14:54:36.932] Odometry x=0.045469, y=0.053609, theta=2.144512 -[14:54:37.027] Odometry x=0.045468, y=0.053625, theta=2.114550 -[14:54:37.127] Odometry x=0.045465, y=0.053622, theta=2.089877 -[14:54:37.227] Odometry x=0.045471, y=0.053619, theta=2.061801 -[14:54:37.326] Odometry x=0.045476, y=0.053624, theta=2.026968 -[14:54:37.423] Odometry x=0.045475, y=0.053621, theta=1.997559 -[14:54:37.530] Odometry x=0.045476, y=0.053627, theta=1.971135 -[14:54:37.624] Odometry x=0.045465, y=0.053624, theta=1.949290 -[14:54:37.726] Odometry x=0.045474, y=0.053616, theta=1.919120 -[14:54:37.831] Odometry x=0.045473, y=0.053617, theta=1.892634 -[14:54:37.924] Odometry x=0.045464, y=0.053614, theta=1.863119 -[14:54:38.026] Odometry x=0.045466, y=0.053612, theta=1.835398 -[14:54:38.132] Odometry x=0.045463, y=0.053626, theta=1.806299 -[14:54:38.224] Odometry x=0.045450, y=0.053622, theta=1.776055 -[14:54:38.327] Odometry x=0.045436, y=0.053621, theta=1.750870 -[14:54:38.423] Odometry x=0.045441, y=0.053627, theta=1.723841 -[14:54:38.526] Odometry x=0.045431, y=0.053623, theta=1.696987 -[14:54:38.623] Odometry x=0.045425, y=0.053624, theta=1.669553 -[14:54:38.721] Odometry x=0.045424, y=0.053624, theta=1.637084 -[14:54:38.823] Odometry x=0.045419, y=0.053627, theta=1.608500 -[14:54:38.924] Odometry x=0.045413, y=0.053638, theta=1.581520 -[14:54:39.027] Odometry x=0.045407, y=0.053631, theta=1.551862 -[14:54:39.125] Odometry x=0.045398, y=0.053625, theta=1.524507 -[14:54:39.227] Odometry x=0.045392, y=0.053624, theta=1.493597 -[14:54:39.324] Odometry x=0.045383, y=0.053619, theta=1.466624 -[14:54:39.422] Odometry x=0.045373, y=0.053618, theta=1.437756 -[14:54:39.520] Odometry x=0.045371, y=0.053616, theta=1.414044 -[14:54:39.623] Odometry x=0.045366, y=0.053615, theta=1.387914 -[14:54:39.726] Odometry x=0.045364, y=0.053620, theta=1.356492 -[14:54:39.829] Odometry x=0.045362, y=0.053637, theta=1.324877 -[14:54:39.922] Odometry x=0.045360, y=0.053645, theta=1.298664 -[14:54:40.031] Odometry x=0.045359, y=0.053655, theta=1.269392 -[14:54:40.123] Odometry x=0.045365, y=0.053657, theta=1.235006 -[14:54:40.226] Odometry x=0.045350, y=0.053668, theta=1.203790 -[14:54:40.329] Odometry x=0.045343, y=0.053667, theta=1.176926 -[14:54:40.425] After Reprojection Map nPoint:71 ,nCell:17 ,nMatches:8 ,distance between ekf and vo x:0.000000 ,z=0.000001,angle between two frames:0.000000 -[14:54:40.425] n deleted obs = 3 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =68 error init =0.000500 error end=0.000500 -[14:54:40.425] After pose optimization, distance between ekf and vo x:0.000000 ,z=0.000001,angle between two frames:0.000000 -[14:54:40.425] Update EKF and 3D points the number of feature in the new frame: 68 and number of obs: 68 -[14:54:40.425] need key frame pitch dis: 0.374707 translation dif:0.070259 -[14:54:40.425] Choose frame as a key frame Scene Depth mean:0.437849 ,depth min:-0.684870 -[14:54:40.425] frame process finished the id is: 121 the obs is:68 -[14:54:40.427] Odometry x=0.045342, y=0.053670, theta=0.374707 -[14:54:40.524] Odometry x=0.045338, y=0.053675, theta=0.341512 -[14:54:40.537] After Reprojection Map nPoint:67 ,nCell:19 ,nMatches:7 ,distance between ekf and vo x:0.000147 ,z=-0.000048,angle between two frames:0.000027 -[14:54:40.539] n deleted obs = 24 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =43 error init =0.014116 error end=0.013852 -[14:54:40.539] After pose optimization, distance between ekf and vo x:-0.001913 ,z=-0.001795,angle between two frames:0.010201 -[14:54:40.539] Update EKF and 3D points the number of feature in the new frame: 43 and number of obs: 43 -[14:54:40.540] need key frame pitch dis: 3.222373 translation dif:0.022891 -[14:54:40.540] Choose frame as a key frame Scene Depth mean:1.536634 ,depth min:-8.770200 -[14:54:40.540] frame process finished the id is: 122 the obs is:43 -[14:54:40.620] Odometry x=0.044909, y=0.053277, theta=0.317021 -[14:54:40.640] After Reprojection Map nPoint:68 ,nCell:19 ,nMatches:9 ,distance between ekf and vo x:0.000018 ,z=-0.000005,angle between two frames:0.000009 -[14:54:40.640] n deleted obs = 24 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =44 error init =0.029721 error end=0.029713 -[14:54:40.640] After pose optimization, distance between ekf and vo x:-0.002592 ,z=-0.000239,angle between two frames:0.029659 -[14:54:40.640] frame process finished the id is: 122 the obs is:43 -[14:54:40.726] Odometry x=0.044908, y=0.053294, theta=0.295002 -[14:54:40.746] After Reprojection Map nPoint:77 ,nCell:23 ,nMatches:10 ,distance between ekf and vo x:0.000144 ,z=0.000030,angle between two frames:0.000072 -[14:54:40.746] n deleted obs = 33 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =44 error init =0.142290 error end=0.136626 -[14:54:40.746] After pose optimization, distance between ekf and vo x:-0.013180 ,z=-0.000053,angle between two frames:0.179748 -[14:54:40.746] frame process finished the id is: 122 the obs is:43 -[14:54:40.827] Odometry x=0.044903, y=0.053304, theta=0.274652 -[14:54:40.853] After Reprojection Map nPoint:67 ,nCell:18 ,nMatches:8 ,distance between ekf and vo x:0.000127 ,z=0.000039,angle between two frames:0.000077 -[14:54:40.853] n deleted obs = 17 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =50 error init =0.442104 error end=0.181812 -[14:54:40.853] After pose optimization, distance between ekf and vo x:-0.096734 ,z=-0.007451,angle between two frames:1.039874 -[14:54:40.853] frame process finished the id is: 122 the obs is:43 -[14:54:40.925] Odometry x=0.044892, y=0.053319, theta=0.247848 -[14:54:40.959] After Reprojection Map nPoint:103 ,nCell:38 ,nMatches:11 ,distance between ekf and vo x:0.000122 ,z=0.000044,angle between two frames:0.000078 -[14:54:40.963] n deleted obs = 24 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =79 error init =0.004307 error end=0.002859 -[14:54:40.963] After pose optimization, distance between ekf and vo x:-0.022293 ,z=-0.016218,angle between two frames:0.063531 -[14:54:40.963] frame process finished the id is: 122 the obs is:43 -[14:54:41.019] Odometry x=0.044890, y=0.053329, theta=0.216701 -[14:54:41.076] After Reprojection Map nPoint:117 ,nCell:47 ,nMatches:12 ,distance between ekf and vo x:0.000109 ,z=0.000113,angle between two frames:0.000097 -[14:54:41.076] n deleted obs = 68 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =49 error init =0.204432 error end=0.159365 -[14:54:41.076] After pose optimization, distance between ekf and vo x:-0.037062 ,z=-0.019382,angle between two frames:0.066556 -[14:54:41.076] frame process finished the id is: 122 the obs is:43 -[14:54:41.119] Odometry x=0.044880, y=0.053340, theta=0.183976 -[14:54:41.183] After Reprojection Map nPoint:120 ,nCell:25 ,nMatches:15 ,distance between ekf and vo x:0.000183 ,z=0.000068,angle between two frames:0.000097 -[14:54:41.184] n deleted obs = 64 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =56 error init =0.464115 error end=0.117819 -[14:54:41.184] After pose optimization, distance between ekf and vo x:-0.653805 ,z=-0.848181,angle between two frames:0.623002 -[14:54:41.184] frame process finished the id is: 122 the obs is:43 -[14:54:41.227] Odometry x=0.044877, y=0.053351, theta=0.149738 -[14:54:41.298] After Reprojection Map nPoint:117 ,nCell:35 ,nMatches:13 ,distance between ekf and vo x:0.000219 ,z=0.000111,angle between two frames:0.000094 -[14:54:41.298] n deleted obs = 64 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =53 error init =0.105545 error end=0.056291 -[14:54:41.298] After pose optimization, distance between ekf and vo x:-0.128619 ,z=-0.205406,angle between two frames:0.162532 -[14:54:41.298] frame process finished the id is: 122 the obs is:43 -[14:54:41.329] Odometry x=0.044866, y=0.053354, theta=0.117719 -[14:54:41.410] After Reprojection Map nPoint:189 ,nCell:67 ,nMatches:20 ,distance between ekf and vo x:0.000281 ,z=-0.000100,angle between two frames:0.000050 -[14:54:41.411] n deleted obs = 83 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =106 error init =0.034076 error end=0.025503 -[14:54:41.411] After pose optimization, distance between ekf and vo x:-0.089173 ,z=0.011269,angle between two frames:1.049364 -[14:54:41.411] frame process finished the id is: 122 the obs is:43 -[14:54:41.420] Odometry x=0.044870, y=0.053361, theta=0.089309 -[14:54:41.523] Odometry x=0.044870, y=0.053371, theta=0.060178 -[14:54:41.524] After Reprojection Map nPoint:192 ,nCell:68 ,nMatches:18 ,distance between ekf and vo x:0.000234 ,z=-0.000136,angle between two frames:0.000024 -[14:54:41.526] n deleted obs = 97 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =95 error init =0.063968 error end=0.023456 -[14:54:41.526] After pose optimization, distance between ekf and vo x:-0.023760 ,z=0.029251,angle between two frames:0.940016 -[14:54:41.526] frame process finished the id is: 122 the obs is:43 -[14:54:41.625] Odometry x=0.044869, y=0.053376, theta=0.029206 -[14:54:41.649] After Reprojection Map nPoint:191 ,nCell:78 ,nMatches:18 ,distance between ekf and vo x:0.000821 ,z=-0.000652,angle between two frames:0.000042 -[14:54:41.651] n deleted obs = 58 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =133 error init =0.038750 error end=0.025445 -[14:54:41.651] After pose optimization, distance between ekf and vo x:-0.058287 ,z=-0.009704,angle between two frames:0.051715 -[14:54:41.651] frame process finished the id is: 122 the obs is:43 -[14:54:41.727] Odometry x=0.044863, y=0.053384, theta=0.003608 -[14:54:41.771] After Reprojection Map nPoint:246 ,nCell:83 ,nMatches:27 ,distance between ekf and vo x:0.001411 ,z=-0.001407,angle between two frames:0.000015 -[14:54:41.771] n deleted obs = 82 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =164 error init =0.042313 error end=0.042313 -[14:54:41.771] After pose optimization, distance between ekf and vo x:0.001411 ,z=-0.001407,angle between two frames:0.000015 -[14:54:41.771] Update EKF and 3D points the number of feature in the new frame: 164 and number of obs: 164 -[14:54:41.772] need key frame pitch dis: 2.934390 translation dif:0.029180 -[14:54:41.772] Choose frame as a key frame Scene Depth mean:-491.670447 ,depth min:-33291.900303 -[14:54:41.772] frame process finished the id is: 133 the obs is:164 -[14:54:41.825] Odometry x=0.046094, y=0.052157, theta=-0.000045 -[14:54:41.901] After Reprojection Map nPoint:233 ,nCell:76 ,nMatches:26 ,distance between ekf and vo x:-0.000000 ,z=0.000000,angle between two frames:0.000000 -[14:54:41.902] n deleted obs = 69 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =164 error init =0.028216 error end=0.015267 -[14:54:41.902] After pose optimization, distance between ekf and vo x:-0.128266 ,z=0.118766,angle between two frames:0.001165 -[14:54:41.902] Update EKF and 3D points the number of feature in the new frame: 164 and number of obs: 164 -[14:54:41.903] need key frame pitch dis: 2.943816 translation dif:0.165737 -[14:54:41.903] Choose frame as a key frame Scene Depth mean:-129.061839 ,depth min:-2401.003030 -[14:54:41.903] frame process finished the id is: 134 the obs is:164 -[14:54:41.934] Odometry x=-0.064751, y=0.154792, theta=-0.006191 -[14:54:42.023] Odometry x=-0.064750, y=0.154791, theta=-0.046954 -[14:54:42.126] Odometry x=-0.064746, y=0.154805, theta=-0.078164 -[14:54:42.172] After Reprojection Map nPoint:271 ,nCell:105 ,nMatches:26 ,distance between ekf and vo x:-0.000006 ,z=-0.000004,angle between two frames:0.000000 -[14:54:42.172] n deleted obs = 98 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =173 error init =0.032251 error end=0.032251 -[14:54:42.172] After pose optimization, distance between ekf and vo x:-0.000006 ,z=-0.000004,angle between two frames:0.000000 -[14:54:42.172] Update EKF and 3D points the number of feature in the new frame: 173 and number of obs: 173 -[14:54:42.173] need key frame pitch dis: 2.954308 translation dif:0.187709 -[14:54:42.173] Choose frame as a key frame Scene Depth mean:0.037531 ,depth min:-8.257975 -[14:54:42.173] frame process finished the id is: 135 the obs is:173 -[14:54:42.225] Odometry x=-0.064752, y=0.154791, theta=-0.019242 -[14:54:42.338] Odometry x=-0.064750, y=0.154794, theta=-0.053861 -[14:54:42.420] Odometry x=-0.064747, y=0.154801, theta=-0.089351 -[14:54:42.469] After Reprojection Map nPoint:310 ,nCell:136 ,nMatches:28 ,distance between ekf and vo x:-0.000118 ,z=0.000052,angle between two frames:0.000009 -[14:54:42.527] Odometry x=-0.064742, y=0.154807, theta=-0.119053 -[14:54:42.626] Odometry x=-0.064747, y=0.154816, theta=-0.149274 -[14:54:42.719] n deleted obs = 105 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =205 error init =0.020032 error end=0.020032 -[14:54:42.719] After pose optimization, distance between ekf and vo x:-0.000118 ,z=0.000052,angle between two frames:0.000009 -[14:54:42.719] Update EKF and 3D points the number of feature in the new frame: 205 and number of obs: 205 -[14:54:42.721] need key frame pitch dis: 2.859229 translation dif:0.294911 -[14:54:42.721] Choose frame as a key frame Scene Depth mean:-58.734308 ,depth min:-2404.180416 -[14:54:42.722] frame process finished the id is: 136 the obs is:205 -[14:54:42.738] Odometry x=-0.064852, y=0.154841, theta=-0.039360 -[14:54:42.825] Odometry x=-0.064853, y=0.154843, theta=-0.069985 -[14:54:42.930] Odometry x=-0.064849, y=0.154849, theta=-0.096038 -[14:54:43.025] Odometry x=-0.064862, y=0.154856, theta=-0.128292 -[14:54:43.123] Odometry x=-0.064857, y=0.154862, theta=-0.157395 -[14:54:43.139] After Reprojection Map nPoint:255 ,nCell:81 ,nMatches:26 ,distance between ekf and vo x:-0.000016 ,z=0.000017,angle between two frames:0.000000 -[14:54:43.226] Odometry x=-0.064850, y=0.154876, theta=-0.184858 -[14:54:43.329] Odometry x=-0.064852, y=0.154871, theta=-0.214001 -[14:54:43.338] n deleted obs = 95 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =160 error init =0.023912 error end=0.023912 -[14:54:43.338] After pose optimization, distance between ekf and vo x:-0.000016 ,z=0.000017,angle between two frames:0.000000 -[14:54:43.338] Update EKF and 3D points the number of feature in the new frame: 160 and number of obs: 160 -[14:54:43.340] need key frame pitch dis: 0.018663 translation dif:0.221880 -[14:54:43.340] Choose frame as a key frame Scene Depth mean:-120.254952 ,depth min:-2403.751247 -[14:54:43.340] frame process finished the id is: 137 the obs is:160 -[14:54:43.423] Odometry x=-0.064838, y=0.154884, theta=-0.198856 -[14:54:43.532] Odometry x=-0.064833, y=0.154881, theta=-0.226717 -[14:54:43.625] Odometry x=-0.064818, y=0.154884, theta=-0.256598 -[14:54:43.691] After Reprojection Map nPoint:239 ,nCell:101 ,nMatches:22 ,distance between ekf and vo x:0.000028 ,z=0.000171,angle between two frames:0.000000 -[14:54:43.691] n deleted obs = 53 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =186 error init =0.044215 error end=0.044215 -[14:54:43.691] After pose optimization, distance between ekf and vo x:0.000028 ,z=0.000171,angle between two frames:0.000000 -[14:54:43.691] Update EKF and 3D points the number of feature in the new frame: 186 and number of obs: 186 -[14:54:43.692] need key frame pitch dis: 0.380819 translation dif:0.053965 -[14:54:43.692] Choose frame as a key frame Scene Depth mean:-206.594480 ,depth min:-2401.183540 -[14:54:43.692] frame process finished the id is: 138 the obs is:186 -[14:54:43.722] Odometry x=-0.064831, y=0.155016, theta=-0.117696 -[14:54:43.820] Odometry x=-0.064825, y=0.155027, theta=-0.154098 -[14:54:43.920] Odometry x=-0.064818, y=0.155024, theta=-0.190504 -[14:54:44.028] Odometry x=-0.064811, y=0.155037, theta=-0.218126 -[14:54:44.116] After Reprojection Map nPoint:256 ,nCell:97 ,nMatches:27 ,distance between ekf and vo x:0.000081 ,z=0.000140,angle between two frames:0.000000 -[14:54:44.125] Odometry x=-0.064808, y=0.155040, theta=-0.244387 -[14:54:44.225] Odometry x=-0.064804, y=0.155045, theta=-0.272506 -[14:54:44.326] Odometry x=-0.064787, y=0.155061, theta=-0.305578 -[14:54:44.426] Odometry x=-0.064782, y=0.155070, theta=-0.335502 -[14:54:44.524] Odometry x=-0.064768, y=0.155076, theta=-0.362165 -[14:54:44.530] n deleted obs = 42 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =214 error init =0.017079 error end=0.016989 -[14:54:44.530] After pose optimization, distance between ekf and vo x:0.000380 ,z=0.001599,angle between two frames:0.000012 -[14:54:44.530] Update EKF and 3D points the number of feature in the new frame: 214 and number of obs: 214 -[14:54:44.532] need key frame pitch dis: 0.162397 translation dif:0.030851 -[14:54:44.532] frame process finished the id is: 139 the obs is:214 -[14:54:44.626] Odometry x=-0.064686, y=0.155488, theta=-0.320391 -[14:54:44.725] Odometry x=-0.064689, y=0.155494, theta=-0.353150 -[14:54:44.739] After Reprojection Map nPoint:190 ,nCell:86 ,nMatches:16 ,distance between ekf and vo x:0.000000 ,z=0.000002,angle between two frames:0.000000 -[14:54:44.739] n deleted obs = 17 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =173 error init =0.017046 error end=0.017046 -[14:54:44.739] After pose optimization, distance between ekf and vo x:0.000000 ,z=0.000002,angle between two frames:0.000000 -[14:54:44.739] Update EKF and 3D points the number of feature in the new frame: 173 and number of obs: 173 -[14:54:44.740] need key frame pitch dis: 0.213247 translation dif:0.030730 -[14:54:44.740] Choose frame as a key frame Scene Depth mean:-109.510734 ,depth min:-2367.466798 -[14:54:44.740] frame process finished the id is: 140 the obs is:173 -[14:54:44.822] Odometry x=-0.064639, y=0.155682, theta=-0.337382 -[14:54:44.923] Odometry x=-0.064630, y=0.155686, theta=-0.364568 -[14:54:45.018] After Reprojection Map nPoint:88 ,nCell:49 ,nMatches:6 ,distance between ekf and vo x:0.000002 ,z=0.000000,angle between two frames:0.000000 -[14:54:45.025] Odometry x=-0.064622, y=0.155670, theta=-0.390590 -[14:54:45.067] n deleted obs = 19 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =69 error init =0.038656 error end=0.038656 -[14:54:45.067] After pose optimization, distance between ekf and vo x:0.000002 ,z=0.000000,angle between two frames:0.000000 -[14:54:45.067] Update EKF and 3D points the number of feature in the new frame: 69 and number of obs: 69 -[14:54:45.068] need key frame pitch dis: 0.226386 translation dif:0.049292 -[14:54:45.068] Choose frame as a key frame Scene Depth mean:0.116715 ,depth min:-1.978781 -[14:54:45.068] frame process finished the id is: 141 the obs is:69 -[14:54:45.127] Odometry x=-0.064574, y=0.155976, theta=-0.262149 -[14:54:45.225] Odometry x=-0.064569, y=0.155977, theta=-0.294846 -[14:54:45.322] Odometry x=-0.064565, y=0.155980, theta=-0.327654 -[14:54:45.340] After Reprojection Map nPoint:125 ,nCell:59 ,nMatches:10 ,distance between ekf and vo x:0.000034 ,z=0.000016,angle between two frames:0.000002 -[14:54:45.345] n deleted obs = 34 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =91 error init =0.096905 error end=0.096905 -[14:54:45.345] After pose optimization, distance between ekf and vo x:0.000034 ,z=0.000016,angle between two frames:0.000002 -[14:54:45.345] Update EKF and 3D points the number of feature in the new frame: 91 and number of obs: 91 -[14:54:45.346] need key frame pitch dis: 2.627618 translation dif:0.195288 -[14:54:45.346] Choose frame as a key frame Scene Depth mean:0.157314 ,depth min:-9.739112 -[14:54:45.346] frame process finished the id is: 142 the obs is:91 -[14:54:45.424] Odometry x=-0.064539, y=0.155991, theta=-0.274442 -[14:54:45.526] Odometry x=-0.064532, y=0.155991, theta=-0.308714 -[14:54:45.656] After Reprojection Map nPoint:168 ,nCell:74 ,nMatches:15 ,distance between ekf and vo x:0.000004 ,z=0.000000,angle between two frames:0.000004 -[14:54:46.057] n deleted obs = 27 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =141 error init =0.015688 error end=0.015688 -[14:54:46.057] After pose optimization, distance between ekf and vo x:0.000004 ,z=0.000000,angle between two frames:0.000004 -[14:54:46.057] Update EKF and 3D points the number of feature in the new frame: 141 and number of obs: 141 -[14:54:46.057] need key frame pitch dis: 0.234688 translation dif:0.092221 -[14:54:46.058] Choose frame as a key frame Scene Depth mean:0.234359 ,depth min:-9.720966 -[14:54:46.058] frame process finished the id is: 143 the obs is:141 -[14:54:46.505] After Reprojection Map nPoint:255 ,nCell:112 ,nMatches:23 ,distance between ekf and vo x:0.000000 ,z=0.000000,angle between two frames:0.000002 -[14:54:46.746] n deleted obs = 60 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =195 error init =0.011132 error end=0.010251 -[14:54:46.746] After pose optimization, distance between ekf and vo x:-0.011279 ,z=0.006137,angle between two frames:0.054483 -[14:54:46.746] frame process finished the id is: 143 the obs is:141 -[14:54:46.979] After Reprojection Map nPoint:272 ,nCell:70 ,nMatches:32 ,distance between ekf and vo x:-0.000013 ,z=-0.000001,angle between two frames:0.000075 -[14:54:46.980] n deleted obs = 45 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =227 error init =0.115634 error end=0.115634 -[14:54:46.980] After pose optimization, distance between ekf and vo x:-0.000013 ,z=-0.000001,angle between two frames:0.000075 -[14:54:46.980] Update EKF and 3D points the number of feature in the new frame: 227 and number of obs: 227 -[14:54:46.981] need key frame pitch dis: 0.180271 translation dif:0.029027 -[14:54:46.981] frame process finished the id is: 145 the obs is:227 -[14:54:47.186] After Reprojection Map nPoint:302 ,nCell:118 ,nMatches:28 ,distance between ekf and vo x:0.000003 ,z=0.000023,angle between two frames:0.000000 -[14:54:47.186] n deleted obs = 24 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =278 error init =0.013832 error end=0.013832 -[14:54:47.186] After pose optimization, distance between ekf and vo x:0.000003 ,z=0.000023,angle between two frames:0.000000 -[14:54:47.186] Update EKF and 3D points the number of feature in the new frame: 278 and number of obs: 278 -[14:54:47.188] need key frame pitch dis: 0.187163 translation dif:0.029027 -[14:54:47.188] frame process finished the id is: 146 the obs is:278 -[14:54:47.391] After Reprojection Map nPoint:314 ,nCell:140 ,nMatches:26 ,distance between ekf and vo x:0.000035 ,z=-0.000006,angle between two frames:0.000000 -[14:54:47.392] n deleted obs = 37 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =277 error init =0.075724 error end=0.075724 -[14:54:47.392] After pose optimization, distance between ekf and vo x:0.000035 ,z=-0.000006,angle between two frames:0.000000 -[14:54:47.392] Update EKF and 3D points the number of feature in the new frame: 277 and number of obs: 277 -[14:54:47.393] need key frame pitch dis: 0.193161 translation dif:0.028993 -[14:54:47.393] frame process finished the id is: 147 the obs is:277 +[19:59:17.117] frame process finished the id is: 0 the obs is:0 +[19:59:17.212] frame process finished the id is: 1 the obs is:0 +[19:59:17.310] Init: frame zero: 0.000000, 0.000000 0.000000 +[19:59:17.310] Init: Selected first frame. +[19:59:17.310] frame process finished the id is: 2 the obs is:0 +[19:59:17.345] Init: px average disparity is:0.152753 ,While minimum is: 10.000000 KLT tracked : 109 +[19:59:17.345] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:17.345] frame process finished the id is: 3 the obs is:0 +[19:59:17.444] Init: px average disparity is:0.221920 ,While minimum is: 10.000000 KLT tracked : 109 +[19:59:17.444] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:17.444] frame process finished the id is: 4 the obs is:0 +[19:59:17.553] Init: px average disparity is:0.152235 ,While minimum is: 10.000000 KLT tracked : 109 +[19:59:17.553] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:17.553] frame process finished the id is: 5 the obs is:0 +[19:59:17.642] Init: px average disparity is:0.197958 ,While minimum is: 10.000000 KLT tracked : 109 +[19:59:17.642] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:17.642] frame process finished the id is: 6 the obs is:0 +[19:59:17.745] Init: px average disparity is:0.221111 ,While minimum is: 10.000000 KLT tracked : 109 +[19:59:17.745] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:17.745] frame process finished the id is: 7 the obs is:0 +[19:59:17.851] Init: px average disparity is:0.224726 ,While minimum is: 10.000000 KLT tracked : 109 +[19:59:17.851] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:17.851] frame process finished the id is: 8 the obs is:0 +[19:59:17.952] Init: px average disparity is:0.158653 ,While minimum is: 10.000000 KLT tracked : 109 +[19:59:17.952] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:17.952] frame process finished the id is: 9 the obs is:0 +[19:59:18.039] Odometry x=0.000000, y=0.000000, theta=-0.000000 +[19:59:18.049] Init: px average disparity is:0.179287 ,While minimum is: 10.000000 KLT tracked : 109 +[19:59:18.049] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:18.049] frame process finished the id is: 10 the obs is:0 +[19:59:18.132] Odometry x=0.000001, y=0.000003, theta=-0.033384 +[19:59:18.149] Init: px average disparity is:0.148421 ,While minimum is: 10.000000 KLT tracked : 109 +[19:59:18.149] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:18.149] frame process finished the id is: 11 the obs is:0 +[19:59:18.230] Odometry x=0.000002, y=0.000007, theta=-0.059169 +[19:59:18.245] Init: px average disparity is:0.137330 ,While minimum is: 10.000000 KLT tracked : 109 +[19:59:18.245] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:18.245] frame process finished the id is: 12 the obs is:0 +[19:59:18.333] Odometry x=0.000009, y=0.000010, theta=-0.083266 +[19:59:18.353] Init: px average disparity is:4.945143 ,While minimum is: 10.000000 KLT tracked : 109 +[19:59:18.353] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:18.353] frame process finished the id is: 13 the obs is:0 +[19:59:18.431] Odometry x=0.000000, y=0.000029, theta=-0.014218 +[19:59:18.519] Init: Homography RANSAC (inlier) is:0 ,While 30 inliers minimum required. px average disparity is:11.912397 ,While minimum is: 10.000000 KLT tracked: 109 +[19:59:18.519] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:18.519] frame process finished the id is: 14 the obs is:0 +[19:59:18.533] Odometry x=0.000006, y=0.000037, theta=-0.041722 +[19:59:18.619] Init: Homography RANSAC (inlier) is:0 ,While 30 inliers minimum required. px average disparity is:19.121424 ,While minimum is: 10.000000 KLT tracked: 109 +[19:59:18.619] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:18.619] frame process finished the id is: 15 the obs is:0 +[19:59:18.631] Odometry x=0.000010, y=0.000047, theta=-0.082338 +[19:59:18.716] Init: Homography RANSAC (inlier) is:1 ,While 30 inliers minimum required. px average disparity is:29.212896 ,While minimum is: 10.000000 KLT tracked: 109 +[19:59:18.716] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:18.716] frame process finished the id is: 16 the obs is:0 +[19:59:18.733] Odometry x=0.000014, y=0.000051, theta=-0.116735 +[19:59:18.819] Init: Homography RANSAC (inlier) is:6 ,While 30 inliers minimum required. px average disparity is:40.866547 ,While minimum is: 10.000000 KLT tracked: 109 +[19:59:18.819] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:18.819] frame process finished the id is: 17 the obs is:0 +[19:59:18.831] Odometry x=0.000012, y=0.000065, theta=-0.150406 +[19:59:18.918] Init: Homography RANSAC (inlier) is:7 ,While 30 inliers minimum required. px average disparity is:51.065260 ,While minimum is: 10.000000 KLT tracked: 109 +[19:59:18.918] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:18.918] frame process finished the id is: 18 the obs is:0 +[19:59:18.932] Odometry x=0.000032, y=0.000065, theta=-0.175625 +[19:59:19.018] Init: Homography RANSAC (inlier) is:9 ,While 30 inliers minimum required. px average disparity is:58.238817 ,While minimum is: 10.000000 KLT tracked: 108 +[19:59:19.018] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:19.018] frame process finished the id is: 19 the obs is:0 +[19:59:19.032] Odometry x=0.000037, y=0.000068, theta=-0.203385 +[19:59:19.118] Init: Homography RANSAC (inlier) is:9 ,While 30 inliers minimum required. px average disparity is:68.778966 ,While minimum is: 10.000000 KLT tracked: 106 +[19:59:19.118] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:19.118] frame process finished the id is: 20 the obs is:0 +[19:59:19.133] Odometry x=0.000050, y=0.000075, theta=-0.234988 +[19:59:19.218] Init: Homography RANSAC (inlier) is:23 ,While 30 inliers minimum required. px average disparity is:79.322258 ,While minimum is: 10.000000 KLT tracked: 105 +[19:59:19.218] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:59:19.218] frame process finished the id is: 21 the obs is:0 +[19:59:19.230] Odometry x=0.000058, y=0.000081, theta=-0.267405 +[19:59:19.318] Init finished: Homography RANSAC (inlier) is:35 ,While 30 inliers minimum required. px average disparity is:90.575691 ,While minimum is: 10.000000 KLT tracked: 105 +[19:59:19.318] Init: distance between the first and current frame is x:0.000062 ,z=0.000083,angle between two frames: -0.296235 +[19:59:19.318] Init: Selected Second frame. The number of features: 35 depth mean:-0.000641 min:-0.001309 +[19:59:19.318] frame process finished the id is: 22 the obs is:35 +[19:59:19.330] Odometry x=0.000062, y=0.000083, theta=-0.296235 +[19:59:19.434] Odometry x=0.000065, y=0.000086, theta=-0.324513 +[19:59:19.531] Odometry x=0.000066, y=0.000090, theta=-0.357137 +[19:59:19.630] Odometry x=0.000075, y=0.000100, theta=-0.387188 +[19:59:19.732] Odometry x=0.000075, y=0.000109, theta=-0.416785 +[19:59:19.830] Odometry x=0.000082, y=0.000111, theta=-0.445093 +[19:59:19.932] Odomet \ No newline at end of file diff --git a/GPU_version/vio/include/vio/matcher.h b/GPU_version/vio/include/vio/matcher.h index 8058e0d..d9e5102 100644 --- a/GPU_version/vio/include/vio/matcher.h +++ b/GPU_version/vio/include/vio/matcher.h @@ -124,7 +124,7 @@ public: void createPatchFromPatchWithBorder(); uint i=0; - void debug(cv::Mat ref,cv::Mat cur, Vector2d ref_px,Vector2d cur_px_in,Vector2d cur_px_out,bool res,uint8_t* patch,uint8_t* patch_border){ + void debug(cv::Mat ref,cv::Mat cur, Vector2d ref_px,Vector2d cur_px_in,Vector2d cur_px_out,bool res,uint8_t* patch,uint8_t* patch_border, double depth){ cv::Mat Ref,Cur,Patch,Patch_b; cv::cvtColor(cv::Mat(patch_size_,patch_size_,CV_8UC1,patch),Patch,cv::COLOR_GRAY2RGB); cv::cvtColor(cv::Mat(patch_size_+2,patch_size_+2,CV_8UC1,patch_border),Patch_b,cv::COLOR_GRAY2RGB); @@ -133,7 +133,8 @@ public: cv::circle(Ref, cv::Point(ref_px.x(),ref_px.y() ), 2, cv::Scalar(0, 0,255),2); cv::circle(Cur, cv::Point(cur_px_in.x(),cur_px_in.y() ), 2, cv::Scalar(0, 0,255),2); cv::circle(Cur, cv::Point(cur_px_out.x(),cur_px_out.y() ), 2, cv::Scalar(0, 255,0),2); - cv::putText(Cur,res? "Accept": "reject",cv::Point(10,10),cv::FONT_HERSHEY_COMPLEX_SMALL,1.0,cv::Scalar(0,255,0),1,cv:: LINE_AA); + cv::putText(Cur,res? "Accept": "reject",cv::Point(10,30),cv::FONT_HERSHEY_COMPLEX_SMALL,1.0,cv::Scalar(0,255,0),1,cv:: LINE_AA); + cv::putText(Cur,std::to_string(depth),cv::Point(100,30),cv::FONT_HERSHEY_COMPLEX_SMALL,1.0,cv::Scalar(0,255,0),1,cv:: LINE_AA); cv::imwrite(std::string(PROJECT_DIR)+"/Ref"+std::to_string(i)+".png",Ref); cv::imwrite(std::string(PROJECT_DIR)+"/Cur"+std::to_string(i)+".png",Cur); cv::imwrite(std::string(PROJECT_DIR)+"/patch"+std::to_string(i)+".png",Patch); diff --git a/GPU_version/vio/src/feature_alignment.cpp b/GPU_version/vio/src/feature_alignment.cpp index ea02d3a..cdb1162 100644 --- a/GPU_version/vio/src/feature_alignment.cpp +++ b/GPU_version/vio/src/feature_alignment.cpp @@ -69,7 +69,7 @@ bool align1D( float v = cur_px_estimate.y(); // termination condition - const float min_update_squared = 0.001; + const float min_update_squared = 1.0; const int cur_step = cur_img.step.p[0]; float chi2 = 0; Vector2f update; update.setZero(); @@ -337,7 +337,7 @@ bool align2D_SSE2( float v = cur_px_estimate.y(); // termination condition - const float min_update_squared = 0.001; + const float min_update_squared = 1.0; const int cur_step = cur_img.step.p[0]; const float Dinv = 1.0f/(A11*A22 - A12*A12); // we are missing an extra factor 16 float chi2 = 0; @@ -502,7 +502,7 @@ bool align2D_NEON ( float v = cur_px_estimate.y(); // termination condition - const float min_update_squared = 0.001; + const float min_update_squared = 1.0; const int cur_step = cur_img.step.p[0]; Vector3f update; Vector3f Jres; diff --git a/GPU_version/vio/src/matcher.cpp b/GPU_version/vio/src/matcher.cpp index 0905e19..852c26d 100644 --- a/GPU_version/vio/src/matcher.cpp +++ b/GPU_version/vio/src/matcher.cpp @@ -41,7 +41,7 @@ void getWarpMatrixAffine( Matrix2d& A_cur_ref) { // Compute affine warp matrix A_ref_cur - const int halfpatch_size = 4; + const int halfpatch_size = 8; const Vector3d xyz_ref(f_ref*depth_ref); Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1<frame->se3(), ref_ftr_->level, A_cur_ref_); //search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1); + /// TODO paches will be mirrored while robot is rotating around it self if(!warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px, ref_ftr_->level, ref_ftr_->level, halfpatch_size_+1, patch_with_border_))return false; createPatchFromPatchWithBorder(); @@ -187,8 +188,8 @@ bool Matcher::findMatchDirect( cur_frame.img_pyr_[ref_ftr_->level], patch_with_border_, patch_, options_.align_max_iter, px_scaled); } -/* debug(ref_ftr_->frame->img_pyr_[ref_ftr_->level],cur_frame.img_pyr_[ref_ftr_->level],ref_ftr_->px, - px_cur,px_scaled ,success,patch_,patch_with_border_);*/ + debug(ref_ftr_->frame->img_pyr_[ref_ftr_->level],cur_frame.img_pyr_[ref_ftr_->level],ref_ftr_->px, + px_cur,px_scaled ,success,patch_,patch_with_border_,(ref_ftr_->frame->se3().inverse()*pt.pos_).norm()); px_cur = px_scaled; return success; } diff --git a/GPU_version/vio/src/math_utils.cpp b/GPU_version/vio/src/math_utils.cpp index 9267e2b..97b6065 100644 --- a/GPU_version/vio/src/math_utils.cpp +++ b/GPU_version/vio/src/math_utils.cpp @@ -19,36 +19,44 @@ Vector3d triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t, const Vector3d& feature1, const Vector3d& feature2 ) { - Vector3d R_F2 = R * feature2;//T*A=R*A+t +/* Vector3d R_F2 = R * feature2;//T*A=R*A+t Vector2d b; - /* + *//* * b=cross(t,f1) * corss(t,Rf2) - * */ + * *//* b[0] = t.dot(feature1); b[1] = t.dot(R_F2);//cross(t,R*f2) Matrix2d A; - /** + *//** * A<< cross(f1,f1), -z * z , -corss(R*f2,R*f2) * * det(A)= -corss(R*f2,R*f2)*cross(f1,f1) + pow(z,2) - */ + *//* A(0,0) = feature1.dot(feature1); A(1,0) = feature1.dot(R_F2);//z A(0,1) = -A(1,0); A(1,1) = -R_F2.dot(R_F2); - /* + *//* * A_inv<< cross(f1,f1)/det(A) , z/det(A) * -z/det(A) , -corss(R*f2,R*f2)/det(A) * * lambda[0]= cross(t,f1)*cross(f1,f1)/det(A) + corss(t,Rf2)*z/det(A) * lambda[1]= -cross(t,f1)*z/det(A) - corss(t,Rf2) * corss(R*f2,R*f2)/det(A) - * */ + * *//* Vector2d lambda = A.inverse() * b; Vector3d xm = lambda[0] * feature1; Vector3d xn = t + lambda[1] * R_F2; - return ( xm + xn )/2; + return ( xm + xn )/2;*/ + Vector3d R_F2 = R * feature2;//T*A=R*A+t + Vector3d z = feature1.cross(R_F2); + Vector3d b0 = t.cross(feature1); + Vector3d b1 = t.cross(R_F2); + + Vector3d xm = ((z.dot(b1)/pow(z.norm(), 2))*(feature1)); + Vector3d xn = t + ((z.dot(b0)/pow(z.norm(), 2))*R_F2); + return ( xm + xn )/2; } bool diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index 1e7b6c9..51fef97 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -179,6 +179,7 @@ namespace vio { break; } } + exit(0); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////