public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

change the ba pipline to avoid a vertex with not edge

Browse Source
This commit is contained in:
gfuse
2021-10-15 17:52:08 +02:00
parent da1242746d
commit cd209204bf
9 changed files with 346 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/CMakeLists.txt
+4 -4
View File
@@ -64,8 +64,8 @@ LIST(APPEND LINK_LIBS
${Boost_Fibers_LIBRARIES} ${Boost_Fibers_LIBRARIES}
${OpenCL_LIBRARIES} ${OpenCL_LIBRARIES}
${catkin_LIBRARIES} ${catkin_LIBRARIES}
${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CHOLMOD} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CSPARSE} ${G2O_SOLVER_STRUCTURE_ONLY}
${G2O_SOLVER_PCG} ${G2O_SOLVER_PCG} ${G2O_SOLVER_CSPARSE_EXTENSION}
${G2O_TYPES_SBA} ${G2O_TYPES_SBA}
${G2O_TYPES_SLAM3D} ${G2O_TYPES_SLAM3D}
${G2O_TYPES_ICP} ${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(-DKERNEL_DIR=\"${PROJECT_SOURCE_DIR}/kernel\")
ADD_DEFINITIONS(-DPROJECT_DIR=\"${PROJECT_SOURCE_DIR}\") ADD_DEFINITIONS(-DPROJECT_DIR=\"${PROJECT_SOURCE_DIR}\")
ADD_DEFINITIONS(-DVIO_DEBUG=true) ADD_DEFINITIONS(-DVIO_DEBUG=true)
ADD_DEFINITIONS(-DG2O_USE_VENDORED_CERES=ON) #ADD_DEFINITIONS(-DG2O_USE_VENDORED_CERES=ON)
ADD_DEFINITIONS(-DG2O_DELETE_IMPLICITLY_OWNED_OBJECTS=OFF) #ADD_DEFINITIONS(-DG2O_DELETE_IMPLICITLY_OWNED_OBJECTS=OFF)
GPU_version/vio/depth_filter_log.txt
Executable
View File
GPU_version/vio/frame_handler_log.txt
Executable
+298
View File
@@ -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
GPU_version/vio/include/vio/frame.h
+1 -1
View File
@@ -52,7 +52,7 @@ namespace vio {
Features fts_; //!< List of features in the image. Features fts_; //!< List of features in the image.
vector<std::shared_ptr<Feature>> key_pts_; //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view. vector<std::shared_ptr<Feature>> 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? 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. int last_published_ts_; //!< Timestamp of last publishing.
Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp); Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp);
GPU_version/vio/include/vio/global_optimizer.h
+7 -3
View File
@@ -30,14 +30,18 @@
#include <g2o/core/solver.h> #include <g2o/core/solver.h>
#include <g2o/core/robust_kernel_impl.h> #include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h> #include <g2o/core/optimization_algorithm_levenberg.h>
/*#if defined G2O_HAVE_CHOLMOD
#include <g2o/solvers/cholmod/linear_solver_cholmod.h> #include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#elif defined G2O_HAVE_CSPARSE*/
#include <g2o/solvers/csparse/linear_solver_csparse.h>
/*#endif*/
#include <g2o/solvers/structure_only/structure_only_solver.h> #include <g2o/solvers/structure_only/structure_only_solver.h>
#include <g2o/core/optimization_algorithm_factory.h> #include <g2o/core/optimization_algorithm_factory.h>
#include <g2o/types/icp/types_icp.h> #include <g2o/types/icp/types_icp.h>
#include <g2o/stuff/sampler.h> #include <g2o/stuff/sampler.h>
#include <memory> #include <memory>
G2O_USE_OPTIMIZATION_LIBRARY(cholmod) G2O_USE_OPTIMIZATION_LIBRARY(csparse)
namespace vio { namespace vio {
@@ -79,8 +83,8 @@ protected:
/// Temporary container to hold the g2o edge with reference to frame and point. /// Temporary container to hold the g2o edge with reference to frame and point.
struct EdgeContainerSE3{ struct EdgeContainerSE3{
g2o::EdgeProjectXYZ2UV* edge; g2o::EdgeProjectXYZ2UV* edge;
std::shared_ptr<Frame> frame; g2o::VertexSE3Expmap* expmap;
std::shared_ptr<Feature> feature; g2o::VertexSBAPointXYZ* point;
}; };
/// Create a g2o vertice from a keyframe object. /// Create a g2o vertice from a keyframe object.
GPU_version/vio/include/vio/point.h
+3 -1
View File
@@ -22,6 +22,7 @@
#include <boost/thread.hpp> #include <boost/thread.hpp>
#include <boost/function.hpp> #include <boost/function.hpp>
#include <g2o/types/sba/types_six_dof_expmap.h> #include <g2o/types/sba/types_six_dof_expmap.h>
/*#include <g2o/types/icp/types_icp.h>*/
static boost::mutex point_mut_; static boost::mutex point_mut_;
namespace vio { namespace vio {
@@ -48,7 +49,8 @@ public:
Vector3d pos_; //!< 3d pos of the point in the world coordinate frame. Vector3d pos_; //!< 3d pos of the point in the world coordinate frame.
list<std::shared_ptr<Feature>> obs_; //!< References to keyframes which observe the point. list<std::shared_ptr<Feature>> obs_; //!< References to keyframes which observe the point.
size_t n_obs_; //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames. 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<g2o::VertexSBAPointXYZ*,std::vector<g2o::EdgeProjectXYZ2UV*>> v_pt_=std::make_pair<g2o::VertexSBAPointXYZ*,std::vector<g2o::EdgeProjectXYZ2UV*>>(NULL,
std::vector<g2o::EdgeProjectXYZ2UV*>()); //!< 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. int last_frame_overlap_id_; //!< Flag for the reprojection: don't reproject a pt twice.
PointType type_; //!< Quality of the point. PointType type_; //!< Quality of the point.
int n_failed_reproj_; //!< Number of failed reprojections. Used to assess the quality of the point. int n_failed_reproj_; //!< Number of failed reprojections. Used to assess the quality of the point.
GPU_version/vio/src/frame_handler_base.cpp
-4
View File
@@ -98,7 +98,6 @@ void FrameHandlerBase::optimizeStructure(
size_t max_n_pts, size_t max_n_pts,
int max_iter) int max_iter)
{ {
deque<std::shared_ptr<Point>> pts;
for(auto&& it:frame->fts_){ for(auto&& it:frame->fts_){
if(it->point->obs_.size()<2){ if(it->point->obs_.size()<2){
it->point->last_frame_overlap_id_= frame->id_; it->point->last_frame_overlap_id_= frame->id_;
@@ -107,9 +106,6 @@ void FrameHandlerBase::optimizeStructure(
it->point->optimize(max_iter); it->point->optimize(max_iter);
it->point->last_frame_overlap_id_= frame->id_; 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( void FrameHandlerBase::posEdit(
FramePtr frame) FramePtr frame)
GPU_version/vio/src/global_optimizer.cpp
+29 -62
View File
@@ -81,7 +81,8 @@ namespace vio {
g2o::SparseOptimizer optimizer; g2o::SparseOptimizer optimizer;
optimizer.setVerbose(false); optimizer.setVerbose(false);
g2o::BlockSolver_6_3::LinearSolverType * linearSolver=new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>(); /*g2o::BlockSolver_6_3::LinearSolverType * linearSolver=new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();*/
g2o::BlockSolver_6_3::LinearSolverType * linearSolver=new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3 * solver_ptr= new g2o::BlockSolver_6_3(linearSolver); g2o::BlockSolver_6_3 * solver_ptr= new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
@@ -95,54 +96,40 @@ namespace vio {
assert(false); assert(false);
} }
// init g2o // init g2o
list<EdgeContainerSE3> edges;
list< pair<FramePtr,std::shared_ptr<Feature>> > incorrect_edges; list< pair<FramePtr,std::shared_ptr<Feature>> > incorrect_edges;
ba_mux_.lock(); ba_mux_.lock();
g2o::OptimizableGraph::VertexContainer points;
// Go through all Keyframes // Go through all Keyframes
size_t v_id = 0; size_t v_id = 0;
for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin(); for(auto&& it_kf : map_.keyframes_)
it_kf != map_.keyframes_.end(); ++it_kf)
{ {
// New Keyframe Vertex // New Keyframe Vertex
g2o::VertexSE3Expmap* v_kf = createG2oFrameSE3(*it_kf, v_id++, false); it_kf->v_kf_ = createG2oFrameSE3(it_kf, v_id++, false);
(*it_kf)->v_kf_ = v_kf; for(auto&& it_ftr:it_kf->fts_)
optimizer.addVertex(v_kf);
for(auto&& it_ftr:(*it_kf)->fts_)
{ {
if(it_ftr->point==NULL)continue; if(it_ftr->point==NULL)continue;
if(it_ftr->point->type_ != vio::Point::TYPE_GOOD)continue; if(it_ftr->point->type_ != vio::Point::TYPE_GOOD)continue;
// for each keyframe add edges to all observed mapoints // for each keyframe add edges to all observed mapoints
std::shared_ptr<Point> mp = it_ftr->point; if(it_ftr->point->pos_.hasNaN())continue;
if(mp->pos_.hasNaN())continue; if(it_ftr->point->pos_.norm()==0.)continue;
if(mp->pos_.norm()==0.)continue; if(it_ftr->point->v_pt_.first == NULL)
g2o::VertexSBAPointXYZ* v_mp = mp->v_pt_;
if(v_mp == NULL)
{ {
// mappoint-vertex doesn't exist yet. create a new one: // mappoint-vertex doesn't exist yet. create a new one:
v_mp = createG2oPoint(mp->pos_, v_id++, true); it_ftr->point->v_pt_.first = createG2oPoint(it_ftr->point->pos_, v_id++, true);
mp->v_pt_ = v_mp;
optimizer.addVertex(v_mp);
} }
it_ftr->point->v_pt_.second.push_back(createG2oEdgeSE3(it_kf->v_kf_, it_ftr->point->v_pt_.first, vk::project2d(it_ftr->f),
// 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<FramePtr,std::shared_ptr<Feature>>(*it_kf, it_ftr));
else
{*/
g2o::EdgeProjectXYZ2UV* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d(it_ftr->f),
true, true,
Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth()); 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);
// }
} }
} }
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){ if(optimizer.vertices().empty() || optimizer.vertices().size()<1){
ba_mux_.unlock(); ba_mux_.unlock();
continue; continue;
@@ -150,16 +137,8 @@ namespace vio {
// Optimization // Optimization
optimizer.initializeOptimization(); optimizer.initializeOptimization();
optimizer.computeActiveErrors(); optimizer.computeActiveErrors();
g2o::StructureOnlySolver<3> structure_only_ba;
/* g2o::StructureOnlySolver<3> structure_only_ba; structure_only_ba.calc(points, vio::Config::lobaNumIter());
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<g2o::OptimizableGraph::Vertex*>(it->second);
if (v->dimension() == 3)
points.push_back(v);
}
structure_only_ba.calc(points, 10);*/
#if VIO_DEBUG #if VIO_DEBUG
fprintf(log_,"[%s] init error: %f \n", fprintf(log_,"[%s] init error: %f \n",
vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2()); vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
@@ -181,28 +160,15 @@ namespace vio {
(*it_kf)->v_kf_ = NULL; (*it_kf)->v_kf_ = NULL;
for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr) for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
{ {
std::shared_ptr<Point> mp = (*it_ftr)->point; if((*it_ftr)->point == NULL)
if(mp == NULL)
continue; continue;
if(mp->v_pt_ == NULL) if((*it_ftr)->point->v_pt_.first == NULL)
continue; // mp was updated before continue; // mp was updated before
mp->pos_ = mp->v_pt_->estimate(); (*it_ftr)->point->pos_ = (*it_ftr)->point->v_pt_.first->estimate();
mp->v_pt_ = NULL; (*it_ftr)->point->v_pt_ = std::make_pair<g2o::VertexSBAPointXYZ*,std::vector<g2o::EdgeProjectXYZ2UV*>>(NULL,
std::vector<g2o::EdgeProjectXYZ2UV*>());
} }
} }
/*// Remove Measurements with too large reprojection error
for(list< pair<FramePtr,std::shared_ptr<Feature>> >::iterator it=incorrect_edges.begin();
it!=incorrect_edges.end(); ++it)
map_.removePtFrameRef(it->first, it->second);
for(list<EdgeContainerSE3>::iterator it = edges.begin(); it != edges.end(); ++it)
{
if(it->edge->chi2() > 0.001)
{
map_.removePtFrameRef(it->frame, it->feature);
}
}*/
ba_mux_.unlock(); ba_mux_.unlock();
} }
} }
@@ -217,6 +183,7 @@ namespace vio {
v->setFixed(true); v->setFixed(true);
v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation())); 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(); g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap();
v->setId(id); v->setId(id);
v->setFixed(false); v->setFixed(false);
@@ -231,7 +198,7 @@ namespace vio {
{ {
g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ(); g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ();
v->setId(id); v->setId(id);
//v->setFixed(fixed); v->setFixed(false);
v->setMarginalized(true); v->setMarginalized(true);
v->setEstimate(pos); v->setEstimate(pos);
return v; return v;
GPU_version/vio/src/point.cpp
+4 -2
View File
@@ -29,7 +29,8 @@ Point::Point(const Vector3d& pos) :
id_(point_counter_++), id_(point_counter_++),
pos_(pos), pos_(pos),
n_obs_(0), n_obs_(0),
v_pt_(NULL), v_pt_(std::make_pair<g2o::VertexSBAPointXYZ*,std::vector<g2o::EdgeProjectXYZ2UV*>>(NULL,
std::vector<g2o::EdgeProjectXYZ2UV*>())),
last_frame_overlap_id_(-10), last_frame_overlap_id_(-10),
type_(TYPE_UNKNOWN), type_(TYPE_UNKNOWN),
n_failed_reproj_(0), n_failed_reproj_(0),
@@ -40,7 +41,8 @@ Point::Point(const Vector3d& pos, std::shared_ptr<Feature> ftr) :
id_(point_counter_++), id_(point_counter_++),
pos_(pos), pos_(pos),
n_obs_(1), n_obs_(1),
v_pt_(NULL), v_pt_(std::make_pair<g2o::VertexSBAPointXYZ*,std::vector<g2o::EdgeProjectXYZ2UV*>>(NULL,
std::vector<g2o::EdgeProjectXYZ2UV*>())),
last_frame_overlap_id_(-10), last_frame_overlap_id_(-10),
type_(TYPE_UNKNOWN), type_(TYPE_UNKNOWN),
n_failed_reproj_(0), n_failed_reproj_(0),