diff --git a/GPU_version/vio/depth_filter_log.txt b/GPU_version/vio/depth_filter_log.txt
deleted file mode 100755
index e69de29..0000000
diff --git a/GPU_version/vio/frame_handler_log.txt b/GPU_version/vio/frame_handler_log.txt
deleted file mode 100755
index 4ad350b..0000000
--- a/GPU_version/vio/frame_handler_log.txt
+++ /dev/null
@@ -1,298 +0,0 @@
-[17:50:31.411] Init: frame zero: 0.000000, 0.000000 0.000000
-[17:50:31.412] Init: Selected first frame. 
-[17:50:31.412] frame process finished the id is: 0 the obs is:0 
-[17:50:31.433] Init: px average disparity is:0.035702 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:31.433] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:31.433] frame process finished the id is: 1 the obs is:0 
-[17:50:31.536] Init: px average disparity is:0.035710 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:31.536] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:31.536] frame process finished the id is: 2 the obs is:0 
-[17:50:31.633] Init: px average disparity is:0.027114 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:31.633] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:31.633] frame process finished the id is: 3 the obs is:0 
-[17:50:31.731] Init: px average disparity is:0.028933 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:31.731] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:31.731] frame process finished the id is: 4 the obs is:0 
-[17:50:31.830] Init: px average disparity is:0.032903 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:31.830] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:31.830] frame process finished the id is: 5 the obs is:0 
-[17:50:31.921] Init: px average disparity is:0.029436 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:31.921] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:31.921] frame process finished the id is: 6 the obs is:0 
-[17:50:32.017] Init: px average disparity is:0.030897 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:32.017] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:32.017] frame process finished the id is: 7 the obs is:0 
-[17:50:32.115] Init: px average disparity is:0.029863 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:32.115] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:32.115] frame process finished the id is: 8 the obs is:0 
-[17:50:32.214] Init: px average disparity is:0.033033 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:32.214] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:32.214] frame process finished the id is: 9 the obs is:0 
-[17:50:32.318] Init: px average disparity is:0.032262 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:32.318] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:32.318] frame process finished the id is: 10 the obs is:0 
-[17:50:32.423] Init: px average disparity is:0.028435 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:32.423] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:32.423] frame process finished the id is: 11 the obs is:0 
-[17:50:32.521] Init: px average disparity is:0.038753 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:32.521] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:32.521] frame process finished the id is: 12 the obs is:0 
-[17:50:32.583] Odometry x=0.000000, y=0.000000, theta=-0.000000
-[17:50:32.644] Init: px average disparity is:0.033534 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:32.644] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:32.644] frame process finished the id is: 13 the obs is:0 
-[17:50:32.735] Init: px average disparity is:0.028411 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:32.735] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:32.735] frame process finished the id is: 14 the obs is:0 
-[17:50:32.810] Odometry x=0.000000, y=0.019168, theta=0.000000
-[17:50:32.842] Init: px average disparity is:0.146053 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:32.842] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:32.842] frame process finished the id is: 15 the obs is:0 
-[17:50:32.918] Init: px average disparity is:6.453716 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:32.918] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:32.918] frame process finished the id is: 16 the obs is:0 
-[17:50:33.020] Init: px average disparity is:4.606047 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:33.020] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:33.020] frame process finished the id is: 17 the obs is:0 
-[17:50:33.074] Odometry x=0.000245, y=0.051530, theta=0.002321
-[17:50:33.123] Init: px average disparity is:4.914490 ,While minimum is: 10.000000  KLT tracked : 895
-[17:50:33.123] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:33.123] frame process finished the id is: 18 the obs is:0 
-[17:50:33.223] Init: px average disparity is:9.779386 ,While minimum is: 10.000000  KLT tracked : 894
-[17:50:33.223] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:33.223] frame process finished the id is: 19 the obs is:0 
-[17:50:33.352] Odometry x=0.000199, y=0.082338, theta=-0.000122
-[17:50:33.458] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required.  px average disparity is:12.724287 ,While minimum is: 10.000000  KLT tracked: 894
-[17:50:33.458] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:33.458] frame process finished the id is: 20 the obs is:0 
-[17:50:33.559] Odometry x=0.000727, y=0.112621, theta=0.001152
-[17:50:33.587] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required.  px average disparity is:15.784901 ,While minimum is: 10.000000  KLT tracked: 886
-[17:50:33.587] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:33.587] frame process finished the id is: 21 the obs is:0 
-[17:50:33.715] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required.  px average disparity is:19.932699 ,While minimum is: 10.000000  KLT tracked: 886
-[17:50:33.715] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:33.715] frame process finished the id is: 22 the obs is:0 
-[17:50:33.844] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required.  px average disparity is:27.322473 ,While minimum is: 10.000000  KLT tracked: 886
-[17:50:33.844] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:33.844] frame process finished the id is: 23 the obs is:0 
-[17:50:33.864] Odometry x=0.000558, y=0.141449, theta=0.010821
-[17:50:33.973] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required.  px average disparity is:28.563517 ,While minimum is: 10.000000  KLT tracked: 885
-[17:50:33.973] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:33.973] frame process finished the id is: 24 the obs is:0 
-[17:50:34.091] Odometry x=0.000199, y=0.170487, theta=0.015299
-[17:50:34.093] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required.  px average disparity is:34.025067 ,While minimum is: 10.000000  KLT tracked: 885
-[17:50:34.094] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:34.094] frame process finished the id is: 25 the obs is:0 
-[17:50:34.214] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required.  px average disparity is:41.010976 ,While minimum is: 10.000000  KLT tracked: 881
-[17:50:34.214] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:34.214] frame process finished the id is: 26 the obs is:0 
-[17:50:34.328] Odometry x=-0.000118, y=0.199585, theta=0.015206
-[17:50:34.340] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:47.159182 ,While minimum is: 10.000000  KLT tracked: 843
-[17:50:34.340] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:34.340] frame process finished the id is: 27 the obs is:0 
-[17:50:34.462] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:42.626608 ,While minimum is: 10.000000  KLT tracked: 782
-[17:50:34.462] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:34.462] frame process finished the id is: 28 the obs is:0 
-[17:50:34.577] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:45.790779 ,While minimum is: 10.000000  KLT tracked: 752
-[17:50:34.577] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:34.577] frame process finished the id is: 29 the obs is:0 
-[17:50:34.609] Odometry x=-0.000599, y=0.228582, theta=0.016890
-[17:50:34.741] Init: Homography RANSAC (inlier) is:1 ,While 40 inliers minimum required.  px average disparity is:43.837220 ,While minimum is: 10.000000  KLT tracked: 747
-[17:50:34.741] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:34.741] frame process finished the id is: 30 the obs is:0 
-[17:50:34.813] Odometry x=-0.000827, y=0.256863, theta=0.016911
-[17:50:34.852] Init: Homography RANSAC (inlier) is:1 ,While 40 inliers minimum required.  px average disparity is:53.269346 ,While minimum is: 10.000000  KLT tracked: 738
-[17:50:34.852] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:34.852] frame process finished the id is: 31 the obs is:0 
-[17:50:34.976] Init: Homography RANSAC (inlier) is:1 ,While 40 inliers minimum required.  px average disparity is:49.698939 ,While minimum is: 10.000000  KLT tracked: 729
-[17:50:34.976] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:34.976] frame process finished the id is: 32 the obs is:0 
-[17:50:35.047] Odometry x=-0.001150, y=0.284645, theta=0.003733
-[17:50:35.120] Init: Homography RANSAC (inlier) is:2 ,While 40 inliers minimum required.  px average disparity is:43.891760 ,While minimum is: 10.000000  KLT tracked: 729
-[17:50:35.120] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:35.120] frame process finished the id is: 33 the obs is:0 
-[17:50:35.301] Odometry x=-0.000790, y=0.310881, theta=0.007198
-[17:50:35.333] Init: Homography RANSAC (inlier) is:2 ,While 40 inliers minimum required.  px average disparity is:83.937581 ,While minimum is: 10.000000  KLT tracked: 716
-[17:50:35.333] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:35.333] frame process finished the id is: 34 the obs is:0 
-[17:50:35.456] Init: Homography RANSAC (inlier) is:2 ,While 40 inliers minimum required.  px average disparity is:41.798021 ,While minimum is: 10.000000  KLT tracked: 700
-[17:50:35.456] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:35.456] frame process finished the id is: 35 the obs is:0 
-[17:50:35.556] Odometry x=-0.001214, y=0.337069, theta=0.033919
-[17:50:35.564] Init: Homography RANSAC (inlier) is:2 ,While 40 inliers minimum required.  px average disparity is:41.888346 ,While minimum is: 10.000000  KLT tracked: 695
-[17:50:35.564] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:35.564] frame process finished the id is: 36 the obs is:0 
-[17:50:35.675] Init: Homography RANSAC (inlier) is:1 ,While 40 inliers minimum required.  px average disparity is:49.870518 ,While minimum is: 10.000000  KLT tracked: 695
-[17:50:35.675] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:35.675] frame process finished the id is: 37 the obs is:0 
-[17:50:35.783] Init: Homography RANSAC (inlier) is:2 ,While 40 inliers minimum required.  px average disparity is:50.665883 ,While minimum is: 10.000000  KLT tracked: 695
-[17:50:35.783] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:35.783] frame process finished the id is: 38 the obs is:0 
-[17:50:35.890] Init: Homography RANSAC (inlier) is:2 ,While 40 inliers minimum required.  px average disparity is:56.009125 ,While minimum is: 10.000000  KLT tracked: 695
-[17:50:35.890] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:35.890] frame process finished the id is: 39 the obs is:0 
-[17:50:35.893] Odometry x=-0.002172, y=0.361497, theta=0.047289
-[17:50:35.997] Init: Homography RANSAC (inlier) is:2 ,While 40 inliers minimum required.  px average disparity is:55.714194 ,While minimum is: 10.000000  KLT tracked: 694
-[17:50:35.997] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:35.997] frame process finished the id is: 40 the obs is:0 
-[17:50:36.059] Odometry x=-0.003696, y=0.385156, theta=0.063176
-[17:50:36.105] Init: Homography RANSAC (inlier) is:2 ,While 40 inliers minimum required.  px average disparity is:54.680002 ,While minimum is: 10.000000  KLT tracked: 690
-[17:50:36.105] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:36.105] frame process finished the id is: 41 the obs is:0 
-[17:50:36.211] Init: Homography RANSAC (inlier) is:7 ,While 40 inliers minimum required.  px average disparity is:59.079536 ,While minimum is: 10.000000  KLT tracked: 685
-[17:50:36.211] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:36.211] frame process finished the id is: 42 the obs is:0 
-[17:50:36.308] Odometry x=-0.005270, y=0.410339, theta=0.062916
-[17:50:36.316] Init: Homography RANSAC (inlier) is:10 ,While 40 inliers minimum required.  px average disparity is:63.253156 ,While minimum is: 10.000000  KLT tracked: 683
-[17:50:36.316] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:36.316] frame process finished the id is: 43 the obs is:0 
-[17:50:36.420] Init: Homography RANSAC (inlier) is:10 ,While 40 inliers minimum required.  px average disparity is:64.019856 ,While minimum is: 10.000000  KLT tracked: 681
-[17:50:36.420] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:36.420] frame process finished the id is: 44 the obs is:0 
-[17:50:36.525] Init: Homography RANSAC (inlier) is:11 ,While 40 inliers minimum required.  px average disparity is:61.805289 ,While minimum is: 10.000000  KLT tracked: 680
-[17:50:36.525] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:36.525] frame process finished the id is: 45 the obs is:0 
-[17:50:36.550] Odometry x=-0.008561, y=0.437785, theta=0.059562
-[17:50:36.632] Init: Homography RANSAC (inlier) is:11 ,While 40 inliers minimum required.  px average disparity is:62.020099 ,While minimum is: 10.000000  KLT tracked: 680
-[17:50:36.632] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:36.632] frame process finished the id is: 46 the obs is:0 
-[17:50:36.743] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:64.020284 ,While minimum is: 10.000000  KLT tracked: 679
-[17:50:36.743] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:36.743] frame process finished the id is: 47 the obs is:0 
-[17:50:36.827] Odometry x=-0.007797, y=0.462194, theta=0.054948
-[17:50:36.852] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:64.197038 ,While minimum is: 10.000000  KLT tracked: 677
-[17:50:36.852] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:36.852] frame process finished the id is: 48 the obs is:0 
-[17:50:36.958] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:61.479662 ,While minimum is: 10.000000  KLT tracked: 574
-[17:50:36.958] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:36.958] frame process finished the id is: 49 the obs is:0 
-[17:50:37.059] Odometry x=-0.008775, y=0.478765, theta=0.068109
-[17:50:37.059] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:62.428546 ,While minimum is: 10.000000  KLT tracked: 550
-[17:50:37.059] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:37.059] frame process finished the id is: 50 the obs is:0 
-[17:50:37.160] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:66.159833 ,While minimum is: 10.000000  KLT tracked: 539
-[17:50:37.160] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:37.160] frame process finished the id is: 51 the obs is:0 
-[17:50:37.264] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:66.867114 ,While minimum is: 10.000000  KLT tracked: 539
-[17:50:37.264] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:37.264] frame process finished the id is: 52 the obs is:0 
-[17:50:37.314] Odometry x=-0.007862, y=0.494711, theta=0.061638
-[17:50:37.364] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:61.918039 ,While minimum is: 10.000000  KLT tracked: 539
-[17:50:37.364] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:37.364] frame process finished the id is: 53 the obs is:0 
-[17:50:37.464] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:59.145431 ,While minimum is: 10.000000  KLT tracked: 539
-[17:50:37.464] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:37.464] frame process finished the id is: 54 the obs is:0 
-[17:50:37.565] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:57.093263 ,While minimum is: 10.000000  KLT tracked: 539
-[17:50:37.565] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:37.565] frame process finished the id is: 55 the obs is:0 
-[17:50:37.567] Odometry x=-0.008503, y=0.512210, theta=0.057686
-[17:50:37.667] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:56.741348 ,While minimum is: 10.000000  KLT tracked: 539
-[17:50:37.667] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:37.667] frame process finished the id is: 56 the obs is:0 
-[17:50:37.771] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:57.555356 ,While minimum is: 10.000000  KLT tracked: 539
-[17:50:37.771] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:37.771] frame process finished the id is: 57 the obs is:0 
-[17:50:37.870] Odometry x=-0.009642, y=0.532168, theta=0.057057
-[17:50:37.872] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:58.978449 ,While minimum is: 10.000000  KLT tracked: 539
-[17:50:37.872] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:37.872] frame process finished the id is: 58 the obs is:0 
-[17:50:37.974] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:60.911267 ,While minimum is: 10.000000  KLT tracked: 539
-[17:50:37.974] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:37.974] frame process finished the id is: 59 the obs is:0 
-[17:50:38.044] Odometry x=-0.010007, y=0.554133, theta=0.065516
-[17:50:38.075] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:66.148182 ,While minimum is: 10.000000  KLT tracked: 535
-[17:50:38.075] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:38.075] frame process finished the id is: 60 the obs is:0 
-[17:50:38.177] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:66.284359 ,While minimum is: 10.000000  KLT tracked: 535
-[17:50:38.177] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:38.177] frame process finished the id is: 61 the obs is:0 
-[17:50:38.281] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:59.361306 ,While minimum is: 10.000000  KLT tracked: 535
-[17:50:38.281] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:38.281] frame process finished the id is: 62 the obs is:0 
-[17:50:38.331] Odometry x=-0.011163, y=0.573468, theta=0.049597
-[17:50:38.382] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:57.549421 ,While minimum is: 10.000000  KLT tracked: 535
-[17:50:38.382] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:38.382] frame process finished the id is: 63 the obs is:0 
-[17:50:38.484] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:55.672159 ,While minimum is: 10.000000  KLT tracked: 534
-[17:50:38.484] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:38.484] frame process finished the id is: 64 the obs is:0 
-[17:50:38.543] Odometry x=-0.012779, y=0.592351, theta=0.053998
-[17:50:38.586] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:56.877505 ,While minimum is: 10.000000  KLT tracked: 532
-[17:50:38.586] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:38.586] frame process finished the id is: 65 the obs is:0 
-[17:50:38.691] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:59.757591 ,While minimum is: 10.000000  KLT tracked: 532
-[17:50:38.691] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:38.691] frame process finished the id is: 66 the obs is:0 
-[17:50:38.793] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:60.977541 ,While minimum is: 10.000000  KLT tracked: 532
-[17:50:38.793] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:38.793] frame process finished the id is: 67 the obs is:0 
-[17:50:38.801] Odometry x=-0.013783, y=0.610537, theta=0.056385
-[17:50:38.895] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:62.845157 ,While minimum is: 10.000000  KLT tracked: 532
-[17:50:38.895] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:38.895] frame process finished the id is: 68 the obs is:0 
-[17:50:38.995] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:66.353970 ,While minimum is: 10.000000  KLT tracked: 530
-[17:50:38.995] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:38.995] frame process finished the id is: 69 the obs is:0 
-[17:50:39.065] Odometry x=-0.014543, y=0.628569, theta=0.063463
-[17:50:39.096] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:68.514352 ,While minimum is: 10.000000  KLT tracked: 530
-[17:50:39.096] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:39.096] frame process finished the id is: 70 the obs is:0 
-[17:50:39.202] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:66.615560 ,While minimum is: 10.000000  KLT tracked: 530
-[17:50:39.202] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:39.202] frame process finished the id is: 71 the obs is:0 
-[17:50:39.296] Odometry x=-0.015858, y=0.646928, theta=0.083319
-[17:50:39.303] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:67.226885 ,While minimum is: 10.000000  KLT tracked: 529
-[17:50:39.303] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:39.303] frame process finished the id is: 72 the obs is:0 
-[17:50:39.402] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:68.437670 ,While minimum is: 10.000000  KLT tracked: 528
-[17:50:39.402] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:39.402] frame process finished the id is: 73 the obs is:0 
-[17:50:39.504] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:69.508952 ,While minimum is: 10.000000  KLT tracked: 528
-[17:50:39.504] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:39.504] frame process finished the id is: 74 the obs is:0 
-[17:50:39.547] Odometry x=-0.017335, y=0.665474, theta=0.086044
-[17:50:39.604] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:68.999405 ,While minimum is: 10.000000  KLT tracked: 528
-[17:50:39.604] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:39.604] frame process finished the id is: 75 the obs is:0 
-[17:50:39.712] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:74.253572 ,While minimum is: 10.000000  KLT tracked: 528
-[17:50:39.712] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:39.712] frame process finished the id is: 76 the obs is:0 
-[17:50:39.812] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:71.703765 ,While minimum is: 10.000000  KLT tracked: 528
-[17:50:39.812] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:39.812] frame process finished the id is: 77 the obs is:0 
-[17:50:39.878] Odometry x=-0.018968, y=0.684542, theta=0.090161
-[17:50:39.911] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:72.506681 ,While minimum is: 10.000000  KLT tracked: 528
-[17:50:39.912] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:39.912] frame process finished the id is: 78 the obs is:0 
-[17:50:40.011] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:69.933815 ,While minimum is: 10.000000  KLT tracked: 528
-[17:50:40.011] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:40.011] frame process finished the id is: 79 the obs is:0 
-[17:50:40.042] Odometry x=-0.020779, y=0.703455, theta=0.103384
-[17:50:40.111] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:73.374817 ,While minimum is: 10.000000  KLT tracked: 527
-[17:50:40.111] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:40.111] frame process finished the id is: 80 the obs is:0 
-[17:50:40.218] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:77.413818 ,While minimum is: 10.000000  KLT tracked: 526
-[17:50:40.218] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:40.218] frame process finished the id is: 81 the obs is:0 
-[17:50:40.318] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:78.690793 ,While minimum is: 10.000000  KLT tracked: 521
-[17:50:40.318] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:40.318] frame process finished the id is: 82 the obs is:0 
-[17:50:40.347] Odometry x=-0.023292, y=0.721982, theta=0.111924
-[17:50:40.417] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:87.487700 ,While minimum is: 10.000000  KLT tracked: 508
-[17:50:40.417] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:40.417] frame process finished the id is: 83 the obs is:0 
-[17:50:40.516] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:89.148491 ,While minimum is: 10.000000  KLT tracked: 504
-[17:50:40.516] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:40.516] frame process finished the id is: 84 the obs is:0 
-[17:50:40.547] Odometry x=-0.025504, y=0.740643, theta=0.105352
-[17:50:40.618] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:91.776485 ,While minimum is: 10.000000  KLT tracked: 502
-[17:50:40.618] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:40.618] frame process finished the id is: 85 the obs is:0 
-[17:50:40.718] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:86.958165 ,While minimum is: 10.000000  KLT tracked: 495
-[17:50:40.718] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:40.719] frame process finished the id is: 86 the obs is:0 
-[17:50:40.799] Odometry x=-0.027781, y=0.760877, theta=0.100422
-[17:50:40.817] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required.  px average disparity is:85.094985 ,While minimum is: 10.000000  KLT tracked: 495
-[17:50:40.817] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 
-[17:50:40.817] frame process finished the id is: 87 th
\ No newline at end of file
diff --git a/GPU_version/vio/include/vio/global_optimizer.h b/GPU_version/vio/include/vio/global_optimizer.h
index 3b884ea..bdb1ddc 100644
--- a/GPU_version/vio/include/vio/global_optimizer.h
+++ b/GPU_version/vio/include/vio/global_optimizer.h
@@ -76,6 +76,7 @@ protected:
   boost::mutex mtx_;
   boost::thread* thread_;
   Map& map_;
+  size_t v_id_ = 0;
 
 #if VIO_DEBUG
   FILE* log_=nullptr;
@@ -89,14 +90,10 @@ protected:
 
 /// Create a g2o vertice from a keyframe object.
         g2o::VertexSE3Expmap* createG2oFrameSE3(
-                FramePtr kf,
-                size_t id,
-                bool fixed);
+                FramePtr kf);
     /// Creates a g2o vertice from a mappoint object.
         g2o::VertexSBAPointXYZ* createG2oPoint(
-                Vector3d pos,
-                size_t id,
-                bool fixed);
+                Vector3d pos);
   /// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement.
         g2o::EdgeProjectXYZ2UV* createG2oEdgeSE3(
                 g2o::VertexSE3Expmap* v_kf,
diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml
index ed946b9..73f21c3 100644
--- a/GPU_version/vio/param/vo_fast.yaml
+++ b/GPU_version/vio/param/vo_fast.yaml
@@ -8,7 +8,7 @@ vio:
   init_min_disparity: 10
   structureoptim_max_pts: 50
   kfselect_mindist: 0.01
-  poseoptim_thresh: 1.25
+  poseoptim_thresh: 1.3
   ACC_ekf: 0.2          #acc white noise in continuous in EKF
   GYO_ekf: 0.1         #gyro white noise in continuous in EKF
   VO_ekf_translation: 1.0              #VO noise  in EKF
diff --git a/GPU_version/vio/src/global_optimizer.cpp b/GPU_version/vio/src/global_optimizer.cpp
index 093bb51..61edcc0 100644
--- a/GPU_version/vio/src/global_optimizer.cpp
+++ b/GPU_version/vio/src/global_optimizer.cpp
@@ -78,6 +78,32 @@ namespace vio {
                     vio::time_in_HH_MM_SS_MMM().c_str());
 #endif
             new_keyframe_=false;
+
+            ba_mux_.lock();
+            // Go through all Keyframes
+            v_id_ = 0;
+            for(auto it_kf=map_.keyframes_.begin();it_kf!=map_.keyframes_.end();++it_kf)
+            {
+                // New Keyframe Vertex
+                (*it_kf)->v_kf_ = createG2oFrameSE3(*it_kf);
+                for(auto&& it_ftr:(*it_kf)->fts_)
+                {
+                    if(it_ftr->point==NULL)continue;
+                    if(it_ftr->point->type_ != vio::Point::TYPE_GOOD)continue;
+                    // for each keyframe add edges to all observed mapoints
+                    if(it_ftr->point->pos_.hasNaN())continue;
+                    if(it_ftr->point->pos_.norm()==0.)continue;
+                    if(it_ftr->point->v_pt_.first == NULL)
+                    {
+                        // mappoint-vertex doesn't exist yet. create a new one:
+                        it_ftr->point->v_pt_.first = createG2oPoint(it_ftr->point->pos_);
+                    }
+                    it_ftr->point->v_pt_.second.push_back(createG2oEdgeSE3((*it_kf)->v_kf_, it_ftr->point->v_pt_.first, vk::project2d(it_ftr->f),
+                                                                         true,
+                                                                         Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth()));
+                }
+            }
+            ba_mux_.unlock();
             g2o::SparseOptimizer optimizer;
             optimizer.setVerbose(false);
 
@@ -97,44 +123,35 @@ namespace vio {
             }
             // init g2o
             list< pair<FramePtr,std::shared_ptr<Feature>> > incorrect_edges;
-            ba_mux_.lock();
             g2o::OptimizableGraph::VertexContainer points;
-            // Go through all Keyframes
-            size_t v_id = 0;
-            for(auto&& it_kf : map_.keyframes_)
-            {
-                // New Keyframe Vertex
-                it_kf->v_kf_ = createG2oFrameSE3(it_kf, v_id++, false);
-                for(auto&& it_ftr:it_kf->fts_)
-                {
-                    if(it_ftr->point==NULL)continue;
-                    if(it_ftr->point->type_ != vio::Point::TYPE_GOOD)continue;
-                    // for each keyframe add edges to all observed mapoints
-                    if(it_ftr->point->pos_.hasNaN())continue;
-                    if(it_ftr->point->pos_.norm()==0.)continue;
-                    if(it_ftr->point->v_pt_.first == NULL)
-                    {
-                        // mappoint-vertex doesn't exist yet. create a new one:
-                        it_ftr->point->v_pt_.first = createG2oPoint(it_ftr->point->pos_, v_id++, true);
+            auto end=map_.keyframes_.end();
+            auto end_1=end--;
+            for(auto it=map_.keyframes_.begin(); it!=map_.keyframes_.end();++it){
+                optimizer.addVertex((*it)->v_kf_);
+                for(auto&& p:(*it)->fts_){
+                    if(p->point == NULL)continue;
+                    if(p->point->v_pt_.first == NULL)continue;
+                    if(!p->point->v_pt_.second.empty()){
+                        if(p->point->v_pt_.second.size()>1){
+                            optimizer.addVertex(p->point->v_pt_.first);///TODO
+                            if(p->point->v_pt_.first->dimension() == 3)points.push_back(p->point->v_pt_.first);
+                            for(auto&& e:p->point->v_pt_.second)optimizer.addEdge(e);
+                            p->point->v_pt_.second.clear();
+                        }
                     }
-                    it_ftr->point->v_pt_.second.push_back(createG2oEdgeSE3(it_kf->v_kf_, it_ftr->point->v_pt_.first, vk::project2d(it_ftr->f),
-                                                                         true,
-                                                                         Config::poseOptimThresh()/it_kf->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth()));
                 }
-            }
-            for(auto&& it:map_.keyframes_){
-                optimizer.addVertex(it->v_kf_);
-                for(auto&& p:it->fts_)if(p->point->v_pt_.second.size()>1){
-                        optimizer.addVertex(p->point->v_pt_.first);
-                        for(auto&& e:p->point->v_pt_.second)optimizer.addEdge(e);
+                if(!(*it)->v_kf_->edges().empty()){
+                    if((*it)->v_kf_->edges().size()>1 && (*it)->v_kf_->dimension()==3)points.push_back((*it)->v_kf_);
+                    if(it == end_1-- || it ==end || it==map_.keyframes_.end()) (*it)->v_kf_->setFixed(false);
+                }else{
+                    optimizer.removeVertex((*it)->v_kf_);
                 }
-                if(it->v_kf_->edges().size()>1)points.push_back(it->v_kf_);
-            }
-            if(optimizer.vertices().empty() || optimizer.vertices().size()<1){
-                ba_mux_.unlock();
-                continue;
             }
             // Optimization
+            if(points.empty()){
+                optimizer.clear();
+                continue;
+            }
             optimizer.initializeOptimization();
             optimizer.computeActiveErrors();
             g2o::StructureOnlySolver<3> structure_only_ba;
@@ -144,18 +161,19 @@ namespace vio {
                     vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
 #endif
             if(optimizer.optimize(vio::Config::lobaNumIter())<1){
-                ba_mux_.unlock();
+                optimizer.clear();
                 continue;
             }
 #if VIO_DEBUG
             fprintf(log_,"[%s] end error: %f \n",
                     vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
 #endif
+            ba_mux_.lock();
             // Update Keyframe and MapPoint Positions
             for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
                 it_kf != map_.keyframes_.end();++it_kf)
             {
-                (*it_kf)->T_f_w_ = SE2_5(SE3((*it_kf)->v_kf_->estimate().rotation(),
+                (*it_kf)->T_f_w_ = SE2_5(SE3((*it_kf)->v_kf_->estimate().rotation().toRotationMatrix(),
                                         (*it_kf)->v_kf_->estimate().translation()));
                 (*it_kf)->v_kf_ = NULL;
                 for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
@@ -170,34 +188,27 @@ namespace vio {
                 }
             }
             ba_mux_.unlock();
+            optimizer.clear();
         }
     }
 
    g2o::VertexSE3Expmap*
-   BA_Glob::createG2oFrameSE3(FramePtr frame, size_t id, bool fixed)
+   BA_Glob::createG2oFrameSE3(FramePtr frame)
    {
-/*        if(frame->id_==0 || frame->id_==1){
-            g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap();
-            v->setId(id);
-            v->setToOrigin();
-            v->setFixed(true);
-            v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation()));
-        }*/
-/*std::cerr<<frame->se3().unit_quaternion().matrix()<<'\n'<<frame->se3().translation()<<'\n';*/
        g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap();
-       v->setId(id);
-       v->setFixed(false);
+       ++v_id_;
+       v->setId(v_id_);
+       v->setFixed(true);
        v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation()));
        return v;
    }
 
    g2o::VertexSBAPointXYZ*
-   BA_Glob::createG2oPoint(Vector3d pos,
-                  size_t id,
-                  bool fixed)
+   BA_Glob::createG2oPoint(Vector3d pos)
    {
+       ++v_id_;
        g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ();
-       v->setId(id);
+       v->setId(v_id_);
        v->setFixed(false);
        v->setMarginalized(true);
        v->setEstimate(pos);
diff --git a/GPU_version/vio/src/initialization.cpp b/GPU_version/vio/src/initialization.cpp
index 6efbc04..f25b8fe 100644
--- a/GPU_version/vio/src/initialization.cpp
+++ b/GPU_version/vio/src/initialization.cpp
@@ -75,7 +75,7 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
     }
   computeHomography(frame_cur,
        features_ref_, px_cur_,
-      frame_ref_->cam_->errorMultiplier2(), 5.0,
+      frame_ref_->cam_->errorMultiplier2(), 6.0,
       inliers_, xyz_in_cur_, T_cur_from_ref_);
   if(inliers_.size() < Config::initMinInliers()){
 #if VIO_DEBUG