From 060d58536b22ce8e47306632587e639d6e5772c9 Mon Sep 17 00:00:00 2001 From: gfuse Date: Mon, 4 Oct 2021 21:14:59 +0200 Subject: [PATCH] debugging opencl --- GPU_version/vio/depth_filter_log.txt | 136 -- GPU_version/vio/frame_handler_log.txt | 1915 ++---------------- GPU_version/vio/include/vio/cl_class.h | 143 +- GPU_version/vio/launch/Px30.launch | 2 +- GPU_version/vio/param/vo_fast.yaml | 4 +- GPU_version/vio/src/cl_class.cpp | 9 +- GPU_version/vio/src/feature_detection.cpp | 37 +- GPU_version/vio/src/frame_handler_mono.cpp | 33 - GPU_version/vio/src/sparse_img_align_gpu.cpp | 85 +- 9 files changed, 344 insertions(+), 2020 deletions(-) diff --git a/GPU_version/vio/depth_filter_log.txt b/GPU_version/vio/depth_filter_log.txt index bfe23b7..e69de29 100755 --- a/GPU_version/vio/depth_filter_log.txt +++ b/GPU_version/vio/depth_filter_log.txt @@ -1,136 +0,0 @@ -[11:32:26.215] New key frame -[11:32:26.215] BA loop run -[11:32:26.217] init error: 8.472095 -[11:32:26.222] end error: 0.002451 -[11:32:26.332] New key frame -[11:32:26.332] BA loop run -[11:32:26.334] init error: 12.629072 -[11:32:26.340] end error: 2.173331 -[11:32:26.468] New key frame -[11:32:26.468] BA loop run -[11:32:26.470] init error: 65.010863 -[11:32:26.477] end error: 1.928134 -[11:32:26.617] New key frame -[11:32:26.617] BA loop run -[11:32:26.619] init error: 111.982387 -[11:32:26.627] end error: 8.215693 -[11:32:26.780] New key frame -[11:32:26.780] BA loop run -[11:32:26.782] init error: 133637.531428 -[11:32:26.789] end error: 11.632299 -[11:32:27.443] New key frame -[11:32:27.443] BA loop run -[11:32:27.447] init error: 1319.422136 -[11:32:27.455] end error: 12.848739 -[11:32:27.621] New key frame -[11:32:27.621] BA loop run -[11:32:27.625] init error: 182.371153 -[11:32:27.641] end error: 13.471113 -[11:32:27.930] New key frame -[11:32:27.930] BA loop run -[11:32:27.935] init error: 281.210624 -[11:32:27.952] end error: 70.606181 -[11:32:28.077] New key frame -[11:32:28.077] BA loop run -[11:32:28.082] init error: 309.023063 -[11:32:28.422] end error: 23.878624 -[11:32:28.430] New key frame -[11:32:28.430] BA loop run -[11:32:28.440] init error: 399.805659 -[11:32:29.963] end error: 21.797671 -[11:32:29.976] New key frame -[11:32:29.976] BA loop run -[11:32:29.984] init error: 10406.282878 -[11:32:30.266] end error: 27.018722 -[11:32:30.497] New key frame -[11:32:30.497] BA loop run -[11:32:30.505] init error: 306.484531 -[11:32:30.770] end error: 29.021864 -[11:32:30.843] New key frame -[11:32:30.843] BA loop run -[11:32:30.868] init error: 218.967075 -[11:32:31.623] end error: 38.436167 -[11:32:31.642] New key frame -[11:32:31.642] BA loop run -[11:32:31.668] init error: 242.205017 -[11:32:31.915] end error: 39.613393 -[11:32:32.126] New key frame -[11:32:32.126] BA loop run -[11:32:32.137] init error: 1068.056513 -[11:32:32.471] end error: 32.717972 -[11:32:32.628] New key frame -[11:32:32.628] BA loop run -[11:32:32.640] init error: 205.632654 -[11:32:33.290] end error: 46.394407 -[11:32:33.307] New key frame -[11:32:33.307] BA loop run -[11:32:33.327] init error: 257.984168 -[11:32:34.235] end error: 45.118705 -[11:32:34.253] New key frame -[11:32:34.253] BA loop run -[11:32:34.269] init error: 378.196667 -[11:32:35.135] end error: 72.485312 -[11:32:35.152] New key frame -[11:32:35.152] BA loop run -[11:32:35.166] init error: 2940.286773 -[11:32:36.051] end error: 70.470916 -[11:32:36.064] New key frame -[11:32:36.064] BA loop run -[11:32:36.098] init error: 175.651875 -[11:32:36.564] end error: 51.774350 -[11:32:36.576] New key frame -[11:32:36.576] BA loop run -[11:32:36.594] init error: 212.690168 -[11:32:37.025] end error: 60.040802 -[11:32:37.035] New key frame -[11:32:37.035] BA loop run -[11:32:37.065] init error: 224.779653 -[11:32:37.748] end error: 50.944995 -[11:32:37.762] New key frame -[11:32:37.762] BA loop run -[11:32:37.799] init error: 144.458647 -[11:32:38.281] end error: 55.943794 -[11:32:38.292] New key frame -[11:32:38.292] BA loop run -[11:32:38.308] init error: 417.348226 -[11:32:38.776] end error: 65.937311 -[11:32:38.793] New key frame -[11:32:38.793] BA loop run -[11:32:38.826] init error: 321.374078 -[11:32:39.508] end error: 117.666568 -[11:32:39.522] New key frame -[11:32:39.522] BA loop run -[11:32:39.560] init error: 582.482623 -[11:32:40.106] end error: 56.439056 -[11:32:40.120] New key frame -[11:32:40.120] BA loop run -[11:32:40.145] init error: 406.636760 -[11:32:40.672] end error: 56.901814 -[11:32:40.685] New key frame -[11:32:40.685] BA loop run -[11:32:40.728] init error: 855.372149 -[11:32:41.341] end error: 65.671640 -[11:32:41.357] New key frame -[11:32:41.357] BA loop run -[11:32:41.397] init error: 587.088866 -[11:32:41.858] end error: 114.055639 -[11:32:41.936] New key frame -[11:32:41.936] BA loop run -[11:32:41.963] init error: 439.136273 -[11:32:42.631] end error: 63.348724 -[11:32:42.646] New key frame -[11:32:42.646] BA loop run -[11:32:42.681] init error: 661.332414 -[11:32:43.163] end error: 88.189274 -[11:32:43.327] New key frame -[11:32:43.327] BA loop run -[11:32:43.341] init error: 526.133967 -[11:32:44.023] end error: 79.628375 -[11:32:44.038] New key frame -[11:32:44.038] BA loop run -[11:32:44.060] init error: 490.175656 -[11:32:44.972] end error: 50.982413 -[11:32:44.982] New key frame -[11:32:44.982] BA loop run -[11:32:45.005] init error: 610.616898 -[11:32:45.434] end error: 57.851118 diff --git a/GPU_version/vio/frame_handler_log.txt b/GPU_version/vio/frame_handler_log.txt index 6ee0ce5..aa7f502 100755 --- a/GPU_version/vio/frame_handler_log.txt +++ b/GPU_version/vio/frame_handler_log.txt @@ -1,1704 +1,211 @@ -[11:32:17.330] Init: frame zero: 0.000000, 0.000000 0.000000 -[11:32:17.330] Init: Selected first frame. -[11:32:17.330] frame process finished the id is: 0 the obs is:0 -[11:32:17.354] Init: px average disparity is:0.096920 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:17.354] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:17.354] frame process finished the id is: 1 the obs is:0 -[11:32:17.448] Init: px average disparity is:0.120159 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:17.448] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:17.448] frame process finished the id is: 2 the obs is:0 -[11:32:17.549] Init: px average disparity is:0.166353 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:17.549] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:17.549] frame process finished the id is: 3 the obs is:0 -[11:32:17.650] Init: px average disparity is:0.191240 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:17.650] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:17.650] frame process finished the id is: 4 the obs is:0 -[11:32:17.746] Init: px average disparity is:0.207745 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:17.746] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:17.746] frame process finished the id is: 5 the obs is:0 -[11:32:17.854] Init: px average disparity is:0.227178 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:17.854] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:17.854] frame process finished the id is: 6 the obs is:0 -[11:32:17.956] Init: px average disparity is:0.236689 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:17.956] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:17.956] frame process finished the id is: 7 the obs is:0 -[11:32:18.057] Init: px average disparity is:0.269724 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:18.057] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:18.057] frame process finished the id is: 8 the obs is:0 -[11:32:18.159] Init: px average disparity is:0.279740 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:18.159] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:18.159] frame process finished the id is: 9 the obs is:0 -[11:32:18.259] Init: px average disparity is:0.324433 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:18.259] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:18.259] frame process finished the id is: 10 the obs is:0 -[11:32:18.359] Init: px average disparity is:0.332425 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:18.359] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:18.359] frame process finished the id is: 11 the obs is:0 -[11:32:18.458] Init: px average disparity is:0.360695 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:18.458] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:18.459] frame process finished the id is: 12 the obs is:0 -[11:32:18.560] Init: px average disparity is:0.331429 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:18.560] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:18.560] frame process finished the id is: 13 the obs is:0 -[11:32:18.658] Init: px average disparity is:0.364890 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:18.658] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:18.658] frame process finished the id is: 14 the obs is:0 -[11:32:18.757] Init: px average disparity is:0.336525 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:18.757] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:18.757] frame process finished the id is: 15 the obs is:0 -[11:32:18.857] Init: px average disparity is:0.393741 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:18.857] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:18.857] frame process finished the id is: 16 the obs is:0 -[11:32:18.958] Init: px average disparity is:0.365818 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:18.958] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:18.958] frame process finished the id is: 17 the obs is:0 -[11:32:19.056] Init: px average disparity is:0.362293 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:19.056] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:19.056] frame process finished the id is: 18 the obs is:0 -[11:32:19.158] Init: px average disparity is:0.361204 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:19.158] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:19.158] frame process finished the id is: 19 the obs is:0 -[11:32:19.246] Init: px average disparity is:0.348439 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:19.246] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:19.246] frame process finished the id is: 20 the obs is:0 -[11:32:19.337] Init: px average disparity is:0.350235 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:19.337] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:19.337] frame process finished the id is: 21 the obs is:0 -[11:32:19.458] Init: px average disparity is:0.355781 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:19.458] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:19.458] frame process finished the id is: 22 the obs is:0 -[11:32:19.559] Init: px average disparity is:0.350137 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:19.559] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:19.559] frame process finished the id is: 23 the obs is:0 -[11:32:19.657] Init: px average disparity is:0.351329 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:19.657] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:19.657] frame process finished the id is: 24 the obs is:0 -[11:32:19.757] Init: px average disparity is:0.309508 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:19.757] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:19.757] frame process finished the id is: 25 the obs is:0 -[11:32:19.858] Init: px average disparity is:0.347856 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:19.858] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:19.858] frame process finished the id is: 26 the obs is:0 -[11:32:19.959] Init: px average disparity is:0.323454 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:19.959] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:19.959] frame process finished the id is: 27 the obs is:0 -[11:32:20.056] Init: px average disparity is:0.334123 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:20.056] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:20.057] frame process finished the id is: 28 the obs is:0 -[11:32:20.161] Init: px average disparity is:0.373803 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:20.161] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:20.161] frame process finished the id is: 29 the obs is:0 -[11:32:20.242] Init: px average disparity is:0.323537 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:20.243] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:20.243] frame process finished the id is: 30 the obs is:0 -[11:32:20.338] Init: px average disparity is:0.339853 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:20.338] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:20.338] frame process finished the id is: 31 the obs is:0 -[11:32:20.440] Init: px average disparity is:0.355549 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:20.440] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:20.440] frame process finished the id is: 32 the obs is:0 -[11:32:20.538] Init: px average disparity is:0.327988 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:20.538] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:20.538] frame process finished the id is: 33 the obs is:0 -[11:32:20.638] Init: px average disparity is:0.334005 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:20.638] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:20.638] frame process finished the id is: 34 the obs is:0 -[11:32:20.739] Init: px average disparity is:0.329450 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:20.739] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:20.739] frame process finished the id is: 35 the obs is:0 -[11:32:20.841] Init: px average disparity is:0.353743 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:20.841] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:20.841] frame process finished the id is: 36 the obs is:0 -[11:32:20.937] Init: px average disparity is:0.325713 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:20.937] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:20.937] frame process finished the id is: 37 the obs is:0 -[11:32:21.038] Init: px average disparity is:0.336127 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:21.038] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:21.038] frame process finished the id is: 38 the obs is:0 -[11:32:21.141] Init: px average disparity is:0.339370 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:21.141] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:21.141] frame process finished the id is: 39 the obs is:0 -[11:32:21.237] Init: px average disparity is:0.341702 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:21.237] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:21.237] frame process finished the id is: 40 the obs is:0 -[11:32:21.422] Init: px average disparity is:0.322163 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:21.422] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:21.422] frame process finished the id is: 41 the obs is:0 -[11:32:21.446] Init: px average disparity is:0.327227 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:21.446] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:21.446] frame process finished the id is: 42 the obs is:0 -[11:32:21.556] Init: px average disparity is:0.337917 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:21.556] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:21.556] frame process finished the id is: 43 the obs is:0 -[11:32:21.655] Init: px average disparity is:0.347184 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:21.656] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:21.656] frame process finished the id is: 44 the obs is:0 -[11:32:21.757] Init: px average disparity is:0.345335 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:21.757] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:21.757] frame process finished the id is: 45 the obs is:0 -[11:32:21.857] Init: px average disparity is:0.349314 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:21.857] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:21.857] frame process finished the id is: 46 the obs is:0 -[11:32:21.959] Init: px average disparity is:0.325428 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:21.959] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:21.959] frame process finished the id is: 47 the obs is:0 -[11:32:22.057] Init: px average disparity is:0.333423 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:22.057] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:22.057] frame process finished the id is: 48 the obs is:0 -[11:32:22.157] Init: px average disparity is:0.322207 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:22.157] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:22.157] frame process finished the id is: 49 the obs is:0 -[11:32:22.259] Init: px average disparity is:0.332989 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:22.259] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:22.259] frame process finished the id is: 50 the obs is:0 -[11:32:22.357] Init: px average disparity is:0.326103 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:22.357] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:22.357] frame process finished the id is: 51 the obs is:0 -[11:32:22.457] Init: px average disparity is:0.338467 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:22.457] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:22.457] frame process finished the id is: 52 the obs is:0 -[11:32:22.558] Init: px average disparity is:0.351026 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:22.558] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:22.558] frame process finished the id is: 53 the obs is:0 -[11:32:22.658] Init: px average disparity is:0.344845 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:22.658] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:22.658] frame process finished the id is: 54 the obs is:0 -[11:32:22.757] Init: px average disparity is:0.332508 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:22.757] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:22.757] frame process finished the id is: 55 the obs is:0 -[11:32:22.857] Init: px average disparity is:0.340312 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:22.858] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:22.858] frame process finished the id is: 56 the obs is:0 -[11:32:22.956] Init: px average disparity is:0.359988 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:22.956] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:22.956] frame process finished the id is: 57 the obs is:0 -[11:32:23.058] Init: px average disparity is:0.342481 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:23.058] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:23.058] frame process finished the id is: 58 the obs is:0 -[11:32:23.157] Init: px average disparity is:0.336252 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:23.157] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:23.157] frame process finished the id is: 59 the obs is:0 -[11:32:23.258] Init: px average disparity is:0.332147 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:23.258] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:23.258] frame process finished the id is: 60 the obs is:0 -[11:32:23.360] Init: px average disparity is:0.330074 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:23.360] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:23.360] frame process finished the id is: 61 the obs is:0 -[11:32:23.462] Init: px average disparity is:0.344951 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:23.462] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:23.462] frame process finished the id is: 62 the obs is:0 -[11:32:23.554] Init: px average disparity is:0.336023 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:23.554] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:23.554] frame process finished the id is: 63 the obs is:0 -[11:32:23.657] Init: px average disparity is:0.336179 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:23.657] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:23.657] frame process finished the id is: 64 the obs is:0 -[11:32:23.756] Init: px average disparity is:0.331562 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:23.756] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:23.756] frame process finished the id is: 65 the obs is:0 -[11:32:23.857] Init: px average disparity is:0.343314 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:23.857] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:23.857] frame process finished the id is: 66 the obs is:0 -[11:32:23.957] Init: px average disparity is:0.344914 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:23.957] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:23.957] frame process finished the id is: 67 the obs is:0 -[11:32:24.058] Init: px average disparity is:0.343931 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:24.058] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:24.058] frame process finished the id is: 68 the obs is:0 -[11:32:24.157] Init: px average disparity is:0.342843 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:24.157] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:24.157] frame process finished the id is: 69 the obs is:0 -[11:32:24.258] Init: px average disparity is:0.319272 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:24.258] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:24.258] frame process finished the id is: 70 the obs is:0 -[11:32:24.358] Init: px average disparity is:0.332929 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:24.358] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:24.358] frame process finished the id is: 71 the obs is:0 -[11:32:24.458] Init: px average disparity is:0.335166 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:24.458] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:24.458] frame process finished the id is: 72 the obs is:0 -[11:32:24.557] Init: px average disparity is:0.322458 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:24.557] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:24.557] frame process finished the id is: 73 the obs is:0 -[11:32:24.671] Init: px average disparity is:0.333361 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:24.671] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:24.671] frame process finished the id is: 74 the obs is:0 -[11:32:24.757] Init: px average disparity is:0.364620 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:24.757] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:24.757] frame process finished the id is: 75 the obs is:0 -[11:32:24.865] Init: px average disparity is:0.361013 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:24.865] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:24.865] frame process finished the id is: 76 the obs is:0 -[11:32:24.956] Init: px average disparity is:0.352700 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:24.957] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:24.957] frame process finished the id is: 77 the obs is:0 -[11:32:25.058] Init: px average disparity is:0.342237 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:25.058] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:25.058] frame process finished the id is: 78 the obs is:0 -[11:32:25.084] Odometry x=0.000000, y=0.000000, theta=-0.000000 -[11:32:25.158] Init: px average disparity is:0.338066 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:25.158] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:25.158] frame process finished the id is: 79 the obs is:0 -[11:32:25.185] Odometry x=0.000001, y=0.031996, theta=-0.000009 -[11:32:25.260] Init: px average disparity is:0.342626 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:25.260] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:25.260] frame process finished the id is: 80 the obs is:0 -[11:32:25.284] Odometry x=0.000006, y=0.059582, theta=0.000466 -[11:32:25.368] Init: px average disparity is:6.551406 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:25.368] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:25.368] frame process finished the id is: 81 the obs is:0 -[11:32:25.382] Odometry x=0.000020, y=0.085205, theta=0.001202 -[11:32:25.460] Init: px average disparity is:3.524687 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:25.460] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:25.460] frame process finished the id is: 82 the obs is:0 -[11:32:25.481] Odometry x=-0.000021, y=0.110075, theta=0.002741 -[11:32:25.560] Init: px average disparity is:5.077399 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:25.560] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:25.560] frame process finished the id is: 83 the obs is:0 -[11:32:25.581] Odometry x=-0.000087, y=0.134640, theta=0.005900 -[11:32:25.662] Init: px average disparity is:4.551499 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:25.662] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:25.662] frame process finished the id is: 84 the obs is:0 -[11:32:25.675] Odometry x=-0.000206, y=0.159072, theta=0.007554 -[11:32:25.764] Init: px average disparity is:6.954029 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:25.764] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:25.764] frame process finished the id is: 85 the obs is:0 -[11:32:25.781] Odometry x=-0.000404, y=0.183499, theta=0.005726 -[11:32:25.862] Init: px average disparity is:8.102174 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:25.862] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:25.862] frame process finished the id is: 86 the obs is:0 -[11:32:25.882] Odometry x=-0.000535, y=0.207888, theta=0.009038 -[11:32:25.986] Odometry x=-0.000728, y=0.232270, theta=0.012466 -[11:32:26.070] Init: Homography RANSAC (inlier) is:0 ,While 50 inliers minimum required. px average disparity is:12.099699 ,While minimum is: 10.000000 KLT tracked: 1212 -[11:32:26.070] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:26.070] frame process finished the id is: 87 the obs is:0 -[11:32:26.082] Odometry x=-0.001044, y=0.256663, theta=0.009062 -[11:32:26.096] Init: px average disparity is:9.941098 ,While minimum is: 10.000000 KLT tracked : 1212 -[11:32:26.096] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[11:32:26.096] frame process finished the id is: 88 the obs is:0 -[11:32:26.180] Odometry x=-0.001251, y=0.281037, theta=0.008997 -[11:32:26.215] Init finished: Homography RANSAC (inlier) is:176 ,While 50 inliers minimum required. px average disparity is:11.320633 ,While minimum is: 10.000000 KLT tracked: 1212 -[11:32:26.215] Init: distance between the first and current frame is x:-0.001474 ,z=0.305438,angle between two frames: 0.011551 -[11:32:26.215] Init: Selected Second frame. The number of features: 176 depth mean:-0.431382 min:-1.054871 -[11:32:26.215] frame process finished the id is: 89 the obs is:176 -[11:32:26.279] Odometry x=-0.001474, y=0.305438, theta=0.011551 -[11:32:26.331] After Reprojection Map nMatches:190 ,distance between ekf and vo x:0.000037 ,z=-0.000043,angle between two frames:0.000112 -[11:32:26.331] n deleted obs = 35 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =155 error init =0.018453 error end=0.018453 -[11:32:26.331] After pose optimization, distance between ekf and vo x:0.000037 ,z=-0.000043,angle between two frames:0.000112 -[11:32:26.331] Update EKF and 3D points the number of feature in the new frame: 155 and number of obs: 155 -[11:32:26.332] need key frame pitch dis: 0.011460 translation dif:0.309115 -[11:32:26.332] Choose frame as a key frame Scene Depth mean:-0.286836 ,depth min:-1.022053 -[11:32:26.332] frame process finished the id is: 90 the obs is:155 -[11:32:26.378] Odometry x=-0.001483, y=0.309112, theta=0.011460 -[11:32:26.465] After Reprojection Map nMatches:265 ,distance between ekf and vo x:0.000101 ,z=-0.000225,angle between two frames:0.000306 -[11:32:26.466] n deleted obs = 52 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =213 error init =0.015022 error end=0.014173 -[11:32:26.466] After pose optimization, distance between ekf and vo x:-0.002982 ,z=-0.011829,angle between two frames:0.008450 -[11:32:26.466] Update EKF and 3D points the number of feature in the new frame: 213 and number of obs: 213 -[11:32:26.468] need key frame pitch dis: 0.020496 translation dif:0.109230 -[11:32:26.468] Choose frame as a key frame Scene Depth mean:-0.383560 ,depth min:-4.324615 -[11:32:26.468] frame process finished the id is: 91 the obs is:213 -[11:32:26.481] Odometry x=-0.004041, y=0.303543, theta=0.018824 -[11:32:26.580] Odometry x=-0.004549, y=0.330945, theta=0.018343 -[11:32:26.612] After Reprojection Map nMatches:281 ,distance between ekf and vo x:0.000005 ,z=-0.000015,angle between two frames:0.000011 -[11:32:26.613] n deleted obs = 50 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =231 error init =0.010394 error end=0.010069 -[11:32:26.613] After pose optimization, distance between ekf and vo x:0.001030 ,z=0.000331,angle between two frames:0.002532 -[11:32:26.613] Update EKF and 3D points the number of feature in the new frame: 231 and number of obs: 231 -[11:32:26.617] need key frame pitch dis: 3.102728 translation dif:0.067422 -[11:32:26.617] Choose frame as a key frame Scene Depth mean:-0.236570 ,depth min:-4.251899 -[11:32:26.617] frame process finished the id is: 92 the obs is:231 -[11:32:26.682] Odometry x=-0.003319, y=0.312034, theta=0.023730 -[11:32:26.776] After Reprojection Map nMatches:360 ,distance between ekf and vo x:0.000010 ,z=-0.000020,angle between two frames:0.000027 -[11:32:26.776] n deleted obs = 91 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =269 error init =0.151705 error end=0.151705 -[11:32:26.776] After pose optimization, distance between ekf and vo x:0.000010 ,z=-0.000020,angle between two frames:0.000027 -[11:32:26.776] Update EKF and 3D points the number of feature in the new frame: 269 and number of obs: 269 -[11:32:26.780] need key frame pitch dis: 3.156125 translation dif:0.431282 -[11:32:26.780] Choose frame as a key frame Scene Depth mean:-0.132685 ,depth min:-9.544774 -[11:32:26.780] frame process finished the id is: 93 the obs is:269 -[11:32:26.781] Odometry x=-0.003402, y=0.316395, theta=0.024196 -[11:32:26.880] Odometry x=-0.004104, y=0.343784, theta=0.015783 -[11:32:26.943] After Reprojection Map nMatches:391 ,distance between ekf and vo x:0.000015 ,z=-0.000098,angle between two frames:0.000007 -[11:32:26.944] n deleted obs = 131 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =260 error init =0.027811 error end=0.027811 -[11:32:26.944] After pose optimization, distance between ekf and vo x:0.000015 ,z=-0.000098,angle between two frames:0.000007 -[11:32:26.944] Update EKF and 3D points the number of feature in the new frame: 260 and number of obs: 260 -[11:32:26.946] need key frame pitch dis: 0.052741 translation dif:0.044082 -[11:32:26.946] frame process finished the id is: 94 the obs is:260 -[11:32:26.982] Odometry x=-0.003562, y=0.324527, theta=0.024337 -[11:32:27.081] Odometry x=-0.004212, y=0.351895, theta=0.027853 -[11:32:27.114] After Reprojection Map nMatches:336 ,distance between ekf and vo x:-0.000007 ,z=-0.000181,angle between two frames:0.000006 -[11:32:27.115] n deleted obs = 64 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =272 error init =0.025647 error end=0.025647 -[11:32:27.115] After pose optimization, distance between ekf and vo x:-0.000007 ,z=-0.000181,angle between two frames:0.000006 -[11:32:27.115] Update EKF and 3D points the number of feature in the new frame: 272 and number of obs: 272 -[11:32:27.117] need key frame pitch dis: 0.053851 translation dif:0.051426 -[11:32:27.117] frame process finished the id is: 95 the obs is:272 -[11:32:27.179] Odometry x=-0.003774, y=0.332586, theta=0.025447 -[11:32:27.277] After Reprojection Map nMatches:392 ,distance between ekf and vo x:-0.000005 ,z=-0.000013,angle between two frames:0.000000 -[11:32:27.278] n deleted obs = 93 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =299 error init =0.029747 error end=0.029747 -[11:32:27.278] After pose optimization, distance between ekf and vo x:-0.000005 ,z=-0.000013,angle between two frames:0.000000 -[11:32:27.278] Update EKF and 3D points the number of feature in the new frame: 299 and number of obs: 299 -[11:32:27.280] need key frame pitch dis: 0.055652 translation dif:0.055478 -[11:32:27.280] frame process finished the id is: 96 the obs is:299 -[11:32:27.292] Odometry x=-0.003879, y=0.336938, theta=0.027248 -[11:32:27.386] Odometry x=-0.004653, y=0.364328, theta=0.022051 -[11:32:27.439] After Reprojection Map nMatches:401 ,distance between ekf and vo x:-0.000002 ,z=-0.000037,angle between two frames:0.000003 -[11:32:27.441] n deleted obs = 62 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =339 error init =0.103242 error end=0.042839 -[11:32:27.441] After pose optimization, distance between ekf and vo x:0.035938 ,z=0.078177,angle between two frames:0.025780 -[11:32:27.441] Update EKF and 3D points the number of feature in the new frame: 339 and number of obs: 339 -[11:32:27.443] need key frame pitch dis: 0.032166 translation dif:0.135588 -[11:32:27.443] Choose frame as a key frame Scene Depth mean:-0.026640 ,depth min:-6.870626 -[11:32:27.443] frame process finished the id is: 97 the obs is:339 -[11:32:27.484] Odometry x=0.026293, y=0.411194, theta=0.003762 -[11:32:27.576] Odometry x=0.026152, y=0.438582, theta=-0.006887 -[11:32:27.617] After Reprojection Map nMatches:411 ,distance between ekf and vo x:0.000034 ,z=-0.000081,angle between two frames:0.000005 -[11:32:27.618] n deleted obs = 80 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =331 error init =0.027805 error end=0.027805 -[11:32:27.618] After pose optimization, distance between ekf and vo x:0.000034 ,z=-0.000081,angle between two frames:0.000005 -[11:32:27.618] Update EKF and 3D points the number of feature in the new frame: 331 and number of obs: 331 -[11:32:27.621] need key frame pitch dis: 3.116656 translation dif:0.246529 -[11:32:27.621] Choose frame as a key frame Scene Depth mean:-0.542208 ,depth min:-72.710638 -[11:32:27.621] frame process finished the id is: 98 the obs is:331 -[11:32:27.679] Odometry x=0.026328, y=0.419340, theta=0.002090 -[11:32:27.779] After Reprojection Map nMatches:401 ,distance between ekf and vo x:0.000073 ,z=-0.000008,angle between two frames:0.000004 -[11:32:27.779] n deleted obs = 87 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =314 error init =2.703475 error end=0.207958 -[11:32:27.779] After pose optimization, distance between ekf and vo x:-0.306685 ,z=-0.220684,angle between two frames:0.050956 -[11:32:27.779] frame process finished the id is: 98 the obs is:331 -[11:32:27.781] Odometry x=0.026231, y=0.446720, theta=-0.005001 -[11:32:27.879] Odometry x=0.026352, y=0.472278, theta=0.001596 -[11:32:27.925] After Reprojection Map nMatches:404 ,distance between ekf and vo x:-0.000018 ,z=-0.000051,angle between two frames:0.000001 -[11:32:27.927] n deleted obs = 89 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =315 error init =0.030853 error end=0.030307 -[11:32:27.927] After pose optimization, distance between ekf and vo x:-0.012245 ,z=-0.004072,angle between two frames:0.000276 -[11:32:27.927] Update EKF and 3D points the number of feature in the new frame: 315 and number of obs: 315 -[11:32:27.930] need key frame pitch dis: 3.128348 translation dif:0.247930 -[11:32:27.930] Choose frame as a key frame Scene Depth mean:-0.262903 ,depth min:-29.491323 -[11:32:27.930] frame process finished the id is: 100 the obs is:315 -[11:32:27.981] Odometry x=0.015898, y=0.450992, theta=-0.005523 -[11:32:28.073] After Reprojection Map nMatches:402 ,distance between ekf and vo x:0.000002 ,z=0.000001,angle between two frames:0.000000 -[11:32:28.074] n deleted obs = 97 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =305 error init =0.036685 error end=0.036685 -[11:32:28.074] After pose optimization, distance between ekf and vo x:0.000002 ,z=0.000001,angle between two frames:0.000000 -[11:32:28.074] Update EKF and 3D points the number of feature in the new frame: 305 and number of obs: 305 -[11:32:28.077] need key frame pitch dis: 3.101580 translation dif:0.543103 -[11:32:28.077] Choose frame as a key frame Scene Depth mean:-0.591945 ,depth min:-84.031884 -[11:32:28.077] frame process finished the id is: 101 the obs is:305 -[11:32:28.078] Odometry x=0.015911, y=0.455374, theta=-0.007854 -[11:32:28.181] Odometry x=0.016234, y=0.482763, theta=0.013110 -[11:32:28.274] Odometry x=0.015862, y=0.508324, theta=0.022683 -[11:32:28.386] Odometry x=0.015247, y=0.533170, theta=0.033285 -[11:32:28.402] After Reprojection Map nMatches:430 ,distance between ekf and vo x:-0.000001 ,z=0.000018,angle between two frames:0.000000 -[11:32:28.425] n deleted obs = 108 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =322 error init =0.024234 error end=0.024234 -[11:32:28.425] After pose optimization, distance between ekf and vo x:-0.000001 ,z=0.000018,angle between two frames:0.000000 -[11:32:28.425] Update EKF and 3D points the number of feature in the new frame: 322 and number of obs: 322 -[11:32:28.430] need key frame pitch dis: 3.118440 translation dif:0.669385 -[11:32:28.430] Choose frame as a key frame Scene Depth mean:-0.409449 ,depth min:-6.247414 -[11:32:28.430] frame process finished the id is: 102 the obs is:322 -[11:32:28.487] Odometry x=0.015725, y=0.471035, theta=0.000218 -[11:32:28.583] Odometry x=0.015760, y=0.498409, theta=0.008975 -[11:32:28.680] Odometry x=0.015539, y=0.523952, theta=0.001274 -[11:32:28.780] Odometry x=0.015507, y=0.548782, theta=0.005282 -[11:32:28.880] Odometry x=0.015382, y=0.573335, theta=0.005073 -[11:32:28.963] After Reprojection Map nMatches:459 ,distance between ekf and vo x:-0.000015 ,z=-0.000022,angle between two frames:0.000001 -[11:32:28.976] Odometry x=0.015258, y=0.597795, theta=0.009327 -[11:32:29.074] Odometry x=0.015032, y=0.622232, theta=0.011322 -[11:32:29.176] Odometry x=0.014750, y=0.646617, theta=0.013988 -[11:32:29.277] Odometry x=0.014398, y=0.671003, theta=0.015433 -[11:32:29.377] Odometry x=0.014007, y=0.695409, theta=0.012154 -[11:32:29.479] Odometry x=0.013712, y=0.719800, theta=0.014397 -[11:32:29.578] Odometry x=0.013372, y=0.744210, theta=0.017445 -[11:32:29.680] Odometry x=0.012934, y=0.768590, theta=0.017710 -[11:32:29.777] Odometry x=0.012514, y=0.792986, theta=0.021833 -[11:32:29.877] Odometry x=0.011986, y=0.817380, theta=0.024094 -[11:32:29.968] n deleted obs = 83 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =376 error init =0.018778 error end=0.018761 -[11:32:29.968] After pose optimization, distance between ekf and vo x:0.000426 ,z=-0.000365,angle between two frames:0.000428 -[11:32:29.968] Update EKF and 3D points the number of feature in the new frame: 376 and number of obs: 376 -[11:32:29.976] need key frame pitch dis: 3.128084 translation dif:0.641537 -[11:32:29.976] Choose frame as a key frame Scene Depth mean:-0.014445 ,depth min:-6.308068 -[11:32:29.976] frame process finished the id is: 103 the obs is:376 -[11:32:29.978] Odometry x=0.015444, y=0.527244, theta=0.002840 -[11:32:30.075] Odometry x=0.015323, y=0.554601, theta=-0.003745 -[11:32:30.180] Odometry x=0.015418, y=0.580158, theta=-0.007601 -[11:32:30.282] Odometry x=0.015604, y=0.605005, theta=-0.007674 -[11:32:30.374] Odometry x=0.015803, y=0.629567, theta=-0.010975 -[11:32:30.477] Odometry x=0.016091, y=0.654027, theta=-0.012322 -[11:32:30.491] After Reprojection Map nMatches:550 ,distance between ekf and vo x:0.000017 ,z=0.000005,angle between two frames:0.000005 -[11:32:30.492] n deleted obs = 116 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =434 error init =0.010660 error end=0.010660 -[11:32:30.492] After pose optimization, distance between ekf and vo x:0.000017 ,z=0.000005,angle between two frames:0.000005 -[11:32:30.492] Update EKF and 3D points the number of feature in the new frame: 434 and number of obs: 434 -[11:32:30.497] need key frame pitch dis: 3.139714 translation dif:0.475743 -[11:32:30.497] Choose frame as a key frame Scene Depth mean:-0.072726 ,depth min:-6.332547 -[11:32:30.497] frame process finished the id is: 104 the obs is:434 -[11:32:30.579] Odometry x=0.015591, y=0.550309, theta=0.000926 -[11:32:30.681] Odometry x=0.015567, y=0.577669, theta=0.001165 -[11:32:30.780] Odometry x=0.015542, y=0.603245, theta=-0.003613 -[11:32:30.831] After Reprojection Map nMatches:675 ,distance between ekf and vo x:-0.000014 ,z=-0.000001,angle between two frames:0.000001 -[11:32:30.833] n deleted obs = 119 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =556 error init =0.037529 error end=0.037529 -[11:32:30.833] After pose optimization, distance between ekf and vo x:-0.000014 ,z=-0.000001,angle between two frames:0.000001 -[11:32:30.833] Update EKF and 3D points the number of feature in the new frame: 556 and number of obs: 556 -[11:32:30.842] need key frame pitch dis: 3.115849 translation dif:0.455984 -[11:32:30.843] Choose frame as a key frame Scene Depth mean:-0.102017 ,depth min:-9.992358 -[11:32:30.858] frame process finished the id is: 105 the obs is:556 -[11:32:30.882] Odometry x=0.015574, y=0.562243, theta=0.000590 -[11:32:30.980] Odometry x=0.015510, y=0.589601, theta=-0.007756 -[11:32:31.077] Odometry x=0.015699, y=0.615140, theta=-0.006833 -[11:32:31.181] Odometry x=0.015857, y=0.639965, theta=-0.001587 -[11:32:31.282] Odometry x=0.015870, y=0.664517, theta=0.008842 -[11:32:31.378] Odometry x=0.015670, y=0.688967, theta=0.019389 -[11:32:31.487] Odometry x=0.015187, y=0.713374, theta=0.022752 -[11:32:31.509] After Reprojection Map nMatches:602 ,distance between ekf and vo x:-0.000025 ,z=-0.000009,angle between two frames:0.000001 -[11:32:31.580] Odometry x=0.014657, y=0.737765, theta=0.031203 -[11:32:31.630] n deleted obs = 96 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =506 error init =0.007685 error end=0.007681 -[11:32:31.630] After pose optimization, distance between ekf and vo x:-0.000247 ,z=-0.000074,angle between two frames:0.000022 -[11:32:31.630] Update EKF and 3D points the number of feature in the new frame: 506 and number of obs: 506 -[11:32:31.637] need key frame pitch dis: 3.135212 translation dif:0.476473 -[11:32:31.638] Choose frame as a key frame Scene Depth mean:-0.044486 ,depth min:-10.085976 -[11:32:31.658] frame process finished the id is: 106 the obs is:506 -[11:32:31.681] Odometry x=0.015142, y=0.592644, theta=0.006130 -[11:32:31.780] Odometry x=0.014818, y=0.620009, theta=-0.018648 -[11:32:31.882] Odometry x=0.015305, y=0.645550, theta=-0.019617 -[11:32:31.979] Odometry x=0.015746, y=0.670386, theta=-0.012404 -[11:32:32.082] Odometry x=0.016033, y=0.694945, theta=-0.008503 -[11:32:32.120] After Reprojection Map nMatches:688 ,distance between ekf and vo x:-0.000008 ,z=0.000001,angle between two frames:0.000001 -[11:32:32.122] n deleted obs = 165 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =523 error init =0.048481 error end=0.048481 -[11:32:32.122] After pose optimization, distance between ekf and vo x:-0.000008 ,z=0.000001,angle between two frames:0.000001 -[11:32:32.122] Update EKF and 3D points the number of feature in the new frame: 523 and number of obs: 523 -[11:32:32.125] need key frame pitch dis: 3.111961 translation dif:0.512741 -[11:32:32.126] Choose frame as a key frame Scene Depth mean:-14.428677 ,depth min:-922.205821 -[11:32:32.126] frame process finished the id is: 107 the obs is:523 -[11:32:32.177] Odometry x=0.015303, y=0.611991, theta=0.003962 -[11:32:32.280] Odometry x=0.015146, y=0.639362, theta=-0.004296 -[11:32:32.382] Odometry x=0.015239, y=0.664915, theta=-0.000768 -[11:32:32.483] Odometry x=0.015243, y=0.689751, theta=0.000953 -[11:32:32.576] Odometry x=0.015220, y=0.714323, theta=-0.004777 -[11:32:32.622] After Reprojection Map nMatches:704 ,distance between ekf and vo x:-0.000011 ,z=0.000039,angle between two frames:0.000000 -[11:32:32.624] n deleted obs = 180 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =524 error init =0.020284 error end=0.020284 -[11:32:32.624] After pose optimization, distance between ekf and vo x:-0.000011 ,z=0.000039,angle between two frames:0.000000 -[11:32:32.624] Update EKF and 3D points the number of feature in the new frame: 524 and number of obs: 524 -[11:32:32.628] need key frame pitch dis: 3.113562 translation dif:0.523407 -[11:32:32.628] Choose frame as a key frame Scene Depth mean:-15.366708 ,depth min:-923.243252 -[11:32:32.628] frame process finished the id is: 108 the obs is:524 -[11:32:32.676] Odometry x=0.015298, y=0.631368, theta=0.002115 -[11:32:32.777] Odometry x=0.015251, y=0.658724, theta=0.000721 -[11:32:32.880] Odometry x=0.015254, y=0.684280, theta=-0.005880 -[11:32:32.982] Odometry x=0.015399, y=0.709088, theta=0.000264 -[11:32:33.006] After Reprojection Map nMatches:705 ,distance between ekf and vo x:-0.000020 ,z=0.000077,angle between two frames:0.000000 -[11:32:33.080] Odometry x=0.015376, y=0.733651, theta=0.005315 -[11:32:33.178] Odometry x=0.015244, y=0.758103, theta=0.010702 -[11:32:33.280] Odometry x=0.014968, y=0.782522, theta=0.009203 -[11:32:33.297] n deleted obs = 130 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =575 error init =0.017708 error end=0.017704 -[11:32:33.297] After pose optimization, distance between ekf and vo x:-0.004673 ,z=0.002358,angle between two frames:0.000113 -[11:32:33.297] Update EKF and 3D points the number of feature in the new frame: 575 and number of obs: 575 -[11:32:33.306] need key frame pitch dis: 3.072346 translation dif:0.574377 -[11:32:33.306] Choose frame as a key frame Scene Depth mean:-13.031509 ,depth min:-922.274546 -[11:32:33.307] frame process finished the id is: 109 the obs is:575 -[11:32:33.380] Odometry x=0.013565, y=0.769541, theta=0.007495 -[11:32:33.482] Odometry x=0.013411, y=0.793926, theta=0.009405 -[11:32:33.583] Odometry x=0.013160, y=0.818329, theta=0.008947 -[11:32:33.681] Odometry x=0.012966, y=0.842723, theta=0.010498 -[11:32:33.778] Odometry x=0.012732, y=0.867124, theta=0.013822 -[11:32:33.874] Odometry x=0.012407, y=0.891520, theta=0.019807 -[11:32:33.949] After Reprojection Map nMatches:783 ,distance between ekf and vo x:-0.000011 ,z=0.000035,angle between two frames:0.000000 -[11:32:33.980] Odometry x=0.011923, y=0.915916, theta=0.025734 -[11:32:34.079] Odometry x=0.011302, y=0.940301, theta=0.024456 -[11:32:34.176] Odometry x=0.010686, y=0.964651, theta=0.018391 -[11:32:34.244] n deleted obs = 218 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =565 error init =0.022388 error end=0.022387 -[11:32:34.244] After pose optimization, distance between ekf and vo x:-0.000063 ,z=0.000034,angle between two frames:0.000000 -[11:32:34.244] Update EKF and 3D points the number of feature in the new frame: 565 and number of obs: 565 -[11:32:34.252] need key frame pitch dis: 3.094426 translation dif:0.746329 -[11:32:34.253] Choose frame as a key frame Scene Depth mean:-0.109958 ,depth min:-18.970613 -[11:32:34.253] frame process finished the id is: 110 the obs is:565 -[11:32:34.282] Odometry x=0.013013, y=0.803024, theta=0.009316 -[11:32:34.380] Odometry x=0.012727, y=0.830400, theta=0.004532 -[11:32:34.482] Odometry x=0.012600, y=0.855965, theta=0.008531 -[11:32:34.582] Odometry x=0.012402, y=0.880792, theta=0.009045 -[11:32:34.679] Odometry x=0.012171, y=0.905335, theta=0.012706 -[11:32:34.781] Odometry x=0.011856, y=0.929801, theta=0.010278 -[11:32:34.867] After Reprojection Map nMatches:723 ,distance between ekf and vo x:-0.000001 ,z=-0.000099,angle between two frames:0.000000 -[11:32:34.879] Odometry x=0.011622, y=0.954173, theta=0.007220 -[11:32:34.981] Odometry x=0.011446, y=0.978570, theta=0.008312 -[11:32:35.083] Odometry x=0.011237, y=1.002969, theta=0.007838 -[11:32:35.144] n deleted obs = 110 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =613 error init =0.015481 error end=0.015392 -[11:32:35.145] After pose optimization, distance between ekf and vo x:-0.001339 ,z=0.022006,angle between two frames:0.002693 -[11:32:35.145] Update EKF and 3D points the number of feature in the new frame: 613 and number of obs: 613 -[11:32:35.151] need key frame pitch dis: 3.093285 translation dif:0.810285 -[11:32:35.151] Choose frame as a key frame Scene Depth mean:-0.724512 ,depth min:-66.132653 -[11:32:35.152] frame process finished the id is: 111 the obs is:613 -[11:32:35.180] Odometry x=0.011583, y=0.855869, theta=0.006003 -[11:32:35.282] Odometry x=0.011517, y=0.883248, theta=0.021235 -[11:32:35.383] Odometry x=0.011517, y=0.883248, theta=0.021235 -[11:32:35.481] Odometry x=0.010983, y=0.908801, theta=0.017314 -[11:32:35.583] Odometry x=0.010560, y=0.933529, theta=0.007593 -[11:32:35.682] Odometry x=0.010560, y=0.933529, theta=0.007593 -[11:32:35.783] After Reprojection Map nMatches:615 ,distance between ekf and vo x:-0.000013 ,z=0.000009,angle between two frames:0.000000 -[11:32:35.788] Odometry x=0.010560, y=0.955403, theta=0.019692 -[11:32:35.882] Odometry x=0.009969, y=0.977114, theta=0.012441 -[11:32:35.979] Odometry x=0.009619, y=1.000552, theta=0.003926 -[11:32:36.058] n deleted obs = 99 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =516 error init =0.016885 error end=0.016885 -[11:32:36.058] After pose optimization, distance between ekf and vo x:-0.000030 ,z=0.000013,angle between two frames:0.000107 -[11:32:36.058] Update EKF and 3D points the number of feature in the new frame: 516 and number of obs: 516 -[11:32:36.064] need key frame pitch dis: 3.131407 translation dif:0.800670 -[11:32:36.064] Choose frame as a key frame Scene Depth mean:-0.669719 ,depth min:-66.155746 -[11:32:36.064] frame process finished the id is: 112 the obs is:516 -[11:32:36.084] Odometry x=0.011267, y=0.881443, theta=0.007229 -[11:32:36.181] Odometry x=0.011197, y=0.908804, theta=0.028173 -[11:32:36.283] Odometry x=0.010441, y=0.934326, theta=0.041033 -[11:32:36.374] Odometry x=0.009418, y=0.959141, theta=0.040768 -[11:32:36.391] After Reprojection Map nMatches:538 ,distance between ekf and vo x:-0.000025 ,z=0.000010,angle between two frames:0.000002 -[11:32:36.475] Odometry x=0.008451, y=0.983666, theta=0.033538 -[11:32:36.571] n deleted obs = 103 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =435 error init =0.013066 error end=0.013066 -[11:32:36.571] After pose optimization, distance between ekf and vo x:-0.000025 ,z=0.000010,angle between two frames:0.000002 -[11:32:36.571] Update EKF and 3D points the number of feature in the new frame: 435 and number of obs: 435 -[11:32:36.575] need key frame pitch dis: 3.113797 translation dif:0.784460 -[11:32:36.575] Choose frame as a key frame Scene Depth mean:-1.060066 ,depth min:-66.166889 -[11:32:36.576] frame process finished the id is: 113 the obs is:435 -[11:32:36.588] Odometry x=0.010707, y=0.900775, theta=0.010097 -[11:32:36.674] Odometry x=0.010495, y=0.928126, theta=0.022625 -[11:32:36.774] Odometry x=0.009912, y=0.953646, theta=0.028105 -[11:32:36.879] Odometry x=0.009257, y=0.978484, theta=0.023627 -[11:32:36.965] After Reprojection Map nMatches:650 ,distance between ekf and vo x:-0.000002 ,z=-0.000023,angle between two frames:0.000000 -[11:32:36.979] Odometry x=0.008679, y=1.003038, theta=0.027125 -[11:32:37.031] n deleted obs = 167 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =483 error init =0.036249 error end=0.036096 -[11:32:37.031] After pose optimization, distance between ekf and vo x:-0.009956 ,z=0.003148,angle between two frames:0.009344 -[11:32:37.031] Update EKF and 3D points the number of feature in the new frame: 483 and number of obs: 483 -[11:32:37.035] need key frame pitch dis: 3.083646 translation dif:0.779916 -[11:32:37.035] Choose frame as a key frame Scene Depth mean:-0.998488 ,depth min:-66.153800 -[11:32:37.035] frame process finished the id is: 114 the obs is:483 -[11:32:37.088] Odometry x=0.001844, y=0.922761, theta=0.019678 -[11:32:37.180] Odometry x=0.001350, y=0.950090, theta=0.028789 -[11:32:37.280] Odometry x=0.000606, y=0.975621, theta=0.039012 -[11:32:37.380] Odometry x=-0.000339, y=1.000393, theta=0.034075 -[11:32:37.387] After Reprojection Map nMatches:573 ,distance between ekf and vo x:-0.000006 ,z=-0.000004,angle between two frames:0.000000 -[11:32:37.489] Odometry x=-0.001138, y=1.024956, theta=0.019610 -[11:32:37.585] Odometry x=-0.001593, y=1.049412, theta=0.023173 -[11:32:37.677] Odometry x=-0.002172, y=1.073801, theta=0.030600 -[11:32:37.755] n deleted obs = 168 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =405 error init =0.061555 error end=0.060755 -[11:32:37.755] After pose optimization, distance between ekf and vo x:-0.026395 ,z=-0.002455,angle between two frames:0.000594 -[11:32:37.755] Update EKF and 3D points the number of feature in the new frame: 405 and number of obs: 405 -[11:32:37.762] need key frame pitch dis: 3.097624 translation dif:0.807675 -[11:32:37.762] Choose frame as a key frame Scene Depth mean:-0.845741 ,depth min:-30.785237 -[11:32:37.778] frame process finished the id is: 115 the obs is:405 -[11:32:37.789] Odometry x=-0.021233, y=0.947404, theta=0.024379 -[11:32:37.882] Odometry x=-0.021832, y=0.974769, theta=0.034313 -[11:32:37.981] Odometry x=-0.022706, y=1.000311, theta=0.027427 -[11:32:38.082] Odometry x=-0.023339, y=1.025128, theta=0.013473 -[11:32:38.122] After Reprojection Map nMatches:583 ,distance between ekf and vo x:-0.000010 ,z=-0.000007,angle between two frames:0.000000 -[11:32:38.181] Odometry x=-0.023644, y=1.049667, theta=0.004392 -[11:32:38.277] Odometry x=-0.023765, y=1.074117, theta=0.008210 -[11:32:38.287] n deleted obs = 149 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =434 error init =0.035989 error end=0.035964 -[11:32:38.287] After pose optimization, distance between ekf and vo x:-0.001030 ,z=0.000026,angle between two frames:0.000003 -[11:32:38.287] Update EKF and 3D points the number of feature in the new frame: 434 and number of obs: 434 -[11:32:38.292] need key frame pitch dis: 3.021011 translation dif:0.951317 -[11:32:38.292] Choose frame as a key frame Scene Depth mean:-0.706635 ,depth min:-8.704209 -[11:32:38.292] frame process finished the id is: 116 the obs is:434 -[11:32:38.382] Odometry x=-0.023745, y=1.067427, theta=0.024636 -[11:32:38.484] Odometry x=-0.024362, y=1.091799, theta=0.030882 -[11:32:38.578] Odometry x=-0.025108, y=1.116210, theta=0.027117 -[11:32:38.680] Odometry x=-0.025769, y=1.140589, theta=0.014314 -[11:32:38.721] After Reprojection Map nMatches:644 ,distance between ekf and vo x:-0.000017 ,z=0.000018,angle between two frames:0.000001 -[11:32:38.776] Odometry x=-0.026113, y=1.164973, theta=0.009481 -[11:32:38.785] n deleted obs = 168 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =476 error init =0.024380 error end=0.023777 -[11:32:38.785] After pose optimization, distance between ekf and vo x:-0.028703 ,z=0.010089,angle between two frames:0.006334 -[11:32:38.785] Update EKF and 3D points the number of feature in the new frame: 476 and number of obs: 476 -[11:32:38.792] need key frame pitch dis: 3.011969 translation dif:1.038597 -[11:32:38.792] Choose frame as a key frame Scene Depth mean:-0.301384 ,depth min:-11.415562 -[11:32:38.793] frame process finished the id is: 117 the obs is:476 -[11:32:38.883] Odometry x=-0.033585, y=1.148683, theta=0.020053 -[11:32:38.980] Odometry x=-0.034088, y=1.173076, theta=0.024089 -[11:32:39.079] Odometry x=-0.034653, y=1.197466, theta=0.031478 -[11:32:39.182] Odometry x=-0.035354, y=1.221813, theta=0.047994 -[11:32:39.253] After Reprojection Map nMatches:543 ,distance between ekf and vo x:-0.000014 ,z=0.000005,angle between two frames:0.000001 -[11:32:39.286] Odometry x=-0.036498, y=1.246173, theta=0.056335 -[11:32:39.382] Odometry x=-0.037886, y=1.270492, theta=0.057802 -[11:32:39.475] Odometry x=-0.039328, y=1.294802, theta=0.049416 -[11:32:39.516] n deleted obs = 120 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =423 error init =0.018477 error end=0.018477 -[11:32:39.516] After pose optimization, distance between ekf and vo x:-0.000004 ,z=0.000004,angle between two frames:0.000001 -[11:32:39.516] Update EKF and 3D points the number of feature in the new frame: 423 and number of obs: 423 -[11:32:39.522] need key frame pitch dis: 3.139233 translation dif:1.030319 -[11:32:39.522] Choose frame as a key frame Scene Depth mean:-0.546760 ,depth min:-7.250742 -[11:32:39.522] frame process finished the id is: 118 the obs is:423 -[11:32:39.581] Odometry x=-0.043948, y=1.100111, theta=0.018227 -[11:32:39.680] Odometry x=-0.044407, y=1.127479, theta=0.024469 -[11:32:39.780] Odometry x=-0.045023, y=1.153031, theta=0.022350 -[11:32:39.879] Odometry x=-0.045580, y=1.177860, theta=0.032570 -[11:32:39.976] Odometry x=-0.046403, y=1.202389, theta=0.047368 -[11:32:39.983] After Reprojection Map nMatches:596 ,distance between ekf and vo x:-0.000001 ,z=-0.000019,angle between two frames:0.000004 -[11:32:40.078] Odometry x=-0.047573, y=1.226811, theta=0.057833 -[11:32:40.114] n deleted obs = 151 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =445 error init =0.033428 error end=0.032908 -[11:32:40.114] After pose optimization, distance between ekf and vo x:-0.063675 ,z=0.030558,angle between two frames:0.028806 -[11:32:40.114] Update EKF and 3D points the number of feature in the new frame: 445 and number of obs: 445 -[11:32:40.119] need key frame pitch dis: 3.077485 translation dif:1.064497 -[11:32:40.120] Choose frame as a key frame Scene Depth mean:-1.200794 ,depth min:-77.108636 -[11:32:40.120] frame process finished the id is: 119 the obs is:445 -[11:32:40.179] Odometry x=-0.065435, y=1.228667, theta=0.036527 -[11:32:40.277] Odometry x=-0.066336, y=1.253047, theta=0.036169 -[11:32:40.376] Odometry x=-0.067229, y=1.277446, theta=0.033052 -[11:32:40.478] Odometry x=-0.067986, y=1.301811, theta=0.036246 -[11:32:40.578] After Reprojection Map nMatches:595 ,distance between ekf and vo x:0.000009 ,z=0.000071,angle between two frames:0.000001 -[11:32:40.580] Odometry x=-0.068874, y=1.326184, theta=0.034043 -[11:32:40.679] n deleted obs = 177 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =418 error init =0.051282 error end=0.051267 -[11:32:40.679] After pose optimization, distance between ekf and vo x:0.000120 ,z=-0.003071,angle between two frames:0.000096 -[11:32:40.679] Update EKF and 3D points the number of feature in the new frame: 418 and number of obs: 418 -[11:32:40.684] need key frame pitch dis: 3.066082 translation dif:1.100218 -[11:32:40.685] Choose frame as a key frame Scene Depth mean:-1.298467 ,depth min:-78.367347 -[11:32:40.685] frame process finished the id is: 120 the obs is:418 -[11:32:40.687] Odometry x=-0.065967, y=1.244655, theta=0.037604 -[11:32:40.778] Odometry x=-0.066913, y=1.272018, theta=0.051855 -[11:32:40.879] Odometry x=-0.068250, y=1.297552, theta=0.041744 -[11:32:40.978] Odometry x=-0.069255, y=1.322347, theta=0.033247 -[11:32:41.080] Odometry x=-0.070073, y=1.346866, theta=0.027814 -[11:32:41.175] Odometry x=-0.070759, y=1.371278, theta=0.021499 -[11:32:41.215] After Reprojection Map nMatches:695 ,distance between ekf and vo x:0.000015 ,z=0.000101,angle between two frames:0.000001 -[11:32:41.276] Odometry x=-0.071283, y=1.395641, theta=0.022547 -[11:32:41.350] n deleted obs = 179 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =516 error init =0.041752 error end=0.040106 -[11:32:41.350] After pose optimization, distance between ekf and vo x:-0.004389 ,z=-0.010461,angle between two frames:0.000168 -[11:32:41.350] Update EKF and 3D points the number of feature in the new frame: 516 and number of obs: 516 -[11:32:41.357] need key frame pitch dis: 3.151938 translation dif:1.095105 -[11:32:41.357] Choose frame as a key frame Scene Depth mean:-0.834102 ,depth min:-78.310823 -[11:32:41.375] frame process finished the id is: 121 the obs is:516 -[11:32:41.378] Odometry x=-0.070590, y=1.262533, theta=0.035226 -[11:32:41.476] Odometry x=-0.071510, y=1.289900, theta=0.043062 -[11:32:41.583] Odometry x=-0.072652, y=1.315393, theta=0.045364 -[11:32:41.680] Odometry x=-0.073740, y=1.340219, theta=0.037299 -[11:32:41.776] Odometry x=-0.074656, y=1.364754, theta=0.042022 -[11:32:41.880] Odometry x=-0.075686, y=1.389187, theta=0.035830 -[11:32:41.922] After Reprojection Map nMatches:730 ,distance between ekf and vo x:0.000006 ,z=-0.000017,angle between two frames:0.000000 -[11:32:41.928] n deleted obs = 174 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =556 error init =0.018299 error end=0.018266 -[11:32:41.928] After pose optimization, distance between ekf and vo x:0.008122 ,z=-0.002038,angle between two frames:0.000328 -[11:32:41.928] Update EKF and 3D points the number of feature in the new frame: 556 and number of obs: 556 -[11:32:41.935] need key frame pitch dis: 3.129895 translation dif:1.154951 -[11:32:41.936] Choose frame as a key frame Scene Depth mean:-0.414075 ,depth min:-78.379694 -[11:32:41.936] frame process finished the id is: 122 the obs is:556 -[11:32:41.990] Odometry x=-0.064603, y=1.283832, theta=0.036367 -[11:32:42.082] Odometry x=-0.065529, y=1.311166, theta=0.051409 -[11:32:42.350] After Reprojection Map nMatches:695 ,distance between ekf and vo x:0.000013 ,z=-0.000032,angle between two frames:0.000000 -[11:32:42.640] n deleted obs = 162 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =533 error init =0.042343 error end=0.042332 -[11:32:42.640] After pose optimization, distance between ekf and vo x:0.002171 ,z=0.000043,angle between two frames:0.000020 -[11:32:42.640] Update EKF and 3D points the number of feature in the new frame: 533 and number of obs: 533 -[11:32:42.645] need key frame pitch dis: 3.096440 translation dif:1.119709 -[11:32:42.646] Choose frame as a key frame Scene Depth mean:-0.744444 ,depth min:-12.757831 -[11:32:42.662] frame process finished the id is: 123 the obs is:533 -[11:32:43.319] After Reprojection Map nMatches:738 ,distance between ekf and vo x:-0.000003 ,z=-0.000022,angle between two frames:0.000005 -[11:32:43.323] n deleted obs = 149 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =589 error init =0.024365 error end=0.024346 -[11:32:43.323] After pose optimization, distance between ekf and vo x:0.004316 ,z=-0.000675,angle between two frames:0.000443 -[11:32:43.323] Update EKF and 3D points the number of feature in the new frame: 589 and number of obs: 589 -[11:32:43.326] need key frame pitch dis: 3.108392 translation dif:1.085851 -[11:32:43.326] Choose frame as a key frame Scene Depth mean:1.060522 ,depth min:-25.268198 -[11:32:43.327] frame process finished the id is: 124 the obs is:589 -[11:32:43.933] After Reprojection Map nMatches:627 ,distance between ekf and vo x:0.000084 ,z=0.000018,angle between two frames:0.000007 -[11:32:44.031] n deleted obs = 204 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =423 error init =0.056061 error end=0.055962 -[11:32:44.031] After pose optimization, distance between ekf and vo x:0.010728 ,z=-0.023455,angle between two frames:0.000581 -[11:32:44.031] Update EKF and 3D points the number of feature in the new frame: 423 and number of obs: 423 -[11:32:44.037] need key frame pitch dis: 3.101957 translation dif:1.077278 -[11:32:44.037] Choose frame as a key frame Scene Depth mean:3.435905 ,depth min:-17.438895 -[11:32:44.038] frame process finished the id is: 125 the obs is:423 -[11:32:44.659] After Reprojection Map nMatches:573 ,distance between ekf and vo x:0.000024 ,z=0.000002,angle between two frames:0.000000 -[11:32:44.976] n deleted obs = 123 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =450 error init =0.025035 error end=0.025035 -[11:32:44.976] After pose optimization, distance between ekf and vo x:0.000024 ,z=0.000002,angle between two frames:0.000000 -[11:32:44.976] Update EKF and 3D points the number of feature in the new frame: 450 and number of obs: 450 -[11:32:44.981] need key frame pitch dis: 3.075397 translation dif:1.080714 -[11:32:44.981] Choose frame as a key frame Scene Depth mean:11188.199790 ,depth min:-17.424678 -[11:32:44.982] frame process finished the id is: 126 the obs is:450 -[11:32:44.996] Image Alignment failed 0 -[11:32:44.996] frame process finished the id is: 126 the obs is:450 -[11:32:45.062] Image Alignment failed 0 -[11:32:45.062] frame process finished the id is: 126 the obs is:450 -[11:32:45.160] Image Alignment failed 0 -[11:32:45.160] frame process finished the id is: 126 the obs is:450 -[11:32:45.260] Image Alignment failed 0 -[11:32:45.260] frame process finished the id is: 126 the obs is:450 -[11:32:45.361] Image Alignment failed 0 -[11:32:45.361] frame process finished the id is: 126 the obs is:450 -[11:32:45.459] Image Alignment failed 0 -[11:32:45.459] frame process finished the id is: 126 the obs is:450 -[11:32:45.563] Image Alignment failed 0 -[11:32:45.563] frame process finished the id is: 126 the obs is:450 -[11:32:45.664] Image Alignment failed 0 -[11:32:45.664] frame process finished the id is: 126 the obs is:450 -[11:32:45.767] Image Alignment failed 0 -[11:32:45.767] frame process finished the id is: 126 the obs is:450 -[11:32:45.864] Image Alignment failed 0 -[11:32:45.864] frame process finished the id is: 126 the obs is:450 -[11:32:45.965] Image Alignment failed 0 -[11:32:45.965] frame process finished the id is: 126 the obs is:450 -[11:32:46.064] Image Alignment failed 0 -[11:32:46.064] frame process finished the id is: 126 the obs is:450 -[11:32:46.165] Image Alignment failed 0 -[11:32:46.165] frame process finished the id is: 126 the obs is:450 -[11:32:46.264] Image Alignment failed 0 -[11:32:46.264] frame process finished the id is: 126 the obs is:450 -[11:32:46.365] Image Alignment failed 0 -[11:32:46.365] frame process finished the id is: 126 the obs is:450 -[11:32:46.464] Image Alignment failed 0 -[11:32:46.464] frame process finished the id is: 126 the obs is:450 -[11:32:46.565] Image Alignment failed 0 -[11:32:46.565] frame process finished the id is: 126 the obs is:450 -[11:32:46.664] Image Alignment failed 0 -[11:32:46.664] frame process finished the id is: 126 the obs is:450 -[11:32:46.765] Image Alignment failed 0 -[11:32:46.765] frame process finished the id is: 126 the obs is:450 -[11:32:46.865] Image Alignment failed 0 -[11:32:46.865] frame process finished the id is: 126 the obs is:450 -[11:32:46.965] Image Alignment failed 0 -[11:32:46.965] frame process finished the id is: 126 the obs is:450 -[11:32:47.065] Image Alignment failed 0 -[11:32:47.065] frame process finished the id is: 126 the obs is:450 -[11:32:47.167] Image Alignment failed 0 -[11:32:47.167] frame process finished the id is: 126 the obs is:450 -[11:32:47.266] Image Alignment failed 0 -[11:32:47.266] frame process finished the id is: 126 the obs is:450 -[11:32:47.369] Image Alignment failed 0 -[11:32:47.369] frame process finished the id is: 126 the obs is:450 -[11:32:47.464] Image Alignment failed 0 -[11:32:47.464] frame process finished the id is: 126 the obs is:450 -[11:32:47.564] Image Alignment failed 0 -[11:32:47.564] frame process finished the id is: 126 the obs is:450 -[11:32:47.664] Image Alignment failed 0 -[11:32:47.664] frame process finished the id is: 126 the obs is:450 -[11:32:47.764] Image Alignment failed 0 -[11:32:47.764] frame process finished the id is: 126 the obs is:450 -[11:32:47.864] Image Alignment failed 0 -[11:32:47.864] frame process finished the id is: 126 the obs is:450 -[11:32:47.963] Image Alignment failed 0 -[11:32:47.963] frame process finished the id is: 126 the obs is:450 -[11:32:48.064] Image Alignment failed 0 -[11:32:48.065] frame process finished the id is: 126 the obs is:450 -[11:32:48.165] Image Alignment failed 0 -[11:32:48.165] frame process finished the id is: 126 the obs is:450 -[11:32:48.264] Image Alignment failed 0 -[11:32:48.264] frame process finished the id is: 126 the obs is:450 -[11:32:48.365] Image Alignment failed 0 -[11:32:48.365] frame process finished the id is: 126 the obs is:450 -[11:32:48.464] Image Alignment failed 0 -[11:32:48.464] frame process finished the id is: 126 the obs is:450 -[11:32:48.566] Image Alignment failed 0 -[11:32:48.566] frame process finished the id is: 126 the obs is:450 -[11:32:48.665] Image Alignment failed 0 -[11:32:48.665] frame process finished the id is: 126 the obs is:450 -[11:32:48.765] Image Alignment failed 0 -[11:32:48.765] frame process finished the id is: 126 the obs is:450 -[11:32:48.865] Image Alignment failed 0 -[11:32:48.865] frame process finished the id is: 126 the obs is:450 -[11:32:48.965] Image Alignment failed 0 -[11:32:48.965] frame process finished the id is: 126 the obs is:450 -[11:32:49.064] Image Alignment failed 0 -[11:32:49.064] frame process finished the id is: 126 the obs is:450 -[11:32:49.164] Image Alignment failed 0 -[11:32:49.164] frame process finished the id is: 126 the obs is:450 -[11:32:49.264] Image Alignment failed 0 -[11:32:49.264] frame process finished the id is: 126 the obs is:450 -[11:32:49.361] Image Alignment failed 0 -[11:32:49.361] frame process finished the id is: 126 the obs is:450 -[11:32:49.465] Image Alignment failed 0 -[11:32:49.465] frame process finished the id is: 126 the obs is:450 -[11:32:49.565] Image Alignment failed 0 -[11:32:49.566] frame process finished the id is: 126 the obs is:450 -[11:32:49.665] Image Alignment failed 0 -[11:32:49.665] frame process finished the id is: 126 the obs is:450 -[11:32:49.765] Image Alignment failed 0 -[11:32:49.765] frame process finished the id is: 126 the obs is:450 -[11:32:49.865] Image Alignment failed 0 -[11:32:49.865] frame process finished the id is: 126 the obs is:450 -[11:32:49.965] Image Alignment failed 0 -[11:32:49.965] frame process finished the id is: 126 the obs is:450 -[11:32:50.065] Image Alignment failed 0 -[11:32:50.065] frame process finished the id is: 126 the obs is:450 -[11:32:50.165] Image Alignment failed 0 -[11:32:50.165] frame process finished the id is: 126 the obs is:450 -[11:32:50.264] Image Alignment failed 0 -[11:32:50.264] frame process finished the id is: 126 the obs is:450 -[11:32:50.366] Image Alignment failed 0 -[11:32:50.366] frame process finished the id is: 126 the obs is:450 -[11:32:50.465] Image Alignment failed 0 -[11:32:50.465] frame process finished the id is: 126 the obs is:450 -[11:32:50.564] Image Alignment failed 0 -[11:32:50.564] frame process finished the id is: 126 the obs is:450 -[11:32:50.665] Image Alignment failed 0 -[11:32:50.665] frame process finished the id is: 126 the obs is:450 -[11:32:50.766] Image Alignment failed 0 -[11:32:50.766] frame process finished the id is: 126 the obs is:450 -[11:32:50.864] Image Alignment failed 0 -[11:32:50.864] frame process finished the id is: 126 the obs is:450 -[11:32:50.964] Image Alignment failed 0 -[11:32:50.964] frame process finished the id is: 126 the obs is:450 -[11:32:51.064] Image Alignment failed 0 -[11:32:51.065] frame process finished the id is: 126 the obs is:450 -[11:32:51.164] Image Alignment failed 0 -[11:32:51.164] frame process finished the id is: 126 the obs is:450 -[11:32:51.264] Image Alignment failed 0 -[11:32:51.264] frame process finished the id is: 126 the obs is:450 -[11:32:51.365] Image Alignment failed 0 -[11:32:51.365] frame process finished the id is: 126 the obs is:450 -[11:32:51.464] Image Alignment failed 0 -[11:32:51.464] frame process finished the id is: 126 the obs is:450 -[11:32:51.564] Image Alignment failed 0 -[11:32:51.564] frame process finished the id is: 126 the obs is:450 -[11:32:51.665] Image Alignment failed 0 -[11:32:51.665] frame process finished the id is: 126 the obs is:450 -[11:32:51.764] Image Alignment failed 0 -[11:32:51.765] frame process finished the id is: 126 the obs is:450 -[11:32:51.863] Image Alignment failed 0 -[11:32:51.863] frame process finished the id is: 126 the obs is:450 -[11:32:51.964] Image Alignment failed 0 -[11:32:51.964] frame process finished the id is: 126 the obs is:450 -[11:32:52.064] Image Alignment failed 0 -[11:32:52.064] frame process finished the id is: 126 the obs is:450 -[11:32:52.165] Image Alignment failed 0 -[11:32:52.165] frame process finished the id is: 126 the obs is:450 -[11:32:52.265] Image Alignment failed 0 -[11:32:52.265] frame process finished the id is: 126 the obs is:450 -[11:32:52.365] Image Alignment failed 0 -[11:32:52.365] frame process finished the id is: 126 the obs is:450 -[11:32:52.466] Image Alignment failed 0 -[11:32:52.466] frame process finished the id is: 126 the obs is:450 -[11:32:52.565] Image Alignment failed 0 -[11:32:52.565] frame process finished the id is: 126 the obs is:450 -[11:32:52.668] Image Alignment failed 0 -[11:32:52.668] frame process finished the id is: 126 the obs is:450 -[11:32:52.768] Image Alignment failed 0 -[11:32:52.768] frame process finished the id is: 126 the obs is:450 -[11:32:52.867] Image Alignment failed 0 -[11:32:52.867] frame process finished the id is: 126 the obs is:450 -[11:32:52.961] Image Alignment failed 0 -[11:32:52.961] frame process finished the id is: 126 the obs is:450 -[11:32:53.065] Image Alignment failed 0 -[11:32:53.065] frame process finished the id is: 126 the obs is:450 -[11:32:53.165] Image Alignment failed 0 -[11:32:53.165] frame process finished the id is: 126 the obs is:450 -[11:32:53.265] Image Alignment failed 0 -[11:32:53.265] frame process finished the id is: 126 the obs is:450 -[11:32:53.365] Image Alignment failed 0 -[11:32:53.365] frame process finished the id is: 126 the obs is:450 -[11:32:53.465] Image Alignment failed 0 -[11:32:53.465] frame process finished the id is: 126 the obs is:450 -[11:32:53.566] Image Alignment failed 0 -[11:32:53.566] frame process finished the id is: 126 the obs is:450 -[11:32:53.664] Image Alignment failed 0 -[11:32:53.665] frame process finished the id is: 126 the obs is:450 -[11:32:53.765] Image Alignment failed 0 -[11:32:53.765] frame process finished the id is: 126 the obs is:450 -[11:32:53.865] Image Alignment failed 0 -[11:32:53.865] frame process finished the id is: 126 the obs is:450 -[11:32:53.966] Image Alignment failed 0 -[11:32:53.966] frame process finished the id is: 126 the obs is:450 -[11:32:54.065] Image Alignment failed 0 -[11:32:54.065] frame process finished the id is: 126 the obs is:450 -[11:32:54.166] Image Alignment failed 0 -[11:32:54.166] frame process finished the id is: 126 the obs is:450 -[11:32:54.264] Image Alignment failed 0 -[11:32:54.264] frame process finished the id is: 126 the obs is:450 -[11:32:54.367] Image Alignment failed 0 -[11:32:54.367] frame process finished the id is: 126 the obs is:450 -[11:32:54.468] Image Alignment failed 0 -[11:32:54.468] frame process finished the id is: 126 the obs is:450 -[11:32:54.566] Image Alignment failed 0 -[11:32:54.566] frame process finished the id is: 126 the obs is:450 -[11:32:54.666] Image Alignment failed 0 -[11:32:54.666] frame process finished the id is: 126 the obs is:450 -[11:32:54.768] Image Alignment failed 0 -[11:32:54.768] frame process finished the id is: 126 the obs is:450 -[11:32:54.864] Image Alignment failed 0 -[11:32:54.864] frame process finished the id is: 126 the obs is:450 -[11:32:54.966] Image Alignment failed 0 -[11:32:54.966] frame process finished the id is: 126 the obs is:450 -[11:32:55.064] Image Alignment failed 0 -[11:32:55.064] frame process finished the id is: 126 the obs is:450 -[11:32:55.165] Image Alignment failed 0 -[11:32:55.165] frame process finished the id is: 126 the obs is:450 -[11:32:55.264] Image Alignment failed 0 -[11:32:55.264] frame process finished the id is: 126 the obs is:450 -[11:32:55.365] Image Alignment failed 0 -[11:32:55.365] frame process finished the id is: 126 the obs is:450 -[11:32:55.465] Image Alignment failed 0 -[11:32:55.465] frame process finished the id is: 126 the obs is:450 -[11:32:55.566] Image Alignment failed 0 -[11:32:55.566] frame process finished the id is: 126 the obs is:450 -[11:32:55.664] Image Alignment failed 0 -[11:32:55.665] frame process finished the id is: 126 the obs is:450 -[11:32:55.768] Image Alignment failed 0 -[11:32:55.768] frame process finished the id is: 126 the obs is:450 -[11:32:55.866] Image Alignment failed 0 -[11:32:55.866] frame process finished the id is: 126 the obs is:450 -[11:32:55.963] Image Alignment failed 0 -[11:32:55.963] frame process finished the id is: 126 the obs is:450 -[11:32:56.066] Image Alignment failed 0 -[11:32:56.066] frame process finished the id is: 126 the obs is:450 -[11:32:56.167] Image Alignment failed 0 -[11:32:56.167] frame process finished the id is: 126 the obs is:450 -[11:32:56.265] Image Alignment failed 0 -[11:32:56.265] frame process finished the id is: 126 the obs is:450 -[11:32:56.366] Image Alignment failed 0 -[11:32:56.366] frame process finished the id is: 126 the obs is:450 -[11:32:56.466] Image Alignment failed 0 -[11:32:56.466] frame process finished the id is: 126 the obs is:450 -[11:32:56.565] Image Alignment failed 0 -[11:32:56.565] frame process finished the id is: 126 the obs is:450 -[11:32:56.663] Image Alignment failed 0 -[11:32:56.663] frame process finished the id is: 126 the obs is:450 -[11:32:56.767] Image Alignment failed 0 -[11:32:56.767] frame process finished the id is: 126 the obs is:450 -[11:32:56.865] Image Alignment failed 0 -[11:32:56.865] frame process finished the id is: 126 the obs is:450 -[11:32:56.965] Image Alignment failed 0 -[11:32:56.965] frame process finished the id is: 126 the obs is:450 -[11:32:57.066] Image Alignment failed 0 -[11:32:57.066] frame process finished the id is: 126 the obs is:450 -[11:32:57.168] Image Alignment failed 0 -[11:32:57.168] frame process finished the id is: 126 the obs is:450 -[11:32:57.265] Image Alignment failed 0 -[11:32:57.265] frame process finished the id is: 126 the obs is:450 -[11:32:57.365] Image Alignment failed 0 -[11:32:57.366] frame process finished the id is: 126 the obs is:450 -[11:32:57.465] Image Alignment failed 0 -[11:32:57.465] frame process finished the id is: 126 the obs is:450 -[11:32:57.565] Image Alignment failed 0 -[11:32:57.565] frame process finished the id is: 126 the obs is:450 -[11:32:57.665] Image Alignment failed 0 -[11:32:57.665] frame process finished the id is: 126 the obs is:450 -[11:32:57.765] Image Alignment failed 0 -[11:32:57.765] frame process finished the id is: 126 the obs is:450 -[11:32:57.864] Image Alignment failed 0 -[11:32:57.865] frame process finished the id is: 126 the obs is:450 -[11:32:57.966] Image Alignment failed 0 -[11:32:57.966] frame process finished the id is: 126 the obs is:450 -[11:32:58.068] Image Alignment failed 0 -[11:32:58.068] frame process finished the id is: 126 the obs is:450 -[11:32:58.164] Image Alignment failed 0 -[11:32:58.164] frame process finished the id is: 126 the obs is:450 -[11:32:58.264] Image Alignment failed 0 -[11:32:58.264] frame process finished the id is: 126 the obs is:450 -[11:32:58.365] Image Alignment failed 0 -[11:32:58.366] frame process finished the id is: 126 the obs is:450 -[11:32:58.464] Image Alignment failed 0 -[11:32:58.464] frame process finished the id is: 126 the obs is:450 -[11:32:58.565] Image Alignment failed 0 -[11:32:58.565] frame process finished the id is: 126 the obs is:450 -[11:32:58.665] Image Alignment failed 0 -[11:32:58.665] frame process finished the id is: 126 the obs is:450 -[11:32:58.766] Image Alignment failed 0 -[11:32:58.766] frame process finished the id is: 126 the obs is:450 -[11:32:58.865] Image Alignment failed 0 -[11:32:58.865] frame process finished the id is: 126 the obs is:450 -[11:32:58.967] Image Alignment failed 0 -[11:32:58.967] frame process finished the id is: 126 the obs is:450 -[11:32:59.068] Image Alignment failed 0 -[11:32:59.068] frame process finished the id is: 126 the obs is:450 -[11:32:59.167] Image Alignment failed 0 -[11:32:59.167] frame process finished the id is: 126 the obs is:450 -[11:32:59.265] Image Alignment failed 0 -[11:32:59.265] frame process finished the id is: 126 the obs is:450 -[11:32:59.366] Image Alignment failed 0 -[11:32:59.367] frame process finished the id is: 126 the obs is:450 -[11:32:59.466] Image Alignment failed 0 -[11:32:59.466] frame process finished the id is: 126 the obs is:450 -[11:32:59.566] Image Alignment failed 0 -[11:32:59.566] frame process finished the id is: 126 the obs is:450 -[11:32:59.666] Image Alignment failed 0 -[11:32:59.666] frame process finished the id is: 126 the obs is:450 -[11:32:59.767] Image Alignment failed 0 -[11:32:59.767] frame process finished the id is: 126 the obs is:450 -[11:32:59.864] Image Alignment failed 0 -[11:32:59.864] frame process finished the id is: 126 the obs is:450 -[11:32:59.966] Image Alignment failed 0 -[11:32:59.966] frame process finished the id is: 126 the obs is:450 -[11:33:00.067] Image Alignment failed 0 -[11:33:00.067] frame process finished the id is: 126 the obs is:450 -[11:33:00.165] Image Alignment failed 0 -[11:33:00.165] frame process finished the id is: 126 the obs is:450 -[11:33:00.267] Image Alignment failed 0 -[11:33:00.267] frame process finished the id is: 126 the obs is:450 -[11:33:00.365] Image Alignment failed 0 -[11:33:00.365] frame process finished the id is: 126 the obs is:450 -[11:33:00.466] Image Alignment failed 0 -[11:33:00.466] frame process finished the id is: 126 the obs is:450 -[11:33:00.572] Image Alignment failed 0 -[11:33:00.572] frame process finished the id is: 126 the obs is:450 -[11:33:00.658] Image Alignment failed 0 -[11:33:00.658] frame process finished the id is: 126 the obs is:450 -[11:33:00.765] Image Alignment failed 0 -[11:33:00.765] frame process finished the id is: 126 the obs is:450 -[11:33:00.865] Image Alignment failed 0 -[11:33:00.865] frame process finished the id is: 126 the obs is:450 -[11:33:00.965] Image Alignment failed 0 -[11:33:00.965] frame process finished the id is: 126 the obs is:450 -[11:33:01.065] Image Alignment failed 0 -[11:33:01.065] frame process finished the id is: 126 the obs is:450 -[11:33:01.168] Image Alignment failed 0 -[11:33:01.168] frame process finished the id is: 126 the obs is:450 -[11:33:01.265] Image Alignment failed 0 -[11:33:01.265] frame process finished the id is: 126 the obs is:450 -[11:33:01.366] Image Alignment failed 0 -[11:33:01.366] frame process finished the id is: 126 the obs is:450 -[11:33:01.465] Image Alignment failed 0 -[11:33:01.465] frame process finished the id is: 126 the obs is:450 -[11:33:01.566] Image Alignment failed 0 -[11:33:01.566] frame process finished the id is: 126 the obs is:450 -[11:33:01.666] Image Alignment failed 0 -[11:33:01.666] frame process finished the id is: 126 the obs is:450 -[11:33:01.766] Image Alignment failed 0 -[11:33:01.766] frame process finished the id is: 126 the obs is:450 -[11:33:01.867] Image Alignment failed 0 -[11:33:01.867] frame process finished the id is: 126 the obs is:450 -[11:33:01.965] Image Alignment failed 0 -[11:33:01.965] frame process finished the id is: 126 the obs is:450 -[11:33:02.065] Image Alignment failed 0 -[11:33:02.065] frame process finished the id is: 126 the obs is:450 -[11:33:02.167] Image Alignment failed 0 -[11:33:02.167] frame process finished the id is: 126 the obs is:450 -[11:33:02.265] Image Alignment failed 0 -[11:33:02.265] frame process finished the id is: 126 the obs is:450 -[11:33:02.366] Image Alignment failed 0 -[11:33:02.366] frame process finished the id is: 126 the obs is:450 -[11:33:02.466] Image Alignment failed 0 -[11:33:02.466] frame process finished the id is: 126 the obs is:450 -[11:33:02.566] Image Alignment failed 0 -[11:33:02.566] frame process finished the id is: 126 the obs is:450 -[11:33:02.666] Image Alignment failed 0 -[11:33:02.666] frame process finished the id is: 126 the obs is:450 -[11:33:02.767] Image Alignment failed 0 -[11:33:02.767] frame process finished the id is: 126 the obs is:450 -[11:33:02.865] Image Alignment failed 0 -[11:33:02.865] frame process finished the id is: 126 the obs is:450 -[11:33:02.966] Image Alignment failed 0 -[11:33:02.966] frame process finished the id is: 126 the obs is:450 -[11:33:03.065] Image Alignment failed 0 -[11:33:03.065] frame process finished the id is: 126 the obs is:450 -[11:33:03.166] Image Alignment failed 0 -[11:33:03.166] frame process finished the id is: 126 the obs is:450 -[11:33:03.265] Image Alignment failed 0 -[11:33:03.265] frame process finished the id is: 126 the obs is:450 -[11:33:03.367] Image Alignment failed 0 -[11:33:03.368] frame process finished the id is: 126 the obs is:450 -[11:33:03.465] Image Alignment failed 0 -[11:33:03.465] frame process finished the id is: 126 the obs is:450 -[11:33:03.566] Image Alignment failed 0 -[11:33:03.566] frame process finished the id is: 126 the obs is:450 -[11:33:03.668] Image Alignment failed 0 -[11:33:03.669] frame process finished the id is: 126 the obs is:450 -[11:33:03.766] Image Alignment failed 0 -[11:33:03.766] frame process finished the id is: 126 the obs is:450 -[11:33:03.865] Image Alignment failed 0 -[11:33:03.865] frame process finished the id is: 126 the obs is:450 -[11:33:03.966] Image Alignment failed 0 -[11:33:03.966] frame process finished the id is: 126 the obs is:450 -[11:33:04.065] Image Alignment failed 0 -[11:33:04.065] frame process finished the id is: 126 the obs is:450 -[11:33:04.166] Image Alignment failed 0 -[11:33:04.166] frame process finished the id is: 126 the obs is:450 -[11:33:04.273] Image Alignment failed 0 -[11:33:04.273] frame process finished the id is: 126 the obs is:450 -[11:33:04.361] Image Alignment failed 0 -[11:33:04.361] frame process finished the id is: 126 the obs is:450 -[11:33:04.465] Image Alignment failed 0 -[11:33:04.465] frame process finished the id is: 126 the obs is:450 -[11:33:04.567] Image Alignment failed 0 -[11:33:04.567] frame process finished the id is: 126 the obs is:450 -[11:33:04.666] Image Alignment failed 0 -[11:33:04.666] frame process finished the id is: 126 the obs is:450 -[11:33:04.766] Image Alignment failed 0 -[11:33:04.766] frame process finished the id is: 126 the obs is:450 -[11:33:04.865] Image Alignment failed 0 -[11:33:04.865] frame process finished the id is: 126 the obs is:450 -[11:33:04.965] Image Alignment failed 0 -[11:33:04.965] frame process finished the id is: 126 the obs is:450 -[11:33:05.065] Image Alignment failed 0 -[11:33:05.065] frame process finished the id is: 126 the obs is:450 -[11:33:05.165] Image Alignment failed 0 -[11:33:05.165] frame process finished the id is: 126 the obs is:450 -[11:33:05.265] Image Alignment failed 0 -[11:33:05.266] frame process finished the id is: 126 the obs is:450 -[11:33:05.366] Image Alignment failed 0 -[11:33:05.366] frame process finished the id is: 126 the obs is:450 -[11:33:05.466] Image Alignment failed 0 -[11:33:05.466] frame process finished the id is: 126 the obs is:450 -[11:33:05.566] Image Alignment failed 0 -[11:33:05.566] frame process finished the id is: 126 the obs is:450 -[11:33:05.667] Image Alignment failed 0 -[11:33:05.667] frame process finished the id is: 126 the obs is:450 -[11:33:05.766] Image Alignment failed 0 -[11:33:05.767] frame process finished the id is: 126 the obs is:450 -[11:33:05.865] Image Alignment failed 0 -[11:33:05.865] frame process finished the id is: 126 the obs is:450 -[11:33:05.965] Image Alignment failed 0 -[11:33:05.965] frame process finished the id is: 126 the obs is:450 -[11:33:06.065] Image Alignment failed 0 -[11:33:06.065] frame process finished the id is: 126 the obs is:450 -[11:33:06.166] Image Alignment failed 0 -[11:33:06.166] frame process finished the id is: 126 the obs is:450 -[11:33:06.268] Image Alignment failed 0 -[11:33:06.268] frame process finished the id is: 126 the obs is:450 -[11:33:06.341] Odometry x=-0.054308, y=1.292630, theta=0.039501 -[11:33:06.366] Image Alignment failed 0 -[11:33:06.366] frame process finished the id is: 126 the obs is:450 -[11:33:06.442] Odometry x=-0.053236, y=1.265280, theta=0.041255 -[11:33:06.466] Image Alignment failed 0 -[11:33:06.467] frame process finished the id is: 126 the obs is:450 -[11:33:06.542] Odometry x=-0.052176, y=1.239758, theta=0.042955 -[11:33:06.567] Image Alignment failed 0 -[11:33:06.567] frame process finished the id is: 126 the obs is:450 -[11:33:06.642] Odometry x=-0.051104, y=1.214960, theta=0.044360 -[11:33:06.666] Image Alignment failed 0 -[11:33:06.666] frame process finished the id is: 126 the obs is:450 -[11:33:06.742] Odometry x=-0.050025, y=1.190405, theta=0.039098 -[11:33:06.766] Image Alignment failed 0 -[11:33:06.766] frame process finished the id is: 126 the obs is:450 -[11:33:06.841] Odometry x=-0.049080, y=1.165956, theta=0.051278 -[11:33:06.868] Image Alignment failed 0 -[11:33:06.868] frame process finished the id is: 126 the obs is:450 -[11:33:06.942] Odometry x=-0.047836, y=1.141593, theta=0.059902 -[11:33:06.966] Image Alignment failed 0 -[11:33:06.966] frame process finished the id is: 126 the obs is:450 -[11:33:07.042] Odometry x=-0.046343, y=1.117250, theta=0.055289 -[11:33:07.066] Image Alignment failed 0 -[11:33:07.066] frame process finished the id is: 126 the obs is:450 -[11:33:07.141] Odometry x=-0.045005, y=1.092918, theta=0.055711 -[11:33:07.168] Image Alignment failed 0 -[11:33:07.168] frame process finished the id is: 126 the obs is:450 -[11:33:07.240] Odometry x=-0.043604, y=1.068546, theta=0.046945 -[11:33:07.264] Image Alignment failed 0 -[11:33:07.264] frame process finished the id is: 126 the obs is:450 -[11:33:07.341] Odometry x=-0.042517, y=1.044174, theta=0.054174 -[11:33:07.368] Image Alignment failed 0 -[11:33:07.368] frame process finished the id is: 126 the obs is:450 -[11:33:07.444] Odometry x=-0.041165, y=1.019827, theta=0.052163 -[11:33:07.468] Image Alignment failed 0 -[11:33:07.468] frame process finished the id is: 126 the obs is:450 -[11:33:07.538] Odometry x=-0.039922, y=0.995470, theta=0.055324 -[11:33:07.569] Image Alignment failed 0 -[11:33:07.569] frame process finished the id is: 126 the obs is:450 -[11:33:07.643] Odometry x=-0.038618, y=0.971147, theta=0.070483 -[11:33:07.668] Image Alignment failed 0 -[11:33:07.668] frame process finished the id is: 126 the obs is:450 -[11:33:07.742] Odometry x=-0.036866, y=0.946817, theta=0.068634 -[11:33:07.760] Image Alignment failed 0 -[11:33:07.760] frame process finished the id is: 126 the obs is:450 -[11:33:07.841] Odometry x=-0.035243, y=0.922517, theta=0.070405 -[11:33:07.866] Image Alignment failed 0 -[11:33:07.866] frame process finished the id is: 126 the obs is:450 -[11:33:07.941] Odometry x=-0.033545, y=0.898169, theta=0.068827 -[11:33:07.967] Image Alignment failed 0 -[11:33:07.967] frame process finished the id is: 126 the obs is:450 -[11:33:08.037] Odometry x=-0.031831, y=0.873883, theta=0.063407 -[11:33:08.068] Image Alignment failed 0 -[11:33:08.068] frame process finished the id is: 126 the obs is:450 -[11:33:08.141] Odometry x=-0.030286, y=0.849535, theta=0.064626 -[11:33:08.169] Image Alignment failed 0 -[11:33:08.169] frame process finished the id is: 126 the obs is:450 -[11:33:08.241] Odometry x=-0.028705, y=0.825193, theta=0.068556 -[11:33:08.270] Image Alignment failed 0 -[11:33:08.270] frame process finished the id is: 126 the obs is:450 -[11:33:08.346] Odometry x=-0.027060, y=0.800884, theta=0.072221 -[11:33:08.368] Image Alignment failed 0 -[11:33:08.368] frame process finished the id is: 126 the obs is:450 -[11:33:08.441] Odometry x=-0.025261, y=0.776554, theta=0.071650 -[11:33:08.468] Image Alignment failed 0 -[11:33:08.468] frame process finished the id is: 126 the obs is:450 -[11:33:08.541] Odometry x=-0.023492, y=0.752270, theta=0.070596 -[11:33:08.568] Image Alignment failed 0 -[11:33:08.568] frame process finished the id is: 126 the obs is:450 -[11:33:08.641] Odometry x=-0.021751, y=0.727928, theta=0.062211 -[11:33:08.668] Image Alignment failed 0 -[11:33:08.668] frame process finished the id is: 126 the obs is:450 -[11:33:08.743] Odometry x=-0.020230, y=0.703600, theta=0.053469 -[11:33:08.767] Image Alignment failed 0 -[11:33:08.767] frame process finished the id is: 126 the obs is:450 -[11:33:08.841] Odometry x=-0.018937, y=0.679210, theta=0.061983 -[11:33:08.868] Image Alignment failed 0 -[11:33:08.868] frame process finished the id is: 126 the obs is:450 -[11:33:08.941] Odometry x=-0.017426, y=0.654911, theta=0.062654 -[11:33:08.966] Image Alignment failed 0 -[11:33:08.966] frame process finished the id is: 126 the obs is:450 -[11:33:09.041] Odometry x=-0.015898, y=0.630579, theta=0.066373 -[11:33:09.066] Image Alignment failed 0 -[11:33:09.066] frame process finished the id is: 126 the obs is:450 -[11:33:09.143] Odometry x=-0.014276, y=0.606239, theta=0.070622 -[11:33:09.166] Image Alignment failed 0 -[11:33:09.166] frame process finished the id is: 126 the obs is:450 -[11:33:09.243] Odometry x=-0.012504, y=0.581946, theta=0.065702 -[11:33:09.266] Image Alignment failed 0 -[11:33:09.266] frame process finished the id is: 126 the obs is:450 -[11:33:09.344] Odometry x=-0.010922, y=0.557587, theta=0.069493 -[11:33:09.367] Image Alignment failed 0 -[11:33:09.367] frame process finished the id is: 126 the obs is:450 -[11:33:09.442] Odometry x=-0.009248, y=0.533249, theta=0.070172 -[11:33:09.468] Image Alignment failed 0 -[11:33:09.468] frame process finished the id is: 126 the obs is:450 -[11:33:09.542] Odometry x=-0.007516, y=0.508952, theta=0.057413 -[11:33:09.566] Image Alignment failed 0 -[11:33:09.566] frame process finished the id is: 126 the obs is:450 -[11:33:09.642] Odometry x=-0.006123, y=0.484607, theta=0.062585 -[11:33:09.666] Image Alignment failed 0 -[11:33:09.666] frame process finished the id is: 126 the obs is:450 -[11:33:09.742] Odometry x=-0.004593, y=0.460280, theta=0.067243 -[11:33:09.767] Image Alignment failed 0 -[11:33:09.767] frame process finished the id is: 126 the obs is:450 -[11:33:09.842] Odometry x=-0.002956, y=0.435929, theta=0.077865 -[11:33:09.866] Image Alignment failed 0 -[11:33:09.866] frame process finished the id is: 126 the obs is:450 -[11:33:09.941] Odometry x=-0.001054, y=0.411649, theta=0.078179 -[11:33:09.967] Image Alignment failed 0 -[11:33:09.967] frame process finished the id is: 126 the obs is:450 -[11:33:10.041] Odometry x=0.000850, y=0.387339, theta=0.077449 -[11:33:10.066] Image Alignment failed 0 -[11:33:10.066] frame process finished the id is: 126 the obs is:450 -[11:33:10.145] Odometry x=0.002699, y=0.363006, theta=0.083197 -[11:33:10.160] Image Alignment failed 0 -[11:33:10.160] frame process finished the id is: 126 the obs is:450 -[11:33:10.245] Odometry x=0.004764, y=0.338734, theta=0.080568 -[11:33:10.266] Image Alignment failed 0 -[11:33:10.266] frame process finished the id is: 126 the obs is:450 -[11:33:10.344] Odometry x=0.006750, y=0.314444, theta=0.076180 -[11:33:10.366] Image Alignment failed 0 -[11:33:10.366] frame process finished the id is: 126 the obs is:450 -[11:33:10.441] Odometry x=0.008589, y=0.290109, theta=0.076314 -[11:33:10.466] Image Alignment failed 0 -[11:33:10.466] frame process finished the id is: 126 the obs is:450 -[11:33:10.536] Odometry x=0.010438, y=0.265803, theta=0.077942 -[11:33:10.567] Image Alignment failed 0 -[11:33:10.567] frame process finished the id is: 126 the obs is:450 -[11:33:10.642] Odometry x=0.012329, y=0.241493, theta=0.079011 -[11:33:10.666] Image Alignment failed 0 -[11:33:10.666] frame process finished the id is: 126 the obs is:450 -[11:33:10.737] Odometry x=0.014308, y=0.217181, theta=0.070552 -[11:33:10.767] Image Alignment failed 0 -[11:33:10.767] frame process finished the id is: 126 the obs is:450 -[11:33:10.842] Odometry x=0.016012, y=0.192882, theta=0.069323 -[11:33:10.866] Image Alignment failed 0 -[11:33:10.866] frame process finished the id is: 126 the obs is:450 -[11:33:10.947] Odometry x=0.017738, y=0.168552, theta=0.066985 -[11:33:10.967] Image Alignment failed 0 -[11:33:10.967] frame process finished the id is: 126 the obs is:450 -[11:33:11.041] Odometry x=0.019376, y=0.144244, theta=0.056723 -[11:33:11.066] Image Alignment failed 0 -[11:33:11.066] frame process finished the id is: 126 the obs is:450 -[11:33:11.142] Odometry x=0.020776, y=0.119913, theta=0.054775 -[11:33:11.167] Image Alignment failed 0 -[11:33:11.167] frame process finished the id is: 126 the obs is:450 -[11:33:11.236] Odometry x=0.022102, y=0.095588, theta=0.061411 -[11:33:11.266] Image Alignment failed 0 -[11:33:11.266] frame process finished the id is: 126 the obs is:450 -[11:33:11.342] Odometry x=0.023598, y=0.071262, theta=0.065053 -[11:33:11.367] Image Alignment failed 0 -[11:33:11.367] frame process finished the id is: 126 the obs is:450 -[11:33:11.442] Odometry x=0.025154, y=0.046947, theta=0.067525 -[11:33:11.468] Image Alignment failed 0 -[11:33:11.468] frame process finished the id is: 126 the obs is:450 -[11:33:11.540] Odometry x=0.026815, y=0.022627, theta=0.062567 -[11:33:11.569] Image Alignment failed 0 -[11:33:11.569] frame process finished the id is: 126 the obs is:450 -[11:33:11.639] Odometry x=0.028357, y=-0.001699, theta=0.059138 -[11:33:11.669] Image Alignment failed 0 -[11:33:11.669] frame process finished the id is: 126 the obs is:450 -[11:33:11.741] Odometry x=0.029769, y=-0.026026, theta=0.067850 -[11:33:11.768] Image Alignment failed 0 -[11:33:11.768] frame process finished the id is: 126 the obs is:450 -[11:33:11.844] Odometry x=0.031452, y=-0.050344, theta=0.072096 -[11:33:11.866] Image Alignment failed 0 -[11:33:11.866] frame process finished the id is: 126 the obs is:450 -[11:33:11.940] Odometry x=0.033237, y=-0.074681, theta=0.075476 -[11:33:11.966] Image Alignment failed 0 -[11:33:11.966] frame process finished the id is: 126 the obs is:450 -[11:33:12.036] Odometry x=0.035077, y=-0.098978, theta=0.078750 -[11:33:12.067] Image Alignment failed 0 -[11:33:12.067] frame process finished the id is: 126 the obs is:450 -[11:33:12.140] Odometry x=0.036986, y=-0.123292, theta=0.084971 -[11:33:12.166] Image Alignment failed 0 -[11:33:12.166] frame process finished the id is: 126 the obs is:450 -[11:33:12.240] Odometry x=0.039049, y=-0.147622, theta=0.085633 -[11:33:12.267] Image Alignment failed 0 -[11:33:12.267] frame process finished the id is: 126 the obs is:450 -[11:33:12.345] Odometry x=0.041137, y=-0.171922, theta=0.084549 -[11:33:12.372] Image Alignment failed 0 -[11:33:12.372] frame process finished the id is: 126 the obs is:450 -[11:33:12.443] Odometry x=0.043194, y=-0.196212, theta=0.086007 -[11:33:12.457] Image Alignment failed 0 -[11:33:12.457] frame process finished the id is: 126 the obs is:450 -[11:33:12.543] Odometry x=0.045285, y=-0.220499, theta=0.087818 -[11:33:12.567] Image Alignment failed 0 -[11:33:12.567] frame process finished the id is: 126 the obs is:450 -[11:33:12.642] Odometry x=0.047400, y=-0.244819, theta=0.093396 -[11:33:12.666] Image Alignment failed 0 -[11:33:12.666] frame process finished the id is: 126 the obs is:450 -[11:33:12.738] Odometry x=0.049651, y=-0.269119, theta=0.096917 -[11:33:12.767] Image Alignment failed 0 -[11:33:12.767] frame process finished the id is: 126 the obs is:450 -[11:33:12.838] Odometry x=0.052005, y=-0.293405, theta=0.103276 -[11:33:12.867] Image Alignment failed 0 -[11:33:12.867] frame process finished the id is: 126 the obs is:450 -[11:33:12.942] Odometry x=0.054477, y=-0.317671, theta=0.116393 -[11:33:12.968] Image Alignment failed 0 -[11:33:12.968] frame process finished the id is: 126 the obs is:450 -[11:33:13.040] Odometry x=0.057330, y=-0.341881, theta=0.109720 -[11:33:13.067] Image Alignment failed 0 -[11:33:13.067] frame process finished the id is: 126 the obs is:450 -[11:33:13.139] Odometry x=0.060067, y=-0.366139, theta=0.093712 -[11:33:13.166] Image Alignment failed 0 -[11:33:13.166] frame process finished the id is: 126 the obs is:450 -[11:33:13.239] Odometry x=0.062319, y=-0.390410, theta=0.095369 -[11:33:13.267] Image Alignment failed 0 -[11:33:13.267] frame process finished the id is: 126 the obs is:450 -[11:33:13.338] Odometry x=0.064628, y=-0.414689, theta=0.093620 -[11:33:13.377] Image Alignment failed 0 -[11:33:13.377] frame process finished the id is: 126 the obs is:450 -[11:33:13.438] Odometry x=0.066890, y=-0.438986, theta=0.101079 -[11:33:13.467] Image Alignment failed 0 -[11:33:13.467] frame process finished the id is: 126 the obs is:450 -[11:33:13.539] Odometry x=0.069403, y=-0.463226, theta=0.098150 -[11:33:13.567] Image Alignment failed 0 -[11:33:13.567] frame process finished the id is: 126 the obs is:450 -[11:33:13.639] Odometry x=0.071776, y=-0.487496, theta=0.104984 -[11:33:13.667] Image Alignment failed 0 -[11:33:13.667] frame process finished the id is: 126 the obs is:450 -[11:33:13.739] Odometry x=0.074357, y=-0.511720, theta=0.099072 -[11:33:13.767] Image Alignment failed 0 -[11:33:13.767] frame process finished the id is: 126 the obs is:450 -[11:33:13.843] Odometry x=0.076820, y=-0.535962, theta=0.091674 -[11:33:13.866] Image Alignment failed 0 -[11:33:13.866] frame process finished the id is: 126 the obs is:450 -[11:33:13.943] Odometry x=0.079085, y=-0.560257, theta=0.081421 -[11:33:13.967] Image Alignment failed 0 -[11:33:13.967] frame process finished the id is: 126 the obs is:450 -[11:33:14.039] Odometry x=0.081065, y=-0.584596, theta=0.081652 -[11:33:14.066] Image Alignment failed 0 -[11:33:14.066] frame process finished the id is: 126 the obs is:450 -[11:33:14.138] Odometry x=0.083054, y=-0.608875, theta=0.084785 -[11:33:14.167] Image Alignment failed 0 -[11:33:14.167] frame process finished the id is: 126 the obs is:450 -[11:33:14.240] Odometry x=0.085133, y=-0.633194, theta=0.084019 -[11:33:14.267] Image Alignment failed 0 -[11:33:14.267] frame process finished the id is: 126 the obs is:450 -[11:33:14.345] Odometry x=0.085133, y=-0.633194, theta=0.084019 -[11:33:14.367] Image Alignment failed 0 -[11:33:14.367] frame process finished the id is: 126 the obs is:450 -[11:33:14.441] Odometry x=0.087161, y=-0.657468, theta=0.090464 -[11:33:14.467] Image Alignment failed 0 -[11:33:14.467] frame process finished the id is: 126 the obs is:450 -[11:33:14.545] Odometry x=0.087161, y=-0.657468, theta=0.090464 -[11:33:14.566] Image Alignment failed 0 -[11:33:14.567] frame process finished the id is: 126 the obs is:450 -[11:33:14.638] Odometry x=0.089151, y=-0.679126, theta=0.092712 -[11:33:14.666] Image Alignment failed 0 -[11:33:14.666] frame process finished the id is: 126 the obs is:450 -[11:33:14.744] Odometry x=0.091295, y=-0.700758, theta=0.086325 -[11:33:14.768] Image Alignment failed 0 -[11:33:14.768] frame process finished the id is: 126 the obs is:450 -[11:33:14.844] Odometry x=0.093383, y=-0.724101, theta=0.078234 -[11:33:14.867] Image Alignment failed 0 -[11:33:14.867] frame process finished the id is: 126 the obs is:450 -[11:33:14.942] Odometry x=0.095266, y=-0.748046, theta=0.078124 -[11:33:14.970] Image Alignment failed 0 -[11:33:14.970] frame process finished the id is: 126 the obs is:450 -[11:33:15.060] Odometry x=0.097147, y=-0.772218, theta=0.079891 -[11:33:15.068] Image Alignment failed 0 -[11:33:15.068] frame process finished the id is: 126 the obs is:450 -[11:33:15.142] Odometry x=0.099101, y=-0.796494, theta=0.082792 -[11:33:15.167] Image Alignment failed 0 -[11:33:15.167] frame process finished the id is: 126 the obs is:450 -[11:33:15.244] Odometry x=0.101101, y=-0.820784, theta=0.088321 -[11:33:15.267] Image Alignment failed 0 -[11:33:15.267] frame process finished the id is: 126 the obs is:450 -[11:33:15.338] Odometry x=0.103247, y=-0.845081, theta=0.093091 -[11:33:15.368] Image Alignment failed 0 -[11:33:15.368] frame process finished the id is: 126 the obs is:450 -[11:33:15.437] Odometry x=0.105527, y=-0.869335, theta=0.093115 -[11:33:15.468] Image Alignment failed 0 -[11:33:15.468] frame process finished the id is: 126 the obs is:450 -[11:33:15.537] Odometry x=0.107807, y=-0.893578, theta=0.087321 -[11:33:15.567] Image Alignment failed 0 -[11:33:15.567] frame process finished the id is: 126 the obs is:450 -[11:33:15.638] Odometry x=0.109986, y=-0.917840, theta=0.070739 -[11:33:15.742] Odometry x=0.109986, y=-0.917840, theta=0.070739 -[11:33:15.757] Image Alignment failed 0 -[11:33:15.757] frame process finished the id is: 126 the obs is:450 -[11:33:15.771] Image Alignment failed 0 -[11:33:15.771] frame process finished the id is: 126 the obs is:450 -[11:33:15.840] Odometry x=0.111709, y=-0.942136, theta=0.061175 -[11:33:15.866] Image Alignment failed 0 -[11:33:15.866] frame process finished the id is: 126 the obs is:450 -[11:33:15.944] Odometry x=0.112999, y=-0.963955, theta=0.064452 -[11:33:15.968] Image Alignment failed 0 -[11:33:15.968] frame process finished the id is: 126 the obs is:450 -[11:33:16.044] Odometry x=0.114512, y=-0.987361, theta=0.064522 -[11:33:16.068] Image Alignment failed 0 -[11:33:16.068] frame process finished the id is: 126 the obs is:450 -[11:33:16.144] Odometry x=0.116063, y=-1.011344, theta=0.064794 -[11:33:16.168] Image Alignment failed 0 -[11:33:16.168] frame process finished the id is: 126 the obs is:450 -[11:33:16.244] Odometry x=0.117657, y=-1.035535, theta=0.061210 -[11:33:16.267] Image Alignment failed 0 -[11:33:16.268] frame process finished the id is: 126 the obs is:450 -[11:33:16.339] Odometry x=0.119144, y=-1.059839, theta=0.061159 -[11:33:16.368] Image Alignment failed 0 -[11:33:16.368] frame process finished the id is: 126 the obs is:450 -[11:33:16.444] Odometry x=0.120609, y=-1.084159, theta=0.067031 -[11:33:16.467] Image Alignment failed 0 -[11:33:16.467] frame process finished the id is: 126 the obs is:450 -[11:33:16.544] Odometry x=0.122222, y=-1.108488, theta=0.071259 -[11:33:16.568] Image Alignment failed 0 -[11:33:16.568] frame process finished the id is: 126 the obs is:450 -[11:33:16.644] Odometry x=0.123938, y=-1.132801, theta=0.074929 -[11:33:16.668] Image Alignment failed 0 -[11:33:16.668] frame process finished the id is: 126 the obs is:450 -[11:33:16.744] Odometry x=0.125752, y=-1.157099, theta=0.077801 -[11:33:16.768] Image Alignment failed 0 -[11:33:16.768] frame process finished the id is: 126 the obs is:450 -[11:33:16.839] Odometry x=0.127650, y=-1.181408, theta=0.078220 -[11:33:16.868] Image Alignment failed 0 -[11:33:16.868] frame process finished the id is: 126 the obs is:450 -[11:33:16.944] Odometry x=0.129549, y=-1.205729, theta=0.082414 -[11:33:16.967] Image Alignment failed 0 -[11:33:16.967] frame process finished the id is: 126 the obs is:450 -[11:33:17.040] Odometry x=0.131554, y=-1.230013, theta=0.081088 -[11:33:17.068] Image Alignment failed 0 -[11:33:17.068] frame process finished the id is: 126 the obs is:450 -[11:33:17.145] Odometry x=0.133509, y=-1.254314, theta=0.083882 -[11:33:17.170] Image Alignment failed 0 -[11:33:17.170] frame process finished the id is: 126 the obs is:450 -[11:33:17.245] Odometry x=0.135542, y=-1.278599, theta=0.082022 -[11:33:17.261] Image Alignment failed 0 -[11:33:17.261] frame process finished the id is: 126 the obs is:450 -[11:33:17.338] Odometry x=0.137542, y=-1.302895, theta=0.081907 -[11:33:17.368] Image Alignment failed 0 -[11:33:17.368] frame process finished the id is: 126 the obs is:450 -[11:33:17.436] Odometry x=0.139535, y=-1.327179, theta=0.083116 -[11:33:17.466] Image Alignment failed 0 -[11:33:17.466] frame process finished the id is: 126 the obs is:450 -[11:33:17.543] Odometry x=0.141594, y=-1.351460, theta=0.076198 -[11:33:17.567] Image Alignment failed 0 -[11:33:17.567] frame process finished the id is: 126 the obs is:450 -[11:33:17.637] Odometry x=0.143448, y=-1.375782, theta=0.081423 -[11:33:17.666] Image Alignment failed 0 -[11:33:17.666] frame process finished the id is: 126 the obs is:450 -[11:33:17.743] Odometry x=0.145423, y=-1.400085, theta=0.085216 -[11:33:17.769] Image Alignment failed 0 -[11:33:17.769] frame process finished the id is: 126 the obs is:450 -[11:33:17.844] Odometry x=0.147509, y=-1.424361, theta=0.084080 -[11:33:17.868] Image Alignment failed 0 -[11:33:17.868] frame process finished the id is: 126 the obs is:450 -[11:33:17.945] Odometry x=0.149561, y=-1.448625, theta=0.088897 -[11:33:17.969] Image Alignment failed 0 -[11:33:17.969] frame process finished the id is: 126 the obs is:450 -[11:33:18.041] Odometry x=0.151729, y=-1.472890, theta=0.084750 -[11:33:18.069] Image Alignment failed 0 -[11:33:18.070] frame process finished the id is: 126 the obs is:450 -[11:33:18.141] Odometry x=0.153789, y=-1.497197, theta=0.088616 -[11:33:18.168] Image Alignment failed 0 -[11:33:18.168] frame process finished the id is: 126 the obs is:450 -[11:33:18.236] Odometry x=0.155936, y=-1.521486, theta=0.096097 -[11:33:18.269] Image Alignment failed 0 -[11:33:18.269] frame process finished the id is: 126 the obs is:450 -[11:33:18.341] Odometry x=0.158245, y=-1.545777, theta=0.101256 -[11:33:18.371] Image Alignment failed 0 -[11:33:18.371] frame process finished the id is: 126 the obs is:450 -[11:33:18.442] Odometry x=0.160698, y=-1.570031, theta=0.102564 -[11:33:18.468] Image Alignment failed 0 -[11:33:18.468] frame process finished the id is: 126 the obs is:450 -[11:33:18.542] Odometry x=0.163229, y=-1.594273, theta=0.098366 -[11:33:18.569] Image Alignment failed 0 -[11:33:18.569] frame process finished the id is: 126 the obs is:450 -[11:33:18.642] Odometry x=0.165648, y=-1.618509, theta=0.094673 -[11:33:18.668] Image Alignment failed 0 -[11:33:18.668] frame process finished the id is: 126 the obs is:450 -[11:33:18.737] Odometry x=0.167944, y=-1.642781, theta=0.102550 -[11:33:18.769] Image Alignment failed 0 -[11:33:18.769] frame process finished the id is: 126 the obs is:450 -[11:33:18.838] Odometry x=0.170436, y=-1.667046, theta=0.104288 -[11:33:18.870] Image Alignment failed 0 -[11:33:18.870] frame process finished the id is: 126 the obs is:450 -[11:33:18.942] Odometry x=0.172944, y=-1.691308, theta=0.111281 -[11:33:18.969] Image Alignment failed 0 -[11:33:18.969] frame process finished the id is: 126 the obs is:450 -[11:33:19.044] Odometry x=0.175661, y=-1.715542, theta=0.110532 -[11:33:19.069] Image Alignment failed 0 -[11:33:19.069] frame process finished the id is: 126 the obs is:450 -[11:33:19.138] Odometry x=0.178347, y=-1.739772, theta=0.108916 -[11:33:19.170] Image Alignment failed 0 -[11:33:19.170] frame process finished the id is: 126 the obs is:450 -[11:33:19.242] Odometry x=0.181002, y=-1.764004, theta=0.108735 -[11:33:19.268] Image Alignment failed 0 -[11:33:19.268] frame process finished the id is: 126 the obs is:450 -[11:33:19.341] Odometry x=0.183622, y=-1.788242, theta=0.115114 -[11:33:19.370] Image Alignment failed 0 -[11:33:19.370] frame process finished the id is: 126 the obs is:450 -[11:33:19.440] Odometry x=0.186393, y=-1.812468, theta=0.120505 -[11:33:19.474] Image Alignment failed 0 -[11:33:19.474] frame process finished the id is: 126 the obs is:450 -[11:33:19.543] Odometry x=0.189296, y=-1.836680, theta=0.126268 -[11:33:19.561] Image Alignment failed 0 -[11:33:19.561] frame process finished the id is: 126 the obs is:450 -[11:33:19.645] Odometry x=0.192371, y=-1.860871, theta=0.125368 -[11:33:19.668] Image Alignment failed 0 -[11:33:19.668] frame process finished the id is: 126 the obs is:450 -[11:33:19.741] Odometry x=0.195426, y=-1.885048, theta=0.129438 -[11:33:19.769] Image Alignment failed 0 -[11:33:19.769] frame process finished the id is: 126 the obs is:450 -[11:33:19.837] Odometry x=0.198557, y=-1.909219, theta=0.136674 -[11:33:19.867] Image Alignment failed 0 -[11:33:19.867] frame process finished the id is: 126 the obs is:450 -[11:33:19.943] Odometry x=0.201901, y=-1.933370, theta=0.138700 -[11:33:19.969] Image Alignment failed 0 -[11:33:19.969] frame process finished the id is: 126 the obs is:450 -[11:33:20.040] Odometry x=0.205283, y=-1.957512, theta=0.131270 -[11:33:20.067] Image Alignment failed 0 -[11:33:20.067] frame process finished the id is: 126 the obs is:450 -[11:33:20.139] Odometry x=0.208466, y=-1.981689, theta=0.127667 -[11:33:20.177] Image Alignment failed 0 -[11:33:20.177] frame process finished the id is: 126 the obs is:450 -[11:33:20.244] Odometry x=0.211574, y=-2.005871, theta=0.129658 -[11:33:20.267] Image Alignment failed 0 -[11:33:20.267] frame process finished the id is: 126 the obs is:450 -[11:33:20.337] Odometry x=0.214741, y=-2.030030, theta=0.127195 -[11:33:20.367] Image Alignment failed 0 -[11:33:20.367] frame process finished the id is: 126 the obs is:450 -[11:33:20.437] Odometry x=0.217849, y=-2.054207, theta=0.122806 -[11:33:20.466] Image Alignment failed 0 -[11:33:20.466] frame process finished the id is: 126 the obs is:450 -[11:33:20.543] Odometry x=0.220802, y=-2.078381, theta=0.125315 -[11:33:20.567] Image Alignment failed 0 -[11:33:20.567] frame process finished the id is: 126 the obs is:450 -[11:33:20.636] Odometry x=0.223822, y=-2.102578, theta=0.129952 -[11:33:20.667] Image Alignment failed 0 -[11:33:20.667] frame process finished the id is: 126 the obs is:450 -[11:33:20.739] Odometry x=0.226967, y=-2.126748, theta=0.132716 -[11:33:20.769] Image Alignment failed 0 -[11:33:20.769] frame process finished the id is: 126 the obs is:450 -[11:33:20.840] Odometry x=0.230219, y=-2.150904, theta=0.127980 -[11:33:20.869] Image Alignment failed 0 -[11:33:20.869] frame process finished the id is: 126 the obs is:450 -[11:33:20.941] Odometry x=0.233354, y=-2.175071, theta=0.122768 -[11:33:20.968] Image Alignment failed 0 -[11:33:20.968] frame process finished the id is: 126 the obs is:450 -[11:33:21.041] Odometry x=0.236330, y=-2.199249, theta=0.125144 -[11:33:21.068] Image Alignment failed 0 -[11:33:21.068] frame process finished the id is: 126 the obs is:450 -[11:33:21.141] Odometry x=0.239351, y=-2.223430, theta=0.127809 -[11:33:21.168] Image Alignment failed 0 -[11:33:21.168] frame process finished the id is: 126 the obs is:450 -[11:33:21.235] Odometry x=0.242451, y=-2.247610, theta=0.130982 -[11:33:21.267] Image Alignment failed 0 -[11:33:21.267] frame process finished the id is: 126 the obs is:450 -[11:33:21.345] Odometry x=0.245648, y=-2.271788, theta=0.127086 -[11:33:21.370] Image Alignment failed 0 -[11:33:21.370] frame process finished the id is: 126 the obs is:450 -[11:33:21.441] Odometry x=0.248729, y=-2.295978, theta=0.124391 -[11:33:21.467] Image Alignment failed 0 -[11:33:21.467] frame process finished the id is: 126 the obs is:450 -[11:33:21.541] Odometry x=0.251748, y=-2.320173, theta=0.126367 -[11:33:21.569] Image Alignment failed 0 -[11:33:21.569] frame process finished the id is: 126 the obs is:450 -[11:33:21.641] Odometry x=0.254811, y=-2.344352, theta=0.132725 -[11:33:21.669] Image Alignment failed 0 -[11:33:21.669] frame process finished the id is: 126 the obs is:450 -[11:33:21.742] Odometry x=0.258013, y=-2.368529, theta=0.140044 -[11:33:21.769] Image Alignment failed 0 -[11:33:21.769] frame process finished the id is: 126 the obs is:450 -[11:33:21.836] Odometry x=0.261399, y=-2.392684, theta=0.143283 -[11:33:21.868] Image Alignment failed 0 -[11:33:21.868] frame process finished the id is: 126 the obs is:450 -[11:33:21.941] Odometry x=0.264854, y=-2.416809, theta=0.149555 -[11:33:21.959] Image Alignment failed 0 -[11:33:21.960] frame process finished the id is: 126 the obs is:450 -[11:33:22.041] Odometry x=0.268506, y=-2.440923, theta=0.150044 -[11:33:22.067] Image Alignment failed 0 -[11:33:22.067] frame process finished the id is: 126 the obs is:450 -[11:33:22.142] Odometry x=0.272152, y=-2.465026, theta=0.152980 -[11:33:22.171] Image Alignment failed 0 -[11:33:22.171] frame process finished the id is: 126 the obs is:450 -[11:33:22.239] Odometry x=0.275844, y=-2.489126, theta=0.159846 -[11:33:22.268] Image Alignment failed 0 -[11:33:22.268] frame process finished the id is: 126 the obs is:450 -[11:33:22.344] Odometry x=0.279689, y=-2.513208, theta=0.165144 -[11:33:22.368] Image Alignment failed 0 -[11:33:22.368] frame process finished the id is: 126 the obs is:450 -[11:33:22.444] Odometry x=0.283688, y=-2.537265, theta=0.164441 -[11:33:22.467] Image Alignment failed 0 -[11:33:22.467] frame process finished the id is: 126 the obs is:450 -[11:33:22.540] Odometry x=0.287687, y=-2.561332, theta=0.166945 -[11:33:22.568] Image Alignment failed 0 -[11:33:22.568] frame process finished the id is: 126 the obs is:450 -[11:33:22.639] Odometry x=0.291731, y=-2.585365, theta=0.170375 -[11:33:22.667] Image Alignment failed 0 -[11:33:22.667] frame process finished the id is: 126 the obs is:450 -[11:33:22.742] Odometry x=0.295887, y=-2.609397, theta=0.171587 -[11:33:22.767] Image Alignment failed 0 -[11:33:22.767] frame process finished the id is: 126 the obs is:450 -[11:33:22.838] Odometry x=0.300065, y=-2.633423, theta=0.170359 -[11:33:22.867] Image Alignment failed 0 -[11:33:22.867] frame process finished the id is: 126 the obs is:450 -[11:33:22.937] Odometry x=0.304172, y=-2.657466, theta=0.175537 -[11:33:22.967] Image Alignment failed 0 -[11:33:22.967] frame process finished the id is: 126 the obs is:450 -[11:33:23.046] Odometry x=0.308453, y=-2.681465, theta=0.170240 -[11:33:23.069] Image Alignment failed 0 -[11:33:23.069] frame process finished the id is: 126 the obs is:450 -[11:33:23.140] Odometry x=0.312596, y=-2.705489, theta=0.164733 -[11:33:23.169] Image Alignment failed 0 -[11:33:23.169] frame process finished the id is: 126 the obs is:450 -[11:33:23.241] Odometry x=0.316617, y=-2.729527, theta=0.159814 -[11:33:23.272] Image Alignment failed 0 -[11:33:23.272] frame process finished the id is: 126 the obs is:450 -[11:33:23.341] Odometry x=0.320500, y=-2.753612, theta=0.160948 -[11:33:23.367] Image Alignment failed 0 -[11:33:23.367] frame process finished the id is: 126 the obs is:450 -[11:33:23.477] Image Alignment failed 0 -[11:33:23.477] frame process finished the id is: 126 the obs is:450 -[11:33:23.567] Image Alignment failed 0 -[11:33:23.567] frame process finished the id is: 126 the obs is:450 -[11:33:23.669] Image Alignment failed 0 -[11:33:23.669] frame process finished the id is: 126 the obs is:450 -[11:33:23.769] Image Alignment failed 0 -[11:33:23.769] frame process finished the id is: 126 the obs is:450 -[11:33:23.869] Image Alignment failed 0 -[11:33:23.869] frame process finished the id is: 126 the obs is:450 -[11:33:23.969] Image Alignment failed 0 -[11:33:23.969] frame process finished the id is: 126 the obs is:450 -[11:33:24.069] Image Alignment failed 0 -[11:33:24.069] frame process finished the id is: 126 the obs is:450 -[11:33:24.168] Image Alignment failed 0 -[11:33:24.168] frame process finished the id is: 126 the obs is:450 -[11:33:24.267] Image Alignment failed 0 -[11:33:24.267] frame process finished the id is: 126 the obs is:450 -[11:33:24.362] Image Alignment failed 0 -[11:33:24.362] frame process finished the id is: 126 the obs is:450 -[11:33:24.468] Image Alignment failed 0 -[11:33:24.468] frame process finished the id is: 126 the obs is:450 -[11:33:24.568] Image Alignment failed 0 -[11:33:24.568] frame process finished the id is: 126 the obs is:450 -[11:33:24.661] Image Alignment failed 0 -[11:33:24.661] frame process finished the id is: 126 the obs is:450 -[11:33:24.767] Image Alignment failed 0 -[11:33:24.767] frame process finished the id is: 126 the obs is:450 -[11:33:24.868] Image Alignment failed 0 -[11:33:24.868] frame process finished the id is: 126 the obs is:450 -[11:33:24.972] Image Alignment failed 0 -[11:33:24.972] frame process finished the id is: 126 the obs is:450 -[11:33:25.069] Image Alignment failed 0 -[11:33:25.069] frame process finished the id is: 126 the obs is:450 -[11:33:25.167] Image Alignment failed 0 -[11:33:25.167] frame process finished the id is: 126 the obs is:450 -[11:33:25.267] Image Alignment failed 0 -[11:33:25.267] frame process finished the id is: 126 the obs is:450 -[11:33:25.370] Image Alignment failed 0 -[11:33:25.370] frame process finished the id is: 126 the obs is:450 -[11:33:25.471] Image Alignment failed 0 -[11:33:25.471] frame process finished the id is: 126 the obs is:450 -[11:33:25.568] Image Alignment failed 0 -[11:33:25.568] frame process finished the id is: 126 the obs is:450 -[11:33:25.668] Image Alignment failed 0 -[11:33:25.668] frame process finished the id is: 126 the obs is:450 -[11:33:25.767] Image Alignment failed 0 -[11:33:25.767] frame process finished the id is: 126 the obs is:450 -[11:33:25.867] Image Alignment failed 0 -[11:33:25.867] frame process finished the id is: 126 the obs is:450 -[11:33:25.967] Image Alignment failed 0 -[11:33:25.967] frame process finished the id is: 126 the obs is:450 -[11:33:26.067] Image Alignment failed 0 -[11:33:26.067] frame process finished the id is: 126 the obs is:450 -[11:33:26.167] Image Alignment failed 0 -[11:33:26.167] frame process finished the id is: 126 the obs is:450 -[11:33:26.268] Image Alignment failed 0 -[11:33:26.268] frame process finished the id is: 126 the obs is:450 -[11:33:26.368] Image Alignment failed 0 -[11:33:26.368] frame process finished the id is: 126 the obs is:450 -[11:33:26.468] Image Alignment failed 0 -[11:33:26.468] frame process finished the id is: 126 the obs is:450 -[11:33:26.568] Image Alignment failed 0 -[11:33:26.568] frame process finished the id is: 126 the obs is:450 -[11:33:26.668] Image Alignment failed 0 -[11:33:26.668] frame process finished the id is: 126 the obs is:450 -[11:33:26.768] Image Alignment failed 0 -[11:33:26.768] frame process finished the id is: 126 the obs is:450 +[21:12:22.596] frame process finished the id is: 0 the obs is:0 +[21:12:22.699] frame process finished the id is: 1 the obs is:0 +[21:12:22.788] Init: frame zero: 0.000000, 0.000000 0.000000 +[21:12:22.788] Init: Selected first frame. +[21:12:22.788] frame process finished the id is: 2 the obs is:0 +[21:12:22.821] Init: px average disparity is:0.152753 ,While minimum is: 10.000000 KLT tracked : 109 +[21:12:22.821] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[21:12:22.821] frame process finished the id is: 3 the obs is:0 +[21:12:22.922] Init: px average disparity is:0.221920 ,While minimum is: 10.000000 KLT tracked : 109 +[21:12:22.922] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[21:12:22.922] frame process finished the id is: 4 the obs is:0 +[21:12:23.018] Init: px average disparity is:0.152235 ,While minimum is: 10.000000 KLT tracked : 109 +[21:12:23.018] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[21:12:23.018] frame process finished the id is: 5 the obs is:0 +[21:12:23.119] Init: px average disparity is:0.197958 ,While minimum is: 10.000000 KLT tracked : 109 +[21:12:23.119] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[21:12:23.119] frame process finished the id is: 6 the obs is:0 +[21:12:23.219] Init: px average disparity is:0.221111 ,While minimum is: 10.000000 KLT tracked : 109 +[21:12:23.219] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[21:12:23.219] frame process finished the id is: 7 the obs is:0 +[21:12:23.323] Init: px average disparity is:0.224726 ,While minimum is: 10.000000 KLT tracked : 109 +[21:12:23.323] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[21:12:23.323] frame process finished the id is: 8 the obs is:0 +[21:12:23.426] Init: px average disparity is:0.158653 ,While minimum is: 10.000000 KLT tracked : 109 +[21:12:23.426] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[21:12:23.426] frame process finished the id is: 9 the obs is:0 +[21:12:23.521] Odometry x=0.000000, y=0.000000, theta=-0.000000 +[21:12:23.532] Init: px average disparity is:0.179287 ,While minimum is: 10.000000 KLT tracked : 109 +[21:12:23.532] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[21:12:23.532] frame process finished the id is: 10 the obs is:0 +[21:12:23.610] Odometry x=0.000001, y=0.000003, theta=-0.032076 +[21:12:23.626] Init: px average disparity is:0.148421 ,While minimum is: 10.000000 KLT tracked : 109 +[21:12:23.626] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[21:12:23.626] frame process finished the id is: 11 the obs is:0 +[21:12:23.710] Odometry x=0.000002, y=0.000008, theta=-0.056009 +[21:12:23.725] Init: px average disparity is:0.137330 ,While minimum is: 10.000000 KLT tracked : 109 +[21:12:23.725] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[21:12:23.725] frame process finished the id is: 12 the obs is:0 +[21:12:23.810] Odometry x=0.000006, y=0.000006, theta=-0.079174 +[21:12:23.826] Init: px average disparity is:4.945143 ,While minimum is: 10.000000 KLT tracked : 109 +[21:12:23.826] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[21:12:23.826] frame process finished the id is: 13 the obs is:0 +[21:12:23.911] Odometry x=-0.000002, y=0.000034, theta=-0.027668 +[21:12:23.999] Init: Homography RANSAC (inlier) is:0 ,While 50 inliers minimum required. px average disparity is:11.912397 ,While minimum is: 10.000000 KLT tracked: 109 +[21:12:23.999] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[21:12:23.999] frame process finished the id is: 14 the obs is:0 +[21:12:24.011] Odometry x=0.000003, y=0.000041, theta=-0.049698 +[21:12:24.083] Init: Homography RANSAC (inlier) is:0 ,While 50 inliers minimum required. px average disparity is:19.121424 ,While minimum is: 10.000000 KLT tracked: 109 +[21:12:24.083] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[21:12:24.083] frame process finished the id is: 15 the obs is:0 +[21:12:24.109] Odometry x=0.000005, y=0.000050, theta=-0.083814 +[21:12:24.183] Init: Homography RANSAC (inlier) is:1 ,While 50 inliers minimum required. px average disparity is:29.212896 ,While minimum is: 10.000000 KLT tracked: 109 +[21:12:24.183] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[21:12:24.183] frame process finished the id is: 16 the obs is:0 +[21:12:24.207] Odometry x=0.000004, y=0.000059, theta=-0.117281 +[21:12:24.291] Init: Homography RANSAC (inlier) is:9 ,While 50 inliers minimum required. px average disparity is:40.866547 ,While minimum is: 10.000000 KLT tracked: 109 +[21:12:24.291] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[21:12:24.291] frame process finished the id is: 17 the obs is:0 +[21:12:24.308] Odometry x=0.000003, y=0.000065, theta=-0.147650 +[21:12:24.385] Init: Homography RANSAC (inlier) is:15 ,While 50 inliers minimum required. px average disparity is:51.065260 ,While minimum is: 10.000000 KLT tracked: 109 +[21:12:24.385] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[21:12:24.385] frame process finished the id is: 18 the obs is:0 +[21:12:24.410] Odometry x=0.000008, y=0.000084, theta=-0.175657 +[21:12:24.484] Init: Homography RANSAC (inlier) is:40 ,While 50 inliers minimum required. px average disparity is:58.238817 ,While minimum is: 10.000000 KLT tracked: 108 +[21:12:24.484] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[21:12:24.484] frame process finished the id is: 19 the obs is:0 +[21:12:24.510] Odometry x=-0.000002, y=0.000077, theta=-0.199934 +[21:12:24.584] Init finished: Homography RANSAC (inlier) is:51 ,While 50 inliers minimum required. px average disparity is:68.778966 ,While minimum is: 10.000000 KLT tracked: 106 +[21:12:24.584] Init: distance between the first and current frame is x:-0.000001 ,z=0.000078,angle between two frames: -0.232329 +[21:12:24.584] Init: Selected Second frame. The number of features: 51 depth mean:-0.000433 min:-0.001097 +[21:12:24.585] frame process finished the id is: 20 the obs is:51 +[21:12:24.610] Odometry x=-0.000001, y=0.000078, theta=-0.232329 +[21:12:24.694] After Reprojection Map nMatches:50 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000078,angle between two frames:2.326349 +[21:12:24.694] n deleted obs = 48 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =2 error init =502109862778754569089759184587797299200.000000 error end=502109862778754569089759184587797299200.000000 +[21:12:24.694] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000078,angle between two frames:2.326349 +[21:12:24.695] frame process finished the id is: 20 the obs is:51 +[21:12:24.707] Odometry x=0.000006, y=0.000085, theta=-0.263960 +[21:12:24.801] After Reprojection Map nMatches:90 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000085,angle between two frames:2.357979 +[21:12:24.802] n deleted obs = 44 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =46 error init =25775521026954656677316802904064.000000 error end=25775521026954656677316802904064.000000 +[21:12:24.802] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000085,angle between two frames:2.357979 +[21:12:24.802] frame process finished the id is: 20 the obs is:51 +[21:12:24.807] Odometry x=0.000000, y=0.000095, theta=-0.292996 +[21:12:24.905] After Reprojection Map nMatches:90 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000107,angle between two frames:2.416558 +[21:12:24.905] n deleted obs = 32 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =58 error init =1539596119430629465874866634752.000000 error end=1539596119430629465874866634752.000000 +[21:12:24.905] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000107,angle between two frames:2.416558 +[21:12:24.905] frame process finished the id is: 20 the obs is:51 +[21:12:24.911] Odometry x=0.000001, y=0.000107, theta=-0.322539 +[21:12:25.000] After Reprojection Map nMatches:51 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000107,angle between two frames:2.416558 +[21:12:25.000] n deleted obs = 25 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =26 error init =1.434829 error end=1.434829 +[21:12:25.000] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000107,angle between two frames:2.416558 +[21:12:25.000] frame process finished the id is: 20 the obs is:51 +[21:12:25.004] Odometry x=0.000008, y=0.000123, theta=-0.352174 +[21:12:25.101] After Reprojection Map nMatches:50 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000123,angle between two frames:2.446193 +[21:12:25.101] n deleted obs = 25 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =25 error init =0.156401 error end=0.156401 +[21:12:25.101] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000123,angle between two frames:2.446193 +[21:12:25.101] frame process finished the id is: 20 the obs is:51 +[21:12:25.107] Odometry x=0.000013, y=0.000125, theta=-0.381009 +[21:12:25.202] After Reprojection Map nMatches:45 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000125,angle between two frames:2.475028 +[21:12:25.202] n deleted obs = 20 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =25 error init =0.492040 error end=0.492040 +[21:12:25.202] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000125,angle between two frames:2.475028 +[21:12:25.202] frame process finished the id is: 20 the obs is:51 +[21:12:25.207] Odometry x=0.000028, y=0.000134, theta=-0.408363 +[21:12:25.302] After Reprojection Map nMatches:49 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000134,angle between two frames:2.502382 +[21:12:25.302] n deleted obs = 27 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =22 error init =1.905550 error end=1.905550 +[21:12:25.302] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000134,angle between two frames:2.502382 +[21:12:25.302] frame process finished the id is: 20 the obs is:51 +[21:12:25.307] Odometry x=0.000027, y=0.000131, theta=-0.433405 +[21:12:25.401] After Reprojection Map nMatches:47 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000131,angle between two frames:2.527425 +[21:12:25.401] n deleted obs = 21 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =26 error init =1.189615 error end=1.189615 +[21:12:25.401] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000131,angle between two frames:2.527425 +[21:12:25.401] frame process finished the id is: 20 the obs is:51 +[21:12:25.407] Odometry x=0.000037, y=0.000140, theta=-0.461984 +[21:12:25.501] After Reprojection Map nMatches:49 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000152,angle between two frames:2.583437 +[21:12:25.501] n deleted obs = 28 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =21 error init =0.909562 error end=0.909562 +[21:12:25.501] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000152,angle between two frames:2.583437 +[21:12:25.501] frame process finished the id is: 20 the obs is:51 +[21:12:25.507] Odometry x=0.000042, y=0.000152, theta=-0.489417 +[21:12:25.601] After Reprojection Map nMatches:51 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000152,angle between two frames:2.583437 +[21:12:25.601] n deleted obs = 14 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =37 error init =0.882902 error end=0.882902 +[21:12:25.601] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000152,angle between two frames:2.583437 +[21:12:25.601] frame process finished the id is: 20 the obs is:51 +[21:12:25.609] Odometry x=0.000051, y=0.000156, theta=-0.514409 +[21:12:25.702] After Reprojection Map nMatches:51 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000156,angle between two frames:2.608428 +[21:12:25.702] n deleted obs = 27 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =24 error init =28.762689 error end=28.762689 +[21:12:25.702] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000156,angle between two frames:2.608428 +[21:12:25.702] frame process finished the id is: 20 the obs is:51 +[21:12:25.707] Odometry x=0.000060, y=0.000144, theta=-0.541731 +[21:12:25.802] After Reprojection Map nMatches:51 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000144,angle between two frames:2.635750 +[21:12:25.802] n deleted obs = 18 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =33 error init =0.667553 error end=0.667553 +[21:12:25.802] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000144,angle between two frames:2.635750 +[21:12:25.802] frame process finished the id is: 20 the obs is:51 +[21:12:25.809] Odometry x=0.000068, y=0.000147, theta=-0.570891 +[21:12:25.902] After Reprojection Map nMatches:50 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000147,angle between two frames:2.664911 +[21:12:25.902] n deleted obs = 20 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =30 error init =0.617649 error end=0.617649 +[21:12:25.902] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000147,angle between two frames:2.664911 +[21:12:25.902] frame process finished the id is: 20 the obs is:51 +[21:12:25.908] Odometry x=0.000081, y=0.000146, theta=-0.598870 +[21:12:26.002] After Reprojection Map nMatches:49 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000146,angle between two frames:2.692890 +[21:12:26.002] n deleted obs = 14 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =35 error init =1.463091 error end=1.463091 +[21:12:26.002] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000146,angle between two frames:2.692890 +[21:12:26.002] frame process finished the id is: 20 the obs is:51 +[21:12:26.007] Odometry x=0.000094, y=0.000150, theta=-0.627529 +[21:12:26.102] After Reprojection Map nMatches:49 ,distance between ekf and vo x:0.000000 ,z=0.000000,angle between two frames:0.000000 +[21:12:26.102] n deleted obs = 16 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =33 error init =0.455852 error end=0.141142 +[21:12:26.103] After pose optimization, distance between ekf and vo x:0.000144 ,z=-0.000111,angle between two frames:0.209075 +[21:12:26.103] frame process finished the id is: 20 the obs is:51 +[21:12:26.108] Odometry x=0.000103, y=0.000159, theta=-0.654079 +[21:12:26.202] After Reprojection Map nMatches:46 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000159,angle between two frames:2.748098 +[21:12:26.202] n deleted obs = 14 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =32 error init =2.060323 error end=2.060323 +[21:12:26.202] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000159,angle between two frames:2.748098 +[21:12:26.202] frame process finished the id is: 20 the obs is:51 +[21:12:26.208] Odometry x=0.000115, y=0.000160, theta=-0.679799 +[21:12:26.300] After Reprojection Map nMatches:46 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000160,angle between two frames:2.773818 +[21:12:26.300] n deleted obs = 23 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =23 error init =2.509140 error end=2.509140 +[21:12:26.300] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000160,angle between two frames:2.773818 +[21:12:26.300] frame process finished the id is: 20 the obs is:51 +[21:12:26.311] Odometry x=0.000125, y=0.000150, theta=-0.702135 +[21:12:26.401] After Reprojection Map nMatches:48 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000150,angle between two frames:2.796154 +[21:12:26.402] n deleted obs = 12 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =36 error init =128.473177 error end=128.473177 +[21:12:26.402] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000150,angle between two frames:2.796154 +[21:12:26.402] frame process finished the id is: 20 the obs is:51 +[21:12:26.409] Odometry x=0.000135, y=0.000152, theta=-0.724993 +[21:12:26.500] After Reprojection Map nMatches:51 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000152,angle between two frames:2.819012 +[21:12:26.500] n deleted obs = 28 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =23 error init =1.513610 error end=1.513610 +[21:12:26.500] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000152,angle between two frames:2.819012 +[21:12:26.500] frame process finished the id is: 20 the obs is:51 +[21:12:26.508] Odometry x=0.000142, y=0.000152, theta=-0.749440 +[21:12:26.601] After Reprojection Map nMatches:49 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000152,angle between two frames:2.843459 +[21:12:26.602] n deleted obs = 22 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =27 error init =1.509610 error end=1.509610 +[21:12:26.602] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000152,angle between two frames:2.843459 +[21:12:26.602] frame process finished the id is: 20 the obs is:51 +[21:12:26.608] Odometry x=0.000152, y=0.000151, theta=-0.772233 +[21:12:26.700] After Reprojection Map nMatches:51 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000151,angle between two frames:2.866253 +[21:12:26.700] n deleted obs = 14 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =37 error init =0.413570 error end=0.413570 +[21:12:26.700] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000151,angle between two frames:2.866253 +[21:12:26.700] frame process finished the id is: 20 the obs is:51 +[21:12:26.708] Odometry x=0.000161, y=0.000154, theta=-0.797019 +[21:12:26.800] After Reprojection Map nMatches:45 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000154,angle between two frames:2.891039 +[21:12:26.800] n deleted obs = 29 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =16 error init =16.749681 error end=16.749681 +[21:12:26.800] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000154,angle between two frames:2.891039 +[21:12:26.800] frame process finished the id is: 20 the obs is:51 +[21:12:26.808] Odometry x=0.000179, y=0.000149, theta=-0.825330 +[21:12:26.900] After Reprojection Map nMatches:51 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000149,angle between two frames:2.919349 +[21:12:26.900] n deleted obs = 31 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =20 error init =2.888775 error end=2.888775 +[21:12:26.900] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000149,angle between two frames:2.919349 +[21:12:26.900] frame process finished the id is: 20 the obs is:51 +[21:12:26.908] Odometry x=0.000182, y=0.000149, theta=-0.848463 +[21:12:27.001] After Reprojection Map nMatches:51 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000149,angle between two frames:2.942482 +[21:12:27.002] n deleted obs = 24 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =27 error init =0.824872 error end=0.824872 +[21:12:27.002] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000149,angle between two frames:2.942482 +[21:12:27.002] frame process finished the id is: 20 the obs is:51 +[21:12:27.008] Odometry x=0.000186, y=0.000151, theta=-0.873981 +[21:12:27.102] After Reprojection Map nMatches:51 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000151,angle between two frames:2.968001 +[21:12:27.102] n deleted obs = 14 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =37 error init =0.540298 error end=0.540298 +[21:12:27.102] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000151,angle between two frames:2.968001 +[21:12:27.102] frame process finished the id is: 20 the obs is:51 +[21:12:27.103] Odometry x=0.000193, y=0.000153, theta=-0.899816 +[21:12:27.199] After Reprojection Map nMatches:42 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000153,angle between two frames:2.993836 +[21:12:27.199] n deleted obs = 21 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =21 error init =8.084876 error end=8.084876 +[21:12:27.199] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000153,angle between two frames:2.993836 +[21:12:27.199] frame process finished the id is: 20 the obs is:51 +[21:12:27.199] Odometry x=0.000200, y=0.000156, theta=-0.929196 +[21:12:27.301] After Reprojection Map nMatches:51 ,distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000156,angle between two frames:3.023216 +[21:12:27.301] n deleted obs = 16 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =35 error init =0.960905 error end=0.960905 +[21:12:27.301] After pose optimization, distance between ekf and vo x:-3293655424934593847227188149523316736.000000 ,z=-0.000156,angle between two frames:3.023216 +[21:12:27.301] frame process finished the id is: 20 the obs is:51 +[21:12:27.303] Odometry x=0.000213, y=0.000166, theta=-0.956462 +[21:12:27.400] After Reprojection Map nMatches:51 ,distance between ekf and vo x:-0.000020 ,z=-0.000012,angle between two frames:0.050465 +[21:12:27.401] n deleted obs = 17 n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =34 error init =0.204225 error end=0.174169 +[21:12:27.401] After pose optimization, distance between ekf and vo x:-0.000036 ,z=-0.000120,angle between two frames:0.017539 +[21:12:27.4 \ No newline at end of file diff --git a/GPU_version/vio/include/vio/cl_class.h b/GPU_version/vio/include/vio/cl_class.h index 473bea2..076f0ba 100644 --- a/GPU_version/vio/include/vio/cl_class.h +++ b/GPU_version/vio/include/vio/cl_class.h @@ -13,6 +13,7 @@ #include #include #include +#include class kernel{ public: @@ -22,90 +23,71 @@ public: assert(error== CL_SUCCESS); }; ~kernel(){ - for(auto&& i:_images)i.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL); - for(auto&& i:_buffers)i.first.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL); + for(auto&& i:_images)i.first->setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL); + for(auto&& i:_buffers)i.first->setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL); _buffers.clear(); _images.clear(); }; template - int32_t write(size_t id/*buffer ID*/,T* buf,cl::CommandQueue* queue,cl::Context* context,size_t buf_size){ + void load(size_t id/*buffer ID*/,T* buf,cl::CommandQueue* queue,cl::Context* context,size_t buf_size){ assert(buf); cl_int error; - _buffers.push_back(std::pair,size_t>(std::pair(cl::Buffer(*context, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR - , sizeof(T) * buf_size,buf,&error), - buf_size),id)); - cl_int err=_kernel->setArg(id,_buffers.back().first.first); - assert(err == CL_SUCCESS); + _buffers.push_back(std::pair,size_t>(std::make_shared(*context, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR, sizeof(T) * buf_size,buf,&error),id)); assert(error == CL_SUCCESS); - return error; +/* cl::Event event; + T* Map_buf=(T*)queue->enqueueMapBuffer(*_buffers.back().first,CL_NON_BLOCKING,CL_MAP_WRITE,0,sizeof(T) * buf_size,NULL,&event,&error); + event.wait(); + memcpy(Map_buf,buf,sizeof(T) * buf_size); + assert(queue->enqueueUnmapMemObject(*_buffers.back().first,Map_buf,NULL,&event)==CL_SUCCESS); + event.wait();*/ + cl_int err=_kernel->setArg(id,*_buffers.back().first); + assert(err == CL_SUCCESS); }; - int32_t write(size_t id/*buffer ID*/,cv::Mat& buf,cl::Context* context){ + void load(size_t id/*buffer ID*/,cv::Mat& buf,cl::Context* context){ assert(!buf.empty()); cl_int error; - _images.push_back(std::pair(cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_USE_HOST_PTR, + _images.push_back(std::pair,size_t>(std::make_shared(*context, CL_MEM_READ_ONLY | CL_MEM_USE_HOST_PTR, cl::ImageFormat(CL_R, CL_UNSIGNED_INT8), buf.size().width, buf.size().height, 0, reinterpret_cast(buf.data),&error),id)); - cl_int err=_kernel->setArg(id,_images.back().first); + assert(error == CL_SUCCESS); + cl_int err=_kernel->setArg(id,*_images.back().first); assert(err == CL_SUCCESS); - assert(error == CL_SUCCESS); - return error; }; - - template - int32_t reload(size_t id,T* buf,cl::CommandQueue* queue){ - assert(buf); - cl_int error; - for(auto&& i:_buffers)if(i.second==id){ - cl::Event event; - T* Map_buf=(T*)queue->enqueueMapBuffer(i.first.first,CL_NON_BLOCKING,CL_MAP_WRITE,0,sizeof(T) * i.first.second,NULL,&event,&error); - event.wait(); - memcpy(Map_buf,buf,sizeof(T) * i.first.second); - assert(queue->enqueueUnmapMemObject(i.first.first,Map_buf,NULL,&event)==CL_SUCCESS); - event.wait(); - + void release(size_t id/*buffer ID*/){ + for(auto it=_buffers.begin();it!=_buffers.end();++it){ + if((*it).second==id){ + (*it).first->setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL); + (*it).first.reset(); + _buffers.erase(it); + return; + } } - assert(error == CL_SUCCESS); - return error; - }; - int32_t reload(size_t id,cv::Mat& buf,cl::Context* context){ - assert(!buf.empty()); - cl_int error; - try{ - for(auto&& i:_images)if(i.second==id){ - i.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL); - i.first=cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_USE_HOST_PTR, - cl::ImageFormat(CL_R, CL_UNSIGNED_INT8), - buf.size().width, - buf.size().height, - 0, - reinterpret_cast(buf.data),&error); - cl_int err=_kernel->setArg(id,i.first); - assert(err == CL_SUCCESS); - } - assert(error == CL_SUCCESS); - return error; - }catch (std::exception& err){ - std::cerr<<"Reload buffer on GPU failed\n"<setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL); + (*it).first.reset(); + _images.erase(it); + return; + } } }; - cl::Kernel* _kernel = NULL; - std::pair read(size_t id){ - for(auto i:_buffers)if(i.second==id)return i.first; + void release(){ + _buffers.clear(); + _images.clear(); } - cl::Image2D read_img(size_t id){ - for(auto i:_images)if(i.second==id)return i.first; + cl::Kernel* _kernel = NULL; + std::shared_ptr read(size_t id){ + for(auto&& i:_buffers)if(i.second==id)return i.first; } private: static void notify(cl_mem *, void * user_data) { - //std::cerr << "Memory object was deleted." << std::endl; } - std::vector,size_t>> _buffers; - std::vector> _images; + std::list,size_t>> _buffers; + std::list,size_t>> _images; }; class read_cl{ @@ -129,35 +111,20 @@ public: ~opencl(); int32_t make_kernel(std::string name){_kernels.push_back(kernel(program,name));}; template - bool write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, int size,T* buf) { - if(_kernels.at(id1).write(id2,buf,queue,context,size)!= CL_SUCCESS)return false; - return true; + void load(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, int size,T* buf) { + _kernels.at(id1).load(id2,buf,queue,context,size); } - bool write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,cv::Mat& buf) { - if(_kernels.at(id1).write(id2,buf,context)!= CL_SUCCESS)return false; - return true; + void load(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,cv::Mat& buf) { + _kernels.at(id1).load(id2,buf,context); } template - bool write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T&& value){ - if(_kernels.at(id1)._kernel->setArg(id2,value)!= CL_SUCCESS)return false; - return true; + void load(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T&& value){ + _kernels.at(id1)._kernel->setArg(id2,value); }; - template - bool reload_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T* buf) { - if(_kernels.at(id1).reload(id2,buf,queue)!= CL_SUCCESS)return false; - return true; - } - bool reload_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,cv::Mat& buf) { - if(_kernels.at(id1).reload(id2,buf,context)!= CL_SUCCESS)return false; - return true; - } cl_int run(size_t id1/*kernal ID*/,std::size_t x=1,std::size_t y=1,std::size_t z=1) { cl_int err=0; cl::Event event; - if(queue->flush()==CL_OUT_OF_HOST_MEMORY){ - std::cerr<<"GPU out of memory goodbye:)\n"; - exit(0); - } + assert(queue->flush()==CL_SUCCESS); if(z>1 && y>1){ err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y,z)/*Global*/, cl::NullRange/*local*/,NULL,&event); }else if(z<2 && y>1){ @@ -167,10 +134,7 @@ public: }; assert(err==CL_SUCCESS); event.wait(); - if(queue->finish()==CL_OUT_OF_HOST_MEMORY){ - std::cerr<<"GPU out of memory goodbye:)\n"; - exit(0); - } + assert(queue->finish()==CL_SUCCESS); return 1; } @@ -179,14 +143,19 @@ public: assert(out); cl_int error; cl::Event event; - T* Map_buf=(T*)queue->enqueueMapBuffer(_kernels.at(id1).read(id2).first,CL_NON_BLOCKING,CL_MAP_READ,0,sizeof(T) * size,NULL,&event,&error); + T* Map_buf=(T*)queue->enqueueMapBuffer(*_kernels.at(id1).read(id2),CL_NON_BLOCKING,CL_MAP_READ,0,sizeof(T) * size,NULL,&event,&error); assert(error == CL_SUCCESS); event.wait(); memcpy(out,Map_buf,sizeof(T) * size); - if(queue->enqueueUnmapMemObject(_kernels.at(id1).read(id2).first,Map_buf,NULL,&event)!=CL_SUCCESS)std::cerr<<"Read buffer on GPU failed"<<'\n'; + assert(queue->enqueueUnmapMemObject(*_kernels.at(id1).read(id2),Map_buf,NULL,&event)==CL_SUCCESS); event.wait(); } - void clear_buf(); + void clear_buf(){ + for(auto k:_kernels)k.release(); + }; + void release(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/){ + _kernels.at(id1).release(id2); + } private: std::vector _kernels; cl::Context* context = nullptr; diff --git a/GPU_version/vio/launch/Px30.launch b/GPU_version/vio/launch/Px30.launch index d12f08d..7c4cbdf 100644 --- a/GPU_version/vio/launch/Px30.launch +++ b/GPU_version/vio/launch/Px30.launch @@ -1,6 +1,6 @@ - + diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml index 0a29dc7..b791080 100644 --- a/GPU_version/vio/param/vo_fast.yaml +++ b/GPU_version/vio/param/vo_fast.yaml @@ -1,6 +1,6 @@ svo: grid_size: 8 #Feature grid size of a cell in [px]. - max_n_kfs: 20 #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed. Minimum number of keyframes is 3. + max_n_kfs: 15 #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed. Minimum number of keyframes is 3. loba_num_iter: 10 #Number of iterations in the local bundle adjustment. quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad. quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad. @@ -12,5 +12,5 @@ svo: init_min_inliers: 50 ACC_noise: 0.4 #acc white noise in continuous in EKF GYO_noise: 0.4 #gyro white noise in continuous in EKF - svo_ekf: 0.1 #Svo noise that will be devided by number of Reprojected points after opmization in EKF + svo_ekf: 0.2 #Svo noise that will be devided by number of Reprojected points after opmization in EKF Cmd_Cov: 0.1 #control vector noise in EKF diff --git a/GPU_version/vio/src/cl_class.cpp b/GPU_version/vio/src/cl_class.cpp index f615102..449611a 100644 --- a/GPU_version/vio/src/cl_class.cpp +++ b/GPU_version/vio/src/cl_class.cpp @@ -20,6 +20,7 @@ opencl::opencl(vk::AbstractCamera* cam):cam(cam) { std::cout << " No devices found. Check OpenCL installation!\n"; exit(1); } + device = new cl::Device(all_devices[0]); std::cout << "CL_DEVICE_NAME: " << device->getInfo() <<'\n' << "CL_DRIVER_VERSION: " <getInfo()<<'\n' @@ -51,12 +52,10 @@ opencl::opencl(vk::AbstractCamera* cam):cam(cam) { queue=new cl::CommandQueue(*context,*device,CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE,NULL); } opencl::~opencl() { + clear_buf(); + queue->finish(); + delete program; delete context; delete device; } -void opencl::clear_buf() { - queue->finish(); - _kernels.clear(); - delete program; -} diff --git a/GPU_version/vio/src/feature_detection.cpp b/GPU_version/vio/src/feature_detection.cpp index 6c45bcb..ea12704 100644 --- a/GPU_version/vio/src/feature_detection.cpp +++ b/GPU_version/vio/src/feature_detection.cpp @@ -70,51 +70,54 @@ void FastDetector::detect( std::shared_ptr frame, const ImgPyr& img_pyr, const double detection_threshold, - list>& fts){ + list>& fts) + { Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,0.0,0,0.0f)); std::vector keypoints; - fts.clear(); for(int L=0; Limg_pyr.size())return; int scale = (1<reload_buf(0,0,img)){ - ROS_ERROR("Failed to write into GPU goodbye :)"); - exit(0); - }; + gpu_fast_->load(0,0,img); + cl_int2 fast_corner[1000]={0}; + gpu_fast_->load(0,1,1000,fast_corner); cl_int icorner[1]={0}; - if(!gpu_fast_->reload_buf(0,2,icorner)){ - ROS_ERROR("Failed to write into GPU goodbye :)"); - exit(0); - }; + gpu_fast_->load(0,2,1,icorner); gpu_fast_->run(0,img.cols,img.rows); - cl_int count[1]={0}; + cl_int count[1]; gpu_fast_->read(0,2,1,count); - cl_int2 fast_corners[350000]={0}; - gpu_fast_->read(0,1,350000,fast_corners); if(count[0]<1 ){ - ROS_ERROR("Can not communicate with GPU"); - exit(0); + ROS_ERROR("Can not communicate with GPU"); + exit(0); } int size=count[0]; + cl_int2 fast_corners[size]; + gpu_fast_->read(0,1,size,fast_corners); for(uint i=0;iimg_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<1){ + if(fast_corners[i].x<5 || fast_corners[i].x>img_pyr[L].cols-5 || fast_corners[i].y>img_pyr[L].rows-5 || fast_corners[i].y<5){ continue; } float score = vk::shiTomasiScore(img_pyr[L], fast_corners[i].x, fast_corners[i].y); keypoints.push_back(cv::KeyPoint(fast_corners[i].x*scale, fast_corners[i].y*scale, 7.f,-1,score)); } + gpu_fast_->release(0,0); + gpu_fast_->release(0,1); + gpu_fast_->release(0,2); } if(keypoints.size()<1){ ROS_ERROR("GPU Driver crash try again!"); assert(0); } + std::cerr< extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4); cv::Mat descriptor; extractor->compute(frame->img(), keypoints, descriptor); - for(auto&& p:_for(keypoints))fts.push_back(make_shared(frame, Vector2d(p.item.pt.x, p.item.pt.y), p.item.response ,0,descriptor.data+(p.index*64))); + fts.clear(); + for(auto&& p:_for(keypoints)){ + fts.push_back(make_shared(frame, Vector2d(p.item.pt.x, p.item.pt.y), p.item.response ,0,descriptor.data+(p.index*64))); + } resetGrid(); } diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 0722c44..a7664b3 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -40,37 +40,7 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrixmake_kernel("fast_gray"); - cv::Mat img=cv::Mat(cv::Size(700, 500),CV_8UC1); - std::vector errors; - errors.push_back(gpu_fast_->write_buf(0,0,img)); - cl_int2 corners_[350000]={0}; - errors.push_back(gpu_fast_->write_buf(0,1,350000,corners_)); - cl_int icorner[1]={0}; - errors.push_back(gpu_fast_->write_buf(0,2,1,icorner)); gpu_fast_->make_kernel("compute_residual"); - errors.push_back(gpu_fast_->write_buf(1,0,img)); - errors.push_back(gpu_fast_->write_buf(1,1,img)); - cl_float3 pose[1]={0.0}; - errors.push_back(gpu_fast_->write_buf(1,2,3,pose)); - errors.push_back(gpu_fast_->write_buf(1,3,3,pose)); - cl_float3 f[300]={0}; - errors.push_back(gpu_fast_->write_buf(1,4,300,f)); - cl_float2 px[300]={0}; - errors.push_back(gpu_fast_->write_buf(1,5,300,px)); - cl_float e[300]={0}; - errors.push_back(gpu_fast_->write_buf(1,7,300,e)); - cl_float H[9*300]={0}; - errors.push_back(gpu_fast_->write_buf(1,8,9*300,H)); - cl_float3 J[300]; - errors.push_back(gpu_fast_->write_buf(1,9,300,J)); - cl_float chi2[300]={0.01}; - errors.push_back(gpu_fast_->write_buf(1,10,300,chi2)); - for(auto e:errors){ - if(e!=true){ - std::cerr<<"Failed to write into GPU goodbye :) "<<'\n'; - exit(0); - } - } initialize(); #if VIO_DEBUG log_ =fopen((std::string(PROJECT_DIR)+"/frame_handler_log.txt").c_str(),"w+"); @@ -84,9 +54,6 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrixwidth(), cam_->height(), Config::gridSize(),gpu_fast_, Config::nPyrLevels())); ba_glob_ = new BA_Glob(map_); ba_glob_->startThread(); } diff --git a/GPU_version/vio/src/sparse_img_align_gpu.cpp b/GPU_version/vio/src/sparse_img_align_gpu.cpp index aea39f4..4818f91 100644 --- a/GPU_version/vio/src/sparse_img_align_gpu.cpp +++ b/GPU_version/vio/src/sparse_img_align_gpu.cpp @@ -15,6 +15,7 @@ // along with this program. If not, see . #include +#include namespace vio { @@ -35,49 +36,58 @@ SparseImgAlignGpu::SparseImgAlignGpu( size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log) { reset(); - errors.clear(); cl_float3 ref_pos[1]={(float)ref_frame->pos()(0),(float)ref_frame->pos()(1),(float)ref_frame->T_f_w_.pitch()}; cl_float3 cur_pos[1]={(float)cur_frame->pos()(0),(float)cur_frame->pos()(1),(float)cur_frame->T_f_w_.pitch()}; - errors.push_back(residual_->reload_buf(1,2,cur_pos)); - errors.push_back(residual_->reload_buf(1,3,ref_pos)); + residual_->load(1,2,1,cur_pos); + residual_->load(1,3,1,ref_pos); cl_float3 features[ref_frame->fts_.size()]; cl_float2 featue_px[ref_frame->fts_.size()]; feature_counter_ = 0; // is used to compute the index of the cached jacobian - for(auto it=ref_frame->fts_.begin(); it!=ref_frame->fts_.end();++it){ - if(*it == nullptr)continue; - if((*it)->point == nullptr)continue; - Vector3d xyz_ref=(*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0].x,0.0,ref_pos[0].y)).norm(); - features[feature_counter_].x=xyz_ref(0); - features[feature_counter_].y=xyz_ref(1); - features[feature_counter_].z=xyz_ref(2); - featue_px[feature_counter_].x=(*it)->px(0); - featue_px[feature_counter_].y=(*it)->px(1); - if(feature_counter_>299)break; - ++feature_counter_; + for(auto it:_for(ref_frame->fts_)){ + ++feature_counter_; + if(it.item->point == nullptr){ + features[it.index].x=0.f; + features[it.index].y=0.f; + features[it.index].z=0.f; + featue_px[it.index].x=0.f; + featue_px[it.index].y=0.f; + continue; + } + Vector3d xyz_ref=it.item->f*(ref_frame->se3().inverse()*it.item->point->pos_).norm(); + features[it.index].x=xyz_ref(0); + features[it.index].y=xyz_ref(1); + features[it.index].z=xyz_ref(2); + featue_px[it.index].x=it.item->px(0); + featue_px[it.index].y=it.item->px(1); + } if(!feature_counter_) // more than 10 { return 0; } - errors.push_back(residual_->reload_buf(1,4,features)); - errors.push_back(residual_->reload_buf(1,5,featue_px)); + feature_counter_=ref_frame->fts_.size(); + cl_float error[feature_counter_]; + cl_float chi[feature_counter_]; + residual_->load(1,10,feature_counter_,chi); + residual_->load(1,7,feature_counter_,error); + residual_->load(1,4,feature_counter_,features); + residual_->load(1,5,feature_counter_,featue_px); SE2 T_cur(cur_frame->T_f_w_.se2());///TODO temporary, we can remove it for(level_=max_level_; level_>=min_level_; --level_) { cv::Mat cur_img = cur_frame->img_pyr_.at(level_); cv::Mat ref_img = ref_frame->img_pyr_.at(level_); - errors.push_back(residual_->reload_buf(1,0,cur_img)); - errors.push_back(residual_->reload_buf(1,1,ref_img)); - errors.push_back(residual_->write_buf(1,6,level_)); + residual_->load(1,0,cur_img); + residual_->load(1,1,ref_img); + residual_->load(1,6,level_); mu_ = 1.0; - for(auto&& e:errors)if(!e)return -1; - errors.clear(); optimize(T_cur); + residual_->release(1,0); + residual_->release(1,1); } - for(auto&& e:errors)if(!e)return -1; cl_float3 pos[1]={0}; residual_->read(1,2,1,pos); - if(isnan(pos[0].x) || isnan(pos[0].y) || isnan(pos[0].z))return 0; + if(isnan(pos[0].x) || isnan(pos[0].y) || isnan(pos[0].z))return 1; cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z); return 1; } @@ -86,17 +96,17 @@ double SparseImgAlignGpu::computeResiduals( bool linearize_system, bool compute_weight_scale) { - cl_float H[9*300]={0}; - cl_float3 J[300]={0.0}; - cl_float chi2_[300]={0.0}; + cl_float H[9*feature_counter_]; + cl_float3 J[feature_counter_]; + cl_float chi2_[feature_counter_]; cl_float scale=(float)scale_; - errors.push_back(residual_->write_buf(1,11,scale)); - errors.push_back(residual_->reload_buf(1,10,chi2_)); - errors.push_back(residual_->reload_buf(1,8,H)); - errors.push_back(residual_->reload_buf(1,9,J)); + residual_->load(1,11,scale); + residual_->load(1,10,feature_counter_,chi2_); + residual_->load(1,8,9*feature_counter_,H); + residual_->load(1,9,feature_counter_,J); residual_->run(1,feature_counter_); - cl_float error[300]={0.0}; - cl_float chi[300]={0.0}; + cl_float error[feature_counter_]; + cl_float chi[feature_counter_]; residual_->read(1,10,feature_counter_,chi); residual_->read(1,7,feature_counter_,error); for(int i = 1; i < feature_counter_; ++i){ @@ -109,8 +119,8 @@ double SparseImgAlignGpu::computeResiduals( bool SparseImgAlignGpu::solve() { - cl_float H[300*9]={0.0}; - cl_float3 J[300]={0.0}; + cl_float H[feature_counter_*9]; + cl_float3 J[feature_counter_]; residual_->read(1,8,feature_counter_*9,H); residual_->read(1,9,feature_counter_,J); for(int i = 1; i < feature_counter_; ++i){ @@ -133,6 +143,10 @@ bool SparseImgAlignGpu::solve() x_ = Eigen::Matrix(Hd).ldlt().solve(Eigen::Vector3d((double)J[0].x,(double)J[0].y,(double)J[0].z)); double norm=x_.norm(); if(norm<=0 ||norm > 1.0)x_=Eigen::Vector3d(0.1,0.1,0.1); + residual_->release(1,10); + residual_->release(1,11); + residual_->release(1,8); + residual_->release(1,9); return true; } void SparseImgAlignGpu::update() @@ -143,7 +157,8 @@ void SparseImgAlignGpu::update() pos[0].x=(float)update.translation()(0); pos[0].y=(float)update.translation()(1); pos[0].z=(float)atan2(update.so2().unit_complex().imag(),update.so2().unit_complex().real()); - errors.push_back(residual_->reload_buf(1,2,pos)); + residual_->release(1,2); + residual_->load(1,2,1,pos); } void SparseImgAlignGpu::startIteration()