From edc5e8fe9665ffc40912275059c440940db6d5d9 Mon Sep 17 00:00:00 2001 From: gfuse Date: Thu, 30 Sep 2021 15:05:12 +0200 Subject: [PATCH] debug bandel adjustment --- GPU_version/vio/CMakeLists.txt | 2 + GPU_version/vio/depth_filter_log.txt | 2 + GPU_version/vio/frame_handler_log.txt | 1141 ++++++++++++++--- GPU_version/vio/include/vio/frame.h | 2 +- .../vio/include/vio/global_optimizer.h | 17 +- GPU_version/vio/include/vio/point.h | 2 +- GPU_version/vio/src/feature_detection.cpp | 2 + GPU_version/vio/src/global_optimizer.cpp | 80 +- 8 files changed, 994 insertions(+), 254 deletions(-) diff --git a/GPU_version/vio/CMakeLists.txt b/GPU_version/vio/CMakeLists.txt index 9cabae0..b02ad39 100755 --- a/GPU_version/vio/CMakeLists.txt +++ b/GPU_version/vio/CMakeLists.txt @@ -85,5 +85,7 @@ ADD_DEFINITIONS(-DKERNEL_DIR=\"${PROJECT_SOURCE_DIR}/kernel\") ADD_DEFINITIONS(-DPROJECT_DIR=\"${PROJECT_SOURCE_DIR}\") ADD_DEFINITIONS(-DVIO_DEBUG=true) ADD_DEFINITIONS(-DG2O_USE_VENDORED_CERES=ON) +ADD_DEFINITIONS(-DG2O_DELETE_IMPLICITLY_OWNED_OBJECTS=OFF) + diff --git a/GPU_version/vio/depth_filter_log.txt b/GPU_version/vio/depth_filter_log.txt index e69de29..c5f0517 100755 --- a/GPU_version/vio/depth_filter_log.txt +++ b/GPU_version/vio/depth_filter_log.txt @@ -0,0 +1,2 @@ +[15:02:36.165] New key frame +[15:02:36.165] BA loop diff --git a/GPU_version/vio/frame_handler_log.txt b/GPU_version/vio/frame_handler_log.txt index bcca6e2..70685b3 100755 --- a/GPU_version/vio/frame_handler_log.txt +++ b/GPU_version/vio/frame_handler_log.txt @@ -1,212 +1,929 @@ -[18:28:02.505] Init: frame zero: 0.000000, 0.000000 0.000000 -[18:28:02.505] Init: Selected first frame. -[18:28:02.505] frame process finished the id is: 0 the obs is:0 -[18:28:02.519] Init: px average disparity is:0.197090 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:02.519] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:02.519] frame process finished the id is: 1 the obs is:0 -[18:28:02.622] Init: px average disparity is:0.231461 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:02.622] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:02.622] frame process finished the id is: 2 the obs is:0 -[18:28:02.723] Init: px average disparity is:0.249074 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:02.723] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:02.723] frame process finished the id is: 3 the obs is:0 -[18:28:02.825] Init: px average disparity is:0.245644 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:02.825] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:02.825] frame process finished the id is: 4 the obs is:0 -[18:28:02.923] Init: px average disparity is:0.250400 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:02.923] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:02.923] frame process finished the id is: 5 the obs is:0 -[18:28:03.024] Init: px average disparity is:0.234691 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:03.024] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:03.024] frame process finished the id is: 6 the obs is:0 -[18:28:03.127] Init: px average disparity is:0.240224 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:03.127] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:03.127] frame process finished the id is: 7 the obs is:0 -[18:28:03.226] Init: px average disparity is:0.239245 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:03.226] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:03.227] frame process finished the id is: 8 the obs is:0 -[18:28:03.322] Init: px average disparity is:0.267733 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:03.322] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:03.322] frame process finished the id is: 9 the obs is:0 -[18:28:03.422] Init: px average disparity is:0.241786 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:03.422] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:03.422] frame process finished the id is: 10 the obs is:0 -[18:28:03.523] Init: px average disparity is:0.244610 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:03.523] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:03.523] frame process finished the id is: 11 the obs is:0 -[18:28:03.623] Init: px average disparity is:0.259375 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:03.623] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:03.623] frame process finished the id is: 12 the obs is:0 -[18:28:03.723] Init: px average disparity is:0.244493 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:03.723] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:03.723] frame process finished the id is: 13 the obs is:0 -[18:28:03.822] Init: px average disparity is:0.247162 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:03.823] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:03.823] frame process finished the id is: 14 the obs is:0 -[18:28:03.922] Init: px average disparity is:0.235326 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:03.922] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:03.922] frame process finished the id is: 15 the obs is:0 -[18:28:04.022] Init: px average disparity is:0.253053 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:04.022] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:04.022] frame process finished the id is: 16 the obs is:0 -[18:28:04.123] Init: px average disparity is:0.270201 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:04.123] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:04.123] frame process finished the id is: 17 the obs is:0 -[18:28:04.222] Init: px average disparity is:0.251625 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:04.222] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:04.222] frame process finished the id is: 18 the obs is:0 -[18:28:04.323] Init: px average disparity is:0.238201 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:04.323] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:04.323] frame process finished the id is: 19 the obs is:0 -[18:28:04.423] Init: px average disparity is:0.235124 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:04.423] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:04.423] frame process finished the id is: 20 the obs is:0 -[18:28:04.514] Init: px average disparity is:0.264161 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:04.514] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:04.514] frame process finished the id is: 21 the obs is:0 -[18:28:04.614] Init: px average disparity is:0.267489 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:04.614] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:04.614] frame process finished the id is: 22 the obs is:0 -[18:28:04.723] Init: px average disparity is:0.238925 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:04.723] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:04.723] frame process finished the id is: 23 the obs is:0 -[18:28:04.821] Init: px average disparity is:0.238865 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:04.821] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:04.822] frame process finished the id is: 24 the obs is:0 -[18:28:04.912] Init: px average disparity is:0.270427 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:04.912] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:04.913] frame process finished the id is: 25 the obs is:0 -[18:28:05.025] Init: px average disparity is:0.256237 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:05.025] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:05.025] frame process finished the id is: 26 the obs is:0 -[18:28:05.129] Init: px average disparity is:0.216764 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:05.129] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:05.129] frame process finished the id is: 27 the obs is:0 -[18:28:05.216] Init: px average disparity is:0.277987 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:05.216] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:05.216] frame process finished the id is: 28 the obs is:0 -[18:28:05.315] Init: px average disparity is:0.230108 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:05.315] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:05.315] frame process finished the id is: 29 the obs is:0 -[18:28:05.362] Odometry x=0.000000, y=0.000000, theta=-0.000000 -[18:28:05.414] Init: px average disparity is:0.237735 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:05.414] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:05.414] frame process finished the id is: 30 the obs is:0 -[18:28:05.457] Odometry x=0.000000, y=0.000000, theta=-0.000000 -[18:28:05.514] Init: px average disparity is:0.218215 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:05.514] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:05.514] frame process finished the id is: 31 the obs is:0 -[18:28:05.558] Odometry x=0.000040, y=0.006144, theta=-0.001157 -[18:28:05.614] Init: px average disparity is:1.712327 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:05.614] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:05.614] frame process finished the id is: 32 the obs is:0 -[18:28:05.659] Odometry x=0.000170, y=0.013148, theta=-0.000448 -[18:28:05.715] Init: px average disparity is:5.987677 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:05.715] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:05.715] frame process finished the id is: 33 the obs is:0 -[18:28:05.760] Odometry x=0.000258, y=0.019764, theta=-0.010322 -[18:28:05.824] Init: px average disparity is:7.661083 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:05.824] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:05.824] frame process finished the id is: 34 the obs is:0 -[18:28:05.861] Odometry x=0.000338, y=0.025878, theta=-0.003238 -[18:28:05.917] Init: px average disparity is:4.968104 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:05.917] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:05.917] frame process finished the id is: 35 the obs is:0 -[18:28:05.958] Odometry x=0.003378, y=0.032417, theta=-0.002440 -[18:28:06.014] Init: px average disparity is:5.353786 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:06.014] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:06.014] frame process finished the id is: 36 the obs is:0 -[18:28:06.065] Odometry x=0.003912, y=0.038428, theta=-0.006393 -[18:28:06.115] Init: px average disparity is:4.237338 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:06.115] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:06.115] frame process finished the id is: 37 the obs is:0 -[18:28:06.162] Odometry x=0.003835, y=0.042872, theta=-0.012875 -[18:28:06.215] Init: px average disparity is:5.731940 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:06.215] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:06.215] frame process finished the id is: 38 the obs is:0 -[18:28:06.259] Odometry x=0.004051, y=0.049578, theta=-0.000691 -[18:28:06.315] Init: px average disparity is:4.180975 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:06.315] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:06.315] frame process finished the id is: 39 the obs is:0 -[18:28:06.361] Odometry x=0.004110, y=0.054252, theta=-0.026867 -[18:28:06.415] Init: px average disparity is:6.100350 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:06.415] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:06.415] frame process finished the id is: 40 the obs is:0 -[18:28:06.459] Odometry x=0.004212, y=0.060117, theta=-0.017781 -[18:28:06.515] Init: px average disparity is:7.244833 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:06.515] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:06.515] frame process finished the id is: 41 the obs is:0 -[18:28:06.563] Odometry x=0.004257, y=0.063659, theta=-0.012887 -[18:28:06.623] Init: px average disparity is:8.958573 ,While minimum is: 10.000000 KLT tracked : 131 -[18:28:06.623] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:06.623] frame process finished the id is: 42 the obs is:0 -[18:28:06.663] Odometry x=0.004357, y=0.071317, theta=-0.013456 -[18:28:06.763] Odometry x=0.004522, y=0.076936, theta=-0.025164 -[18:28:06.794] Init: Homography RANSAC (inlier) is:0 ,While 30 inliers minimum required. px average disparity is:12.266455 ,While minimum is: 10.000000 KLT tracked: 131 -[18:28:06.794] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:06.794] frame process finished the id is: 43 the obs is:0 -[18:28:06.863] Odometry x=0.004839, y=0.083127, theta=-0.013441 -[18:28:06.877] Init: Homography RANSAC (inlier) is:20 ,While 30 inliers minimum required. px average disparity is:15.024849 ,While minimum is: 10.000000 KLT tracked: 131 -[18:28:06.877] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:06.878] frame process finished the id is: 44 the obs is:0 -[18:28:06.962] Odometry x=0.004825, y=0.088371, theta=-0.011639 -[18:28:06.978] Init: Homography RANSAC (inlier) is:21 ,While 30 inliers minimum required. px average disparity is:13.151287 ,While minimum is: 10.000000 KLT tracked: 131 -[18:28:06.978] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:06.978] frame process finished the id is: 45 the obs is:0 -[18:28:07.058] Odometry x=0.004983, y=0.096203, theta=-0.010491 -[18:28:07.089] Init: Homography RANSAC (inlier) is:21 ,While 30 inliers minimum required. px average disparity is:13.062843 ,While minimum is: 10.000000 KLT tracked: 131 -[18:28:07.089] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:07.089] frame process finished the id is: 46 the obs is:0 -[18:28:07.162] Odometry x=0.007213, y=0.101610, theta=-0.004462 -[18:28:07.178] Init: Homography RANSAC (inlier) is:21 ,While 30 inliers minimum required. px average disparity is:11.528166 ,While minimum is: 10.000000 KLT tracked: 131 -[18:28:07.178] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:07.178] frame process finished the id is: 47 the obs is:0 -[18:28:07.260] Odometry x=0.007410, y=0.107276, theta=-0.003474 -[18:28:07.278] Init: Homography RANSAC (inlier) is:28 ,While 30 inliers minimum required. px average disparity is:13.324742 ,While minimum is: 10.000000 KLT tracked: 131 -[18:28:07.278] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:07.278] frame process finished the id is: 48 the obs is:0 -[18:28:07.363] Odometry x=0.007641, y=0.124522, theta=-0.004625 -[18:28:07.378] Init: Homography RANSAC (inlier) is:26 ,While 30 inliers minimum required. px average disparity is:13.410493 ,While minimum is: 10.000000 KLT tracked: 130 -[18:28:07.378] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:07.378] frame process finished the id is: 49 the obs is:0 -[18:28:07.472] Odometry x=0.009022, y=0.133285, theta=0.000549 -[18:28:07.480] Init: Homography RANSAC (inlier) is:26 ,While 30 inliers minimum required. px average disparity is:19.576334 ,While minimum is: 10.000000 KLT tracked: 130 -[18:28:07.480] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:07.480] frame process finished the id is: 50 the obs is:0 -[18:28:07.560] Odometry x=0.012895, y=0.139349, theta=-0.015426 -[18:28:07.578] Init: Homography RANSAC (inlier) is:18 ,While 30 inliers minimum required. px average disparity is:13.856049 ,While minimum is: 10.000000 KLT tracked: 130 -[18:28:07.578] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:07.578] frame process finished the id is: 51 the obs is:0 -[18:28:07.664] Odometry x=0.012890, y=0.155111, theta=-0.014808 -[18:28:07.677] Init: Homography RANSAC (inlier) is:11 ,While 30 inliers minimum required. px average disparity is:13.536742 ,While minimum is: 10.000000 KLT tracked: 130 -[18:28:07.677] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:07.677] frame process finished the id is: 52 the obs is:0 -[18:28:07.716] Init: px average disparity is:8.407667 ,While minimum is: 10.000000 KLT tracked : 130 -[18:28:07.716] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:07.716] frame process finished the id is: 53 the obs is:0 -[18:28:07.761] Odometry x=0.014538, y=0.160252, theta=-0.006813 -[18:28:07.823] Init: px average disparity is:8.928041 ,While minimum is: 10.000000 KLT tracked : 130 -[18:28:07.823] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:07.823] frame process finished the id is: 54 the obs is:0 -[18:28:07.858] Odometry x=0.014969, y=0.165578, theta=-0.017024 -[18:28:07.923] Init: px average disparity is:8.519840 ,While minimum is: 10.000000 KLT tracked : 129 -[18:28:07.923] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:07.923] frame process finished the id is: 55 the obs is:0 -[18:28:07.964] Odometry x=0.016254, y=0.170745, theta=-0.016508 -[18:28:08.024] Init: px average disparity is:9.417081 ,While minimum is: 10.000000 KLT tracked : 129 -[18:28:08.024] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:08.024] frame process finished the id is: 56 the obs is:0 -[18:28:08.064] Odometry x=0.016414, y=0.176273, theta=-0.014725 -[18:28:08.165] Odometry x=0.022732, y=0.181561, theta=-0.011214 -[18:28:08.193] Init: Homography RANSAC (inlier) is:12 ,While 30 inliers minimum required. px average disparity is:10.308725 ,While minimum is: 10.000000 KLT tracked: 129 -[18:28:08.193] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:08.193] frame process finished the id is: 57 the obs is:0 -[18:28:08.215] Init: px average disparity is:8.129985 ,While minimum is: 10.000000 KLT tracked : 129 -[18:28:08.215] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:08.215] frame process finished the id is: 58 the obs is:0 -[18:28:08.266] Odometry x=0.022748, y=0.187362, theta=0.001457 -[18:28:08.366] Odometry x=0.022879, y=0.192814, theta=-0.012806 -[18:28:08.394] Init: Homography RANSAC (inlier) is:26 ,While 30 inliers minimum required. px average disparity is:10.681360 ,While minimum is: 10.000000 KLT tracked: 129 -[18:28:08.394] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[18:28:08.394] frame process finished the id is: 59 the obs is:0 -[18:28:08.463] Odometry x=0.022908 \ No newline at end of file +[15:02:31.173] Init: frame zero: 0.000000, 0.000000 0.000000 +[15:02:31.173] Init: Selected first frame. +[15:02:31.173] frame process finished the id is: 0 the obs is:0 +[15:02:31.203] Init: px average disparity is:0.204766 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:31.203] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:31.203] frame process finished the id is: 1 the obs is:0 +[15:02:31.296] Init: px average disparity is:0.254710 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:31.296] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:31.296] frame process finished the id is: 2 the obs is:0 +[15:02:31.396] Init: px average disparity is:0.258687 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:31.396] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:31.396] frame process finished the id is: 3 the obs is:0 +[15:02:31.500] Init: px average disparity is:0.257112 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:31.500] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:31.500] frame process finished the id is: 4 the obs is:0 +[15:02:31.595] Init: px average disparity is:0.260131 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:31.595] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:31.595] frame process finished the id is: 5 the obs is:0 +[15:02:31.695] Init: px average disparity is:0.253240 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:31.695] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:31.695] frame process finished the id is: 6 the obs is:0 +[15:02:31.806] Init: px average disparity is:0.265735 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:31.807] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:31.807] frame process finished the id is: 7 the obs is:0 +[15:02:31.905] Init: px average disparity is:0.255464 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:31.905] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:31.905] frame process finished the id is: 8 the obs is:0 +[15:02:32.001] Init: px average disparity is:0.281982 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:32.001] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:32.001] frame process finished the id is: 9 the obs is:0 +[15:02:32.100] Init: px average disparity is:0.256724 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:32.100] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:32.100] frame process finished the id is: 10 the obs is:0 +[15:02:32.202] Init: px average disparity is:0.265233 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:32.202] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:32.202] frame process finished the id is: 11 the obs is:0 +[15:02:32.300] Init: px average disparity is:0.271957 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:32.300] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:32.300] frame process finished the id is: 12 the obs is:0 +[15:02:32.403] Init: px average disparity is:0.261671 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:32.403] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:32.403] frame process finished the id is: 13 the obs is:0 +[15:02:32.501] Init: px average disparity is:0.268405 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:32.501] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:32.501] frame process finished the id is: 14 the obs is:0 +[15:02:32.603] Init: px average disparity is:0.251128 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:32.603] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:32.603] frame process finished the id is: 15 the obs is:0 +[15:02:32.702] Init: px average disparity is:0.265931 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:32.702] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:32.702] frame process finished the id is: 16 the obs is:0 +[15:02:32.801] Init: px average disparity is:0.296831 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:32.801] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:32.802] frame process finished the id is: 17 the obs is:0 +[15:02:32.900] Init: px average disparity is:0.265149 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:32.900] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:32.901] frame process finished the id is: 18 the obs is:0 +[15:02:33.001] Init: px average disparity is:0.251746 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:33.001] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:33.001] frame process finished the id is: 19 the obs is:0 +[15:02:33.101] Init: px average disparity is:0.257138 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:33.101] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:33.101] frame process finished the id is: 20 the obs is:0 +[15:02:33.204] Init: px average disparity is:0.276642 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:33.204] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:33.204] frame process finished the id is: 21 the obs is:0 +[15:02:33.303] Init: px average disparity is:0.281102 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:33.303] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:33.303] frame process finished the id is: 22 the obs is:0 +[15:02:33.401] Init: px average disparity is:0.259442 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:33.401] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:33.401] frame process finished the id is: 23 the obs is:0 +[15:02:33.501] Init: px average disparity is:0.246822 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:33.502] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:33.502] frame process finished the id is: 24 the obs is:0 +[15:02:33.600] Init: px average disparity is:0.285573 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:33.600] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:33.600] frame process finished the id is: 25 the obs is:0 +[15:02:33.707] Init: px average disparity is:0.267547 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:33.707] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:33.707] frame process finished the id is: 26 the obs is:0 +[15:02:33.802] Init: px average disparity is:0.241625 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:33.802] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:33.802] frame process finished the id is: 27 the obs is:0 +[15:02:33.905] Init: px average disparity is:0.285951 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:33.905] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:33.905] frame process finished the id is: 28 the obs is:0 +[15:02:34.004] Init: px average disparity is:0.254980 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:34.004] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:34.004] frame process finished the id is: 29 the obs is:0 +[15:02:34.043] Odometry x=0.000000, y=0.000000, theta=-0.000000 +[15:02:34.103] Init: px average disparity is:0.250407 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:34.103] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:34.103] frame process finished the id is: 30 the obs is:0 +[15:02:34.138] Odometry x=0.000000, y=0.000000, theta=-0.000000 +[15:02:34.202] Init: px average disparity is:0.238669 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:34.202] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:34.202] frame process finished the id is: 31 the obs is:0 +[15:02:34.239] Odometry x=0.000040, y=0.006142, theta=-0.001144 +[15:02:34.301] Init: px average disparity is:1.732545 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:34.301] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:34.301] frame process finished the id is: 32 the obs is:0 +[15:02:34.340] Odometry x=0.000087, y=0.011880, theta=-0.001201 +[15:02:34.402] Init: px average disparity is:6.026566 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:34.402] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:34.402] frame process finished the id is: 33 the obs is:0 +[15:02:34.439] Odometry x=0.000315, y=0.023540, theta=-0.008529 +[15:02:34.504] Init: px average disparity is:7.695938 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:34.504] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:34.504] frame process finished the id is: 34 the obs is:0 +[15:02:34.542] Odometry x=0.000725, y=0.031388, theta=-0.000516 +[15:02:34.605] Init: px average disparity is:5.012340 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:34.605] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:34.605] frame process finished the id is: 35 the obs is:0 +[15:02:34.642] Odometry x=0.000780, y=0.037020, theta=-0.001593 +[15:02:34.702] Init: px average disparity is:5.378174 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:34.703] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:34.703] frame process finished the id is: 36 the obs is:0 +[15:02:34.740] Odometry x=0.001849, y=0.045007, theta=-0.000630 +[15:02:34.805] Init: px average disparity is:4.273644 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:34.805] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:34.805] frame process finished the id is: 37 the obs is:0 +[15:02:34.843] Odometry x=0.001864, y=0.050416, theta=-0.000524 +[15:02:34.905] Init: px average disparity is:5.753049 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:34.905] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:34.905] frame process finished the id is: 38 the obs is:0 +[15:02:34.942] Odometry x=0.001872, y=0.060419, theta=0.005156 +[15:02:35.005] Init: px average disparity is:4.094173 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:35.005] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:35.005] frame process finished the id is: 39 the obs is:0 +[15:02:35.040] Odometry x=0.001879, y=0.065651, theta=-0.008522 +[15:02:35.105] Init: px average disparity is:5.968848 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:35.105] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:35.105] frame process finished the id is: 40 the obs is:0 +[15:02:35.143] Odometry x=0.001903, y=0.071816, theta=0.001989 +[15:02:35.204] Init: px average disparity is:7.017181 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:35.204] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:35.204] frame process finished the id is: 41 the obs is:0 +[15:02:35.242] Odometry x=0.001912, y=0.072980, theta=0.000849 +[15:02:35.303] Init: px average disparity is:8.879703 ,While minimum is: 10.000000 KLT tracked : 126 +[15:02:35.303] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:35.303] frame process finished the id is: 42 the obs is:0 +[15:02:35.340] Odometry x=0.001904, y=0.078215, theta=0.007803 +[15:02:35.443] Odometry x=0.001968, y=0.083577, theta=-0.014216 +[15:02:35.475] Init: Homography RANSAC (inlier) is:0 ,While 30 inliers minimum required. px average disparity is:12.100441 ,While minimum is: 10.000000 KLT tracked: 126 +[15:02:35.475] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:35.475] frame process finished the id is: 43 the obs is:0 +[15:02:35.543] Odometry x=0.002275, y=0.090136, theta=-0.002056 +[15:02:35.561] Init: Homography RANSAC (inlier) is:0 ,While 30 inliers minimum required. px average disparity is:14.878219 ,While minimum is: 10.000000 KLT tracked: 126 +[15:02:35.561] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:35.561] frame process finished the id is: 44 the obs is:0 +[15:02:35.641] Odometry x=0.002232, y=0.095379, theta=-0.000266 +[15:02:35.664] Init: Homography RANSAC (inlier) is:0 ,While 30 inliers minimum required. px average disparity is:12.946838 ,While minimum is: 10.000000 KLT tracked: 126 +[15:02:35.664] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:35.664] frame process finished the id is: 45 the obs is:0 +[15:02:35.739] Odometry x=0.002345, y=0.103215, theta=0.000882 +[15:02:35.772] Init: Homography RANSAC (inlier) is:22 ,While 30 inliers minimum required. px average disparity is:12.883808 ,While minimum is: 10.000000 KLT tracked: 126 +[15:02:35.772] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:35.772] frame process finished the id is: 46 the obs is:0 +[15:02:35.842] Odometry x=0.002318, y=0.108817, theta=0.007854 +[15:02:35.863] Init: Homography RANSAC (inlier) is:22 ,While 30 inliers minimum required. px average disparity is:11.397537 ,While minimum is: 10.000000 KLT tracked: 126 +[15:02:35.863] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:35.863] frame process finished the id is: 47 the obs is:0 +[15:02:35.939] Odometry x=0.002409, y=0.114709, theta=0.009961 +[15:02:35.963] Init: Homography RANSAC (inlier) is:25 ,While 30 inliers minimum required. px average disparity is:13.611910 ,While minimum is: 10.000000 KLT tracked: 126 +[15:02:35.963] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:35.963] frame process finished the id is: 48 the obs is:0 +[15:02:36.042] Odometry x=0.003150, y=0.137099, theta=0.007237 +[15:02:36.066] Init: Homography RANSAC (inlier) is:25 ,While 30 inliers minimum required. px average disparity is:13.673733 ,While minimum is: 10.000000 KLT tracked: 125 +[15:02:36.066] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[15:02:36.066] frame process finished the id is: 49 the obs is:0 +[15:02:36.150] Odometry x=0.003188, y=0.148639, theta=0.012113 +[15:02:36.165] Init finished: Homography RANSAC (inlier) is:36 ,While 30 inliers minimum required. px average disparity is:19.754425 ,While minimum is: 10.000000 KLT tracked: 125 +[15:02:36.165] Init: distance between the first and current frame is x:0.005483 ,z=0.155246,angle between two frames: 0.003028 +[15:02:36.165] Init: Selected Second frame. The number of features: 36 depth mean:-0.194038 min:-0.893048 +[15:02:36.165] frame process finished the id is: 50 the obs is:36 +[15:02:36.242] Odometry x=0.005483, y=0.155246, theta=0.003028 +[15:02:36.282] After Reprojection Map nPoint:72 ,nCell:16 ,nMatches:12 ,distance between ekf and vo x:0.003275 ,z=0.000631,angle between two frames:0.006234 +[15:02:36.282] n deleted obs = 10 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =62 error init =0.013697 error end=0.013697 +[15:02:36.282] After pose optimization, distance between ekf and vo x:0.003275 ,z=0.000631,angle between two frames:0.006234 +[15:02:36.282] Update EKF and 3D points the number of feature in the new frame: 72 and number of obs: 72 +[15:02:36.282] frame process finished the id is: 51 the obs is:72 +[15:02:36.344] Odometry x=0.008588, y=0.158087, theta=0.009087 +[15:02:36.372] After Reprojection Map nPoint:108 ,nCell:17 ,nMatches:13 ,distance between ekf and vo x:0.000993 ,z=-0.000095,angle between two frames:0.001558 +[15:02:36.372] n deleted obs = 21 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =87 error init =0.027702 error end=0.027039 +[15:02:36.372] After pose optimization, distance between ekf and vo x:-0.015248 ,z=-0.021310,angle between two frames:0.025756 +[15:02:36.372] Update EKF and 3D points the number of feature in the new frame: 108 and number of obs: 108 +[15:02:36.372] frame process finished the id is: 52 the obs is:108 +[15:02:36.440] Odometry x=-0.005571, y=0.138338, theta=0.033776 +[15:02:36.484] After Reprojection Map nPoint:123 ,nCell:29 ,nMatches:14 ,distance between ekf and vo x:0.000162 ,z=0.000044,angle between two frames:0.000158 +[15:02:36.485] n deleted obs = 13 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =110 error init =0.013347 error end=0.009386 +[15:02:36.485] After pose optimization, distance between ekf and vo x:-0.002074 ,z=0.036481,angle between two frames:0.005315 +[15:02:36.485] Update EKF and 3D points the number of feature in the new frame: 123 and number of obs: 123 +[15:02:36.485] frame process finished the id is: 53 the obs is:123 +[15:02:36.538] Odometry x=-0.007446, y=0.173401, theta=0.029843 +[15:02:36.569] After Reprojection Map nPoint:158 ,nCell:31 ,nMatches:17 ,distance between ekf and vo x:-0.000013 ,z=0.000026,angle between two frames:0.000019 +[15:02:36.569] n deleted obs = 10 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =148 error init =0.001994 error end=0.001977 +[15:02:36.569] After pose optimization, distance between ekf and vo x:-0.000403 ,z=-0.000119,angle between two frames:0.000908 +[15:02:36.569] Update EKF and 3D points the number of feature in the new frame: 158 and number of obs: 158 +[15:02:36.570] frame process finished the id is: 54 the obs is:158 +[15:02:36.641] Odometry x=-0.007706, y=0.173660, theta=0.031358 +[15:02:36.678] After Reprojection Map nPoint:184 ,nCell:32 ,nMatches:17 ,distance between ekf and vo x:0.000110 ,z=0.000034,angle between two frames:0.000277 +[15:02:36.679] n deleted obs = 17 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =167 error init =0.001773 error end=0.001061 +[15:02:36.679] After pose optimization, distance between ekf and vo x:0.017708 ,z=0.005378,angle between two frames:0.025630 +[15:02:36.679] Update EKF and 3D points the number of feature in the new frame: 184 and number of obs: 184 +[15:02:36.680] frame process finished the id is: 55 the obs is:184 +[15:02:36.743] Odometry x=0.009019, y=0.179111, theta=0.008037 +[15:02:36.768] After Reprojection Map nPoint:220 ,nCell:33 ,nMatches:16 ,distance between ekf and vo x:-0.000029 ,z=0.000035,angle between two frames:0.000085 +[15:02:36.768] n deleted obs = 16 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =204 error init =0.021039 error end=0.021039 +[15:02:36.768] After pose optimization, distance between ekf and vo x:-0.000029 ,z=0.000035,angle between two frames:0.000085 +[15:02:36.768] Update EKF and 3D points the number of feature in the new frame: 220 and number of obs: 220 +[15:02:36.769] frame process finished the id is: 56 the obs is:220 +[15:02:36.846] Odometry x=0.009009, y=0.179520, theta=0.008902 +[15:02:36.877] After Reprojection Map nPoint:243 ,nCell:35 ,nMatches:17 ,distance between ekf and vo x:0.000008 ,z=-0.000009,angle between two frames:0.000006 +[15:02:36.877] n deleted obs = 44 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =199 error init =0.019397 error end=0.019397 +[15:02:36.877] After pose optimization, distance between ekf and vo x:0.000008 ,z=-0.000009,angle between two frames:0.000006 +[15:02:36.877] Update EKF and 3D points the number of feature in the new frame: 243 and number of obs: 243 +[15:02:36.878] frame process finished the id is: 57 the obs is:243 +[15:02:36.943] Odometry x=0.009015, y=0.179974, theta=0.009204 +[15:02:36.984] After Reprojection Map nPoint:272 ,nCell:27 ,nMatches:18 ,distance between ekf and vo x:0.000010 ,z=-0.000013,angle between two frames:0.000002 +[15:02:36.984] n deleted obs = 37 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =235 error init =0.005655 error end=0.005655 +[15:02:36.985] After pose optimization, distance between ekf and vo x:0.000010 ,z=-0.000013,angle between two frames:0.000002 +[15:02:36.985] Update EKF and 3D points the number of feature in the new frame: 272 and number of obs: 272 +[15:02:36.985] frame process finished the id is: 58 the obs is:272 +[15:02:37.046] Odometry x=0.009038, y=0.180336, theta=0.008917 +[15:02:37.080] After Reprojection Map nPoint:294 ,nCell:71 ,nMatches:25 ,distance between ekf and vo x:0.000000 ,z=0.000012,angle between two frames:0.000001 +[15:02:37.081] n deleted obs = 54 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =240 error init =0.008672 error end=0.006811 +[15:02:37.081] After pose optimization, distance between ekf and vo x:-0.020594 ,z=0.001015,angle between two frames:0.002934 +[15:02:37.081] Update EKF and 3D points the number of feature in the new frame: 294 and number of obs: 294 +[15:02:37.082] frame process finished the id is: 59 the obs is:294 +[15:02:37.143] Odometry x=-0.010399, y=0.181710, theta=0.012264 +[15:02:37.171] After Reprojection Map nPoint:283 ,nCell:49 ,nMatches:27 ,distance between ekf and vo x:0.000017 ,z=-0.000017,angle between two frames:0.000000 +[15:02:37.171] n deleted obs = 60 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =223 error init =0.026463 error end=0.026463 +[15:02:37.171] After pose optimization, distance between ekf and vo x:0.000017 ,z=-0.000017,angle between two frames:0.000000 +[15:02:37.171] Update EKF and 3D points the number of feature in the new frame: 283 and number of obs: 283 +[15:02:37.172] frame process finished the id is: 60 the obs is:283 +[15:02:37.193] Image Alignment failed 0 +[15:02:37.193] frame process finished the id is: 60 the obs is:283 +[15:02:37.246] Odometry x=-0.010361, y=0.182059, theta=0.012535 +[15:02:37.301] Image Alignment failed 0 +[15:02:37.301] frame process finished the id is: 60 the obs is:283 +[15:02:37.340] Odometry x=-0.010406, y=0.188695, theta=0.018798 +[15:02:37.402] Image Alignment failed 0 +[15:02:37.402] frame process finished the id is: 60 the obs is:283 +[15:02:37.438] Odometry x=-0.010250, y=0.194369, theta=0.013003 +[15:02:37.501] Image Alignment failed 0 +[15:02:37.501] frame process finished the id is: 60 the obs is:283 +[15:02:37.541] Odometry x=-0.010297, y=0.199986, theta=0.018133 +[15:02:37.602] Image Alignment failed 0 +[15:02:37.602] frame process finished the id is: 60 the obs is:283 +[15:02:37.640] Odometry x=-0.009076, y=0.205649, theta=0.008567 +[15:02:37.701] Image Alignment failed 0 +[15:02:37.701] frame process finished the id is: 60 the obs is:283 +[15:02:37.743] Odometry x=-0.009078, y=0.211139, theta=0.013741 +[15:02:37.801] Image Alignment failed 0 +[15:02:37.801] frame process finished the id is: 60 the obs is:283 +[15:02:37.841] Odometry x=-0.008995, y=0.220973, theta=0.021974 +[15:02:37.903] Image Alignment failed 0 +[15:02:37.903] frame process finished the id is: 60 the obs is:283 +[15:02:37.944] Odometry x=-0.011606, y=0.226948, theta=0.005677 +[15:02:38.000] Image Alignment failed 0 +[15:02:38.000] frame process finished the id is: 60 the obs is:283 +[15:02:38.042] Odometry x=-0.011367, y=0.233968, theta=0.032275 +[15:02:38.101] Image Alignment failed 0 +[15:02:38.101] frame process finished the id is: 60 the obs is:283 +[15:02:38.140] Odometry x=-0.011013, y=0.240120, theta=0.022067 +[15:02:38.201] Image Alignment failed 0 +[15:02:38.201] frame process finished the id is: 60 the obs is:283 +[15:02:38.243] Odometry x=-0.011236, y=0.265136, theta=0.026515 +[15:02:38.302] Image Alignment failed 0 +[15:02:38.302] frame process finished the id is: 60 the obs is:283 +[15:02:38.342] Odometry x=-0.011293, y=0.271562, theta=0.024102 +[15:02:38.401] Image Alignment failed 0 +[15:02:38.401] frame process finished the id is: 60 the obs is:283 +[15:02:38.440] Odometry x=-0.011354, y=0.276956, theta=0.014935 +[15:02:38.501] Image Alignment failed 0 +[15:02:38.501] frame process finished the id is: 60 the obs is:283 +[15:02:38.543] Odometry x=-0.011299, y=0.282448, theta=0.015099 +[15:02:38.601] Image Alignment failed 0 +[15:02:38.601] frame process finished the id is: 60 the obs is:283 +[15:02:38.641] Odometry x=-0.010752, y=0.287835, theta=0.004638 +[15:02:38.701] Image Alignment failed 0 +[15:02:38.701] frame process finished the id is: 60 the obs is:283 +[15:02:38.739] Odometry x=-0.010756, y=0.293326, theta=0.009108 +[15:02:38.801] Image Alignment failed 0 +[15:02:38.801] frame process finished the id is: 60 the obs is:283 +[15:02:38.842] Odometry x=-0.013758, y=0.308149, theta=0.026125 +[15:02:38.901] Image Alignment failed 0 +[15:02:38.901] frame process finished the id is: 60 the obs is:283 +[15:02:38.940] Odometry x=-0.013477, y=0.313243, theta=0.015855 +[15:02:39.002] Image Alignment failed 0 +[15:02:39.002] frame process finished the id is: 60 the obs is:283 +[15:02:39.044] Odometry x=-0.013518, y=0.318664, theta=0.021524 +[15:02:39.102] Image Alignment failed 0 +[15:02:39.102] frame process finished the id is: 60 the obs is:283 +[15:02:39.147] Odometry x=-0.013444, y=0.324191, theta=0.018469 +[15:02:39.201] Image Alignment failed 0 +[15:02:39.201] frame process finished the id is: 60 the obs is:283 +[15:02:39.243] Odometry x=-0.013506, y=0.330347, theta=0.021898 +[15:02:39.301] Image Alignment failed 0 +[15:02:39.301] frame process finished the id is: 60 the obs is:283 +[15:02:39.341] Odometry x=-0.013552, y=0.335734, theta=0.009540 +[15:02:39.402] Image Alignment failed 0 +[15:02:39.402] frame process finished the id is: 60 the obs is:283 +[15:02:39.444] Odometry x=-0.013484, y=0.364138, theta=0.029626 +[15:02:39.502] Image Alignment failed 0 +[15:02:39.502] frame process finished the id is: 60 the obs is:283 +[15:02:39.540] Odometry x=-0.013439, y=0.369706, theta=0.001261 +[15:02:39.601] Image Alignment failed 0 +[15:02:39.601] frame process finished the id is: 60 the obs is:283 +[15:02:39.643] Odometry x=-0.013457, y=0.374790, theta=0.025020 +[15:02:39.701] Image Alignment failed 0 +[15:02:39.701] frame process finished the id is: 60 the obs is:283 +[15:02:39.746] Odometry x=-0.011003, y=0.380423, theta=0.009205 +[15:02:39.801] Image Alignment failed 0 +[15:02:39.801] frame process finished the id is: 60 the obs is:283 +[15:02:39.844] Odometry x=-0.010702, y=0.387821, theta=-0.000378 +[15:02:39.901] Image Alignment failed 0 +[15:02:39.901] frame process finished the id is: 60 the obs is:283 +[15:02:39.942] Odometry x=-0.009455, y=0.392825, theta=0.011839 +[15:02:40.003] Image Alignment failed 0 +[15:02:40.003] frame process finished the id is: 60 the obs is:283 +[15:02:40.046] Odometry x=-0.009303, y=0.401043, theta=0.003689 +[15:02:40.101] Image Alignment failed 0 +[15:02:40.101] frame process finished the id is: 60 the obs is:283 +[15:02:40.144] Odometry x=-0.011225, y=0.404770, theta=-0.017035 +[15:02:40.201] Image Alignment failed 0 +[15:02:40.201] frame process finished the id is: 60 the obs is:283 +[15:02:40.242] Odometry x=-0.010470, y=0.419372, theta=-0.008975 +[15:02:40.305] Image Alignment failed 0 +[15:02:40.305] frame process finished the id is: 60 the obs is:283 +[15:02:40.346] Odometry x=-0.010712, y=0.424573, theta=-0.012906 +[15:02:40.401] Image Alignment failed 0 +[15:02:40.401] frame process finished the id is: 60 the obs is:283 +[15:02:40.444] Odometry x=-0.010579, y=0.430476, theta=-0.007647 +[15:02:40.504] Image Alignment failed 0 +[15:02:40.504] frame process finished the id is: 60 the obs is:283 +[15:02:40.546] Odometry x=-0.010536, y=0.435889, theta=-0.017156 +[15:02:40.603] Image Alignment failed 0 +[15:02:40.603] frame process finished the id is: 60 the obs is:283 +[15:02:40.641] Odometry x=-0.010483, y=0.441525, theta=-0.010229 +[15:02:40.702] Image Alignment failed 0 +[15:02:40.702] frame process finished the id is: 60 the obs is:283 +[15:02:40.741] Odometry x=-0.010922, y=0.468790, theta=0.009780 +[15:02:40.802] Image Alignment failed 0 +[15:02:40.803] frame process finished the id is: 60 the obs is:283 +[15:02:40.846] Odometry x=-0.010950, y=0.474327, theta=0.000044 +[15:02:40.904] Image Alignment failed 0 +[15:02:40.904] frame process finished the id is: 60 the obs is:283 +[15:02:40.945] Odometry x=-0.010808, y=0.480168, theta=0.008478 +[15:02:41.004] Image Alignment failed 0 +[15:02:41.004] frame process finished the id is: 60 the obs is:283 +[15:02:41.043] Odometry x=-0.010813, y=0.485655, theta=0.000972 +[15:02:41.102] Image Alignment failed 0 +[15:02:41.102] frame process finished the id is: 60 the obs is:283 +[15:02:41.146] Odometry x=-0.019949, y=0.505202, theta=-0.004160 +[15:02:41.203] Image Alignment failed 0 +[15:02:41.203] frame process finished the id is: 60 the obs is:283 +[15:02:41.244] Odometry x=-0.019708, y=0.510732, theta=-0.003394 +[15:02:41.303] Image Alignment failed 0 +[15:02:41.303] frame process finished the id is: 60 the obs is:283 +[15:02:41.346] Odometry x=-0.019695, y=0.516258, theta=-0.005263 +[15:02:41.401] Image Alignment failed 0 +[15:02:41.401] frame process finished the id is: 60 the obs is:283 +[15:02:41.444] Odometry x=-0.019615, y=0.521650, theta=-0.005684 +[15:02:41.505] Image Alignment failed 0 +[15:02:41.505] frame process finished the id is: 60 the obs is:283 +[15:02:41.546] Odometry x=-0.015134, y=0.526840, theta=0.002171 +[15:02:41.602] Image Alignment failed 0 +[15:02:41.602] frame process finished the id is: 60 the obs is:283 +[15:02:41.644] Odometry x=-0.015141, y=0.532223, theta=0.001832 +[15:02:41.701] Image Alignment failed 0 +[15:02:41.701] frame process finished the id is: 60 the obs is:283 +[15:02:41.742] Odometry x=-0.014556, y=0.537736, theta=0.001101 +[15:02:41.803] Image Alignment failed 0 +[15:02:41.803] frame process finished the id is: 60 the obs is:283 +[15:02:41.841] Odometry x=-0.020129, y=0.543586, theta=-0.007544 +[15:02:41.905] Image Alignment failed 0 +[15:02:41.905] frame process finished the id is: 60 the obs is:283 +[15:02:41.946] Odometry x=-0.020010, y=0.550494, theta=0.011956 +[15:02:42.003] Image Alignment failed 0 +[15:02:42.003] frame process finished the id is: 60 the obs is:283 +[15:02:42.043] Odometry x=-0.019671, y=0.555549, theta=-0.007303 +[15:02:42.101] Image Alignment failed 0 +[15:02:42.101] frame process finished the id is: 60 the obs is:283 +[15:02:42.143] Odometry x=-0.019113, y=0.560726, theta=0.000292 +[15:02:42.203] Image Alignment failed 0 +[15:02:42.203] frame process finished the id is: 60 the obs is:283 +[15:02:42.242] Odometry x=-0.019109, y=0.566062, theta=-0.010128 +[15:02:42.301] Image Alignment failed 0 +[15:02:42.301] frame process finished the id is: 60 the obs is:283 +[15:02:42.341] Odometry x=-0.019039, y=0.572341, theta=0.001368 +[15:02:42.401] Image Alignment failed 0 +[15:02:42.401] frame process finished the id is: 60 the obs is:283 +[15:02:42.441] Odometry x=-0.019755, y=0.577660, theta=0.000378 +[15:02:42.501] Image Alignment failed 0 +[15:02:42.501] frame process finished the id is: 60 the obs is:283 +[15:02:42.544] Odometry x=-0.019350, y=0.585308, theta=-0.000985 +[15:02:42.603] Image Alignment failed 0 +[15:02:42.603] frame process finished the id is: 60 the obs is:283 +[15:02:42.642] Odometry x=-0.019470, y=0.570300, theta=0.009208 +[15:02:42.704] Image Alignment failed 0 +[15:02:42.704] frame process finished the id is: 60 the obs is:283 +[15:02:42.741] Odometry x=-0.019399, y=0.579029, theta=0.000587 +[15:02:42.804] Image Alignment failed 0 +[15:02:42.804] frame process finished the id is: 60 the obs is:283 +[15:02:42.844] Odometry x=-0.014596, y=0.578075, theta=0.019506 +[15:02:42.903] Image Alignment failed 0 +[15:02:42.903] frame process finished the id is: 60 the obs is:283 +[15:02:42.942] Odometry x=-0.014457, y=0.586998, theta=0.009846 +[15:02:43.003] Image Alignment failed 0 +[15:02:43.003] frame process finished the id is: 60 the obs is:283 +[15:02:43.040] Odometry x=-0.013886, y=0.592667, theta=0.021376 +[15:02:43.103] Image Alignment failed 0 +[15:02:43.103] frame process finished the id is: 60 the obs is:283 +[15:02:43.143] Odometry x=-0.012837, y=0.600892, theta=0.010482 +[15:02:43.203] Image Alignment failed 0 +[15:02:43.203] frame process finished the id is: 60 the obs is:283 +[15:02:43.241] Odometry x=-0.012898, y=0.605158, theta=0.010074 +[15:02:43.303] Image Alignment failed 0 +[15:02:43.303] frame process finished the id is: 60 the obs is:283 +[15:02:43.344] Odometry x=-0.012150, y=0.611360, theta=0.016933 +[15:02:43.403] Image Alignment failed 0 +[15:02:43.403] frame process finished the id is: 60 the obs is:283 +[15:02:43.442] Odometry x=-0.012709, y=0.617766, theta=-0.009002 +[15:02:43.503] Image Alignment failed 0 +[15:02:43.503] frame process finished the id is: 60 the obs is:283 +[15:02:43.540] Odometry x=-0.012537, y=0.623366, theta=0.001732 +[15:02:43.604] Image Alignment failed 0 +[15:02:43.604] frame process finished the id is: 60 the obs is:283 +[15:02:43.645] Odometry x=-0.011762, y=0.628644, theta=-0.009264 +[15:02:43.703] Image Alignment failed 0 +[15:02:43.703] frame process finished the id is: 60 the obs is:283 +[15:02:43.743] Odometry x=-0.011472, y=0.634348, theta=-0.001387 +[15:02:43.803] Image Alignment failed 0 +[15:02:43.803] frame process finished the id is: 60 the obs is:283 +[15:02:43.841] Odometry x=-0.011768, y=0.639657, theta=-0.005644 +[15:02:43.903] Image Alignment failed 0 +[15:02:43.903] frame process finished the id is: 60 the obs is:283 +[15:02:43.944] Odometry x=-0.011540, y=0.646296, theta=-0.012513 +[15:02:44.009] Image Alignment failed 0 +[15:02:44.009] frame process finished the id is: 60 the obs is:283 +[15:02:44.046] Odometry x=-0.010219, y=0.651572, theta=0.007700 +[15:02:44.103] Image Alignment failed 0 +[15:02:44.103] frame process finished the id is: 60 the obs is:283 +[15:02:44.144] Odometry x=-0.010032, y=0.657069, theta=-0.007375 +[15:02:44.204] Image Alignment failed 0 +[15:02:44.204] frame process finished the id is: 60 the obs is:283 +[15:02:44.237] Odometry x=-0.010030, y=0.662425, theta=0.012611 +[15:02:44.305] Image Alignment failed 0 +[15:02:44.305] frame process finished the id is: 60 the obs is:283 +[15:02:44.340] Odometry x=-0.009990, y=0.667523, theta=-0.002173 +[15:02:44.404] Image Alignment failed 0 +[15:02:44.404] frame process finished the id is: 60 the obs is:283 +[15:02:44.438] Odometry x=-0.009984, y=0.672872, theta=0.003514 +[15:02:44.504] Image Alignment failed 0 +[15:02:44.504] frame process finished the id is: 60 the obs is:283 +[15:02:44.547] Odometry x=-0.010035, y=0.678274, theta=0.004580 +[15:02:44.603] Image Alignment failed 0 +[15:02:44.603] frame process finished the id is: 60 the obs is:283 +[15:02:44.643] Odometry x=-0.010032, y=0.684075, theta=-0.002190 +[15:02:44.701] Image Alignment failed 0 +[15:02:44.701] frame process finished the id is: 60 the obs is:283 +[15:02:44.745] Odometry x=-0.010028, y=0.691598, theta=0.011392 +[15:02:44.802] Image Alignment failed 0 +[15:02:44.802] frame process finished the id is: 60 the obs is:283 +[15:02:44.842] Odometry x=-0.009932, y=0.697361, theta=0.007289 +[15:02:44.901] Image Alignment failed 0 +[15:02:44.901] frame process finished the id is: 60 the obs is:283 +[15:02:44.941] Odometry x=-0.017563, y=0.705839, theta=0.004363 +[15:02:45.002] Image Alignment failed 0 +[15:02:45.002] frame process finished the id is: 60 the obs is:283 +[15:02:45.041] Odometry x=-0.015894, y=0.711702, theta=0.017092 +[15:02:45.102] Image Alignment failed 0 +[15:02:45.102] frame process finished the id is: 60 the obs is:283 +[15:02:45.141] Odometry x=-0.017366, y=0.717164, theta=-0.000552 +[15:02:45.201] Image Alignment failed 0 +[15:02:45.201] frame process finished the id is: 60 the obs is:283 +[15:02:45.243] Odometry x=-0.017097, y=0.722913, theta=0.006456 +[15:02:45.302] Image Alignment failed 0 +[15:02:45.302] frame process finished the id is: 60 the obs is:283 +[15:02:45.342] Odometry x=-0.017058, y=0.728424, theta=-0.001337 +[15:02:45.404] Image Alignment failed 0 +[15:02:45.404] frame process finished the id is: 60 the obs is:283 +[15:02:45.441] Odometry x=-0.016383, y=0.733862, theta=0.006207 +[15:02:45.503] Image Alignment failed 0 +[15:02:45.503] frame process finished the id is: 60 the obs is:283 +[15:02:45.544] Odometry x=-0.016450, y=0.739484, theta=0.005991 +[15:02:45.602] Image Alignment failed 0 +[15:02:45.602] frame process finished the id is: 60 the obs is:283 +[15:02:45.642] Odometry x=-0.016442, y=0.745148, theta=-0.007291 +[15:02:45.702] Image Alignment failed 0 +[15:02:45.702] frame process finished the id is: 60 the obs is:283 +[15:02:45.740] Odometry x=-0.017759, y=0.749488, theta=-0.004679 +[15:02:45.802] Image Alignment failed 0 +[15:02:45.803] frame process finished the id is: 60 the obs is:283 +[15:02:45.838] Odometry x=-0.017665, y=0.754698, theta=-0.007837 +[15:02:45.903] Image Alignment failed 0 +[15:02:45.903] frame process finished the id is: 60 the obs is:283 +[15:02:45.941] Odometry x=-0.017633, y=0.760058, theta=0.002810 +[15:02:46.002] Image Alignment failed 0 +[15:02:46.002] frame process finished the id is: 60 the obs is:283 +[15:02:46.044] Odometry x=-0.017048, y=0.765428, theta=-0.007674 +[15:02:46.104] Image Alignment failed 0 +[15:02:46.104] frame process finished the id is: 60 the obs is:283 +[15:02:46.143] Odometry x=-0.017034, y=0.770858, theta=0.003830 +[15:02:46.203] Image Alignment failed 0 +[15:02:46.203] frame process finished the id is: 60 the obs is:283 +[15:02:46.241] Odometry x=-0.017144, y=0.776440, theta=-0.003684 +[15:02:46.301] Image Alignment failed 0 +[15:02:46.302] frame process finished the id is: 60 the obs is:283 +[15:02:46.344] Odometry x=-0.017125, y=0.781862, theta=-0.005377 +[15:02:46.402] Image Alignment failed 0 +[15:02:46.402] frame process finished the id is: 60 the obs is:283 +[15:02:46.442] Odometry x=-0.017114, y=0.788353, theta=0.006152 +[15:02:46.503] Image Alignment failed 0 +[15:02:46.503] frame process finished the id is: 60 the obs is:283 +[15:02:46.540] Odometry x=-0.016993, y=0.794122, theta=0.001191 +[15:02:46.602] Image Alignment failed 0 +[15:02:46.602] frame process finished the id is: 60 the obs is:283 +[15:02:46.642] Odometry x=-0.017906, y=0.800385, theta=0.012881 +[15:02:46.703] Image Alignment failed 0 +[15:02:46.703] frame process finished the id is: 60 the obs is:283 +[15:02:46.740] Odometry x=-0.017934, y=0.805918, theta=0.003776 +[15:02:46.801] Image Alignment failed 0 +[15:02:46.802] frame process finished the id is: 60 the obs is:283 +[15:02:46.840] Odometry x=-0.017957, y=0.811680, theta=0.012141 +[15:02:46.902] Image Alignment failed 0 +[15:02:46.902] frame process finished the id is: 60 the obs is:283 +[15:02:46.944] Odometry x=-0.017988, y=0.817342, theta=0.003177 +[15:02:47.003] Image Alignment failed 0 +[15:02:47.003] frame process finished the id is: 60 the obs is:283 +[15:02:47.042] Odometry x=-0.018613, y=0.825820, theta=0.004213 +[15:02:47.103] Image Alignment failed 0 +[15:02:47.103] frame process finished the id is: 60 the obs is:283 +[15:02:47.146] Odometry x=-0.018624, y=0.831043, theta=0.002388 +[15:02:47.202] Image Alignment failed 0 +[15:02:47.202] frame process finished the id is: 60 the obs is:283 +[15:02:47.254] Odometry x=-0.018637, y=0.838318, theta=0.007977 +[15:02:47.302] Image Alignment failed 0 +[15:02:47.302] frame process finished the id is: 60 the obs is:283 +[15:02:47.340] Odometry x=-0.018651, y=0.844233, theta=0.000391 +[15:02:47.403] Image Alignment failed 0 +[15:02:47.403] frame process finished the id is: 60 the obs is:283 +[15:02:47.453] Odometry x=-0.018651, y=0.849635, theta=0.001258 +[15:02:47.503] Image Alignment failed 0 +[15:02:47.503] frame process finished the id is: 60 the obs is:283 +[15:02:47.546] Odometry x=-0.018651, y=0.855494, theta=-0.000398 +[15:02:47.602] Image Alignment failed 0 +[15:02:47.602] frame process finished the id is: 60 the obs is:283 +[15:02:47.650] Odometry x=-0.018624, y=0.860830, theta=0.002459 +[15:02:47.701] Image Alignment failed 0 +[15:02:47.701] frame process finished the id is: 60 the obs is:283 +[15:02:47.743] Odometry x=-0.018624, y=0.866171, theta=-0.000643 +[15:02:47.802] Image Alignment failed 0 +[15:02:47.802] frame process finished the id is: 60 the obs is:283 +[15:02:47.842] Odometry x=-0.018611, y=0.871314, theta=-0.004974 +[15:02:47.901] Image Alignment failed 0 +[15:02:47.901] frame process finished the id is: 60 the obs is:283 +[15:02:47.938] Odometry x=-0.018561, y=0.877473, theta=-0.000904 +[15:02:48.002] Image Alignment failed 0 +[15:02:48.002] frame process finished the id is: 60 the obs is:283 +[15:02:48.043] Odometry x=-0.018556, y=0.882561, theta=-0.003246 +[15:02:48.102] Image Alignment failed 0 +[15:02:48.102] frame process finished the id is: 60 the obs is:283 +[15:02:48.141] Odometry x=-0.018369, y=0.890705, theta=0.007861 +[15:02:48.201] Image Alignment failed 0 +[15:02:48.201] frame process finished the id is: 60 the obs is:283 +[15:02:48.244] Odometry x=-0.018387, y=0.896529, theta=0.004492 +[15:02:48.303] Image Alignment failed 0 +[15:02:48.303] frame process finished the id is: 60 the obs is:283 +[15:02:48.344] Odometry x=-0.018392, y=0.901747, theta=0.009235 +[15:02:48.401] Image Alignment failed 0 +[15:02:48.401] frame process finished the id is: 60 the obs is:283 +[15:02:48.442] Odometry x=-0.018416, y=0.907566, theta=0.013851 +[15:02:48.502] Image Alignment failed 0 +[15:02:48.502] frame process finished the id is: 60 the obs is:283 +[15:02:48.545] Odometry x=-0.018445, y=0.913172, theta=0.011128 +[15:02:48.601] Image Alignment failed 0 +[15:02:48.602] frame process finished the id is: 60 the obs is:283 +[15:02:48.644] Odometry x=-0.018457, y=0.918758, theta=0.005352 +[15:02:48.702] Image Alignment failed 0 +[15:02:48.702] frame process finished the id is: 60 the obs is:283 +[15:02:48.742] Odometry x=-0.018398, y=0.925069, theta=0.010680 +[15:02:48.801] Image Alignment failed 0 +[15:02:48.801] frame process finished the id is: 60 the obs is:283 +[15:02:48.841] Odometry x=-0.018429, y=0.930449, theta=0.014043 +[15:02:48.902] Image Alignment failed 0 +[15:02:48.902] frame process finished the id is: 60 the obs is:283 +[15:02:48.941] Odometry x=-0.018474, y=0.936549, theta=0.011630 +[15:02:49.002] Image Alignment failed 0 +[15:02:49.002] frame process finished the id is: 60 the obs is:283 +[15:02:49.042] Odometry x=-0.018497, y=0.941787, theta=0.007552 +[15:02:49.101] Image Alignment failed 0 +[15:02:49.101] frame process finished the id is: 60 the obs is:283 +[15:02:49.142] Odometry x=-0.018509, y=0.947396, theta=0.008542 +[15:02:49.202] Image Alignment failed 0 +[15:02:49.202] frame process finished the id is: 60 the obs is:283 +[15:02:49.241] Odometry x=-0.018520, y=0.952811, theta=0.003147 +[15:02:49.302] Image Alignment failed 0 +[15:02:49.302] frame process finished the id is: 60 the obs is:283 +[15:02:49.342] Odometry x=-0.018525, y=0.958351, theta=0.005615 +[15:02:49.402] Image Alignment failed 0 +[15:02:49.402] frame process finished the id is: 60 the obs is:283 +[15:02:49.445] Odometry x=-0.018530, y=0.963667, theta=0.004425 +[15:02:49.502] Image Alignment failed 0 +[15:02:49.502] frame process finished the id is: 60 the obs is:283 +[15:02:49.543] Odometry x=-0.018475, y=0.969575, theta=0.006077 +[15:02:49.602] Image Alignment failed 0 +[15:02:49.602] frame process finished the id is: 60 the obs is:283 +[15:02:49.642] Odometry x=-0.018489, y=0.974854, theta=0.008428 +[15:02:49.702] Image Alignment failed 0 +[15:02:49.702] frame process finished the id is: 60 the obs is:283 +[15:02:49.741] Odometry x=-0.018471, y=0.980747, theta=0.012476 +[15:02:49.803] Image Alignment failed 0 +[15:02:49.803] frame process finished the id is: 60 the obs is:283 +[15:02:49.844] Odometry x=-0.018486, y=0.985976, theta=0.017433 +[15:02:49.902] Image Alignment failed 0 +[15:02:49.902] frame process finished the id is: 60 the obs is:283 +[15:02:49.943] Odometry x=-0.018498, y=0.992197, theta=0.007734 +[15:02:50.002] Image Alignment failed 0 +[15:02:50.002] frame process finished the id is: 60 the obs is:283 +[15:02:50.043] Odometry x=-0.018518, y=0.997488, theta=0.017893 +[15:02:50.104] Image Alignment failed 0 +[15:02:50.105] frame process finished the id is: 60 the obs is:283 +[15:02:50.141] Odometry x=-0.018562, y=1.002538, theta=0.014137 +[15:02:50.202] Image Alignment failed 0 +[15:02:50.202] frame process finished the id is: 60 the obs is:283 +[15:02:50.239] Odometry x=-0.018625, y=1.008338, theta=0.018344 +[15:02:50.304] Image Alignment failed 0 +[15:02:50.304] frame process finished the id is: 60 the obs is:283 +[15:02:50.342] Odometry x=-0.018677, y=1.014313, theta=0.014163 +[15:02:50.402] Image Alignment failed 0 +[15:02:50.402] frame process finished the id is: 60 the obs is:283 +[15:02:50.446] Odometry x=-0.018726, y=1.021691, theta=0.014078 +[15:02:50.502] Image Alignment failed 0 +[15:02:50.502] frame process finished the id is: 60 the obs is:283 +[15:02:50.541] Odometry x=-0.018772, y=1.027988, theta=0.016185 +[15:02:50.602] Image Alignment failed 0 +[15:02:50.602] frame process finished the id is: 60 the obs is:283 +[15:02:50.637] Odometry x=-0.018815, y=1.033143, theta=0.016879 +[15:02:50.701] Image Alignment failed 0 +[15:02:50.701] frame process finished the id is: 60 the obs is:283 +[15:02:50.743] Odometry x=-0.018864, y=1.038662, theta=0.017027 +[15:02:50.803] Image Alignment failed 0 +[15:02:50.803] frame process finished the id is: 60 the obs is:283 +[15:02:50.840] Odometry x=-0.018916, y=1.044836, theta=0.015200 +[15:02:50.903] Image Alignment failed 0 +[15:02:50.903] frame process finished the id is: 60 the obs is:283 +[15:02:50.939] Odometry x=-0.018958, y=1.050842, theta=0.016283 +[15:02:51.003] Image Alignment failed 0 +[15:02:51.003] frame process finished the id is: 60 the obs is:283 +[15:02:51.042] Odometry x=-0.018975, y=1.056412, theta=0.012575 +[15:02:51.103] Image Alignment failed 0 +[15:02:51.103] frame process finished the id is: 60 the obs is:283 +[15:02:51.140] Odometry x=-0.018993, y=1.061858, theta=0.011118 +[15:02:51.203] Image Alignment failed 0 +[15:02:51.203] frame process finished the id is: 60 the obs is:283 +[15:02:51.243] Odometry x=-0.019031, y=1.067322, theta=0.017684 +[15:02:51.303] Image Alignment failed 0 +[15:02:51.303] frame process finished the id is: 60 the obs is:283 +[15:02:51.341] Odometry x=-0.019039, y=1.072570, theta=0.001029 +[15:02:51.403] Image Alignment failed 0 +[15:02:51.403] frame process finished the id is: 60 the obs is:283 +[15:02:51.444] Odometry x=-0.019036, y=1.077929, theta=0.003522 +[15:02:51.503] Image Alignment failed 0 +[15:02:51.503] frame process finished the id is: 60 the obs is:283 +[15:02:51.542] Odometry x=-0.019038, y=1.083616, theta=0.002981 +[15:02:51.602] Image Alignment failed 0 +[15:02:51.602] frame process finished the id is: 60 the obs is:283 +[15:02:51.641] Odometry x=-0.019040, y=1.089041, theta=0.004237 +[15:02:51.703] Image Alignment failed 0 +[15:02:51.703] frame process finished the id is: 60 the obs is:283 +[15:02:51.738] Odometry x=-0.018750, y=1.094763, theta=0.013076 +[15:02:51.803] Image Alignment failed 0 +[15:02:51.803] frame process finished the id is: 60 the obs is:283 +[15:02:51.842] Odometry x=-0.018747, y=1.100249, theta=0.012407 +[15:02:51.895] Image Alignment failed 0 +[15:02:51.895] frame process finished the id is: 60 the obs is:283 +[15:02:51.940] Odometry x=-0.018782, y=1.105745, theta=0.015406 +[15:02:52.002] Image Alignment failed 0 +[15:02:52.002] frame process finished the id is: 60 the obs is:283 +[15:02:52.043] Odometry x=-0.018840, y=1.113004, theta=0.013040 +[15:02:52.102] Image Alignment failed 0 +[15:02:52.102] frame process finished the id is: 60 the obs is:283 +[15:02:52.141] Odometry x=-0.018879, y=1.118721, theta=0.014065 +[15:02:52.202] Image Alignment failed 0 +[15:02:52.202] frame process finished the id is: 60 the obs is:283 +[15:02:52.239] Odometry x=-0.018904, y=1.124638, theta=0.011554 +[15:02:52.303] Image Alignment failed 0 +[15:02:52.303] frame process finished the id is: 60 the obs is:283 +[15:02:52.337] Odometry x=-0.019001, y=1.130013, theta=0.010869 +[15:02:52.396] Image Alignment failed 0 +[15:02:52.397] frame process finished the id is: 60 the obs is:283 +[15:02:52.440] Odometry x=-0.019028, y=1.136042, theta=0.013501 +[15:02:52.496] Image Alignment failed 0 +[15:02:52.496] frame process finished the id is: 60 the obs is:283 +[15:02:52.543] Odometry x=-0.019066, y=1.142499, theta=0.013264 +[15:02:52.597] Image Alignment failed 0 +[15:02:52.597] frame process finished the id is: 60 the obs is:283 +[15:02:52.640] Odometry x=-0.019014, y=1.147863, theta=0.016874 +[15:02:52.703] Image Alignment failed 0 +[15:02:52.703] frame process finished the id is: 60 the obs is:283 +[15:02:52.739] Odometry x=-0.019058, y=1.153477, theta=0.014100 +[15:02:52.803] Image Alignment failed 0 +[15:02:52.803] frame process finished the id is: 60 the obs is:283 +[15:02:52.837] Odometry x=-0.019049, y=1.159098, theta=0.008586 +[15:02:52.905] Image Alignment failed 0 +[15:02:52.905] frame process finished the id is: 60 the obs is:283 +[15:02:52.940] Odometry x=-0.019068, y=1.165262, theta=0.005574 +[15:02:53.007] Image Alignment failed 0 +[15:02:53.007] frame process finished the id is: 60 the obs is:283 +[15:02:53.043] Odometry x=-0.018997, y=1.172285, theta=0.012097 +[15:02:53.102] Image Alignment failed 0 +[15:02:53.102] frame process finished the id is: 60 the obs is:283 +[15:02:53.141] Odometry x=-0.019022, y=1.177918, theta=0.007409 +[15:02:53.202] Image Alignment failed 0 +[15:02:53.202] frame process finished the id is: 60 the obs is:283 +[15:02:53.240] Odometry x=-0.017788, y=1.183254, theta=0.017899 +[15:02:53.302] Image Alignment failed 0 +[15:02:53.302] frame process finished the id is: 60 the obs is:283 +[15:02:53.351] Odometry x=-0.017864, y=1.188499, theta=0.013928 +[15:02:53.401] Image Alignment failed 0 +[15:02:53.401] frame process finished the id is: 60 the obs is:283 +[15:02:53.441] Odometry x=-0.017897, y=1.194303, theta=0.013070 +[15:02:53.502] Image Alignment failed 0 +[15:02:53.502] frame process finished the id is: 60 the obs is:283 +[15:02:53.545] Odometry x=-0.017910, y=1.199407, theta=0.008407 +[15:02:53.603] Image Alignment failed 0 +[15:02:53.603] frame process finished the id is: 60 the obs is:283 +[15:02:53.640] Odometry x=-0.017799, y=1.206116, theta=0.016718 +[15:02:53.701] Image Alignment failed 0 +[15:02:53.701] frame process finished the id is: 60 the obs is:283 +[15:02:53.744] Odometry x=-0.017858, y=1.211559, theta=0.015861 +[15:02:53.802] Image Alignment failed 0 +[15:02:53.802] frame process finished the id is: 60 the obs is:283 +[15:02:53.837] Odometry x=-0.017840, y=1.217057, theta=0.014834 +[15:02:53.904] Image Alignment failed 0 +[15:02:53.904] frame process finished the id is: 60 the obs is:283 +[15:02:53.939] Odometry x=-0.017888, y=1.222219, theta=0.024994 +[15:02:54.003] Image Alignment failed 0 +[15:02:54.003] frame process finished the id is: 60 the obs is:283 +[15:02:54.041] Odometry x=-0.017906, y=1.227813, theta=0.027587 +[15:02:54.105] Image Alignment failed 0 +[15:02:54.105] frame process finished the id is: 60 the obs is:283 +[15:02:54.144] Odometry x=-0.017987, y=1.233306, theta=0.026054 +[15:02:54.202] Image Alignment failed 0 +[15:02:54.202] frame process finished the id is: 60 the obs is:283 +[15:02:54.243] Odometry x=-0.018030, y=1.238982, theta=0.029917 +[15:02:54.302] Image Alignment failed 0 +[15:02:54.302] frame process finished the id is: 60 the obs is:283 +[15:02:54.347] Odometry x=-0.018154, y=1.245547, theta=0.034000 +[15:02:54.398] Image Alignment failed 0 +[15:02:54.398] frame process finished the id is: 60 the obs is:283 +[15:02:54.441] Odometry x=-0.018267, y=1.252039, theta=0.030416 +[15:02:54.504] Image Alignment failed 0 +[15:02:54.504] frame process finished the id is: 60 the obs is:283 +[15:02:54.539] Odometry x=-0.018341, y=1.257371, theta=0.026345 +[15:02:54.603] Image Alignment failed 0 +[15:02:54.603] frame process finished the id is: 60 the obs is:283 +[15:02:54.641] Odometry x=-0.018412, y=1.262770, theta=0.024680 +[15:02:54.704] Image Alignment failed 0 +[15:02:54.704] frame process finished the id is: 60 the obs is:283 +[15:02:54.741] Odometry x=-0.018442, y=1.268200, theta=0.022287 +[15:02:54.804] Image Alignment failed 0 +[15:02:54.804] frame process finished the id is: 60 the obs is:283 +[15:02:54.839] Odometry x=-0.018504, y=1.273495, theta=0.033061 +[15:02:54.902] Image Alignment failed 0 +[15:02:54.902] frame process finished the id is: 60 the obs is:283 +[15:02:54.939] Odometry x=-0.018577, y=1.279143, theta=0.023299 +[15:02:55.003] Image Alignment failed 0 +[15:02:55.003] frame process finished the id is: 60 the obs is:283 +[15:02:55.043] Odometry x=-0.018635, y=1.284496, theta=0.024077 +[15:02:55.103] Image Alignment failed 0 +[15:02:55.104] frame process finished the id is: 60 the obs is:283 +[15:02:55.141] Odometry x=-0.018632, y=1.289876, theta=0.032255 +[15:02:55.205] Image Alignment failed 0 +[15:02:55.205] frame process finished the id is: 60 the obs is:283 +[15:02:55.244] Odometry x=-0.018721, y=1.295233, theta=0.032191 +[15:02:55.303] Image Alignment failed 0 +[15:02:55.303] frame process finished the id is: 60 the obs is:283 +[15:02:55.342] Odometry x=-0.018776, y=1.301511, theta=0.022909 +[15:02:55.403] Image Alignment failed 0 +[15:02:55.403] frame process finished the id is: 60 the obs is:283 +[15:02:55.438] Odometry x=-0.018821, y=1.306846, theta=0.021096 +[15:02:55.503] Image Alignment failed 0 +[15:02:55.503] frame process finished the id is: 60 the obs is:283 +[15:02:55.541] Odometry x=-0.018770, y=1.312187, theta=0.028348 +[15:02:55.604] Image Alignment failed 0 +[15:02:55.604] frame process finished the id is: 60 the obs is:283 +[15:02:55.642] Odometry x=-0.018613, y=1.317573, theta=0.019348 +[15:02:55.709] Image Alignment failed 0 +[15:02:55.709] frame process finished the id is: 60 the obs is:283 +[15:02:55.740] Odometry x=-0.018668, y=1.323995, theta=0.019608 +[15:02:55.803] Image Alignment failed 0 +[15:02:55.803] frame process finished the id is: 60 the obs is:283 +[15:02:55.844] Odometry x=-0.018661, y=1.330668, theta=0.025150 +[15:02:55.904] Image Alignment failed 0 +[15:02:55.904] frame process finished the id is: 60 the obs is:283 +[15:02:55.942] Odometry x=-0.018752, y=1.336392, theta=0.030936 +[15:02:56.002] Image Alignment failed 0 +[15:02:56.002] frame process finished the id is: 60 the obs is:283 +[15:02:56.049] Odometry x=-0.018755, y=1.341575, theta=0.030736 +[15:02:56.102] Image Alignment failed 0 +[15:02:56.102] frame process finished the id is: 60 the obs is:283 +[15:02:56.139] Odometry x=-0.018841, y=1.347536, theta=0.025581 +[15:02:56.202] Image Alignment failed 0 +[15:02:56.202] frame process finished the id is: 60 the obs is:283 +[15:02:56.244] Odometry x=-0.018876, y=1.352460, theta=0.032223 +[15:02:56.304] Image Alignment failed 0 +[15:02:56.304] frame process finished the id is: 60 the obs is:283 +[15:02:56.342] Odometry x=-0.018971, y=1.358002, theta=0.033411 +[15:02:56.402] Image Alignment failed 0 +[15:02:56.402] frame process finished the id is: 60 the obs is:283 +[15:02:56.439] Odometry x=-0.019052, y=1.363386, theta=0.029067 +[15:02:56.503] Image Alignment failed 0 +[15:02:56.503] frame process finished the id is: 60 the obs is:283 +[15:02:56.543] Odometry x=-0.019156, y=1.370752, theta=0.016795 +[15:02:56.603] Image Alignment failed 0 +[15:02:56.603] frame process finished the id is: 60 the obs is:283 +[15:02:56.643] Odometry x=-0.019193, y=1.376122, theta=0.018474 +[15:02:56.703] Image Alignment failed 0 +[15:02:56.703] frame process finished the id is: 60 the obs is:283 +[15:02:56.740] Odometry x=-0.019128, y=1.381735, theta=0.024164 +[15:02:56.802] Image Alignment failed 0 +[15:02:56.802] frame process finished the id is: 60 the obs is:283 +[15:02:56.842] Odometry x=-0.019208, y=1.387118, theta=0.021724 +[15:02:56.902] Image Alignment failed 0 +[15:02:56.902] frame process finished the id is: 60 the obs is:283 +[15:02:57.002] Image Alignment failed 0 +[15:02:57.002] frame process finished the id is: 60 the obs is:283 +[15:02:57.102] Image Alignment failed 0 +[15:02:57.102] frame process finished the id is: 60 the obs is:283 +[15:02:57.208] Image Alignment failed 0 +[15:02:57.208] frame process finished the id is: 60 the obs is:283 +[15:02:57.302] Image Alignment failed 0 +[15:02:57.302] frame process finished the id is: 60 the obs is:283 +[15:02:57.403] Image Alignment failed 0 +[15:02:57.403] frame process finished the id is: 60 the obs is:283 +[15:02:57.503] Image Alignment failed 0 +[15:02:57.503] frame process finished the id is: 60 the obs is:283 +[15:02:57.603] Image Alignment failed 0 +[15:02:57.603] frame process finished the id is: 60 the obs is:283 +[15:02:57.702] Image Alignment failed 0 +[15:02:57.702] frame process finished the id is: 60 the obs is:283 +[15:02:57.801] Image Alignment failed 0 +[15:02:57.801] frame process finished the id is: 60 the obs is:283 +[15:02:57.903] Image Alignment failed 0 +[15:02:57.903] frame process finished the id is: 60 the obs is:283 +[15:02:58.001] Image Alignment failed 0 +[15:02:58.001] frame process finished the id is: 60 the obs is:283 +[15:02:58.102] Image Alignment failed 0 +[15:02:58.103] frame process finished the id is: 60 the obs is:283 +[15:02:58.203] Image Alignment failed 0 +[15:02:58.203] frame process finished the id is: 60 the obs is:283 +[15:02:58.303] Image Alignment failed 0 +[15:02:58.303] frame process finished the id is: 60 the obs is:283 +[15:02:58.403] Image Alignment failed 0 +[15:02:58.403] frame process finished the id is: 60 the obs is:283 +[15:02:58.503] Image Alignment failed 0 +[15:02:58.503] frame process finished the id is: 60 the obs is:283 +[15:02:58.602] Image Alignment failed 0 +[15:02:58.603] frame process finished the id is: 60 the obs is:283 +[15:02:58.703] Image Alignment failed 0 +[15:02:58.703] frame process finished the id is: 60 the obs is:283 +[15:02:58.800] Image Alignment failed 0 +[15:02:58.800] frame process finished the id is: 60 the obs is:283 +[15:02:58.900] Image Alignment failed 0 +[15:02:58.900] frame process finished the id is: 60 the obs is:283 +[15:02:59.012] Image Alignment failed 0 +[15:02:59.012] frame process finished the id is: 60 the obs is:283 +[15:02:59.102] Image Alignment failed 0 +[15:02:59.102] frame process finished the id is: 60 the obs is:283 +[15:02:59.203] Image Alignment failed 0 +[15:02:59.203] frame process finished the id is: 60 the obs is:283 +[15:02:59.303] Image Alignment failed 0 +[15:02:59.303] frame process finished the id is: 60 the obs is:283 +[15:02:59.403] Image Alignment failed 0 +[15:02:59.403] frame process finished the id is: 60 the obs is:283 +[15:02:59.503] Image Alignment failed 0 +[15:02:59.503] frame process finished the id is: 60 the obs is:283 +[15:02:59.603] Image Alignment failed 0 +[15:02:59.603] frame process finished the id is: 60 the obs is:283 +[15:02:59.703] Image Alignment failed 0 +[15:02:59.703] frame process finished the id is: 60 the obs is:283 +[15:02:59.803] Image Alignment failed 0 +[15:02:59.803] frame process finished the id is: 60 the obs is:283 +[15:02:59.903] Image Alignment failed 0 +[15:02:59.903] frame process finished the id is: 60 the obs is:283 +[15:03:00.003] Image Alignment failed 0 +[15:03:00.003] frame process finished the id is: 60 the obs is:283 +[15:03:00.102] Image Alignment failed 0 +[15:03:00.102] frame process finished the id is: 60 the obs is:283 +[15:03:00.203] Image Alignment failed 0 +[15:03:00.203] frame process finished the id is: 60 the obs is:283 +[15:03:00.303] Image Alignment failed 0 +[15:03:00.303] frame process finished the id is: 60 the obs is:283 +[15:03:00.402] Image Alignment failed 0 +[15:03:00.402] frame process finished the id is: 60 the obs is:283 +[15:03:00.502] Image Alignment failed 0 +[15:03:00.502] frame process finished the id is: 60 the obs is:283 +[15:03:00.604] Image Alignment failed 0 +[15:03:00.604] frame process finished the id is: 60 the obs is:283 +[15:03:00.703] Image Alignment failed 0 +[15:03:00.703] frame process finished the id is: 60 the obs is:283 +[15:03:00.804] Image Alignment failed 0 +[15:03:00.804] frame process finished the id is: 60 the obs is:283 +[15:03:00.903] Image Alignment failed 0 +[15:03:00.903] frame process finished the id is: 60 the obs is:283 +[15:03:01.004] Image Alignment failed 0 +[15:03:01.004] frame process finished the id is: 60 the obs is:283 +[15:03:01.104] Image Alignment failed 0 +[15:03:01.104] frame process finished the id is: 60 the obs is:283 +[15:03:01.203] Image Alignment failed 0 +[15:03:01.203] frame process finished the id is: 60 the obs is:283 +[15:03:01.304] Image Alignment failed 0 +[15:03:01.304] frame process finished the id is: 60 the obs is:283 +[15:03:01.403] Image Alignment failed 0 +[15:03:01.403] frame process finished the id is: 60 the obs is:283 +[15:03:01.502] Image Alignment failed 0 +[15:03:01.503] frame process finished the id is: 60 the obs is:283 +[15:03:01.604] Image Alignment failed 0 +[15:03:01.604] frame process finished the id is: 60 the obs is:283 +[15:03:01.703] Image Alignment failed 0 +[15:03:01.703] frame process finished the id is: 60 the obs is:283 +[15:03:01.804] Image Alignment failed 0 +[15:03:01.804] frame process finished the id is: 60 the obs is:283 +[15:03:01.904] Image Alignment failed 0 +[15:03:01.904] frame process finished the id is: 60 the obs is:283 diff --git a/GPU_version/vio/include/vio/frame.h b/GPU_version/vio/include/vio/frame.h index 41aaaae..a80c6fd 100644 --- a/GPU_version/vio/include/vio/frame.h +++ b/GPU_version/vio/include/vio/frame.h @@ -50,7 +50,7 @@ namespace vio { Features fts_; //!< List of features in the image. vector> key_pts_; //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view. bool is_keyframe_; //!< Was this frames selected as keyframe? - std::shared_ptr v_kf_; //!< Temporary pointer to the g2o node object of the keyframe. + g2o::VertexSE3Expmap* v_kf_=nullptr; //!< Temporary pointer to the g2o node object of the keyframe. int last_published_ts_; //!< Timestamp of last publishing. Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp); diff --git a/GPU_version/vio/include/vio/global_optimizer.h b/GPU_version/vio/include/vio/global_optimizer.h index 06b6e08..b87f547 100644 --- a/GPU_version/vio/include/vio/global_optimizer.h +++ b/GPU_version/vio/include/vio/global_optimizer.h @@ -34,6 +34,11 @@ #include #include #include +#include +#include +#include + +G2O_USE_OPTIMIZATION_LIBRARY(cholmod) namespace vio { @@ -76,7 +81,7 @@ protected: #endif /// Temporary container to hold the g2o edge with reference to frame and point. struct EdgeContainerSE3{ - std::shared_ptr edge; + g2o::EdgeProjectXYZ2UV* edge; std::shared_ptr frame; std::shared_ptr feature; }; @@ -90,19 +95,19 @@ protected: double& final_error); /// Create a g2o vertice from a keyframe object. - std::shared_ptr createG2oFrameSE3( + g2o::VertexSE3Expmap* createG2oFrameSE3( FramePtr kf, size_t id, bool fixed); /// Creates a g2o vertice from a mappoint object. - std::shared_ptr createG2oPoint( + g2o::VertexSBAPointXYZ* createG2oPoint( Vector3d pos, size_t id, bool fixed); /// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement. - std::shared_ptr createG2oEdgeSE3( - std::shared_ptr v_kf, - std::shared_ptr v_mp, + g2o::EdgeProjectXYZ2UV* createG2oEdgeSE3( + g2o::VertexSE3Expmap* v_kf, + g2o::VertexSBAPointXYZ* v_mp, const Vector2d& f_up, bool robust_kernel, double huber_width, diff --git a/GPU_version/vio/include/vio/point.h b/GPU_version/vio/include/vio/point.h index 0aebe4e..cb0f3a2 100644 --- a/GPU_version/vio/include/vio/point.h +++ b/GPU_version/vio/include/vio/point.h @@ -51,7 +51,7 @@ public: bool normal_set_; //!< Flag whether the surface normal was estimated or not. list> obs_; //!< References to keyframes which observe the point. size_t n_obs_; //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames. - std::shared_ptr v_pt_; //!< Temporary pointer to the point-vertex in g2o during bundle adjustment. + g2o::VertexSBAPointXYZ* v_pt_=nullptr; //!< Temporary pointer to the point-vertex in g2o during bundle adjustment. int last_published_ts_; //!< Timestamp of last publishing. int last_frame_overlap_id_; //!< Flag for the reprojection: don't reproject a pt twice. PointType type_; //!< Quality of the point. diff --git a/GPU_version/vio/src/feature_detection.cpp b/GPU_version/vio/src/feature_detection.cpp index 525193f..2f4db73 100644 --- a/GPU_version/vio/src/feature_detection.cpp +++ b/GPU_version/vio/src/feature_detection.cpp @@ -76,6 +76,7 @@ void FastDetector::detect( fts.clear(); for(int L=0; Limg_pyr.size())return; const int scale = (1<reload_buf(0,0,img)){ @@ -98,6 +99,7 @@ void FastDetector::detect( } for(uint i=0;iimg_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<1){ continue; } diff --git a/GPU_version/vio/src/global_optimizer.cpp b/GPU_version/vio/src/global_optimizer.cpp index cd0e0c5..463c344 100644 --- a/GPU_version/vio/src/global_optimizer.cpp +++ b/GPU_version/vio/src/global_optimizer.cpp @@ -33,7 +33,7 @@ namespace vio { BA_Glob::BA_Glob(Map& map) :map_(map),thread_(NULL) { - setupG2o(); + #if VIO_DEBUG log_ =fopen((std::string(PROJECT_DIR)+"/depth_filter_log.txt").c_str(),"w+"); assert(log_); @@ -77,34 +77,48 @@ namespace vio { vio::time_in_HH_MM_SS_MMM().c_str()); #endif new_keyframe_=false; - std::cerr<<"here\t"; + g2o::SparseOptimizer optimizer; + optimizer.setVerbose(false); + + g2o::BlockSolver_6_3::LinearSolverType * linearSolver=new g2o::LinearSolverCholmod(); + g2o::BlockSolver_6_3 * solver_ptr= new g2o::BlockSolver_6_3(linearSolver); + + g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + + optimizer.setAlgorithm(solver); + + // setup camera + g2o::CameraParameters* cam_params = new g2o::CameraParameters(1.0, Vector2d(0.,0.), 0.); + cam_params->setId(0); + if (!optimizer.addParameter(cam_params)) { + assert(false); + } // init g2o list edges; list< pair> > incorrect_edges; // Go through all Keyframes size_t v_id = 0; point_mut_.lock(); - std::cerr<<"78\t"; for(list::iterator it_kf = map_.keyframes_.begin(); it_kf != map_.keyframes_.end(); ++it_kf) { // New Keyframe Vertex - std::shared_ptr v_kf = createG2oFrameSE3(*it_kf, v_id++, false); + g2o::VertexSE3Expmap* v_kf = createG2oFrameSE3(*it_kf, v_id++, false); (*it_kf)->v_kf_ = v_kf; - optimizer_->addVertex(v_kf.get()); + optimizer.addVertex(v_kf); for(auto&& it_ftr:(*it_kf)->fts_) { // for each keyframe add edges to all observed mapoints std::shared_ptr mp = it_ftr->point; if(mp == NULL) continue; - std::shared_ptr v_mp = mp->v_pt_; + g2o::VertexSBAPointXYZ* v_mp = mp->v_pt_; if(v_mp == NULL) { // mappoint-vertex doesn't exist yet. create a new one: v_mp = createG2oPoint(mp->pos_, v_id++, false); mp->v_pt_ = v_mp; - optimizer_->addVertex(v_mp.get()); + optimizer.addVertex(v_mp); } // Due to merging of mappoints it is possible that references in kfs suddenly @@ -114,7 +128,7 @@ namespace vio { incorrect_edges.push_back(pair>(*it_kf, it_ftr)); else { - std::shared_ptr e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d(it_ftr->f), + g2o::EdgeProjectXYZ2UV* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d(it_ftr->f), true, Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth()); EdgeContainerSE3 edge; @@ -122,11 +136,10 @@ namespace vio { edge.edge=e; edge.feature=it_ftr; edges.push_back(edge); - optimizer_->addEdge(e.get()); + optimizer.addEdge(e); } } } - std::cerr<<"129\t"; // Optimization double init_error=0.0, final_error=0.0; @@ -151,7 +164,6 @@ namespace vio { mp->v_pt_ = NULL; } } - std::cerr<<"154\t"; // Remove Measurements with too large reprojection error for(list< pair> >::iterator it=incorrect_edges.begin(); @@ -165,8 +177,6 @@ namespace vio { map_.removePtFrameRef(it->frame, it->feature); } } - std::cerr<<"168\n"; - optimizer_->clear(); point_mut_.unlock(); } } @@ -174,21 +184,20 @@ namespace vio { void BA_Glob::setupG2o() { // TODO: What's happening with all this HEAP stuff? Memory Leak? - optimizer_=std::make_shared(); +/* optimizer_=std::make_shared(); optimizer_->setVerbose(false); - using LinearSolver = g2o::LinearSolverCholmod; - Levenberg_ = std::make_shared( - g2o::make_unique(g2o::make_unique())); + auto linearSolver = g2o::make_unique>(); + g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( + g2o::make_unique(std::move(linearSolver))); - Levenberg_->setMaxTrialsAfterFailure(5); - optimizer_->setAlgorithm(Levenberg_.get()); + optimizer_->setAlgorithm(solver); // setup camera std::shared_ptr cam_params = std::make_shared(1.0, Vector2d(0.,0.), 0.); cam_params->setId(0); if (!optimizer_->addParameter(cam_params.get())) { - assert(false); - } + assert(false);*/ + // } } void @@ -202,42 +211,45 @@ namespace vio { final_error = optimizer_->activeChi2(); } - std::shared_ptr + g2o::VertexSE3Expmap* BA_Glob::createG2oFrameSE3(FramePtr frame, size_t id, bool fixed) { - std::shared_ptr v=std::make_shared(); + g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap(); v->setId(id); v->setFixed(fixed); v->setEstimate(g2o::SE3Quat(frame->T_f_w_.se3().unit_quaternion(), frame->T_f_w_.se3().translation())); return v; } - std::shared_ptr + g2o::VertexSBAPointXYZ* BA_Glob::createG2oPoint(Vector3d pos, size_t id, bool fixed) { - std::shared_ptr v =std::make_shared(); + g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ(); v->setId(id); v->setFixed(fixed); v->setEstimate(pos); return v; + } - std::shared_ptr BA_Glob::createG2oEdgeSE3( std::shared_ptr v_frame, - std::shared_ptr v_point, + g2o::EdgeProjectXYZ2UV* BA_Glob::createG2oEdgeSE3( g2o::VertexSE3Expmap* v_frame, + g2o::VertexSBAPointXYZ* v_point, const Vector2d& f_up, bool robust_kernel, double huber_width, double weight) { - std::shared_ptr e=std::make_shared(); - e->setVertex(0, (g2o::OptimizableGraph::Vertex*)(v_point).get()); - e->setVertex(1, (g2o::OptimizableGraph::Vertex*)(v_frame).get()); - e->setMeasurement(f_up); - e->information() = weight * Eigen::Matrix2d::Identity(2,2); - - e->setRobustKernel(new g2o::RobustKernelHuber()); + g2o::EdgeProjectXYZ2UV* e= new g2o::EdgeProjectXYZ2UV; + e->setVertex(0, dynamic_cast(v_point)); + e->setVertex(1, dynamic_cast(v_frame)); + e->setMeasurement(Vector2d(static_cast(g2o::Sampler::uniformRand(0, 640)), + static_cast(g2o::Sampler::uniformRand(0, 480)))); + e->information() = Eigen::Matrix2d::Identity(); + g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; + rk->setDelta(huber_width); + e->setRobustKernel(rk); e->setParameterId(0, 0); //old: e->setId(v_point->id()); return e; }