diff --git a/GPU_version/vio/depth_filter_log.txt b/GPU_version/vio/depth_filter_log.txt
index e69de29..1ed7d76 100755
--- a/GPU_version/vio/depth_filter_log.txt
+++ b/GPU_version/vio/depth_filter_log.txt
@@ -0,0 +1,2 @@
+[20:26:15.484] New key frame 
+[20:26:15.484] BA loop 
diff --git a/GPU_version/vio/frame_handler_log.txt b/GPU_version/vio/frame_handler_log.txt
index a13e405..63f96af 100755
--- a/GPU_version/vio/frame_handler_log.txt
+++ b/GPU_version/vio/frame_handler_log.txt
@@ -1,249 +1,554 @@
-[19:03:41.830] frame process finished the id is: 0 the obs is:0 
-[19:03:41.919] frame process finished the id is: 1 the obs is:0 
-[19:03:42.009] frame process finished the id is: 2 the obs is:0 
-[19:03:42.112] frame process finished the id is: 3 the obs is:0 
-[19:03:42.209] frame process finished the id is: 4 the obs is:0 
-[19:03:42.308] frame process finished the id is: 5 the obs is:0 
-[19:03:42.408] frame process finished the id is: 6 the obs is:0 
-[19:03:42.508] frame process finished the id is: 7 the obs is:0 
-[19:03:42.607] frame process finished the id is: 8 the obs is:0 
-[19:03:42.709] frame process finished the id is: 9 the obs is:0 
-[19:03:42.746] Odometry x=0.000000, y=0.000000, theta=-0.000000
-[19:03:42.815] frame process finished the id is: 10 the obs is:0 
-[19:03:42.835] Odometry x=0.000000, y=0.000000, theta=-0.000000
-[19:03:42.908] frame process finished the id is: 11 the obs is:0 
-[19:03:42.933] Odometry x=0.000000, y=0.000035, theta=-0.014739
-[19:03:43.008] frame process finished the id is: 12 the obs is:0 
-[19:03:43.032] Odometry x=-0.000016, y=-0.000458, theta=-0.029702
-[19:03:43.108] frame process finished the id is: 13 the obs is:0 
-[19:03:43.136] Odometry x=-0.000030, y=0.000053, theta=-0.040488
-[19:03:43.208] frame process finished the id is: 14 the obs is:0 
-[19:03:43.234] Odometry x=-0.000002, y=0.000114, theta=-0.067076
-[19:03:43.308] frame process finished the id is: 15 the obs is:0 
-[19:03:43.333] Odometry x=0.000002, y=0.000144, theta=-0.072754
-[19:03:43.408] frame process finished the id is: 16 the obs is:0 
-[19:03:43.432] Odometry x=0.000199, y=0.000115, theta=-0.093481
-[19:03:43.517] frame process finished the id is: 17 the obs is:0 
-[19:03:43.533] Odometry x=0.000439, y=0.000166, theta=-0.108529
-[19:03:43.608] frame process finished the id is: 18 the obs is:0 
-[19:03:43.634] Odometry x=0.000517, y=0.000660, theta=-0.136896
-[19:03:43.708] frame process finished the id is: 19 the obs is:0 
-[19:03:43.734] Odometry x=0.000513, y=0.000632, theta=-0.156055
-[19:03:43.808] frame process finished the id is: 20 the obs is:0 
-[19:03:43.835] Odometry x=0.000528, y=0.000706, theta=-0.171952
-[19:03:43.909] frame process finished the id is: 21 the obs is:0 
-[19:03:43.937] Odometry x=0.000633, y=0.001607, theta=-0.186130
-[19:03:44.010] frame process finished the id is: 22 the obs is:0 
-[19:03:44.031] Odometry x=0.000712, y=0.001578, theta=-0.213187
-[19:03:44.113] frame process finished the id is: 23 the obs is:0 
-[19:03:44.135] Odometry x=0.000723, y=0.001702, theta=-0.238404
-[19:03:44.210] Init: frame zero: 0.000000, 0.000000 0.000000
-[19:03:44.210] Init: Selected first frame. 
-[19:03:44.210] frame process finished the id is: 24 the obs is:0 
-[19:03:44.229] Odometry x=0.000725, y=0.001732, theta=-0.250716
-[19:03:44.310] Init: Homography RANSAC (inlier) is:20 ,While 30 inliers minimum required.  px average disparity is:14.526801 ,While minimum is: 10.000000  KLT tracked: 128
-[19:03:44.310] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[19:03:44.310] frame process finished the id is: 25 the obs is:0 
-[19:03:44.332] Odometry x=0.000949, y=0.001734, theta=-0.279430
-[19:03:44.408] Init finished: Homography RANSAC (inlier) is:32 ,While 30 inliers minimum required.  px average disparity is:26.256217 ,While minimum is: 10.000000  KLT tracked: 128
-[19:03:44.408] Init: distance between the first and current frame is x:0.000966 ,z=0.001920,angle between two frames: -0.296072 
-[19:03:44.408] Init: Selected Second frame. 	 The number of features: 32 depth mean:0.005307 min:0.002690
-[19:03:44.408] frame process finished the id is: 26 the obs is:32 
-[19:03:44.435] Odometry x=0.000966, y=0.001920, theta=-0.296072
-[19:03:44.531] Odometry x=0.000975, y=0.001961, theta=-0.317395
-[19:03:44.633] Odometry x=0.001046, y=0.002359, theta=-0.336592
-[19:03:44.732] Odometry x=0.001104, y=0.002685, theta=-0.355629
-[19:03:44.834] Odometry x=0.001159, y=0.002986, theta=-0.377720
-[19:03:44.932] Odometry x=0.001584, y=0.002417, theta=-0.401479
-[19:03:45.032] Odometry x=0.001610, y=0.002432, theta=-0.416364
-[19:03:45.132] Odometry x=0.001706, y=0.002866, theta=-0.436506
-[19:03:45.232] Odometry x=0.002272, y=0.002762, theta=-0.461576
-[19:03:45.334] Odometry x=0.002354, y=0.003092, theta=-0.480614
-[19:03:45.434] Odometry x=0.002441, y=0.003028, theta=-0.500689
-[19:03:45.536] Odometry x=0.002518, y=0.002993, theta=-0.523179
-[19:03:45.636] Odometry x=0.002620, y=0.003166, theta=-0.544427
-[19:03:45.733] Odometry x=0.002642, y=0.003191, theta=-0.560905
-[19:03:45.833] Odometry x=0.002756, y=0.003916, theta=-0.581583
-[19:03:45.934] Odometry x=0.002788, y=0.004004, theta=-0.600291
-[19:03:46.034] Odometry x=0.003736, y=0.003123, theta=-0.625975
-[19:03:46.132] Odometry x=0.003734, y=0.003124, theta=-0.638540
-[19:03:46.234] Odometry x=0.003737, y=0.003125, theta=-0.656723
-[19:03:46.328] Odometry x=0.003808, y=0.003306, theta=-0.679839
-[19:03:46.424] Odometry x=0.003845, y=0.003391, theta=-0.693880
-[19:03:46.526] Odometry x=0.004147, y=0.004122, theta=-0.715479
-[19:03:46.628] Odometry x=0.004188, y=0.004159, theta=-0.740028
-[19:03:46.734] Odometry x=0.004320, y=0.004462, theta=-0.751417
-[19:03:46.828] Odometry x=0.004689, y=0.004159, theta=-0.776666
-[19:03:46.925] Odometry x=0.004845, y=0.003863, theta=-0.798853
-[19:03:47.025] Odometry x=0.004982, y=0.003988, theta=-0.811391
-[19:03:47.131] Odometry x=0.004986, y=0.003990, theta=-0.826543
-[19:03:47.227] Odometry x=0.005416, y=0.004450, theta=-0.859956
-[19:03:47.325] Odometry x=0.005503, y=0.004525, theta=-0.880350
-[19:03:47.425] Odometry x=0.005598, y=0.004628, theta=-0.899165
-[19:03:47.529] Odometry x=0.005898, y=0.004681, theta=-0.916101
-[19:03:47.629] Odometry x=0.006133, y=0.004716, theta=-0.938758
-[19:03:47.729] Odometry x=0.006647, y=0.005388, theta=-0.960163
-[19:03:47.829] Odometry x=0.006661, y=0.005367, theta=-0.980779
-[19:03:47.925] Odometry x=0.006784, y=0.005512, theta=-0.998739
-[19:03:48.028] Odometry x=0.007789, y=0.006762, theta=-1.014226
-[19:03:48.130] Odometry x=0.008178, y=0.007550, theta=-1.031749
-[19:03:48.225] Odometry x=0.008186, y=0.007549, theta=-1.050436
-[19:03:48.325] Odometry x=0.008208, y=0.007527, theta=-1.070095
-[19:03:48.425] Odometry x=0.008229, y=0.007454, theta=-1.092333
-[19:03:48.525] Odometry x=0.008394, y=0.007627, theta=-1.112394
-[19:03:48.626] Odometry x=0.008490, y=0.007914, theta=-1.127474
-[19:03:48.727] Odometry x=0.008507, y=0.007920, theta=-1.151183
-[19:03:48.829] Odometry x=0.008551, y=0.007974, theta=-1.176545
-[19:03:48.935] Odometry x=0.008610, y=0.007789, theta=-1.193454
-[19:03:49.033] Odometry x=0.008623, y=0.007737, theta=-1.208680
-[19:03:49.129] Odometry x=0.008761, y=0.007959, theta=-1.235255
-[19:03:49.231] Odometry x=0.008825, y=0.007868, theta=-1.257892
-[19:03:49.335] Odometry x=0.008851, y=0.007808, theta=-1.286898
-[19:03:49.428] Odometry x=0.008761, y=0.007739, theta=-1.287849
-[19:03:49.529] Odometry x=0.008803, y=0.007764, theta=-1.307717
-[19:03:49.631] Odometry x=0.009279, y=0.007995, theta=-1.341373
-[19:03:49.726] Odometry x=0.009278, y=0.007995, theta=-1.345607
-[19:03:49.825] Odometry x=0.009493, y=0.006214, theta=-1.377549
-[19:03:49.926] Odometry x=0.009491, y=0.006213, theta=-1.387627
-[19:03:50.030] Odometry x=0.009506, y=0.006066, theta=-1.422065
-[19:03:50.132] Odometry x=0.009511, y=0.005999, theta=-1.442972
-[19:03:50.229] Odometry x=0.009511, y=0.005998, theta=-1.460928
-[19:03:50.326] Odometry x=0.009603, y=0.004593, theta=-1.490979
-[19:03:50.431] Odometry x=0.009603, y=0.004587, theta=-1.485082
-[19:03:50.529] Odometry x=0.009655, y=0.004310, theta=-1.508717
-[19:03:50.626] Odometry x=0.009730, y=0.004140, theta=-1.528381
-[19:03:50.731] Odometry x=0.010140, y=0.002926, theta=-1.551244
-[19:03:50.826] Odometry x=0.010168, y=0.002738, theta=-1.567563
-[19:03:50.931] Odometry x=0.011157, y=0.001043, theta=-1.591306
-[19:03:51.028] Odometry x=0.011442, y=0.000775, theta=-1.625367
-[19:03:51.131] Odometry x=0.011494, y=0.000718, theta=-1.635808
-[19:03:51.228] Odometry x=0.011406, y=0.000731, theta=-1.646737
-[19:03:51.326] Odometry x=0.011459, y=0.000726, theta=-1.664412
-[19:03:51.431] Odometry x=0.011455, y=0.000553, theta=-1.687280
-[19:03:51.527] Odometry x=0.011507, y=0.000545, theta=-1.704368
-[19:03:51.627] Odometry x=0.011572, y=0.000527, theta=-1.726423
-[19:03:51.736] Odometry x=0.011790, y=0.000435, theta=-1.747009
-[19:03:51.829] Odometry x=0.011833, y=0.000382, theta=-1.773826
-[19:03:51.930] Odometry x=0.012100, y=0.000249, theta=-1.789493
-[19:03:52.032] Odometry x=0.012885, y=-0.000437, theta=-1.809109
-[19:03:52.133] Odometry x=0.012946, y=-0.002033, theta=-1.831527
-[19:03:52.232] Odometry x=0.012954, y=-0.002035, theta=-1.846979
-[19:03:52.330] Odometry x=0.012956, y=-0.002037, theta=-1.868389
-[19:03:52.426] Odometry x=0.013007, y=-0.001713, theta=-1.885884
-[19:03:52.533] Odometry x=0.013007, y=-0.001713, theta=-1.927876
-[19:03:52.630] Odometry x=0.013023, y=-0.002070, theta=-1.928569
-[19:03:52.726] Odometry x=0.012664, y=-0.001853, theta=-1.950556
-[19:03:52.832] Odometry x=0.012648, y=-0.001941, theta=-1.971663
-[19:03:52.934] Odometry x=0.012652, y=-0.001967, theta=-1.992112
-[19:03:53.031] Odometry x=0.012734, y=-0.002040, theta=-2.013194
-[19:03:53.134] Odometry x=0.013053, y=-0.002340, theta=-2.041808
-[19:03:53.231] Odometry x=0.012848, y=-0.002407, theta=-2.055036
-[19:03:53.334] Odometry x=0.012812, y=-0.003671, theta=-2.075133
-[19:03:53.431] Odometry x=0.012815, y=-0.003675, theta=-2.094489
-[19:03:53.534] Odometry x=0.013023, y=-0.003912, theta=-2.118870
-[19:03:53.631] Odometry x=0.013076, y=-0.003965, theta=-2.134809
-[19:03:53.726] Odometry x=0.013136, y=-0.004042, theta=-2.154602
-[19:03:53.833] Odometry x=0.013188, y=-0.004123, theta=-2.171104
-[19:03:53.930] Odometry x=0.012887, y=-0.005014, theta=-2.198842
-[19:03:54.031] Odometry x=0.012882, y=-0.005036, theta=-2.222160
-[19:03:54.131] Odometry x=0.012881, y=-0.005038, theta=-2.238859
-[19:03:54.226] Odometry x=0.012881, y=-0.005039, theta=-2.256346
-[19:03:54.332] Odometry x=0.012910, y=-0.005085, theta=-2.278863
-[19:03:54.432] Odometry x=0.012609, y=-0.005084, theta=-2.295296
-[19:03:54.536] Odometry x=0.012549, y=-0.006042, theta=-2.325994
-[19:03:54.632] Odometry x=0.012478, y=-0.006188, theta=-2.336751
-[19:03:54.733] Odometry x=0.012664, y=-0.005981, theta=-2.362241
-[19:03:54.835] Odometry x=0.012083, y=-0.009130, theta=-2.380840
-[19:03:54.931] Odometry x=0.012084, y=-0.009157, theta=-2.401395
-[19:03:55.026] Odometry x=0.010170, y=-0.012687, theta=-2.426134
-[19:03:55.131] Odometry x=0.009932, y=-0.013095, theta=-2.441288
-[19:03:55.234] Odometry x=0.009926, y=-0.013179, theta=-2.459024
-[19:03:55.335] Odometry x=0.008940, y=-0.014067, theta=-2.480464
-[19:03:55.425] Odometry x=0.008955, y=-0.014154, theta=-2.502435
-[19:03:55.532] Odometry x=0.008929, y=-0.014190, theta=-2.519539
-[19:03:55.628] Odometry x=0.009107, y=-0.014691, theta=-2.536558
-[19:03:55.727] Odometry x=0.008473, y=-0.014609, theta=-2.561450
-[19:03:55.834] Odometry x=0.008448, y=-0.014656, theta=-2.581408
-[19:03:55.930] Odometry x=0.008504, y=-0.014871, theta=-2.607664
-[19:03:56.031] Odometry x=0.008514, y=-0.014904, theta=-2.628230
-[19:03:56.127] Odometry x=0.008498, y=-0.014898, theta=-2.644026
-[19:03:56.234] Odometry x=0.008660, y=-0.015495, theta=-2.665277
-[19:03:56.336] Odometry x=0.008834, y=-0.015391, theta=-2.674092
-[19:03:56.433] Odometry x=0.008803, y=-0.015431, theta=-2.710954
-[19:03:56.533] Odometry x=0.008780, y=-0.015329, theta=-2.722390
-[19:03:56.635] Odometry x=0.008781, y=-0.015351, theta=-2.737733
-[19:03:56.729] Odometry x=0.008777, y=-0.015355, theta=-2.763386
-[19:03:56.836] Odometry x=0.008776, y=-0.015351, theta=-2.784200
-[19:03:56.927] Odometry x=0.008822, y=-0.015319, theta=-2.801707
-[19:03:57.030] Odometry x=0.008791, y=-0.015302, theta=-2.824509
-[19:03:57.132] Odometry x=0.008785, y=-0.015290, theta=-2.839928
-[19:03:57.230] Odometry x=0.008830, y=-0.015827, theta=-2.860562
-[19:03:57.329] Odometry x=0.008738, y=-0.015895, theta=-2.881298
-[19:03:57.429] Odometry x=0.008739, y=-0.015897, theta=-2.902525
-[19:03:57.527] Odometry x=0.008733, y=-0.015907, theta=-2.917228
-[19:03:57.629] Odometry x=0.008718, y=-0.015918, theta=-2.943145
-[19:03:57.736] Odometry x=0.008713, y=-0.015920, theta=-2.964944
-[19:03:57.833] Odometry x=0.007364, y=-0.016168, theta=-2.982891
-[19:03:57.933] Odometry x=0.007335, y=-0.016174, theta=-2.998146
-[19:03:58.029] Odometry x=0.007346, y=-0.016186, theta=-3.024654
-[19:03:58.132] Odometry x=0.007419, y=-0.017356, theta=-3.039007
-[19:03:58.230] Odometry x=0.007430, y=-0.017456, theta=-3.059337
-[19:03:58.331] Odometry x=0.007551, y=-0.017429, theta=-3.078037
-[19:03:58.433] Odometry x=0.007552, y=-0.017453, theta=-3.096738
-[19:03:58.530] Odometry x=0.007567, y=-0.018106, theta=-3.118657
-[19:03:58.631] Odometry x=0.007568, y=-0.018106, theta=-3.137664
-[19:03:58.731] Odometry x=0.007570, y=-0.018102, theta=3.127204
-[19:03:58.826] Odometry x=0.007495, y=-0.017425, theta=3.104639
-[19:03:58.926] Odometry x=0.007501, y=-0.017425, theta=3.089735
-[19:03:59.028] Odometry x=0.007501, y=-0.017424, theta=3.067425
-[19:03:59.128] Odometry x=0.007502, y=-0.017425, theta=3.048360
-[19:03:59.224] Odometry x=0.007499, y=-0.017488, theta=3.027172
-[19:03:59.328] Odometry x=0.007504, y=-0.017553, theta=3.010471
-[19:03:59.435] Odometry x=0.007496, y=-0.017542, theta=2.991329
-[19:03:59.528] Odometry x=0.008968, y=-0.017968, theta=2.962129
-[19:03:59.629] Odometry x=0.008960, y=-0.018059, theta=2.942655
-[19:03:59.725] Odometry x=0.008926, y=-0.018060, theta=2.931529
-[19:03:59.833] Odometry x=0.008942, y=-0.018064, theta=2.908479
-[19:03:59.929] Odometry x=0.008928, y=-0.018184, theta=2.888073
-[19:04:00.025] Odometry x=0.008920, y=-0.018240, theta=2.869678
-[19:04:00.126] Odometry x=0.008974, y=-0.018269, theta=2.843394
-[19:04:00.231] Odometry x=0.009053, y=-0.018319, theta=2.827975
-[19:04:00.330] Odometry x=0.008876, y=-0.019114, theta=2.809267
-[19:04:00.428] Odometry x=0.008893, y=-0.019109, theta=2.784339
-[19:04:00.531] Odometry x=0.009001, y=-0.019189, theta=2.761568
-[19:04:00.632] Odometry x=0.009011, y=-0.019196, theta=2.739901
-[19:04:00.728] Odometry x=0.009014, y=-0.019204, theta=2.720543
-[19:04:00.835] Odometry x=0.008974, y=-0.019382, theta=2.706724
-[19:04:00.932] Odometry x=0.008849, y=-0.019886, theta=2.688520
-[19:04:01.031] Odometry x=0.008965, y=-0.019993, theta=2.667348
-[19:04:01.131] Odometry x=0.008965, y=-0.019992, theta=2.644108
-[19:04:01.228] Odometry x=0.008959, y=-0.020017, theta=2.627208
-[19:04:01.335] Odometry x=0.008941, y=-0.020082, theta=2.604776
-[19:04:01.436] Odometry x=0.008923, y=-0.020110, theta=2.587249
-[19:04:01.532] Odometry x=0.008963, y=-0.019989, theta=2.575068
-[19:04:01.634] Odometry x=0.008968, y=-0.019995, theta=2.551009
-[19:04:01.737] Odometry x=0.009010, y=-0.020033, theta=2.537524
-[19:04:01.833] Odometry x=0.008602, y=-0.019470, theta=2.514726
-[19:04:01.933] Odometry x=0.008673, y=-0.019627, theta=2.488872
-[19:04:02.032] Odometry x=0.008504, y=-0.019354, theta=2.471651
-[19:04:02.131] Odometry x=0.008467, y=-0.019444, theta=2.455800
-[19:04:02.228] Odometry x=0.008463, y=-0.019448, theta=2.434341
-[19:04:02.336] Odometry x=0.008491, y=-0.019426, theta=2.412975
-[19:04:02.436] Odometry x=0.008141, y=-0.020016, theta=2.391785
-[19:04:02.532] Odometry x=0.008128, y=-0.020043, theta=2.371354
-[19:04:02.637] Odometry x=0.008126, y=-0.020038, theta=2.350333
-[19:04:02.730] Odometry x=0.008055, y=-0.020178, theta=2.334459
-[19:04:02.834] Odometry x=0.007819, y=-0.021943, theta=2.313763
-[19:04:02.932] Odometry x=0.007909, y=-0.022144, theta=2.292213
-[19:04:03.027] Odometry x=0.007905, y=-0.022139, theta=2.275083
-[19:04:03.134] Odometry x=0.007509, y=-0.022709, theta=2.254945
-[19:04:03.230] Odometry x=0.007436, y=-0.022965, theta=2.241917
-[19:04:03.330] Odometry x=0.007229, y=-0.023300, theta=2.215787
-[19:04:03.431] Odometry x=0.007214, y=-0.023319, theta=2.196995
-[19:04:03.526] Odometry x=0.007153, y=-0.023402, theta=2.180305
-[19:04:03.627] Odometry x=0.007195, y=-0.023549, theta=2.158489
-[19:04:03.731] Odometry x=0.007050, y=-0.023255, theta=2.137535
-[19:04:03.835] Odometry x=0.006992, y=-0.023071, theta=2.117542
-[19:04:03.930] Odometry x=0.006981, y=-0.023039, theta=2.098960
-[19:04:04.031] Odometry x=0.006274, y=-0.020648, theta=2.077011
-[19:04:04.126] Odometry
\ No newline at end of file
+[20:26:07.395] Init: frame zero: 0.000000, 0.000000 0.000000
+[20:26:07.396] Init: Selected first frame. 
+[20:26:07.396] frame process finished the id is: 0 the obs is:0 
+[20:26:07.428] Init: px average disparity is:0.193038 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:07.428] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:07.428] frame process finished the id is: 1 the obs is:0 
+[20:26:07.518] Init: px average disparity is:0.233485 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:07.518] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:07.518] frame process finished the id is: 2 the obs is:0 
+[20:26:07.619] Init: px average disparity is:0.249695 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:07.619] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:07.619] frame process finished the id is: 3 the obs is:0 
+[20:26:07.720] Init: px average disparity is:0.246420 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:07.720] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:07.720] frame process finished the id is: 4 the obs is:0 
+[20:26:07.818] Init: px average disparity is:0.246253 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:07.818] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:07.819] frame process finished the id is: 5 the obs is:0 
+[20:26:07.927] Init: px average disparity is:0.239550 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:07.927] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:07.927] frame process finished the id is: 6 the obs is:0 
+[20:26:08.025] Init: px average disparity is:0.248205 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:08.025] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:08.025] frame process finished the id is: 7 the obs is:0 
+[20:26:08.125] Init: px average disparity is:0.233784 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:08.125] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:08.125] frame process finished the id is: 8 the obs is:0 
+[20:26:08.218] Init: px average disparity is:0.261833 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:08.218] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:08.218] frame process finished the id is: 9 the obs is:0 
+[20:26:08.317] Init: px average disparity is:0.242433 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:08.317] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:08.317] frame process finished the id is: 10 the obs is:0 
+[20:26:08.421] Init: px average disparity is:0.246183 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:08.421] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:08.421] frame process finished the id is: 11 the obs is:0 
+[20:26:08.517] Init: px average disparity is:0.256843 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:08.517] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:08.517] frame process finished the id is: 12 the obs is:0 
+[20:26:08.618] Init: px average disparity is:0.242095 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:08.618] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:08.618] frame process finished the id is: 13 the obs is:0 
+[20:26:08.717] Init: px average disparity is:0.244411 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:08.718] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:08.718] frame process finished the id is: 14 the obs is:0 
+[20:26:08.817] Init: px average disparity is:0.231794 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:08.817] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:08.817] frame process finished the id is: 15 the obs is:0 
+[20:26:08.925] Init: px average disparity is:0.245239 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:08.925] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:08.925] frame process finished the id is: 16 the obs is:0 
+[20:26:09.026] Init: px average disparity is:0.274690 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:09.026] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:09.026] frame process finished the id is: 17 the obs is:0 
+[20:26:09.118] Init: px average disparity is:0.245508 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:09.118] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:09.118] frame process finished the id is: 18 the obs is:0 
+[20:26:09.218] Init: px average disparity is:0.235310 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:09.218] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:09.218] frame process finished the id is: 19 the obs is:0 
+[20:26:09.318] Init: px average disparity is:0.239237 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:09.318] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:09.318] frame process finished the id is: 20 the obs is:0 
+[20:26:09.418] Init: px average disparity is:0.255277 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:09.418] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:09.418] frame process finished the id is: 21 the obs is:0 
+[20:26:09.517] Init: px average disparity is:0.264634 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:09.517] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:09.517] frame process finished the id is: 22 the obs is:0 
+[20:26:09.618] Init: px average disparity is:0.239783 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:09.618] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:09.618] frame process finished the id is: 23 the obs is:0 
+[20:26:09.717] Init: px average disparity is:0.236109 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:09.717] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:09.717] frame process finished the id is: 24 the obs is:0 
+[20:26:09.817] Init: px average disparity is:0.275557 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:09.817] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:09.817] frame process finished the id is: 25 the obs is:0 
+[20:26:09.920] Init: px average disparity is:0.253125 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:09.920] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:09.920] frame process finished the id is: 26 the obs is:0 
+[20:26:10.025] Init: px average disparity is:0.221022 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:10.025] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:10.025] frame process finished the id is: 27 the obs is:0 
+[20:26:10.126] Init: px average disparity is:0.276293 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:10.126] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:10.126] frame process finished the id is: 28 the obs is:0 
+[20:26:10.218] Init: px average disparity is:0.235747 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:10.219] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:10.219] frame process finished the id is: 29 the obs is:0 
+[20:26:10.266] Odometry x=0.000000, y=0.000000, theta=-0.000000
+[20:26:10.318] Init: px average disparity is:0.238283 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:10.318] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:10.318] frame process finished the id is: 30 the obs is:0 
+[20:26:10.362] Odometry x=0.000000, y=0.000000, theta=-0.000000
+[20:26:10.418] Init: px average disparity is:0.216285 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:10.418] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:10.418] frame process finished the id is: 31 the obs is:0 
+[20:26:10.462] Odometry x=0.000034, y=0.006145, theta=-0.001079
+[20:26:10.518] Init: px average disparity is:1.756643 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:10.518] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:10.518] frame process finished the id is: 32 the obs is:0 
+[20:26:10.563] Odometry x=0.000268, y=0.012819, theta=-0.001817
+[20:26:10.618] Init: px average disparity is:6.015268 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:10.619] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:10.619] frame process finished the id is: 33 the obs is:0 
+[20:26:10.663] Odometry x=0.000378, y=0.019884, theta=-0.004688
+[20:26:10.719] Init: px average disparity is:7.672971 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:10.719] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:10.719] frame process finished the id is: 34 the obs is:0 
+[20:26:10.765] Odometry x=0.000444, y=0.027054, theta=-0.000615
+[20:26:10.819] Init: px average disparity is:5.004219 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:10.819] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:10.819] frame process finished the id is: 35 the obs is:0 
+[20:26:10.867] Odometry x=0.000491, y=0.032464, theta=-0.001716
+[20:26:10.918] Init: px average disparity is:5.386235 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:10.918] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:10.918] frame process finished the id is: 36 the obs is:0 
+[20:26:10.966] Odometry x=0.002166, y=0.040503, theta=0.000096
+[20:26:11.026] Init: px average disparity is:4.313007 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:11.026] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:11.026] frame process finished the id is: 37 the obs is:0 
+[20:26:11.067] Odometry x=0.002069, y=0.045355, theta=-0.008244
+[20:26:11.128] Init: px average disparity is:5.801908 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:11.128] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:11.128] frame process finished the id is: 38 the obs is:0 
+[20:26:11.162] Odometry x=0.002113, y=0.054256, theta=0.005705
+[20:26:11.219] Init: px average disparity is:4.169210 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:11.219] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:11.219] frame process finished the id is: 39 the obs is:0 
+[20:26:11.265] Odometry x=0.002137, y=0.058516, theta=-0.015250
+[20:26:11.319] Init: px average disparity is:6.068905 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:11.319] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:11.319] frame process finished the id is: 40 the obs is:0 
+[20:26:11.362] Odometry x=0.002180, y=0.064597, theta=-0.003391
+[20:26:11.419] Init: px average disparity is:7.075865 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:11.419] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:11.419] frame process finished the id is: 41 the obs is:0 
+[20:26:11.465] Odometry x=0.002191, y=0.065761, theta=-0.004520
+[20:26:11.519] Init: px average disparity is:8.984360 ,While minimum is: 10.000000  KLT tracked : 122
+[20:26:11.519] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:11.519] frame process finished the id is: 42 the obs is:0 
+[20:26:11.563] Odometry x=0.002198, y=0.070996, theta=0.002432
+[20:26:11.665] Odometry x=0.002276, y=0.076357, theta=-0.019586
+[20:26:11.683] Init: Homography RANSAC (inlier) is:20 ,While 30 inliers minimum required.  px average disparity is:12.212644 ,While minimum is: 10.000000  KLT tracked: 122
+[20:26:11.683] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:11.683] frame process finished the id is: 43 the obs is:0 
+[20:26:11.768] Odometry x=0.002692, y=0.082072, theta=-0.008944
+[20:26:11.783] Init: Homography RANSAC (inlier) is:16 ,While 30 inliers minimum required.  px average disparity is:14.997284 ,While minimum is: 10.000000  KLT tracked: 122
+[20:26:11.783] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:11.783] frame process finished the id is: 44 the obs is:0 
+[20:26:11.865] Odometry x=0.002698, y=0.087441, theta=-0.004901
+[20:26:11.882] Init: Homography RANSAC (inlier) is:23 ,While 30 inliers minimum required.  px average disparity is:13.054565 ,While minimum is: 10.000000  KLT tracked: 122
+[20:26:11.882] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:11.882] frame process finished the id is: 45 the obs is:0 
+[20:26:11.962] Odometry x=0.002935, y=0.095479, theta=-0.004836
+[20:26:11.983] Init: Homography RANSAC (inlier) is:23 ,While 30 inliers minimum required.  px average disparity is:12.995508 ,While minimum is: 10.000000  KLT tracked: 122
+[20:26:11.983] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:11.983] frame process finished the id is: 46 the obs is:0 
+[20:26:12.082] Init: Homography RANSAC (inlier) is:23 ,While 30 inliers minimum required.  px average disparity is:11.497112 ,While minimum is: 10.000000  KLT tracked: 122
+[20:26:12.082] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:12.082] frame process finished the id is: 47 the obs is:0 
+[20:26:12.176] Odometry x=0.002940, y=0.101466, theta=0.001964
+[20:26:12.246] Init: Homography RANSAC (inlier) is:23 ,While 30 inliers minimum required.  px average disparity is:13.703494 ,While minimum is: 10.000000  KLT tracked: 122
+[20:26:12.246] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:12.246] frame process finished the id is: 48 the obs is:0 
+[20:26:12.263] Odometry x=0.004079, y=0.136473, theta=0.001322
+[20:26:12.317] Init: Homography RANSAC (inlier) is:23 ,While 30 inliers minimum required.  px average disparity is:13.685114 ,While minimum is: 10.000000  KLT tracked: 121
+[20:26:12.317] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:12.317] frame process finished the id is: 49 the obs is:0 
+[20:26:12.376] Odometry x=0.005514, y=0.143098, theta=0.006983
+[20:26:12.389] Init: Homography RANSAC (inlier) is:25 ,While 30 inliers minimum required.  px average disparity is:19.846595 ,While minimum is: 10.000000  KLT tracked: 121
+[20:26:12.389] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:12.389] frame process finished the id is: 50 the obs is:0 
+[20:26:12.468] Odometry x=0.008547, y=0.148823, theta=-0.011650
+[20:26:12.482] Init: Homography RANSAC (inlier) is:21 ,While 30 inliers minimum required.  px average disparity is:13.966839 ,While minimum is: 10.000000  KLT tracked: 121
+[20:26:12.482] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:12.482] frame process finished the id is: 51 the obs is:0 
+[20:26:12.565] Odometry x=0.008679, y=0.162002, theta=-0.011356
+[20:26:12.582] Init: Homography RANSAC (inlier) is:17 ,While 30 inliers minimum required.  px average disparity is:13.602436 ,While minimum is: 10.000000  KLT tracked: 121
+[20:26:12.582] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:12.582] frame process finished the id is: 52 the obs is:0 
+[20:26:12.620] Init: px average disparity is:8.264399 ,While minimum is: 10.000000  KLT tracked : 121
+[20:26:12.620] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:12.620] frame process finished the id is: 53 the obs is:0 
+[20:26:12.665] Odometry x=0.010518, y=0.167322, theta=-0.015843
+[20:26:12.718] Init: px average disparity is:8.702670 ,While minimum is: 10.000000  KLT tracked : 121
+[20:26:12.719] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:12.719] frame process finished the id is: 54 the obs is:0 
+[20:26:12.761] Odometry x=0.011297, y=0.174707, theta=-0.018702
+[20:26:12.819] Init: px average disparity is:8.172228 ,While minimum is: 10.000000  KLT tracked : 120
+[20:26:12.819] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:12.819] frame process finished the id is: 55 the obs is:0 
+[20:26:12.865] Odometry x=0.013217, y=0.179789, theta=-0.015301
+[20:26:12.918] Init: px average disparity is:9.042699 ,While minimum is: 10.000000  KLT tracked : 120
+[20:26:12.918] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:12.918] frame process finished the id is: 56 the obs is:0 
+[20:26:12.967] Odometry x=0.013295, y=0.186605, theta=-0.013279
+[20:26:13.070] Odometry x=0.013985, y=0.192115, theta=-0.019430
+[20:26:13.084] Init: Homography RANSAC (inlier) is:17 ,While 30 inliers minimum required.  px average disparity is:10.031950 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:13.084] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:13.084] frame process finished the id is: 57 the obs is:0 
+[20:26:13.119] Init: px average disparity is:7.801427 ,While minimum is: 10.000000  KLT tracked : 120
+[20:26:13.119] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:13.119] frame process finished the id is: 58 the obs is:0 
+[20:26:13.170] Odometry x=0.014012, y=0.197635, theta=0.005024
+[20:26:13.270] Odometry x=0.014179, y=0.203075, theta=-0.005712
+[20:26:13.289] Init: Homography RANSAC (inlier) is:17 ,While 30 inliers minimum required.  px average disparity is:10.614943 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:13.289] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:13.289] frame process finished the id is: 59 the obs is:0 
+[20:26:13.367] Odometry x=0.014204, y=0.211022, theta=-0.001525
+[20:26:13.381] Init: Homography RANSAC (inlier) is:17 ,While 30 inliers minimum required.  px average disparity is:12.945698 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:13.381] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:13.381] frame process finished the id is: 60 the obs is:0 
+[20:26:13.470] Odometry x=0.014739, y=0.216315, theta=-0.004075
+[20:26:13.482] Init: Homography RANSAC (inlier) is:17 ,While 30 inliers minimum required.  px average disparity is:13.766396 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:13.482] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:13.482] frame process finished the id is: 61 the obs is:0 
+[20:26:13.563] Odometry x=0.014752, y=0.222059, theta=0.004348
+[20:26:13.583] Init: Homography RANSAC (inlier) is:17 ,While 30 inliers minimum required.  px average disparity is:14.888374 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:13.583] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:13.583] frame process finished the id is: 62 the obs is:0 
+[20:26:13.665] Odometry x=0.014820, y=0.227487, theta=-0.003363
+[20:26:13.682] Init: Homography RANSAC (inlier) is:17 ,While 30 inliers minimum required.  px average disparity is:15.213157 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:13.682] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:13.682] frame process finished the id is: 63 the obs is:0 
+[20:26:13.763] Odometry x=0.014823, y=0.233135, theta=0.001407
+[20:26:13.783] Init: Homography RANSAC (inlier) is:17 ,While 30 inliers minimum required.  px average disparity is:15.544931 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:13.783] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:13.783] frame process finished the id is: 64 the obs is:0 
+[20:26:13.866] Odometry x=0.014842, y=0.238937, theta=-0.007679
+[20:26:13.883] Init: Homography RANSAC (inlier) is:24 ,While 30 inliers minimum required.  px average disparity is:17.781527 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:13.884] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:13.884] frame process finished the id is: 65 the obs is:0 
+[20:26:13.964] Odometry x=0.014863, y=0.244441, theta=-0.004143
+[20:26:13.983] Init: Homography RANSAC (inlier) is:24 ,While 30 inliers minimum required.  px average disparity is:21.221079 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:13.983] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:13.983] frame process finished the id is: 66 the obs is:0 
+[20:26:14.066] Odometry x=0.014944, y=0.252357, theta=0.006257
+[20:26:14.082] Init: Homography RANSAC (inlier) is:24 ,While 30 inliers minimum required.  px average disparity is:19.235613 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:14.082] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:14.082] frame process finished the id is: 67 the obs is:0 
+[20:26:14.182] Init: Homography RANSAC (inlier) is:24 ,While 30 inliers minimum required.  px average disparity is:18.318094 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:14.182] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:14.182] frame process finished the id is: 68 the obs is:0 
+[20:26:14.244] Odometry x=0.011538, y=0.257891, theta=-0.008820
+[20:26:14.264] Odometry x=0.003988, y=0.269652, theta=0.011341
+[20:26:14.311] Init: Homography RANSAC (inlier) is:22 ,While 30 inliers minimum required.  px average disparity is:17.502340 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:14.311] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:14.311] frame process finished the id is: 69 the obs is:0 
+[20:26:14.367] Odometry x=0.004064, y=0.270816, theta=0.011376
+[20:26:14.387] Init: Homography RANSAC (inlier) is:22 ,While 30 inliers minimum required.  px average disparity is:17.451370 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:14.387] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:14.387] frame process finished the id is: 70 the obs is:0 
+[20:26:14.464] Odometry x=0.004011, y=0.282976, theta=0.028089
+[20:26:14.484] Init: Homography RANSAC (inlier) is:22 ,While 30 inliers minimum required.  px average disparity is:19.749812 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:14.484] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:14.484] frame process finished the id is: 71 the obs is:0 
+[20:26:14.567] Odometry x=0.004224, y=0.288597, theta=-0.000842
+[20:26:14.585] Init: Homography RANSAC (inlier) is:22 ,While 30 inliers minimum required.  px average disparity is:19.885912 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:14.585] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:14.585] frame process finished the id is: 72 the obs is:0 
+[20:26:14.664] Odometry x=0.003372, y=0.294763, theta=0.002552
+[20:26:14.684] Init: Homography RANSAC (inlier) is:22 ,While 30 inliers minimum required.  px average disparity is:20.853653 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:14.684] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:14.684] frame process finished the id is: 73 the obs is:0 
+[20:26:14.766] Odometry x=0.003379, y=0.300152, theta=-0.009445
+[20:26:14.784] Init: Homography RANSAC (inlier) is:23 ,While 30 inliers minimum required.  px average disparity is:21.283597 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:14.784] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:14.784] frame process finished the id is: 74 the obs is:0 
+[20:26:14.863] Odometry x=0.003181, y=0.312033, theta=-0.004359
+[20:26:14.884] Init: Homography RANSAC (inlier) is:23 ,While 30 inliers minimum required.  px average disparity is:19.664587 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:14.884] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:14.884] frame process finished the id is: 75 the obs is:0 
+[20:26:14.965] Odometry x=0.003208, y=0.317280, theta=-0.013285
+[20:26:14.984] Init: Homography RANSAC (inlier) is:23 ,While 30 inliers minimum required.  px average disparity is:20.784579 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:14.984] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:14.984] frame process finished the id is: 76 the obs is:0 
+[20:26:15.061] Odometry x=0.002756, y=0.329010, theta=-0.002899
+[20:26:15.085] Init: Homography RANSAC (inlier) is:28 ,While 30 inliers minimum required.  px average disparity is:20.404713 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:15.085] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:15.085] frame process finished the id is: 77 the obs is:0 
+[20:26:15.163] Odometry x=0.002826, y=0.334238, theta=-0.015803
+[20:26:15.185] Init: Homography RANSAC (inlier) is:28 ,While 30 inliers minimum required.  px average disparity is:22.426086 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:15.185] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:15.185] frame process finished the id is: 78 the obs is:0 
+[20:26:15.267] Odometry x=0.002871, y=0.339884, theta=-0.011735
+[20:26:15.285] Init: Homography RANSAC (inlier) is:28 ,While 30 inliers minimum required.  px average disparity is:20.809869 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:15.285] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:15.285] frame process finished the id is: 79 the obs is:0 
+[20:26:15.368] Odometry x=0.002916, y=0.345347, theta=-0.018312
+[20:26:15.385] Init: Homography RANSAC (inlier) is:28 ,While 30 inliers minimum required.  px average disparity is:21.881003 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:15.385] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
+[20:26:15.385] frame process finished the id is: 80 the obs is:0 
+[20:26:15.465] Odometry x=0.002768, y=0.357855, theta=-0.006515
+[20:26:15.484] Init finished: Homography RANSAC (inlier) is:31 ,While 30 inliers minimum required.  px average disparity is:20.756724 ,While minimum is: 10.000000  KLT tracked: 120
+[20:26:15.484] Init: distance between the first and current frame is x:0.002768 ,z=0.357855,angle between two frames: -0.006515 
+[20:26:15.484] Init: Selected Second frame. 	 The number of features: 31 depth mean:-1.039710 min:-12.255072
+[20:26:15.484] frame process finished the id is: 81 the obs is:31 
+[20:26:15.564] Odometry x=0.002959, y=0.362196, theta=-0.022348
+[20:26:15.667] Odometry x=0.004059, y=0.367156, theta=-0.002329
+[20:26:15.764] Odometry x=0.004449, y=0.372713, theta=-0.031232
+[20:26:15.866] Odometry x=-0.004151, y=0.392704, theta=0.000852
+[20:26:15.967] Odometry x=-0.002742, y=0.396878, theta=-0.018497
+[20:26:16.067] Odometry x=-0.002238, y=0.404822, theta=-0.022665
+[20:26:16.165] Odometry x=-0.000611, y=0.409503, theta=-0.009577
+[20:26:16.278] Odometry x=-0.000425, y=0.418797, theta=-0.029908
+[20:26:16.366] Odometry x=-0.002896, y=0.424955, theta=-0.042433
+[20:26:16.465] Odometry x=-0.002140, y=0.437052, theta=-0.035986
+[20:26:16.569] Odometry x=-0.003961, y=0.440769, theta=-0.050165
+[20:26:16.587] After Reprojection Map nPoint:31 ,nCell:23 ,nMatches:4 ,distance between ekf and vo x:-0.000015 ,z=0.000084,angle between two frames:0.000001
+[20:26:16.587] n deleted obs = 0 	 n obs with reprojection error less than 0.5 px =4 	 error init =0.135214 	 error end=0.135214
+[20:26:16.587] After pose optimization, distance between ekf and vo x:-0.000015 ,z=0.000084,angle between two frames:0.000001
+[20:26:16.587] frame process finished the id is: 81 the obs is:31 
+[20:26:16.668] Odometry x=-0.003568, y=0.447286, theta=-0.028085
+[20:26:16.769] Odometry x=-0.003472, y=0.452226, theta=-0.033942
+[20:26:16.865] Odometry x=-0.003362, y=0.457962, theta=-0.024311
+[20:26:16.964] Odometry x=-0.003283, y=0.463670, theta=-0.009574
+[20:26:17.069] Odometry x=-0.003204, y=0.469157, theta=-0.025033
+[20:26:17.168] Odometry x=-0.003163, y=0.478374, theta=-0.014665
+[20:26:17.265] Odometry x=-0.002752, y=0.483840, theta=-0.022804
+[20:26:17.368] Odometry x=-0.007956, y=0.501350, theta=-0.020378
+[20:26:17.468] Odometry x=-0.007849, y=0.506856, theta=-0.027906
+[20:26:17.569] Odometry x=-0.013239, y=0.521342, theta=-0.020892
+[20:26:17.667] Odometry x=-0.013220, y=0.526666, theta=-0.034911
+[20:26:17.706] After Reprojection Map nPoint:31 ,nCell:25 ,nMatches:4 ,distance between ekf and vo x:-0.000059 ,z=0.000575,angle between two frames:0.000010
+[20:26:17.706] n deleted obs = 0 	 n obs with reprojection error less than 0.5 px =4 	 error init =0.155124 	 error end=0.155124
+[20:26:17.706] After pose optimization, distance between ekf and vo x:-0.000059 ,z=0.000575,angle between two frames:0.000010
+[20:26:17.706] frame process finished the id is: 81 the obs is:31 
+[20:26:17.769] Odometry x=-0.013093, y=0.532400, theta=-0.014265
+[20:26:17.865] Odometry x=-0.020223, y=0.554510, theta=-0.037547
+[20:26:17.963] Odometry x=-0.019661, y=0.560334, theta=-0.038813
+[20:26:18.065] Odometry x=-0.030401, y=0.572928, theta=-0.042148
+[20:26:18.168] Odometry x=-0.030113, y=0.578457, theta=-0.042838
+[20:26:18.266] Odometry x=-0.039932, y=0.584835, theta=-0.051481
+[20:26:18.459] Odometry x=-0.038352, y=0.594345, theta=-0.032860
+[20:26:18.469] Odometry x=-0.038352, y=0.594345, theta=-0.032860
+[20:26:18.564] Odometry x=-0.038326, y=0.595262, theta=-0.048934
+[20:26:18.664] Odometry x=-0.038204, y=0.600348, theta=-0.043864
+[20:26:18.767] Odometry x=-0.037194, y=0.610185, theta=-0.048238
+[20:26:18.774] After Reprojection Map nPoint:31 ,nCell:23 ,nMatches:3 ,distance between ekf and vo x:0.000080 ,z=0.001192,angle between two frames:0.000042
+[20:26:18.774] n deleted obs = 1 	 n obs with reprojection error less than 0.5 px =2 	 error init =0.247528 	 error end=0.247528
+[20:26:18.774] After pose optimization, distance between ekf and vo x:0.000080 ,z=0.001192,angle between two frames:0.000042
+[20:26:18.774] frame process finished the id is: 81 the obs is:31 
+[20:26:18.864] Odometry x=-0.036730, y=0.615724, theta=-0.034175
+[20:26:18.966] Odometry x=-0.035584, y=0.621815, theta=-0.049151
+[20:26:19.063] Odometry x=-0.035248, y=0.627126, theta=-0.037456
+[20:26:19.166] Odometry x=-0.035027, y=0.637122, theta=-0.054286
+[20:26:19.268] Odometry x=-0.034666, y=0.642439, theta=-0.037696
+[20:26:19.365] Odometry x=-0.033568, y=0.648554, theta=-0.054037
+[20:26:19.468] Odometry x=-0.037392, y=0.651311, theta=-0.058590
+[20:26:19.567] Odometry x=-0.036858, y=0.657982, theta=-0.047562
+[20:26:19.663] After Reprojection Map nPoint:31 ,nCell:19 ,nMatches:3 ,distance between ekf and vo x:-0.000009 ,z=0.000093,angle between two frames:0.000002
+[20:26:19.663] n deleted obs = 1 	 n obs with reprojection error less than 0.5 px =2 	 error init =0.381981 	 error end=0.381981
+[20:26:19.663] After pose optimization, distance between ekf and vo x:-0.000009 ,z=0.000093,angle between two frames:0.000002
+[20:26:19.663] frame process finished the id is: 81 the obs is:31 
+[20:26:19.664] Odometry x=-0.037804, y=0.663383, theta=-0.070568
+[20:26:19.766] Odometry x=-0.037040, y=0.668265, theta=-0.049306
+[20:26:19.863] Odometry x=-0.038930, y=0.674067, theta=-0.061344
+[20:26:19.965] Odometry x=-0.038617, y=0.679761, theta=-0.040650
+[20:26:20.067] Odometry x=-0.038995, y=0.685255, theta=-0.048780
+[20:26:20.164] Odometry x=-0.038801, y=0.690905, theta=-0.050336
+[20:26:20.268] Odometry x=-0.038704, y=0.696394, theta=-0.022132
+[20:26:20.365] Odometry x=-0.038229, y=0.701948, theta=-0.042376
+[20:26:20.565] Odometry x=-0.038107, y=0.707352, theta=-0.017871
+[20:26:20.662] Odometry x=-0.037961, y=0.717772, theta=-0.046644
+[20:26:20.770] Odometry x=-0.037898, y=0.720576, theta=-0.048790
+[20:26:20.867] Odometry x=-0.035273, y=0.726794, theta=-0.044302
+[20:26:20.955] After Reprojection Map nPoint:31 ,nCell:30 ,nMatches:1 ,distance between ekf and vo x:0.000012 ,z=0.000057,angle between two frames:0.000002
+[20:26:20.955] n deleted obs = 1 	 n obs with reprojection error less than 0.5 px =0 	 error init =0.541064 	 error end=0.507535
+[20:26:20.955] After pose optimization, distance between ekf and vo x:-0.006164 ,z=0.239177,angle between two frames:0.057821
+[20:26:20.955] frame process finished the id is: 81 the obs is:31 
+[20:26:20.968] Odometry x=-0.037006, y=0.731871, theta=-0.039972
+[20:26:21.066] Odometry x=-0.036812, y=0.737416, theta=-0.039587
+[20:26:21.165] Odometry x=-0.044236, y=0.746598, theta=-0.042537
+[20:26:21.264] Odometry x=-0.042431, y=0.752299, theta=-0.029822
+[20:26:21.362] Odometry x=-0.043712, y=0.755008, theta=-0.046206
+[20:26:21.465] Odometry x=-0.042593, y=0.760352, theta=-0.033126
+[20:26:21.567] Odometry x=-0.042573, y=0.766226, theta=-0.040295
+[20:26:21.667] Odometry x=-0.041129, y=0.771491, theta=-0.029487
+[20:26:21.769] Odometry x=-0.040941, y=0.776864, theta=-0.033620
+[20:26:21.866] Odometry x=-0.040668, y=0.782282, theta=-0.042565
+[20:26:21.964] Odometry x=-0.042718, y=0.787886, theta=-0.031715
+[20:26:22.061] Odometry x=-0.042410, y=0.793870, theta=-0.039384
+[20:26:22.163] Odometry x=-0.042308, y=0.799227, theta=-0.027134
+[20:26:22.227] After Reprojection Map nPoint:31 ,nCell:29 ,nMatches:3 ,distance between ekf and vo x:-0.000011 ,z=0.000086,angle between two frames:0.000001
+[20:26:22.227] n deleted obs = 1 	 n obs with reprojection error less than 0.5 px =2 	 error init =0.364013 	 error end=0.354530
+[20:26:22.227] After pose optimization, distance between ekf and vo x:-0.002930 ,z=-0.027553,angle between two frames:0.009583
+[20:26:22.227] frame process finished the id is: 81 the obs is:31 
+[20:26:22.265] Odometry x=-0.041568, y=0.804593, theta=-0.040819
+[20:26:22.368] Odometry x=-0.041350, y=0.810086, theta=-0.029528
+[20:26:22.465] Odometry x=-0.041238, y=0.815532, theta=-0.038601
+[20:26:22.626] Odometry x=-0.041097, y=0.820914, theta=-0.038516
+[20:26:22.667] Odometry x=-0.040466, y=0.829592, theta=-0.040508
+[20:26:22.765] Odometry x=-0.040423, y=0.831732, theta=-0.039028
+[20:26:22.868] Odometry x=-0.040330, y=0.837148, theta=-0.032769
+[20:26:22.965] Odometry x=-0.040208, y=0.842763, theta=-0.041727
+[20:26:23.063] Odometry x=-0.043828, y=0.869504, theta=-0.034186
+[20:26:23.170] Odometry x=-0.043727, y=0.875397, theta=-0.035731
+[20:26:23.263] Odometry x=-0.043939, y=0.882178, theta=-0.033438
+[20:26:23.371] Odometry x=-0.043848, y=0.887451, theta=-0.036437
+[20:26:23.480] Odometry x=-0.043726, y=0.894473, theta=-0.030440
+[20:26:23.486] After Reprojection Map nPoint:31 ,nCell:29 ,nMatches:3 ,distance between ekf and vo x:0.000062 ,z=0.001089,angle between two frames:0.000036
+[20:26:23.486] n deleted obs = 1 	 n obs with reprojection error less than 0.5 px =2 	 error init =0.390475 	 error end=0.275312
+[20:26:23.486] After pose optimization, distance between ekf and vo x:0.004607 ,z=-0.402751,angle between two frames:0.008993
+[20:26:23.486] frame process finished the id is: 81 the obs is:31 
+[20:26:23.563] Odometry x=-0.043608, y=0.900444, theta=-0.041948
+[20:26:23.677] Odometry x=-0.043483, y=0.906077, theta=-0.040998
+[20:26:23.770] Odometry x=-0.043359, y=0.911870, theta=-0.042429
+[20:26:23.868] Odometry x=-0.043212, y=0.917213, theta=-0.037396
+[20:26:23.966] Odometry x=-0.043111, y=0.922277, theta=-0.040553
+[20:26:24.065] Odometry x=-0.043000, y=0.927670, theta=-0.044954
+[20:26:24.163] Odometry x=-0.042823, y=0.934846, theta=-0.040254
+[20:26:24.266] Odometry x=-0.042718, y=0.939858, theta=-0.042215
+[20:26:24.365] Odometry x=-0.042568, y=0.946907, theta=-0.034576
+[20:26:24.467] Odometry x=-0.042450, y=0.952245, theta=-0.041843
+[20:26:24.567] Odometry x=-0.042305, y=0.957574, theta=-0.037739
+[20:26:24.756] Odometry x=-0.042184, y=0.963334, theta=-0.032353
+[20:26:24.766] Odometry x=-0.042184, y=0.963334, theta=-0.032353
+[20:26:24.866] Odometry x=-0.042141, y=0.966861, theta=-0.042916
+[20:26:24.892] After Reprojection Map nPoint:31 ,nCell:27 ,nMatches:2 ,distance between ekf and vo x:-0.000030 ,z=0.000309,angle between two frames:0.000006
+[20:26:24.892] n deleted obs = 1 	 n obs with reprojection error less than 0.5 px =1 	 error init =0.445321 	 error end=0.426966
+[20:26:24.892] After pose optimization, distance between ekf and vo x:-0.086092 ,z=0.413921,angle between two frames:0.037276
+[20:26:24.892] frame process finished the id is: 81 the obs is:31 
+[20:26:24.965] Odometry x=-0.041918, y=0.973379, theta=-0.039174
+[20:26:25.065] Odometry x=-0.041814, y=0.978766, theta=-0.035815
+[20:26:25.164] Odometry x=-0.041682, y=0.986323, theta=-0.037215
+[20:26:25.265] Odometry x=-0.041557, y=0.991816, theta=-0.040112
+[20:26:25.365] Odometry x=-0.041433, y=0.997552, theta=-0.043376
+[20:26:25.465] Odometry x=-0.041305, y=1.002992, theta=-0.048353
+[20:26:25.565] Odometry x=-0.041169, y=1.008340, theta=-0.044942
+[20:26:25.667] Odometry x=-0.041039, y=1.013660, theta=-0.047179
+[20:26:25.766] Odometry x=-0.040832, y=1.019555, theta=-0.045577
+[20:26:25.863] Odometry x=-0.040700, y=1.024947, theta=-0.046298
+[20:26:25.966] Odometry x=-0.040573, y=1.030453, theta=-0.040540
+[20:26:26.014] After Reprojection Map nPoint:31 ,nCell:24 ,nMatches:3 ,distance between ekf and vo x:-0.000025 ,z=0.000265,angle between two frames:0.000005
+[20:26:26.014] n deleted obs = 2 	 n obs with reprojection error less than 0.5 px =1 	 error init =1.252336 	 error end=0.629747
+[20:26:26.014] After pose optimization, distance between ekf and vo x:-0.081462 ,z=2.179925,angle between two frames:1.969599
+[20:26:26.014] frame process finished the id is: 81 the obs is:31 
+[20:26:26.068] Odometry x=-0.040463, y=1.036270, theta=-0.034185
+[20:26:26.166] Odometry x=-0.040332, y=1.042054, theta=-0.042629
+[20:26:26.266] Odometry x=-0.040213, y=1.047275, theta=-0.038848
+[20:26:26.364] Odometry x=-0.040115, y=1.052259, theta=-0.040457
+[20:26:26.460] Odometry x=-0.040008, y=1.058038, theta=-0.036259
+[20:26:26.566] Odometry x=-0.039898, y=1.064497, theta=-0.038612
+[20:26:26.669] Odometry x=-0.039770, y=1.070667, theta=-0.039051
+[20:26:26.847] Odometry x=-0.039661, y=1.076136, theta=-0.037755
+[20:26:26.862] Odometry x=-0.039396, y=1.085747, theta=-0.040154
+[20:26:26.966] Odometry x=-0.039376, y=1.086816, theta=-0.039941
+[20:26:27.063] Odometry x=-0.039258, y=1.092704, theta=-0.040462
+[20:26:27.083] After Reprojection Map nPoint:31 ,nCell:24 ,nMatches:3 ,distance between ekf and vo x:-0.000299 ,z=0.001329,angle between two frames:0.000011
+[20:26:27.084] n deleted obs = 0 	 n obs with reprojection error less than 0.5 px =3 	 error init =3.083095 	 error end=0.275522
+[20:26:27.084] After pose optimization, distance between ekf and vo x:1.449481 ,z=-38.012484,angle between two frames:0.405429
+[20:26:27.084] frame process finished the id is: 81 the obs is:31 
+[20:26:27.165] Odometry x=-0.039135, y=1.098879, theta=-0.038890
+[20:26:27.264] Odometry x=-0.039021, y=1.104342, theta=-0.043414
+[20:26:27.366] Odometry x=-0.038869, y=1.109813, theta=-0.045407
+[20:26:27.464] Odometry x=-0.038751, y=1.115412, theta=-0.037301
+[20:26:27.566] Odometry x=-0.038633, y=1.120725, theta=-0.048791
+[20:26:27.667] Odometry x=-0.038492, y=1.126335, theta=-0.045280
+[20:26:27.764] Odometry x=-0.038350, y=1.131694, theta=-0.044643
+[20:26:27.864] Odometry x=-0.038202, y=1.137785, theta=-0.042735
+[20:26:27.961] Odometry x=-0.038028, y=1.143152, theta=-0.037563
+[20:26:28.064] Odometry x=-0.037824, y=1.148789, theta=-0.040160
+[20:26:28.161] Odometry x=-0.037719, y=1.154201, theta=-0.038217
+[20:26:28.263] Odometry x=-0.037487, y=1.159883, theta=-0.041133
+[20:26:28.318] After Reprojection Map nPoint:31 ,nCell:28 ,nMatches:2 ,distance between ekf and vo x:-0.000285 ,z=0.001119,angle between two frames:0.000009
+[20:26:28.319] n deleted obs = 2 	 n obs with reprojection error less than 0.5 px =0 	 error init =17.264821 	 error end=2.834575
+[20:26:28.319] After pose optimization, distance between ekf and vo x:-293.587490 ,z=76.367300,angle between two frames:1.991056
+[20:26:28.319] frame process finished the id is: 81 the obs is:31 
+[20:26:28.365] Odometry x=-0.037329, y=1.166075, theta=-0.034777
+[20:26:28.463] Odometry x=-0.037228, y=1.171513, theta=-0.038444
+[20:26:28.560] Odometry x=-0.037105, y=1.176926, theta=-0.038216
+[20:26:28.664] Odometry x=-0.036225, y=1.186635, theta=-0.038331
+[20:26:28.766] Odometry x=-0.036104, y=1.192941, theta=-0.036752
+[20:26:28.941] Odometry x=-0.036003, y=1.198414, theta=-0.033100
+[20:26:28.962] Odometry x=-0.036003, y=1.198414, theta=-0.033100
+[20:26:29.060] Odometry x=-0.035575, y=1.204581, theta=-0.044474
+[20:26:29.163] Odometry x=-0.035433, y=1.210880, theta=-0.046296
+[20:26:29.266] Odometry x=-0.035181, y=1.217895, theta=-0.039868
+[20:26:29.364] Odometry x=-0.035056, y=1.223555, theta=-0.044803
+[20:26:29.464] Odometry x=-0.034259, y=1.228782, theta=-0.038386
+[20:26:29.569] Odometry x=-0.034178, y=1.234205, theta=-0.042273
+[20:26:29.644] After Reprojection Map nPoint:31 ,nCell:31 ,nMatches:1 ,distance between ekf and vo x:-0.000063 ,z=0.000205,angle between two frames:0.000000
+[20:26:29.644] n deleted obs = 0 	 n obs with reprojection error less than 0.5 px =1 	 error init =0.292014 	 error end=0.292014
+[20:26:29.644] After pose optimization, distance between ekf and vo x:-0.000063 ,z=0.000205,angle between two frames:0.000000
+[20:26:29.644] frame process finished the id is: 81 the obs is:31 
+[20:26:29.664] Odometry x=-0.034001, y=1.242005, theta=-0.042461
+[20:26:29.768] Odometry x=-0.033846, y=1.247994, theta=-0.049966
+[20:26:29.868] Odometry x=-0.033702, y=1.253501, theta=-0.043194
+[20:26:29.966] Odometry x=-0.033594, y=1.259720, theta=-0.044979
+[20:26:30.063] Odometry x=-0.033415, y=1.264922, theta=-0.046170
+[20:26:30.162] Odometry x=-0.033299, y=1.270058, theta=-0.040218
+[20:26:30.264] Odometry x=-0.033156, y=1.276478, theta=-0.035366
+[20:26:30.366] Odometry x=-0.033049, y=1.282463, theta=-0.036643
+[20:26:30.467] Odometry x=-0.032938, y=1.288106, theta=-0.037474
+[20:26:30.569] Odometry x=-0.032827, y=1.294584, theta=-0.028460
+[20:26:30.667] Odometry x=-0.032744, y=1.300697, theta=-0.031770
+[20:26:30.764] Odometry x=-0.032654, y=1.306005, theta=-0.036323
+[20:26:30.866] Odometry x=-0.032553, y=1.311478, theta=-0.034441
+[20:26:30.875] After Reprojection Map nPoint:31 ,nCell:30 ,nMatches:2 ,distance between ekf and vo x:-0.000053 ,z=0.000222,angle between two frames:0.000001
+[20:26:30.875] n deleted obs = 0 	 n obs with reprojection error less than 0.5 px =2 	 error init =0.335952 	 error end=0.335952
+[20:26:30.875] After pose optimization, distance between ekf and vo x:-0.000053 ,z=0.000222,angle between two frames:0.000001
+[20:26:30.875] frame process finished the id is: 81 the obs is:31 
+[20:26:30.999] Odometry x=-0.032403, y=1.316942, theta=-0.041096
+[20:26:31.066] Odometry x=-0.032259, y=1.324251, theta=-0.036347
+[20:26:31.162] Odometry x=-0.032185, y=1.327773, theta=-0.039052
+[20:26:31.264] Odometry x=-0.032082, y=1.333110, theta=-0.035987
+[20:26:31.367] Odometry x=-0.031810, y=1.339227, theta=-0.033491
+[20:26:31.469] Odometry x=-0.031712, y=1.344823, theta=-0.031573
+[20:26:31.566] Odometry x=-0.031593, y=1.352046, theta=-0.041452
+[20:26:31.661] Odometry x=-0.031435, y=1.357677, theta=-0.041044
+[20:26:31.764] Odometry x=-0.031297, y=1.364014, theta=-0.035545
+[20:26:31.868] Odometry x=-0.031012, y=1.369362, theta=-0.042896
+[20:26:31.968] Odometry x=-0.030869, y=1.375860, theta=-0.040173
+[20:26:31.974] After Reprojection Map nPoint:31 ,nCell:26 ,nMatches:3 ,distance between ekf and vo x:-0.000061 ,z=0.000234,angle between two frames:0.000000
+[20:26:31.974] n deleted obs = 0 	 n obs with reprojection error less than 0.5 px =3 	 error init =0.313891 	 error end=0.224968
+[20:26:31.974] After pose optimization, distance between ekf and vo x:0.043322 ,z=0.191423,angle between two frames:0.015114
+[20:26:31.974] frame process finished the id is: 81 the obs is:31 
+[20:26:32.067] Odometry x=-0.030710, y=1.381596, theta=-0.037891
+[20:26:32.164] Odometry x=-0.030620, y=1.387357, theta=-0.032205
+[20:26:32.271] Odometry x=-0.030527, y=1.392575, theta=-0.030521
+[20:26:32.362] Odometry x=-0.030420, y=1.398767, theta=-0.037775
+[20:26:32.466] Odometry x=-0.030298, y=1.403674, theta=-0.034794
+[20:26:32.568] Odometry x=-0.030191, y=1.409803, theta=-0.036730
+[20:26:32.665] Odometry x=-0.030069, y=1.415254, theta=-0.037828
+[20:26:32.767] Odometry x=-0.029930, y=1.422205, theta=-0.044290
+[20:26:32.866] Odometry x=-0.029810, y=1.427451, theta=-0.042759
+[20:26:32.963] Odometry x=-0.029677, y=1.433009, theta=-0.039379
+[20:26:33.066] Odometry x=-0.029583, y=1.438388, theta=-0.041958
+[20:26:33.206] After Reprojection Map nPoint:31 ,nCell:29 ,nMatches:2 ,distance between ekf and vo x:-0.000130 ,z=0.000454,angle between two frames:0.000002
+[20:26:33.206] n deleted obs = 0 	 n obs with reprojection error less than 0.5 px =2 	 error init =0.330052 	 error end=0.310865
+[20:26:33.206] After pose optimization, distance between ekf and vo x:0.052962 ,z=-0.390687,angle between two frames:0.005766
+[20:26:33.206] frame process finished the id is: 81 the obs is:31 
+[20:26:34.451] After Reprojection Map nPoint:31 ,nCell:29 ,nMatches:2 ,distance between ekf and vo x:-0.000126 ,z=0.000441,angle between two frames:0.000002
+[20:26:34.451] n deleted obs = 0 	 n obs with reprojection error less than 0.5 px =2 	 error init =0.276487 	 error end=0.276487
+[20:26:34.451] After pose optimization, distance between ekf and vo x:-0.000126 ,z=0.000441,angle between two frames:0.000002
+[20:26:34.451] frame process finished the id is: 81 the obs is:31 
+[20:26:35.684] After Reprojection Map nPoint:31 ,nCell:30 ,nMatches:1 ,distance between ekf and vo x:0.000140 ,z=0.000807,angle between two frames:0.000038
+[20:26:35.685] n deleted obs = 0 	 n obs with reprojection error less than 0.5 px =1 	 error init =0.500104 	 error end=0.499851
+[20:26:35.685] After pose optimization, distance between ekf and vo x:-0.003848 ,z=0.002489,angle between two frames:0.007851
+[20:26:35.685] frame process finished the id is: 81 the obs is:31 
+[20:26:36.970] After Reprojection Map nPoint:31 ,nCell:31 ,nMatches:3 ,distance between ekf and vo x:0.000139 ,z=0.000807,angle between two frames:0.000038
+[20:26:36.971] n deleted obs = 0 	 n obs with reprojection error less than 0.5 px =3 	 error init =0.247215 	 error end=0.246601
+[20:26:36.971] After pose optimization, distance between ekf and vo x:-0.001263 ,z=0.005600,angle between two frames:0.000026
+[20:26:36.971] frame process finished the id is: 81 the obs is:31 
+[20:26:38.220] After Reprojection Map nPoint:31 ,nCell:30 ,nMatches:2 ,distance between ekf and vo x:0.000142 ,z=0.000807,angle between two frames:0.000038
+[20:26:38.220] n deleted obs = 0 	 n obs with reprojection error less than 0.5 px =2 	 error init =0.690397 	 error end=0.434915
+[20:26:38.220] After pose optimization, distance between ekf and vo x:-0.185023 ,z=1.621422,angle between two frames:0.026439
+[20:26:38.220] frame process finished the id is: 81 the obs is:31 
+[20:26:39.460] After Reprojection Map nPoint:31 ,nCell:30 ,nMatches:2 ,distance between ekf and vo x:0.000138 ,z=0.000806,angle between two frames:0.000037
+[20:26:39.460] n deleted obs = 0 	 n obs with reprojection error less than 0.5 px =2 	 error init =0.724237 	 error end=0.380743
+[20:26:39.460] After pose optimization, distance between ekf and vo x:-0.323531 ,z=0.848136,angle between two frames:0.245274
+[20:26:39.460] frame process finished the id is: 81 the obs is:31 
diff --git a/GPU_version/vio/include/vio/matcher.h b/GPU_version/vio/include/vio/matcher.h
index 1ede992..e73e49e 100644
--- a/GPU_version/vio/include/vio/matcher.h
+++ b/GPU_version/vio/include/vio/matcher.h
@@ -126,7 +126,6 @@ public:
   uint i=0;
   void debug(cv::Mat ref,cv::Mat cur, Vector2d ref_px,Vector2d cur_px_in,Vector2d cur_px_out,bool res){
       cv::Mat Ref,Cur;
-      if(!res)return;
       cv::cvtColor(cur,Cur,cv::COLOR_GRAY2RGB);
       cv::cvtColor(ref,Ref,cv::COLOR_GRAY2RGB);
       cv::circle(Ref, cv::Point(ref_px.x(),ref_px.y() ), 2, cv::Scalar(0, 0,255),2);
diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp
index 5fc79b6..f348968 100644
--- a/GPU_version/vio/src/reprojector.cpp
+++ b/GPU_version/vio/src/reprojector.cpp
@@ -93,7 +93,7 @@ namespace vio {
             std::vector<cv::DMatch>  matches;
             overlap_kfs.push_back(pair<FramePtr, size_t>(it_frame.item.first, 0));
             cv::Mat ref_des=cv::Mat(it_frame.item.first->fts_.size(),64,CV_8UC1);
-            for (auto &&it_ftr:_for(it_frame.item.first->fts_)) {
+/*            for (auto &&it_ftr:_for(it_frame.item.first->fts_)) {
                 keypoints_kfs.push_back(cv::KeyPoint(it_ftr.item->px.x(), it_ftr.item->px.y(), 7.f,it_ftr.item->score));
                 memcpy(ref_des.data+(it_ftr.index*64),it_ftr.item->descriptor,sizeof(uint8_t)*64);
             }
@@ -101,6 +101,19 @@ namespace vio {
             for (auto &&f:_for(keypoints)) {
                 keypoints_cur.push_back(cv::KeyPoint(f.item->px.x(), f.item->px.y(), 7.f,f.item->score));
                 memcpy(cur_des.data+(f.index*64),f.item->descriptor,sizeof(uint8_t)*64);
+            }*/
+            list<std::shared_ptr<Feature>>::iterator it_ref=it_frame.item.first->fts_.begin();
+            for (int i=0;i<it_frame.item.first->fts_.size() && it_ref !=it_frame.item.first->fts_.end();++i) {
+                keypoints_kfs.push_back(cv::KeyPoint((*it_ref)->px.x(), (*it_ref)->px.y(), 7.f,(*it_ref)->score));
+                memcpy(ref_des.data+(i*64),(*it_ref)->descriptor,sizeof(uint8_t)*64);
+                ++it_ref;
+            }
+            list<std::shared_ptr<Feature>>::iterator it_cur=keypoints.begin();
+            cv::Mat cur_des=cv::Mat(keypoints.size(),64,CV_8UC1);
+            for (int i=0;i<keypoints.size() && it_cur !=keypoints.end();++i) {
+                keypoints_cur.push_back(cv::KeyPoint((*it_cur)->px.x(), (*it_cur)->px.y(), 7.f,(*it_cur)->score));
+                memcpy(cur_des.data+(i*64),(*it_cur)->descriptor,sizeof(uint8_t)*64);
+                ++it_cur;
             }
             matcher->match(ref_des, cur_des, matches);
 /*            cv::Mat imgMatch,key_point_ref,key_point_cur;
@@ -111,49 +124,46 @@ namespace vio {
             cv::imshow("key_point_ref",key_point_ref);
             cv::imshow("key_point_current",key_point_cur);
             cv::waitKey();
-            exit(0);
-            std::cerr<<it_frame.item.first->fts_.size()<<","<<keypoints_kfs.size()<<'\n';*/
-            for (auto &&f: _for(it_frame.item.first->fts_)) {
-                for (auto &&match:matches.at(f.index)) {
-                    if (match.queryIdx != f.index)continue;
-                    if(keypoints_cur.size() < match.trainIdx)continue;
-                    if(match.distance > 30.0 )continue;
-                    list<std::shared_ptr<Feature>>::iterator point=keypoints_cur.begin();
-                    std::advance(point,match.trainIdx);
-                    if(!(*point))continue;
-                    if (f.item->point == NULL){
-                        const int k = static_cast<int>((*point)->px.y() / grid_.cell_size) *
-                                      grid_.grid_n_cols
-                                      + static_cast<int>((*point)->px.x() / grid_.cell_size);
-                        if(k>grid_.cells.size()-1)continue;
-                        assert(grid_.cells.at(k) != nullptr);
-                        Vector2d px((int) (*point)->px.x(),
-                                    (int) (*point)->px.y());
-                        SE3 T_ref_cur=it_frame.item.first->se3().inverse()*frame->se3();
-                        Vector3d new_point=vk::triangulateFeatureNonLin(T_ref_cur.rotation_matrix(),T_ref_cur.translation(),
-                                                                        frame->c2f(px),f.item->f);
-                        if(new_point.z()<0.0)continue;
-                        frame->fts_.push_back(std::make_shared<Feature>(it_frame.item.first,
-                                                                        std::make_shared<Point>(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));
-                        overlap_kfs.back().second++;
+            exit(0);*/
+            for (auto &&m: matches) {
+                it_cur=keypoints.begin();
+                it_ref=it_frame.item.first->fts_.begin();
+                std::advance(it_ref,m.queryIdx);
+                std::advance(it_cur,m.trainIdx);
+                if(!(*it_ref))continue;
+                if(!(*it_cur))continue;
+                if ((*it_ref)->point == NULL){
+                    const int k = static_cast<int>((*it_cur)->px.y() / grid_.cell_size) *
+                            grid_.grid_n_cols
+                            + static_cast<int>((*it_cur)->px.x() / grid_.cell_size);
+                    if(k>grid_.cells.size()-1)continue;
+                    assert(grid_.cells.at(k) != nullptr);
+                    Vector2d px((int) (*it_cur)->px.x(),
+                                (int) (*it_cur)->px.y());
+                    SE3 T_ref_cur=it_frame.item.first->se3().inverse()*frame->se3();
+                    Vector3d new_point=vk::triangulateFeatureNonLin(T_ref_cur.rotation_matrix(),T_ref_cur.translation(),
+                                                                        frame->c2f(px),(*it_ref)->f);
+                    if(new_point.z()<0.0)continue;
+                    frame->fts_.push_back(std::make_shared<Feature>(it_frame.item.first,
+                                                                        std::make_shared<Point>(it_frame.item.first->se3()*new_point,(*it_ref)),
+                                                                                px,(*it_ref)->f,(*it_cur)->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));
+                    overlap_kfs.back().second++;
                         //break;
-                    }else{
-                        if (f.item->point->last_frame_overlap_id_ == frame->id_)continue;
-                        f.item->point->last_frame_overlap_id_ = frame->id_;
-                        const int k = static_cast<int>((*point)->px.y()/ grid_.cell_size) *
+                }else{
+                    if ((*it_ref)->point->last_frame_overlap_id_ == frame->id_)continue;
+                    (*it_ref)->point->last_frame_overlap_id_ = frame->id_;
+                    const int k = static_cast<int>((*it_cur)->px.y()/ grid_.cell_size) *
                                       grid_.grid_n_cols
-                                      + static_cast<int>((*point)->px.x() / grid_.cell_size);
-                        if(k>grid_.cells.size()-1)continue;
-                        assert(grid_.cells.at(k) != nullptr);
-                        Vector2d px((int) (*point)->px.x(),
-                                    (int) (*point)->px.y());
-                        grid_.cells.at(k)->push_back(Candidate(f.item->point, px));
-                        overlap_kfs.back().second++;
+                                      + static_cast<int>((*it_cur)->px.x() / grid_.cell_size);
+                    if(k>grid_.cells.size()-1)continue;
+                    assert(grid_.cells.at(k) != nullptr);
+                    Vector2d px((int) (*it_cur)->px.x(),
+                                    (int) (*it_cur)->px.y());
+                    grid_.cells.at(k)->push_back(Candidate((*it_ref)->point, px));
+                    overlap_kfs.back().second++;
                         //break;
-                    }
                 }
             }
         }
@@ -193,21 +203,7 @@ namespace vio {
                 continue;
             }
 ///TODO check point candidate handling
-            //cv::Point pointb(it->px.x(), it->px.y());
-            //cv::circle(image, pointb,1,(255, 0, 0), 1);
-            Vector2d uvb(it->px.x(), it->px.y());
-#if VIO_DEBUG
-            fprintf(log_,"before : %f  %f  after : ", it->px.x(), it->px.y());
-#endif
-            //std::cerr<<"before : "<<it->px.x()<<" "<<it->px.y()<<" after : ";
             bool res = matcher_.findMatchDirect(*it->pt, *frame, it->px);
-            Vector2d uva(it->px.x(), it->px.y());
-#if VIO_DEBUG
-            fprintf(log_,"%f  %f  %s %f\n", it->px.x(), it->px.y(), (res?" TRUE":" FALSE"), (uvb - uva).norm());
-#endif
-            //std::cerr<<it->px.x()<<" "<<it->px.y()<<(res?" TRUE":" FALSE")<<std::endl;
-            //cv::Point pointa(it->px.x(), it->px.y());
-            //cv::circle(image, pointa,1,(0, 255, 0), 1);
             if(!res)
             {
                 it->pt->n_failed_reproj_++;
@@ -246,8 +242,7 @@ namespace vio {
             // Maximum one point per cell.
             return true;
         }
-        //cv::imwrite("/root/Projects/ROS_p_33_dev/src/p_33_vio/GPU_version/vio/image.png", image);
-        //std::exit(223);
+
         return false;
     }