diff --git a/GPU_version/vio/CMakeLists.txt b/GPU_version/vio/CMakeLists.txt index bc8efdb..ebe326a 100755 --- a/GPU_version/vio/CMakeLists.txt +++ b/GPU_version/vio/CMakeLists.txt @@ -64,8 +64,8 @@ LIST(APPEND LINK_LIBS ${Boost_Fibers_LIBRARIES} ${OpenCL_LIBRARIES} ${catkin_LIBRARIES} - ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CHOLMOD} - ${G2O_SOLVER_PCG} + ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CSPARSE} ${G2O_SOLVER_STRUCTURE_ONLY} + ${G2O_SOLVER_PCG} ${G2O_SOLVER_CSPARSE_EXTENSION} ${G2O_TYPES_SBA} ${G2O_TYPES_SLAM3D} ${G2O_TYPES_ICP} @@ -85,6 +85,6 @@ target_compile_features(${PROJECT_NAME} PRIVATE cxx_range_for) ADD_DEFINITIONS(-DKERNEL_DIR=\"${PROJECT_SOURCE_DIR}/kernel\") ADD_DEFINITIONS(-DPROJECT_DIR=\"${PROJECT_SOURCE_DIR}\") ADD_DEFINITIONS(-DVIO_DEBUG=true) -ADD_DEFINITIONS(-DG2O_USE_VENDORED_CERES=ON) -ADD_DEFINITIONS(-DG2O_DELETE_IMPLICITLY_OWNED_OBJECTS=OFF) +#ADD_DEFINITIONS(-DG2O_USE_VENDORED_CERES=ON) +#ADD_DEFINITIONS(-DG2O_DELETE_IMPLICITLY_OWNED_OBJECTS=OFF) diff --git a/GPU_version/vio/depth_filter_log.txt b/GPU_version/vio/depth_filter_log.txt new file mode 100755 index 0000000..e69de29 diff --git a/GPU_version/vio/frame_handler_log.txt b/GPU_version/vio/frame_handler_log.txt new file mode 100755 index 0000000..4ad350b --- /dev/null +++ b/GPU_version/vio/frame_handler_log.txt @@ -0,0 +1,298 @@ +[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/frame.h b/GPU_version/vio/include/vio/frame.h index 9f9fc89..f72cf94 100644 --- a/GPU_version/vio/include/vio/frame.h +++ b/GPU_version/vio/include/vio/frame.h @@ -52,7 +52,7 @@ namespace vio { Features fts_; //!< List of features in the image. vector> key_pts_; //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view. bool is_keyframe_; //!< Was this frames selected as keyframe? - g2o::VertexSE3Expmap* v_kf_=nullptr; //!< Temporary pointer to the g2o node object of the keyframe. + g2o::VertexSE3Expmap* v_kf_=NULL; //!< Temporary pointer to the g2o node object of the keyframe. int last_published_ts_; //!< Timestamp of last publishing. Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp); diff --git a/GPU_version/vio/include/vio/global_optimizer.h b/GPU_version/vio/include/vio/global_optimizer.h index 14ac164..3b884ea 100644 --- a/GPU_version/vio/include/vio/global_optimizer.h +++ b/GPU_version/vio/include/vio/global_optimizer.h @@ -30,14 +30,18 @@ #include #include #include +/*#if defined G2O_HAVE_CHOLMOD #include +#elif defined G2O_HAVE_CSPARSE*/ +#include +/*#endif*/ #include #include #include #include #include -G2O_USE_OPTIMIZATION_LIBRARY(cholmod) +G2O_USE_OPTIMIZATION_LIBRARY(csparse) namespace vio { @@ -79,8 +83,8 @@ protected: /// Temporary container to hold the g2o edge with reference to frame and point. struct EdgeContainerSE3{ g2o::EdgeProjectXYZ2UV* edge; - std::shared_ptr frame; - std::shared_ptr feature; + g2o::VertexSE3Expmap* expmap; + g2o::VertexSBAPointXYZ* point; }; /// Create a g2o vertice from a keyframe object. diff --git a/GPU_version/vio/include/vio/point.h b/GPU_version/vio/include/vio/point.h index d6dcb06..ab237c3 100644 --- a/GPU_version/vio/include/vio/point.h +++ b/GPU_version/vio/include/vio/point.h @@ -22,6 +22,7 @@ #include #include #include +/*#include */ static boost::mutex point_mut_; namespace vio { @@ -48,7 +49,8 @@ public: Vector3d pos_; //!< 3d pos of the point in the world coordinate frame. list> obs_; //!< References to keyframes which observe the point. size_t n_obs_; //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames. - g2o::VertexSBAPointXYZ* v_pt_=nullptr; //!< Temporary pointer to the point-vertex in g2o during bundle adjustment. + std::pair> v_pt_=std::make_pair>(NULL, + std::vector()); //!< Temporary pointer to the point-vertex in g2o during bundle adjustment. int last_frame_overlap_id_; //!< Flag for the reprojection: don't reproject a pt twice. PointType type_; //!< Quality of the point. int n_failed_reproj_; //!< Number of failed reprojections. Used to assess the quality of the point. diff --git a/GPU_version/vio/src/frame_handler_base.cpp b/GPU_version/vio/src/frame_handler_base.cpp index a243901..9bc6258 100644 --- a/GPU_version/vio/src/frame_handler_base.cpp +++ b/GPU_version/vio/src/frame_handler_base.cpp @@ -98,7 +98,6 @@ void FrameHandlerBase::optimizeStructure( size_t max_n_pts, int max_iter) { - deque> pts; for(auto&& it:frame->fts_){ if(it->point->obs_.size()<2){ it->point->last_frame_overlap_id_= frame->id_; @@ -107,9 +106,6 @@ void FrameHandlerBase::optimizeStructure( it->point->optimize(max_iter); it->point->last_frame_overlap_id_= frame->id_; } - //max_n_pts = min(max_n_pts, pts.size()); - //nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator); - } void FrameHandlerBase::posEdit( FramePtr frame) diff --git a/GPU_version/vio/src/global_optimizer.cpp b/GPU_version/vio/src/global_optimizer.cpp index 728c3ac..093bb51 100644 --- a/GPU_version/vio/src/global_optimizer.cpp +++ b/GPU_version/vio/src/global_optimizer.cpp @@ -81,7 +81,8 @@ namespace vio { g2o::SparseOptimizer optimizer; optimizer.setVerbose(false); - g2o::BlockSolver_6_3::LinearSolverType * linearSolver=new g2o::LinearSolverCholmod(); + /*g2o::BlockSolver_6_3::LinearSolverType * linearSolver=new g2o::LinearSolverCholmod();*/ + g2o::BlockSolver_6_3::LinearSolverType * linearSolver=new g2o::LinearSolverCSparse(); g2o::BlockSolver_6_3 * solver_ptr= new g2o::BlockSolver_6_3(linearSolver); g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); @@ -95,54 +96,40 @@ namespace vio { assert(false); } // init g2o - list edges; list< pair> > incorrect_edges; ba_mux_.lock(); + g2o::OptimizableGraph::VertexContainer points; // Go through all Keyframes size_t v_id = 0; - for(list::iterator it_kf = map_.keyframes_.begin(); - it_kf != map_.keyframes_.end(); ++it_kf) + for(auto&& it_kf : map_.keyframes_) { // New Keyframe Vertex - g2o::VertexSE3Expmap* v_kf = createG2oFrameSE3(*it_kf, v_id++, false); - (*it_kf)->v_kf_ = v_kf; - optimizer.addVertex(v_kf); - for(auto&& it_ftr:(*it_kf)->fts_) + 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 - std::shared_ptr mp = it_ftr->point; - if(mp->pos_.hasNaN())continue; - if(mp->pos_.norm()==0.)continue; - g2o::VertexSBAPointXYZ* v_mp = mp->v_pt_; - if(v_mp == NULL) + 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: - v_mp = createG2oPoint(mp->pos_, v_id++, true); - mp->v_pt_ = v_mp; - optimizer.addVertex(v_mp); + it_ftr->point->v_pt_.first = createG2oPoint(it_ftr->point->pos_, v_id++, true); } - - // Due to merging of mappoints it is possible that references in kfs suddenly - // have a very large reprojection error which may result in distorted results. -/* Vector2d error = vk::project2d(it_ftr->f) - vk::project2d((*it_kf)->w2f(mp->pos_)); - if(error.squaredNorm() > pow(2,Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2())) - incorrect_edges.push_back(pair>(*it_kf, it_ftr)); - else - {*/ - g2o::EdgeProjectXYZ2UV* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d(it_ftr->f), + 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()); - EdgeContainerSE3 edge; - edge.frame=(*it_kf); - edge.edge=e; - edge.feature=it_ftr; - edges.push_back(edge); - optimizer.addEdge(e); - // } + 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().size()>1)points.push_back(it->v_kf_); + } if(optimizer.vertices().empty() || optimizer.vertices().size()<1){ ba_mux_.unlock(); continue; @@ -150,16 +137,8 @@ namespace vio { // Optimization optimizer.initializeOptimization(); optimizer.computeActiveErrors(); - -/* g2o::StructureOnlySolver<3> structure_only_ba; - g2o::OptimizableGraph::VertexContainer points; - for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) { - g2o::OptimizableGraph::Vertex* v = static_cast(it->second); - if (v->dimension() == 3) - points.push_back(v); - } - - structure_only_ba.calc(points, 10);*/ + g2o::StructureOnlySolver<3> structure_only_ba; + structure_only_ba.calc(points, vio::Config::lobaNumIter()); #if VIO_DEBUG fprintf(log_,"[%s] init error: %f \n", vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2()); @@ -181,28 +160,15 @@ namespace vio { (*it_kf)->v_kf_ = NULL; for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr) { - std::shared_ptr mp = (*it_ftr)->point; - if(mp == NULL) + if((*it_ftr)->point == NULL) continue; - if(mp->v_pt_ == NULL) + if((*it_ftr)->point->v_pt_.first == NULL) continue; // mp was updated before - mp->pos_ = mp->v_pt_->estimate(); - mp->v_pt_ = NULL; + (*it_ftr)->point->pos_ = (*it_ftr)->point->v_pt_.first->estimate(); + (*it_ftr)->point->v_pt_ = std::make_pair>(NULL, + std::vector()); } } - - /*// Remove Measurements with too large reprojection error - for(list< pair> >::iterator it=incorrect_edges.begin(); - it!=incorrect_edges.end(); ++it) - map_.removePtFrameRef(it->first, it->second); - - for(list::iterator it = edges.begin(); it != edges.end(); ++it) - { - if(it->edge->chi2() > 0.001) - { - map_.removePtFrameRef(it->frame, it->feature); - } - }*/ ba_mux_.unlock(); } } @@ -217,6 +183,7 @@ namespace vio { v->setFixed(true); v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation())); }*/ +/*std::cerr<se3().unit_quaternion().matrix()<<'\n'<se3().translation()<<'\n';*/ g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap(); v->setId(id); v->setFixed(false); @@ -231,7 +198,7 @@ namespace vio { { g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ(); v->setId(id); - //v->setFixed(fixed); + v->setFixed(false); v->setMarginalized(true); v->setEstimate(pos); return v; diff --git a/GPU_version/vio/src/point.cpp b/GPU_version/vio/src/point.cpp index d5b2597..e56bdf6 100644 --- a/GPU_version/vio/src/point.cpp +++ b/GPU_version/vio/src/point.cpp @@ -29,7 +29,8 @@ Point::Point(const Vector3d& pos) : id_(point_counter_++), pos_(pos), n_obs_(0), - v_pt_(NULL), + v_pt_(std::make_pair>(NULL, + std::vector())), last_frame_overlap_id_(-10), type_(TYPE_UNKNOWN), n_failed_reproj_(0), @@ -40,7 +41,8 @@ Point::Point(const Vector3d& pos, std::shared_ptr ftr) : id_(point_counter_++), pos_(pos), n_obs_(1), - v_pt_(NULL), + v_pt_(std::make_pair>(NULL, + std::vector())), last_frame_overlap_id_(-10), type_(TYPE_UNKNOWN), n_failed_reproj_(0),