From 8f4ddb9c727007ff3234cde51e612dd001e59f67 Mon Sep 17 00:00:00 2001 From: gfuse Date: Fri, 24 Sep 2021 19:51:09 +0200 Subject: [PATCH] correct triangulation --- GPU_version/vio/frame_handler_log.txt | 2580 +++++++++++--------- GPU_version/vio/include/vio/global.h | 2 +- GPU_version/vio/include/vio/ukf.h | 6 +- GPU_version/vio/kernel/compute-residual.cl | 2 +- GPU_version/vio/param/vo_fast.yaml | 4 +- GPU_version/vio/src/feature_detection.cpp | 2 +- GPU_version/vio/src/frame_handler_mono.cpp | 1 - GPU_version/vio/src/initialization.cpp | 2 +- GPU_version/vio/src/math_utils.cpp | 12 +- GPU_version/vio/src/reprojector.cpp | 2 +- 10 files changed, 1439 insertions(+), 1174 deletions(-) diff --git a/GPU_version/vio/frame_handler_log.txt b/GPU_version/vio/frame_handler_log.txt index 1d9800f..a8b9a96 100755 --- a/GPU_version/vio/frame_handler_log.txt +++ b/GPU_version/vio/frame_handler_log.txt @@ -1,1157 +1,1423 @@ -[16:45:42.378] Init: frame zero: 0.000000, 0.000000 0.000000 -[16:45:42.378] Init: Selected first frame. -[16:45:42.378] frame process finished the id is: 0 the obs is:0 -[16:45:42.477] Init: px average disparity is:0.195337 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:42.477] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:42.477] frame process finished the id is: 1 the obs is:0 -[16:45:42.572] Init: px average disparity is:0.226763 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:42.572] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:42.572] frame process finished the id is: 2 the obs is:0 -[16:45:42.672] Init: px average disparity is:0.237553 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:42.672] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:42.672] frame process finished the id is: 3 the obs is:0 -[16:45:42.774] Init: px average disparity is:0.258666 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:42.774] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:42.774] frame process finished the id is: 4 the obs is:0 -[16:45:42.873] Init: px average disparity is:0.231123 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:42.873] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:42.873] frame process finished the id is: 5 the obs is:0 -[16:45:42.973] Init: px average disparity is:0.224912 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:42.973] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:42.973] frame process finished the id is: 6 the obs is:0 -[16:45:43.076] Init: px average disparity is:0.233781 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:43.076] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:43.076] frame process finished the id is: 7 the obs is:0 -[16:45:43.176] Init: px average disparity is:0.235062 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:43.176] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:43.176] frame process finished the id is: 8 the obs is:0 -[16:45:43.272] Init: px average disparity is:0.257024 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:43.272] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:43.272] frame process finished the id is: 9 the obs is:0 -[16:45:43.371] Init: px average disparity is:0.226920 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:43.371] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:43.371] frame process finished the id is: 10 the obs is:0 -[16:45:43.472] Init: px average disparity is:0.237733 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:43.472] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:43.472] frame process finished the id is: 11 the obs is:0 -[16:45:43.572] Init: px average disparity is:0.243488 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:43.572] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:43.572] frame process finished the id is: 12 the obs is:0 -[16:45:43.673] Init: px average disparity is:0.238793 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:43.673] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:43.673] frame process finished the id is: 13 the obs is:0 -[16:45:43.771] Init: px average disparity is:0.246205 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:43.771] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:43.771] frame process finished the id is: 14 the obs is:0 -[16:45:43.871] Init: px average disparity is:0.231229 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:43.871] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:43.871] frame process finished the id is: 15 the obs is:0 -[16:45:43.971] Init: px average disparity is:0.246222 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:43.971] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:43.971] frame process finished the id is: 16 the obs is:0 -[16:45:44.072] Init: px average disparity is:0.259163 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:44.072] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:44.072] frame process finished the id is: 17 the obs is:0 -[16:45:44.171] Init: px average disparity is:0.246588 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:44.171] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:44.171] frame process finished the id is: 18 the obs is:0 -[16:45:44.271] Init: px average disparity is:0.230749 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:44.271] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:44.271] frame process finished the id is: 19 the obs is:0 -[16:45:44.371] Init: px average disparity is:0.232215 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:44.371] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:44.371] frame process finished the id is: 20 the obs is:0 -[16:45:44.472] Init: px average disparity is:0.257071 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:44.472] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:44.472] frame process finished the id is: 21 the obs is:0 -[16:45:44.571] Init: px average disparity is:0.265590 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:44.571] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:44.571] frame process finished the id is: 22 the obs is:0 -[16:45:44.673] Init: px average disparity is:0.234640 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:44.673] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:44.673] frame process finished the id is: 23 the obs is:0 -[16:45:44.771] Init: px average disparity is:0.222834 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:44.771] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:44.771] frame process finished the id is: 24 the obs is:0 -[16:45:44.875] Init: px average disparity is:0.248688 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:44.875] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:44.875] frame process finished the id is: 25 the obs is:0 -[16:45:44.980] Init: px average disparity is:0.241508 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:44.980] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:44.980] frame process finished the id is: 26 the obs is:0 -[16:45:45.083] Init: px average disparity is:0.220067 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:45.084] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:45.084] frame process finished the id is: 27 the obs is:0 -[16:45:45.178] Init: px average disparity is:0.253990 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:45.179] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:45.179] frame process finished the id is: 28 the obs is:0 -[16:45:45.279] Init: px average disparity is:0.226867 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:45.279] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:45.279] frame process finished the id is: 29 the obs is:0 -[16:45:45.318] Odometry x=0.000000, y=0.000000, theta=0.000000 -[16:45:45.376] Init: px average disparity is:0.228814 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:45.376] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:45.376] frame process finished the id is: 30 the obs is:0 -[16:45:45.415] Odometry x=0.000000, y=0.000000, theta=0.000000 -[16:45:45.476] Init: px average disparity is:0.211986 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:45.476] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:45.476] frame process finished the id is: 31 the obs is:0 -[16:45:45.516] Odometry x=0.000030, y=0.006144, theta=0.001009 -[16:45:45.577] Init: px average disparity is:1.728968 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:45.578] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:45.578] frame process finished the id is: 32 the obs is:0 -[16:45:45.617] Odometry x=0.000264, y=0.012817, theta=0.001739 -[16:45:45.676] Init: px average disparity is:5.969511 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:45.676] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:45.676] frame process finished the id is: 33 the obs is:0 -[16:45:45.717] Odometry x=0.000373, y=0.019883, theta=0.004609 -[16:45:45.773] Init: px average disparity is:7.696527 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:45.773] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:45.773] frame process finished the id is: 34 the obs is:0 -[16:45:45.820] Odometry x=0.000439, y=0.027053, theta=0.000536 -[16:45:45.873] Init: px average disparity is:5.019382 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:45.873] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:45.873] frame process finished the id is: 35 the obs is:0 -[16:45:45.917] Odometry x=0.000485, y=0.032463, theta=0.001637 -[16:45:45.972] Init: px average disparity is:5.397388 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:45.972] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:45.972] frame process finished the id is: 36 the obs is:0 -[16:45:46.019] Odometry x=0.002160, y=0.040232, theta=-0.000166 -[16:45:46.073] Init: px average disparity is:4.399790 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:46.073] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:46.073] frame process finished the id is: 37 the obs is:0 -[16:45:46.116] Odometry x=0.002087, y=0.044824, theta=0.007467 -[16:45:46.173] Init: px average disparity is:5.837076 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:46.173] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:46.173] frame process finished the id is: 38 the obs is:0 -[16:45:46.219] Odometry x=0.002111, y=0.051240, theta=-0.006289 -[16:45:46.273] Init: px average disparity is:4.549396 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:46.273] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:46.273] frame process finished the id is: 39 the obs is:0 -[16:45:46.316] Odometry x=0.002153, y=0.055969, theta=0.019988 -[16:45:46.373] Init: px average disparity is:6.377915 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:46.373] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:46.373] frame process finished the id is: 40 the obs is:0 -[16:45:46.419] Odometry x=0.002669, y=0.061882, theta=0.009396 -[16:45:46.473] Init: px average disparity is:7.749400 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:46.473] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:46.473] frame process finished the id is: 41 the obs is:0 -[16:45:46.516] Odometry x=0.002687, y=0.067236, theta=0.002295 -[16:45:46.573] Init: px average disparity is:9.398943 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:46.573] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:46.573] frame process finished the id is: 42 the obs is:0 -[16:45:46.619] Odometry x=0.002803, y=0.073105, theta=0.005819 -[16:45:46.673] Init: px average disparity is:12.650750 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:46.673] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:46.673] frame process finished the id is: 43 the obs is:0 -[16:45:46.716] Odometry x=0.002866, y=0.076747, theta=0.022906 -[16:45:46.773] Init: px average disparity is:15.372350 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:46.773] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:46.773] frame process finished the id is: 44 the obs is:0 -[16:45:46.821] Odometry x=0.003398, y=0.083289, theta=0.014410 -[16:45:46.873] Init: px average disparity is:13.601748 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:46.873] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:46.873] frame process finished the id is: 45 the obs is:0 -[16:45:46.919] Odometry x=0.000144, y=0.087348, theta=0.015381 -[16:45:46.974] Init: px average disparity is:12.911185 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:46.974] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:46.974] frame process finished the id is: 46 the obs is:0 -[16:45:47.016] Odometry x=0.000272, y=0.094313, theta=0.010783 -[16:45:47.073] Init: px average disparity is:11.388552 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:47.073] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:47.073] frame process finished the id is: 47 the obs is:0 -[16:45:47.119] Odometry x=0.000494, y=0.099812, theta=0.006026 -[16:45:47.173] Init: px average disparity is:12.987979 ,While minimum is: 40.000000 KLT tracked : 141 -[16:45:47.173] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:47.173] frame process finished the id is: 48 the obs is:0 -[16:45:47.216] Odometry x=0.000642, y=0.105393, theta=0.006068 -[16:45:47.273] Init: px average disparity is:13.196436 ,While minimum is: 40.000000 KLT tracked : 140 -[16:45:47.273] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:47.273] frame process finished the id is: 49 the obs is:0 -[16:45:47.319] Odometry x=0.000840, y=0.111363, theta=0.010317 -[16:45:47.373] Init: px average disparity is:19.365494 ,While minimum is: 40.000000 KLT tracked : 140 -[16:45:47.373] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:47.373] frame process finished the id is: 50 the obs is:0 -[16:45:47.431] Odometry x=0.001818, y=0.118832, theta=0.003070 -[16:45:47.473] Init: px average disparity is:14.017763 ,While minimum is: 40.000000 KLT tracked : 140 -[16:45:47.473] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:47.473] frame process finished the id is: 51 the obs is:0 -[16:45:47.519] Odometry x=0.006040, y=0.124887, theta=0.020004 -[16:45:47.573] Init: px average disparity is:13.778574 ,While minimum is: 40.000000 KLT tracked : 140 -[16:45:47.573] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:47.573] frame process finished the id is: 52 the obs is:0 -[16:45:47.621] Odometry x=0.005997, y=0.138801, theta=0.018212 -[16:45:47.679] Init: px average disparity is:9.031697 ,While minimum is: 40.000000 KLT tracked : 140 -[16:45:47.679] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:47.679] frame process finished the id is: 53 the obs is:0 -[16:45:47.718] Odometry x=0.008016, y=0.143634, theta=0.016310 -[16:45:47.777] Init: px average disparity is:9.554431 ,While minimum is: 40.000000 KLT tracked : 140 -[16:45:47.777] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:47.777] frame process finished the id is: 54 the obs is:0 -[16:45:47.815] Odometry x=0.008794, y=0.149266, theta=0.021474 -[16:45:47.877] Init: px average disparity is:9.439856 ,While minimum is: 40.000000 KLT tracked : 139 -[16:45:47.877] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:47.877] frame process finished the id is: 55 the obs is:0 -[16:45:47.919] Odometry x=0.009888, y=0.154403, theta=0.022781 -[16:45:47.979] Init: px average disparity is:10.355324 ,While minimum is: 40.000000 KLT tracked : 139 -[16:45:47.979] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:47.979] frame process finished the id is: 56 the obs is:0 -[16:45:48.021] Odometry x=0.010167, y=0.159802, theta=0.028145 -[16:45:48.080] Init: px average disparity is:11.633310 ,While minimum is: 40.000000 KLT tracked : 139 -[16:45:48.080] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:48.080] frame process finished the id is: 57 the obs is:0 -[16:45:48.123] Odometry x=0.011785, y=0.165219, theta=0.023996 -[16:45:48.178] Init: px average disparity is:9.985569 ,While minimum is: 40.000000 KLT tracked : 139 -[16:45:48.178] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:48.178] frame process finished the id is: 58 the obs is:0 -[16:45:48.220] Odometry x=0.011829, y=0.170719, theta=0.004971 -[16:45:48.278] Init: px average disparity is:12.505885 ,While minimum is: 40.000000 KLT tracked : 139 -[16:45:48.278] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:48.278] frame process finished the id is: 59 the obs is:0 -[16:45:48.323] Odometry x=0.012173, y=0.176202, theta=0.014182 -[16:45:48.373] Init: px average disparity is:14.976710 ,While minimum is: 40.000000 KLT tracked : 139 -[16:45:48.373] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:48.373] frame process finished the id is: 60 the obs is:0 -[16:45:48.423] Odometry x=0.012229, y=0.183939, theta=0.013182 -[16:45:48.473] Init: px average disparity is:15.919399 ,While minimum is: 40.000000 KLT tracked : 139 -[16:45:48.473] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:48.473] frame process finished the id is: 61 the obs is:0 -[16:45:48.521] Odometry x=0.014246, y=0.189275, theta=0.020772 -[16:45:48.575] Init: px average disparity is:16.986342 ,While minimum is: 40.000000 KLT tracked : 139 -[16:45:48.575] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:48.575] frame process finished the id is: 62 the obs is:0 -[16:45:48.618] Odometry x=0.014295, y=0.195044, theta=0.014102 -[16:45:48.673] Init: px average disparity is:17.369218 ,While minimum is: 40.000000 KLT tracked : 139 -[16:45:48.673] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:48.673] frame process finished the id is: 63 the obs is:0 -[16:45:48.716] Odometry x=0.015098, y=0.200882, theta=0.018974 -[16:45:48.774] Init: px average disparity is:17.744380 ,While minimum is: 40.000000 KLT tracked : 139 -[16:45:48.774] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:48.774] frame process finished the id is: 64 the obs is:0 -[16:45:48.818] Odometry x=0.015107, y=0.206302, theta=0.014067 -[16:45:48.875] Init: px average disparity is:19.680449 ,While minimum is: 40.000000 KLT tracked : 139 -[16:45:48.875] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:48.875] frame process finished the id is: 65 the obs is:0 -[16:45:48.920] Odometry x=0.016537, y=0.211906, theta=0.024605 -[16:45:48.974] Init: px average disparity is:21.940514 ,While minimum is: 40.000000 KLT tracked : 139 -[16:45:48.974] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:48.974] frame process finished the id is: 66 the obs is:0 -[16:45:49.018] Odometry x=0.015016, y=0.217538, theta=0.016655 -[16:45:49.074] Init: px average disparity is:19.891189 ,While minimum is: 40.000000 KLT tracked : 139 -[16:45:49.074] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:49.074] frame process finished the id is: 67 the obs is:0 -[16:45:49.120] Odometry x=0.015154, y=0.224921, theta=0.010038 -[16:45:49.175] Init: px average disparity is:19.173957 ,While minimum is: 40.000000 KLT tracked : 139 -[16:45:49.175] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:49.175] frame process finished the id is: 68 the obs is:0 -[16:45:49.217] Odometry x=0.011765, y=0.230274, theta=0.022781 -[16:45:49.274] Init: px average disparity is:18.893633 ,While minimum is: 40.000000 KLT tracked : 138 -[16:45:49.274] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:49.275] frame process finished the id is: 69 the obs is:0 -[16:45:49.319] Odometry x=0.013405, y=0.237326, theta=-0.001481 -[16:45:49.375] Init: px average disparity is:19.179060 ,While minimum is: 40.000000 KLT tracked : 138 -[16:45:49.375] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:49.375] frame process finished the id is: 70 the obs is:0 -[16:45:49.422] Odometry x=0.013852, y=0.243076, theta=0.011350 -[16:45:49.474] Init: px average disparity is:21.607310 ,While minimum is: 40.000000 KLT tracked : 138 -[16:45:49.474] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:49.474] frame process finished the id is: 71 the obs is:0 -[16:45:49.519] Odometry x=0.013948, y=0.256087, theta=-0.008203 -[16:45:49.575] Init: px average disparity is:21.197031 ,While minimum is: 40.000000 KLT tracked : 138 -[16:45:49.575] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:49.575] frame process finished the id is: 72 the obs is:0 -[16:45:49.621] Odometry x=0.014036, y=0.262366, theta=0.019339 -[16:45:49.674] Init: px average disparity is:21.803058 ,While minimum is: 40.000000 KLT tracked : 134 -[16:45:49.674] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:49.674] frame process finished the id is: 73 the obs is:0 -[16:45:49.719] Odometry x=0.013122, y=0.268682, theta=0.015871 -[16:45:49.774] Init: px average disparity is:22.236026 ,While minimum is: 40.000000 KLT tracked : 134 -[16:45:49.774] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:49.775] frame process finished the id is: 74 the obs is:0 -[16:45:49.821] Odometry x=0.013179, y=0.274069, theta=0.027891 -[16:45:49.874] Init: px average disparity is:20.580449 ,While minimum is: 40.000000 KLT tracked : 134 -[16:45:49.874] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:49.874] frame process finished the id is: 75 the obs is:0 -[16:45:49.918] Odometry x=0.012825, y=0.287155, theta=0.025605 -[16:45:49.974] Init: px average disparity is:21.321663 ,While minimum is: 40.000000 KLT tracked : 134 -[16:45:49.974] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:49.974] frame process finished the id is: 76 the obs is:0 -[16:45:50.016] Odometry x=0.012989, y=0.292801, theta=0.034093 -[16:45:50.075] Init: px average disparity is:21.081142 ,While minimum is: 40.000000 KLT tracked : 134 -[16:45:50.075] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:50.075] frame process finished the id is: 77 the obs is:0 -[16:45:50.118] Odometry x=0.011475, y=0.304326, theta=0.017043 -[16:45:50.174] Init: px average disparity is:22.979781 ,While minimum is: 40.000000 KLT tracked : 134 -[16:45:50.174] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:50.174] frame process finished the id is: 78 the obs is:0 -[16:45:50.215] Odometry x=0.011532, y=0.309937, theta=0.023449 -[16:45:50.275] Init: px average disparity is:21.637618 ,While minimum is: 40.000000 KLT tracked : 134 -[16:45:50.276] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:50.276] frame process finished the id is: 79 the obs is:0 -[16:45:50.320] Odometry x=0.011602, y=0.315425, theta=0.012641 -[16:45:50.375] Init: px average disparity is:22.527517 ,While minimum is: 40.000000 KLT tracked : 134 -[16:45:50.375] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:50.375] frame process finished the id is: 80 the obs is:0 -[16:45:50.423] Odometry x=0.011771, y=0.320944, theta=0.015694 -[16:45:50.475] Init: px average disparity is:21.696075 ,While minimum is: 40.000000 KLT tracked : 134 -[16:45:50.475] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:50.475] frame process finished the id is: 81 the obs is:0 -[16:45:50.519] Odometry x=0.011845, y=0.326800, theta=0.012137 -[16:45:50.574] Init: px average disparity is:23.619170 ,While minimum is: 40.000000 KLT tracked : 134 -[16:45:50.574] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:50.574] frame process finished the id is: 82 the obs is:0 -[16:45:50.618] Odometry x=0.011909, y=0.332233, theta=0.027821 -[16:45:50.675] Init: px average disparity is:22.169395 ,While minimum is: 40.000000 KLT tracked : 134 -[16:45:50.675] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:50.675] frame process finished the id is: 83 the obs is:0 -[16:45:50.721] Odometry x=0.013009, y=0.340411, theta=0.011576 -[16:45:50.775] Init: px average disparity is:23.200878 ,While minimum is: 40.000000 KLT tracked : 134 -[16:45:50.775] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:50.775] frame process finished the id is: 84 the obs is:0 -[16:45:50.818] Odometry x=0.013320, y=0.345970, theta=0.034525 -[16:45:50.875] Init: px average disparity is:22.127173 ,While minimum is: 40.000000 KLT tracked : 134 -[16:45:50.875] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:50.875] frame process finished the id is: 85 the obs is:0 -[16:45:50.920] Odometry x=0.013199, y=0.353746, theta=0.000630 -[16:45:50.975] Init: px average disparity is:23.783920 ,While minimum is: 40.000000 KLT tracked : 131 -[16:45:50.975] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:50.975] frame process finished the id is: 86 the obs is:0 -[16:45:51.023] Odometry x=0.014957, y=0.359413, theta=0.017228 -[16:45:51.074] Init: px average disparity is:24.098322 ,While minimum is: 40.000000 KLT tracked : 131 -[16:45:51.074] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:51.074] frame process finished the id is: 87 the obs is:0 -[16:45:51.121] Odometry x=0.014879, y=0.367409, theta=0.024471 -[16:45:51.175] Init: px average disparity is:26.525476 ,While minimum is: 40.000000 KLT tracked : 131 -[16:45:51.175] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:51.175] frame process finished the id is: 88 the obs is:0 -[16:45:51.219] Odometry x=0.014949, y=0.373408, theta=0.012727 -[16:45:51.276] Init: px average disparity is:27.979359 ,While minimum is: 40.000000 KLT tracked : 131 -[16:45:51.276] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:51.276] frame process finished the id is: 89 the obs is:0 -[16:45:51.322] Odometry x=0.016517, y=0.383675, theta=0.023173 -[16:45:51.375] Init: px average disparity is:27.833860 ,While minimum is: 40.000000 KLT tracked : 131 -[16:45:51.375] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:51.375] frame process finished the id is: 90 the obs is:0 -[16:45:51.420] Odometry x=0.013236, y=0.389204, theta=0.036515 -[16:45:51.475] Init: px average disparity is:27.244740 ,While minimum is: 40.000000 KLT tracked : 131 -[16:45:51.475] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:51.475] frame process finished the id is: 91 the obs is:0 -[16:45:51.518] Odometry x=0.013646, y=0.395227, theta=0.026898 -[16:45:51.577] Init: px average disparity is:26.446092 ,While minimum is: 40.000000 KLT tracked : 131 -[16:45:51.577] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:51.577] frame process finished the id is: 92 the obs is:0 -[16:45:51.622] Odometry x=0.012215, y=0.399215, theta=0.041790 -[16:45:51.675] Init: px average disparity is:25.980279 ,While minimum is: 40.000000 KLT tracked : 131 -[16:45:51.675] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:51.675] frame process finished the id is: 93 the obs is:0 -[16:45:51.721] Odometry x=0.012581, y=0.405738, theta=0.019651 -[16:45:51.778] Init: px average disparity is:25.457305 ,While minimum is: 40.000000 KLT tracked : 131 -[16:45:51.778] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:51.778] frame process finished the id is: 94 the obs is:0 -[16:45:51.823] Odometry x=0.011998, y=0.411194, theta=0.027945 -[16:45:51.877] Init: px average disparity is:26.109460 ,While minimum is: 40.000000 KLT tracked : 131 -[16:45:51.877] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:51.877] frame process finished the id is: 95 the obs is:0 -[16:45:51.918] Odometry x=0.012085, y=0.416740, theta=0.020379 -[16:45:51.975] Init: px average disparity is:26.070761 ,While minimum is: 40.000000 KLT tracked : 131 -[16:45:51.975] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:51.975] frame process finished the id is: 96 the obs is:0 -[16:45:52.020] Odometry x=0.012153, y=0.422448, theta=0.005848 -[16:45:52.077] Init: px average disparity is:27.052952 ,While minimum is: 40.000000 KLT tracked : 131 -[16:45:52.077] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:52.077] frame process finished the id is: 97 the obs is:0 -[16:45:52.123] Odometry x=0.012182, y=0.428103, theta=0.021724 -[16:45:52.178] Init: px average disparity is:26.432629 ,While minimum is: 40.000000 KLT tracked : 131 -[16:45:52.178] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:52.178] frame process finished the id is: 98 the obs is:0 -[16:45:52.222] Odometry x=0.012208, y=0.437744, theta=0.009955 -[16:45:52.277] Init: px average disparity is:27.385010 ,While minimum is: 40.000000 KLT tracked : 130 -[16:45:52.277] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:52.277] frame process finished the id is: 99 the obs is:0 -[16:45:52.319] Odometry x=0.012606, y=0.443214, theta=0.018077 -[16:45:52.375] Init: px average disparity is:27.539318 ,While minimum is: 40.000000 KLT tracked : 130 -[16:45:52.375] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:52.375] frame process finished the id is: 100 the obs is:0 -[16:45:52.421] Odometry x=0.007360, y=0.460675, theta=0.015649 -[16:45:52.476] Init: px average disparity is:28.427391 ,While minimum is: 40.000000 KLT tracked : 130 -[16:45:52.476] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:52.476] frame process finished the id is: 101 the obs is:0 -[16:45:52.520] Odometry x=0.007524, y=0.466160, theta=0.022065 -[16:45:52.577] Init: px average disparity is:30.207459 ,While minimum is: 40.000000 KLT tracked : 130 -[16:45:52.577] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:52.577] frame process finished the id is: 102 the obs is:0 -[16:45:52.623] Odometry x=0.002719, y=0.479510, theta=0.016596 -[16:45:52.676] Init: px average disparity is:29.817531 ,While minimum is: 40.000000 KLT tracked : 130 -[16:45:52.676] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:52.676] frame process finished the id is: 103 the obs is:0 -[16:45:52.721] Odometry x=0.002727, y=0.484836, theta=0.030605 -[16:45:52.777] Init: px average disparity is:32.395762 ,While minimum is: 40.000000 KLT tracked : 130 -[16:45:52.777] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:52.777] frame process finished the id is: 104 the obs is:0 -[16:45:52.823] Odometry x=0.003193, y=0.492904, theta=0.009112 -[16:45:52.876] Init: px average disparity is:31.605598 ,While minimum is: 40.000000 KLT tracked : 130 -[16:45:52.876] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:52.876] frame process finished the id is: 105 the obs is:0 -[16:45:52.920] Odometry x=-0.000268, y=0.503399, theta=0.033336 -[16:45:52.976] Init: px average disparity is:35.354856 ,While minimum is: 40.000000 KLT tracked : 130 -[16:45:52.976] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:52.976] frame process finished the id is: 106 the obs is:0 -[16:45:53.017] Odometry x=0.000224, y=0.508822, theta=0.033395 -[16:45:53.077] Init: px average disparity is:32.520412 ,While minimum is: 40.000000 KLT tracked : 130 -[16:45:53.077] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:53.077] frame process finished the id is: 107 the obs is:0 -[16:45:53.118] Odometry x=-0.010101, y=0.518130, theta=0.035636 -[16:45:53.175] Init: px average disparity is:34.924574 ,While minimum is: 40.000000 KLT tracked : 130 -[16:45:53.175] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:53.175] frame process finished the id is: 108 the obs is:0 -[16:45:53.222] Odometry x=-0.010012, y=0.523485, theta=0.036230 -[16:45:53.277] Init: px average disparity is:33.919194 ,While minimum is: 40.000000 KLT tracked : 130 -[16:45:53.277] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:53.277] frame process finished the id is: 109 the obs is:0 -[16:45:53.319] Odometry x=-0.012451, y=0.529110, theta=0.046610 -[16:45:53.375] Init: px average disparity is:33.553183 ,While minimum is: 40.000000 KLT tracked : 130 -[16:45:53.375] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:53.375] frame process finished the id is: 110 the obs is:0 -[16:45:53.422] Odometry x=-0.012200, y=0.534383, theta=0.041027 -[16:45:53.476] Init: px average disparity is:32.310595 ,While minimum is: 40.000000 KLT tracked : 130 -[16:45:53.476] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:53.476] frame process finished the id is: 111 the obs is:0 -[16:45:53.519] Odometry x=-0.012097, y=0.542850, theta=0.047280 -[16:45:53.575] Init: px average disparity is:32.355729 ,While minimum is: 40.000000 KLT tracked : 130 -[16:45:53.575] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:53.575] frame process finished the id is: 112 the obs is:0 -[16:45:53.618] Odometry x=-0.011947, y=0.548705, theta=0.045742 -[16:45:53.680] Init: px average disparity is:32.060990 ,While minimum is: 40.000000 KLT tracked : 130 -[16:45:53.680] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:53.680] frame process finished the id is: 113 the obs is:0 -[16:45:53.755] Odometry x=-0.012201, y=0.554756, theta=0.042498 -[16:45:53.783] Init: px average disparity is:32.352324 ,While minimum is: 40.000000 KLT tracked : 130 -[16:45:53.783] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:53.783] frame process finished the id is: 114 the obs is:0 -[16:45:53.817] Odometry x=-0.012034, y=0.562229, theta=0.041628 -[16:45:53.883] Init: px average disparity is:32.920836 ,While minimum is: 40.000000 KLT tracked : 124 -[16:45:53.884] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:53.884] frame process finished the id is: 115 the obs is:0 -[16:45:53.920] Odometry x=-0.012240, y=0.553791, theta=0.036998 -[16:45:53.984] Init: px average disparity is:32.419031 ,While minimum is: 40.000000 KLT tracked : 124 -[16:45:53.984] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:53.985] frame process finished the id is: 116 the obs is:0 -[16:45:54.031] Odometry x=-0.011977, y=0.562505, theta=0.045472 -[16:45:54.092] Init: px average disparity is:33.577164 ,While minimum is: 40.000000 KLT tracked : 121 -[16:45:54.092] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:54.092] frame process finished the id is: 117 the obs is:0 -[16:45:54.119] Odometry x=-0.006480, y=0.567729, theta=0.028819 -[16:45:54.177] Init: px average disparity is:33.169091 ,While minimum is: 40.000000 KLT tracked : 121 -[16:45:54.177] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:54.177] frame process finished the id is: 118 the obs is:0 -[16:45:54.221] Odometry x=-0.006361, y=0.574150, theta=0.039226 -[16:45:54.278] Init: px average disparity is:34.499186 ,While minimum is: 40.000000 KLT tracked : 121 -[16:45:54.278] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:54.278] frame process finished the id is: 119 the obs is:0 -[16:45:54.318] Odometry x=-0.005990, y=0.579732, theta=0.021908 -[16:45:54.377] Init: px average disparity is:36.603601 ,While minimum is: 40.000000 KLT tracked : 120 -[16:45:54.377] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:54.377] frame process finished the id is: 120 the obs is:0 -[16:45:54.421] Odometry x=-0.005236, y=0.586917, theta=0.035847 -[16:45:54.477] Init: px average disparity is:37.342228 ,While minimum is: 40.000000 KLT tracked : 120 -[16:45:54.477] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 -[16:45:54.477] frame process finished the id is: 121 the obs is:0 -[16:45:54.518] Odometry x=-0.009499, y=0.589535, theta=0.040664 -[16:45:54.620] Odometry x=-0.009024, y=0.596226, theta=0.029615 -[16:45:54.642] cur: x=-0.014157, y=-0.488537, z=0.872429 ref: x=-0.045826 y=-0.352076 z=0.934849 -[16:45:54.642] cur: x=-0.012972, y=-0.492571, z=0.870176 ref: x=-0.045820 y=-0.354195 z=0.934048 -[16:45:54.642] cur: x=-0.041314, y=-0.458899, z=0.887527 ref: x=-0.083259 y=-0.320054 z=0.943734 -[16:45:54.642] cur: x=0.017462, y=-0.445923, z=0.894901 ref: x=-0.022916 y=-0.313819 z=0.949206 -[16:45:54.642] cur: x=-0.184407, y=-0.303458, z=0.934830 ref: x=-0.212743 y=-0.237814 z=0.947726 -[16:45:54.642] cur: x=-0.181215, y=-0.303421, z=0.935466 ref: x=-0.209888 y=-0.237838 z=0.948357 -[16:45:54.642] cur: x=-0.178564, y=-0.301462, z=0.936608 ref: x=-0.207049 y=-0.235711 z=0.949511 -[16:45:54.642] cur: x=-0.170898, y=-0.293478, z=0.940566 ref: x=-0.198538 y=-0.227168 z=0.953403 -[16:45:54.642] cur: x=-0.167521, y=-0.293047, z=0.941308 ref: x=-0.195677 y=-0.227188 z=0.953990 -[16:45:54.642] cur: x=-0.170911, y=-0.295644, z=0.939885 ref: x=-0.198522 y=-0.229321 z=0.952891 -[16:45:54.642] cur: x=-0.170873, y=-0.297786, z=0.939216 ref: x=-0.198506 y=-0.231473 z=0.952374 -[16:45:54.642] cur: x=-0.176059, y=-0.299558, z=0.937693 ref: x=-0.204208 y=-0.233582 z=0.950652 -[16:45:54.642] cur: x=-0.161206, y=-0.285476, z=0.944731 ref: x=-0.178575 y=-0.214366 z=0.960291 -[16:45:54.642] cur: x=-0.158400, y=-0.285613, z=0.945164 ref: x=-0.175707 y=-0.214383 z=0.960816 -[16:45:54.642] cur: x=-0.164575, y=-0.290394, z=0.942648 ref: x=-0.192830 y=-0.225054 z=0.955074 -[16:45:54.642] cur: x=-0.163992, y=-0.289635, z=0.942984 ref: x=-0.181415 y=-0.218663 z=0.958788 -[16:45:54.642] cur: x=-0.360246, y=0.116696, z=0.925530 ref: x=-0.289722 y=-0.213448 z=0.933006 -[16:45:54.642] cur: x=-0.135448, y=-0.273564, z=0.952269 ref: x=-0.152805 y=-0.201543 z=0.967487 -[16:45:54.642] cur: x=-0.355808, y=-0.268696, z=0.895100 ref: x=-0.346836 y=-0.135421 z=0.928098 -[16:45:54.642] cur: x=-0.156986, y=-0.108652, z=0.981606 ref: x=-0.176271 y=-0.062410 z=0.982361 -[16:45:54.642] cur: x=-0.154343, y=-0.108680, z=0.982022 ref: x=-0.173393 y=-0.062415 z=0.982873 -[16:45:54.642] cur: x=-0.142794, y=-0.100788, z=0.984607 ref: x=-0.161884 y=-0.053710 z=0.985347 -[16:45:54.642] cur: x=-0.393080, y=-0.000138, z=0.919504 ref: x=-0.358532 y=-0.053273 z=0.931996 -[16:45:54.642] cur: x=-0.389048, y=-0.003311, z=0.921212 ref: x=-0.355725 y=-0.053282 z=0.933071 -[16:45:54.642] cur: x=-0.142983, y=-0.103001, z=0.984351 ref: x=-0.161882 y=-0.055891 z=0.985226 -[16:45:54.642] cur: x=-0.340501, y=0.012862, z=0.940156 ref: x=-0.369771 y=-0.044588 z=0.928053 -[16:45:54.642] cur: x=-0.335429, y=0.014856, z=0.941949 ref: x=-0.366970 y=-0.044596 z=0.929163 -[16:45:54.642] cur: x=-0.332421, y=0.014994, z=0.943012 ref: x=-0.361364 y=-0.044612 z=0.931357 -[16:45:54.642] cur: x=-0.329798, y=0.018159, z=0.943877 ref: x=-0.358564 y=-0.042456 z=0.932539 -[16:45:54.642] cur: x=-0.324079, y=0.021456, z=0.945787 ref: x=-0.355762 y=-0.040299 z=0.933707 -[16:45:54.642] cur: x=-0.150548, y=-0.085945, z=0.984860 ref: x=-0.170544 y=-0.038433 z=0.984600 -[16:45:54.642] cur: x=-0.150635, y=-0.083780, z=0.985033 ref: x=-0.170546 y=-0.036252 z=0.984683 -[16:45:54.642] cur: x=-0.153579, y=-0.081648, z=0.984757 ref: x=-0.173427 y=-0.034068 z=0.984257 -[16:45:54.642] cur: x=-0.346435, y=0.055093, z=0.936455 ref: x=-0.375420 y=-0.018631 z=0.926667 -[16:45:54.643] cur: x=-0.323750, y=-0.078753, z=0.942859 ref: x=-0.367026 y=-0.014315 z=0.930100 -[16:45:54.643] cur: x=-0.323171, y=-0.066673, z=0.943989 ref: x=-0.367032 y=0.002992 z=0.930203 -[16:45:54.643] cur: x=-0.336062, y=-0.047783, z=0.940627 ref: x=-0.378226 y=0.007314 z=0.925685 -[16:45:54.643] cur: x=-0.264220, y=-0.049280, z=0.963203 ref: x=-0.262329 y=0.018235 z=0.964806 -[16:45:54.643] cur: x=-0.299617, y=-0.001803, z=0.954058 ref: x=-0.313400 y=0.063744 z=0.947479 -[16:45:54.643] cur: x=-0.328730, y=0.049591, z=0.943121 ref: x=-0.369687 y=0.067856 z=0.926675 -[16:45:54.643] cur: x=-0.340200, y=0.090678, z=0.935971 ref: x=-0.383618 y=0.076425 z=0.920324 -[16:45:54.643] cur: x=-0.337075, y=0.086608, z=0.937486 ref: x=-0.380828 y=0.076439 z=0.921481 -[16:45:54.643] cur: x=-0.329530, y=0.087415, z=0.940090 ref: x=-0.372445 y=0.076482 z=0.924897 -[16:45:54.643] cur: x=-0.342948, y=0.034884, z=0.938707 ref: x=-0.383670 y=0.065634 z=0.921135 -[16:45:54.643] cur: x=-0.151222, y=0.023626, z=0.988217 ref: x=-0.170488 y=0.077166 z=0.982334 -[16:45:54.643] cur: x=-0.148237, y=0.023430, z=0.988674 ref: x=-0.167609 y=0.077171 z=0.982829 -[16:45:54.643] cur: x=-0.214569, y=0.028343, z=0.976298 ref: x=-0.259356 y=0.079114 z=0.962536 -[16:45:54.643] cur: x=-0.211662, y=0.028143, z=0.976938 ref: x=-0.256503 y=0.079123 z=0.963300 -[16:45:54.643] cur: x=-0.460806, y=-0.417953, z=0.782926 ref: x=-0.490642 y=0.092823 z=0.866403 -[16:45:54.643] cur: x=-0.326475, y=0.089219, z=0.940986 ref: x=-0.377719 y=0.123887 z=0.917595 -[16:45:54.643] cur: x=-0.316985, y=0.090855, z=0.944069 ref: x=-0.372153 y=0.121780 z=0.920148 -[16:45:54.643] cur: x=-0.008243, y=0.141202, z=0.989946 ref: x=-0.051982 y=0.149244 z=0.987433 -[16:45:54.643] cur: x=-0.025888, y=0.142652, z=0.989434 ref: x=-0.069329 y=0.151396 z=0.986039 -[16:45:54.643] cur: x=-0.025887, y=0.144322, z=0.989192 ref: x=-0.069325 y=0.153571 z=0.985703 -[16:45:54.643] cur: x=-0.014037, y=0.147534, z=0.988957 ref: x=-0.057755 y=0.157936 z=0.985759 -[16:45:54.643] cur: x=-0.022320, y=0.165449, z=0.985966 ref: x=-0.066400 y=0.175303 z=0.982273 -[16:45:54.643] cur: x=-0.028378, y=0.170450, z=0.984958 ref: x=-0.072170 y=0.179634 z=0.981083 -[16:45:54.643] cur: x=-0.007640, y=0.168245, z=0.985716 ref: x=-0.051947 y=0.177496 z=0.982750 -[16:45:54.643] cur: x=-0.022241, y=0.168008, z=0.985535 ref: x=-0.066396 y=0.177474 z=0.981883 -[16:45:54.643] cur: x=0.075241, y=0.177102, z=0.981312 ref: x=0.005883 y=0.188383 z=0.982078 -[16:45:54.643] cur: x=0.075198, y=0.174950, z=0.981701 ref: x=0.005883 y=0.186213 z=0.982492 -[16:45:54.643] cur: x=0.066543, y=0.179631, z=0.981481 ref: x=-0.002791 y=0.190552 z=0.981673 -[16:45:54.643] cur: x=0.069475, y=0.181648, z=0.980906 ref: x=0.000100 y=0.192721 z=0.981254 -[16:45:54.643] cur: x=0.005195, y=0.199907, z=0.979801 ref: x=-0.040349 y=0.207866 z=0.977325 -[16:45:54.643] cur: x=0.014117, y=0.199384, z=0.979820 ref: x=-0.037460 y=0.207869 z=0.977439 -[16:45:54.643] cur: x=-0.030925, y=0.184185, z=0.982405 ref: x=-0.075031 y=0.192641 z=0.978397 -[16:45:54.643] cur: x=-0.567246, y=0.288683, z=0.771294 ref: x=-0.514089 y=0.261491 z=0.816906 -[16:45:54.643] cur: x=0.297148, y=0.261298, z=0.918382 ref: x=0.158330 y=0.272080 z=0.949160 -[16:45:54.643] cur: x=0.300132, y=0.261547, z=0.917341 ref: x=0.161195 y=0.272059 z=0.948683 -[16:45:54.643] cur: x=0.165110, y=0.344702, z=0.924077 ref: x=0.074811 y=0.328112 z=0.941672 -[16:45:54.643] cur: x=0.168273, y=0.344923, z=0.923424 ref: x=0.077682 y=0.328100 z=0.941444 -[16:45:54.643] cur: x=0.167219, y=0.346458, z=0.923041 ref: x=0.077673 y=0.330228 z=0.940700 -[16:45:54.643] cur: x=-0.166632, y=0.353437, z=0.920498 ref: x=-0.256887 y=0.343274 z=0.903423 -[16:45:54.643] cur: x=0.206888, y=0.385552, z=0.899192 ref: x=0.157216 y=0.392984 z=0.906006 -[16:45:54.643] cur: x=0.209797, y=0.387699, z=0.897594 ref: x=0.160034 y=0.395045 z=0.904615 -[16:45:54.643] cur: x=-0.572958, y=-0.353968, z=0.739206 ref: x=-0.484343 y=-0.279612 z=0.828993 -[16:45:54.643] cur: x=-0.190706, y=-0.303507, z=0.933550 ref: x=-0.218452 y=-0.237766 z=0.946439 -[16:45:54.643] cur: x=-0.184407, y=-0.303458, z=0.934830 ref: x=-0.212743 y=-0.237814 z=0.947726 -[16:45:54.643] cur: x=-0.178665, y=-0.299354, z=0.937265 ref: x=-0.207066 y=-0.233560 z=0.950039 -[16:45:54.643] cur: x=-0.149730, y=-0.279588, z=0.948373 ref: x=-0.167136 y=-0.207954 z=0.963753 -[16:45:54.643] cur: x=-0.147828, y=0.020119, z=0.988808 ref: x=-0.167613 y=0.074992 z=0.982997 -[16:45:54.643] cur: x=-0.385946, y=-0.137696, z=0.912187 ref: x=-0.282264 y=0.026912 z=0.958959 -[16:45:54.643] cur: x=-0.516272, y=-0.218692, z=0.828032 ref: x=-0.471626 y=0.099419 z=0.876176 -[16:45:54.643] cur: x=-0.513861, y=-0.222363, z=0.828554 ref: x=-0.471585 y=0.103697 z=0.875702 -[16:45:54.643] cur: x=-0.654146, y=0.201013, z=0.729169 ref: x=-0.570122 y=0.220076 z=0.791535 -[16:45:54.643] cur: x=-0.663411, y=0.251033, z=0.704889 ref: x=-0.548490 y=0.249926 z=0.797932 -[16:45:54.643] cur: x=0.194499, y=0.267793, z=0.943640 ref: x=0.086521 y=0.274611 z=0.957655 -[16:45:54.643] cur: x=-0.627678, y=0.268555, z=0.730684 ref: x=-0.532699 y=0.254627 z=0.807092 -[16:45:54.643] cur: x=-0.022935, y=0.273829, z=0.961505 ref: x=-0.069064 y=0.270374 z=0.960275 -[16:45:54.643] cur: x=-0.022654, y=0.269930, z=0.962613 ref: x=-0.069077 y=0.266075 z=0.961474 -[16:45:54.643] cur: x=0.300207, y=0.255354, z=0.919059 ref: x=0.161240 y=0.265625 z=0.950497 -[16:45:54.643] cur: x=-0.036344, y=0.314156, z=0.948675 ref: x=-0.080452 y=0.304625 z=0.949068 -[16:45:54.643] cur: x=0.300149, y=0.259495, z=0.917918 ref: x=0.161210 y=0.269915 z=0.949293 -[16:45:54.643] cur: x=-0.048037, y=0.311070, z=0.949172 ref: x=-0.091965 y=0.300301 z=0.949401 -[16:45:54.643] cur: x=0.166055, y=0.343028, z=0.924531 ref: x=0.074820 y=0.325983 z=0.942410 -[16:45:54.643] cur: x=-0.649999, y=0.313992, z=0.692034 ref: x=-0.566466 y=0.315027 z=0.761495 -[16:45:54.643] cur: x=-0.162690, y=0.354829, z=0.920667 ref: x=-0.251209 y=0.345473 z=0.904181 -[16:45:54.643] cur: x=-0.654331, y=0.310136, z=0.689686 ref: x=-0.571748 y=0.310689 z=0.759326 -[16:45:54.643] cur: x=-0.185739, y=0.374196, z=0.908558 ref: x=-0.267739 y=0.366186 z=0.891192 -[16:45:54.643] cur: x=-0.300886, y=0.407859, z=0.862043 ref: x=-0.383200 y=0.400471 z=0.832335 -[16:45:54.643] cur: x=-0.185140, y=-0.290873, z=0.938678 ref: x=-0.212847 y=-0.224907 z=0.950849 -[16:45:54.643] cur: x=-0.271310, y=-0.309961, z=0.911216 ref: x=-0.258255 y=-0.241678 z=0.935359 -[16:45:54.643] cur: x=-0.161325, y=-0.279487, z=0.946500 ref: x=-0.178614 y=-0.207890 z=0.961706 -[16:45:54.643] cur: x=-0.357048, y=-0.345694, z=0.867763 ref: x=-0.360759 y=-0.146068 z=0.921150 -[16:45:54.643] cur: x=-0.376678, y=-0.236378, z=0.895678 ref: x=-0.360677 y=-0.154673 z=0.919776 -[16:45:54.643] cur: x=-0.142546, y=-0.144329, z=0.979209 ref: x=-0.202114 y=-0.077607 z=0.976282 -[16:45:54.643] cur: x=-0.167972, y=-0.122767, z=0.978117 ref: x=-0.190623 y=-0.077633 z=0.978589 -[16:45:54.643] cur: x=-0.384276, y=-0.148128, z=0.911258 ref: x=-0.282273 y=0.018219 z=0.959161 -[16:45:54.643] cur: x=-0.460930, y=-0.407411, z=0.788391 ref: x=-0.493234 y=0.103470 z=0.863721 -[16:45:54.643] cur: x=-0.588876, y=-0.411972, z=0.695345 ref: x=-0.504080 y=0.094823 z=0.858436 -[16:45:54.643] cur: x=-0.161457, y=0.037737, z=0.986158 ref: x=-0.179098 y=0.088039 z=0.979884 -[16:45:54.643] cur: x=-0.342118, y=0.077249, z=0.936476 ref: x=-0.394516 y=0.112984 z=0.911916 -[16:45:54.643] cur: x=-0.332003, y=0.077057, z=0.940126 ref: x=-0.383383 y=0.113074 z=0.916642 -[16:45:54.643] cur: x=0.034896, y=0.165528, z=0.985588 ref: x=-0.028820 y=0.175349 z=0.984084 -[16:45:54.643] cur: x=-0.148298, y=0.155111, z=0.976703 ref: x=-0.178381 y=0.244024 z=0.953222 -[16:45:54.643] cur: x=0.294538, y=0.241584, z=0.924600 ref: x=0.155592 y=0.252776 z=0.954932 -[16:45:54.643] cur: x=-0.143701, y=0.174667, z=0.974085 ref: x=-0.178319 y=0.252622 z=0.950991 -[16:45:54.643] cur: x=-0.494465, y=0.316260, z=0.809620 ref: x=-0.642545 y=0.282517 z=0.712264 -[16:45:54.643] cur: x=0.169705, y=0.416954, z=0.892944 ref: x=0.119921 y=0.422593 z=0.898351 -[16:45:54.643] Init finished: Homography RANSAC (inlier) is:20 ,While 20 inliers minimum required. px average disparity is:40.199410 ,While minimum is: 40.000000 KLT tracked: 119 -[16:45:54.643] Init: distance between the first and current frame is x:-0.009024 ,z=0.596226,angle between two frames: 0.029615 -[16:45:54.643] Init: Selected Second frame. The number of features: 20 depth mean:1.647713 min:1.209012 -[16:45:54.643] frame process finished the id is: 122 the obs is:20 -[16:45:54.722] Odometry x=-0.009392, y=0.602583, theta=0.053936 -[16:45:54.819] Odometry x=-0.009156, y=0.608335, theta=0.042429 -[16:45:54.821] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:4 ,distance between ekf and vo x:0.001113 ,z=0.000562,angle between two frames:0.000687 -[16:45:54.821] n deleted obs = 0 n obs with reprojection error less than 0.5 px =4 error init =0.234306 error end=0.205214 -[16:45:54.821] After pose optimization, distance between ekf and vo x:0.226139 ,z=-0.017161,angle between two frames:0.054862 -[16:45:54.821] frame process finished the id is: 122 the obs is:20 -[16:45:54.921] Odometry x=-0.008334, y=0.613285, theta=0.053165 -[16:45:54.982] After Reprojection Map nPoint:13 ,nCell:13 ,nMatches:0 ,distance between ekf and vo x:0.002718 ,z=0.001638,angle between two frames:0.001690 -[16:45:54.982] After pose optimization, distance between ekf and vo x:0.002718 ,z=0.001638,angle between two frames:0.001690 -[16:45:54.982] frame process finished the id is: 122 the obs is:20 -[16:45:55.018] Odometry x=-0.007925, y=0.618694, theta=0.045289 -[16:45:55.120] Odometry x=-0.008098, y=0.624288, theta=0.049519 -[16:45:55.132] After Reprojection Map nPoint:13 ,nCell:13 ,nMatches:0 ,distance between ekf and vo x:0.000602 ,z=0.000394,angle between two frames:0.000504 -[16:45:55.132] After pose optimization, distance between ekf and vo x:0.000602 ,z=0.000394,angle between two frames:0.000504 -[16:45:55.132] frame process finished the id is: 122 the obs is:20 -[16:45:55.217] Odometry x=-0.007838, y=0.630806, theta=0.054426 -[16:45:55.288] After Reprojection Map nPoint:14 ,nCell:13 ,nMatches:3 ,distance between ekf and vo x:0.001743 ,z=0.001161,angle between two frames:0.001357 -[16:45:55.288] n deleted obs = 0 n obs with reprojection error less than 0.5 px =3 error init =0.259780 error end=0.190755 -[16:45:55.288] After pose optimization, distance between ekf and vo x:-0.201661 ,z=0.054233,angle between two frames:0.056984 -[16:45:55.288] frame process finished the id is: 122 the obs is:20 -[16:45:55.322] Odometry x=-0.007161, y=0.636305, theta=0.037314 -[16:45:55.419] Odometry x=-0.006583, y=0.642274, theta=0.062325 -[16:45:55.452] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:0.004609 ,z=0.002099,angle between two frames:0.002659 -[16:45:55.452] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.135012 error end=0.134906 -[16:45:55.452] After pose optimization, distance between ekf and vo x:0.004833 ,z=0.003224,angle between two frames:0.002808 -[16:45:55.452] frame process finished the id is: 122 the obs is:20 -[16:45:55.516] Odometry x=-0.006272, y=0.647539, theta=0.044518 -[16:45:55.614] Odometry x=-0.006283, y=0.651272, theta=0.063549 -[16:45:55.615] After Reprojection Map nPoint:15 ,nCell:12 ,nMatches:3 ,distance between ekf and vo x:0.003256 ,z=0.003094,angle between two frames:0.002446 -[16:45:55.615] n deleted obs = 1 n obs with reprojection error less than 0.5 px =2 error init =0.353293 error end=0.352760 -[16:45:55.615] After pose optimization, distance between ekf and vo x:-0.004736 ,z=0.025963,angle between two frames:0.012402 -[16:45:55.615] frame process finished the id is: 122 the obs is:20 -[16:45:55.716] Odometry x=-0.006104, y=0.656622, theta=0.057638 -[16:45:55.765] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:1 ,distance between ekf and vo x:0.001550 ,z=0.001790,angle between two frames:0.001196 -[16:45:55.765] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.109225 error end=0.109225 -[16:45:55.765] After pose optimization, distance between ekf and vo x:0.001550 ,z=0.001790,angle between two frames:0.001196 -[16:45:55.765] frame process finished the id is: 122 the obs is:20 -[16:45:55.824] Odometry x=-0.005985, y=0.660605, theta=0.059205 -[16:45:55.908] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:0.000291 ,z=0.000301,angle between two frames:0.000219 -[16:45:55.908] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.107839 error end=0.107681 -[16:45:55.908] After pose optimization, distance between ekf and vo x:0.002817 ,z=-0.000961,angle between two frames:0.000268 -[16:45:55.908] frame process finished the id is: 122 the obs is:20 -[16:45:55.920] Odometry x=-0.005126, y=0.666795, theta=0.058622 -[16:45:56.022] Odometry x=-0.007248, y=0.672130, theta=0.053160 -[16:45:56.059] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:0.001848 ,z=0.001451,angle between two frames:0.001175 -[16:45:56.059] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.342611 error end=0.277133 -[16:45:56.059] After pose optimization, distance between ekf and vo x:-0.222287 ,z=0.019109,angle between two frames:0.079604 -[16:45:56.059] frame process finished the id is: 122 the obs is:20 -[16:45:56.119] Odometry x=-0.007095, y=0.677303, theta=0.051198 -[16:45:56.203] After Reprojection Map nPoint:15 ,nCell:14 ,nMatches:1 ,distance between ekf and vo x:0.000411 ,z=0.000382,angle between two frames:0.000301 -[16:45:56.203] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.065625 error end=0.065076 -[16:45:56.203] After pose optimization, distance between ekf and vo x:-0.000858 ,z=0.003380,angle between two frames:0.000031 -[16:45:56.203] frame process finished the id is: 122 the obs is:20 -[16:45:56.218] Odometry x=-0.014847, y=0.688815, theta=0.057150 -[16:45:56.318] Odometry x=-0.014488, y=0.694004, theta=0.051693 -[16:45:56.368] After Reprojection Map nPoint:17 ,nCell:16 ,nMatches:3 ,distance between ekf and vo x:0.001109 ,z=0.001099,angle between two frames:0.000885 -[16:45:56.368] n deleted obs = 0 n obs with reprojection error less than 0.5 px =3 error init =0.233487 error end=0.218410 -[16:45:56.368] After pose optimization, distance between ekf and vo x:-0.120523 ,z=0.003531,angle between two frames:0.049866 -[16:45:56.368] frame process finished the id is: 122 the obs is:20 -[16:45:56.418] Odometry x=-0.014082, y=0.707923, theta=0.058630 -[16:45:56.518] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:1 ,distance between ekf and vo x:0.001890 ,z=0.001554,angle between two frames:0.001351 -[16:45:56.518] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.062500 error end=0.062500 -[16:45:56.518] After pose optimization, distance between ekf and vo x:0.001890 ,z=0.001554,angle between two frames:0.001351 -[16:45:56.518] frame process finished the id is: 122 the obs is:20 -[16:45:56.520] Odometry x=-0.013302, y=0.713324, theta=0.057850 -[16:45:56.617] Odometry x=-0.012713, y=0.719028, theta=0.067700 -[16:45:56.667] After Reprojection Map nPoint:19 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:0.000920 ,z=0.000666,angle between two frames:0.000653 -[16:45:56.667] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.169070 error end=0.169070 -[16:45:56.667] After pose optimization, distance between ekf and vo x:0.000920 ,z=0.000666,angle between two frames:0.000653 -[16:45:56.667] frame process finished the id is: 122 the obs is:20 -[16:45:56.720] Odometry x=-0.011525, y=0.724265, theta=0.057063 -[16:45:56.816] After Reprojection Map nPoint:15 ,nCell:12 ,nMatches:3 ,distance between ekf and vo x:0.000953 ,z=0.000917,angle between two frames:0.000807 -[16:45:56.816] n deleted obs = 1 n obs with reprojection error less than 0.5 px =2 error init =0.254894 error end=0.245641 -[16:45:56.816] After pose optimization, distance between ekf and vo x:-0.035971 ,z=0.115396,angle between two frames:0.025593 -[16:45:56.816] frame process finished the id is: 122 the obs is:20 -[16:45:56.822] Odometry x=-0.011368, y=0.729690, theta=0.059477 -[16:45:56.920] Odometry x=-0.011058, y=0.735560, theta=0.070373 -[16:45:56.965] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.000858 ,z=0.000583,angle between two frames:0.000617 -[16:45:56.965] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.144447 error end=0.131688 -[16:45:56.965] After pose optimization, distance between ekf and vo x:0.104578 ,z=0.017025,angle between two frames:0.036394 -[16:45:56.965] frame process finished the id is: 122 the obs is:20 -[16:45:57.017] Odometry x=-0.010873, y=0.741204, theta=0.058646 -[16:45:57.109] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:2 ,distance between ekf and vo x:0.000954 ,z=0.000497,angle between two frames:0.000528 -[16:45:57.109] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.244610 error end=0.159280 -[16:45:57.109] After pose optimization, distance between ekf and vo x:0.260887 ,z=0.028825,angle between two frames:0.106947 -[16:45:57.109] frame process finished the id is: 122 the obs is:20 -[16:45:57.115] Odometry x=-0.010520, y=0.746520, theta=0.079174 -[16:45:57.217] Odometry x=-0.010318, y=0.751837, theta=0.068451 -[16:45:57.272] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.002486 ,z=0.001945,angle between two frames:0.001708 -[16:45:57.272] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.201352 error end=0.200460 -[16:45:57.272] After pose optimization, distance between ekf and vo x:0.024456 ,z=0.010009,angle between two frames:0.015682 -[16:45:57.272] frame process finished the id is: 122 the obs is:20 -[16:45:57.319] Odometry x=-0.009353, y=0.757126, theta=0.079005 -[16:45:57.421] Odometry x=-0.009145, y=0.762535, theta=0.069382 -[16:45:57.423] After Reprojection Map nPoint:16 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:0.000712 ,z=0.001030,angle between two frames:0.000860 -[16:45:57.423] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =1.107770 error end=1.107770 -[16:45:57.423] After pose optimization, distance between ekf and vo x:0.000712 ,z=0.001030,angle between two frames:0.000860 -[16:45:57.423] frame process finished the id is: 122 the obs is:20 -[16:45:57.519] Odometry x=-0.008954, y=0.768011, theta=0.077982 -[16:45:57.572] After Reprojection Map nPoint:16 ,nCell:15 ,nMatches:3 ,distance between ekf and vo x:0.000428 ,z=0.000455,angle between two frames:0.000469 -[16:45:57.572] n deleted obs = 0 n obs with reprojection error less than 0.5 px =3 error init =0.181879 error end=0.176468 -[16:45:57.572] After pose optimization, distance between ekf and vo x:-0.130075 ,z=0.018131,angle between two frames:0.048198 -[16:45:57.572] frame process finished the id is: 122 the obs is:20 -[16:45:57.621] Odometry x=-0.008735, y=0.773411, theta=0.079117 -[16:45:57.718] Odometry x=-0.008507, y=0.779010, theta=0.067594 -[16:45:57.726] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:3 ,distance between ekf and vo x:0.001180 ,z=0.000800,angle between two frames:0.000805 -[16:45:57.726] n deleted obs = 1 n obs with reprojection error less than 0.5 px =2 error init =0.309976 error end=0.309976 -[16:45:57.726] After pose optimization, distance between ekf and vo x:0.001180 ,z=0.000800,angle between two frames:0.000805 -[16:45:57.726] frame process finished the id is: 122 the obs is:20 -[16:45:57.820] Odometry x=-0.008153, y=0.784796, theta=0.073256 -[16:45:57.888] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:0.002496 ,z=0.001929,angle between two frames:0.001748 -[16:45:57.888] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.055908 error end=0.055763 -[16:45:57.888] After pose optimization, distance between ekf and vo x:0.002662 ,z=0.003535,angle between two frames:0.001723 -[16:45:57.888] frame process finished the id is: 122 the obs is:20 -[16:45:57.919] Odometry x=-0.008783, y=0.791090, theta=0.062200 -[16:45:58.016] Odometry x=-0.008604, y=0.796513, theta=0.072234 -[16:45:58.036] After Reprojection Map nPoint:13 ,nCell:13 ,nMatches:0 ,distance between ekf and vo x:0.001474 ,z=0.002031,angle between two frames:0.001425 -[16:45:58.036] After pose optimization, distance between ekf and vo x:0.001474 ,z=0.002031,angle between two frames:0.001425 -[16:45:58.036] frame process finished the id is: 122 the obs is:20 -[16:45:58.116] Odometry x=-0.008382, y=0.801641, theta=0.063647 -[16:45:58.178] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:0 ,distance between ekf and vo x:0.000139 ,z=0.000171,angle between two frames:0.000132 -[16:45:58.178] After pose optimization, distance between ekf and vo x:0.000139 ,z=0.000171,angle between two frames:0.000132 -[16:45:58.178] frame process finished the id is: 122 the obs is:20 -[16:45:58.224] Odometry x=-0.008201, y=0.807258, theta=0.070492 -[16:45:58.317] Odometry x=-0.008106, y=0.814616, theta=0.067009 -[16:45:58.349] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:0 ,distance between ekf and vo x:0.001998 ,z=0.002030,angle between two frames:0.001578 -[16:45:58.349] After pose optimization, distance between ekf and vo x:0.001998 ,z=0.002030,angle between two frames:0.001578 -[16:45:58.349] frame process finished the id is: 122 the obs is:20 -[16:45:58.424] Odometry x=-0.007920, y=0.819965, theta=0.072251 -[16:45:58.506] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:0 ,distance between ekf and vo x:0.000302 ,z=0.000470,angle between two frames:0.000363 -[16:45:58.506] After pose optimization, distance between ekf and vo x:0.000302 ,z=0.000470,angle between two frames:0.000363 -[16:45:58.506] frame process finished the id is: 122 the obs is:20 -[16:45:58.531] Odometry x=-0.007667, y=0.827098, theta=0.064309 -[16:45:58.617] Odometry x=-0.007466, y=0.833035, theta=0.073086 -[16:45:58.654] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:0.000965 ,z=0.001026,angle between two frames:0.000909 -[16:45:58.654] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.160177 error end=0.153030 -[16:45:58.654] After pose optimization, distance between ekf and vo x:0.068348 ,z=0.001457,angle between two frames:0.018202 -[16:45:58.654] frame process finished the id is: 122 the obs is:20 -[16:45:58.730] Odometry x=-0.007292, y=0.837630, theta=0.071305 -[16:45:58.803] After Reprojection Map nPoint:17 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.000688 ,z=0.000895,angle between two frames:0.000701 -[16:45:58.804] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.095338 error end=0.092224 -[16:45:58.804] After pose optimization, distance between ekf and vo x:0.057615 ,z=0.002330,angle between two frames:0.039468 -[16:45:58.804] frame process finished the id is: 122 the obs is:20 -[16:45:58.823] Odometry x=-0.007078, y=0.843445, theta=0.074487 -[16:45:58.922] Odometry x=-0.006862, y=0.848756, theta=0.070359 -[16:45:58.952] After Reprojection Map nPoint:15 ,nCell:14 ,nMatches:2 ,distance between ekf and vo x:0.000563 ,z=0.000512,angle between two frames:0.000454 -[16:45:58.952] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.469263 error end=0.431768 -[16:45:58.952] After pose optimization, distance between ekf and vo x:-0.148271 ,z=0.473949,angle between two frames:0.193216 -[16:45:58.952] frame process finished the id is: 122 the obs is:20 -[16:45:59.020] Odometry x=-0.006673, y=0.853948, theta=0.073514 -[16:45:59.105] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:0.001130 ,z=0.001293,angle between two frames:0.000929 -[16:45:59.105] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.090880 error end=0.090244 -[16:45:59.105] After pose optimization, distance between ekf and vo x:-0.000214 ,z=0.012813,angle between two frames:0.002719 -[16:45:59.105] frame process finished the id is: 122 the obs is:20 -[16:45:59.119] Odometry x=-0.006477, y=0.859328, theta=0.078924 -[16:45:59.216] Odometry x=-0.006172, y=0.865868, theta=0.074790 -[16:45:59.247] After Reprojection Map nPoint:14 ,nCell:13 ,nMatches:1 ,distance between ekf and vo x:0.000126 ,z=0.000163,angle between two frames:0.000123 -[16:45:59.247] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.060313 error end=0.060313 -[16:45:59.247] After pose optimization, distance between ekf and vo x:0.000126 ,z=0.000163,angle between two frames:0.000123 -[16:45:59.247] frame process finished the id is: 122 the obs is:20 -[16:45:59.319] Odometry x=-0.005980, y=0.870873, theta=0.076499 -[16:45:59.397] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:0.000927 ,z=0.000762,angle between two frames:0.000650 -[16:45:59.397] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.291202 error end=0.291117 -[16:45:59.397] After pose optimization, distance between ekf and vo x:-0.001104 ,z=0.002103,angle between two frames:0.001742 -[16:45:59.397] frame process finished the id is: 122 the obs is:20 -[16:45:59.418] Odometry x=-0.005710, y=0.877907, theta=0.068858 -[16:45:59.521] Odometry x=-0.005507, y=0.883592, theta=0.075031 -[16:45:59.541] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:0.000348 ,z=0.000553,angle between two frames:0.000422 -[16:45:59.541] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.413990 error end=0.300830 -[16:45:59.541] After pose optimization, distance between ekf and vo x:-0.823437 ,z=0.059255,angle between two frames:0.331080 -[16:45:59.541] frame process finished the id is: 122 the obs is:20 -[16:45:59.621] Odometry x=-0.005237, y=0.888842, theta=0.075872 -[16:45:59.686] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.000585 ,z=0.000704,angle between two frames:0.000523 -[16:45:59.687] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.216706 error end=0.203437 -[16:45:59.687] After pose optimization, distance between ekf and vo x:0.141695 ,z=-0.016719,angle between two frames:0.028732 -[16:45:59.687] frame process finished the id is: 122 the obs is:20 -[16:45:59.719] Odometry x=-0.005023, y=0.894648, theta=0.062315 -[16:45:59.820] Odometry x=-0.004836, y=0.900215, theta=0.065244 -[16:45:59.841] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:0 ,distance between ekf and vo x:0.001776 ,z=0.001358,angle between two frames:0.001166 -[16:45:59.841] After pose optimization, distance between ekf and vo x:0.001776 ,z=0.001358,angle between two frames:0.001166 -[16:45:59.841] frame process finished the id is: 122 the obs is:20 -[16:45:59.919] Odometry x=-0.004600, y=0.905756, theta=0.070963 -[16:45:59.989] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:0 ,distance between ekf and vo x:0.001241 ,z=0.001276,angle between two frames:0.000977 -[16:45:59.989] After pose optimization, distance between ekf and vo x:0.001241 ,z=0.001276,angle between two frames:0.000977 -[16:45:59.989] frame process finished the id is: 122 the obs is:20 -[16:46:00.019] Odometry x=-0.004407, y=0.911392, theta=0.064411 -[16:46:00.118] Odometry x=-0.004259, y=0.916808, theta=0.064230 -[16:46:00.131] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:1 ,distance between ekf and vo x:0.000276 ,z=0.000361,angle between two frames:0.000276 -[16:46:00.131] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =1.225134 error end=1.225134 -[16:46:00.131] After pose optimization, distance between ekf and vo x:0.000276 ,z=0.000361,angle between two frames:0.000276 -[16:46:00.131] frame process finished the id is: 122 the obs is:20 -[16:46:00.218] Odometry x=-0.004072, y=0.922679, theta=0.065458 -[16:46:00.281] After Reprojection Map nPoint:16 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:0.000779 ,z=0.000732,angle between two frames:0.000645 -[16:46:00.281] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.013679 error end=0.013665 -[16:46:00.281] After pose optimization, distance between ekf and vo x:0.000721 ,z=0.000752,angle between two frames:0.000589 -[16:46:00.281] frame process finished the id is: 122 the obs is:20 -[16:46:00.318] Odometry x=-0.003869, y=0.928284, theta=0.068051 -[16:46:00.421] Odometry x=-0.003637, y=0.934960, theta=0.069980 -[16:46:00.423] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:0.000133 ,z=0.000151,angle between two frames:0.000129 -[16:46:00.423] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =0.553204 error end=0.553204 -[16:46:00.423] After pose optimization, distance between ekf and vo x:0.000133 ,z=0.000151,angle between two frames:0.000129 -[16:46:00.423] frame process finished the id is: 122 the obs is:20 -[16:46:00.519] Odometry x=-0.003412, y=0.940450, theta=0.076925 -[16:46:00.578] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:0 ,distance between ekf and vo x:0.000858 ,z=0.000895,angle between two frames:0.000848 -[16:46:00.579] After pose optimization, distance between ekf and vo x:0.000858 ,z=0.000895,angle between two frames:0.000848 -[16:46:00.579] frame process finished the id is: 122 the obs is:20 -[16:46:00.619] Odometry x=-0.003171, y=0.946122, theta=0.074330 -[16:46:00.720] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:0.000090 ,z=0.000112,angle between two frames:0.000094 -[16:46:00.720] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.120153 error end=0.120136 -[16:46:00.720] After pose optimization, distance between ekf and vo x:0.000044 ,z=0.000419,angle between two frames:0.000093 -[16:46:00.720] frame process finished the id is: 122 the obs is:20 -[16:46:00.721] Odometry x=-0.002927, y=0.952432, theta=0.080917 -[16:46:00.819] Odometry x=-0.002622, y=0.959135, theta=0.078409 -[16:46:00.870] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:0 ,distance between ekf and vo x:0.001240 ,z=0.001196,angle between two frames:0.000960 -[16:46:00.870] After pose optimization, distance between ekf and vo x:0.001240 ,z=0.001196,angle between two frames:0.000960 -[16:46:00.870] frame process finished the id is: 122 the obs is:20 -[16:46:00.917] Odometry x=-0.002403, y=0.964527, theta=0.076933 -[16:46:01.019] Odometry x=-0.002189, y=0.970095, theta=0.070627 -[16:46:01.024] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:3 ,distance between ekf and vo x:0.001083 ,z=0.001621,angle between two frames:0.001223 -[16:46:01.024] n deleted obs = 1 n obs with reprojection error less than 0.5 px =2 error init =0.417204 error end=0.339551 -[16:46:01.024] After pose optimization, distance between ekf and vo x:-0.836969 ,z=0.941156,angle between two frames:0.097263 -[16:46:01.024] frame process finished the id is: 122 the obs is:20 -[16:46:01.121] Odometry x=-0.001991, y=0.975903, theta=0.064235 -[16:46:01.182] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:1 ,distance between ekf and vo x:0.001187 ,z=0.001219,angle between two frames:0.001049 -[16:46:01.183] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.069806 error end=0.069806 -[16:46:01.183] After pose optimization, distance between ekf and vo x:0.001187 ,z=0.001219,angle between two frames:0.001049 -[16:46:01.183] frame process finished the id is: 122 the obs is:20 -[16:46:01.219] Odometry x=-0.001769, y=0.981843, theta=0.072079 -[16:46:01.320] Odometry x=-0.001587, y=0.987074, theta=0.063949 -[16:46:01.334] After Reprojection Map nPoint:16 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:0.000580 ,z=0.000671,angle between two frames:0.000484 -[16:46:01.334] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.134346 error end=0.134312 -[16:46:01.334] After pose optimization, distance between ekf and vo x:0.000580 ,z=0.001103,angle between two frames:0.000624 -[16:46:01.334] frame process finished the id is: 122 the obs is:20 -[16:46:01.419] Odometry x=-0.001421, y=0.992569, theta=0.064672 -[16:46:01.482] After Reprojection Map nPoint:13 ,nCell:13 ,nMatches:0 ,distance between ekf and vo x:0.000506 ,z=0.000969,angle between two frames:0.000677 -[16:46:01.482] After pose optimization, distance between ekf and vo x:0.000506 ,z=0.000969,angle between two frames:0.000677 -[16:46:01.482] frame process finished the id is: 122 the obs is:20 -[16:46:01.518] Odometry x=-0.001240, y=0.998314, theta=0.063765 -[16:46:01.619] Odometry x=-0.001064, y=1.003761, theta=0.067354 -[16:46:01.625] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:2 ,distance between ekf and vo x:0.000105 ,z=0.000203,angle between two frames:0.000146 -[16:46:01.625] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.140101 error end=0.140101 -[16:46:01.625] After pose optimization, distance between ekf and vo x:0.000105 ,z=0.000203,angle between two frames:0.000146 -[16:46:01.625] frame process finished the id is: 122 the obs is:20 -[16:46:01.723] Odometry x=-0.000844, y=1.010192, theta=0.067720 -[16:46:01.772] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:0.000983 ,z=0.001474,angle between two frames:0.001066 -[16:46:01.772] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.497492 error end=0.456540 -[16:46:01.772] After pose optimization, distance between ekf and vo x:-0.298262 ,z=0.320490,angle between two frames:0.059301 -[16:46:01.772] frame process finished the id is: 122 the obs is:20 -[16:46:01.818] Odometry x=-0.000631, y=1.015804, theta=0.065551 -[16:46:01.914] Odometry x=-0.000451, y=1.021037, theta=0.067531 -[16:46:01.929] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:2 ,distance between ekf and vo x:0.001738 ,z=0.001909,angle between two frames:0.001595 -[16:46:01.929] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.275576 error end=0.275576 -[16:46:01.929] After pose optimization, distance between ekf and vo x:0.001738 ,z=0.001909,angle between two frames:0.001595 -[16:46:01.929] frame process finished the id is: 122 the obs is:20 -[16:46:02.019] Odometry x=-0.000272, y=1.026387, theta=0.066717 -[16:46:02.085] After Reprojection Map nPoint:14 ,nCell:14 ,nMatches:0 ,distance between ekf and vo x:0.001640 ,z=0.001958,angle between two frames:0.001581 -[16:46:02.085] After pose optimization, distance between ekf and vo x:0.001640 ,z=0.001958,angle between two frames:0.001581 -[16:46:02.085] frame process finished the id is: 122 the obs is:20 -[16:46:02.116] Odometry x=-0.000056, y=1.033174, theta=0.067070 -[16:46:02.219] Odometry x=0.000128, y=1.038656, theta=0.066937 -[16:46:02.235] After Reprojection Map nPoint:18 ,nCell:17 ,nMatches:3 ,distance between ekf and vo x:0.000553 ,z=0.001058,angle between two frames:0.000771 -[16:46:02.235] n deleted obs = 0 n obs with reprojection error less than 0.5 px =3 error init =0.115111 error end=0.113273 -[16:46:02.235] After pose optimization, distance between ekf and vo x:-0.001260 ,z=-0.009880,angle between two frames:0.034078 -[16:46:02.235] frame process finished the id is: 122 the obs is:20 -[16:46:02.318] Odometry x=0.000374, y=1.044686, theta=0.071830 -[16:46:02.380] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:0.000600 ,z=0.000742,angle between two frames:0.000602 -[16:46:02.380] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.226310 error end=0.209525 -[16:46:02.380] After pose optimization, distance between ekf and vo x:-0.123472 ,z=-0.018925,angle between two frames:0.079409 -[16:46:02.380] frame process finished the id is: 122 the obs is:20 -[16:46:02.420] Odometry x=0.000567, y=1.049883, theta=0.073095 -[16:46:02.518] Odometry x=0.000758, y=1.055255, theta=0.068517 -[16:46:02.533] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:2 ,distance between ekf and vo x:0.000924 ,z=0.001344,angle between two frames:0.001054 -[16:46:02.533] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.227620 error end=0.208668 -[16:46:02.533] After pose optimization, distance between ekf and vo x:-0.193549 ,z=0.014537,angle between two frames:0.083947 -[16:46:02.533] frame process finished the id is: 122 the obs is:20 -[16:46:02.620] Odometry x=0.000975, y=1.060473, theta=0.083678 -[16:46:02.682] After Reprojection Map nPoint:17 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.000382 ,z=0.000333,angle between two frames:0.000401 -[16:46:02.682] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.081242 error end=0.081242 -[16:46:02.682] After pose optimization, distance between ekf and vo x:0.000382 ,z=0.000333,angle between two frames:0.000401 -[16:46:02.682] frame process finished the id is: 122 the obs is:20 -[16:46:02.717] Odometry x=0.001210, y=1.065981, theta=0.079266 -[16:46:02.820] Odometry x=0.001446, y=1.071369, theta=0.079129 -[16:46:02.824] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:0.000056 ,z=0.000054,angle between two frames:0.000048 -[16:46:02.824] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.289851 error end=0.287784 -[16:46:02.824] After pose optimization, distance between ekf and vo x:-0.021666 ,z=0.031793,angle between two frames:0.006936 -[16:46:02.824] frame process finished the id is: 122 the obs is:20 -[16:46:02.918] Odometry x=0.001695, y=1.077387, theta=0.076424 -[16:46:02.966] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:0 ,distance between ekf and vo x:0.000059 ,z=0.000110,angle between two frames:0.000079 -[16:46:02.966] After pose optimization, distance between ekf and vo x:0.000059 ,z=0.000110,angle between two frames:0.000079 -[16:46:02.966] frame process finished the id is: 122 the obs is:20 -[16:46:03.015] Odometry x=0.001973, y=1.082874, theta=0.071552 -[16:46:03.108] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:2 ,distance between ekf and vo x:0.000083 ,z=0.000138,angle between two frames:0.000107 -[16:46:03.108] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.132074 error end=0.132074 -[16:46:03.108] After pose optimization, distance between ekf and vo x:0.000083 ,z=0.000138,angle between two frames:0.000107 -[16:46:03.108] frame process finished the id is: 122 the obs is:20 -[16:46:03.117] Odometry x=0.002216, y=1.088231, theta=0.074094 -[16:46:03.215] Odometry x=0.002413, y=1.093666, theta=0.069176 -[16:46:03.255] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.000298 ,z=0.000661,angle between two frames:0.000478 -[16:46:03.255] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.202305 error end=0.202305 -[16:46:03.255] After pose optimization, distance between ekf and vo x:0.000298 ,z=0.000661,angle between two frames:0.000478 -[16:46:03.255] frame process finished the id is: 122 the obs is:20 -[16:46:03.317] Odometry x=0.002643, y=1.100333, theta=0.070082 -[16:46:03.405] After Reprojection Map nPoint:18 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.000582 ,z=0.000723,angle between two frames:0.000600 -[16:46:03.406] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.153518 error end=0.121611 -[16:46:03.406] After pose optimization, distance between ekf and vo x:0.280021 ,z=0.124743,angle between two frames:0.180534 -[16:46:03.406] frame process finished the id is: 122 the obs is:20 -[16:46:03.419] Odometry x=0.002938, y=1.106440, theta=0.067113 -[16:46:03.516] Odometry x=0.003189, y=1.113359, theta=0.071208 -[16:46:03.555] After Reprojection Map nPoint:16 ,nCell:15 ,nMatches:3 ,distance between ekf and vo x:0.000623 ,z=0.000378,angle between two frames:0.000452 -[16:46:03.555] n deleted obs = 1 n obs with reprojection error less than 0.5 px =2 error init =0.185935 error end=0.185935 -[16:46:03.555] After pose optimization, distance between ekf and vo x:0.000623 ,z=0.000378,angle between two frames:0.000452 -[16:46:03.555] frame process finished the id is: 122 the obs is:20 -[16:46:03.614] Odometry x=0.003389, y=1.118834, theta=0.071881 -[16:46:03.697] After Reprojection Map nPoint:16 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:0.000059 ,z=0.000220,angle between two frames:0.000145 -[16:46:03.697] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.164763 error end=0.164115 -[16:46:03.697] After pose optimization, distance between ekf and vo x:-0.002904 ,z=0.004042,angle between two frames:0.000588 -[16:46:03.697] frame process finished the id is: 122 the obs is:20 -[16:46:03.716] Odometry x=0.003795, y=1.128542, theta=0.071510 -[16:46:03.819] Odometry x=0.004018, y=1.134792, theta=0.070446 -[16:46:03.849] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.000522 ,z=0.001102,angle between two frames:0.000803 -[16:46:03.849] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.099152 error end=0.097455 -[16:46:03.849] After pose optimization, distance between ekf and vo x:0.029752 ,z=0.045967,angle between two frames:0.037538 -[16:46:03.849] frame process finished the id is: 122 the obs is:20 -[16:46:03.916] Odometry x=0.004282, y=1.140131, theta=0.069112 -[16:46:03.991] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:1 ,distance between ekf and vo x:0.000185 ,z=0.000307,angle between two frames:0.000233 -[16:46:03.991] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.013479 error end=0.013479 -[16:46:03.991] After pose optimization, distance between ekf and vo x:0.000185 ,z=0.000307,angle between two frames:0.000233 -[16:46:03.991] frame process finished the id is: 122 the obs is:20 -[16:46:04.016] Odometry x=0.004482, y=1.145811, theta=0.071118 -[16:46:04.114] Odometry x=0.004731, y=1.151410, theta=0.076678 -[16:46:04.140] After Reprojection Map nPoint:17 ,nCell:16 ,nMatches:4 ,distance between ekf and vo x:0.001042 ,z=0.002152,angle between two frames:0.001537 -[16:46:04.140] n deleted obs = 1 n obs with reprojection error less than 0.5 px =3 error init =0.278145 error end=0.273992 -[16:46:04.140] After pose optimization, distance between ekf and vo x:0.041572 ,z=0.058455,angle between two frames:0.045368 -[16:46:04.140] frame process finished the id is: 122 the obs is:20 -[16:46:04.217] Odometry x=0.004986, y=1.157368, theta=0.075971 -[16:46:04.283] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.000292 ,z=0.000671,angle between two frames:0.000462 -[16:46:04.283] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.464571 error end=0.452702 -[16:46:04.283] After pose optimization, distance between ekf and vo x:-0.314807 ,z=0.118342,angle between two frames:0.068079 -[16:46:04.283] frame process finished the id is: 122 the obs is:20 -[16:46:04.320] Odometry x=0.005305, y=1.163992, theta=0.074964 -[16:46:04.418] Odometry x=0.005528, y=1.169645, theta=0.080200 -[16:46:04.428] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:0 ,distance between ekf and vo x:0.000182 ,z=0.000467,angle between two frames:0.000336 -[16:46:04.428] After pose optimization, distance between ekf and vo x:0.000182 ,z=0.000467,angle between two frames:0.000336 -[16:46:04.428] frame process finished the id is: 122 the obs is:20 -[16:46:04.517] Odometry x=0.006523, y=1.174817, theta=0.072854 -[16:46:04.575] After Reprojection Map nPoint:14 ,nCell:12 ,nMatches:2 ,distance between ekf and vo x:0.000502 ,z=0.000774,angle between two frames:0.000560 -[16:46:04.575] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.330717 error end=0.330717 -[16:46:04.575] After pose optimization, distance between ekf and vo x:0.000502 ,z=0.000774,angle between two frames:0.000560 -[16:46:04.575] frame process finished the id is: 122 the obs is:20 -[16:46:04.623] Odometry x=0.006685, y=1.180054, theta=0.076753 -[16:46:04.716] After Reprojection Map nPoint:13 ,nCell:13 ,nMatches:0 ,distance between ekf and vo x:0.000040 ,z=0.000123,angle between two frames:0.000089 -[16:46:04.716] After pose optimization, distance between ekf and vo x:0.000040 ,z=0.000123,angle between two frames:0.000089 -[16:46:04.716] frame process finished the id is: 122 the obs is:20 -[16:46:04.718] Odometry x=0.006978, y=1.187401, theta=0.076913 -[16:46:04.822] Odometry x=0.007235, y=1.193376, theta=0.084332 -[16:46:04.858] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:0 ,distance between ekf and vo x:0.000080 ,z=0.000171,angle between two frames:0.000127 -[16:46:04.858] After pose optimization, distance between ekf and vo x:0.000080 ,z=0.000171,angle between two frames:0.000127 -[16:46:04.858] frame process finished the id is: 122 the obs is:20 -[16:46:04.917] Odometry x=0.007475, y=1.198820, theta=0.078423 -[16:46:05.001] After Reprojection Map nPoint:15 ,nCell:14 ,nMatches:3 ,distance between ekf and vo x:0.000102 ,z=0.000194,angle between two frames:0.000153 -[16:46:05.001] n deleted obs = 0 n obs with reprojection error less than 0.5 px =3 error init =0.246231 error end=0.242312 -[16:46:05.001] After pose optimization, distance between ekf and vo x:0.089861 ,z=0.150415,angle between two frames:0.099423 -[16:46:05.001] frame process finished the id is: 122 the obs is:20 -[16:46:05.020] Odometry x=0.007694, y=1.204363, theta=0.079484 -[16:46:05.117] Odometry x=0.008024, y=1.209770, theta=0.080546 -[16:46:05.143] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:0.000020 ,z=0.000087,angle between two frames:0.000058 -[16:46:05.143] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.190791 error end=0.190791 -[16:46:05.143] After pose optimization, distance between ekf and vo x:0.000020 ,z=0.000087,angle between two frames:0.000058 -[16:46:05.143] frame process finished the id is: 122 the obs is:20 -[16:46:05.215] Odometry x=0.008232, y=1.215033, theta=0.074116 -[16:46:05.288] After Reprojection Map nPoint:14 ,nCell:14 ,nMatches:2 ,distance between ekf and vo x:0.000139 ,z=0.000383,angle between two frames:0.000267 -[16:46:05.288] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.248443 error end=0.215878 -[16:46:05.288] After pose optimization, distance between ekf and vo x:-0.865262 ,z=0.609088,angle between two frames:0.025616 -[16:46:05.288] frame process finished the id is: 122 the obs is:20 -[16:46:05.317] Odometry x=0.008509, y=1.221474, theta=0.070090 -[16:46:05.420] Odometry x=0.008702, y=1.226924, theta=0.073196 -[16:46:05.441] After Reprojection Map nPoint:15 ,nCell:14 ,nMatches:3 ,distance between ekf and vo x:0.000826 ,z=0.000983,angle between two frames:0.000841 -[16:46:05.441] n deleted obs = 0 n obs with reprojection error less than 0.5 px =3 error init =0.160501 error end=0.160501 -[16:46:05.441] After pose optimization, distance between ekf and vo x:0.000826 ,z=0.000983,angle between two frames:0.000841 -[16:46:05.441] frame process finished the id is: 122 the obs is:20 -[16:46:05.520] Odometry x=0.008926, y=1.232707, theta=0.072276 -[16:46:05.591] After Reprojection Map nPoint:13 ,nCell:13 ,nMatches:1 ,distance between ekf and vo x:0.000388 ,z=0.000723,angle between two frames:0.000566 -[16:46:05.591] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.084215 error end=0.084215 -[16:46:05.591] After pose optimization, distance between ekf and vo x:0.000388 ,z=0.000723,angle between two frames:0.000566 -[16:46:05.591] frame process finished the id is: 122 the obs is:20 -[16:46:05.622] Odometry x=0.009134, y=1.238655, theta=0.066525 -[16:46:05.719] Odometry x=0.009326, y=1.244518, theta=0.066719 -[16:46:05.749] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:1 ,distance between ekf and vo x:0.001125 ,z=0.001665,angle between two frames:0.001286 -[16:46:05.749] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.262674 error end=0.261048 -[16:46:05.749] After pose optimization, distance between ekf and vo x:-0.024468 ,z=0.037214,angle between two frames:0.011359 -[16:46:05.749] frame process finished the id is: 122 the obs is:20 -[16:46:05.818] Odometry x=0.009517, y=1.250215, theta=0.071308 -[16:46:05.899] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:3 ,distance between ekf and vo x:0.000820 ,z=0.001126,angle between two frames:0.000875 -[16:46:05.899] n deleted obs = 0 n obs with reprojection error less than 0.5 px =3 error init =0.257933 error end=0.256397 -[16:46:05.899] After pose optimization, distance between ekf and vo x:0.036663 ,z=0.012074,angle between two frames:0.025269 -[16:46:05.899] frame process finished the id is: 122 the obs is:20 -[16:46:05.920] Odometry x=0.009707, y=1.255614, theta=0.070349 -[16:46:06.018] Odometry x=0.009908, y=1.261039, theta=0.070662 -[16:46:06.057] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:0.001274 ,z=0.001445,angle between two frames:0.001245 -[16:46:06.057] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.217417 error end=0.217417 -[16:46:06.057] After pose optimization, distance between ekf and vo x:0.001274 ,z=0.001445,angle between two frames:0.001245 -[16:46:06.057] frame process finished the id is: 122 the obs is:20 -[16:46:06.120] Odometry x=0.010212, y=1.266397, theta=0.066838 -[16:46:06.207] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:1 ,distance between ekf and vo x:0.000229 ,z=0.000308,angle between two frames:0.000352 -[16:46:06.207] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.177989 error end=0.177989 -[16:46:06.207] After pose optimization, distance between ekf and vo x:0.000229 ,z=0.000308,angle between two frames:0.000352 -[16:46:06.207] frame process finished the id is: 122 the obs is:20 -[16:46:06.216] Odometry x=0.010410, y=1.271778, theta=0.075753 -[16:46:06.319] Odometry x=0.010616, y=1.277116, theta=0.075248 -[16:46:06.364] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:0.000728 ,z=0.001140,angle between two frames:0.000919 -[16:46:06.364] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.254715 error end=0.254715 -[16:46:06.364] After pose optimization, distance between ekf and vo x:0.000728 ,z=0.001140,angle between two frames:0.000919 -[16:46:06.364] frame process finished the id is: 122 the obs is:20 -[16:46:06.416] Odometry x=0.010865, y=1.282493, theta=0.068590 -[16:46:06.523] Odometry x=0.011045, y=1.287841, theta=0.067942 -[16:46:06.531] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.001049 ,z=0.001513,angle between two frames:0.001252 -[16:46:06.531] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.237638 error end=0.231298 -[16:46:06.531] After pose optimization, distance between ekf and vo x:-0.008200 ,z=0.192655,angle between two frames:0.099445 -[16:46:06.531] frame process finished the id is: 122 the obs is:20 -[16:46:06.620] Odometry x=0.011304, y=1.294099, theta=0.077239 -[16:46:06.696] After Reprojection Map nPoint:14 ,nCell:14 ,nMatches:1 ,distance between ekf and vo x:0.001051 ,z=0.001506,angle between two frames:0.001252 -[16:46:06.696] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.208971 error end=0.208971 -[16:46:06.696] After pose optimization, distance between ekf and vo x:0.001051 ,z=0.001506,angle between two frames:0.001252 -[16:46:06.696] frame process finished the id is: 122 the obs is:20 -[16:46:06.715] Odometry x=0.011530, y=1.299317, theta=0.078379 -[16:46:06.818] Odometry x=0.011795, y=1.304864, theta=0.071737 -[16:46:06.860] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:2 ,distance between ekf and vo x:0.000875 ,z=0.001445,angle between two frames:0.001121 -[16:46:06.860] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.293611 error end=0.292337 -[16:46:06.860] After pose optimization, distance between ekf and vo x:-0.166141 ,z=0.047219,angle between two frames:0.102419 -[16:46:06.860] frame process finished the id is: 122 the obs is:20 -[16:46:06.919] Odometry x=0.012019, y=1.310944, theta=0.080139 -[16:46:07.021] Odometry x=0.012254, y=1.316498, theta=0.083126 -[16:46:07.024] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:0 ,distance between ekf and vo x:0.000839 ,z=0.001434,angle between two frames:0.001102 -[16:46:07.024] After pose optimization, distance between ekf and vo x:0.000839 ,z=0.001434,angle between two frames:0.001102 -[16:46:07.024] frame process finished the id is: 122 the obs is:20 -[16:46:07.120] Odometry x=0.012519, y=1.322336, theta=0.077095 -[16:46:07.189] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.000796 ,z=0.001420,angle between two frames:0.001072 -[16:46:07.189] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.149017 error end=0.149017 -[16:46:07.189] After pose optimization, distance between ekf and vo x:0.000796 ,z=0.001420,angle between two frames:0.001072 -[16:46:07.189] frame process finished the id is: 122 the obs is:20 -[16:46:07.217] Odometry x=0.012719, y=1.327790, theta=0.070180 -[16:46:07.325] Odometry x=0.012893, y=1.332965, theta=0.070176 -[16:46:07.354] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:0.000960 ,z=0.001481,angle between two frames:0.001212 -[16:46:07.354] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =0.836498 error end=0.836498 -[16:46:07.354] After pose optimization, distance between ekf and vo x:0.000960 ,z=0.001481,angle between two frames:0.001212 -[16:46:07.354] frame process finished the id is: 122 the obs is:20 -[16:46:07.416] Odometry x=0.013121, y=1.339324, theta=0.076746 -[16:46:07.519] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:2 ,distance between ekf and vo x:0.000852 ,z=0.001451,angle between two frames:0.001138 -[16:46:07.519] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.562182 error end=0.546562 -[16:46:07.519] After pose optimization, distance between ekf and vo x:0.725884 ,z=0.043802,angle between two frames:0.531597 -[16:46:07.519] frame process finished the id is: 122 the obs is:20 -[16:46:07.520] Odometry x=0.013331, y=1.344228, theta=0.072115 -[16:46:07.619] Odometry x=0.013530, y=1.349734, theta=0.071780 -[16:46:07.686] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.000911 ,z=0.001473,angle between two frames:0.001190 -[16:46:07.686] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.386389 error end=0.380484 -[16:46:07.686] After pose optimization, distance between ekf and vo x:-0.109606 ,z=0.010362,angle between two frames:0.088802 -[16:46:07.686] frame process finished the id is: 122 the obs is:20 -[16:46:07.721] Odometry x=0.013737, y=1.355138, theta=0.078120 -[16:46:07.819] Odometry x=0.014020, y=1.362311, theta=0.086327 -[16:46:07.850] After Reprojection Map nPoint:14 ,nCell:14 ,nMatches:1 ,distance between ekf and vo x:0.000816 ,z=0.001445,angle between two frames:0.001124 -[16:46:07.850] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.287262 error end=0.287262 -[16:46:07.850] After pose optimization, distance between ekf and vo x:0.000816 ,z=0.001445,angle between two frames:0.001124 -[16:46:07.850] frame process finished the id is: 122 the obs is:20 -[16:46:07.920] Odometry x=0.014228, y=1.367360, theta=0.078502 -[16:46:08.013] After Reprojection Map nPoint:12 ,nCell:12 ,nMatches:1 ,distance between ekf and vo x:0.000711 ,z=0.001402,angle between two frames:0.001041 -[16:46:08.013] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.141942 error end=0.141632 -[16:46:08.013] After pose optimization, distance between ekf and vo x:0.002929 ,z=0.001556,angle between two frames:0.001790 -[16:46:08.013] frame process finished the id is: 122 the obs is:20 -[16:46:08.017] Odometry x=0.014469, y=1.372991, theta=0.077165 -[16:46:08.119] Odometry x=0.014671, y=1.378333, theta=0.080423 -[16:46:08.177] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.000808 ,z=0.001445,angle between two frames:0.001134 -[16:46:08.177] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.362329 error end=0.307744 -[16:46:08.177] After pose optimization, distance between ekf and vo x:-0.250691 ,z=0.054978,angle between two frames:0.176367 -[16:46:08.177] frame process finished the id is: 122 the obs is:20 -[16:46:08.340] After Reprojection Map nPoint:17 ,nCell:16 ,nMatches:3 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:08.340] n deleted obs = 0 n obs with reprojection error less than 0.5 px =3 error init =0.152285 error end=0.152285 -[16:46:08.340] After pose optimization, distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:08.340] frame process finished the id is: 122 the obs is:20 -[16:46:08.502] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:2 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:08.502] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.266063 error end=0.215871 -[16:46:08.502] After pose optimization, distance between ekf and vo x:-0.244673 ,z=0.034544,angle between two frames:0.106338 -[16:46:08.502] frame process finished the id is: 122 the obs is:20 -[16:46:08.664] After Reprojection Map nPoint:13 ,nCell:13 ,nMatches:1 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:08.664] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.246995 error end=0.244556 -[16:46:08.664] After pose optimization, distance between ekf and vo x:-0.022618 ,z=0.016540,angle between two frames:0.002675 -[16:46:08.664] frame process finished the id is: 122 the obs is:20 -[16:46:08.826] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:08.826] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.409248 error end=0.409248 -[16:46:08.826] After pose optimization, distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:08.826] frame process finished the id is: 122 the obs is:20 -[16:46:08.989] After Reprojection Map nPoint:16 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:08.989] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.097194 error end=0.097194 -[16:46:08.989] After pose optimization, distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:08.989] frame process finished the id is: 122 the obs is:20 -[16:46:09.151] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:0 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:09.151] After pose optimization, distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:09.151] frame process finished the id is: 122 the obs is:20 -[16:46:09.313] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:1 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:09.313] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.359347 error end=0.119922 -[16:46:09.313] After pose optimization, distance between ekf and vo x:-0.368061 ,z=-0.038178,angle between two frames:0.560082 -[16:46:09.313] frame process finished the id is: 122 the obs is:20 -[16:46:09.475] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:09.475] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.358933 error end=0.355460 -[16:46:09.475] After pose optimization, distance between ekf and vo x:-0.024521 ,z=0.001704,angle between two frames:0.001093 -[16:46:09.475] frame process finished the id is: 122 the obs is:20 -[16:46:09.638] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:2 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:09.638] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.689780 error end=0.338402 -[16:46:09.638] After pose optimization, distance between ekf and vo x:1.526374 ,z=0.669155,angle between two frames:0.555666 -[16:46:09.638] frame process finished the id is: 122 the obs is:20 -[16:46:09.800] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:0 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:09.800] After pose optimization, distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:09.800] frame process finished the id is: 122 the obs is:20 -[16:46:09.963] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:09.963] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.222046 error end=0.222046 -[16:46:09.963] After pose optimization, distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:09.963] frame process finished the id is: 122 the obs is:20 -[16:46:10.128] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:0 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:10.128] After pose optimization, distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:10.128] frame process finished the id is: 122 the obs is:20 -[16:46:10.291] After Reprojection Map nPoint:14 ,nCell:12 ,nMatches:2 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:10.291] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.234297 error end=0.206849 -[16:46:10.291] After pose optimization, distance between ekf and vo x:-0.466423 ,z=0.023299,angle between two frames:0.147199 -[16:46:10.291] frame process finished the id is: 122 the obs is:20 -[16:46:10.453] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:10.453] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.513771 error end=0.513771 -[16:46:10.453] After pose optimization, distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:10.453] frame process finished the id is: 122 the obs is:20 -[16:46:10.616] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:10.616] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.532397 error end=0.532397 -[16:46:10.616] After pose optimization, distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:10.616] frame process finished the id is: 122 the obs is:20 -[16:46:10.778] After Reprojection Map nPoint:17 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:10.778] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.224259 error end=0.219652 -[16:46:10.778] After pose optimization, distance between ekf and vo x:-0.019107 ,z=0.052968,angle between two frames:0.005360 -[16:46:10.778] frame process finished the id is: 122 the obs is:20 -[16:46:10.940] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:10.940] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.388207 error end=0.388207 -[16:46:10.940] After pose optimization, distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:10.940] frame process finished the id is: 122 the obs is:20 -[16:46:11.102] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:11.102] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.552864 error end=0.552864 -[16:46:11.102] After pose optimization, distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:11.102] frame process finished the id is: 122 the obs is:20 -[16:46:11.264] After Reprojection Map nPoint:15 ,nCell:13 ,nMatches:1 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:11.264] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.224230 error end=0.210943 -[16:46:11.264] After pose optimization, distance between ekf and vo x:-0.045619 ,z=0.148575,angle between two frames:0.031236 -[16:46:11.264] frame process finished the id is: 122 the obs is:20 -[16:46:11.426] After Reprojection Map nPoint:14 ,nCell:14 ,nMatches:1 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:11.426] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =0.669864 error end=0.669864 -[16:46:11.426] After pose optimization, distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:11.426] frame process finished the id is: 122 the obs is:20 -[16:46:11.588] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:11.588] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.398434 error end=0.321318 -[16:46:11.588] After pose optimization, distance between ekf and vo x:-0.413967 ,z=0.033736,angle between two frames:0.164939 -[16:46:11.588] frame process finished the id is: 122 the obs is:20 -[16:46:11.750] After Reprojection Map nPoint:16 ,nCell:15 ,nMatches:3 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:11.750] n deleted obs = 1 n obs with reprojection error less than 0.5 px =2 error init =0.448823 error end=0.392958 -[16:46:11.750] After pose optimization, distance between ekf and vo x:-0.342864 ,z=-0.002014,angle between two frames:0.401766 -[16:46:11.750] frame process finished the id is: 122 the obs is:20 -[16:46:11.913] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:11.913] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.358725 error end=0.345248 -[16:46:11.913] After pose optimization, distance between ekf and vo x:-0.097349 ,z=0.002531,angle between two frames:0.001093 -[16:46:11.913] frame process finished the id is: 122 the obs is:20 -[16:46:12.074] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:2 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:12.075] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.513667 error end=0.513667 -[16:46:12.075] After pose optimization, distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:12.075] frame process finished the id is: 122 the obs is:20 -[16:46:12.237] After Reprojection Map nPoint:15 ,nCell:14 ,nMatches:1 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:12.237] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.114934 error end=0.114742 -[16:46:12.237] After pose optimization, distance between ekf and vo x:0.002094 ,z=0.002690,angle between two frames:0.001767 -[16:46:12.237] frame process finished the id is: 122 the obs is:20 -[16:46:12.399] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:0.000748 ,z=0.001428,angle between two frames:0.001093 -[16:46:12.399] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.358982 error end=0.354977 -[1 \ No newline at end of file +[19:49:38.521] Init: frame zero: 0.000000, 0.000000 0.000000 +[19:49:38.521] Init: Selected first frame. +[19:49:38.521] frame process finished the id is: 0 the obs is:0 +[19:49:38.624] Init: px average disparity is:0.188269 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:38.624] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:38.624] frame process finished the id is: 1 the obs is:0 +[19:49:38.717] Init: px average disparity is:0.221316 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:38.717] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:38.717] frame process finished the id is: 2 the obs is:0 +[19:49:38.816] Init: px average disparity is:0.233082 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:38.816] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:38.816] frame process finished the id is: 3 the obs is:0 +[19:49:38.917] Init: px average disparity is:0.250148 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:38.917] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:38.918] frame process finished the id is: 4 the obs is:0 +[19:49:39.017] Init: px average disparity is:0.227952 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:39.017] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:39.017] frame process finished the id is: 5 the obs is:0 +[19:49:39.117] Init: px average disparity is:0.226568 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:39.117] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:39.117] frame process finished the id is: 6 the obs is:0 +[19:49:39.220] Init: px average disparity is:0.238835 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:39.220] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:39.220] frame process finished the id is: 7 the obs is:0 +[19:49:39.313] Init: px average disparity is:0.234278 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:39.313] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:39.313] frame process finished the id is: 8 the obs is:0 +[19:49:39.416] Init: px average disparity is:0.258770 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:39.416] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:39.416] frame process finished the id is: 9 the obs is:0 +[19:49:39.506] Init: px average disparity is:0.227133 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:39.506] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:39.506] frame process finished the id is: 10 the obs is:0 +[19:49:39.607] Init: px average disparity is:0.236011 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:39.607] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:39.607] frame process finished the id is: 11 the obs is:0 +[19:49:39.715] Init: px average disparity is:0.239615 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:39.715] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:39.715] frame process finished the id is: 12 the obs is:0 +[19:49:39.817] Init: px average disparity is:0.232306 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:39.817] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:39.817] frame process finished the id is: 13 the obs is:0 +[19:49:39.917] Init: px average disparity is:0.246446 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:39.917] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:39.917] frame process finished the id is: 14 the obs is:0 +[19:49:40.015] Init: px average disparity is:0.228394 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:40.015] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:40.015] frame process finished the id is: 15 the obs is:0 +[19:49:40.115] Init: px average disparity is:0.237524 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:40.115] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:40.115] frame process finished the id is: 16 the obs is:0 +[19:49:40.216] Init: px average disparity is:0.256782 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:40.216] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:40.216] frame process finished the id is: 17 the obs is:0 +[19:49:40.315] Init: px average disparity is:0.242406 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:40.315] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:40.315] frame process finished the id is: 18 the obs is:0 +[19:49:40.415] Init: px average disparity is:0.222805 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:40.415] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:40.415] frame process finished the id is: 19 the obs is:0 +[19:49:40.516] Init: px average disparity is:0.236235 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:40.516] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:40.516] frame process finished the id is: 20 the obs is:0 +[19:49:40.615] Init: px average disparity is:0.252421 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:40.616] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:40.616] frame process finished the id is: 21 the obs is:0 +[19:49:40.708] Init: px average disparity is:0.259641 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:40.708] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:40.708] frame process finished the id is: 22 the obs is:0 +[19:49:40.815] Init: px average disparity is:0.233535 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:40.815] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:40.815] frame process finished the id is: 23 the obs is:0 +[19:49:40.915] Init: px average disparity is:0.218944 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:40.915] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:40.915] frame process finished the id is: 24 the obs is:0 +[19:49:41.014] Init: px average disparity is:0.254586 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:41.014] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:41.015] frame process finished the id is: 25 the obs is:0 +[19:49:41.119] Init: px average disparity is:0.235550 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:41.119] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:41.119] frame process finished the id is: 26 the obs is:0 +[19:49:41.216] Init: px average disparity is:0.215086 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:41.216] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:41.216] frame process finished the id is: 27 the obs is:0 +[19:49:41.318] Init: px average disparity is:0.251079 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:41.318] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:41.318] frame process finished the id is: 28 the obs is:0 +[19:49:41.417] Init: px average disparity is:0.231826 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:41.417] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:41.417] frame process finished the id is: 29 the obs is:0 +[19:49:41.457] Odometry x=0.000000, y=0.000000, theta=-0.000000 +[19:49:41.516] Init: px average disparity is:0.226935 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:41.516] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:41.516] frame process finished the id is: 30 the obs is:0 +[19:49:41.551] Odometry x=0.000000, y=0.000000, theta=-0.000000 +[19:49:41.616] Init: px average disparity is:0.209080 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:41.616] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:41.617] frame process finished the id is: 31 the obs is:0 +[19:49:41.652] Odometry x=0.000035, y=0.006149, theta=-0.001159 +[19:49:41.716] Init: px average disparity is:1.696404 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:41.716] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:41.716] frame process finished the id is: 32 the obs is:0 +[19:49:41.753] Odometry x=0.000041, y=0.017375, theta=0.001838 +[19:49:41.817] Init: px average disparity is:5.936005 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:41.817] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:41.817] frame process finished the id is: 33 the obs is:0 +[19:49:41.853] Odometry x=0.000150, y=0.025982, theta=-0.005847 +[19:49:41.917] Init: px average disparity is:7.669655 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:41.917] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:41.917] frame process finished the id is: 34 the obs is:0 +[19:49:41.954] Odometry x=0.000549, y=0.033832, theta=0.002177 +[19:49:42.018] Init: px average disparity is:4.988225 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:42.018] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:42.018] frame process finished the id is: 35 the obs is:0 +[19:49:42.053] Odometry x=0.000541, y=0.039811, theta=0.002773 +[19:49:42.108] Init: px average disparity is:5.346269 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:42.108] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:42.108] frame process finished the id is: 36 the obs is:0 +[19:49:42.153] Odometry x=0.000613, y=0.045225, theta=-0.000899 +[19:49:42.217] Init: px average disparity is:4.364909 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:42.218] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:42.218] frame process finished the id is: 37 the obs is:0 +[19:49:42.252] Odometry x=0.000666, y=0.050368, theta=-0.001773 +[19:49:42.317] Init: px average disparity is:5.790380 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:42.317] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:42.317] frame process finished the id is: 38 the obs is:0 +[19:49:42.352] Odometry x=0.000856, y=0.057889, theta=0.007395 +[19:49:42.417] Init: px average disparity is:4.606778 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:42.417] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:42.417] frame process finished the id is: 39 the obs is:0 +[19:49:42.452] Odometry x=0.000894, y=0.062013, theta=-0.019343 +[19:49:42.518] Init: px average disparity is:6.395837 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:42.518] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:42.518] frame process finished the id is: 40 the obs is:0 +[19:49:42.556] Odometry x=0.000954, y=0.068169, theta=-0.009028 +[19:49:42.618] Init: px average disparity is:7.822779 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:42.618] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:42.618] frame process finished the id is: 41 the obs is:0 +[19:49:42.657] Odometry x=0.000950, y=0.073294, theta=-0.004372 +[19:49:42.718] Init: px average disparity is:9.373783 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:42.718] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:42.718] frame process finished the id is: 42 the obs is:0 +[19:49:42.752] Odometry x=0.001139, y=0.080271, theta=-0.004513 +[19:49:42.817] Init: px average disparity is:12.570659 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:42.818] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:42.818] frame process finished the id is: 43 the obs is:0 +[19:49:42.852] Odometry x=0.001206, y=0.085630, theta=-0.016900 +[19:49:42.919] Init: px average disparity is:15.233979 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:42.919] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:42.919] frame process finished the id is: 44 the obs is:0 +[19:49:42.957] Odometry x=0.001614, y=0.091347, theta=-0.006125 +[19:49:43.019] Init: px average disparity is:13.471584 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:43.019] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:43.019] frame process finished the id is: 45 the obs is:0 +[19:49:43.051] Odometry x=0.001612, y=0.096716, theta=-0.002080 +[19:49:43.118] Init: px average disparity is:12.670569 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:43.118] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:43.118] frame process finished the id is: 46 the obs is:0 +[19:49:43.152] Odometry x=0.001838, y=0.104755, theta=-0.002014 +[19:49:43.222] Init: px average disparity is:11.116645 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:43.222] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:43.222] frame process finished the id is: 47 the obs is:0 +[19:49:43.257] Odometry x=0.001421, y=0.110147, theta=0.002937 +[19:49:43.318] Init: px average disparity is:12.894508 ,While minimum is: 20.000000 KLT tracked : 132 +[19:49:43.318] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:43.318] frame process finished the id is: 48 the obs is:0 +[19:49:43.356] Odometry x=0.001499, y=0.115949, theta=0.005946 +[19:49:43.418] Init: px average disparity is:13.095316 ,While minimum is: 20.000000 KLT tracked : 131 +[19:49:43.419] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:43.419] frame process finished the id is: 49 the obs is:0 +[19:49:43.457] Odometry x=0.001905, y=0.121058, theta=-0.002575 +[19:49:43.518] Init: px average disparity is:19.247756 ,While minimum is: 20.000000 KLT tracked : 131 +[19:49:43.518] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:43.518] frame process finished the id is: 50 the obs is:0 +[19:49:43.568] Odometry x=0.002537, y=0.128010, theta=0.005480 +[19:49:43.617] Init: px average disparity is:14.037551 ,While minimum is: 20.000000 KLT tracked : 131 +[19:49:43.618] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:43.618] frame process finished the id is: 51 the obs is:0 +[19:49:43.658] Odometry x=0.005611, y=0.134310, theta=-0.010446 +[19:49:43.718] Init: px average disparity is:13.830226 ,While minimum is: 20.000000 KLT tracked : 131 +[19:49:43.718] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:43.718] frame process finished the id is: 52 the obs is:0 +[19:49:43.758] Odometry x=0.005676, y=0.149897, theta=-0.010394 +[19:49:43.818] Init: px average disparity is:9.265397 ,While minimum is: 20.000000 KLT tracked : 131 +[19:49:43.818] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:43.818] frame process finished the id is: 53 the obs is:0 +[19:49:43.858] Odometry x=0.007187, y=0.155089, theta=-0.002603 +[19:49:43.917] Init: px average disparity is:9.777981 ,While minimum is: 20.000000 KLT tracked : 131 +[19:49:43.917] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:43.917] frame process finished the id is: 54 the obs is:0 +[19:49:43.951] Odometry x=0.008259, y=0.160810, theta=-0.017180 +[19:49:44.017] Init: px average disparity is:9.536190 ,While minimum is: 20.000000 KLT tracked : 130 +[19:49:44.017] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:44.017] frame process finished the id is: 55 the obs is:0 +[19:49:44.057] Odometry x=0.009017, y=0.165935, theta=-0.012630 +[19:49:44.117] Init: px average disparity is:10.369553 ,While minimum is: 20.000000 KLT tracked : 130 +[19:49:44.117] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:44.117] frame process finished the id is: 56 the obs is:0 +[19:49:44.160] Odometry x=0.009079, y=0.171773, theta=-0.012267 +[19:49:44.220] Init: px average disparity is:11.258834 ,While minimum is: 20.000000 KLT tracked : 130 +[19:49:44.220] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:44.220] frame process finished the id is: 57 the obs is:0 +[19:49:44.260] Odometry x=0.015880, y=0.177088, theta=-0.009673 +[19:49:44.317] Init: px average disparity is:9.456231 ,While minimum is: 20.000000 KLT tracked : 130 +[19:49:44.318] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:44.318] frame process finished the id is: 58 the obs is:0 +[19:49:44.359] Odometry x=0.015899, y=0.182594, theta=0.002792 +[19:49:44.409] Init: px average disparity is:11.734588 ,While minimum is: 20.000000 KLT tracked : 130 +[19:49:44.409] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:44.410] frame process finished the id is: 59 the obs is:0 +[19:49:44.458] Odometry x=0.016546, y=0.188423, theta=-0.008001 +[19:49:44.517] Init: px average disparity is:13.815966 ,While minimum is: 20.000000 KLT tracked : 130 +[19:49:44.517] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:44.517] frame process finished the id is: 60 the obs is:0 +[19:49:44.559] Odometry x=0.016577, y=0.194581, theta=-0.013777 +[19:49:44.618] Init: px average disparity is:14.838170 ,While minimum is: 20.000000 KLT tracked : 130 +[19:49:44.618] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:44.618] frame process finished the id is: 61 the obs is:0 +[19:49:44.658] Odometry x=0.017349, y=0.199856, theta=-0.012537 +[19:49:44.719] Init: px average disparity is:15.758682 ,While minimum is: 20.000000 KLT tracked : 130 +[19:49:44.719] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:44.719] frame process finished the id is: 62 the obs is:0 +[19:49:44.751] Odometry x=0.017389, y=0.206513, theta=-0.005475 +[19:49:44.818] Init: px average disparity is:16.217376 ,While minimum is: 20.000000 KLT tracked : 130 +[19:49:44.818] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:44.818] frame process finished the id is: 63 the obs is:0 +[19:49:44.852] Odometry x=0.017518, y=0.211920, theta=-0.011487 +[19:49:44.919] Init: px average disparity is:16.601058 ,While minimum is: 20.000000 KLT tracked : 130 +[19:49:44.919] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:44.919] frame process finished the id is: 64 the obs is:0 +[19:49:44.953] Odometry x=0.017558, y=0.218570, theta=-0.006520 +[19:49:45.020] Init: px average disparity is:18.673430 ,While minimum is: 20.000000 KLT tracked : 130 +[19:49:45.020] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:45.020] frame process finished the id is: 65 the obs is:0 +[19:49:45.058] Odometry x=0.017879, y=0.224554, theta=-0.011514 +[19:49:45.156] Odometry x=0.016186, y=0.230152, theta=-0.004655 +[19:49:45.188] cur: x=-0.466419, y=-0.476442, z=0.745289 ref: x=-0.403867 y=-0.484616 z=0.775911 +[19:49:45.188] cur: x=0.147257, y=-0.534141, z=0.832472 ref: x=0.156052 y=-0.478265 z=0.864240 +[19:49:45.188] cur: x=0.147190, y=-0.532144, z=0.833762 ref: x=0.156084 y=-0.476225 z=0.865360 +[19:49:45.188] cur: x=0.064522, y=-0.509227, z=0.858210 ref: x=0.039918 y=-0.471158 z=0.881145 +[19:49:45.188] cur: x=0.068497, y=-0.509223, z=0.857904 ref: x=0.042762 y=-0.471147 z=0.881017 +[19:49:45.188] cur: x=0.081535, y=-0.520356, z=0.850048 ref: x=0.096787 y=-0.464643 z=0.880193 +[19:49:45.188] cur: x=0.075181, y=-0.513342, z=0.854884 ref: x=0.091179 y=-0.456459 z=0.885060 +[19:49:45.188] cur: x=-0.014099, y=-0.438449, z=0.898645 ref: x=0.008683 y=-0.402739 z=0.915274 +[19:49:45.188] cur: x=-0.050501, y=-0.403500, z=0.913585 ref: x=-0.022836 y=-0.373277 z=0.927439 +[19:49:45.188] cur: x=-0.056928, y=-0.398299, z=0.915487 ref: x=-0.028577 y=-0.369045 z=0.928972 +[19:49:45.188] cur: x=-0.076402, y=-0.380436, z=0.921646 ref: x=-0.045820 y=-0.354195 z=0.934048 +[19:49:45.188] cur: x=-0.048134, y=-0.341461, z=0.938663 ref: x=-0.022916 y=-0.313819 z=0.949206 +[19:49:45.188] cur: x=-0.111832, y=-0.345688, z=0.931661 ref: x=-0.083259 y=-0.320054 z=0.943734 +[19:49:45.188] cur: x=-0.244186, y=-0.245493, z=0.938140 ref: x=-0.212743 y=-0.237814 z=0.947726 +[19:49:45.188] cur: x=-0.241265, y=-0.245491, z=0.938896 ref: x=-0.209888 y=-0.237838 z=0.948357 +[19:49:45.188] cur: x=-0.238449, y=-0.243350, z=0.940171 ref: x=-0.207049 y=-0.235711 z=0.949511 +[19:49:45.188] cur: x=-0.229973, y=-0.234674, z=0.944479 ref: x=-0.198538 y=-0.227168 z=0.953403 +[19:49:45.188] cur: x=-0.226999, y=-0.234602, z=0.945216 ref: x=-0.195677 y=-0.227188 z=0.953990 +[19:49:45.188] cur: x=-0.229923, y=-0.236817, z=0.943956 ref: x=-0.198522 y=-0.229321 z=0.952891 +[19:49:45.188] cur: x=-0.235644, y=-0.241194, z=0.941434 ref: x=-0.204208 y=-0.233582 z=0.950652 +[19:49:45.188] cur: x=-0.229842, y=-0.238947, z=0.943439 ref: x=-0.198506 y=-0.231473 z=0.952374 +[19:49:45.188] cur: x=-0.215632, y=-0.227211, z=0.949673 ref: x=-0.181415 y=-0.218663 z=0.958788 +[19:49:45.188] cur: x=-0.338281, y=-0.203740, z=0.918725 ref: x=-0.289722 y=-0.213448 z=0.933006 +[19:49:45.188] cur: x=-0.187654, y=-0.210157, z=0.959489 ref: x=-0.152805 y=-0.201543 z=0.967487 +[19:49:45.188] cur: x=-0.394710, y=-0.138186, z=0.908355 ref: x=-0.346836 y=-0.135421 z=0.928098 +[19:49:45.188] cur: x=-0.208843, y=-0.060691, z=0.976064 ref: x=-0.176271 y=-0.062410 z=0.982361 +[19:49:45.188] cur: x=-0.205973, y=-0.060703, z=0.976673 ref: x=-0.173393 y=-0.062415 z=0.982873 +[19:49:45.188] cur: x=-0.194591, y=-0.054141, z=0.979389 ref: x=-0.161882 y=-0.055891 z=0.985226 +[19:49:45.188] cur: x=-0.414127, y=0.009487, z=0.910170 ref: x=-0.358532 y=-0.053273 z=0.931996 +[19:49:45.188] cur: x=-0.411025, y=0.007947, z=0.911590 ref: x=-0.355725 y=-0.053282 z=0.933071 +[19:49:45.188] cur: x=-0.426643, y=0.042973, z=0.903398 ref: x=-0.369771 y=-0.044588 z=0.928053 +[19:49:45.188] cur: x=-0.424188, y=0.041598, z=0.904618 ref: x=-0.366970 y=-0.044596 z=0.929163 +[19:49:45.188] cur: x=-0.418078, y=0.033844, z=0.907780 ref: x=-0.361364 y=-0.044612 z=0.931357 +[19:49:45.188] cur: x=-0.102354, y=-0.053189, z=0.993325 ref: x=-0.072327 y=-0.045053 z=0.996363 +[19:49:45.188] cur: x=-0.411707, y=0.036789, z=0.910573 ref: x=-0.355762 y=-0.040299 z=0.933707 +[19:49:45.188] cur: x=-0.203726, y=-0.036658, z=0.978341 ref: x=-0.170544 y=-0.038433 z=0.984600 +[19:49:45.188] cur: x=-0.203788, y=-0.034476, z=0.978408 ref: x=-0.170546 y=-0.036252 z=0.984683 +[19:49:45.188] cur: x=-0.206709, y=-0.032287, z=0.977870 ref: x=-0.173427 y=-0.034068 z=0.984257 +[19:49:45.188] cur: x=-0.425681, y=0.011983, z=0.904794 ref: x=-0.375420 y=-0.018631 z=0.926667 +[19:49:45.188] cur: x=-0.418423, y=0.018191, z=0.908070 ref: x=-0.367026 y=-0.014315 z=0.930100 +[19:49:45.188] cur: x=-0.431876, y=0.073998, z=0.898892 ref: x=-0.378226 y=0.007314 z=0.925685 +[19:49:45.188] cur: x=-0.407948, y=0.019058, z=0.912806 ref: x=-0.358617 y=0.007323 z=0.933456 +[19:49:45.188] cur: x=-0.415685, y=0.006210, z=0.909487 ref: x=-0.367032 y=0.002992 z=0.930203 +[19:49:45.188] cur: x=-0.305918, y=0.012434, z=0.951977 ref: x=-0.262329 y=0.018235 z=0.964806 +[19:49:45.188] cur: x=-0.353328, y=0.122272, z=0.927474 ref: x=-0.369687 y=0.067856 z=0.926675 +[19:49:45.188] cur: x=-0.436407, y=0.144030, z=0.888147 ref: x=-0.383670 y=0.065634 z=0.921135 +[19:49:45.188] cur: x=-0.208121, y=0.074190, z=0.975285 ref: x=-0.170492 y=0.074987 z=0.982502 +[19:49:45.188] cur: x=-0.205278, y=0.074178, z=0.975889 ref: x=-0.167613 y=0.074992 z=0.982997 +[19:49:45.188] cur: x=-0.288229, y=0.080796, z=0.954147 ref: x=-0.259356 y=0.079114 z=0.962536 +[19:49:45.188] cur: x=-0.285384, y=0.080599, z=0.955018 ref: x=-0.256503 y=0.079123 z=0.963300 +[19:49:45.188] cur: x=-0.357155, y=0.165104, z=0.919337 ref: x=-0.372445 y=0.076482 z=0.924897 +[19:49:45.188] cur: x=-0.208272, y=0.076675, z=0.975061 ref: x=-0.170488 y=0.077166 z=0.982334 +[19:49:45.188] cur: x=-0.205423, y=0.076649, z=0.975667 ref: x=-0.167609 y=0.077171 z=0.982829 +[19:49:45.188] cur: x=-0.522890, y=-0.031622, z=0.851813 ref: x=-0.490642 y=0.092823 z=0.866403 +[19:49:45.188] cur: x=-0.356062, y=0.088661, z=0.930247 ref: x=-0.377719 y=0.123887 z=0.917595 +[19:49:45.188] cur: x=-0.367346, y=0.079160, z=0.926710 ref: x=-0.372153 y=0.121780 z=0.920148 +[19:49:45.188] cur: x=-0.085177, y=0.166554, z=0.982347 ref: x=-0.054871 y=0.151416 z=0.986946 +[19:49:45.188] cur: x=-0.099838, y=0.168475, z=0.980637 ref: x=-0.069325 y=0.153571 z=0.985703 +[19:49:45.188] cur: x=-0.088008, y=0.172411, z=0.981086 ref: x=-0.057755 y=0.157936 z=0.985759 +[19:49:45.188] cur: x=-0.102508, y=0.192274, z=0.975973 ref: x=-0.072174 y=0.177463 z=0.981477 +[19:49:45.188] cur: x=-0.099559, y=0.192347, z=0.976264 ref: x=-0.069285 y=0.177469 z=0.981684 +[19:49:45.188] cur: x=-0.096496, y=0.192356, z=0.976569 ref: x=-0.066396 y=0.177474 z=0.981883 +[19:49:45.188] cur: x=-0.026426, y=0.205102, z=0.978384 ref: x=-0.002791 y=0.190552 z=0.981673 +[19:49:45.188] cur: x=-0.047746, y=0.198756, z=0.978885 ref: x=-0.023031 y=0.184036 z=0.982650 +[19:49:45.188] cur: x=-0.017798, y=0.200640, z=0.979503 ref: x=0.005883 y=0.186213 z=0.982492 +[19:49:45.188] cur: x=-0.023524, y=0.207232, z=0.978009 ref: x=0.000100 y=0.192721 z=0.981254 +[19:49:45.188] cur: x=-0.105292, y=0.207998, z=0.972446 ref: x=-0.075031 y=0.192641 z=0.978397 +[19:49:45.188] cur: x=-0.588966, y=0.269835, z=0.761779 ref: x=-0.514089 y=0.261491 z=0.816906 +[19:49:45.188] cur: x=0.153621, y=0.290359, z=0.944506 ref: x=0.158330 y=0.272080 z=0.949160 +[19:49:45.188] cur: x=0.156534, y=0.290540, z=0.943972 ref: x=0.161195 y=0.272059 z=0.948683 +[19:49:45.188] cur: x=0.056889, y=0.357477, z=0.932188 ref: x=0.074811 y=0.328112 z=0.941672 +[19:49:45.188] cur: x=0.059844, y=0.357595, z=0.931957 ref: x=0.077682 y=0.328100 z=0.941444 +[19:49:45.188] cur: x=0.059285, y=0.359296, z=0.931339 ref: x=0.077673 y=0.330228 z=0.940700 +[19:49:45.188] cur: x=-0.279474, y=0.359953, z=0.890128 ref: x=-0.256887 y=0.343274 z=0.903423 +[19:49:45.188] cur: x=0.147706, y=0.450639, z=0.880402 ref: x=0.157216 y=0.392984 z=0.906006 +[19:49:45.188] cur: x=0.104762, y=0.409007, z=0.906498 ref: x=0.160034 y=0.395045 z=0.904615 +[19:49:45.188] cur: x=0.075075, y=-0.519062, z=0.851433 ref: x=0.091127 y=-0.462635 z=0.881853 +[19:49:45.188] cur: x=0.061750, y=-0.499426, z=0.864153 ref: x=0.079911 y=-0.442082 z=0.893408 +[19:49:45.188] cur: x=0.075193, y=-0.515334, z=0.853684 ref: x=0.091161 y=-0.458519 z=0.883996 +[19:49:45.188] cur: x=-0.028420, y=-0.431941, z=0.901454 ref: x=-0.005626 y=-0.396448 z=0.918040 +[19:49:45.188] cur: x=-0.016959, y=-0.440546, z=0.897570 ref: x=0.005821 y=-0.404835 z=0.914371 +[19:49:45.188] cur: x=-0.016956, y=-0.436303, z=0.899640 ref: x=0.005823 y=-0.400644 z=0.916215 +[19:49:45.188] cur: x=-0.057100, y=-0.404697, z=0.912666 ref: x=-0.028565 y=-0.375378 z=0.926432 +[19:49:45.188] cur: x=-0.062395, y=-0.400602, z=0.914125 ref: x=-0.034306 y=-0.371145 z=0.927941 +[19:49:45.188] cur: x=-0.544186, y=-0.300186, z=0.783422 ref: x=-0.484343 y=-0.279612 z=0.828993 +[19:49:45.188] cur: x=-0.238684, y=-0.236956, z=0.941744 ref: x=-0.207099 y=-0.229256 z=0.951079 +[19:49:45.188] cur: x=-0.244328, y=-0.241223, z=0.939210 ref: x=-0.212779 y=-0.233514 z=0.948787 +[19:49:45.188] cur: x=-0.238517, y=-0.241216, z=0.940704 ref: x=-0.207066 y=-0.233560 z=0.950039 +[19:49:45.188] cur: x=-0.201721, y=-0.216626, z=0.955187 ref: x=-0.167136 y=-0.207954 z=0.963753 +[19:49:45.188] cur: x=-0.402219, y=-0.096106, z=0.910485 ref: x=-0.282264 y=0.026912 z=0.958959 +[19:49:45.188] cur: x=-0.413582, y=-0.050812, z=0.909048 ref: x=-0.471585 y=0.103697 z=0.875702 +[19:49:45.188] cur: x=-0.205278, y=0.074178, z=0.975889 ref: x=-0.167613 y=0.074992 z=0.982997 +[19:49:45.188] cur: x=-0.645734, y=0.221534, z=0.730719 ref: x=-0.570122 y=0.220076 z=0.791535 +[19:49:45.188] cur: x=0.156626, y=0.284199, z=0.945885 ref: x=0.161240 y=0.265625 z=0.950497 +[19:49:45.188] cur: x=-0.099713, y=0.289238, z=0.952050 ref: x=-0.069064 y=0.270374 z=0.960275 +[19:49:45.188] cur: x=-0.099594, y=0.285171, z=0.953288 ref: x=-0.069077 y=0.266075 z=0.961474 +[19:49:45.188] cur: x=-0.625817, y=0.254240, z=0.737370 ref: x=-0.548490 y=0.249926 z=0.797932 +[19:49:45.188] cur: x=-0.610034, y=0.260052, z=0.748486 ref: x=-0.532699 y=0.254627 z=0.807092 +[19:49:45.188] cur: x=0.156558, y=0.288441, z=0.944612 ref: x=0.161210 y=0.269915 z=0.949293 +[19:49:45.188] cur: x=0.072344, y=0.292768, z=0.953443 ref: x=0.086521 y=0.274611 z=0.957655 +[19:49:45.188] cur: x=-0.717914, y=0.302680, z=0.626885 ref: x=-0.571748 y=0.310689 z=0.759326 +[19:49:45.188] cur: x=-0.112051, y=0.325606, z=0.938843 ref: x=-0.080452 y=0.304625 z=0.949068 +[19:49:45.188] cur: x=0.056334, y=0.359196, z=0.931560 ref: x=0.074802 y=0.330240 z=0.940928 +[19:49:45.188] cur: x=-0.123673, y=0.321370, z=0.938843 ref: x=-0.091965 y=0.300301 z=0.949401 +[19:49:45.188] cur: x=0.057214, y=0.355446, z=0.932944 ref: x=0.074820 y=0.325983 z=0.942410 +[19:49:45.188] cur: x=-0.274427, y=0.362194, z=0.890789 ref: x=-0.251209 y=0.345473 z=0.904181 +[19:49:45.188] cur: x=-0.291224, y=0.384011, z=0.876198 ref: x=-0.267739 y=0.366186 z=0.891192 +[19:49:45.188] cur: x=-0.275595, y=0.394324, z=0.876673 ref: x=-0.383200 y=0.400471 z=0.832335 +[19:49:45.188] cur: x=-0.244621, y=-0.232686, z=0.941285 ref: x=-0.212847 y=-0.224907 z=0.950849 +[19:49:45.188] cur: x=0.117368, y=-0.525216, z=0.842836 ref: x=0.130735 y=-0.470439 z=0.872694 +[19:49:45.188] cur: x=-0.407317, y=-0.157563, z=0.899593 ref: x=-0.360677 y=-0.154673 z=0.919776 +[19:49:45.188] cur: x=-0.324752, y=-0.241754, z=0.914380 ref: x=-0.281023 y=-0.232853 z=0.931024 +[19:49:45.188] cur: x=-0.313059, y=-0.242435, z=0.918270 ref: x=-0.269694 y=-0.232978 z=0.934337 +[19:49:45.188] cur: x=-0.301622, y=-0.242909, z=0.921965 ref: x=-0.258345 y=-0.233097 z=0.937509 +[19:49:45.188] cur: x=-0.301341, y=-0.251438, z=0.919768 ref: x=-0.258255 y=-0.241678 z=0.935359 +[19:49:45.188] cur: x=-0.213062, y=-0.216515, z=0.952747 ref: x=-0.178614 y=-0.207890 z=0.961706 +[19:49:45.188] cur: x=-0.233742, y=-0.075448, z=0.969367 ref: x=-0.202114 y=-0.077607 z=0.976282 +[19:49:45.188] cur: x=-0.223230, y=-0.075774, z=0.971816 ref: x=-0.190623 y=-0.077633 z=0.978589 +[19:49:45.188] cur: x=-0.399499, y=-0.103178, z=0.910909 ref: x=-0.282273 y=0.018219 z=0.959161 +[19:49:45.188] cur: x=-0.348377, y=0.007365, z=0.937326 ref: x=-0.394516 y=0.112984 z=0.911916 +[19:49:45.188] cur: x=-0.333106, y=0.005802, z=0.942871 ref: x=-0.383383 y=0.113074 z=0.916642 +[19:49:45.188] cur: x=-0.217262, y=0.088395, z=0.972103 ref: x=-0.179098 y=0.088039 z=0.979884 +[19:49:45.188] cur: x=-0.527515, y=-0.013401, z=0.849440 ref: x=-0.493234 y=0.103470 z=0.863721 +[19:49:45.188] cur: x=-0.212603, y=0.257800, z=0.942517 ref: x=-0.178381 y=0.244024 z=0.953222 +[19:49:45.188] cur: x=-0.648972, y=0.295058, z=0.701268 ref: x=-0.642545 y=0.282517 z=0.712264 +[19:49:45.188] cur: x=-0.053504, y=0.189892, z=0.980346 ref: x=-0.028820 y=0.175349 z=0.984084 +[19:49:45.188] cur: x=-0.533625, y=-0.026091, z=0.845319 ref: x=-0.504080 y=0.094823 z=0.858436 +[19:49:45.188] cur: x=-0.212703, y=0.266599, z=0.940044 ref: x=-0.178319 y=0.252622 z=0.950991 +[19:49:45.188] cur: x=0.151002, y=0.270971, z=0.950670 ref: x=0.155592 y=0.252776 z=0.954932 +[19:49:45.188] cur: x=0.025240, y=0.391129, z=0.919990 ref: x=0.119921 y=0.422593 z=0.898351 +[19:49:45.188] Init: Homography RANSAC (inlier) is:0 ,While 25 inliers minimum required. px average disparity is:21.062103 ,While minimum is: 20.000000 KLT tracked: 130 +[19:49:45.188] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:45.188] frame process finished the id is: 66 the obs is:0 +[19:49:45.209] Init: px average disparity is:18.990496 ,While minimum is: 20.000000 KLT tracked : 130 +[19:49:45.209] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:45.209] frame process finished the id is: 67 the obs is:0 +[19:49:45.256] Odometry x=0.016278, y=0.237959, theta=0.000778 +[19:49:45.320] Init: px average disparity is:18.447386 ,While minimum is: 20.000000 KLT tracked : 130 +[19:49:45.320] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:45.320] frame process finished the id is: 68 the obs is:0 +[19:49:45.358] Odometry x=0.012411, y=0.243228, theta=-0.011925 +[19:49:45.421] Init: px average disparity is:18.129400 ,While minimum is: 20.000000 KLT tracked : 128 +[19:49:45.421] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:45.421] frame process finished the id is: 69 the obs is:0 +[19:49:45.455] Odometry x=0.013469, y=0.249513, theta=0.012812 +[19:49:45.511] Init: px average disparity is:18.415508 ,While minimum is: 20.000000 KLT tracked : 128 +[19:49:45.511] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:45.511] frame process finished the id is: 70 the obs is:0 +[19:49:45.555] Odometry x=0.013843, y=0.255365, theta=0.001693 +[19:49:45.654] Odometry x=0.013631, y=0.273863, theta=0.020063 +[19:49:45.690] cur: x=-0.434125, y=-0.484281, z=0.759610 ref: x=-0.403867 y=-0.484616 z=0.775911 +[19:49:45.690] cur: x=0.111305, y=-0.508028, z=0.854119 ref: x=0.039918 y=-0.471158 z=0.881145 +[19:49:45.690] cur: x=0.114165, y=-0.508284, z=0.853589 ref: x=0.042762 y=-0.471147 z=0.881017 +[19:49:45.690] cur: x=0.164214, y=-0.503204, z=0.848422 ref: x=0.096787 y=-0.464643 z=0.880193 +[19:49:45.690] cur: x=0.158641, y=-0.495035, z=0.854268 ref: x=0.091179 y=-0.456459 z=0.885060 +[19:49:45.690] cur: x=0.021198, y=-0.438142, z=0.898656 ref: x=0.008683 y=-0.402739 z=0.915274 +[19:49:45.690] cur: x=-0.014032, y=-0.403078, z=0.915058 ref: x=-0.022836 y=-0.373277 z=0.927439 +[19:49:45.690] cur: x=-0.021098, y=-0.396875, z=0.917630 ref: x=-0.028577 y=-0.369045 z=0.928972 +[19:49:45.690] cur: x=-0.039937, y=-0.378315, z=0.924815 ref: x=-0.045820 y=-0.354195 z=0.934048 +[19:49:45.690] cur: x=-0.008934, y=-0.339843, z=0.940440 ref: x=-0.022916 y=-0.313819 z=0.949206 +[19:49:45.690] cur: x=-0.073247, y=-0.343103, z=0.936437 ref: x=-0.083259 y=-0.320054 z=0.943734 +[19:49:45.690] cur: x=-0.202572, y=-0.236280, z=0.950335 ref: x=-0.212743 y=-0.237814 z=0.947726 +[19:49:45.690] cur: x=-0.199512, y=-0.236281, z=0.950982 ref: x=-0.209888 y=-0.237838 z=0.948357 +[19:49:45.690] cur: x=-0.196670, y=-0.234195, z=0.952089 ref: x=-0.207049 y=-0.235711 z=0.949511 +[19:49:45.690] cur: x=-0.188793, y=-0.226107, z=0.955632 ref: x=-0.198538 y=-0.227168 z=0.953403 +[19:49:45.690] cur: x=-0.186082, y=-0.226277, z=0.956124 ref: x=-0.195677 y=-0.227188 z=0.953990 +[19:49:45.690] cur: x=-0.188748, y=-0.228259, z=0.955129 ref: x=-0.198522 y=-0.229321 z=0.952891 +[19:49:45.690] cur: x=-0.193955, y=-0.232162, z=0.953143 ref: x=-0.204208 y=-0.233582 z=0.950652 +[19:49:45.690] cur: x=-0.188692, y=-0.230400, z=0.954626 ref: x=-0.198506 y=-0.231473 z=0.952374 +[19:49:45.690] cur: x=-0.174223, y=-0.218929, z=0.960061 ref: x=-0.181415 y=-0.218663 z=0.958788 +[19:49:45.690] cur: x=-0.299831, y=-0.192924, z=0.934281 ref: x=-0.289722 y=-0.213448 z=0.933006 +[19:49:45.690] cur: x=-0.145886, y=-0.202539, z=0.968347 ref: x=-0.152805 y=-0.201543 z=0.967487 +[19:49:45.690] cur: x=-0.355491, y=-0.124505, z=0.926350 ref: x=-0.346836 y=-0.135421 z=0.928098 +[19:49:45.690] cur: x=-0.162213, y=-0.051822, z=0.985394 ref: x=-0.176271 y=-0.062410 z=0.982361 +[19:49:45.690] cur: x=-0.159344, y=-0.051834, z=0.985861 ref: x=-0.173393 y=-0.062415 z=0.982873 +[19:49:45.690] cur: x=-0.147702, y=-0.045479, z=0.987986 ref: x=-0.161882 y=-0.055891 z=0.985226 +[19:49:45.690] cur: x=-0.373168, y=0.034822, z=0.927110 ref: x=-0.358532 y=-0.053273 z=0.931996 +[19:49:45.690] cur: x=-0.369720, y=0.032581, z=0.928572 ref: x=-0.355725 y=-0.053282 z=0.933071 +[19:49:45.690] cur: x=-0.386825, y=0.095874, z=0.917156 ref: x=-0.369771 y=-0.044588 z=0.928053 +[19:49:45.690] cur: x=-0.384734, y=0.097633, z=0.917850 ref: x=-0.366970 y=-0.044596 z=0.929163 +[19:49:45.690] cur: x=-0.378376, y=0.082082, z=0.922006 ref: x=-0.361364 y=-0.044612 z=0.931357 +[19:49:45.690] cur: x=-0.053301, y=-0.128673, z=0.990254 ref: x=-0.072327 y=-0.045053 z=0.996363 +[19:49:45.690] cur: x=-0.372393, y=0.092603, z=0.923444 ref: x=-0.355762 y=-0.040299 z=0.933707 +[19:49:45.690] cur: x=-0.156624, y=-0.027954, z=0.987263 ref: x=-0.170544 y=-0.038433 z=0.984600 +[19:49:45.690] cur: x=-0.156655, y=-0.025767, z=0.987317 ref: x=-0.170546 y=-0.036252 z=0.984683 +[19:49:45.690] cur: x=-0.159559, y=-0.023570, z=0.986907 ref: x=-0.173427 y=-0.034068 z=0.984257 +[19:49:45.690] cur: x=-0.388055, y=0.092326, z=0.917000 ref: x=-0.375420 y=-0.018631 z=0.926667 +[19:49:45.690] cur: x=-0.376627, y=0.037738, z=0.925596 ref: x=-0.367026 y=-0.014315 z=0.930100 +[19:49:45.690] cur: x=-0.393556, y=0.184108, z=0.900676 ref: x=-0.378226 y=0.007314 z=0.925685 +[19:49:45.690] cur: x=-0.304506, y=0.019577, z=0.952309 ref: x=-0.358617 y=0.007323 z=0.933456 +[19:49:45.690] cur: x=-0.373869, y=0.024308, z=0.927163 ref: x=-0.367032 y=0.002992 z=0.930203 +[19:49:45.690] cur: x=-0.259348, y=0.021859, z=0.965537 ref: x=-0.262329 y=0.018235 z=0.964806 +[19:49:45.690] cur: x=-0.307481, y=0.111546, z=0.944994 ref: x=-0.369687 y=0.067856 z=0.926675 +[19:49:45.690] cur: x=-0.319526, y=0.106456, z=0.941578 ref: x=-0.383670 y=0.065634 z=0.921135 +[19:49:45.690] cur: x=-0.159007, y=0.082002, z=0.983866 ref: x=-0.170492 y=0.074987 z=0.982502 +[19:49:45.690] cur: x=-0.156138, y=0.081964, z=0.984329 ref: x=-0.167613 y=0.074992 z=0.982997 +[19:49:45.690] cur: x=-0.236999, y=0.089276, z=0.967399 ref: x=-0.259356 y=0.079114 z=0.962536 +[19:49:45.690] cur: x=-0.234156, y=0.089099, z=0.968108 ref: x=-0.256503 y=0.079123 z=0.963300 +[19:49:45.690] cur: x=-0.307485, y=0.157251, z=0.938470 ref: x=-0.372445 y=0.076482 z=0.924897 +[19:49:45.690] cur: x=-0.159077, y=0.084533, z=0.983641 ref: x=-0.170488 y=0.077166 z=0.982334 +[19:49:45.690] cur: x=-0.156206, y=0.084478, z=0.984105 ref: x=-0.167609 y=0.077171 z=0.982829 +[19:49:45.690] cur: x=-0.483743, y=-0.014016, z=0.875098 ref: x=-0.490642 y=0.092823 z=0.866403 +[19:49:45.690] cur: x=-0.308777, y=0.102645, z=0.945580 ref: x=-0.377719 y=0.123887 z=0.917595 +[19:49:45.690] cur: x=-0.321584, y=0.093280, z=0.942275 ref: x=-0.372153 y=0.121780 z=0.920148 +[19:49:45.690] cur: x=-0.032631, y=0.172537, z=0.984462 ref: x=-0.054871 y=0.151416 z=0.986946 +[19:49:45.690] cur: x=-0.047419, y=0.174323, z=0.983546 ref: x=-0.069325 y=0.153571 z=0.985703 +[19:49:45.690] cur: x=-0.035411, y=0.178187, z=0.983359 ref: x=-0.057755 y=0.157936 z=0.985759 +[19:49:45.690] cur: x=-0.049694, y=0.198068, z=0.978928 ref: x=-0.072174 y=0.177463 z=0.981477 +[19:49:45.690] cur: x=-0.046699, y=0.198150, z=0.979059 ref: x=-0.069285 y=0.177469 z=0.981684 +[19:49:45.690] cur: x=-0.043610, y=0.198155, z=0.979200 ref: x=-0.066396 y=0.177474 z=0.981883 +[19:49:45.690] cur: x=0.028039, y=0.210054, z=0.977288 ref: x=-0.002791 y=0.190552 z=0.981673 +[19:49:45.690] cur: x=0.005935, y=0.204156, z=0.978920 ref: x=-0.023031 y=0.184036 z=0.982650 +[19:49:45.690] cur: x=0.036630, y=0.205524, z=0.977966 ref: x=0.005883 y=0.186213 z=0.982492 +[19:49:45.690] cur: x=0.030972, y=0.212150, z=0.976746 ref: x=0.000100 y=0.192721 z=0.981254 +[19:49:45.690] cur: x=-0.052345, y=0.213883, z=0.975456 ref: x=-0.075031 y=0.192641 z=0.978397 +[19:49:45.690] cur: x=-0.552169, y=0.288980, z=0.782049 ref: x=-0.514089 y=0.261491 z=0.816906 +[19:49:45.690] cur: x=0.212328, y=0.289448, z=0.933347 ref: x=0.158330 y=0.272080 z=0.949160 +[19:49:45.690] cur: x=0.215214, y=0.289530, z=0.932660 ref: x=0.161195 y=0.272059 z=0.948683 +[19:49:45.690] cur: x=0.111475, y=0.356524, z=0.927612 ref: x=0.074811 y=0.328112 z=0.941672 +[19:49:45.690] cur: x=0.114562, y=0.356836, z=0.927116 ref: x=0.077682 y=0.328100 z=0.941444 +[19:49:45.690] cur: x=0.113962, y=0.358638, z=0.926494 ref: x=0.077673 y=0.330228 z=0.940700 +[19:49:45.690] cur: x=-0.250917, y=0.375170, z=0.892349 ref: x=-0.256887 y=0.343274 z=0.903423 +[19:49:45.690] cur: x=0.239779, y=0.487381, z=0.839622 ref: x=0.157216 y=0.392984 z=0.906006 +[19:49:45.690] cur: x=0.216128, y=0.464026, z=0.859051 ref: x=0.160034 y=0.395045 z=0.904615 +[19:49:45.690] cur: x=0.158243, y=-0.500799, z=0.850976 ref: x=0.091127 y=-0.462635 z=0.881853 +[19:49:45.690] cur: x=0.151528, y=-0.479711, z=0.864243 ref: x=0.079911 y=-0.442082 z=0.893408 +[19:49:45.690] cur: x=0.158356, y=-0.497091, z=0.853126 ref: x=0.091161 y=-0.458519 z=0.883996 +[19:49:45.690] cur: x=0.007027, y=-0.431561, z=0.902057 ref: x=-0.005626 y=-0.396448 z=0.918040 +[19:49:45.690] cur: x=0.018280, y=-0.440263, z=0.897683 ref: x=0.005821 y=-0.404835 z=0.914371 +[19:49:45.690] cur: x=0.018368, y=-0.435956, z=0.899781 ref: x=0.005823 y=-0.400644 z=0.916215 +[19:49:45.690] cur: x=-0.021194, y=-0.403701, z=0.914645 ref: x=-0.028565 y=-0.375378 z=0.926432 +[19:49:45.690] cur: x=-0.025875, y=-0.399902, z=0.916192 ref: x=-0.034306 y=-0.371145 z=0.927941 +[19:49:45.690] cur: x=-0.515588, y=-0.285619, z=0.807831 ref: x=-0.484343 y=-0.279612 z=0.828993 +[19:49:45.690] cur: x=-0.196765, y=-0.227749, z=0.953632 ref: x=-0.207099 y=-0.229256 z=0.951079 +[19:49:45.690] cur: x=-0.202581, y=-0.231964, z=0.951396 ref: x=-0.212779 y=-0.233514 z=0.948787 +[19:49:45.690] cur: x=-0.196677, y=-0.232038, z=0.952616 ref: x=-0.207066 y=-0.233560 z=0.950039 +[19:49:45.690] cur: x=-0.160185, y=-0.208581, z=0.964798 ref: x=-0.167136 y=-0.207954 z=0.963753 +[19:49:45.690] cur: x=-0.359677, y=-0.084927, z=0.929204 ref: x=-0.282264 y=0.026912 z=0.958959 +[19:49:45.690] cur: x=-0.464630, y=-0.012011, z=0.885423 ref: x=-0.471585 y=0.103697 z=0.875702 +[19:49:45.690] cur: x=-0.156138, y=0.081964, z=0.984329 ref: x=-0.167613 y=0.074992 z=0.982997 +[19:49:45.690] cur: x=-0.610686, y=0.241975, z=0.753997 ref: x=-0.570122 y=0.220076 z=0.791535 +[19:49:45.690] cur: x=0.215230, y=0.283238, z=0.934587 ref: x=0.161240 y=0.265625 z=0.950497 +[19:49:45.690] cur: x=-0.045577, y=0.295866, z=0.954142 ref: x=-0.069064 y=0.270374 z=0.960275 +[19:49:45.690] cur: x=-0.045538, y=0.291788, z=0.955398 ref: x=-0.069077 y=0.266075 z=0.961474 +[19:49:45.690] cur: x=-0.592151, y=0.273203, z=0.758102 ref: x=-0.548490 y=0.249926 z=0.797932 +[19:49:45.690] cur: x=-0.576684, y=0.278504, z=0.768030 ref: x=-0.532699 y=0.254627 z=0.807092 +[19:49:45.690] cur: x=0.215202, y=0.287443, z=0.933309 ref: x=0.161210 y=0.269915 z=0.949293 +[19:49:45.690] cur: x=0.129835, y=0.294198, z=0.946884 ref: x=0.086521 y=0.274611 z=0.957655 +[19:49:45.690] cur: x=-0.722930, y=0.317038, z=0.613889 ref: x=-0.571748 y=0.310689 z=0.759326 +[19:49:45.690] cur: x=-0.057772, y=0.333136, z=0.941107 ref: x=-0.080452 y=0.304625 z=0.949068 +[19:49:45.690] cur: x=0.111017, y=0.358512, z=0.926901 ref: x=0.074802 y=0.330240 z=0.940928 +[19:49:45.690] cur: x=-0.069420, y=0.329171, z=0.941715 ref: x=-0.091965 y=0.300301 z=0.949401 +[19:49:45.690] cur: x=0.112078, y=0.354817, z=0.928194 ref: x=0.074820 y=0.325983 z=0.942410 +[19:49:45.690] cur: x=-0.245598, y=0.377739, z=0.892746 ref: x=-0.251209 y=0.345473 z=0.904181 +[19:49:45.690] cur: x=-0.266141, y=0.403022, z=0.875639 ref: x=-0.267739 y=0.366186 z=0.891192 +[19:49:45.690] cur: x=0.088527, y=0.464005, z=0.881398 ref: x=-0.383200 y=0.400471 z=0.832335 +[19:49:45.690] cur: x=-0.202804, y=-0.223395, z=0.953397 ref: x=-0.212847 y=-0.224907 z=0.950849 +[19:49:45.690] cur: x=0.160485, y=-0.521952, z=0.837741 ref: x=0.130735 y=-0.470439 z=0.872694 +[19:49:45.690] cur: x=-0.368706, y=-0.144107, z=0.918308 ref: x=-0.360677 y=-0.154673 z=0.919776 +[19:49:45.690] cur: x=-0.286960, y=-0.231174, z=0.929630 ref: x=-0.281023 y=-0.232853 z=0.931024 +[19:49:45.690] cur: x=-0.275068, y=-0.232239, z=0.932954 ref: x=-0.269694 y=-0.232978 z=0.934337 +[19:49:45.690] cur: x=-0.263531, y=-0.232921, z=0.936108 ref: x=-0.258345 y=-0.233097 z=0.937509 +[19:49:45.690] cur: x=-0.263488, y=-0.241491, z=0.933947 ref: x=-0.258255 y=-0.241678 z=0.935359 +[19:49:45.690] cur: x=-0.171568, y=-0.208387, z=0.962881 ref: x=-0.178614 y=-0.207890 z=0.961706 +[19:49:45.690] cur: x=-0.185961, y=-0.066202, z=0.980324 ref: x=-0.202114 y=-0.077607 z=0.976282 +[19:49:45.690] cur: x=-0.176219, y=-0.066679, z=0.982090 ref: x=-0.190623 y=-0.077633 z=0.978589 +[19:49:45.690] cur: x=-0.357958, y=-0.089468, z=0.929442 ref: x=-0.282273 y=0.018219 z=0.959161 +[19:49:45.690] cur: x=-0.305056, y=0.018223, z=0.952160 ref: x=-0.394516 y=0.112984 z=0.911916 +[19:49:45.690] cur: x=-0.312450, y=0.094847, z=0.945187 ref: x=-0.383383 y=0.113074 z=0.916642 +[19:49:45.690] cur: x=-0.167994, y=0.096703, z=0.981033 ref: x=-0.179098 y=0.088039 z=0.979884 +[19:49:45.690] cur: x=-0.488209, y=0.004951, z=0.872713 ref: x=-0.493234 y=0.103470 z=0.863721 +[19:49:45.690] cur: x=-0.159493, y=0.265235, z=0.950901 ref: x=-0.178381 y=0.244024 z=0.953222 +[19:49:45.690] cur: x=-0.625225, y=0.314919, z=0.714087 ref: x=-0.642545 y=0.282517 z=0.712264 +[19:49:45.690] cur: x=0.000087, y=0.195457, z=0.980712 ref: x=-0.028820 y=0.175349 z=0.984084 +[19:49:45.690] cur: x=-0.495774, y=-0.004864, z=0.868438 ref: x=-0.504080 y=0.094823 z=0.858436 +[19:49:45.690] cur: x=-0.159634, y=0.275025, z=0.948092 ref: x=-0.178319 y=0.252622 z=0.950991 +[19:49:45.690] cur: x=0.209607, y=0.270253, z=0.939696 ref: x=0.155592 y=0.252776 z=0.954932 +[19:49:45.690] cur: x=0.102028, y=0.451039, z=0.886653 ref: x=0.119921 y=0.422593 z=0.898351 +[19:49:45.690] Init: Homography RANSAC (inlier) is:0 ,While 25 inliers minimum required. px average disparity is:20.867987 ,While minimum is: 20.000000 KLT tracked: 128 +[19:49:45.690] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:49:45.690] frame process finished the id is: 71 the obs is:0 +[19:49:45.757] Odometry x=0.013683, y=0.279379, theta=-0.008153 +[19:49:45.773] cur: x=-0.436812, y=-0.480596, z=0.760409 ref: x=-0.403867 y=-0.484616 z=0.775911 +[19:49:45.773] cur: x=0.123287, y=-0.507153, z=0.852992 ref: x=0.039918 y=-0.471158 z=0.881145 +[19:49:45.773] cur: x=0.125849, y=-0.507492, z=0.852417 ref: x=0.042762 y=-0.471147 z=0.881017 +[19:49:45.773] cur: x=0.170066, y=-0.507145, z=0.844915 ref: x=0.096787 y=-0.464643 z=0.880193 +[19:49:45.773] cur: x=0.164083, y=-0.499685, z=0.850524 ref: x=0.091179 y=-0.456459 z=0.885060 +[19:49:45.773] cur: x=0.025024, y=-0.442991, z=0.896177 ref: x=0.008683 y=-0.402739 z=0.915274 +[19:49:45.773] cur: x=-0.010671, y=-0.407427, z=0.913175 ref: x=-0.022836 y=-0.373277 z=0.927439 +[19:49:45.773] cur: x=-0.017540, y=-0.401403, z=0.915734 ref: x=-0.028577 y=-0.369045 z=0.928972 +[19:49:45.773] cur: x=-0.036653, y=-0.382847, z=0.923084 ref: x=-0.045820 y=-0.354195 z=0.934048 +[19:49:45.773] cur: x=-0.006145, y=-0.343789, z=0.939027 ref: x=-0.022916 y=-0.313819 z=0.949206 +[19:49:45.773] cur: x=-0.069937, y=-0.347827, z=0.934947 ref: x=-0.083259 y=-0.320054 z=0.943734 +[19:49:45.773] cur: x=-0.200610, y=-0.239941, z=0.949834 ref: x=-0.212743 y=-0.237814 z=0.947726 +[19:49:45.773] cur: x=-0.197790, y=-0.240019, z=0.950405 ref: x=-0.209888 y=-0.237838 z=0.948357 +[19:49:45.773] cur: x=-0.195097, y=-0.237990, z=0.951472 ref: x=-0.207049 y=-0.235711 z=0.949511 +[19:49:45.773] cur: x=-0.186897, y=-0.229583, z=0.955176 ref: x=-0.198538 y=-0.227168 z=0.953403 +[19:49:45.773] cur: x=-0.183997, y=-0.229593, z=0.955737 ref: x=-0.195677 y=-0.227188 z=0.953990 +[19:49:45.773] cur: x=-0.186860, y=-0.231744, z=0.954661 ref: x=-0.198522 y=-0.229321 z=0.952891 +[19:49:45.773] cur: x=-0.192383, y=-0.235928, z=0.952537 ref: x=-0.204208 y=-0.233582 z=0.950652 +[19:49:45.773] cur: x=-0.186793, y=-0.233890, z=0.954151 ref: x=-0.198506 y=-0.231473 z=0.952374 +[19:49:45.773] cur: x=-0.172873, y=-0.222504, z=0.959483 ref: x=-0.181415 y=-0.218663 z=0.958788 +[19:49:45.773] cur: x=-0.299411, y=-0.192767, z=0.934448 ref: x=-0.289722 y=-0.213448 z=0.933006 +[19:49:45.773] cur: x=-0.144274, y=-0.205455, z=0.967974 ref: x=-0.152805 y=-0.201543 z=0.967487 +[19:49:45.773] cur: x=-0.354798, y=-0.128731, z=0.926038 ref: x=-0.346836 y=-0.135421 z=0.928098 +[19:49:45.773] cur: x=-0.161214, y=-0.053295, z=0.985479 ref: x=-0.176271 y=-0.062410 z=0.982361 +[19:49:45.773] cur: x=-0.158359, y=-0.053306, z=0.985942 ref: x=-0.173393 y=-0.062415 z=0.982873 +[19:49:45.773] cur: x=-0.146799, y=-0.046900, z=0.988054 ref: x=-0.161882 y=-0.055891 z=0.985226 +[19:49:45.773] cur: x=-0.373196, y=0.032997, z=0.927166 ref: x=-0.358532 y=-0.053273 z=0.931996 +[19:49:45.773] cur: x=-0.369685, y=0.030356, z=0.928661 ref: x=-0.355725 y=-0.053282 z=0.933071 +[19:49:45.773] cur: x=-0.386908, y=0.093393, z=0.917377 ref: x=-0.369771 y=-0.044588 z=0.928053 +[19:49:45.773] cur: x=-0.384791, y=0.095361, z=0.918064 ref: x=-0.366970 y=-0.044596 z=0.929163 +[19:49:45.773] cur: x=-0.377989, y=0.075691, z=0.922711 ref: x=-0.361364 y=-0.044612 z=0.931357 +[19:49:45.773] cur: x=-0.050523, y=-0.162179, z=0.985467 ref: x=-0.072327 y=-0.045053 z=0.996363 +[19:49:45.773] cur: x=-0.371974, y=0.084594, z=0.924381 ref: x=-0.355762 y=-0.040299 z=0.933707 +[19:49:45.773] cur: x=-0.155724, y=-0.029402, z=0.987363 ref: x=-0.170544 y=-0.038433 z=0.984600 +[19:49:45.773] cur: x=-0.155778, y=-0.027217, z=0.987417 ref: x=-0.170546 y=-0.036252 z=0.984683 +[19:49:45.773] cur: x=-0.158701, y=-0.025026, z=0.987009 ref: x=-0.173427 y=-0.034068 z=0.984257 +[19:49:45.773] cur: x=-0.385178, y=0.050889, z=0.921438 ref: x=-0.375420 y=-0.018631 z=0.926667 +[19:49:45.773] cur: x=-0.376437, y=0.034834, z=0.925787 ref: x=-0.367026 y=-0.014315 z=0.930100 +[19:49:45.773] cur: x=-0.393274, y=0.179701, z=0.901689 ref: x=-0.378226 y=0.007314 z=0.925685 +[19:49:45.773] cur: x=-0.303352, y=0.017490, z=0.952718 ref: x=-0.358617 y=0.007323 z=0.933456 +[19:49:45.773] cur: x=-0.373148, y=0.015244, z=0.927647 ref: x=-0.367032 y=0.002992 z=0.930203 +[19:49:45.773] cur: x=-0.259718, y=0.020133, z=0.965475 ref: x=-0.262329 y=0.018235 z=0.964806 +[19:49:45.773] cur: x=-0.307368, y=0.110253, z=0.945182 ref: x=-0.369687 y=0.067856 z=0.926675 +[19:49:45.773] cur: x=-0.319723, y=0.104075, z=0.941778 ref: x=-0.383670 y=0.065634 z=0.921135 +[19:49:45.773] cur: x=-0.158833, y=0.081545, z=0.983932 ref: x=-0.170492 y=0.074987 z=0.982502 +[19:49:45.773] cur: x=-0.155962, y=0.081519, z=0.984394 ref: x=-0.167613 y=0.074992 z=0.982997 +[19:49:45.773] cur: x=-0.236684, y=0.088752, z=0.967524 ref: x=-0.259356 y=0.079114 z=0.962536 +[19:49:45.773] cur: x=-0.233829, y=0.088551, z=0.968237 ref: x=-0.256503 y=0.079123 z=0.963300 +[19:49:45.773] cur: x=-0.306917, y=0.154980, z=0.939033 ref: x=-0.372445 y=0.076482 z=0.924897 +[19:49:45.773] cur: x=-0.158924, y=0.084083, z=0.983704 ref: x=-0.170488 y=0.077166 z=0.982334 +[19:49:45.773] cur: x=-0.156050, y=0.084044, z=0.984167 ref: x=-0.167609 y=0.077171 z=0.982829 +[19:49:45.773] cur: x=-0.483595, y=-0.019601, z=0.875073 ref: x=-0.490642 y=0.092823 z=0.866403 +[19:49:45.773] cur: x=-0.308376, y=0.101965, z=0.945784 ref: x=-0.377719 y=0.123887 z=0.917595 +[19:49:45.773] cur: x=-0.321155, y=0.092274, z=0.942521 ref: x=-0.372153 y=0.121780 z=0.920148 +[19:49:45.773] cur: x=-0.032531, y=0.175038, z=0.984024 ref: x=-0.054871 y=0.151416 z=0.986946 +[19:49:45.773] cur: x=-0.047273, y=0.177186, z=0.983041 ref: x=-0.069325 y=0.153571 z=0.985703 +[19:49:45.773] cur: x=-0.035285, y=0.180902, z=0.982868 ref: x=-0.057755 y=0.157936 z=0.985759 +[19:49:45.773] cur: x=-0.049704, y=0.200912, z=0.978348 ref: x=-0.072174 y=0.177463 z=0.981477 +[19:49:45.773] cur: x=-0.046709, y=0.200996, z=0.978478 ref: x=-0.069285 y=0.177469 z=0.981684 +[19:49:45.773] cur: x=-0.043628, y=0.201018, z=0.978615 ref: x=-0.066396 y=0.177474 z=0.981883 +[19:49:45.773] cur: x=0.028225, y=0.213415, z=0.976554 ref: x=-0.002791 y=0.190552 z=0.981673 +[19:49:45.773] cur: x=0.006684, y=0.207219, z=0.978272 ref: x=-0.023031 y=0.184036 z=0.982650 +[19:49:45.773] cur: x=0.036850, y=0.208890, z=0.977245 ref: x=0.005883 y=0.186213 z=0.982492 +[19:49:45.773] cur: x=0.031137, y=0.215498, z=0.976008 ref: x=0.000100 y=0.192721 z=0.981254 +[19:49:45.773] cur: x=-0.052334, y=0.216692, z=0.974836 ref: x=-0.075031 y=0.192641 z=0.978397 +[19:49:45.773] cur: x=-0.553634, y=0.287933, z=0.781399 ref: x=-0.514089 y=0.261491 z=0.816906 +[19:49:45.773] cur: x=0.213305, y=0.294603, z=0.931510 ref: x=0.158330 y=0.272080 z=0.949160 +[19:49:45.773] cur: x=0.216193, y=0.294690, z=0.930816 ref: x=0.161195 y=0.272059 z=0.948683 +[19:49:45.773] cur: x=0.112574, y=0.361609, z=0.925509 ref: x=0.074811 y=0.328112 z=0.941672 +[19:49:45.773] cur: x=0.115567, y=0.361786, z=0.925070 ref: x=0.077682 y=0.328100 z=0.941444 +[19:49:45.773] cur: x=0.115082, y=0.363703, z=0.924379 ref: x=0.077673 y=0.330228 z=0.940700 +[19:49:45.773] cur: x=-0.255940, y=0.378555, z=0.889489 ref: x=-0.256887 y=0.343274 z=0.903423 +[19:49:45.773] cur: x=0.238408, y=0.489493, z=0.838784 ref: x=0.157216 y=0.392984 z=0.906006 +[19:49:45.773] cur: x=0.220530, y=0.472537, z=0.853273 ref: x=0.160034 y=0.395045 z=0.904615 +[19:49:45.773] cur: x=0.163779, y=-0.505316, z=0.847250 ref: x=0.091127 y=-0.462635 z=0.881853 +[19:49:45.773] cur: x=0.158019, y=-0.484213, z=0.860563 ref: x=0.079911 y=-0.442082 z=0.893408 +[19:49:45.773] cur: x=0.163749, y=-0.501748, z=0.849373 ref: x=0.091161 y=-0.458519 z=0.883996 +[19:49:45.773] cur: x=0.010865, y=-0.436437, z=0.899669 ref: x=-0.005626 y=-0.396448 z=0.918040 +[19:49:45.773] cur: x=0.022121, y=-0.445130, z=0.895192 ref: x=0.005821 y=-0.404835 z=0.914371 +[19:49:45.773] cur: x=0.022209, y=-0.440811, z=0.897325 ref: x=0.005823 y=-0.400644 z=0.916215 +[19:49:45.773] cur: x=-0.017412, y=-0.408446, z=0.912617 ref: x=-0.028565 y=-0.375378 z=0.926432 +[19:49:45.773] cur: x=-0.022024, y=-0.404718, z=0.914176 ref: x=-0.034306 y=-0.371145 z=0.927941 +[19:49:45.773] cur: x=-0.514150, y=-0.292984, z=0.806108 ref: x=-0.484343 y=-0.279612 z=0.828993 +[19:49:45.773] cur: x=-0.195286, y=-0.231567, z=0.953016 ref: x=-0.207099 y=-0.229256 z=0.951079 +[19:49:45.773] cur: x=-0.200700, y=-0.235662, z=0.950885 ref: x=-0.212779 y=-0.233514 z=0.948787 +[19:49:45.773] cur: x=-0.195145, y=-0.235850, z=0.951994 ref: x=-0.207066 y=-0.233560 z=0.950039 +[19:49:45.773] cur: x=-0.158608, y=-0.211923, z=0.964330 ref: x=-0.167136 y=-0.207954 z=0.963753 +[19:49:45.773] cur: x=-0.360231, y=-0.086978, z=0.928800 ref: x=-0.282264 y=0.026912 z=0.958959 +[19:49:45.773] cur: x=-0.465178, y=-0.015599, z=0.885080 ref: x=-0.471585 y=0.103697 z=0.875702 +[19:49:45.773] cur: x=-0.155962, y=0.081519, z=0.984394 ref: x=-0.167613 y=0.074992 z=0.982997 +[19:49:45.773] cur: x=-0.612149, y=0.240346, z=0.753331 ref: x=-0.570122 y=0.220076 z=0.791535 +[19:49:45.773] cur: x=0.216232, y=0.288421, z=0.932769 ref: x=0.161240 y=0.265625 z=0.950497 +[19:49:45.773] cur: x=-0.045701, y=0.298921, z=0.953183 ref: x=-0.069064 y=0.270374 z=0.960275 +[19:49:45.773] cur: x=-0.045658, y=0.294951, z=0.954421 ref: x=-0.069077 y=0.266075 z=0.961474 +[19:49:45.773] cur: x=-0.593503, y=0.271916, z=0.757506 ref: x=-0.548490 y=0.249926 z=0.797932 +[19:49:45.773] cur: x=-0.577790, y=0.277464, z=0.767576 ref: x=-0.532699 y=0.254627 z=0.807092 +[19:49:45.773] cur: x=0.216210, y=0.292609, z=0.931468 ref: x=0.161210 y=0.269915 z=0.949293 +[19:49:45.773] cur: x=0.130967, y=0.298989, z=0.945227 ref: x=0.086521 y=0.274611 z=0.957655 +[19:49:45.773] cur: x=-0.578669, y=0.339761, z=0.741421 ref: x=-0.571748 y=0.310689 z=0.759326 +[19:49:45.773] cur: x=-0.057796, y=0.336856, z=0.939781 ref: x=-0.080452 y=0.304625 z=0.949068 +[19:49:45.773] cur: x=0.112099, y=0.363545, z=0.924807 ref: x=0.074802 y=0.330240 z=0.940928 +[19:49:45.773] cur: x=-0.069442, y=0.332778, z=0.940445 ref: x=-0.091965 y=0.300301 z=0.949401 +[19:49:45.773] cur: x=0.113063, y=0.359763, z=0.926168 ref: x=0.074820 y=0.325983 z=0.942410 +[19:49:45.773] cur: x=-0.250657, y=0.381379, z=0.889787 ref: x=-0.251209 y=0.345473 z=0.904181 +[19:49:45.773] cur: x=-0.276897, y=0.409150, z=0.869439 ref: x=-0.267739 y=0.366186 z=0.891192 +[19:49:45.773] cur: x=-0.248086, y=0.413705, z=0.875958 ref: x=-0.383200 y=0.400471 z=0.832335 +[19:49:45.773] cur: x=-0.200934, y=-0.227075, z=0.952923 ref: x=-0.212847 y=-0.224907 z=0.950849 +[19:49:45.773] cur: x=0.198888, y=-0.505956, z=0.839316 ref: x=0.130735 y=-0.470439 z=0.872694 +[19:49:45.773] cur: x=-0.367861, y=-0.148199, z=0.917995 ref: x=-0.360677 y=-0.154673 z=0.919776 +[19:49:45.773] cur: x=-0.285626, y=-0.235459, z=0.928965 ref: x=-0.281023 y=-0.232853 z=0.931024 +[19:49:45.773] cur: x=-0.273731, y=-0.236417, z=0.932297 ref: x=-0.269694 y=-0.232978 z=0.934337 +[19:49:45.773] cur: x=-0.262168, y=-0.237083, z=0.935446 ref: x=-0.258345 y=-0.233097 z=0.937509 +[19:49:45.773] cur: x=-0.262111, y=-0.245656, z=0.933247 ref: x=-0.258255 y=-0.241678 z=0.935359 +[19:49:45.773] cur: x=-0.170123, y=-0.211830, z=0.962386 ref: x=-0.178614 y=-0.207890 z=0.961706 +[19:49:45.773] cur: x=-0.184924, y=-0.067589, z=0.980426 ref: x=-0.202114 y=-0.077607 z=0.976282 +[19:49:45.773] cur: x=-0.175120, y=-0.068089, z=0.982190 ref: x=-0.190623 y=-0.077633 z=0.978589 +[19:49:45.773] cur: x=-0.357157, y=-0.090578, z=0.929642 ref: x=-0.282273 y=0.018219 z=0.959161 +[19:49:45.773] cur: x=-0.304990, y=0.015637, z=0.952227 ref: x=-0.394516 y=0.112984 z=0.911916 +[19:49:45.773] cur: x=-0.312224, y=0.094271, z=0.945320 ref: x=-0.383383 y=0.113074 z=0.916642 +[19:49:45.773] cur: x=-0.167881, y=0.096216, z=0.981101 ref: x=-0.179098 y=0.088039 z=0.979884 +[19:49:45.774] cur: x=-0.487485, y=-0.003562, z=0.873124 ref: x=-0.493234 y=0.103470 z=0.863721 +[19:49:45.774] cur: x=-0.159534, y=0.265114, z=0.950928 ref: x=-0.178381 y=0.244024 z=0.953222 +[19:49:45.774] cur: x=-0.629839, y=0.313260, z=0.710754 ref: x=-0.642545 y=0.282517 z=0.712264 +[19:49:45.774] cur: x=0.000816, y=0.198407, z=0.980119 ref: x=-0.028820 y=0.175349 z=0.984084 +[19:49:45.774] cur: x=-0.495044, y=-0.013359, z=0.868765 ref: x=-0.504080 y=0.094823 z=0.858436 +[19:49:45.774] cur: x=-0.159626, y=0.275411, z=0.947981 ref: x=-0.178319 y=0.252622 z=0.950991 +[19:49:45.774] cur: x=0.210595, y=0.275270, z=0.938017 ref: x=0.155592 y=0.252776 z=0.954932 +[19:49:45.774] cur: x=0.102568, y=0.458438, z=0.882788 ref: x=0.119921 y=0.422593 z=0.898351 +[19:49:45.774] Init finished: Homography RANSAC (inlier) is:28 ,While 25 inliers minimum required. px average disparity is:20.342007 ,While minimum is: 20.000000 KLT tracked: 128 +[19:49:45.774] Init: distance between the first and current frame is x:0.013655 ,z=0.285386,angle between two frames: -0.012099 +[19:49:45.774] Init: Selected Second frame. The number of features: 27 depth mean:0.539896 min:-1.519440 +[19:49:45.774] frame process finished the id is: 72 the obs is:27 +[19:49:45.855] Odometry x=0.013655, y=0.285386, theta=-0.012099 +[19:49:45.954] After Reprojection Map nPoint:20 ,nCell:20 ,nMatches:1 ,distance between ekf and vo x:0.000310 ,z=0.000306,angle between two frames:0.000128 +[19:49:45.954] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.052903 error end=0.052903 +[19:49:45.954] After pose optimization, distance between ekf and vo x:0.000310 ,z=0.000306,angle between two frames:0.000128 +[19:49:45.954] frame process finished the id is: 72 the obs is:27 +[19:49:45.957] Odometry x=0.013701, y=0.290745, theta=-0.009176 +[19:49:46.055] Odometry x=0.013753, y=0.296979, theta=-0.014349 +[19:49:46.106] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:0.000201 ,z=0.000218,angle between two frames:0.000057 +[19:49:46.106] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.443223 error end=0.091135 +[19:49:46.106] After pose optimization, distance between ekf and vo x:0.067910 ,z=-0.355097,angle between two frames:0.310998 +[19:49:46.106] frame process finished the id is: 72 the obs is:27 +[19:49:46.152] Odometry x=0.013803, y=0.302593, theta=-0.015931 +[19:49:46.250] After Reprojection Map nPoint:22 ,nCell:22 ,nMatches:1 ,distance between ekf and vo x:-0.000045 ,z=0.000041,angle between two frames:0.000012 +[19:49:46.250] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.283810 error end=0.283810 +[19:49:46.250] After pose optimization, distance between ekf and vo x:-0.000045 ,z=0.000041,angle between two frames:0.000012 +[19:49:46.250] frame process finished the id is: 72 the obs is:27 +[19:49:46.254] Odometry x=0.007529, y=0.309465, theta=-0.003639 +[19:49:46.351] Odometry x=0.007550, y=0.314819, theta=-0.007610 +[19:49:46.394] After Reprojection Map nPoint:21 ,nCell:21 ,nMatches:2 ,distance between ekf and vo x:-0.000051 ,z=0.000042,angle between two frames:0.000018 +[19:49:46.394] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.160618 error end=0.118608 +[19:49:46.394] After pose optimization, distance between ekf and vo x:-0.018617 ,z=0.013437,angle between two frames:0.010789 +[19:49:46.394] frame process finished the id is: 72 the obs is:27 +[19:49:46.456] Odometry x=0.007568, y=0.321012, theta=0.003553 +[19:49:46.545] After Reprojection Map nPoint:22 ,nCell:21 ,nMatches:4 ,distance between ekf and vo x:0.000096 ,z=0.000010,angle between two frames:0.000008 +[19:49:46.545] n deleted obs = 0 n obs with reprojection error less than 0.5 px =4 error init =0.118594 error end=0.113851 +[19:49:46.545] After pose optimization, distance between ekf and vo x:-0.004467 ,z=-0.000569,angle between two frames:0.004326 +[19:49:46.545] frame process finished the id is: 72 the obs is:27 +[19:49:46.558] Odometry x=0.007625, y=0.326434, theta=-0.004443 +[19:49:46.655] Odometry x=0.007663, y=0.333651, theta=0.006665 +[19:49:46.689] After Reprojection Map nPoint:23 ,nCell:23 ,nMatches:4 ,distance between ekf and vo x:-0.000078 ,z=0.000033,angle between two frames:0.000029 +[19:49:46.689] n deleted obs = 1 n obs with reprojection error less than 0.5 px =3 error init =0.220460 error end=0.210470 +[19:49:46.689] After pose optimization, distance between ekf and vo x:-0.061447 ,z=-0.027524,angle between two frames:0.030177 +[19:49:46.689] frame process finished the id is: 72 the obs is:27 +[19:49:46.753] Odometry x=0.007699, y=0.339039, theta=-0.009782 +[19:49:46.831] After Reprojection Map nPoint:21 ,nCell:20 ,nMatches:1 ,distance between ekf and vo x:-0.000054 ,z=0.000044,angle between two frames:0.000014 +[19:49:46.831] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.118916 error end=0.107877 +[19:49:46.831] After pose optimization, distance between ekf and vo x:-0.006002 ,z=-0.009901,angle between two frames:0.004382 +[19:49:46.831] frame process finished the id is: 72 the obs is:27 +[19:49:46.851] Odometry x=0.008665, y=0.384703, theta=0.029010 +[19:49:46.953] Odometry x=0.008683, y=0.390007, theta=0.000288 +[19:49:46.990] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:2 ,distance between ekf and vo x:0.000433 ,z=0.000084,angle between two frames:0.000077 +[19:49:46.990] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.153854 error end=0.153854 +[19:49:46.990] After pose optimization, distance between ekf and vo x:0.000433 ,z=0.000084,angle between two frames:0.000077 +[19:49:46.990] frame process finished the id is: 72 the obs is:27 +[19:49:47.055] Odometry x=-0.003151, y=0.397040, theta=0.036708 +[19:49:47.148] After Reprojection Map nPoint:22 ,nCell:22 ,nMatches:2 ,distance between ekf and vo x:0.000173 ,z=0.000157,angle between two frames:0.000004 +[19:49:47.149] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.151370 error end=0.151370 +[19:49:47.149] After pose optimization, distance between ekf and vo x:0.000173 ,z=0.000157,angle between two frames:0.000004 +[19:49:47.149] frame process finished the id is: 72 the obs is:27 +[19:49:47.157] Odometry x=0.000099, y=0.400603, theta=0.015345 +[19:49:47.256] Odometry x=0.000483, y=0.408753, theta=0.013666 +[19:49:47.299] After Reprojection Map nPoint:19 ,nCell:18 ,nMatches:2 ,distance between ekf and vo x:0.000079 ,z=0.000104,angle between two frames:0.000027 +[19:49:47.299] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.197063 error end=0.194166 +[19:49:47.299] After pose optimization, distance between ekf and vo x:0.016758 ,z=-0.006970,angle between two frames:0.017103 +[19:49:47.299] frame process finished the id is: 72 the obs is:27 +[19:49:47.354] Odometry x=0.002293, y=0.413454, theta=0.026987 +[19:49:47.450] After Reprojection Map nPoint:21 ,nCell:16 ,nMatches:3 ,distance between ekf and vo x:0.000138 ,z=0.000127,angle between two frames:0.000048 +[19:49:47.450] n deleted obs = 0 n obs with reprojection error less than 0.5 px =3 error init =0.251135 error end=0.214024 +[19:49:47.450] After pose optimization, distance between ekf and vo x:-0.029642 ,z=-0.092257,angle between two frames:0.023072 +[19:49:47.450] frame process finished the id is: 72 the obs is:27 +[19:49:47.458] Odometry x=0.002309, y=0.422756, theta=0.006636 +[19:49:47.555] Odometry x=0.002300, y=0.428297, theta=0.002632 +[19:49:47.594] After Reprojection Map nPoint:21 ,nCell:20 ,nMatches:3 ,distance between ekf and vo x:-0.000103 ,z=0.000071,angle between two frames:0.000034 +[19:49:47.594] n deleted obs = 0 n obs with reprojection error less than 0.5 px =3 error init =0.162899 error end=0.162220 +[19:49:47.594] After pose optimization, distance between ekf and vo x:-0.003368 ,z=-0.001571,angle between two frames:0.003026 +[19:49:47.594] frame process finished the id is: 72 the obs is:27 +[19:49:47.654] Odometry x=0.002595, y=0.436219, theta=0.006295 +[19:49:47.739] After Reprojection Map nPoint:18 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:-0.000083 ,z=0.000039,angle between two frames:0.000030 +[19:49:47.739] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.262343 error end=0.262343 +[19:49:47.739] After pose optimization, distance between ekf and vo x:-0.000083 ,z=0.000039,angle between two frames:0.000030 +[19:49:47.739] frame process finished the id is: 72 the obs is:27 +[19:49:47.758] Odometry x=0.002627, y=0.439225, theta=-0.011812 +[19:49:47.857] Odometry x=0.002705, y=0.445883, theta=0.000127 +[19:49:47.892] After Reprojection Map nPoint:24 ,nCell:23 ,nMatches:3 ,distance between ekf and vo x:0.000017 ,z=0.000342,angle between two frames:0.000077 +[19:49:47.892] n deleted obs = 0 n obs with reprojection error less than 0.5 px =3 error init =0.116559 error end=0.116559 +[19:49:47.892] After pose optimization, distance between ekf and vo x:0.000017 ,z=0.000342,angle between two frames:0.000077 +[19:49:47.892] frame process finished the id is: 72 the obs is:27 +[19:49:47.958] Odometry x=0.003177, y=0.449928, theta=-0.010241 +[19:49:48.035] After Reprojection Map nPoint:21 ,nCell:21 ,nMatches:1 ,distance between ekf and vo x:-0.000063 ,z=0.000043,angle between two frames:0.000019 +[19:49:48.035] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.373524 error end=0.373524 +[19:49:48.035] After pose optimization, distance between ekf and vo x:-0.000063 ,z=0.000043,angle between two frames:0.000019 +[19:49:48.035] frame process finished the id is: 72 the obs is:27 +[19:49:48.055] Odometry x=0.003195, y=0.455075, theta=0.002417 +[19:49:48.153] Odometry x=0.002836, y=0.472874, theta=0.016623 +[19:49:48.185] After Reprojection Map nPoint:19 ,nCell:19 ,nMatches:1 ,distance between ekf and vo x:-0.000055 ,z=0.000195,angle between two frames:0.000012 +[19:49:48.185] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.241648 error end=0.241648 +[19:49:48.185] After pose optimization, distance between ekf and vo x:-0.000055 ,z=0.000195,angle between two frames:0.000012 +[19:49:48.185] frame process finished the id is: 72 the obs is:27 +[19:49:48.258] Odometry x=0.002789, y=0.479177, theta=0.005595 +[19:49:48.335] After Reprojection Map nPoint:20 ,nCell:20 ,nMatches:4 ,distance between ekf and vo x:0.000012 ,z=0.000054,angle between two frames:0.000012 +[19:49:48.335] n deleted obs = 0 n obs with reprojection error less than 0.5 px =4 error init =0.188749 error end=0.188749 +[19:49:48.335] After pose optimization, distance between ekf and vo x:0.000012 ,z=0.000054,angle between two frames:0.000012 +[19:49:48.335] frame process finished the id is: 72 the obs is:27 +[19:49:48.357] Odometry x=0.002261, y=0.507988, theta=0.018527 +[19:49:48.454] Odometry x=0.002210, y=0.513368, theta=0.011816 +[19:49:48.486] After Reprojection Map nPoint:15 ,nCell:14 ,nMatches:1 ,distance between ekf and vo x:0.000037 ,z=0.000173,angle between two frames:0.000012 +[19:49:48.486] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.179614 error end=0.179614 +[19:49:48.486] After pose optimization, distance between ekf and vo x:0.000037 ,z=0.000173,angle between two frames:0.000012 +[19:49:48.486] frame process finished the id is: 72 the obs is:27 +[19:49:48.557] Odometry x=-0.006286, y=0.537190, theta=0.018735 +[19:49:48.637] After Reprojection Map nPoint:21 ,nCell:20 ,nMatches:4 ,distance between ekf and vo x:0.000196 ,z=0.000061,angle between two frames:0.000051 +[19:49:48.637] n deleted obs = 1 n obs with reprojection error less than 0.5 px =3 error init =0.426657 error end=0.426657 +[19:49:48.637] After pose optimization, distance between ekf and vo x:0.000196 ,z=0.000061,angle between two frames:0.000051 +[19:49:48.637] frame process finished the id is: 72 the obs is:27 +[19:49:48.657] Odometry x=-0.006333, y=0.542720, theta=0.014007 +[19:49:48.758] Odometry x=-0.006550, y=0.552487, theta=0.011133 +[19:49:48.788] After Reprojection Map nPoint:18 ,nCell:17 ,nMatches:3 ,distance between ekf and vo x:0.000072 ,z=0.000107,angle between two frames:0.000015 +[19:49:48.788] n deleted obs = 0 n obs with reprojection error less than 0.5 px =3 error init =0.268585 error end=0.266460 +[19:49:48.788] After pose optimization, distance between ekf and vo x:-0.010191 ,z=-0.014556,angle between two frames:0.010395 +[19:49:48.788] frame process finished the id is: 72 the obs is:27 +[19:49:48.856] Odometry x=-0.006525, y=0.557833, theta=0.009625 +[19:49:48.938] After Reprojection Map nPoint:18 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:0.000060 ,z=0.000034,angle between two frames:0.000000 +[19:49:48.938] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.802952 error end=0.802952 +[19:49:48.938] After pose optimization, distance between ekf and vo x:0.000060 ,z=0.000034,angle between two frames:0.000000 +[19:49:48.938] frame process finished the id is: 72 the obs is:27 +[19:49:48.958] Odometry x=-0.003940, y=0.573146, theta=0.026572 +[19:49:49.056] Odometry x=-0.004010, y=0.578529, theta=0.018747 +[19:49:49.091] After Reprojection Map nPoint:20 ,nCell:20 ,nMatches:2 ,distance between ekf and vo x:0.000022 ,z=0.000213,angle between two frames:0.000018 +[19:49:49.091] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.256116 error end=0.256116 +[19:49:49.091] After pose optimization, distance between ekf and vo x:0.000022 ,z=0.000213,angle between two frames:0.000018 +[19:49:49.091] frame process finished the id is: 72 the obs is:27 +[19:49:49.153] Odometry x=-0.003778, y=0.584458, theta=0.015439 +[19:49:49.242] After Reprojection Map nPoint:17 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.000051 ,z=0.000136,angle between two frames:0.000016 +[19:49:49.242] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.455785 error end=0.364537 +[19:49:49.242] After pose optimization, distance between ekf and vo x:-0.121775 ,z=2.549329,angle between two frames:0.027435 +[19:49:49.242] frame process finished the id is: 72 the obs is:27 +[19:49:49.254] Odometry x=-0.015327, y=0.594633, theta=0.015918 +[19:49:49.357] Odometry x=-0.015373, y=0.600339, theta=0.017161 +[19:49:49.393] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:0.000021 ,z=0.000099,angle between two frames:0.000000 +[19:49:49.393] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.724820 error end=0.249935 +[19:49:49.393] After pose optimization, distance between ekf and vo x:-1.042707 ,z=8.149840,angle between two frames:0.836243 +[19:49:49.393] frame process finished the id is: 72 the obs is:27 +[19:49:49.455] Odometry x=-0.017667, y=0.605717, theta=0.003315 +[19:49:49.543] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:1 ,distance between ekf and vo x:0.000033 ,z=-0.000032,angle between two frames:0.000017 +[19:49:49.543] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.339843 error end=0.128671 +[19:49:49.543] After pose optimization, distance between ekf and vo x:0.036549 ,z=-0.434091,angle between two frames:0.004374 +[19:49:49.543] frame process finished the id is: 72 the obs is:27 +[19:49:49.552] Odometry x=-0.017548, y=0.611009, theta=0.008918 +[19:49:49.655] Odometry x=-0.017657, y=0.619476, theta=0.002669 +[19:49:49.695] After Reprojection Map nPoint:20 ,nCell:20 ,nMatches:2 ,distance between ekf and vo x:0.000018 ,z=0.000040,angle between two frames:0.000014 +[19:49:49.696] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.705111 error end=0.444439 +[19:49:49.696] After pose optimization, distance between ekf and vo x:-1.695322 ,z=2.057304,angle between two frames:0.473808 +[19:49:49.696] frame process finished the id is: 72 the obs is:27 +[19:49:49.753] Odometry x=-0.017659, y=0.625040, theta=0.005140 +[19:49:49.847] After Reprojection Map nPoint:21 ,nCell:20 ,nMatches:3 ,distance between ekf and vo x:-0.000023 ,z=0.000171,angle between two frames:0.000003 +[19:49:49.847] n deleted obs = 1 n obs with reprojection error less than 0.5 px =2 error init =0.476875 error end=0.431132 +[19:49:49.847] After pose optimization, distance between ekf and vo x:-0.022702 ,z=-0.274088,angle between two frames:0.069498 +[19:49:49.847] frame process finished the id is: 72 the obs is:27 +[19:49:49.854] Odometry x=-0.017671, y=0.630268, theta=0.007802 +[19:49:49.956] Odometry x=-0.016914, y=0.640189, theta=0.003419 +[19:49:49.999] After Reprojection Map nPoint:20 ,nCell:19 ,nMatches:2 ,distance between ekf and vo x:-0.000061 ,z=0.000188,angle between two frames:0.000015 +[19:49:49.999] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.554193 error end=0.322778 +[19:49:49.999] After pose optimization, distance between ekf and vo x:-2.030066 ,z=4.739137,angle between two frames:0.074690 +[19:49:49.999] frame process finished the id is: 72 the obs is:27 +[19:49:50.053] Odometry x=-0.017029, y=0.628146, theta=0.017469 +[19:49:50.149] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:4 ,distance between ekf and vo x:0.000092 ,z=0.000110,angle between two frames:0.000028 +[19:49:50.149] n deleted obs = 1 n obs with reprojection error less than 0.5 px =3 error init =0.408862 error end=0.408862 +[19:49:50.149] After pose optimization, distance between ekf and vo x:0.000092 ,z=0.000110,angle between two frames:0.000028 +[19:49:50.149] frame process finished the id is: 72 the obs is:27 +[19:49:50.156] Odometry x=-0.016906, y=0.635434, theta=0.007804 +[19:49:50.253] Odometry x=-0.016997, y=0.633037, theta=0.028908 +[19:49:50.300] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:0.000073 ,z=0.000145,angle between two frames:0.000029 +[19:49:50.300] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.425589 error end=0.138752 +[19:49:50.300] After pose optimization, distance between ekf and vo x:-0.013553 ,z=-0.517543,angle between two frames:0.052930 +[19:49:50.300] frame process finished the id is: 72 the obs is:27 +[19:49:50.356] Odometry x=-0.015466, y=0.639691, theta=0.022624 +[19:49:50.453] Odometry x=-0.015598, y=0.659318, theta=0.034846 +[19:49:50.453] After Reprojection Map nPoint:16 ,nCell:15 ,nMatches:3 ,distance between ekf and vo x:0.000221 ,z=-0.000003,angle between two frames:0.000037 +[19:49:50.453] n deleted obs = 0 n obs with reprojection error less than 0.5 px =3 error init =0.382441 error end=0.220354 +[19:49:50.453] After pose optimization, distance between ekf and vo x:-0.428724 ,z=2.893013,angle between two frames:0.006661 +[19:49:50.453] frame process finished the id is: 72 the obs is:27 +[19:49:50.555] Odometry x=-0.014616, y=0.665668, theta=0.026435 +[19:49:50.605] After Reprojection Map nPoint:16 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:0.000225 ,z=0.000110,angle between two frames:0.000076 +[19:49:50.605] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.367576 error end=0.033998 +[19:49:50.605] After pose optimization, distance between ekf and vo x:-0.009605 ,z=-0.203432,angle between two frames:0.015899 +[19:49:50.605] frame process finished the id is: 72 the obs is:27 +[19:49:50.653] Odometry x=-0.017594, y=0.668891, theta=0.014148 +[19:49:50.750] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:0 ,distance between ekf and vo x:-0.000006 ,z=0.000001,angle between two frames:0.000010 +[19:49:50.750] After pose optimization, distance between ekf and vo x:-0.000006 ,z=0.000001,angle between two frames:0.000010 +[19:49:50.750] frame process finished the id is: 72 the obs is:27 +[19:49:50.756] Odometry x=-0.017037, y=0.674913, theta=0.027062 +[19:49:50.853] Odometry x=-0.017401, y=0.681834, theta=-0.000568 +[19:49:50.902] After Reprojection Map nPoint:16 ,nCell:14 ,nMatches:2 ,distance between ekf and vo x:0.000024 ,z=0.000198,angle between two frames:0.000016 +[19:49:50.902] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.382209 error end=0.382209 +[19:49:50.902] After pose optimization, distance between ekf and vo x:0.000024 ,z=0.000198,angle between two frames:0.000016 +[19:49:50.902] frame process finished the id is: 72 the obs is:27 +[19:49:50.955] Odometry x=-0.017234, y=0.687468, theta=0.013077 +[19:49:51.052] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:0 ,distance between ekf and vo x:0.000049 ,z=0.000056,angle between two frames:0.000003 +[19:49:51.052] After pose optimization, distance between ekf and vo x:0.000049 ,z=0.000056,angle between two frames:0.000003 +[19:49:51.052] frame process finished the id is: 72 the obs is:27 +[19:49:51.052] Odometry x=-0.017302, y=0.692822, theta=0.001265 +[19:49:51.155] Odometry x=-0.016943, y=0.698293, theta=0.010050 +[19:49:51.202] After Reprojection Map nPoint:17 ,nCell:16 ,nMatches:4 ,distance between ekf and vo x:-0.000179 ,z=0.000218,angle between two frames:0.000060 +[19:49:51.202] n deleted obs = 2 n obs with reprojection error less than 0.5 px =2 error init =0.592939 error end=0.592939 +[19:49:51.202] After pose optimization, distance between ekf and vo x:-0.000179 ,z=0.000218,angle between two frames:0.000060 +[19:49:51.202] frame process finished the id is: 72 the obs is:27 +[19:49:51.257] Odometry x=-0.021954, y=0.706712, theta=0.001911 +[19:49:51.344] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:0 ,distance between ekf and vo x:-0.000077 ,z=0.000034,angle between two frames:0.000032 +[19:49:51.344] After pose optimization, distance between ekf and vo x:-0.000077 ,z=0.000034,angle between two frames:0.000032 +[19:49:51.344] frame process finished the id is: 72 the obs is:27 +[19:49:51.355] Odometry x=-0.021832, y=0.712312, theta=0.002802 +[19:49:51.458] Odometry x=-0.021894, y=0.718579, theta=0.028073 +[19:49:51.486] After Reprojection Map nPoint:14 ,nCell:14 ,nMatches:1 ,distance between ekf and vo x:-0.000025 ,z=0.000021,angle between two frames:0.000010 +[19:49:51.486] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.409903 error end=0.409903 +[19:49:51.486] After pose optimization, distance between ekf and vo x:-0.000025 ,z=0.000021,angle between two frames:0.000010 +[19:49:51.486] frame process finished the id is: 72 the obs is:27 +[19:49:51.555] Odometry x=-0.021670, y=0.724132, theta=0.009229 +[19:49:51.636] After Reprojection Map nPoint:13 ,nCell:13 ,nMatches:0 ,distance between ekf and vo x:0.000053 ,z=0.000004,angle between two frames:0.000000 +[19:49:51.636] After pose optimization, distance between ekf and vo x:0.000053 ,z=0.000004,angle between two frames:0.000000 +[19:49:51.636] frame process finished the id is: 72 the obs is:27 +[19:49:51.652] Odometry x=-0.021683, y=0.729547, theta=0.032153 +[19:49:51.750] Odometry x=-0.021711, y=0.734648, theta=0.016412 +[19:49:51.782] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.000071 ,z=-0.000014,angle between two frames:0.000004 +[19:49:51.782] n deleted obs = 2 n obs with reprojection error less than 0.5 px =0 error init =7.174252 error end=1.003096 +[19:49:51.782] After pose optimization, distance between ekf and vo x:545.544249 ,z=-139.357909,angle between two frames:2.396184 +[19:49:51.782] frame process finished the id is: 72 the obs is:27 +[19:49:51.852] Odometry x=-0.018375, y=0.740400, theta=0.030802 +[19:49:51.932] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:0 ,distance between ekf and vo x:0.000166 ,z=0.000010,angle between two frames:0.000039 +[19:49:51.932] After pose optimization, distance between ekf and vo x:0.000166 ,z=0.000010,angle between two frames:0.000039 +[19:49:51.932] frame process finished the id is: 72 the obs is:27 +[19:49:51.959] Odometry x=-0.018486, y=0.745792, theta=0.026245 +[19:49:52.056] Odometry x=-0.017777, y=0.752216, theta=0.023784 +[19:49:52.082] After Reprojection Map nPoint:19 ,nCell:19 ,nMatches:1 ,distance between ekf and vo x:0.000204 ,z=0.000026,angle between two frames:0.000055 +[19:49:52.082] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =2.536590 error end=0.381538 +[19:49:52.082] After pose optimization, distance between ekf and vo x:-0.001416 ,z=0.091031,angle between two frames:0.001186 +[19:49:52.082] frame process finished the id is: 72 the obs is:27 +[19:49:52.157] Odometry x=-0.017845, y=0.757246, theta=0.033524 +[19:49:52.232] After Reprojection Map nPoint:21 ,nCell:20 ,nMatches:1 ,distance between ekf and vo x:0.000205 ,z=0.000029,angle between two frames:0.000056 +[19:49:52.232] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.362567 error end=0.347962 +[19:49:52.232] After pose optimization, distance between ekf and vo x:-0.014421 ,z=0.386461,angle between two frames:0.016739 +[19:49:52.232] frame process finished the id is: 72 the obs is:27 +[19:49:52.255] Odometry x=-0.017179, y=0.763191, theta=0.030602 +[19:49:52.354] Odometry x=-0.025140, y=0.768270, theta=0.016118 +[19:49:52.382] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:0 ,distance between ekf and vo x:0.000261 ,z=0.000010,angle between two frames:0.000073 +[19:49:52.382] After pose optimization, distance between ekf and vo x:0.000261 ,z=0.000010,angle between two frames:0.000073 +[19:49:52.382] frame process finished the id is: 72 the obs is:27 +[19:49:52.453] Odometry x=-0.024370, y=0.773562, theta=0.028232 +[19:49:52.532] After Reprojection Map nPoint:19 ,nCell:17 ,nMatches:1 ,distance between ekf and vo x:0.000245 ,z=-0.000028,angle between two frames:0.000059 +[19:49:52.532] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.154957 error end=0.070931 +[19:49:52.532] After pose optimization, distance between ekf and vo x:0.015338 ,z=0.499102,angle between two frames:0.041146 +[19:49:52.532] frame process finished the id is: 72 the obs is:27 +[19:49:52.553] Odometry x=-0.023847, y=0.782810, theta=0.017769 +[19:49:52.655] Odometry x=-0.023470, y=0.788279, theta=0.019453 +[19:49:52.682] After Reprojection Map nPoint:19 ,nCell:19 ,nMatches:0 ,distance between ekf and vo x:0.000216 ,z=-0.000078,angle between two frames:0.000049 +[19:49:52.682] After pose optimization, distance between ekf and vo x:0.000216 ,z=-0.000078,angle between two frames:0.000049 +[19:49:52.682] frame process finished the id is: 72 the obs is:27 +[19:49:52.753] Odometry x=-0.025280, y=0.795196, theta=0.011800 +[19:49:52.841] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:0.000258 ,z=-0.000307,angle between two frames:0.000023 +[19:49:52.841] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.177436 error end=0.177436 +[19:49:52.841] After pose optimization, distance between ekf and vo x:0.000258 ,z=-0.000307,angle between two frames:0.000023 +[19:49:52.841] frame process finished the id is: 72 the obs is:27 +[19:49:52.856] Odometry x=-0.023985, y=0.800621, theta=0.022844 +[19:49:52.954] Odometry x=-0.023927, y=0.806007, theta=0.019700 +[19:49:53.004] After Reprojection Map nPoint:18 ,nCell:17 ,nMatches:1 ,distance between ekf and vo x:0.000365 ,z=-0.000169,angle between two frames:0.000001 +[19:49:53.004] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.493802 error end=0.104257 +[19:49:53.004] After pose optimization, distance between ekf and vo x:0.021024 ,z=0.271454,angle between two frames:0.000896 +[19:49:53.004] frame process finished the id is: 72 the obs is:27 +[19:49:53.056] Odometry x=-0.023799, y=0.811445, theta=0.010710 +[19:49:53.153] Odometry x=-0.025994, y=0.816823, theta=0.021560 +[19:49:53.169] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:1 ,distance between ekf and vo x:0.000614 ,z=-0.000391,angle between two frames:0.000036 +[19:49:53.169] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.206024 error end=0.198080 +[19:49:53.169] After pose optimization, distance between ekf and vo x:0.006588 ,z=0.105802,angle between two frames:0.015776 +[19:49:53.169] frame process finished the id is: 72 the obs is:27 +[19:49:53.251] Odometry x=-0.025976, y=0.822030, theta=0.011961 +[19:49:53.334] After Reprojection Map nPoint:19 ,nCell:18 ,nMatches:2 ,distance between ekf and vo x:0.000449 ,z=-0.000510,angle between two frames:0.000063 +[19:49:53.334] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.300817 error end=0.300817 +[19:49:53.334] After pose optimization, distance between ekf and vo x:0.000449 ,z=-0.000510,angle between two frames:0.000063 +[19:49:53.334] frame process finished the id is: 72 the obs is:27 +[19:49:53.353] Odometry x=-0.025623, y=0.827513, theta=0.038765 +[19:49:53.456] Odometry x=-0.025693, y=0.832731, theta=0.019658 +[19:49:53.492] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:0 ,distance between ekf and vo x:0.000476 ,z=-0.000229,angle between two frames:0.000039 +[19:49:53.492] After pose optimization, distance between ekf and vo x:0.000476 ,z=-0.000229,angle between two frames:0.000039 +[19:49:53.492] frame process finished the id is: 72 the obs is:27 +[19:49:53.554] Odometry x=-0.025760, y=0.838274, theta=0.037208 +[19:49:53.650] After Reprojection Map nPoint:18 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:0.000449 ,z=-0.000258,angle between two frames:0.000025 +[19:49:53.650] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.479446 error end=0.276925 +[19:49:53.650] After pose optimization, distance between ekf and vo x:0.443190 ,z=3.521574,angle between two frames:0.342846 +[19:49:53.650] frame process finished the id is: 72 the obs is:27 +[19:49:53.656] Odometry x=-0.026035, y=0.843986, theta=0.032994 +[19:49:53.753] Odometry x=-0.026116, y=0.849411, theta=0.030473 +[19:49:53.800] After Reprojection Map nPoint:18 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.000292 ,z=-0.000084,angle between two frames:0.000075 +[19:49:53.800] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.103435 error end=0.103435 +[19:49:53.800] After pose optimization, distance between ekf and vo x:0.000292 ,z=-0.000084,angle between two frames:0.000075 +[19:49:53.800] frame process finished the id is: 72 the obs is:27 +[19:49:53.856] Odometry x=-0.026195, y=0.855026, theta=0.042028 +[19:49:53.953] Odometry x=-0.026301, y=0.860377, theta=0.034976 +[19:49:53.957] After Reprojection Map nPoint:19 ,nCell:19 ,nMatches:0 ,distance between ekf and vo x:0.000391 ,z=-0.000243,angle between two frames:0.000011 +[19:49:53.957] After pose optimization, distance between ekf and vo x:0.000391 ,z=-0.000243,angle between two frames:0.000011 +[19:49:53.957] frame process finished the id is: 72 the obs is:27 +[19:49:54.057] Odometry x=-0.026877, y=0.866151, theta=0.042783 +[19:49:54.114] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:0 ,distance between ekf and vo x:0.000550 ,z=-0.000308,angle between two frames:0.000054 +[19:49:54.114] After pose optimization, distance between ekf and vo x:0.000550 ,z=-0.000308,angle between two frames:0.000054 +[19:49:54.114] frame process finished the id is: 72 the obs is:27 +[19:49:54.154] Odometry x=-0.026985, y=0.871635, theta=0.032726 +[19:49:54.252] Odometry x=-0.027039, y=0.876782, theta=0.040437 +[19:49:54.270] After Reprojection Map nPoint:19 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:0.000544 ,z=-0.000248,angle between two frames:0.000078 +[19:49:54.270] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.227352 error end=0.225803 +[19:49:54.270] After pose optimization, distance between ekf and vo x:0.007806 ,z=0.028925,angle between two frames:0.004722 +[19:49:54.270] frame process finished the id is: 72 the obs is:27 +[19:49:54.359] Odometry x=-0.027150, y=0.882405, theta=0.033584 +[19:49:54.428] After Reprojection Map nPoint:19 ,nCell:19 ,nMatches:1 ,distance between ekf and vo x:0.000354 ,z=-0.000204,angle between two frames:0.000003 +[19:49:54.428] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.326664 error end=0.212022 +[19:49:54.428] After pose optimization, distance between ekf and vo x:-0.004205 ,z=0.136352,angle between two frames:0.001780 +[19:49:54.428] frame process finished the id is: 72 the obs is:27 +[19:49:54.453] Odometry x=-0.027438, y=0.889744, theta=0.037068 +[19:49:54.560] Odometry x=-0.027530, y=0.895103, theta=0.031829 +[19:49:54.578] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:1 ,distance between ekf and vo x:-0.000358 ,z=0.000063,angle between two frames:0.000197 +[19:49:54.578] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =0.683736 error end=0.683736 +[19:49:54.578] After pose optimization, distance between ekf and vo x:-0.000358 ,z=0.000063,angle between two frames:0.000197 +[19:49:54.578] frame process finished the id is: 72 the obs is:27 +[19:49:54.670] Odometry x=-0.027648, y=0.901687, theta=0.040126 +[19:49:54.722] After Reprojection Map nPoint:20 ,nCell:20 ,nMatches:2 ,distance between ekf and vo x:-0.000046 ,z=-0.000019,angle between two frames:0.000052 +[19:49:54.723] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.215134 error end=0.191315 +[19:49:54.723] After pose optimization, distance between ekf and vo x:-0.166685 ,z=0.222599,angle between two frames:0.022067 +[19:49:54.723] frame process finished the id is: 72 the obs is:27 +[19:49:54.753] Odometry x=-0.027757, y=0.907628, theta=0.031832 +[19:49:54.866] Odometry x=-0.027831, y=0.912615, theta=0.033196 +[19:49:54.875] After Reprojection Map nPoint:20 ,nCell:20 ,nMatches:1 ,distance between ekf and vo x:-0.000368 ,z=-0.000012,angle between two frames:0.000228 +[19:49:54.875] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.295084 error end=0.118679 +[19:49:54.875] After pose optimization, distance between ekf and vo x:1.109631 ,z=-0.413076,angle between two frames:0.181029 +[19:49:54.875] frame process finished the id is: 72 the obs is:27 +[19:49:54.959] Odometry x=-0.027922, y=0.918397, theta=0.031922 +[19:49:55.030] After Reprojection Map nPoint:19 ,nCell:19 ,nMatches:1 ,distance between ekf and vo x:-0.000323 ,z=-0.000081,angle between two frames:0.000253 +[19:49:55.030] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.242437 error end=0.242437 +[19:49:55.030] After pose optimization, distance between ekf and vo x:-0.000323 ,z=-0.000081,angle between two frames:0.000253 +[19:49:55.030] frame process finished the id is: 72 the obs is:27 +[19:49:55.063] Odometry x=-0.027995, y=0.923740, theta=0.034504 +[19:49:55.155] Odometry x=-0.028085, y=0.929215, theta=0.032299 +[19:49:55.180] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:0 ,distance between ekf and vo x:-0.000391 ,z=0.000007,angle between two frames:0.000346 +[19:49:55.180] After pose optimization, distance between ekf and vo x:-0.000391 ,z=0.000007,angle between two frames:0.000346 +[19:49:55.180] frame process finished the id is: 72 the obs is:27 +[19:49:55.254] Odometry x=-0.028158, y=0.934343, theta=0.027934 +[19:49:55.338] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:0 ,distance between ekf and vo x:-0.000914 ,z=0.000077,angle between two frames:0.000667 +[19:49:55.338] After pose optimization, distance between ekf and vo x:-0.000914 ,z=0.000077,angle between two frames:0.000667 +[19:49:55.338] frame process finished the id is: 72 the obs is:27 +[19:49:55.352] Odometry x=-0.028200, y=0.941419, theta=0.030823 +[19:49:55.455] Odometry x=-0.028160, y=0.946726, theta=0.031311 +[19:49:55.488] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:0 ,distance between ekf and vo x:-0.000313 ,z=-0.000020,angle between two frames:0.000218 +[19:49:55.488] After pose optimization, distance between ekf and vo x:-0.000313 ,z=-0.000020,angle between two frames:0.000218 +[19:49:55.488] frame process finished the id is: 72 the obs is:27 +[19:49:55.554] Odometry x=-0.028227, y=0.955052, theta=0.039345 +[19:49:55.647] After Reprojection Map nPoint:21 ,nCell:21 ,nMatches:1 ,distance between ekf and vo x:-0.000892 ,z=0.000073,angle between two frames:0.001116 +[19:49:55.647] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.228860 error end=0.228860 +[19:49:55.647] After pose optimization, distance between ekf and vo x:-0.000892 ,z=0.000073,angle between two frames:0.001116 +[19:49:55.647] frame process finished the id is: 72 the obs is:27 +[19:49:55.657] Odometry x=-0.028357, y=0.961652, theta=0.038743 +[19:49:55.756] Odometry x=-0.028353, y=0.966898, theta=0.034665 +[19:49:55.805] After Reprojection Map nPoint:19 ,nCell:19 ,nMatches:0 ,distance between ekf and vo x:-0.000980 ,z=0.000224,angle between two frames:0.000332 +[19:49:55.805] After pose optimization, distance between ekf and vo x:-0.000980 ,z=0.000224,angle between two frames:0.000332 +[19:49:55.805] frame process finished the id is: 72 the obs is:27 +[19:49:55.855] Odometry x=-0.028467, y=0.972692, theta=0.045289 +[19:49:55.957] Odometry x=-0.028594, y=0.978974, theta=0.042719 +[19:49:55.962] After Reprojection Map nPoint:19 ,nCell:19 ,nMatches:1 ,distance between ekf and vo x:-0.000859 ,z=0.000180,angle between two frames:0.000538 +[19:49:55.962] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.194999 error end=0.151839 +[19:49:55.962] After pose optimization, distance between ekf and vo x:-0.015306 ,z=0.281845,angle between two frames:0.024710 +[19:49:55.962] frame process finished the id is: 72 the obs is:27 +[19:49:56.055] Odometry x=-0.028709, y=0.984515, theta=0.038439 +[19:49:56.111] After Reprojection Map nPoint:20 ,nCell:20 ,nMatches:0 ,distance between ekf and vo x:-0.000246 ,z=0.000001,angle between two frames:0.000186 +[19:49:56.111] After pose optimization, distance between ekf and vo x:-0.000246 ,z=0.000001,angle between two frames:0.000186 +[19:49:56.111] frame process finished the id is: 72 the obs is:27 +[19:49:56.154] Odometry x=-0.028823, y=0.990116, theta=0.043144 +[19:49:56.254] Odometry x=-0.028966, y=0.995533, theta=0.043307 +[19:49:56.266] After Reprojection Map nPoint:21 ,nCell:21 ,nMatches:2 ,distance between ekf and vo x:-0.000611 ,z=0.000076,angle between two frames:0.000599 +[19:49:56.266] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.596028 error end=0.596028 +[19:49:56.266] After pose optimization, distance between ekf and vo x:-0.000611 ,z=0.000076,angle between two frames:0.000599 +[19:49:56.266] frame process finished the id is: 72 the obs is:27 +[19:49:56.353] Odometry x=-0.029085, y=1.000968, theta=0.042820 +[19:49:56.408] After Reprojection Map nPoint:18 ,nCell:17 ,nMatches:1 ,distance between ekf and vo x:-0.000083 ,z=0.000016,angle between two frames:0.000036 +[19:49:56.408] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.117353 error end=0.116168 +[19:49:56.408] After pose optimization, distance between ekf and vo x:0.002689 ,z=-0.002901,angle between two frames:0.000950 +[19:49:56.408] frame process finished the id is: 72 the obs is:27 +[19:49:56.454] Odometry x=-0.029196, y=1.006446, theta=0.039610 +[19:49:56.556] Odometry x=-0.029317, y=1.012971, theta=0.037791 +[19:49:56.559] After Reprojection Map nPoint:17 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:-0.000125 ,z=-0.000103,angle between two frames:0.000159 +[19:49:56.559] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.220308 error end=0.161254 +[19:49:56.559] After pose optimization, distance between ekf and vo x:-0.005464 ,z=0.225716,angle between two frames:0.044733 +[19:49:56.559] frame process finished the id is: 72 the obs is:27 +[19:49:56.654] Odometry x=-0.029404, y=1.018298, theta=0.031420 +[19:49:56.704] After Reprojection Map nPoint:20 ,nCell:20 ,nMatches:1 ,distance between ekf and vo x:-0.000118 ,z=-0.000008,angle between two frames:0.000125 +[19:49:56.704] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =0.578240 error end=0.575966 +[19:49:56.704] After pose optimization, distance between ekf and vo x:-0.002877 ,z=0.002596,angle between two frames:0.040455 +[19:49:56.704] frame process finished the id is: 72 the obs is:27 +[19:49:56.754] Odometry x=-0.029463, y=1.023694, theta=0.033264 +[19:49:56.856] After Reprojection Map nPoint:20 ,nCell:20 ,nMatches:0 ,distance between ekf and vo x:-0.000360 ,z=-0.000017,angle between two frames:0.000253 +[19:49:56.856] After pose optimization, distance between ekf and vo x:-0.000360 ,z=-0.000017,angle between two frames:0.000253 +[19:49:56.856] frame process finished the id is: 72 the obs is:27 +[19:49:56.857] Odometry x=-0.029539, y=1.028986, theta=0.028633 +[19:49:56.955] Odometry x=-0.029606, y=1.034592, theta=0.032868 +[19:49:57.008] After Reprojection Map nPoint:20 ,nCell:20 ,nMatches:0 ,distance between ekf and vo x:-0.000269 ,z=-0.000074,angle between two frames:0.000291 +[19:49:57.008] After pose optimization, distance between ekf and vo x:-0.000269 ,z=-0.000074,angle between two frames:0.000291 +[19:49:57.008] frame process finished the id is: 72 the obs is:27 +[19:49:57.052] Odometry x=-0.029684, y=1.039914, theta=0.032190 +[19:49:57.155] Odometry x=-0.029772, y=1.045261, theta=0.037499 +[19:49:57.161] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:0 ,distance between ekf and vo x:-0.000465 ,z=-0.000038,angle between two frames:0.000382 +[19:49:57.161] After pose optimization, distance between ekf and vo x:-0.000465 ,z=-0.000038,angle between two frames:0.000382 +[19:49:57.161] frame process finished the id is: 72 the obs is:27 +[19:49:57.257] Odometry x=-0.029885, y=1.051262, theta=0.046090 +[19:49:57.317] After Reprojection Map nPoint:20 ,nCell:19 ,nMatches:1 ,distance between ekf and vo x:-0.000976 ,z=0.000211,angle between two frames:0.000482 +[19:49:57.317] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.076447 error end=0.076447 +[19:49:57.317] After pose optimization, distance between ekf and vo x:-0.000976 ,z=0.000211,angle between two frames:0.000482 +[19:49:57.317] frame process finished the id is: 72 the obs is:27 +[19:49:57.355] Odometry x=-0.029982, y=1.057117, theta=0.035162 +[19:49:57.455] Odometry x=-0.030066, y=1.062342, theta=0.038939 +[19:49:57.462] After Reprojection Map nPoint:18 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:-0.000228 ,z=0.000056,angle between two frames:0.000151 +[19:49:57.462] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.296675 error end=0.290055 +[19:49:57.462] After pose optimization, distance between ekf and vo x:-0.037882 ,z=0.064008,angle between two frames:0.006545 +[19:49:57.462] frame process finished the id is: 72 the obs is:27 +[19:49:57.554] Odometry x=-0.030175, y=1.067301, theta=0.037985 +[19:49:57.618] After Reprojection Map nPoint:19 ,nCell:19 ,nMatches:0 ,distance between ekf and vo x:-0.000689 ,z=0.000086,angle between two frames:0.000394 +[19:49:57.619] After pose optimization, distance between ekf and vo x:-0.000689 ,z=0.000086,angle between two frames:0.000394 +[19:49:57.619] frame process finished the id is: 72 the obs is:27 +[19:49:57.653] Odometry x=-0.030301, y=1.073477, theta=0.042212 +[19:49:57.755] Odometry x=-0.030408, y=1.078812, theta=0.038287 +[19:49:57.776] After Reprojection Map nPoint:19 ,nCell:19 ,nMatches:1 ,distance between ekf and vo x:-0.000800 ,z=0.000143,angle between two frames:0.000405 +[19:49:57.776] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.406245 error end=0.406245 +[19:49:57.776] After pose optimization, distance between ekf and vo x:-0.000800 ,z=0.000143,angle between two frames:0.000405 +[19:49:57.776] frame process finished the id is: 72 the obs is:27 +[19:49:57.859] Odometry x=-0.030529, y=1.084833, theta=0.041255 +[19:49:57.928] After Reprojection Map nPoint:14 ,nCell:14 ,nMatches:1 ,distance between ekf and vo x:-0.000361 ,z=0.000001,angle between two frames:0.000254 +[19:49:57.928] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.134802 error end=0.094273 +[19:49:57.928] After pose optimization, distance between ekf and vo x:0.005906 ,z=0.261380,angle between two frames:0.026893 +[19:49:57.928] frame process finished the id is: 72 the obs is:27 +[19:49:57.953] Odometry x=-0.030660, y=1.091149, theta=0.040199 +[19:49:58.049] Odometry x=-0.030757, y=1.096279, theta=0.039136 +[19:49:58.090] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:-0.000539 ,z=0.000025,angle between two frames:0.000433 +[19:49:58.090] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.137704 error end=0.137704 +[19:49:58.090] After pose optimization, distance between ekf and vo x:-0.000539 ,z=0.000025,angle between two frames:0.000433 +[19:49:58.090] frame process finished the id is: 72 the obs is:27 +[19:49:58.157] Odometry x=-0.030861, y=1.101413, theta=0.040279 +[19:49:58.243] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:-0.000448 ,z=0.000037,angle between two frames:0.000329 +[19:49:58.243] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.174544 error end=0.163641 +[19:49:58.243] After pose optimization, distance between ekf and vo x:-0.049553 ,z=0.034648,angle between two frames:0.030527 +[19:49:58.243] frame process finished the id is: 72 the obs is:27 +[19:49:58.251] Odometry x=-0.030976, y=1.107266, theta=0.039167 +[19:49:58.354] Odometry x=-0.031067, y=1.112803, theta=0.039488 +[19:49:58.387] After Reprojection Map nPoint:18 ,nCell:17 ,nMatches:3 ,distance between ekf and vo x:-0.000071 ,z=-0.000015,angle between two frames:0.000110 +[19:49:58.387] n deleted obs = 0 n obs with reprojection error less than 0.5 px =3 error init =0.342321 error end=0.309535 +[19:49:58.387] After pose optimization, distance between ekf and vo x:0.014153 ,z=0.239469,angle between two frames:0.037967 +[19:49:58.387] frame process finished the id is: 72 the obs is:27 +[19:49:58.456] Odometry x=-0.031169, y=1.118218, theta=0.036400 +[19:49:58.536] After Reprojection Map nPoint:19 ,nCell:18 ,nMatches:3 ,distance between ekf and vo x:-0.000637 ,z=0.000158,angle between two frames:0.000315 +[19:49:58.536] n deleted obs = 0 n obs with reprojection error less than 0.5 px =3 error init =0.157451 error end=0.152788 +[19:49:58.536] After pose optimization, distance between ekf and vo x:-0.069713 ,z=0.087825,angle between two frames:0.006628 +[19:49:58.536] frame process finished the id is: 72 the obs is:27 +[19:49:58.553] Odometry x=-0.031254, y=1.123254, theta=0.034564 +[19:49:58.656] Odometry x=-0.031357, y=1.128684, theta=0.040908 +[19:49:58.683] After Reprojection Map nPoint:18 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:-0.000231 ,z=0.000017,angle between two frames:0.000159 +[19:49:58.683] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.233015 error end=0.233015 +[19:49:58.683] After pose optimization, distance between ekf and vo x:-0.000231 ,z=0.000017,angle between two frames:0.000159 +[19:49:58.683] frame process finished the id is: 72 the obs is:27 +[19:49:58.753] Odometry x=-0.031447, y=1.134037, theta=0.027306 +[19:49:58.836] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:-0.000406 ,z=-0.000014,angle between two frames:0.000317 +[19:49:58.836] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.167105 error end=0.167105 +[19:49:58.836] After pose optimization, distance between ekf and vo x:-0.000406 ,z=-0.000014,angle between two frames:0.000317 +[19:49:58.836] frame process finished the id is: 72 the obs is:27 +[19:49:58.855] Odometry x=-0.031520, y=1.139615, theta=0.031476 +[19:49:58.952] Odometry x=-0.031584, y=1.144979, theta=0.032154 +[19:49:58.986] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:-0.000209 ,z=-0.000071,angle between two frames:0.000223 +[19:49:58.986] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.064553 error end=0.064553 +[19:49:58.986] After pose optimization, distance between ekf and vo x:-0.000209 ,z=-0.000071,angle between two frames:0.000223 +[19:49:58.986] frame process finished the id is: 72 the obs is:27 +[19:49:59.053] Odometry x=-0.031670, y=1.151075, theta=0.034065 +[19:49:59.143] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:-0.000568 ,z=0.000023,angle between two frames:0.000341 +[19:49:59.143] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.130658 error end=0.130658 +[19:49:59.143] After pose optimization, distance between ekf and vo x:-0.000568 ,z=0.000023,angle between two frames:0.000341 +[19:49:59.143] frame process finished the id is: 72 the obs is:27 +[19:49:59.151] Odometry x=-0.031739, y=1.156464, theta=0.039604 +[19:49:59.253] Odometry x=-0.031794, y=1.161733, theta=0.036169 +[19:49:59.301] After Reprojection Map nPoint:19 ,nCell:19 ,nMatches:2 ,distance between ekf and vo x:-0.000664 ,z=0.000072,angle between two frames:0.000428 +[19:49:59.301] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.217515 error end=0.217515 +[19:49:59.301] After pose optimization, distance between ekf and vo x:-0.000664 ,z=0.000072,angle between two frames:0.000428 +[19:49:59.301] frame process finished the id is: 72 the obs is:27 +[19:49:59.352] Odometry x=-0.031898, y=1.167209, theta=0.040845 +[19:49:59.455] Odometry x=-0.032018, y=1.173063, theta=0.040641 +[19:49:59.455] After Reprojection Map nPoint:14 ,nCell:14 ,nMatches:0 ,distance between ekf and vo x:-0.000463 ,z=0.000014,angle between two frames:0.000316 +[19:49:59.455] After pose optimization, distance between ekf and vo x:-0.000463 ,z=0.000014,angle between two frames:0.000316 +[19:49:59.455] frame process finished the id is: 72 the obs is:27 +[19:49:59.552] Odometry x=-0.032137, y=1.178807, theta=0.041711 +[19:49:59.605] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:0 ,distance between ekf and vo x:-0.000264 ,z=0.000014,angle between two frames:0.000221 +[19:49:59.605] After pose optimization, distance between ekf and vo x:-0.000264 ,z=0.000014,angle between two frames:0.000221 +[19:49:59.605] frame process finished the id is: 72 the obs is:27 +[19:49:59.654] Odometry x=-0.032244, y=1.184720, theta=0.039199 +[19:49:59.751] Odometry x=-0.032360, y=1.190092, theta=0.037825 +[19:49:59.762] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:1 ,distance between ekf and vo x:-0.000676 ,z=0.000103,angle between two frames:0.000342 +[19:49:59.762] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =4.988971 error end=0.853456 +[19:49:59.762] After pose optimization, distance between ekf and vo x:9.142666 ,z=15.080118,angle between two frames:0.504727 +[19:49:59.762] frame process finished the id is: 72 the obs is:27 +[19:49:59.854] Odometry x=-0.032478, y=1.196530, theta=0.041789 +[19:49:59.912] After Reprojection Map nPoint:21 ,nCell:21 ,nMatches:1 ,distance between ekf and vo x:-0.000336 ,z=0.000074,angle between two frames:0.000180 +[19:49:59.913] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.296536 error end=0.265287 +[19:49:59.913] After pose optimization, distance between ekf and vo x:-0.041317 ,z=0.578579,angle between two frames:0.056088 +[19:49:59.913] frame process finished the id is: 72 the obs is:27 +[19:49:59.956] Odometry x=-0.032608, y=1.203096, theta=0.040675 +[19:50:00.055] Odometry x=-0.032657, y=1.208461, theta=0.044262 +[19:50:00.065] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:0 ,distance between ekf and vo x:-0.000415 ,z=0.000043,angle between two frames:0.000290 +[19:50:00.065] After pose optimization, distance between ekf and vo x:-0.000415 ,z=0.000043,angle between two frames:0.000290 +[19:50:00.065] frame process finished the id is: 72 the obs is:27 +[19:50:00.151] Odometry x=-0.032774, y=1.213945, theta=0.039258 +[19:50:00.207] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:1 ,distance between ekf and vo x:-0.000010 ,z=-0.000004,angle between two frames:0.000015 +[19:50:00.208] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.108016 error end=0.108016 +[19:50:00.208] After pose optimization, distance between ekf and vo x:-0.000010 ,z=-0.000004,angle between two frames:0.000015 +[19:50:00.208] frame process finished the id is: 72 the obs is:27 +[19:50:00.250] Odometry x=-0.032847, y=1.219313, theta=0.034594 +[19:50:00.352] Odometry x=-0.032955, y=1.225174, theta=0.035797 +[19:50:00.358] After Reprojection Map nPoint:22 ,nCell:22 ,nMatches:3 ,distance between ekf and vo x:-0.000682 ,z=0.000230,angle between two frames:0.000287 +[19:50:00.358] n deleted obs = 0 n obs with reprojection error less than 0.5 px =3 error init =0.208047 error end=0.199207 +[19:50:00.358] After pose optimization, distance between ekf and vo x:-0.053092 ,z=0.085058,angle between two frames:0.023368 +[19:50:00.358] frame process finished the id is: 72 the obs is:27 +[19:50:00.455] Odometry x=-0.033013, y=1.231033, theta=0.037877 +[19:50:00.509] After Reprojection Map nPoint:20 ,nCell:20 ,nMatches:0 ,distance between ekf and vo x:-0.000695 ,z=0.000252,angle between two frames:0.000267 +[19:50:00.509] After pose optimization, distance between ekf and vo x:-0.000695 ,z=0.000252,angle between two frames:0.000267 +[19:50:00.509] frame process finished the id is: 72 the obs is:27 +[19:50:00.553] Odometry x=-0.033112, y=1.236660, theta=0.034893 +[19:50:00.654] Odometry x=-0.032981, y=1.241758, theta=0.040549 +[19:50:00.659] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:0 ,distance between ekf and vo x:-0.000105 ,z=-0.000087,angle between two frames:0.000156 +[19:50:00.659] After pose optimization, distance between ekf and vo x:-0.000105 ,z=-0.000087,angle between two frames:0.000156 +[19:50:00.659] frame process finished the id is: 72 the obs is:27 +[19:50:00.763] Odometry x=-0.033094, y=1.247199, theta=0.033086 +[19:50:00.813] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:-0.000435 ,z=0.000100,angle between two frames:0.000331 +[19:50:00.814] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.305315 error end=0.304918 +[19:50:00.814] After pose optimization, distance between ekf and vo x:0.000739 ,z=0.001017,angle between two frames:0.001351 +[19:50:00.814] frame process finished the id is: 72 the obs is:27 +[19:50:00.854] Odometry x=-0.033184, y=1.252952, theta=0.034263 +[19:50:00.957] Odometry x=-0.033257, y=1.258364, theta=0.029686 +[19:50:00.971] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:1 ,distance between ekf and vo x:-0.000746 ,z=0.000108,angle between two frames:0.000470 +[19:50:00.972] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.379817 error end=0.379377 +[19:50:00.972] After pose optimization, distance between ekf and vo x:-0.000920 ,z=0.002645,angle between two frames:0.001658 +[19:50:00.972] frame process finished the id is: 72 the obs is:27 +[19:50:01.057] Odometry x=-0.033329, y=1.263834, theta=0.035800 +[19:50:01.122] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:0 ,distance between ekf and vo x:-0.000387 ,z=0.000100,angle between two frames:0.000218 +[19:50:01.122] After pose optimization, distance between ekf and vo x:-0.000387 ,z=0.000100,angle between two frames:0.000218 +[19:50:01.122] frame process finished the id is: 72 the obs is:27 +[19:50:01.155] Odometry x=-0.033432, y=1.269675, theta=0.034763 +[19:50:01.253] Odometry x=-0.033321, y=1.275008, theta=0.033882 +[19:50:01.273] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:1 ,distance between ekf and vo x:-0.000664 ,z=0.000259,angle between two frames:0.000344 +[19:50:01.273] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =0.638792 error end=0.638792 +[19:50:01.273] After pose optimization, distance between ekf and vo x:-0.000664 ,z=0.000259,angle between two frames:0.000344 +[19:50:01.273] frame process finished the id is: 72 the obs is:27 +[19:50:01.351] Odometry x=-0.033423, y=1.280195, theta=0.044265 +[19:50:01.426] After Reprojection Map nPoint:18 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:-0.000727 ,z=0.000271,angle between two frames:0.000227 +[19:50:01.426] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =55.262415 error end=1.134420 +[19:50:01.426] After pose optimization, distance between ekf and vo x:9009.778185 ,z=-9834.373191,angle between two frames:2.139673 +[19:50:01.426] frame process finished the id is: 72 the obs is:27 +[19:50:01.453] Odometry x=-0.033541, y=1.286197, theta=0.045420 +[19:50:01.555] Odometry x=-0.033685, y=1.291690, theta=0.045286 +[19:50:01.579] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:2 ,distance between ekf and vo x:-0.000208 ,z=0.000031,angle between two frames:0.000208 +[19:50:01.579] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.471873 error end=0.471873 +[19:50:01.579] After pose optimization, distance between ekf and vo x:-0.000208 ,z=0.000031,angle between two frames:0.000208 +[19:50:01.579] frame process finished the id is: 72 the obs is:27 +[19:50:01.655] Odometry x=-0.033843, y=1.298625, theta=0.045299 +[19:50:01.729] After Reprojection Map nPoint:19 ,nCell:19 ,nMatches:0 ,distance between ekf and vo x:-0.000110 ,z=0.000029,angle between two frames:0.000050 +[19:50:01.729] After pose optimization, distance between ekf and vo x:-0.000110 ,z=0.000029,angle between two frames:0.000050 +[19:50:01.729] frame process finished the id is: 72 the obs is:27 +[19:50:01.758] Odometry x=-0.034010, y=1.305766, theta=0.050501 +[19:50:01.855] Odometry x=-0.034085, y=1.311879, theta=0.048218 +[19:50:01.883] After Reprojection Map nPoint:19 ,nCell:19 ,nMatches:0 ,distance between ekf and vo x:-0.000141 ,z=-0.000044,angle between two frames:0.000194 +[19:50:01.883] After pose optimization, distance between ekf and vo x:-0.000141 ,z=-0.000044,angle between two frames:0.000194 +[19:50:01.883] frame process finished the id is: 72 the obs is:27 +[19:50:01.953] Odometry x=-0.034174, y=1.317213, theta=0.043469 +[19:50:02.040] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:1 ,distance between ekf and vo x:-0.000681 ,z=0.000098,angle between two frames:0.000421 +[19:50:02.040] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =0.890531 error end=0.796496 +[19:50:02.040] After pose optimization, distance between ekf and vo x:0.066187 ,z=0.067521,angle between two frames:0.056162 +[19:50:02.040] frame process finished the id is: 72 the obs is:27 +[19:50:02.055] Odometry x=-0.034313, y=1.322574, theta=0.040874 +[19:50:02.155] Odometry x=-0.034411, y=1.328053, theta=0.042341 +[19:50:02.191] After Reprojection Map nPoint:19 ,nCell:19 ,nMatches:1 ,distance between ekf and vo x:-0.000231 ,z=0.000004,angle between two frames:0.000193 +[19:50:02.191] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =0.558727 error end=0.534721 +[19:50:02.191] After pose optimization, distance between ekf and vo x:0.038705 ,z=0.114673,angle between two frames:0.032173 +[19:50:02.191] frame process finished the id is: 72 the obs is:27 +[19:50:02.254] Odometry x=-0.034423, y=1.333420, theta=0.048050 +[19:50:02.351] After Reprojection Map nPoint:21 ,nCell:21 ,nMatches:0 ,distance between ekf and vo x:-0.000631 ,z=0.000180,angle between two frames:0.000353 +[19:50:02.351] After pose optimization, distance between ekf and vo x:-0.000631 ,z=0.000180,angle between two frames:0.000353 +[19:50:02.351] frame process finished the id is: 72 the obs is:27 +[19:50:02.352] Odometry x=-0.034534, y=1.339104, theta=0.035728 +[19:50:02.454] Odometry x=-0.034624, y=1.344455, theta=0.034780 +[19:50:02.509] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:0 ,distance between ekf and vo x:-0.000614 ,z=0.000185,angle between two frames:0.000392 +[19:50:02.509] After pose optimization, distance between ekf and vo x:-0.000614 ,z=0.000185,angle between two frames:0.000392 +[19:50:02.509] frame process finished the id is: 72 the obs is:27 +[19:50:02.556] Odometry x=-0.034681, y=1.350416, theta=0.035068 +[19:50:02.653] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:1 ,distance between ekf and vo x:-0.000125 ,z=0.000045,angle between two frames:0.000051 +[19:50:02.653] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =1.010127 error end=1.010127 +[19:50:02.653] After pose optimization, distance between ekf and vo x:-0.000125 ,z=0.000045,angle between two frames:0.000051 +[19:50:02.653] frame process finished the id is: 72 the obs is:27 +[19:50:02.658] Odometry x=-0.034763, y=1.356082, theta=0.042348 +[19:50:02.756] Odometry x=-0.034890, y=1.362443, theta=0.036023 +[19:50:02.806] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:0 ,distance between ekf and vo x:-0.000623 ,z=0.000248,angle between two frames:0.000265 +[19:50:02.806] After pose optimization, distance between ekf and vo x:-0.000623 ,z=0.000248,angle between two frames:0.000265 +[19:50:02.806] frame process finished the id is: 72 the obs is:27 +[19:50:02.850] Odometry x=-0.034949, y=1.368131, theta=0.036226 +[19:50:02.953] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:0 ,distance between ekf and vo x:-0.000322 ,z=0.000115,angle between two frames:0.000126 +[19:50:02.953] After pose optimization, distance between ekf and vo x:-0.000322 ,z=0.000115,angle between two frames:0.000126 +[19:50:02.953] frame process finished the id is: 72 the obs is:27 +[19:50:02.954] Odometry x=-0.035007, y=1.373649, theta=0.042140 +[19:50:03.054] Odometry x=-0.034727, y=1.379135, theta=0.033412 +[19:50:03.103] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:0 ,distance between ekf and vo x:-0.000566 ,z=0.000190,angle between two frames:0.000229 +[19:50:03.103] After pose optimization, distance between ekf and vo x:-0.000566 ,z=0.000190,angle between two frames:0.000229 +[19:50:03.103] frame process finished the id is: 72 the obs is:27 +[19:50:03.157] Odometry x=-0.034813, y=1.384846, theta=0.032024 +[19:50:03.253] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:-0.000485 ,z=0.000179,angle between two frames:0.000164 +[19:50:03.253] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =1.978685 error end=0.438928 +[19:50:03.253] After pose optimization, distance between ekf and vo x:-0.264309 ,z=0.103082,angle between two frames:0.083708 +[19:50:03.253] frame process finished the id is: 72 the obs is:27 +[19:50:03.256] Odometry x=-0.034856, y=1.391341, theta=0.038342 +[19:50:03.353] Odometry x=-0.034984, y=1.397062, theta=0.044098 +[19:50:03.404] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:0 ,distance between ekf and vo x:-0.000501 ,z=0.000195,angle between two frames:0.000158 +[19:50:03.404] After pose optimization, distance between ekf and vo x:-0.000501 ,z=0.000195,angle between two frames:0.000158 +[19:50:03.404] frame process finished the id is: 72 the obs is:27 +[19:50:03.461] Odometry x=-0.035049, y=1.402337, theta=0.044724 +[19:50:03.551] After Reprojection Map nPoint:19 ,nCell:19 ,nMatches:1 ,distance between ekf and vo x:-0.000352 ,z=0.000124,angle between two frames:0.000114 +[19:50:03.551] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.330551 error end=0.324872 +[19:50:03.551] After pose optimization, distance between ekf and vo x:-0.013408 ,z=0.056108,angle between two frames:0.003269 +[19:50:03.551] frame process finished the id is: 72 the obs is:27 +[19:50:03.551] Odometry x=-0.035179, y=1.408259, theta=0.039883 +[19:50:03.655] Odometry x=-0.035258, y=1.413271, theta=0.044666 +[19:50:03.694] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:0 ,distance between ekf and vo x:-0.000004 ,z=-0.000006,angle between two frames:0.000009 +[19:50:03.694] After pose optimization, distance between ekf and vo x:-0.000004 ,z=-0.000006,angle between two frames:0.000009 +[19:50:03.694] frame process finished the id is: 72 the obs is:27 +[19:50:03.754] Odometry x=-0.035387, y=1.418815, theta=0.045456 +[19:50:03.837] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:0 ,distance between ekf and vo x:0.000006 ,z=-0.000008,angle between two frames:0.000008 +[19:50:03.837] After pose optimization, distance between ekf and vo x:0.000006 ,z=-0.000008,angle between two frames:0.000008 +[19:50:03.837] frame process finished the id is: 72 the obs is:27 +[19:50:03.857] Odometry x=-0.035505, y=1.424195, theta=0.041928 +[19:50:03.955] Odometry x=-0.035692, y=1.432621, theta=0.029947 +[19:50:03.988] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:-0.000176 ,z=-0.000056,angle between two frames:0.000204 +[19:50:03.988] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.279339 error end=0.235376 +[19:50:03.988] After pose optimization, distance between ekf and vo x:-0.007188 ,z=0.510196,angle between two frames:0.048603 +[19:50:03.988] frame process finished the id is: 72 the obs is:27 +[19:50:04.056] Odometry x=-0.035778, y=1.437685, theta=0.037699 +[19:50:04.134] After Reprojection Map nPoint:16 ,nCell:15 ,nMatches:2 ,distance between ekf and vo x:-0.000166 ,z=0.000025,angle between two frames:0.000090 +[19:50:04.134] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.298895 error end=0.288470 +[19:50:04.134] After pose optimization, distance between ekf and vo x:0.071057 ,z=0.008091,angle between two frames:0.042316 +[19:50:04.134] frame process finished the id is: 72 the obs is:27 +[19:50:04.153] Odometry x=-0.035864, y=1.443333, theta=0.039036 +[19:50:04.255] Odometry x=-0.035991, y=1.448680, theta=0.034644 +[19:50:04.283] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:1 ,distance between ekf and vo x:-0.000541 ,z=0.000160,angle between two frames:0.000202 +[19:50:04.283] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =0.593834 error end=0.593834 +[19:50:04.283] After pose optimization, distance between ekf and vo x:-0.000541 ,z=0.000160,angle between two frames:0.000202 +[19:50:04.283] frame process finished the id is: 72 the obs is:27 +[19:50:04.435] After Reprojection Map nPoint:20 ,nCell:20 ,nMatches:1 ,distance between ekf and vo x:-0.000550 ,z=-0.000004,angle between two frames:0.000223 +[19:50:04.435] n deleted obs = 0 n obs with reprojection error less than 0.5 px =1 error init =0.223196 error end=0.200372 +[19:50:04.435] After pose optimization, distance between ekf and vo x:-0.017869 ,z=-0.006708,angle between two frames:0.010574 +[19:50:04.435] frame process finished the id is: 72 the obs is:27 +[19:50:04.585] After Reprojection Map nPoint:21 ,nCell:21 ,nMatches:0 ,distance between ekf and vo x:-0.000335 ,z=0.000006,angle between two frames:0.000053 +[19:50:04.585] After pose optimization, distance between ekf and vo x:-0.000335 ,z=0.000006,angle between two frames:0.000053 +[19:50:04.585] frame process finished the id is: 72 the obs is:27 +[19:50:04.735] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:-0.000343 ,z=0.000012,angle between two frames:0.000059 +[19:50:04.735] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.308505 error end=0.299888 +[19:50:04.735] After pose optimization, distance between ekf and vo x:0.108199 ,z=0.009746,angle between two frames:0.049810 +[19:50:04.735] frame process finished the id is: 72 the obs is:27 +[19:50:04.886] After Reprojection Map nPoint:15 ,nCell:14 ,nMatches:2 ,distance between ekf and vo x:-0.000510 ,z=0.000120,angle between two frames:0.000135 +[19:50:04.887] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.848270 error end=0.518373 +[19:50:04.887] After pose optimization, distance between ekf and vo x:1.065310 ,z=0.330016,angle between two frames:0.635501 +[19:50:04.887] frame process finished the id is: 72 the obs is:27 +[19:50:05.040] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:2 ,distance between ekf and vo x:-0.000519 ,z=0.000129,angle between two frames:0.000136 +[19:50:05.040] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.476908 error end=0.451530 +[19:50:05.040] After pose optimization, distance between ekf and vo x:0.048383 ,z=0.027742,angle between two frames:0.035309 +[19:50:05.040] frame process finished the id is: 72 the obs is:27 +[19:50:05.195] After Reprojection Map nPoint:12 ,nCell:12 ,nMatches:2 ,distance between ekf and vo x:-0.000505 ,z=0.000120,angle between two frames:0.000124 +[19:50:05.195] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.555841 error end=0.555841 +[19:50:05.195] After pose optimization, distance between ekf and vo x:-0.000505 ,z=0.000120,angle between two frames:0.000124 +[19:50:05.195] frame process finished the id is: 72 the obs is:27 +[19:50:05.350] After Reprojection Map nPoint:16 ,nCell:15 ,nMatches:3 ,distance between ekf and vo x:-0.000513 ,z=0.000112,angle between two frames:0.000132 +[19:50:05.350] n deleted obs = 2 n obs with reprojection error less than 0.5 px =1 error init =0.887812 error end=0.810213 +[19:50:05.350] After pose optimization, distance between ekf and vo x:0.106770 ,z=0.009232,angle between two frames:0.052111 +[19:50:05.350] frame process finished the id is: 72 the obs is:27 +[19:50:05.502] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:3 ,distance between ekf and vo x:-0.000518 ,z=0.000112,angle between two frames:0.000125 +[19:50:05.502] n deleted obs = 2 n obs with reprojection error less than 0.5 px =1 error init =0.809005 error end=0.720113 +[19:50:05.502] After pose optimization, distance between ekf and vo x:0.085137 ,z=0.100349,angle between two frames:0.127872 +[19:50:05.502] frame process finished the id is: 72 the obs is:27 +[19:50:05.653] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:2 ,distance between ekf and vo x:-0.000504 ,z=0.000106,angle between two frames:0.000129 +[19:50:05.653] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.470726 error end=0.421434 +[19:50:05.653] After pose optimization, distance between ekf and vo x:0.071081 ,z=0.127511,angle between two frames:0.126286 +[19:50:05.653] frame process finished the id is: 72 the obs is:27 +[19:50:05.804] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:-0.000499 ,z=0.000107,angle between two frames:0.000132 +[19:50:05.804] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =0.860138 error end=0.747021 +[19:50:05.804] After pose optimization, distance between ekf and vo x:0.122280 ,z=0.225086,angle between two frames:0.216609 +[19:50:05.804] frame process finished the id is: 72 the obs is:27 +[19:50:05.954] After Reprojection Map nPoint:19 ,nCell:19 ,nMatches:3 ,distance between ekf and vo x:-0.000495 ,z=0.000110,angle between two frames:0.000157 +[19:50:05.954] n deleted obs = 1 n obs with reprojection error less than 0.5 px =2 error init =0.824866 error end=0.595128 +[19:50:05.954] After pose optimization, distance between ekf and vo x:0.332314 ,z=0.346719,angle between two frames:0.391559 +[19:50:05.954] frame process finished the id is: 72 the obs is:27 +[19:50:06.105] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:1 ,distance between ekf and vo x:-0.000483 ,z=0.000104,angle between two frames:0.000177 +[19:50:06.105] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =0.704680 error end=0.704680 +[19:50:06.105] After pose optimization, distance between ekf and vo x:-0.000483 ,z=0.000104,angle between two frames:0.000177 +[19:50:06.105] frame process finished the id is: 72 the obs is:27 +[19:50:06.255] After Reprojection Map nPoint:17 ,nCell:16 ,nMatches:3 ,distance between ekf and vo x:-0.000446 ,z=0.000095,angle between two frames:0.000250 +[19:50:06.255] n deleted obs = 2 n obs with reprojection error less than 0.5 px =1 error init =0.853260 error end=0.802842 +[19:50:06.255] After pose optimization, distance between ekf and vo x:0.062259 ,z=0.019683,angle between two frames:0.039026 +[19:50:06.255] frame process finished the id is: 72 the obs is:27 +[19:50:06.406] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:-0.000436 ,z=0.000096,angle between two frames:0.000299 +[19:50:06.406] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =0.705795 error end=0.705795 +[19:50:06.406] After pose optimization, distance between ekf and vo x:-0.000436 ,z=0.000096,angle between two frames:0.000299 +[19:50:06.406] frame process finished the id is: 72 the obs is:27 +[19:50:06.557] After Reprojection Map nPoint:20 ,nCell:20 ,nMatches:3 ,distance between ekf and vo x:-0.000419 ,z=0.000091,angle between two frames:0.000305 +[19:50:06.557] n deleted obs = 2 n obs with reprojection error less than 0.5 px =1 error init =0.808731 error end=0.649853 +[19:50:06.557] After pose optimization, distance between ekf and vo x:0.180683 ,z=0.328956,angle between two frames:0.313619 +[19:50:06.557] frame process finished the id is: 72 the obs is:27 +[19:50:06.707] After Reprojection Map nPoint:16 ,nCell:15 ,nMatches:2 ,distance between ekf and vo x:-0.000432 ,z=0.000094,angle between two frames:0.000306 +[19:50:06.707] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =1.470143 error end=0.350278 +[19:50:06.707] After pose optimization, distance between ekf and vo x:1.863879 ,z=0.442569,angle between two frames:1.188380 +[19:50:06.707] frame process finished the id is: 72 the obs is:27 +[19:50:06.858] After Reprojection Map nPoint:15 ,nCell:14 ,nMatches:2 ,distance between ekf and vo x:-0.000429 ,z=0.000094,angle between two frames:0.000295 +[19:50:06.858] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =1.094619 error end=0.326542 +[19:50:06.858] After pose optimization, distance between ekf and vo x:0.954397 ,z=0.000667,angle between two frames:0.649213 +[19:50:06.858] frame process finished the id is: 72 the obs is:27 +[19:50:07.008] After Reprojection Map nPoint:20 ,nCell:20 ,nMatches:5 ,distance between ekf and vo x:-0.000431 ,z=0.000092,angle between two frames:0.000306 +[19:50:07.008] n deleted obs = 3 n obs with reprojection error less than 0.5 px =2 error init =0.659111 error end=0.659111 +[19:50:07.008] After pose optimization, distance between ekf and vo x:-0.000431 ,z=0.000092,angle between two frames:0.000306 +[19:50:07.008] frame process finished the id is: 72 the obs is:27 +[19:50:07.158] After Reprojection Map nPoint:19 ,nCell:19 ,nMatches:2 ,distance between ekf and vo x:-0.000419 ,z=0.000090,angle between two frames:0.000314 +[19:50:07.159] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =0.559892 error end=0.342748 +[19:50:07.159] After pose optimization, distance between ekf and vo x:0.566433 ,z=0.122553,angle between two frames:0.366084 +[19:50:07.159] frame process finished the id is: 72 the obs is:27 +[19:50:07.309] After Reprojection Map nPoint:19 ,nCell:17 ,nMatches:3 ,distance between ekf and vo x:-0.000423 ,z=0.000093,angle between two frames:0.000317 +[19:50:07.309] n deleted obs = 2 n obs with reprojection error less than 0.5 px =1 error init =0.519462 error end=0.519462 +[19:50:07.309] After pose optimization, distance between ekf and vo x:-0.000423 ,z=0.000093,angle between two frames:0.000317 +[19:50:07.309] frame process finished the id is: 72 the obs is:27 +[19:50:07.459] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:3 ,distance between ekf and vo x:-0.000415 ,z=0.000089,angle between two frames:0.000330 +[19:50:07.459] n deleted obs = 2 n obs with reprojection error less than 0.5 px =1 error init =0.808245 error end=0.724207 +[19:50:07.459] After pose optimization, distance between ekf and vo x:0.081013 ,z=0.086652,angle between two frames:0.117001 +[19:50:07.459] frame process finished the id is: 72 the obs is:27 +[19:50:07.610] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:-0.000423 ,z=0.000094,angle between two frames:0.000320 +[19:50:07.610] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =1.095265 error end=0.325966 +[19:50:07.610] After pose optimization, distance between ekf and vo x:0.955738 ,z=0.005653,angle between two frames:0.651015 +[19:50:07.610] frame process finished the id is: 72 the obs is:27 +[19:50:07.760] After Reprojection Map nPoint:17 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:-0.000438 ,z=0.000092,angle between two frames:0.000282 +[19:50:07.760] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =1.094546 error end=0.325556 +[19:50:07.760] After pose optimization, distance between ekf and vo x:0.955476 ,z=0.005089,angle between two frames:0.650386 +[19:50:07.760] frame process finished the id is: 72 the obs is:27 +[19:50:07.910] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:1 ,distance between ekf and vo x:-0.000465 ,z=0.000102,angle between two frames:0.000223 +[19:50:07.910] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =0.705726 error end=0.705726 +[19:50:07.910] After pose optimization, distance between ekf and vo x:-0.000465 ,z=0.000102,angle between two frames:0.000223 +[19:50:07.910] frame process finished the id is: 72 the obs is:27 +[19:50:08.061] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:-0.000494 ,z=0.000106,angle between two frames:0.000160 +[19:50:08.062] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =1.095196 error end=0.327532 +[19:50:08.062] After pose optimization, distance between ekf and vo x:0.953864 ,z=-0.003770,angle between two frames:0.647763 +[19:50:08.062] frame process finished the id is: 72 the obs is:27 +[19:50:08.212] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:-0.000493 ,z=0.000102,angle between two frames:0.000139 +[19:50:08.212] n deleted obs = 2 n obs with reprojection error less than 0.5 px =0 error init =1.095994 error end=0.934173 +[19:50:08.213] After pose optimization, distance between ekf and vo x:0.124590 ,z=0.138845,angle between two frames:0.140876 +[19:50:08.213] frame process finished the id is: 72 the obs is:27 +[19:50:08.363] After Reprojection Map nPoint:18 ,nCell:18 ,nMatches:2 ,distance between ekf and vo x:-0.000518 ,z=0.000114,angle between two frames:0.000130 +[19:50:08.363] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =1.470512 error end=0.349245 +[19:50:08.363] After pose optimization, distance between ekf and vo x:1.887699 ,z=0.441087,angle between two frames:1.198658 +[19:50:08.363] frame process finished the id is: 72 the obs is:27 +[19:50:08.515] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:-0.000516 ,z=0.000114,angle between two frames:0.000130 +[19:50:08.515] n deleted obs = 1 n obs with reprojection error less than 0.5 px =1 error init =1.095516 error end=0.386744 +[19:50:08.515] After pose optimization, distance between ekf and vo x:0.631280 ,z=-0.224338,angle between two frames:0.322889 +[19:50:08.515] frame process finished the id is: 72 the obs is:27 +[19:50:08.670] After Reprojection Map nPoint:16 ,nCell:16 ,nMatches:2 ,distance between ekf and vo x:-0.000514 ,z=0.000112,angle between two frames:0.000114 +[19:50:08.670] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =1.094567 error end=0.326945 +[19:50:08.670] After pose optimization, distance between ekf and vo x:0.954784 ,z=-0.000654,angle between two frames:0.648372 +[19:50:08.670] frame process finished the id is: 72 the obs is:27 +[19:50:08.822] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:2 ,distance between ekf and vo x:-0.000510 ,z=0.000109,angle between two frames:0.000119 +[19:50:08.822] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =1.094610 error end=0.326843 +[19:50:08.822] After pose optimization, distance between ekf and vo x:0.954973 ,z=-0.003156,angle between two frames:0.648100 +[19:50:08.822] frame process finished the id is: 72 the obs is:27 +[19:50:08.974] After Reprojection Map nPoint:15 ,nCell:15 ,nMatches:3 ,distance between ekf and vo x:-0.000509 ,z=0.000109,angle between two frames:0.000121 +[19:50:08.974] n deleted obs = 2 n obs with reprojection error less than 0.5 px =1 error init =0.809100 error end=0.650534 +[19:50:08.974] After pose optimization, distance between ekf and vo x:0.179821 ,z=0.326609,angle between two frames:0.311912 +[19:50:08.974] frame process finished the id is: 72 the obs is:27 +[19:50:09.126] After Reprojection Map nPoint:20 ,nCell:19 ,nMatches:5 ,distance between ekf and vo x:-0.000506 ,z=0.000108,angle between two frames:0.000119 +[19:50:09.126] n deleted obs = 3 n obs with reprojection error less than 0.5 px =2 error init =0.622118 error end=0.622118 +[19:50:09.126] After pose optimization, distance between ekf and vo x:-0.000506 ,z=0.000108,angle between two frames:0.000119 +[19:50:09.126] frame process finished the id is: 72 the obs is:27 +[19:50:09.277] After Reprojection Map nPoint:15 ,nCell:14 ,nMatches:2 ,distance between ekf and vo x:-0.000520 ,z=0.000114,angle between two frames:0.000126 +[19:50:09.277] n deleted obs = 0 n obs with reprojection error less than 0.5 px =2 error init =1.470709 error end=0.351836 +[19:50:09.277] After pose optimization, distance between ekf and vo x:2.019957 ,z=0.447408,angle between two frames:1.264244 +[19:50:09.277] frame process finished the id is: 72 the obs is:27 +[19:50:09.431] After Reprojection Map nPoint:17 ,nCell:17 ,nMatches:1 ,distance between ekf and vo x:-0.000517 ,z=0.000114,angle between two frames:0.000121 +[19:50:09.431] n deleted obs = 1 n obs with reprojection error less than 0.5 px =0 error init =1.484433 error end=1.484433 +[19:50:09.431] After pose optimization, distance between ekf and vo x:-0.000517 ,z=0.000114,angle between two frames:0.000121 +[19:50:09.431] frame process finished the id is: 72 the \ No newline at end of file diff --git a/GPU_version/vio/include/vio/global.h b/GPU_version/vio/include/vio/global.h index bb627cc..2710129 100644 --- a/GPU_version/vio/include/vio/global.h +++ b/GPU_version/vio/include/vio/global.h @@ -122,7 +122,7 @@ namespace vio //Todo add 15 roll orientation //Camera frame z front, x right, y down -> right hands Quaterniond q; - q = AngleAxisd(-0.122173, Vector3d::UnitX()) // roll + q = AngleAxisd(0.122173, Vector3d::UnitX()) // roll * AngleAxisd(pitch(), Vector3d::UnitY())// pitch * AngleAxisd(0.0, Vector3d::UnitZ()); //yaw SE3 tem=SE3(q.toRotationMatrix(),Vector3d(T2_->translation()(0), 0.0,T2_->translation()(1))); diff --git a/GPU_version/vio/include/vio/ukf.h b/GPU_version/vio/include/vio/ukf.h index a8bb0d3..e27c3f3 100644 --- a/GPU_version/vio/include/vio/ukf.h +++ b/GPU_version/vio/include/vio/ukf.h @@ -147,12 +147,12 @@ public: //Notice T_F_W is the position of the first frame with respect to the new frame while they are looking at one feature std::pair,vio::SE2_5> UpdateSvo(double x/*in camera frame*/,double z/*in camera frame*/,double pitch/*in camera frame*/) { boost::unique_lock lock(ekf_mut_); - filter_->correct(x,z,pitch); + filter_->correct(x,z,-1.0*pitch); Eigen::Matrix cov; cov<cov_(0,0),filter_->cov_(0,1),filter_->cov_(0,2), filter_->cov_(1,0),filter_->cov_(1,1),filter_->cov_(1,2), filter_->cov_(2,0),filter_->cov_(2,1),filter_->cov_(2,2); - return std::pair,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),(filter_->state_(2)))); + return std::pair,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2)))); }; std::pair,vio::SE2_5> get_location(){ boost::unique_lock lock(ekf_mut_); @@ -160,7 +160,7 @@ public: cov<cov_(0,0),filter_->cov_(0,1),filter_->cov_(0,2), filter_->cov_(1,0),filter_->cov_(1,1),filter_->cov_(1,2), filter_->cov_(2,0),filter_->cov_(2,1),filter_->cov_(2,2); - return std::pair,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),(filter_->state_(2)))); + return std::pair,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2)))); } size_t cmd_syn_count_=0; size_t imu_syn_count_=2; diff --git a/GPU_version/vio/kernel/compute-residual.cl b/GPU_version/vio/kernel/compute-residual.cl index 0f387ee..399c121 100644 --- a/GPU_version/vio/kernel/compute-residual.cl +++ b/GPU_version/vio/kernel/compute-residual.cl @@ -26,7 +26,7 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature) float3 error=ref+cur; float yaw=0.0; float pitch=error.z; - float roll=-0.122173; + float roll=0.122173; float R00=cos(yaw)*cos(pitch);//cos(pitch) float R01=cos(yaw)*sin(pitch)*sin(roll)-sin(yaw)*cos(roll);//0 float R02=cos(yaw)*sin(pitch)*cos(roll)+sin(yaw)*sin(roll);//sin(pitch) diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml index d09a936..e2597ef 100644 --- a/GPU_version/vio/param/vo_fast.yaml +++ b/GPU_version/vio/param/vo_fast.yaml @@ -5,11 +5,11 @@ svo: 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. init_min_tracked: 100 - init_min_disparity: 40 + init_min_disparity: 20 structureoptim_max_pts: 50 kfselect_mindist: 0.01 poseoptim_thresh: 8.0 - init_min_inliers: 20 + init_min_inliers: 25 ACC_noise: 0.5 #acc white noise in continuous in EKF GYO_noise: 0.5 #gyro white noise in continuous in EKF svo_ekf: 0.05 #Svo noise that will be devided by number of Reprojected points after opmization in EKF diff --git a/GPU_version/vio/src/feature_detection.cpp b/GPU_version/vio/src/feature_detection.cpp index 125950f..fb1fc22 100644 --- a/GPU_version/vio/src/feature_detection.cpp +++ b/GPU_version/vio/src/feature_detection.cpp @@ -117,7 +117,7 @@ void FastDetector::detect( } if(fts.size()<1){ ROS_ERROR("GPU Driver crash try again!"); - exit(0); + assert(false); } if(descriptors){ cv::Ptr extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4); diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index d0d9f40..c349a65 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -249,7 +249,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() for(auto&& it:new_frame_->fts_){ if(it->point != NULL)it->point->addFrameRef(it); } - //map_.point_candidates_.addCandidatePointToFrame(new_frame_); // if limited number of keyframes, remove the one furthest apart if(Config::maxNKfs() > 2 && map_.size() >= Config::maxNKfs()) diff --git a/GPU_version/vio/src/initialization.cpp b/GPU_version/vio/src/initialization.cpp index f2ae74d..9bb9b57 100644 --- a/GPU_version/vio/src/initialization.cpp +++ b/GPU_version/vio/src/initialization.cpp @@ -90,7 +90,7 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur) std::advance(first,id); if(frame_cur->cam_->isInFrame(Vector2d(px_cur_.at(id).x,px_cur_.at(id).y).cast(), 10) && frame_ref_->cam_->isInFrame((*first)->px.cast(), 10)){ - Vector3d pos = frame_cur->se3() * (xyz_in_cur_.at(id)/* *scale */); + Vector3d pos = xyz_in_cur_.at(id);//frame_cur->se3() * (xyz_in_cur_.at(id)/* *scale */); /*#if VIO_DEBUG fprintf(log_,"[%s] inlier: x=%f, y=%f, z=%f pos: x=%f y=%f z=%f\n", vio::time_in_HH_MM_SS_MMM().c_str(), diff --git a/GPU_version/vio/src/math_utils.cpp b/GPU_version/vio/src/math_utils.cpp index 3bd9708..b523571 100644 --- a/GPU_version/vio/src/math_utils.cpp +++ b/GPU_version/vio/src/math_utils.cpp @@ -22,7 +22,7 @@ triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t, Vector3d f2 = R * feature2; Vector2d b; b[0] = t.dot(feature1); - b[1] = t.dot(f2); + b[1] = t.dot(f2);//cross(t,R*f2) Matrix2d A; A(0,0) = feature1.dot(feature1); A(1,0) = feature1.dot(f2); @@ -67,13 +67,13 @@ reprojError(const Vector3d& f1, } double -computeInliers(const vector& features1, // c2 - const vector& features2, // c1 +computeInliers(const vector& features2, // c2 + const vector& features1, // c1 const Matrix3d& R, // R_c1_to_c2 const Vector3d& t, // T_c1_to_c2 const double reproj_thresh, double error_multiplier2, - vector& xyz_vec, // in frame c1 + vector& xyz_vec, // 3d point: optimal triangulation is to minimally correct the feature rays vector& inliers, vector& outliers) { @@ -85,10 +85,10 @@ computeInliers(const vector& features1, // c2 for(size_t j=0; j0.0){ - xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j], features2[j] )); + xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j]/*refrence*/, features2[j]/*current*/ )); double e1 = reprojError(features1[j], xyz_vec.back(), error_multiplier2); double e2 = reprojError(features2[j], R.transpose()*(xyz_vec.back()-t), error_multiplier2); - if(e1 > reproj_thresh || e2 > reproj_thresh || xyz_vec.back().z()<0.0) + if(e1 > reproj_thresh || e2 > reproj_thresh) outliers.push_back(j); else { diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index c0f758f..0f3dd6c 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -120,7 +120,7 @@ namespace vio { frame->c2f(px),f.item->f); if(new_point.z()<0.0)break; frame->fts_.push_back(std::make_shared(it_frame.item.first, - std::make_shared(frame->se3()*new_point,f.item), + std::make_shared(it_frame.item.first->se3()*new_point,f.item), px,f.item->f,(*point)->level)); frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_; grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));