From 183c2a31e4043da75394f1d5fffe3e930b1ba4c8 Mon Sep 17 00:00:00 2001 From: ymd-stella <7959916+ymd-stella@users.noreply.github.com> Date: Sun, 6 Feb 2022 13:36:37 +0900 Subject: [PATCH] Remove GPL infringing code (#252) * Fix frame/keyframe (Remove GPL infringing code) * Fix graph_node (Remove GPL infringing code) * Fix landmark (Remove GPL infringing code) * Fix map_database (Remove GPL infringing code) * Fix frame_publicher (Remove GPL infringing code (Remove overlay text)) * Fix pangolin_viewer (Remove GPL infringing code) Remove color scheme black * Fix about request (Remove GPL infringing code) * Remove temporary variables for tracking (Remove GPL infringing code) * Fix mapping_module (Remove GPL infringing code) * Remove temporary variables for loop closing (Remove GPL infringing code), and - Remove unused parentheses - Split global bundle adjustment into two member functions - Fix nested ifs to logical operations. * Remove duplicated code in loop_detector (Remove GPL infringing code) * Fix orb_extractor (Remove GPL infringing code) * Minimize unnecessary functions (Remove GPL infringing code) * Move depth_thr to camera (Remove GPL infringing code) * Update config files (Move depth_thr to Camera) * Update feature extraction parameters (Remove GPL infringing code) * Remove database from keyframe/frame (Remove GPL infringing code) * Add fame_observation (Remove GPL infringing code) * Fix match::projection (Remove GPL infringing code) * Fix tracking (Remove GPL infringing code) * Fix missing #include * Add NOTE to README.md --- README.md | 2 +- docs/parameters.rst | 42 +- example/aist/equirectangular.yaml | 13 +- example/aist/fisyeye.yaml | 3 +- example/euroc/EuRoC_mono.yaml | 3 +- example/euroc/EuRoC_stereo.yaml | 5 +- example/kitti/KITTI_mono_00-02.yaml | 3 +- example/kitti/KITTI_mono_03.yaml | 3 +- example/kitti/KITTI_mono_04-12.yaml | 3 +- example/kitti/KITTI_stereo_00-02.yaml | 5 +- example/kitti/KITTI_stereo_03.yaml | 5 +- example/kitti/KITTI_stereo_04-12.yaml | 5 +- example/tum_rgbd/TUM_RGBD_mono_1.yaml | 3 +- example/tum_rgbd/TUM_RGBD_mono_2.yaml | 3 +- example/tum_rgbd/TUM_RGBD_mono_3.yaml | 3 +- example/tum_rgbd/TUM_RGBD_rgbd_1.yaml | 5 +- example/tum_rgbd/TUM_RGBD_rgbd_2.yaml | 5 +- example/tum_rgbd/TUM_RGBD_rgbd_3.yaml | 5 +- example/tum_vi/TUM_VI_mono.yaml | 3 +- example/tum_vi/TUM_VI_stereo.yaml | 3 +- src/openvslam/camera/base.cc | 4 +- src/openvslam/camera/base.h | 6 +- src/openvslam/camera/equirectangular.cc | 2 +- src/openvslam/camera/fisheye.cc | 7 +- src/openvslam/camera/fisheye.h | 2 +- src/openvslam/camera/perspective.cc | 7 +- src/openvslam/camera/perspective.h | 2 +- src/openvslam/camera/radial_division.cc | 7 +- src/openvslam/camera/radial_division.h | 2 +- src/openvslam/config.cc | 23 +- src/openvslam/config.h | 3 + src/openvslam/data/CMakeLists.txt | 4 + src/openvslam/data/bow_vocabulary.cc | 17 + src/openvslam/data/bow_vocabulary.h | 10 + src/openvslam/data/bow_vocabulary_fwd.h | 10 +- src/openvslam/data/camera_database.cc | 2 +- src/openvslam/data/frame.cc | 192 +------- src/openvslam/data/frame.h | 131 +----- src/openvslam/data/frame_observation.h | 41 ++ src/openvslam/data/graph_node.cc | 39 +- src/openvslam/data/graph_node.h | 6 + src/openvslam/data/keyframe.cc | 143 ++---- src/openvslam/data/keyframe.h | 106 +---- src/openvslam/data/landmark.cc | 52 +-- src/openvslam/data/landmark.h | 26 +- src/openvslam/data/map_database.cc | 61 ++- src/openvslam/data/map_database.h | 27 +- src/openvslam/data/orb_params_database.cc | 91 ++++ src/openvslam/data/orb_params_database.h | 45 ++ src/openvslam/feature/orb_extractor.cc | 147 ++---- src/openvslam/feature/orb_extractor.h | 57 +-- src/openvslam/feature/orb_params.cc | 55 +-- src/openvslam/feature/orb_params.h | 33 +- src/openvslam/global_optimization_module.cc | 71 +-- src/openvslam/global_optimization_module.h | 22 +- src/openvslam/initialize/base.cc | 2 +- src/openvslam/initialize/bearing_vector.cc | 6 +- src/openvslam/initialize/perspective.cc | 6 +- src/openvslam/io/map_database_io.cc | 15 +- src/openvslam/io/map_database_io.h | 6 +- src/openvslam/io/trajectory_io.cc | 10 +- src/openvslam/mapping_module.cc | 127 ++--- src/openvslam/mapping_module.h | 59 ++- src/openvslam/match/area.cc | 18 +- src/openvslam/match/bow_tree.cc | 14 +- src/openvslam/match/fuse.cc | 24 +- src/openvslam/match/projection.cc | 95 ++-- src/openvslam/match/projection.h | 7 +- src/openvslam/match/robust.cc | 38 +- src/openvslam/module/frame_tracker.cc | 24 +- src/openvslam/module/frame_tracker.h | 12 +- src/openvslam/module/initializer.cc | 56 ++- src/openvslam/module/initializer.h | 20 +- src/openvslam/module/keyframe_inserter.cc | 109 ++--- src/openvslam/module/keyframe_inserter.h | 35 +- src/openvslam/module/local_map_cleaner.cc | 16 +- src/openvslam/module/local_map_cleaner.h | 9 +- src/openvslam/module/local_map_updater.cc | 35 +- src/openvslam/module/local_map_updater.h | 10 +- src/openvslam/module/loop_bundle_adjuster.cc | 42 +- src/openvslam/module/loop_bundle_adjuster.h | 2 +- src/openvslam/module/loop_detector.cc | 48 +- src/openvslam/module/loop_detector.h | 10 + src/openvslam/module/relocalizer.cc | 29 +- src/openvslam/module/relocalizer.h | 10 +- src/openvslam/module/two_view_triangulator.cc | 26 +- .../optimize/global_bundle_adjuster.cc | 120 +++-- .../optimize/global_bundle_adjuster.h | 13 +- src/openvslam/optimize/graph_optimizer.cc | 25 +- src/openvslam/optimize/graph_optimizer.h | 3 +- .../sim3/mutual_reproj_edge_wrapper.h | 32 +- .../optimize/local_bundle_adjuster.cc | 25 +- .../optimize/local_bundle_adjuster.h | 4 +- src/openvslam/optimize/pose_optimizer.cc | 8 +- src/openvslam/publish/frame_publisher.cc | 65 +-- src/openvslam/publish/frame_publisher.h | 7 +- src/openvslam/publish/map_publisher.cc | 10 - src/openvslam/publish/map_publisher.h | 14 - src/openvslam/solve/sim3_solver.cc | 8 +- src/openvslam/system.cc | 271 +++++++++-- src/openvslam/system.h | 32 ++ src/openvslam/tracking_module.cc | 436 +++++++----------- src/openvslam/tracking_module.h | 74 +-- src/openvslam/util/converter.cc | 11 + src/openvslam/util/converter.h | 3 + src/pangolin_viewer/color_scheme.cc | 13 - src/pangolin_viewer/color_scheme.h | 2 - src/pangolin_viewer/viewer.cc | 2 +- test/openvslam/data/bow_vocabulary.cc | 5 +- test/openvslam/feature/orb_extractor.cc | 67 ++- test/openvslam/feature/orb_params.cc | 133 +----- 111 files changed, 1845 insertions(+), 2033 deletions(-) create mode 100644 src/openvslam/data/bow_vocabulary.cc create mode 100644 src/openvslam/data/frame_observation.h create mode 100644 src/openvslam/data/orb_params_database.cc create mode 100644 src/openvslam/data/orb_params_database.h diff --git a/README.md b/README.md index eee6108a9..2df2a9a9f 100644 --- a/README.md +++ b/README.md @@ -9,7 +9,7 @@ --- -> *NOTE:* This is a community fork of [xdspacelab/openvslam](https://github.com/xdspacelab/openvslam). It was created to continue active development of OpenVSLAM on Jan 31, 2021. The original repository is no longer available. Please read the [official statement of termination](https://github.com/xdspacelab/openvslam/wiki/Termination-of-the-release) carefully and understand it before using this. If you are interested in a more detailed discussion, see [here](https://github.com/OpenVSLAM-Community/openvslam/issues/37). +> *NOTE:* This is a community fork of [xdspacelab/openvslam](https://github.com/xdspacelab/openvslam). It was created to continue active development of OpenVSLAM on Jan 31, 2021. The original repository is no longer available. Please read the [official statement of termination](https://github.com/xdspacelab/openvslam/wiki/Termination-of-the-release) carefully and understand it before using this. The similarities with ORB_SLAM2 in the original version have been removed by . If you find any other issues with the license, please point them out. See and for discussion so far. --- diff --git a/docs/parameters.rst b/docs/parameters.rst index f77b6479d..a119b6847 100644 --- a/docs/parameters.rst +++ b/docs/parameters.rst @@ -41,6 +41,8 @@ Camera * - focal_x_baseline - For stereo cameras, it is the value of the baseline between the left and right cameras multiplied by the focal length fx. For RGBD cameras, if the measurement method is stereo, set it based on its baseline. If the measurement method is other than that, set the appropriate value based on the relationship between depth accuracy and baseline. + * - depth_threshold + - The ratio used to determine the depth threshold. .. _section-parameters-feature: @@ -53,6 +55,8 @@ Feature * - Name - Description + * - name + - name of ORB feature extraction model (id for saving) * - scale_factor - Scale of the image pyramid * - num_levels @@ -62,10 +66,10 @@ Feature * - min_fast_threshold - FAST threshold for try second time -.. _section-parameters-tracking: +.. _section-parameters-preprocessing: -Tracking -======== +Preprocessing +============= .. list-table:: :header-rows: 1 @@ -77,10 +81,20 @@ Tracking - Maximum number of feature points per frame to be used for SLAM. * - ini_max_num_keypoints - Maximum number of feature points per frame to be used for Initialization. It is only used for monocular camera models. - * - depth_threshold - - The ratio used to determine the depth threshold. * - depthmap_factor - The ratio used to convert depth image pixel values to distance. + +.. _section-parameters-tracking: + +Tracking +======== + +.. list-table:: + :header-rows: 1 + :widths: 1, 3 + + * - Name + - Description * - reloc_distance_threshold - Maximum distance threshold (in meters) where close keyframes could be found when doing a relocalization by pose. * - reloc_angle_threshold @@ -164,6 +178,24 @@ Relocalizer * - min_num_valid_obs - +.. _section-parameters-keyframe-inserter: + +KeyframeInserter +================ + +.. list-table:: + :header-rows: 1 + :widths: 1, 3 + + * - Name + - Description + * - max_interval + - max interval to insert keyframe + * - lms_ratio_thr_almost_all_lms_are_tracked + - Threshold at which we consider that we are tracking almost all landmarks. Ratio-threshold of "the number of 3D points observed in the current frame" / "that of 3D points observed in the last keyframe" + * - lms_ratio_thr_view_changed + - Threshold at which we consider the view to have changed. Ratio-threshold of "the number of 3D points observed in the current frame" / "that of 3D points observed in the last keyframe" + .. _section-parameters-pangolin: PangolinViewer diff --git a/example/aist/equirectangular.yaml b/example/aist/equirectangular.yaml index fc8cab7d5..952d52e4a 100755 --- a/example/aist/equirectangular.yaml +++ b/example/aist/equirectangular.yaml @@ -19,23 +19,24 @@ Camera: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 2000 + mask_rectangles: + - [0.0, 1.0, 0.0, 0.1] + - [0.0, 1.0, 0.84, 1.0] + - [0.0, 0.2, 0.7, 1.0] + - [0.8, 1.0, 0.7, 1.0] #================# # ORB Parameters # #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 20 min_fast_threshold: 7 - mask_rectangles: - - [0.0, 1.0, 0.0, 0.1] - - [0.0, 1.0, 0.84, 1.0] - - [0.0, 0.2, 0.7, 1.0] - - [0.8, 1.0, 0.7, 1.0] #====================# # Mapping Parameters # diff --git a/example/aist/fisyeye.yaml b/example/aist/fisyeye.yaml index 6af35ac68..926dbc396 100644 --- a/example/aist/fisyeye.yaml +++ b/example/aist/fisyeye.yaml @@ -29,7 +29,7 @@ Camera: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 2000 #================# @@ -37,6 +37,7 @@ Tracking: #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 20 diff --git a/example/euroc/EuRoC_mono.yaml b/example/euroc/EuRoC_mono.yaml index c6627e744..c3b203d5c 100644 --- a/example/euroc/EuRoC_mono.yaml +++ b/example/euroc/EuRoC_mono.yaml @@ -30,7 +30,7 @@ Camera: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 1000 ini_max_num_keypoints: 2000 @@ -39,6 +39,7 @@ Tracking: #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 20 diff --git a/example/euroc/EuRoC_stereo.yaml b/example/euroc/EuRoC_stereo.yaml index 027323cfe..9f9aa4ec6 100644 --- a/example/euroc/EuRoC_stereo.yaml +++ b/example/euroc/EuRoC_stereo.yaml @@ -33,6 +33,7 @@ Camera: cols: 752 rows: 480 focal_x_baseline: 47.90639384423901 + depth_threshold: 40 color_order: "Gray" @@ -55,16 +56,16 @@ StereoRectifier: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 1000 ini_max_num_keypoints: 2000 - depth_threshold: 40 #================# # ORB Parameters # #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 20 diff --git a/example/kitti/KITTI_mono_00-02.yaml b/example/kitti/KITTI_mono_00-02.yaml index b12f57447..244e33071 100644 --- a/example/kitti/KITTI_mono_00-02.yaml +++ b/example/kitti/KITTI_mono_00-02.yaml @@ -30,7 +30,7 @@ Camera: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 2000 ini_max_num_keypoints: 4000 @@ -39,6 +39,7 @@ Tracking: #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 20 diff --git a/example/kitti/KITTI_mono_03.yaml b/example/kitti/KITTI_mono_03.yaml index b4e0e9323..9bf232775 100644 --- a/example/kitti/KITTI_mono_03.yaml +++ b/example/kitti/KITTI_mono_03.yaml @@ -30,7 +30,7 @@ Camera: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 2000 ini_max_num_keypoints: 4000 @@ -39,6 +39,7 @@ Tracking: #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 20 diff --git a/example/kitti/KITTI_mono_04-12.yaml b/example/kitti/KITTI_mono_04-12.yaml index ba8a2656a..8c33fed31 100644 --- a/example/kitti/KITTI_mono_04-12.yaml +++ b/example/kitti/KITTI_mono_04-12.yaml @@ -30,7 +30,7 @@ Camera: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 2000 ini_max_num_keypoints: 4000 @@ -39,6 +39,7 @@ Tracking: #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 20 diff --git a/example/kitti/KITTI_stereo_00-02.yaml b/example/kitti/KITTI_stereo_00-02.yaml index bc3fb8410..fa7a7c9fa 100644 --- a/example/kitti/KITTI_stereo_00-02.yaml +++ b/example/kitti/KITTI_stereo_00-02.yaml @@ -24,6 +24,7 @@ Camera: cols: 1241 rows: 376 focal_x_baseline: 386.1448 + depth_threshold: 40 color_order: "Gray" @@ -31,16 +32,16 @@ Camera: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 2000 ini_max_num_keypoints: 4000 - depth_threshold: 40 #================# # ORB Parameters # #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 12 diff --git a/example/kitti/KITTI_stereo_03.yaml b/example/kitti/KITTI_stereo_03.yaml index fb2dae77c..1b3083df0 100644 --- a/example/kitti/KITTI_stereo_03.yaml +++ b/example/kitti/KITTI_stereo_03.yaml @@ -24,6 +24,7 @@ Camera: cols: 1242 rows: 375 focal_x_baseline: 387.5744 + depth_threshold: 40 color_order: "Gray" @@ -31,16 +32,16 @@ Camera: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 2000 ini_max_num_keypoints: 4000 - depth_threshold: 40 #================# # ORB Parameters # #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 12 diff --git a/example/kitti/KITTI_stereo_04-12.yaml b/example/kitti/KITTI_stereo_04-12.yaml index 6d7fa050b..c2ede9e3d 100644 --- a/example/kitti/KITTI_stereo_04-12.yaml +++ b/example/kitti/KITTI_stereo_04-12.yaml @@ -24,6 +24,7 @@ Camera: cols: 1226 rows: 370 focal_x_baseline: 379.8145 + depth_threshold: 40 color_order: "Gray" @@ -31,16 +32,16 @@ Camera: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 2000 ini_max_num_keypoints: 4000 - depth_threshold: 40 #================# # ORB Parameters # #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 12 diff --git a/example/tum_rgbd/TUM_RGBD_mono_1.yaml b/example/tum_rgbd/TUM_RGBD_mono_1.yaml index 3fe8ceec2..46f8fb957 100644 --- a/example/tum_rgbd/TUM_RGBD_mono_1.yaml +++ b/example/tum_rgbd/TUM_RGBD_mono_1.yaml @@ -30,7 +30,7 @@ Camera: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 1000 ini_max_num_keypoints: 2000 @@ -39,6 +39,7 @@ Tracking: #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 20 diff --git a/example/tum_rgbd/TUM_RGBD_mono_2.yaml b/example/tum_rgbd/TUM_RGBD_mono_2.yaml index 22a24ff62..6ea6d729a 100644 --- a/example/tum_rgbd/TUM_RGBD_mono_2.yaml +++ b/example/tum_rgbd/TUM_RGBD_mono_2.yaml @@ -30,7 +30,7 @@ Camera: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 1000 ini_max_num_keypoints: 2000 @@ -39,6 +39,7 @@ Tracking: #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 20 diff --git a/example/tum_rgbd/TUM_RGBD_mono_3.yaml b/example/tum_rgbd/TUM_RGBD_mono_3.yaml index cd8616937..712f27f7c 100644 --- a/example/tum_rgbd/TUM_RGBD_mono_3.yaml +++ b/example/tum_rgbd/TUM_RGBD_mono_3.yaml @@ -30,7 +30,7 @@ Camera: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 1000 ini_max_num_keypoints: 2000 @@ -39,6 +39,7 @@ Tracking: #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 20 diff --git a/example/tum_rgbd/TUM_RGBD_rgbd_1.yaml b/example/tum_rgbd/TUM_RGBD_rgbd_1.yaml index c19e790e3..ab91c9f2a 100644 --- a/example/tum_rgbd/TUM_RGBD_rgbd_1.yaml +++ b/example/tum_rgbd/TUM_RGBD_rgbd_1.yaml @@ -24,6 +24,7 @@ Camera: cols: 640 rows: 480 focal_x_baseline: 40.0 + depth_threshold: 40.0 color_order: "RGB" @@ -31,10 +32,9 @@ Camera: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 1000 ini_max_num_keypoints: 2000 - depth_threshold: 40.0 depthmap_factor: 5000.0 # Note: Set it to 1.0 for the rosbag format data set. #================# @@ -42,6 +42,7 @@ Tracking: #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 20 diff --git a/example/tum_rgbd/TUM_RGBD_rgbd_2.yaml b/example/tum_rgbd/TUM_RGBD_rgbd_2.yaml index 81df37b04..4fea12347 100644 --- a/example/tum_rgbd/TUM_RGBD_rgbd_2.yaml +++ b/example/tum_rgbd/TUM_RGBD_rgbd_2.yaml @@ -24,6 +24,7 @@ Camera: cols: 640 rows: 480 focal_x_baseline: 40.0 + depth_threshold: 40.0 color_order: "RGB" @@ -31,10 +32,9 @@ Camera: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 1000 ini_max_num_keypoints: 2000 - depth_threshold: 40.0 depthmap_factor: 5000.0 # Note: Set it to 1.0 for the rosbag format data set. #================# @@ -42,6 +42,7 @@ Tracking: #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 20 diff --git a/example/tum_rgbd/TUM_RGBD_rgbd_3.yaml b/example/tum_rgbd/TUM_RGBD_rgbd_3.yaml index 8260c43b5..f523170b5 100644 --- a/example/tum_rgbd/TUM_RGBD_rgbd_3.yaml +++ b/example/tum_rgbd/TUM_RGBD_rgbd_3.yaml @@ -24,6 +24,7 @@ Camera: cols: 640 rows: 480 focal_x_baseline: 40.0 + depth_threshold: 40.0 color_order: "RGB" @@ -31,10 +32,9 @@ Camera: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 1000 ini_max_num_keypoints: 2000 - depth_threshold: 40.0 depthmap_factor: 5000.0 # Note: Set it to 1.0 for the rosbag format data set. #================# @@ -42,6 +42,7 @@ Tracking: #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 20 diff --git a/example/tum_vi/TUM_VI_mono.yaml b/example/tum_vi/TUM_VI_mono.yaml index 2f4430b3d..565b95fb4 100755 --- a/example/tum_vi/TUM_VI_mono.yaml +++ b/example/tum_vi/TUM_VI_mono.yaml @@ -31,7 +31,7 @@ Camera: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 1000 ini_max_num_keypoints: 2000 @@ -40,6 +40,7 @@ Tracking: #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 20 diff --git a/example/tum_vi/TUM_VI_stereo.yaml b/example/tum_vi/TUM_VI_stereo.yaml index 80ec27d01..600d86b99 100755 --- a/example/tum_vi/TUM_VI_stereo.yaml +++ b/example/tum_vi/TUM_VI_stereo.yaml @@ -58,7 +58,7 @@ StereoRectifier: # Tracking Parameters # #=====================# -Tracking: +Preprocessing: max_num_keypoints: 1000 ini_max_num_keypoints: 2000 @@ -67,6 +67,7 @@ Tracking: #================# Feature: + name: "default ORB feature extraction setting" scale_factor: 1.2 num_levels: 8 ini_fast_threshold: 20 diff --git a/src/openvslam/camera/base.cc b/src/openvslam/camera/base.cc index c9f06c724..e5e8f1556 100644 --- a/src/openvslam/camera/base.cc +++ b/src/openvslam/camera/base.cc @@ -9,11 +9,11 @@ namespace camera { base::base(const std::string& name, const setup_type_t setup_type, const model_type_t model_type, const color_order_t color_order, const unsigned int cols, const unsigned int rows, const double fps, - const double focal_x_baseline, const double true_baseline, + const double focal_x_baseline, const double true_baseline, const double depth_thr, const unsigned int num_grid_cols, const unsigned int num_grid_rows) : name_(name), setup_type_(setup_type), model_type_(model_type), color_order_(color_order), cols_(cols), rows_(rows), fps_(fps), - focal_x_baseline_(focal_x_baseline), true_baseline_(true_baseline), + focal_x_baseline_(focal_x_baseline), true_baseline_(true_baseline), depth_thr_(depth_thr), num_grid_cols_(num_grid_cols), num_grid_rows_(num_grid_rows) { spdlog::debug("CONSTRUCT: camera::base"); } diff --git a/src/openvslam/camera/base.h b/src/openvslam/camera/base.h index 9ed1b2e68..1f9f3bafd 100644 --- a/src/openvslam/camera/base.h +++ b/src/openvslam/camera/base.h @@ -58,7 +58,7 @@ class base { //! Constructor base(const std::string& name, const setup_type_t setup_type, const model_type_t model_type, const color_order_t color_order, const unsigned int cols, const unsigned int rows, const double fps, - const double focal_x_baseline, const double true_baseline, + const double focal_x_baseline, const double true_baseline, const double depth_thr, const unsigned int num_grid_cols = 64, const unsigned int num_grid_rows = 48); //! Destructor @@ -114,6 +114,10 @@ class base { //! true baseline length in metric scale const double true_baseline_; + //! depth threshold in metric scale (Ignore depths farther than depth_thr_ times the baseline. + //! if a stereo-triangulated point is farther than this threshold, it is invalid) + const double depth_thr_; + //! number of columns of grid to accelerate reprojection matching const unsigned int num_grid_cols_; //! number of rows of grid to accelerate reprojection matching diff --git a/src/openvslam/camera/equirectangular.cc b/src/openvslam/camera/equirectangular.cc index 1160e19db..f9e3f4a2f 100644 --- a/src/openvslam/camera/equirectangular.cc +++ b/src/openvslam/camera/equirectangular.cc @@ -8,7 +8,7 @@ namespace camera { equirectangular::equirectangular(const std::string& name, const color_order_t& color_order, const unsigned int cols, const unsigned int rows, const double fps) - : base(name, setup_type_t::Monocular, model_type_t::Equirectangular, color_order, cols, rows, fps, 0.0, 0.0) { + : base(name, setup_type_t::Monocular, model_type_t::Equirectangular, color_order, cols, rows, fps, 0.0, 0.0, 0.0) { spdlog::debug("CONSTRUCT: camera::equirectangular"); img_bounds_ = compute_image_bounds(); diff --git a/src/openvslam/camera/fisheye.cc b/src/openvslam/camera/fisheye.cc index 0aae20ce7..690771654 100644 --- a/src/openvslam/camera/fisheye.cc +++ b/src/openvslam/camera/fisheye.cc @@ -12,8 +12,8 @@ fisheye::fisheye(const std::string& name, const setup_type_t& setup_type, const const unsigned int cols, const unsigned int rows, const double fps, const double fx, const double fy, const double cx, const double cy, const double k1, const double k2, const double k3, const double k4, - const double focal_x_baseline) - : base(name, setup_type, model_type_t::Fisheye, color_order, cols, rows, fps, focal_x_baseline, focal_x_baseline / fx), + const double focal_x_baseline, const double depth_thr) + : base(name, setup_type, model_type_t::Fisheye, color_order, cols, rows, fps, focal_x_baseline, focal_x_baseline / fx, depth_thr), fx_(fx), fy_(fy), cx_(cx), cy_(cy), fx_inv_(1.0 / fx), fy_inv_(1.0 / fy), k1_(k1), k2_(k2), k3_(k3), k4_(k4) { spdlog::debug("CONSTRUCT: camera::fisheye"); @@ -45,7 +45,8 @@ fisheye::fisheye(const YAML::Node& yaml_node) yaml_node["k2"].as(), yaml_node["k3"].as(), yaml_node["k4"].as(), - yaml_node["focal_x_baseline"].as(0.0)) {} + yaml_node["focal_x_baseline"].as(0.0), + yaml_node["depth_threshold"].as(40.0)) {} fisheye::~fisheye() { spdlog::debug("DESTRUCT: camera::fisheye"); diff --git a/src/openvslam/camera/fisheye.h b/src/openvslam/camera/fisheye.h index edfe223e9..f3ae30688 100644 --- a/src/openvslam/camera/fisheye.h +++ b/src/openvslam/camera/fisheye.h @@ -16,7 +16,7 @@ class fisheye final : public base { const unsigned int cols, const unsigned int rows, const double fps, const double fx, const double fy, const double cx, const double cy, const double k1, const double k2, const double k3, const double k4, - const double focal_x_baseline = 0.0); + const double focal_x_baseline = 0.0, const double depth_thr = 0.0); fisheye(const YAML::Node& yaml_node); diff --git a/src/openvslam/camera/perspective.cc b/src/openvslam/camera/perspective.cc index b3a78aa0f..fca14b00d 100644 --- a/src/openvslam/camera/perspective.cc +++ b/src/openvslam/camera/perspective.cc @@ -12,8 +12,8 @@ perspective::perspective(const std::string& name, const setup_type_t& setup_type const unsigned int cols, const unsigned int rows, const double fps, const double fx, const double fy, const double cx, const double cy, const double k1, const double k2, const double p1, const double p2, const double k3, - const double focal_x_baseline) - : base(name, setup_type, model_type_t::Perspective, color_order, cols, rows, fps, focal_x_baseline, focal_x_baseline / fx), + const double focal_x_baseline, const double depth_thr) + : base(name, setup_type, model_type_t::Perspective, color_order, cols, rows, fps, focal_x_baseline, focal_x_baseline / fx, depth_thr), fx_(fx), fy_(fy), cx_(cx), cy_(cy), fx_inv_(1.0 / fx), fy_inv_(1.0 / fy), k1_(k1), k2_(k2), p1_(p1), p2_(p2), k3_(k3) { spdlog::debug("CONSTRUCT: camera::perspective"); @@ -46,7 +46,8 @@ perspective::perspective(const YAML::Node& yaml_node) yaml_node["p1"].as(), yaml_node["p2"].as(), yaml_node["k3"].as(), - yaml_node["focal_x_baseline"].as(0.0)) {} + yaml_node["focal_x_baseline"].as(0.0), + yaml_node["depth_threshold"].as(40.0)) {} perspective::~perspective() { spdlog::debug("DESTRUCT: camera::perspective"); diff --git a/src/openvslam/camera/perspective.h b/src/openvslam/camera/perspective.h index 68cefb553..e2d41dcff 100644 --- a/src/openvslam/camera/perspective.h +++ b/src/openvslam/camera/perspective.h @@ -20,7 +20,7 @@ class perspective final : public base { const unsigned int cols, const unsigned int rows, const double fps, const double fx, const double fy, const double cx, const double cy, const double k1, const double k2, const double p1, const double p2, const double k3, - const double focal_x_baseline = 0.0); + const double focal_x_baseline = 0.0, const double depth_thr = 0.0); perspective(const YAML::Node& yaml_node); diff --git a/src/openvslam/camera/radial_division.cc b/src/openvslam/camera/radial_division.cc index 6af6336e3..3be18f93b 100644 --- a/src/openvslam/camera/radial_division.cc +++ b/src/openvslam/camera/radial_division.cc @@ -13,8 +13,8 @@ namespace camera { radial_division::radial_division(const std::string& name, const setup_type_t& setup_type, const color_order_t& color_order, const unsigned int cols, const unsigned int rows, const double fps, const double fx, const double fy, const double cx, const double cy, - const double distortion, const double focal_x_baseline) - : base(name, setup_type, model_type_t::RadialDivision, color_order, cols, rows, fps, focal_x_baseline, focal_x_baseline / fx), + const double distortion, const double focal_x_baseline, const double depth_thr) + : base(name, setup_type, model_type_t::RadialDivision, color_order, cols, rows, fps, focal_x_baseline, focal_x_baseline / fx, depth_thr), fx_(fx), fy_(fy), cx_(cx), cy_(cy), fx_inv_(1.0 / fx), fy_inv_(1.0 / fy), distortion_(distortion) { spdlog::debug("CONSTRUCT: camera::radial_division"); @@ -41,7 +41,8 @@ radial_division::radial_division(const YAML::Node& yaml_node) yaml_node["cx"].as(), yaml_node["cy"].as(), yaml_node["distortion"].as(), - yaml_node["focal_x_baseline"].as(0.0)) {} + yaml_node["focal_x_baseline"].as(0.0), + yaml_node["depth_threshold"].as(40.0)) {} radial_division::~radial_division() { spdlog::debug("DESTRUCT: camera::radial_division"); diff --git a/src/openvslam/camera/radial_division.h b/src/openvslam/camera/radial_division.h index 8fd57ceae..a2e8d9539 100644 --- a/src/openvslam/camera/radial_division.h +++ b/src/openvslam/camera/radial_division.h @@ -19,7 +19,7 @@ class radial_division final : public base { radial_division(const std::string& name, const setup_type_t& setup_type, const color_order_t& color_order, const unsigned int cols, const unsigned int rows, const double fps, const double fx, const double fy, const double cx, const double cy, - const double distortion, const double focal_x_baseline = 0.0); + const double distortion, const double focal_x_baseline = 0.0, const double depth_thr = 0.0); radial_division(const YAML::Node& yaml_node); diff --git a/src/openvslam/config.cc b/src/openvslam/config.cc index d4a0eefe1..c9f10b3fd 100644 --- a/src/openvslam/config.cc +++ b/src/openvslam/config.cc @@ -4,6 +4,7 @@ #include "openvslam/camera/equirectangular.h" #include "openvslam/camera/radial_division.h" #include "openvslam/util/string.h" +#include "openvslam/util/yaml.h" #include #include @@ -51,8 +52,10 @@ config::config(const YAML::Node& yaml_node, const std::string& config_file_path) } catch (const std::exception& e) { spdlog::debug("failed in loading camera model parameters: {}", e.what()); - delete camera_; - camera_ = nullptr; + if (camera_) { + delete camera_; + camera_ = nullptr; + } throw; } @@ -61,12 +64,28 @@ config::config(const YAML::Node& yaml_node, const std::string& config_file_path) throw std::runtime_error("Not implemented: Stereo or RGBD of equirectangular camera model"); } } + + spdlog::debug("load ORB parameters"); + try { + orb_params_ = new feature::orb_params(util::yaml_optional_ref(yaml_node_, "Feature")); + } + catch (const std::exception& e) { + spdlog::debug("failed in loading ORB feature extraction model: {}", e.what()); + if (orb_params_) { + delete orb_params_; + orb_params_ = nullptr; + } + throw; + } } config::~config() { delete camera_; camera_ = nullptr; + delete orb_params_; + orb_params_ = nullptr; + spdlog::debug("DESTRUCT: config"); } diff --git a/src/openvslam/config.h b/src/openvslam/config.h index 0bd8f1b7b..f5a25cf43 100644 --- a/src/openvslam/config.h +++ b/src/openvslam/config.h @@ -27,6 +27,9 @@ class config { //! Camera model camera::base* camera_ = nullptr; + + //! ORB feature extraction model + feature::orb_params* orb_params_ = nullptr; }; } // namespace openvslam diff --git a/src/openvslam/data/CMakeLists.txt b/src/openvslam/data/CMakeLists.txt index 1c85c055b..8cb5ea4d0 100644 --- a/src/openvslam/data/CMakeLists.txt +++ b/src/openvslam/data/CMakeLists.txt @@ -4,19 +4,23 @@ target_sources(${PROJECT_NAME} ${CMAKE_CURRENT_SOURCE_DIR}/bow_vocabulary.h ${CMAKE_CURRENT_SOURCE_DIR}/common.h ${CMAKE_CURRENT_SOURCE_DIR}/frame.h + ${CMAKE_CURRENT_SOURCE_DIR}/frame_observation.h ${CMAKE_CURRENT_SOURCE_DIR}/keyframe.h ${CMAKE_CURRENT_SOURCE_DIR}/landmark.h ${CMAKE_CURRENT_SOURCE_DIR}/graph_node.h ${CMAKE_CURRENT_SOURCE_DIR}/camera_database.h + ${CMAKE_CURRENT_SOURCE_DIR}/orb_params_database.h ${CMAKE_CURRENT_SOURCE_DIR}/map_database.h ${CMAKE_CURRENT_SOURCE_DIR}/bow_database.h ${CMAKE_CURRENT_SOURCE_DIR}/frame_statistics.h + ${CMAKE_CURRENT_SOURCE_DIR}/bow_vocabulary.cc ${CMAKE_CURRENT_SOURCE_DIR}/common.cc ${CMAKE_CURRENT_SOURCE_DIR}/frame.cc ${CMAKE_CURRENT_SOURCE_DIR}/keyframe.cc ${CMAKE_CURRENT_SOURCE_DIR}/landmark.cc ${CMAKE_CURRENT_SOURCE_DIR}/graph_node.cc ${CMAKE_CURRENT_SOURCE_DIR}/camera_database.cc + ${CMAKE_CURRENT_SOURCE_DIR}/orb_params_database.cc ${CMAKE_CURRENT_SOURCE_DIR}/map_database.cc ${CMAKE_CURRENT_SOURCE_DIR}/bow_database.cc ${CMAKE_CURRENT_SOURCE_DIR}/frame_statistics.cc) diff --git a/src/openvslam/data/bow_vocabulary.cc b/src/openvslam/data/bow_vocabulary.cc new file mode 100644 index 000000000..ebc66fbe4 --- /dev/null +++ b/src/openvslam/data/bow_vocabulary.cc @@ -0,0 +1,17 @@ +#include "openvslam/data/bow_vocabulary.h" + +namespace openvslam { +namespace data { +namespace bow_vocabulary_util { + +void compute_bow(data::bow_vocabulary* bow_vocab, const cv::Mat& descriptors, data::bow_vector& bow_vec, data::bow_feature_vector& bow_feat_vec) { +#ifdef USE_DBOW2 + bow_vocab->transform(util::converter::to_desc_vec(descriptors), bow_vec, bow_feat_vec, 4); +#else + bow_vocab->transform(descriptors, 4, bow_vec, bow_feat_vec); +#endif +} + +}; // namespace bow_vocabulary_util +}; // namespace data +}; // namespace openvslam diff --git a/src/openvslam/data/bow_vocabulary.h b/src/openvslam/data/bow_vocabulary.h index 5b90ca68c..aac067230 100644 --- a/src/openvslam/data/bow_vocabulary.h +++ b/src/openvslam/data/bow_vocabulary.h @@ -10,4 +10,14 @@ #include #endif // USE_DBOW2 +namespace openvslam { +namespace data { +namespace bow_vocabulary_util { + +void compute_bow(data::bow_vocabulary* bow_vocab, const cv::Mat& descriptors, data::bow_vector& bow_vec, data::bow_feature_vector& bow_feat_vec); + +}; // namespace bow_vocabulary_util +}; // namespace data +}; // namespace openvslam + #endif // OPENVSLAM_DATA_BOW_VOCABULARY_H diff --git a/src/openvslam/data/bow_vocabulary_fwd.h b/src/openvslam/data/bow_vocabulary_fwd.h index a245d04dc..14ef18881 100644 --- a/src/openvslam/data/bow_vocabulary_fwd.h +++ b/src/openvslam/data/bow_vocabulary_fwd.h @@ -9,11 +9,15 @@ class FORB; // class FORB::TDescriptor; // can't do forward declaration for inner class. template class TemplatedVocabulary; +class BowVector; +class FeatureVector; } // namespace DBoW2 #else namespace fbow { class Vocabulary; -} +class BoWVector; +class BoWFeatVector; +} // namespace fbow #endif // USE_DBOW2 namespace openvslam { @@ -23,10 +27,14 @@ namespace data { // FORB::TDescriptor is defined as cv::Mat typedef DBoW2::TemplatedVocabulary bow_vocabulary; +typedef DBoW2::BowVector bow_vector; +typedef DBoW2::FeatureVector bow_feature_vector; #else typedef fbow::Vocabulary bow_vocabulary; +typedef fbow::BoWVector bow_vector; +typedef fbow::BoWFeatVector bow_feature_vector; #endif // USE_DBOW2 diff --git a/src/openvslam/data/camera_database.cc b/src/openvslam/data/camera_database.cc index fcfceaef3..a31d204b3 100644 --- a/src/openvslam/data/camera_database.cc +++ b/src/openvslam/data/camera_database.cc @@ -53,7 +53,7 @@ void camera_database::from_json(const nlohmann::json& json_cameras) { const auto& json_camera = json_id_camera.value(); if (camera_name == curr_camera_->name_) { - spdlog::info("load the tracking camera \"{}\" from JSON", camera_name); + spdlog::info("skip the tracking camera \"{}\"", camera_name); continue; } diff --git a/src/openvslam/data/frame.cc b/src/openvslam/data/frame.cc index eb042f435..179dba8e9 100644 --- a/src/openvslam/data/frame.cc +++ b/src/openvslam/data/frame.cc @@ -18,112 +18,12 @@ namespace data { std::atomic frame::next_id_{0}; -frame::frame(const cv::Mat& img_gray, const double timestamp, - feature::orb_extractor* extractor, bow_vocabulary* bow_vocab, - camera::base* camera, const float depth_thr, - const cv::Mat& mask) - : id_(next_id_++), bow_vocab_(bow_vocab), extractor_(extractor), extractor_right_(nullptr), - timestamp_(timestamp), camera_(camera), depth_thr_(depth_thr) { - // Get ORB scale - update_orb_info(); - - // Extract ORB feature - extract_orb(img_gray, mask); - num_keypts_ = keypts_.size(); - if (keypts_.empty()) { - spdlog::warn("frame {}: cannot extract any keypoints", id_); - } - - // Undistort keypoints - camera_->undistort_keypoints(keypts_, undist_keypts_); - - // Ignore stereo parameters - stereo_x_right_ = std::vector(num_keypts_, -1); - depths_ = std::vector(num_keypts_, -1); - - // Convert to bearing vector - camera->convert_keypoints_to_bearings(undist_keypts_, bearings_); - - // Initialize association with 3D points - landmarks_ = std::vector>(num_keypts_, nullptr); - outlier_flags_ = std::vector(num_keypts_, false); - - // Assign all the keypoints into grid - assign_keypoints_to_grid(camera_, undist_keypts_, keypt_indices_in_cells_); -} - -frame::frame(const cv::Mat& left_img_gray, const cv::Mat& right_img_gray, const double timestamp, - feature::orb_extractor* extractor_left, feature::orb_extractor* extractor_right, - bow_vocabulary* bow_vocab, camera::base* camera, const float depth_thr, - const cv::Mat& mask) - : id_(next_id_++), bow_vocab_(bow_vocab), extractor_(extractor_left), extractor_right_(extractor_right), - timestamp_(timestamp), camera_(camera), depth_thr_(depth_thr) { - // Get ORB scale - update_orb_info(); - - // Extract ORB feature - std::thread thread_left(&frame::extract_orb, this, left_img_gray, mask, image_side::Left); - std::thread thread_right(&frame::extract_orb, this, right_img_gray, mask, image_side::Right); - thread_left.join(); - thread_right.join(); - num_keypts_ = keypts_.size(); - if (keypts_.empty()) { - spdlog::warn("frame {}: cannot extract any keypoints", id_); - } - - // Undistort keypoints - camera_->undistort_keypoints(keypts_, undist_keypts_); - - // Estimate depth with stereo match - match::stereo stereo_matcher(extractor_left->image_pyramid_, extractor_right_->image_pyramid_, - keypts_, keypts_right_, descriptors_, descriptors_right_, - scale_factors_, inv_scale_factors_, - camera->focal_x_baseline_, camera_->true_baseline_); - stereo_matcher.compute(stereo_x_right_, depths_); - - // Convert to bearing vector - camera->convert_keypoints_to_bearings(undist_keypts_, bearings_); - - // Initialize association with 3D points - landmarks_ = std::vector>(num_keypts_, nullptr); - outlier_flags_ = std::vector(num_keypts_, false); - - // Assign all the keypoints into grid - assign_keypoints_to_grid(camera_, undist_keypts_, keypt_indices_in_cells_); -} - -frame::frame(const cv::Mat& img_gray, const cv::Mat& img_depth, const double timestamp, - feature::orb_extractor* extractor, bow_vocabulary* bow_vocab, - camera::base* camera, const float depth_thr, - const cv::Mat& mask) - : id_(next_id_++), bow_vocab_(bow_vocab), extractor_(extractor), extractor_right_(nullptr), - timestamp_(timestamp), camera_(camera), depth_thr_(depth_thr) { - // Get ORB scale - update_orb_info(); - - // Extract ORB feature - extract_orb(img_gray, mask); - num_keypts_ = keypts_.size(); - if (keypts_.empty()) { - spdlog::warn("frame {}: cannot extract any keypoints", id_); - } - - // Undistort keypoints - camera_->undistort_keypoints(keypts_, undist_keypts_); - - // Calculate disparity from depth - compute_stereo_from_depth(img_depth); - - // Convert to bearing vector - camera->convert_keypoints_to_bearings(undist_keypts_, bearings_); - - // Initialize association with 3D points - landmarks_ = std::vector>(num_keypts_, nullptr); - outlier_flags_ = std::vector(num_keypts_, false); - - // Assign all the keypoints into grid - assign_keypoints_to_grid(camera_, undist_keypts_, keypt_indices_in_cells_); -} +frame::frame(const double timestamp, camera::base* camera, feature::orb_params* orb_params, + const frame_observation frm_obs) + : id_(next_id_++), timestamp_(timestamp), camera_(camera), orb_params_(orb_params), frm_obs_(frm_obs), + // Initialize association with 3D points + landmarks_(std::vector>(frm_obs_.num_keypts_, nullptr)), + outlier_flags_(std::vector(frm_obs_.num_keypts_, false)) {} void frame::set_cam_pose(const Mat44_t& cam_pose_cw) { cam_pose_cw_is_valid_ = true; @@ -161,24 +61,12 @@ Mat33_t frame::get_rotation_inv() const { return rot_wc_; } -void frame::update_orb_info() { - num_scale_levels_ = extractor_->get_num_scale_levels(); - scale_factor_ = extractor_->get_scale_factor(); - log_scale_factor_ = std::log(scale_factor_); - scale_factors_ = extractor_->get_scale_factors(); - inv_scale_factors_ = extractor_->get_inv_scale_factors(); - level_sigma_sq_ = extractor_->get_level_sigma_sq(); - inv_level_sigma_sq_ = extractor_->get_inv_level_sigma_sq(); +bool frame::bow_is_available() const { + return !bow_vec_.empty() && !bow_feat_vec_.empty(); } -void frame::compute_bow() { - if (bow_vec_.empty()) { -#ifdef USE_DBOW2 - bow_vocab_->transform(util::converter::to_desc_vec(descriptors_), bow_vec_, bow_feat_vec_, 4); -#else - bow_vocab_->transform(descriptors_, 4, bow_vec_, bow_feat_vec_); -#endif - } +void frame::compute_bow(bow_vocabulary* bow_vocab) { + bow_vocabulary_util::compute_bow(bow_vocab, frm_obs_.descriptors_, bow_vec_, bow_feat_vec_); } bool frame::can_observe(const std::shared_ptr& lm, const float ray_cos_thr, @@ -202,12 +90,12 @@ bool frame::can_observe(const std::shared_ptr& lm, const float ray_cos return false; } - pred_scale_level = lm->predict_scale_level(cam_to_lm_dist, this); + pred_scale_level = lm->predict_scale_level(cam_to_lm_dist, this->orb_params_->num_levels_, this->orb_params_->log_scale_factor_); return true; } std::vector frame::get_keypoints_in_cell(const float ref_x, const float ref_y, const float margin, const int min_level, const int max_level) const { - return data::get_keypoints_in_cell(camera_, undist_keypts_, keypt_indices_in_cells_, ref_x, ref_y, margin, min_level, max_level); + return data::get_keypoints_in_cell(camera_, frm_obs_.undist_keypts_, frm_obs_.keypt_indices_in_cells_, ref_x, ref_y, margin, min_level, max_level); } Vec3_t frame::triangulate_stereo(const unsigned int idx) const { @@ -217,10 +105,10 @@ Vec3_t frame::triangulate_stereo(const unsigned int idx) const { case camera::model_type_t::Perspective: { auto camera = static_cast(camera_); - const float depth = depths_.at(idx); + const float depth = frm_obs_.depths_.at(idx); if (0.0 < depth) { - const float x = undist_keypts_.at(idx).pt.x; - const float y = undist_keypts_.at(idx).pt.y; + const float x = frm_obs_.undist_keypts_.at(idx).pt.x; + const float y = frm_obs_.undist_keypts_.at(idx).pt.y; const float unproj_x = (x - camera->cx_) * depth * camera->fx_inv_; const float unproj_y = (y - camera->cy_) * depth * camera->fy_inv_; const Vec3_t pos_c{unproj_x, unproj_y, depth}; @@ -235,10 +123,10 @@ Vec3_t frame::triangulate_stereo(const unsigned int idx) const { case camera::model_type_t::Fisheye: { auto camera = static_cast(camera_); - const float depth = depths_.at(idx); + const float depth = frm_obs_.depths_.at(idx); if (0.0 < depth) { - const float x = undist_keypts_.at(idx).pt.x; - const float y = undist_keypts_.at(idx).pt.y; + const float x = frm_obs_.undist_keypts_.at(idx).pt.x; + const float y = frm_obs_.undist_keypts_.at(idx).pt.y; const float unproj_x = (x - camera->cx_) * depth * camera->fx_inv_; const float unproj_y = (y - camera->cy_) * depth * camera->fy_inv_; const Vec3_t pos_c{unproj_x, unproj_y, depth}; @@ -256,10 +144,10 @@ Vec3_t frame::triangulate_stereo(const unsigned int idx) const { case camera::model_type_t::RadialDivision: { auto camera = static_cast(camera_); - const float depth = depths_.at(idx); + const float depth = frm_obs_.depths_.at(idx); if (0.0 < depth) { - const float x = keypts_.at(idx).pt.x; - const float y = keypts_.at(idx).pt.y; + const float x = frm_obs_.keypts_.at(idx).pt.x; + const float y = frm_obs_.keypts_.at(idx).pt.y; const float unproj_x = (x - camera->cx_) * depth * camera->fx_inv_; const float unproj_y = (y - camera->cy_) * depth * camera->fy_inv_; const Vec3_t pos_c{unproj_x, unproj_y, depth}; @@ -276,43 +164,5 @@ Vec3_t frame::triangulate_stereo(const unsigned int idx) const { return Vec3_t::Zero(); } -void frame::extract_orb(const cv::Mat& img, const cv::Mat& mask, const image_side& img_side) { - switch (img_side) { - case image_side::Left: { - extractor_->extract(img, mask, keypts_, descriptors_); - break; - } - case image_side::Right: { - extractor_right_->extract(img, mask, keypts_right_, descriptors_right_); - break; - } - } -} - -void frame::compute_stereo_from_depth(const cv::Mat& img_depth) { - assert(camera_->setup_type_ == camera::setup_type_t::RGBD); - - // Initialize with invalid value - stereo_x_right_ = std::vector(num_keypts_, -1); - depths_ = std::vector(num_keypts_, -1); - - for (unsigned int idx = 0; idx < num_keypts_; idx++) { - const auto& keypt = keypts_.at(idx); - const auto& undist_keypt = undist_keypts_.at(idx); - - const float x = keypt.pt.x; - const float y = keypt.pt.y; - - const float depth = img_depth.at(y, x); - - if (depth <= 0) { - continue; - } - - depths_.at(idx) = depth; - stereo_x_right_.at(idx) = undist_keypt.pt.x - camera_->focal_x_baseline_ / depth; - } -} - } // namespace data } // namespace openvslam diff --git a/src/openvslam/data/frame.h b/src/openvslam/data/frame.h index e6fa9bf17..f6933729c 100644 --- a/src/openvslam/data/frame.h +++ b/src/openvslam/data/frame.h @@ -3,7 +3,9 @@ #include "openvslam/type.h" #include "openvslam/camera/base.h" +#include "openvslam/feature/orb_params.h" #include "openvslam/util/converter.h" +#include "openvslam/data/frame_observation.h" #include "openvslam/data/bow_vocabulary.h" #include @@ -29,6 +31,7 @@ class base; namespace feature { class orb_extractor; +class orb_params; } // namespace feature namespace data { @@ -47,51 +50,13 @@ class frame { /** * Constructor for monocular frame - * @param img_gray * @param timestamp - * @param extractor - * @param bow_vocab * @param camera - * @param depth_thr - * @param mask + * @param orb_params + * @param frm_obs */ - frame(const cv::Mat& img_gray, const double timestamp, - feature::orb_extractor* extractor, bow_vocabulary* bow_vocab, - camera::base* camera, const float depth_thr, - const cv::Mat& mask = cv::Mat{}); - - /** - * Constructor for stereo frame - * @param left_img_gray - * @param right_img_gray - * @param timestamp - * @param extractor_left - * @param extractor_right - * @param bow_vocab - * @param camera - * @param depth_thr - * @param mask - */ - frame(const cv::Mat& left_img_gray, const cv::Mat& right_img_gray, const double timestamp, - feature::orb_extractor* extractor_left, feature::orb_extractor* extractor_right, bow_vocabulary* bow_vocab, - camera::base* camera, const float depth_thr, - const cv::Mat& mask = cv::Mat{}); - - /** - * Constructor for RGBD frame - * @param img_gray - * @param img_depth - * @param timestamp - * @param extractor - * @param bow_vocab - * @param camera - * @param depth_thr - * @param mask - */ - frame(const cv::Mat& img_gray, const cv::Mat& img_depth, const double timestamp, - feature::orb_extractor* extractor, bow_vocabulary* bow_vocab, - camera::base* camera, const float depth_thr, - const cv::Mat& mask = cv::Mat{}); + frame(const double timestamp, camera::base* camera, feature::orb_params* orb_params, + const frame_observation frm_obs); /** * Set camera pose and refresh rotation and translation @@ -133,14 +98,14 @@ class frame { Mat33_t get_rotation_inv() const; /** - * Update ORB information + * Returns true if BoW has been computed. */ - void update_orb_info(); + bool bow_is_available() const; /** * Compute BoW representation */ - void compute_bow(); + void compute_bow(bow_vocabulary* bow_vocab); /** * Check observability of the landmark @@ -172,42 +137,17 @@ class frame { //! next frame ID static std::atomic next_id_; - //! BoW vocabulary (DBoW2 or FBoW) - bow_vocabulary* bow_vocab_ = nullptr; - - // ORB extractor - //! ORB extractor for monocular or stereo left image - feature::orb_extractor* extractor_ = nullptr; - //! ORB extractor for stereo right image - feature::orb_extractor* extractor_right_ = nullptr; - //! timestamp double timestamp_; //! camera model camera::base* camera_ = nullptr; - // if a stereo-triangulated point is farther than this threshold, it is invalid - //! depth threshold - float depth_thr_; + //! ORB scale pyramid information + const feature::orb_params* orb_params_ = nullptr; - //! number of keypoints - unsigned int num_keypts_ = 0; - - // keypoints - //! keypoints of monocular or stereo left image - std::vector keypts_; - //! keypoints of stereo right image - std::vector keypts_right_; - //! undistorted keypoints of monocular or stereo left image - std::vector undist_keypts_; - //! bearing vectors - eigen_alloc_vector bearings_; - - //! disparities - std::vector stereo_x_right_; - //! depths - std::vector depths_; + //! constant observations + frame_observation frm_obs_; //! BoW features (DBoW2 or FBoW) #ifdef USE_DBOW2 @@ -218,21 +158,12 @@ class frame { fbow::BoWFeatVector bow_feat_vec_; #endif - // ORB descriptors - //! ORB descriptors of monocular or stereo left image - cv::Mat descriptors_; - //! ORB descriptors of stereo right image - cv::Mat descriptors_right_; - //! landmarks, whose nullptr indicates no-association std::vector> landmarks_; //! outlier flags, which are mainly used in pose optimization and bundle adjustment std::vector outlier_flags_; - //! cells for storing keypoint indices - std::vector>> keypt_indices_in_cells_; - //! camera pose: world -> camera bool cam_pose_cw_is_valid_ = false; Mat44_t cam_pose_cw_; @@ -240,41 +171,7 @@ class frame { //! reference keyframe for tracking std::shared_ptr ref_keyfrm_ = nullptr; - // ORB scale pyramid information - //! number of scale levels - unsigned int num_scale_levels_; - //! scale factor - float scale_factor_; - //! log scale factor - float log_scale_factor_; - //! list of scale factors - std::vector scale_factors_; - //! list of inverse of scale factors - std::vector inv_scale_factors_; - //! list of sigma^2 (sigma=1.0 at scale=0) for optimization - std::vector level_sigma_sq_; - //! list of 1 / sigma^2 for optimization - std::vector inv_level_sigma_sq_; - private: - //! enumeration to control the behavior of extract_orb() - enum class image_side { Left, - Right }; - - /** - * Extract ORB feature according to img_size - * @param img - * @param mask - * @param img_side - */ - void extract_orb(const cv::Mat& img, const cv::Mat& mask, const image_side& img_side = image_side::Left); - - /** - * Compute disparities from depth information in depthmap - * @param right_img_depth - */ - void compute_stereo_from_depth(const cv::Mat& right_img_depth); - //! Camera pose //! rotation: world -> camera Mat33_t rot_cw_; diff --git a/src/openvslam/data/frame_observation.h b/src/openvslam/data/frame_observation.h new file mode 100644 index 000000000..bd782066b --- /dev/null +++ b/src/openvslam/data/frame_observation.h @@ -0,0 +1,41 @@ +#ifndef OPENVSLAM_DATA_FRAME_OBSERVATION_H +#define OPENVSLAM_DATA_FRAME_OBSERVATION_H + +#include "openvslam/type.h" + +namespace openvslam { +namespace data { + +struct frame_observation { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + frame_observation() = default; + frame_observation(unsigned int num_keypts, const std::vector& keypts, const cv::Mat& descriptors, + const std::vector& undist_keypts, const eigen_alloc_vector& bearings, + const std::vector& stereo_x_right, const std::vector& depths, + const std::vector>>& keypt_indices_in_cells) + : num_keypts_(num_keypts), keypts_(keypts), descriptors_(descriptors), undist_keypts_(undist_keypts), bearings_(bearings), + stereo_x_right_(stereo_x_right), depths_(depths), keypt_indices_in_cells_(keypt_indices_in_cells) {} + + //! number of keypoints + unsigned int num_keypts_ = 0; + //! keypoints of monocular or stereo left image + std::vector keypts_; + //! descriptors + cv::Mat descriptors_; + //! undistorted keypoints of monocular or stereo left image + std::vector undist_keypts_; + //! bearing vectors + eigen_alloc_vector bearings_; + //! disparities + std::vector stereo_x_right_; + //! depths + std::vector depths_; + //! keypoint indices in each of the cells + std::vector>> keypt_indices_in_cells_; +}; + +} // namespace data +} // namespace openvslam + +#endif // OPENVSLAM_DATA_FRAME_OBSERVATION_H diff --git a/src/openvslam/data/graph_node.cc b/src/openvslam/data/graph_node.cc index d397df565..a443ea76b 100644 --- a/src/openvslam/data/graph_node.cc +++ b/src/openvslam/data/graph_node.cc @@ -9,38 +9,34 @@ graph_node::graph_node(std::shared_ptr& keyfrm, const bool spanning_pa : owner_keyfrm_(keyfrm), spanning_parent_is_not_set_(spanning_parent_is_not_set) {} void graph_node::add_connection(const std::shared_ptr& keyfrm, const unsigned int weight) { + std::lock_guard lock(mtx_); bool need_update = false; - { - std::lock_guard lock(mtx_); - if (!connected_keyfrms_and_weights_.count(keyfrm)) { - // if `keyfrm` not exists - connected_keyfrms_and_weights_[keyfrm] = weight; - need_update = true; - } - else if (connected_keyfrms_and_weights_.at(keyfrm) != weight) { - // if the weight is updated - connected_keyfrms_and_weights_.at(keyfrm) = weight; - need_update = true; - } + if (!connected_keyfrms_and_weights_.count(keyfrm)) { + // if `keyfrm` not exists + connected_keyfrms_and_weights_[keyfrm] = weight; + need_update = true; + } + else if (connected_keyfrms_and_weights_.at(keyfrm) != weight) { + // if the weight is updated + connected_keyfrms_and_weights_.at(keyfrm) = weight; + need_update = true; } if (need_update) { - update_covisibility_orders(); + update_covisibility_orders_impl(); } } void graph_node::erase_connection(const std::shared_ptr& keyfrm) { + std::lock_guard lock(mtx_); bool need_update = false; - { - std::lock_guard lock(mtx_); - if (connected_keyfrms_and_weights_.count(keyfrm)) { - connected_keyfrms_and_weights_.erase(keyfrm); - need_update = true; - } + if (connected_keyfrms_and_weights_.count(keyfrm)) { + connected_keyfrms_and_weights_.erase(keyfrm); + need_update = true; } if (need_update) { - update_covisibility_orders(); + update_covisibility_orders_impl(); } } @@ -149,7 +145,10 @@ void graph_node::update_connections() { void graph_node::update_covisibility_orders() { std::lock_guard lock(mtx_); + update_covisibility_orders_impl(); +} +void graph_node::update_covisibility_orders_impl() { std::vector>> weight_keyfrm_pairs; weight_keyfrm_pairs.reserve(connected_keyfrms_and_weights_.size()); diff --git a/src/openvslam/data/graph_node.h b/src/openvslam/data/graph_node.h index 9dbf3775c..0c19846d4 100644 --- a/src/openvslam/data/graph_node.h +++ b/src/openvslam/data/graph_node.h @@ -141,6 +141,12 @@ class graph_node { bool has_loop_edge() const; private: + /** + * Update the order of the covisibilities (without mutex) + * (NOTE: the new keyframe won't inserted) + */ + void update_covisibility_orders_impl(); + /** * Extract intersection from the two lists of keyframes */ diff --git a/src/openvslam/data/keyframe.cc b/src/openvslam/data/keyframe.cc index 0c1c5818b..973c22b5b 100644 --- a/src/openvslam/data/keyframe.cc +++ b/src/openvslam/data/keyframe.cc @@ -18,55 +18,24 @@ namespace data { std::atomic keyframe::next_id_{0}; -keyframe::keyframe(const frame& frm, map_database* map_db, bow_database* bow_db) - : // meta information - id_(next_id_++), src_frm_id_(frm.id_), timestamp_(frm.timestamp_), - // camera parameters - camera_(frm.camera_), depth_thr_(frm.depth_thr_), - // constant observations - num_keypts_(frm.num_keypts_), keypts_(frm.keypts_), undist_keypts_(frm.undist_keypts_), bearings_(frm.bearings_), - keypt_indices_in_cells_(frm.keypt_indices_in_cells_), - stereo_x_right_(frm.stereo_x_right_), depths_(frm.depths_), descriptors_(frm.descriptors_.clone()), - // BoW - bow_vec_(frm.bow_vec_), bow_feat_vec_(frm.bow_feat_vec_), - // ORB scale pyramid - num_scale_levels_(frm.num_scale_levels_), scale_factor_(frm.scale_factor_), - log_scale_factor_(frm.log_scale_factor_), scale_factors_(frm.scale_factors_), - level_sigma_sq_(frm.level_sigma_sq_), inv_level_sigma_sq_(frm.inv_level_sigma_sq_), - // observations - landmarks_(frm.landmarks_), - // databases - map_db_(map_db), bow_db_(bow_db), bow_vocab_(frm.bow_vocab_) { +keyframe::keyframe(const frame& frm) + : id_(next_id_++), src_frm_id_(frm.id_), timestamp_(frm.timestamp_), + camera_(frm.camera_), orb_params_(frm.orb_params_), + frm_obs_(frm.frm_obs_), bow_vec_(frm.bow_vec_), bow_feat_vec_(frm.bow_feat_vec_), + landmarks_(frm.landmarks_) { // set pose parameters (cam_pose_wc_, cam_center_) using frm.cam_pose_cw_ set_cam_pose(frm.cam_pose_cw_); } keyframe::keyframe(const unsigned int id, const unsigned int src_frm_id, const double timestamp, - const Mat44_t& cam_pose_cw, camera::base* camera, const float depth_thr, - const unsigned int num_keypts, const std::vector& keypts, - const std::vector& undist_keypts, const eigen_alloc_vector& bearings, - const std::vector& stereo_x_right, const std::vector& depths, const cv::Mat& descriptors, - const unsigned int num_scale_levels, const float scale_factor, - bow_vocabulary* bow_vocab, bow_database* bow_db, map_database* map_db) - : // meta information - id_(id), src_frm_id_(src_frm_id), timestamp_(timestamp), - // camera parameters - camera_(camera), depth_thr_(depth_thr), - // constant observations - num_keypts_(num_keypts), keypts_(keypts), undist_keypts_(undist_keypts), bearings_(bearings), - keypt_indices_in_cells_(assign_keypoints_to_grid(camera, undist_keypts)), - stereo_x_right_(stereo_x_right), depths_(depths), descriptors_(descriptors.clone()), - // ORB scale pyramid - num_scale_levels_(num_scale_levels), scale_factor_(scale_factor), log_scale_factor_(std::log(scale_factor)), - scale_factors_(feature::orb_params::calc_scale_factors(num_scale_levels, scale_factor)), - level_sigma_sq_(feature::orb_params::calc_level_sigma_sq(num_scale_levels, scale_factor)), - inv_level_sigma_sq_(feature::orb_params::calc_inv_level_sigma_sq(num_scale_levels, scale_factor)), - // others - landmarks_(std::vector>(num_keypts, nullptr)), - // databases - map_db_(map_db), bow_db_(bow_db), bow_vocab_(bow_vocab) { - // compute BoW (bow_vec_, bow_feat_vec_) using descriptors_ - compute_bow(); + const Mat44_t& cam_pose_cw, camera::base* camera, + const feature::orb_params* orb_params, const frame_observation& frm_obs, + const bow_vector& bow_vec, const bow_feature_vector& bow_feat_vec) + : id_(id), src_frm_id_(src_frm_id), + timestamp_(timestamp), camera_(camera), + orb_params_(orb_params), frm_obs_(frm_obs), + bow_vec_(bow_vec), bow_feat_vec_(bow_feat_vec), + landmarks_(std::vector>(frm_obs_.num_keypts_, nullptr)) { // set pose parameters (cam_pose_wc_, cam_center_) using cam_pose_cw_ set_cam_pose(cam_pose_cw); @@ -80,8 +49,8 @@ keyframe::keyframe(const unsigned int id, const unsigned int src_frm_id, const d keyframe::~keyframe() {} -std::shared_ptr keyframe::make_keyframe(const frame& frm, map_database* map_db, bow_database* bow_db) { - auto ptr = std::allocate_shared(Eigen::aligned_allocator(), frm, map_db, bow_db); +std::shared_ptr keyframe::make_keyframe(const frame& frm) { + auto ptr = std::allocate_shared(Eigen::aligned_allocator(), frm); // covisibility graph node (connections is not assigned yet) ptr->graph_node_ = openvslam::make_unique(ptr, true); return ptr; @@ -89,21 +58,14 @@ std::shared_ptr keyframe::make_keyframe(const frame& frm, map_database std::shared_ptr keyframe::make_keyframe( const unsigned int id, const unsigned int src_frm_id, const double timestamp, - const Mat44_t& cam_pose_cw, camera::base* camera, const float depth_thr, - const unsigned int num_keypts, const std::vector& keypts, - const std::vector& undist_keypts, const eigen_alloc_vector& bearings, - const std::vector& stereo_x_right, const std::vector& depths, const cv::Mat& descriptors, - const unsigned int num_scale_levels, const float scale_factor, - bow_vocabulary* bow_vocab, bow_database* bow_db, map_database* map_db) { + const Mat44_t& cam_pose_cw, camera::base* camera, + const feature::orb_params* orb_params, const frame_observation& frm_obs, + const bow_vector& bow_vec, const bow_feature_vector& bow_feat_vec) { auto ptr = std::allocate_shared( Eigen::aligned_allocator(), id, src_frm_id, timestamp, - cam_pose_cw, camera, depth_thr, - num_keypts, keypts, - undist_keypts, bearings, - stereo_x_right, depths, descriptors, - num_scale_levels, scale_factor, - bow_vocab, bow_db, map_db); + cam_pose_cw, camera, orb_params, + frm_obs, bow_vec, bow_feat_vec); // covisibility graph node (connections is not assigned yet) ptr->graph_node_ = openvslam::make_unique(ptr, false); return ptr; @@ -139,21 +101,18 @@ nlohmann::json keyframe::to_json() const { return {{"src_frm_id", src_frm_id_}, {"ts", timestamp_}, {"cam", camera_->name_}, - {"depth_thr", depth_thr_}, + {"orb_params", orb_params_->name_}, // camera pose {"rot_cw", convert_rotation_to_json(cam_pose_cw_.block<3, 3>(0, 0))}, {"trans_cw", convert_translation_to_json(cam_pose_cw_.block<3, 1>(0, 3))}, // features and observations - {"n_keypts", num_keypts_}, - {"keypts", convert_keypoints_to_json(keypts_)}, - {"undists", convert_undistorted_to_json(undist_keypts_)}, - {"x_rights", stereo_x_right_}, - {"depths", depths_}, - {"descs", convert_descriptors_to_json(descriptors_)}, + {"n_keypts", frm_obs_.num_keypts_}, + {"keypts", convert_keypoints_to_json(frm_obs_.keypts_)}, + {"undists", convert_undistorted_to_json(frm_obs_.undist_keypts_)}, + {"x_rights", frm_obs_.stereo_x_right_}, + {"depths", frm_obs_.depths_}, + {"descs", convert_descriptors_to_json(frm_obs_.descriptors_)}, {"lm_ids", landmark_ids}, - // orb scale information - {"n_scale_levels", num_scale_levels_}, - {"scale_factor", scale_factor_}, // graph information {"span_parent", spanning_parent ? spanning_parent->id_ : -1}, {"span_children", spanning_child_ids}, @@ -203,14 +162,12 @@ Vec3_t keyframe::get_translation() const { return cam_pose_cw_.block<3, 1>(0, 3); } -void keyframe::compute_bow() { - if (bow_vec_.empty() || bow_feat_vec_.empty()) { -#ifdef USE_DBOW2 - bow_vocab_->transform(util::converter::to_desc_vec(descriptors_), bow_vec_, bow_feat_vec_, 4); -#else - bow_vocab_->transform(descriptors_, 4, bow_vec_, bow_feat_vec_); -#endif - } +bool keyframe::bow_is_available() const { + return !bow_vec_.empty() && !bow_feat_vec_.empty(); +} + +void keyframe::compute_bow(bow_vocabulary* bow_vocab) { + bow_vocabulary_util::compute_bow(bow_vocab, frm_obs_.descriptors_, bow_vec_, bow_feat_vec_); } void keyframe::add_landmark(std::shared_ptr lm, const unsigned int idx) { @@ -299,7 +256,7 @@ std::shared_ptr& keyframe::get_landmark(const unsigned int idx) { } std::vector keyframe::get_keypoints_in_cell(const float ref_x, const float ref_y, const float margin) const { - return data::get_keypoints_in_cell(camera_, undist_keypts_, keypt_indices_in_cells_, ref_x, ref_y, margin); + return data::get_keypoints_in_cell(camera_, frm_obs_.undist_keypts_, frm_obs_.keypt_indices_in_cells_, ref_x, ref_y, margin); } Vec3_t keyframe::triangulate_stereo(const unsigned int idx) const { @@ -309,10 +266,10 @@ Vec3_t keyframe::triangulate_stereo(const unsigned int idx) const { case camera::model_type_t::Perspective: { auto camera = static_cast(camera_); - const float depth = depths_.at(idx); + const float depth = frm_obs_.depths_.at(idx); if (0.0 < depth) { - const float x = undist_keypts_.at(idx).pt.x; - const float y = undist_keypts_.at(idx).pt.y; + const float x = frm_obs_.undist_keypts_.at(idx).pt.x; + const float y = frm_obs_.undist_keypts_.at(idx).pt.y; const float unproj_x = (x - camera->cx_) * depth * camera->fx_inv_; const float unproj_y = (y - camera->cy_) * depth * camera->fy_inv_; const Vec3_t pos_c{unproj_x, unproj_y, depth}; @@ -327,10 +284,10 @@ Vec3_t keyframe::triangulate_stereo(const unsigned int idx) const { case camera::model_type_t::Fisheye: { auto camera = static_cast(camera_); - const float depth = depths_.at(idx); + const float depth = frm_obs_.depths_.at(idx); if (0.0 < depth) { - const float x = undist_keypts_.at(idx).pt.x; - const float y = undist_keypts_.at(idx).pt.y; + const float x = frm_obs_.undist_keypts_.at(idx).pt.x; + const float y = frm_obs_.undist_keypts_.at(idx).pt.y; const float unproj_x = (x - camera->cx_) * depth * camera->fx_inv_; const float unproj_y = (y - camera->cy_) * depth * camera->fy_inv_; const Vec3_t pos_c{unproj_x, unproj_y, depth}; @@ -348,10 +305,10 @@ Vec3_t keyframe::triangulate_stereo(const unsigned int idx) const { case camera::model_type_t::RadialDivision: { auto camera = static_cast(camera_); - const float depth = depths_.at(idx); + const float depth = frm_obs_.depths_.at(idx); if (0.0 < depth) { - const float x = keypts_.at(idx).pt.x; - const float y = keypts_.at(idx).pt.y; + const float x = frm_obs_.keypts_.at(idx).pt.x; + const float y = frm_obs_.keypts_.at(idx).pt.y; const float unproj_x = (x - camera->cx_) * depth * camera->fx_inv_; const float unproj_y = (y - camera->cy_) * depth * camera->fy_inv_; const Vec3_t pos_c{unproj_x, unproj_y, depth}; @@ -380,7 +337,7 @@ float keyframe::compute_median_depth(const bool abs) const { } std::vector depths; - depths.reserve(num_keypts_); + depths.reserve(frm_obs_.num_keypts_); const Vec3_t rot_cw_z_row = cam_pose_cw.block<1, 3>(2, 0); const float trans_cw_z = cam_pose_cw(2, 3); @@ -412,9 +369,9 @@ void keyframe::set_to_be_erased() { } } -void keyframe::prepare_for_erasing() { +void keyframe::prepare_for_erasing(map_database* map_db, bow_database* bow_db) { // cannot erase the origin - if (*this == *(map_db_->origin_keyfrm_)) { + if (*this == *(map_db->origin_keyfrm_)) { return; } @@ -435,7 +392,7 @@ void keyframe::prepare_for_erasing() { if (!lm) { continue; } - lm->erase_observation(shared_from_this()); + lm->erase_observation(map_db, shared_from_this()); } } @@ -448,12 +405,12 @@ void keyframe::prepare_for_erasing() { // 3. update frame statistics - map_db_->replace_reference_keyframe(shared_from_this(), graph_node_->get_spanning_parent()); + map_db->replace_reference_keyframe(shared_from_this(), graph_node_->get_spanning_parent()); // 4. remove myself from the databased - map_db_->erase_keyframe(shared_from_this()); - bow_db_->erase_keyframe(shared_from_this()); + map_db->erase_keyframe(shared_from_this()); + bow_db->erase_keyframe(shared_from_this()); } bool keyframe::will_be_erased() { diff --git a/src/openvslam/data/keyframe.h b/src/openvslam/data/keyframe.h index cad432312..36d83c01f 100644 --- a/src/openvslam/data/keyframe.h +++ b/src/openvslam/data/keyframe.h @@ -3,8 +3,10 @@ #include "openvslam/type.h" #include "openvslam/camera/base.h" +#include "openvslam/feature/orb_params.h" #include "openvslam/data/graph_node.h" #include "openvslam/data/bow_vocabulary.h" +#include "openvslam/data/frame_observation.h" #include #include @@ -42,31 +44,25 @@ class keyframe : public std::enable_shared_from_this { /** * Constructor for building from a frame */ - keyframe(const frame& frm, map_database* map_db, bow_database* bow_db); + explicit keyframe(const frame& frm); /** * Constructor for map loading * (NOTE: some variables must be recomputed after the construction. See the definition.) */ - keyframe(const unsigned int id, const unsigned int src_frm_id, const double timestamp, - const Mat44_t& cam_pose_cw, camera::base* camera, const float depth_thr, - const unsigned int num_keypts, const std::vector& keypts, - const std::vector& undist_keypts, const eigen_alloc_vector& bearings, - const std::vector& stereo_x_right, const std::vector& depths, const cv::Mat& descriptors, - const unsigned int num_scale_levels, const float scale_factor, - bow_vocabulary* bow_vocab, bow_database* bow_db, map_database* map_db); + keyframe(const unsigned int id, const unsigned int src_frm_id, + const double timestamp, const Mat44_t& cam_pose_cw, camera::base* camera, + const feature::orb_params* orb_params, const frame_observation& frm_obs, + const bow_vector& bow_vec, const bow_feature_vector& bow_feat_vec); virtual ~keyframe(); // Factory method for create keyframe - static std::shared_ptr make_keyframe(const frame& frm, map_database* map_db, bow_database* bow_db); + static std::shared_ptr make_keyframe(const frame& frm); static std::shared_ptr make_keyframe( - const unsigned int id, const unsigned int src_frm_id, const double timestamp, - const Mat44_t& cam_pose_cw, camera::base* camera, const float depth_thr, - const unsigned int num_keypts, const std::vector& keypts, - const std::vector& undist_keypts, const eigen_alloc_vector& bearings, - const std::vector& stereo_x_right, const std::vector& depths, const cv::Mat& descriptors, - const unsigned int num_scale_levels, const float scale_factor, - bow_vocabulary* bow_vocab, bow_database* bow_db, map_database* map_db); + const unsigned int id, const unsigned int src_frm_id, + const double timestamp, const Mat44_t& cam_pose_cw, camera::base* camera, + const feature::orb_params* orb_params, const frame_observation& frm_obs, + const bow_vector& bow_vec, const bow_feature_vector& bow_feat_vec); // operator overrides bool operator==(const keyframe& keyfrm) const { return id_ == keyfrm.id_; } @@ -122,10 +118,15 @@ class keyframe : public std::enable_shared_from_this { //----------------------------------------- // features and observations + /** + * Returns true if BoW has been computed. + */ + bool bow_is_available() const; + /** * Compute BoW representation */ - void compute_bow(); + void compute_bow(bow_vocabulary* bow_vocab); /** * Add a landmark observed by myself at keypoint idx @@ -204,29 +205,13 @@ class keyframe : public std::enable_shared_from_this { /** * Erase this keyframe */ - void prepare_for_erasing(); + void prepare_for_erasing(map_database* map_db, bow_database* bow_db); /** * Whether this keyframe will be erased shortly or not */ bool will_be_erased(); - //----------------------------------------- - // for local map update - - //! identifier for local map update - unsigned int local_map_update_identifier = 0; - - //----------------------------------------- - // for loop BA - - //! identifier for loop BA - unsigned int loop_BA_identifier_ = 0; - //! camera pose AFTER loop BA - Mat44_t cam_pose_cw_after_loop_BA_; - //! camera pose BEFORE loop BA - Mat44_t cam_pose_cw_before_BA_; - //----------------------------------------- // meta information @@ -246,32 +231,17 @@ class keyframe : public std::enable_shared_from_this { //! camera model camera::base* camera_; - //! depth threshold - const float depth_thr_; //----------------------------------------- - // constant observations - - //! number of keypoints - const unsigned int num_keypts_; - - //! keypoints of monocular or stereo left image - const std::vector keypts_; - //! undistorted keypoints of monocular or stereo left image - const std::vector undist_keypts_; - //! bearing vectors - const eigen_alloc_vector bearings_; + // feature extraction parameters - //! keypoint indices in each of the cells - const std::vector>> keypt_indices_in_cells_; + //! ORB feature extraction model + const feature::orb_params* orb_params_; - //! disparities - const std::vector stereo_x_right_; - //! depths - const std::vector depths_; + //----------------------------------------- + // constant observations - //! descriptors - const cv::Mat descriptors_; + const frame_observation frm_obs_; //! BoW features (DBoW2 or FBoW) #ifdef USE_DBOW2 @@ -288,22 +258,6 @@ class keyframe : public std::enable_shared_from_this { //! graph node std::unique_ptr graph_node_ = nullptr; - //----------------------------------------- - // ORB scale pyramid information - - //! number of scale levels - const unsigned int num_scale_levels_; - //! scale factor - const float scale_factor_; - //! log scale factor - const float log_scale_factor_; - //! list of scale factors - const std::vector scale_factors_; - //! list of sigma^2 (sigma=1.0 at scale=0) for optimization - const std::vector level_sigma_sq_; - //! list of 1 / sigma^2 for optimization - const std::vector inv_level_sigma_sq_; - private: //----------------------------------------- // camera pose @@ -325,16 +279,6 @@ class keyframe : public std::enable_shared_from_this { //! observed landmarks std::vector> landmarks_; - //----------------------------------------- - // databases - - //! map database - map_database* map_db_; - //! BoW database - bow_database* bow_db_; - //! BoW vocabulary - bow_vocabulary* bow_vocab_; - //----------------------------------------- // flags diff --git a/src/openvslam/data/landmark.cc b/src/openvslam/data/landmark.cc index de853e2d3..d6cd653b1 100644 --- a/src/openvslam/data/landmark.cc +++ b/src/openvslam/data/landmark.cc @@ -49,7 +49,7 @@ void landmark::add_observation(const std::shared_ptr& keyfrm, unsigned } observations_[keyfrm] = idx; - if (0 <= keyfrm->stereo_x_right_.at(idx)) { + if (0 <= keyfrm->frm_obs_.stereo_x_right_.at(idx)) { num_observations_ += 2; } else { @@ -57,14 +57,14 @@ void landmark::add_observation(const std::shared_ptr& keyfrm, unsigned } } -void landmark::erase_observation(const std::shared_ptr& keyfrm) { +void landmark::erase_observation(map_database* map_db, const std::shared_ptr& keyfrm) { bool discard = false; { std::lock_guard lock(mtx_observations_); if (observations_.count(keyfrm)) { int idx = observations_.at(keyfrm); - if (0 <= keyfrm->stereo_x_right_.at(idx)) { + if (0 <= keyfrm->frm_obs_.stereo_x_right_.at(idx)) { num_observations_ -= 2; } else { @@ -77,7 +77,6 @@ void landmark::erase_observation(const std::shared_ptr& keyfrm) { ref_keyfrm_ = observations_.begin()->first; } - // If only 2 observations or less, discard point if (num_observations_ <= 2) { discard = true; } @@ -85,7 +84,7 @@ void landmark::erase_observation(const std::shared_ptr& keyfrm) { } if (discard) { - prepare_for_erasing(); + prepare_for_erasing(map_db); } } @@ -146,7 +145,7 @@ void landmark::compute_descriptor() { const auto idx = observation.second; if (!keyfrm->will_be_erased()) { - descriptors.push_back(keyfrm->descriptors_.row(idx)); + descriptors.push_back(keyfrm->frm_obs_.descriptors_.row(idx)); } } @@ -183,7 +182,7 @@ void landmark::compute_descriptor() { } } -void landmark::update_normal_and_depth() { +void landmark::update_mean_normal_and_obs_scale_variance() { observations_t observations; std::shared_ptr ref_keyfrm = nullptr; Vec3_t pos_w; @@ -214,14 +213,14 @@ void landmark::update_normal_and_depth() { const Vec3_t cam_to_lm_vec = pos_w - ref_keyfrm->get_cam_center(); const auto dist = cam_to_lm_vec.norm(); - const auto scale_level = ref_keyfrm->undist_keypts_.at(observations.at(ref_keyfrm)).octave; - const auto scale_factor = ref_keyfrm->scale_factors_.at(scale_level); - const auto num_scale_levels = ref_keyfrm->num_scale_levels_; + const auto scale_level = ref_keyfrm->frm_obs_.undist_keypts_.at(observations.at(ref_keyfrm)).octave; + const auto scale_factor = ref_keyfrm->orb_params_->scale_factors_.at(scale_level); + const auto num_scale_levels = ref_keyfrm->orb_params_->num_levels_; { std::lock_guard lock3(mtx_position_); max_valid_dist_ = dist * scale_factor; - min_valid_dist_ = max_valid_dist_ / ref_keyfrm->scale_factors_.at(num_scale_levels - 1); + min_valid_dist_ = max_valid_dist_ / ref_keyfrm->orb_params_->scale_factors_.at(num_scale_levels - 1); mean_normal_ = mean_normal.normalized(); } } @@ -236,45 +235,26 @@ float landmark::get_max_valid_distance() const { return 1.3 * max_valid_dist_; } -unsigned int landmark::predict_scale_level(const float cam_to_lm_dist, const frame* frm) const { +unsigned int landmark::predict_scale_level(const float cam_to_lm_dist, float num_scale_levels, float log_scale_factor) const { float ratio; { std::lock_guard lock(mtx_position_); ratio = max_valid_dist_ / cam_to_lm_dist; } - const auto pred_scale_level = static_cast(std::ceil(std::log(ratio) / frm->log_scale_factor_)); + const auto pred_scale_level = static_cast(std::ceil(std::log(ratio) / log_scale_factor)); if (pred_scale_level < 0) { return 0; } - else if (frm->num_scale_levels_ <= static_cast(pred_scale_level)) { - return frm->num_scale_levels_ - 1; + else if (num_scale_levels <= static_cast(pred_scale_level)) { + return num_scale_levels - 1; } else { return static_cast(pred_scale_level); } } -unsigned int landmark::predict_scale_level(const float cam_to_lm_dist, const std::shared_ptr& keyfrm) const { - float ratio; - { - std::lock_guard lock(mtx_position_); - ratio = max_valid_dist_ / cam_to_lm_dist; - } - - const auto pred_scale_level = static_cast(std::ceil(std::log(ratio) / keyfrm->log_scale_factor_)); - if (pred_scale_level < 0) { - return 0; - } - else if (keyfrm->num_scale_levels_ <= static_cast(pred_scale_level)) { - return keyfrm->num_scale_levels_ - 1; - } - else { - return static_cast(pred_scale_level); - } -} - -void landmark::prepare_for_erasing() { +void landmark::prepare_for_erasing(map_database* map_db) { observations_t observations; { std::lock_guard lock1(mtx_observations_); @@ -288,7 +268,7 @@ void landmark::prepare_for_erasing() { keyfrm_and_idx.first.lock()->erase_landmark_with_index(keyfrm_and_idx.second); } - map_db_->erase_landmark(this->id_); + map_db->erase_landmark(this->id_); } bool landmark::will_be_erased() { diff --git a/src/openvslam/data/landmark.h b/src/openvslam/data/landmark.h index 34991ea4f..14fc59d4b 100644 --- a/src/openvslam/data/landmark.h +++ b/src/openvslam/data/landmark.h @@ -48,7 +48,7 @@ class landmark { //! add observation void add_observation(const std::shared_ptr& keyfrm, unsigned int idx); //! erase observation - void erase_observation(const std::shared_ptr& keyfrm); + void erase_observation(map_database* map_db, const std::shared_ptr& keyfrm); //! get observations (keyframe and keypoint idx) observations_t get_observations() const; @@ -76,20 +76,18 @@ class landmark { void compute_descriptor(); //! update observation mean normal and ORB scale variance - void update_normal_and_depth(); + void update_mean_normal_and_obs_scale_variance(); //! get max valid distance between landmark and camera float get_min_valid_distance() const; //! get min valid distance between landmark and camera float get_max_valid_distance() const; - //! predict scale level assuming this landmark is observed in the specified frame - unsigned int predict_scale_level(const float cam_to_lm_dist, const frame* frm) const; - //! predict scale level assuming this landmark is observed in the specified keyframe - unsigned int predict_scale_level(const float cam_to_lm_dist, const std::shared_ptr& keyfrm) const; + //! predict scale level assuming this landmark is observed in the specified frame/keyframe + unsigned int predict_scale_level(const float cam_to_lm_dist, float num_scale_levels, float log_scale_factor) const; //! erase this landmark from database - void prepare_for_erasing(); + void prepare_for_erasing(map_database* map_db); //! whether this landmark will be erased shortly or not bool will_be_erased(); @@ -111,20 +109,6 @@ class landmark { unsigned int first_keyfrm_id_ = 0; unsigned int num_observations_ = 0; - // Variables for frame tracking. - Vec2_t reproj_in_tracking_; - float x_right_in_tracking_; - bool is_observable_in_tracking_; - int scale_level_in_tracking_; - unsigned int identifier_in_local_map_update_ = 0; - unsigned int identifier_in_local_lm_search_ = 0; - - // Variables for loop-closing. - unsigned int loop_fusion_identifier_ = 0; - unsigned int ref_keyfrm_id_in_loop_fusion_ = 0; - Vec3_t pos_w_after_global_BA_; - unsigned int loop_BA_identifier_ = 0; - private: //! world coordinates of this landmark Vec3_t pos_w_; diff --git a/src/openvslam/data/map_database.cc b/src/openvslam/data/map_database.cc index e634143c6..9d2a06e9e 100644 --- a/src/openvslam/data/map_database.cc +++ b/src/openvslam/data/map_database.cc @@ -4,7 +4,9 @@ #include "openvslam/data/keyframe.h" #include "openvslam/data/landmark.h" #include "openvslam/data/camera_database.h" +#include "openvslam/data/orb_params_database.h" #include "openvslam/data/map_database.h" +#include "openvslam/data/bow_vocabulary.h" #include "openvslam/util/converter.h" #include @@ -27,9 +29,7 @@ map_database::~map_database() { void map_database::add_keyframe(const std::shared_ptr& keyfrm) { std::lock_guard lock(mtx_map_access_); keyframes_[keyfrm->id_] = keyfrm; - if (keyfrm->id_ > max_keyfrm_id_) { - max_keyfrm_id_ = keyfrm->id_; - } + last_inserted_keyfrm_ = keyfrm; } void map_database::erase_keyframe(const std::shared_ptr& keyfrm) { @@ -139,30 +139,22 @@ std::vector> map_database::get_all_landmarks() const { return landmarks; } -unsigned int map_database::get_num_landmarks() const { +std::shared_ptr map_database::get_last_inserted_keyframe() const { std::lock_guard lock(mtx_map_access_); - return landmarks_.size(); + return last_inserted_keyfrm_; } -unsigned int map_database::get_max_keyframe_id() const { +unsigned int map_database::get_num_landmarks() const { std::lock_guard lock(mtx_map_access_); - return max_keyfrm_id_; + return landmarks_.size(); } void map_database::clear() { std::lock_guard lock(mtx_map_access_); - for (auto& lm : landmarks_) { - lm.second = nullptr; - } - - for (auto& keyfrm : keyframes_) { - keyfrm.second = nullptr; - } - landmarks_.clear(); keyframes_.clear(); - max_keyfrm_id_ = 0; + last_inserted_keyfrm_ = nullptr; local_landmarks_.clear(); origin_keyfrm_ = nullptr; @@ -171,7 +163,7 @@ void map_database::clear() { spdlog::info("clear map database"); } -void map_database::from_json(camera_database* cam_db, bow_vocabulary* bow_vocab, bow_database* bow_db, +void map_database::from_json(camera_database* cam_db, orb_params_database* orb_params_db, bow_vocabulary* bow_vocab, const nlohmann::json& json_keyfrms, const nlohmann::json& json_landmarks) { std::lock_guard lock(mtx_map_access_); @@ -186,7 +178,8 @@ void map_database::from_json(camera_database* cam_db, bow_vocabulary* bow_vocab, landmarks_.clear(); keyframes_.clear(); - max_keyfrm_id_ = 0; + // When loading the map, leave last_inserted_keyfrm_ as nullptr. + last_inserted_keyfrm_ = nullptr; local_landmarks_.clear(); origin_keyfrm_ = nullptr; @@ -198,7 +191,7 @@ void map_database::from_json(camera_database* cam_db, bow_vocabulary* bow_vocab, assert(0 <= id); const auto json_keyfrm = json_id_keyfrm.value(); - register_keyframe(cam_db, bow_vocab, bow_db, id, json_keyfrm); + register_keyframe(cam_db, orb_params_db, bow_vocab, id, json_keyfrm); } // Step 3. Register 3D landmark point @@ -254,19 +247,20 @@ void map_database::from_json(camera_database* cam_db, bow_vocabulary* bow_vocab, assert(landmarks_.count(id)); const auto& lm = landmarks_.at(id); - lm->update_normal_and_depth(); + lm->update_mean_normal_and_obs_scale_variance(); lm->compute_descriptor(); } } -void map_database::register_keyframe(camera_database* cam_db, bow_vocabulary* bow_vocab, bow_database* bow_db, +void map_database::register_keyframe(camera_database* cam_db, orb_params_database* orb_params_db, bow_vocabulary* bow_vocab, const unsigned int id, const nlohmann::json& json_keyfrm) { // Metadata const auto src_frm_id = json_keyfrm.at("src_frm_id").get(); const auto timestamp = json_keyfrm.at("ts").get(); const auto camera_name = json_keyfrm.at("cam").get(); const auto camera = cam_db->get_camera(camera_name); - const auto depth_thr = json_keyfrm.at("depth_thr").get(); + const auto orb_params_name = json_keyfrm.at("orb_params").get(); + const auto orb_params = orb_params_db->get_orb_params(orb_params_name); // Pose information const Mat33_t rot_cw = convert_json_to_rotation(json_keyfrm.at("rot_cw")); @@ -298,22 +292,23 @@ void map_database::register_keyframe(camera_database* cam_db, bow_vocabulary* bo const auto descriptors = convert_json_to_descriptors(json_descriptors); assert(descriptors.rows == static_cast(num_keypts)); - // Scale information in ORB - const auto num_scale_levels = json_keyfrm.at("n_scale_levels").get(); - const auto scale_factor = json_keyfrm.at("scale_factor").get(); - // Construct a new object + data::bow_vector bow_vec; + data::bow_feature_vector bow_feat_vec; + // Assign all the keypoints into grid + std::vector>> keypt_indices_in_cells; + data::assign_keypoints_to_grid(camera, undist_keypts, keypt_indices_in_cells); + // Construct frame_observation + frame_observation frm_obs{num_keypts, keypts, descriptors, undist_keypts, bearings, stereo_x_right, depths, keypt_indices_in_cells}; + // Compute BoW + data::bow_vocabulary_util::compute_bow(bow_vocab, descriptors, bow_vec, bow_feat_vec); auto keyfrm = data::keyframe::make_keyframe( - id, src_frm_id, timestamp, cam_pose_cw, camera, depth_thr, - num_keypts, keypts, undist_keypts, bearings, stereo_x_right, depths, descriptors, - num_scale_levels, scale_factor, bow_vocab, bow_db, this); + id, src_frm_id, timestamp, cam_pose_cw, camera, orb_params, + frm_obs, bow_vec, bow_feat_vec); // Append to map database assert(!keyframes_.count(id)); keyframes_[keyfrm->id_] = keyfrm; - if (keyfrm->id_ > max_keyfrm_id_) { - max_keyfrm_id_ = keyfrm->id_; - } if (id == 0) { origin_keyfrm_ = keyfrm; } @@ -404,7 +399,7 @@ void map_database::to_json(nlohmann::json& json_keyfrms, nlohmann::json& json_la assert(lm); assert(id == lm->id_); assert(!lm->will_be_erased()); - lm->update_normal_and_depth(); + lm->update_mean_normal_and_obs_scale_variance(); assert(!landmarks.count(std::to_string(id))); landmarks[std::to_string(id)] = lm->to_json(); } diff --git a/src/openvslam/data/map_database.h b/src/openvslam/data/map_database.h index ad8bc180b..57100bffa 100644 --- a/src/openvslam/data/map_database.h +++ b/src/openvslam/data/map_database.h @@ -1,7 +1,7 @@ #ifndef OPENVSLAM_DATA_MAP_DATABASE_H #define OPENVSLAM_DATA_MAP_DATABASE_H -#include "openvslam/data/bow_vocabulary.h" +#include "openvslam/data/bow_vocabulary_fwd.h" #include "openvslam/data/frame_statistics.h" #include @@ -23,6 +23,7 @@ class frame; class keyframe; class landmark; class camera_database; +class orb_params_database; class bow_database; class map_database { @@ -116,16 +117,16 @@ class map_database { std::vector> get_all_landmarks() const; /** - * Get the number of landmarks - * @return + * Get the last keyframe added to the database + * @return shared pointer to the last keyframe added to the database */ - unsigned int get_num_landmarks() const; + std::shared_ptr get_last_inserted_keyframe() const; /** - * Get the maximum keyframe ID + * Get the number of landmarks * @return */ - unsigned int get_max_keyframe_id() const; + unsigned int get_num_landmarks() const; /** * Update frame statistics @@ -164,12 +165,12 @@ class map_database { /** * Load keyframes and landmarks from JSON * @param cam_db + * @param orb_params_db * @param bow_vocab - * @param bow_db * @param json_keyfrms * @param json_landmarks */ - void from_json(camera_database* cam_db, bow_vocabulary* bow_vocab, bow_database* bow_db, + void from_json(camera_database* cam_db, orb_params_database* orb_params_db, bow_vocabulary* bow_vocab, const nlohmann::json& json_keyfrms, const nlohmann::json& json_landmarks); /** @@ -191,12 +192,12 @@ class map_database { * Decode JSON and register keyframe information to the map database * (NOTE: objects which are not constructed yet will be set as nullptr) * @param cam_db + * @param orb_params_db * @param bow_vocab - * @param bow_db * @param id * @param json_keyfrm */ - void register_keyframe(camera_database* cam_db, bow_vocabulary* bow_vocab, bow_database* bow_db, + void register_keyframe(camera_database* cam_db, orb_params_database* orb_params_db, bow_vocabulary* bow_vocab, const unsigned int id, const nlohmann::json& json_keyfrm); /** @@ -234,12 +235,12 @@ class map_database { //! IDs and landmarks std::unordered_map> landmarks_; + //! The last keyframe added to the database + std::shared_ptr last_inserted_keyfrm_ = nullptr; + //! local landmarks std::vector> local_landmarks_; - //! max keyframe ID - unsigned int max_keyfrm_id_ = 0; - //----------------------------------------- // frame statistics for odometry evaluation diff --git a/src/openvslam/data/orb_params_database.cc b/src/openvslam/data/orb_params_database.cc new file mode 100644 index 000000000..d049fa2b0 --- /dev/null +++ b/src/openvslam/data/orb_params_database.cc @@ -0,0 +1,91 @@ +#include "openvslam/data/orb_params_database.h" +#include "openvslam/feature/orb_params.h" + +#include +#include + +namespace openvslam { +namespace data { + +orb_params_database::orb_params_database(feature::orb_params* curr_orb_params) + : curr_orb_params_(curr_orb_params) { + spdlog::debug("CONSTRUCT: data::orb_params_database"); +} + +orb_params_database::~orb_params_database() { + for (const auto& name_orb_params : database_) { + const auto& orb_params_name = name_orb_params.first; + const auto orb_params = name_orb_params.second; + + // Since curr_orb_params is held in the config class, do not delete curr_orb_params here + if (orb_params->name_ == curr_orb_params_->name_) { + continue; + } + delete database_.at(orb_params_name); + database_.at(orb_params_name) = nullptr; + } + database_.clear(); + + spdlog::debug("DESTRUCT: data::orb_params_database"); +} + +feature::orb_params* orb_params_database::get_orb_params(const std::string& orb_params_name) const { + std::lock_guard lock(mtx_database_); + if (orb_params_name == curr_orb_params_->name_) { + return curr_orb_params_; + } + else { + assert(database_.count(orb_params_name)); + return database_.at(orb_params_name); + } +} + +void orb_params_database::from_json(const nlohmann::json& json_orb_params) { + std::lock_guard lock(mtx_database_); + + spdlog::info("decoding {} orb_params to load", json_orb_params.size()); + for (const auto& json_id_orb_params : json_orb_params.items()) { + const auto& orb_params_name = json_id_orb_params.key(); + const auto& json_orb_params = json_id_orb_params.value(); + + if (orb_params_name == curr_orb_params_->name_) { + if (std::abs(curr_orb_params_->scale_factor_ - json_orb_params.at("scale_factor").get()) < 1e-6 + && curr_orb_params_->num_levels_ - json_orb_params.at("num_levels").get() == 0 + && curr_orb_params_->ini_fast_thr_ - json_orb_params.at("ini_fast_threshold").get() == 0 + && curr_orb_params_->min_fast_thr_ - json_orb_params.at("min_fast_threshold").get() == 0) { + spdlog::info("load the current orb_params \"{}\"", orb_params_name); + continue; + } + else { + throw std::runtime_error("The same feature extraction settings exist with the same name. Please give them different names."); + } + } + + spdlog::info("load a orb_params \"{}\" from JSON", orb_params_name); + + auto orb_params = new feature::orb_params(orb_params_name, + json_orb_params.at("scale_factor").get(), + json_orb_params.at("num_levels").get(), + json_orb_params.at("ini_fast_threshold").get(), + json_orb_params.at("min_fast_threshold").get()); + assert(!database_.count(orb_params_name)); + database_[orb_params_name] = orb_params; + } +} + +nlohmann::json orb_params_database::to_json() const { + std::lock_guard lock(mtx_database_); + + spdlog::info("encoding {} orb_params to store", database_.size() + 1); + std::map orb_params_jsons; + orb_params_jsons[curr_orb_params_->name_] = curr_orb_params_->to_json(); + for (const auto& name_orb_params : database_) { + const auto& orb_params_name = name_orb_params.first; + const auto orb_params = name_orb_params.second; + orb_params_jsons[orb_params_name] = orb_params->to_json(); + } + return orb_params_jsons; +} + +} // namespace data +} // namespace openvslam diff --git a/src/openvslam/data/orb_params_database.h b/src/openvslam/data/orb_params_database.h new file mode 100644 index 000000000..97822c28b --- /dev/null +++ b/src/openvslam/data/orb_params_database.h @@ -0,0 +1,45 @@ +#ifndef OPENVSLAM_DATA_ORB_PARAMS_DATABASE_H +#define OPENVSLAM_DATA_ORB_PARAMS_DATABASE_H + +#include +#include + +#include + +namespace openvslam { + +namespace feature { +class orb_params; +} // namespace feature + +namespace data { + +class orb_params_database { +public: + explicit orb_params_database(feature::orb_params* curr_orb_params); + + ~orb_params_database(); + + feature::orb_params* get_orb_params(const std::string& orb_params_name) const; + + void from_json(const nlohmann::json& json_orb_params); + + nlohmann::json to_json() const; + +private: + //----------------------------------------- + //! mutex to access the database + mutable std::mutex mtx_database_; + //! pointer to the orb_params which used in the current tracking + //! (NOTE: the object is owned by config class, + //! thus this class does NOT delete the object of curr_orb_params_) + feature::orb_params* curr_orb_params_ = nullptr; + //! database (key: orb_params name, value: pointer of feature::orb_params) + //! (NOTE: tracking orb_params must NOT be contained in the database) + std::unordered_map database_; +}; + +} // namespace data +} // namespace openvslam + +#endif // OPENVSLAM_DATA_ORB_PARAMS_DATABASE_H diff --git a/src/openvslam/feature/orb_extractor.cc b/src/openvslam/feature/orb_extractor.cc index fd5b6e122..7f2106de3 100644 --- a/src/openvslam/feature/orb_extractor.cc +++ b/src/openvslam/feature/orb_extractor.cc @@ -50,19 +50,15 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #endif #endif // USE_SSE_ORB +#include + namespace openvslam { namespace feature { -orb_extractor::orb_extractor(const unsigned int max_num_keypts, - const float scale_factor, const unsigned int num_levels, - const unsigned int ini_fast_thr, const unsigned int min_fast_thr, +orb_extractor::orb_extractor(const orb_params* orb_params, + const unsigned int max_num_keypts, const std::vector>& mask_rects) - : orb_extractor(max_num_keypts, orb_params{scale_factor, num_levels, - ini_fast_thr, min_fast_thr, - mask_rects}) {} - -orb_extractor::orb_extractor(const unsigned int max_num_keypts, const orb_params& orb_params) - : max_num_keypts_(max_num_keypts), orb_params_(orb_params) { + : orb_params_(orb_params), mask_rects_(mask_rects), max_num_keypts_(max_num_keypts) { // initialize parameters initialize(); } @@ -81,7 +77,7 @@ void orb_extractor::extract(const cv::_InputArray& in_image, const cv::_InputArr compute_image_pyramid(image); // mask initialization - if (!mask_is_initialized_ && !orb_params_.mask_rects_.empty()) { + if (!mask_is_initialized_ && !mask_rects_.empty()) { create_rectangle_mask(image.cols, image.rows); mask_is_initialized_ = true; } @@ -108,7 +104,7 @@ void orb_extractor::extract(const cv::_InputArray& in_image, const cv::_InputArr cv::Mat descriptors; unsigned int num_keypts = 0; - for (unsigned int level = 0; level < orb_params_.num_levels_; ++level) { + for (unsigned int level = 0; level < orb_params_->num_levels_; ++level) { num_keypts += all_keypts.at(level).size(); } if (num_keypts == 0) { @@ -123,7 +119,7 @@ void orb_extractor::extract(const cv::_InputArray& in_image, const cv::_InputArr keypts.reserve(num_keypts); unsigned int offset = 0; - for (unsigned int level = 0; level < orb_params_.num_levels_; ++level) { + for (unsigned int level = 0; level < orb_params_->num_levels_; ++level) { auto& keypts_at_level = all_keypts.at(level); const auto num_keypts_at_level = keypts_at_level.size(); @@ -154,75 +150,22 @@ void orb_extractor::set_max_num_keypoints(const unsigned int max_num_keypts) { initialize(); } -float orb_extractor::get_scale_factor() const { - return orb_params_.scale_factor_; -} - -void orb_extractor::set_scale_factor(const float scale_factor) { - orb_params_.scale_factor_ = scale_factor; - initialize(); -} - -unsigned int orb_extractor::get_num_scale_levels() const { - return orb_params_.num_levels_; -} - -void orb_extractor::set_num_scale_levels(const unsigned int num_levels) { - orb_params_.num_levels_ = num_levels; - initialize(); -} - -unsigned int orb_extractor::get_initial_fast_threshold() const { - return orb_params_.ini_fast_thr_; -} - -void orb_extractor::set_initial_fast_threshold(const unsigned int initial_fast_threshold) { - orb_params_.ini_fast_thr_ = initial_fast_threshold; -} - -unsigned int orb_extractor::get_minimum_fast_threshold() const { - return orb_params_.min_fast_thr; -} - -void orb_extractor::set_minimum_fast_threshold(const unsigned int minimum_fast_threshold) { - orb_params_.min_fast_thr = minimum_fast_threshold; -} - -std::vector orb_extractor::get_scale_factors() const { - return scale_factors_; -} - -std::vector orb_extractor::get_inv_scale_factors() const { - return inv_scale_factors_; -} - -std::vector orb_extractor::get_level_sigma_sq() const { - return level_sigma_sq_; -} - -std::vector orb_extractor::get_inv_level_sigma_sq() const { - return inv_level_sigma_sq_; -} - void orb_extractor::initialize() { - // compute scale pyramid information - calc_scale_factors(); - // resize buffers according to the number of levels - image_pyramid_.resize(orb_params_.num_levels_); - num_keypts_per_level_.resize(orb_params_.num_levels_); + image_pyramid_.resize(orb_params_->num_levels_); + num_keypts_per_level_.resize(orb_params_->num_levels_); // compute the desired number of keypoints per scale double desired_num_keypts_per_scale - = max_num_keypts_ * (1.0 - 1.0 / orb_params_.scale_factor_) - / (1.0 - std::pow(1.0 / orb_params_.scale_factor_, static_cast(orb_params_.num_levels_))); + = max_num_keypts_ * (1.0 - 1.0 / orb_params_->scale_factor_) + / (1.0 - std::pow(1.0 / orb_params_->scale_factor_, static_cast(orb_params_->num_levels_))); unsigned int total_num_keypts = 0; - for (unsigned int level = 0; level < orb_params_.num_levels_ - 1; ++level) { + for (unsigned int level = 0; level < orb_params_->num_levels_ - 1; ++level) { num_keypts_per_level_.at(level) = std::round(desired_num_keypts_per_scale); total_num_keypts += num_keypts_per_level_.at(level); - desired_num_keypts_per_scale *= 1.0 / orb_params_.scale_factor_; + desired_num_keypts_per_scale *= 1.0 / orb_params_->scale_factor_; } - num_keypts_per_level_.at(orb_params_.num_levels_ - 1) = std::max(static_cast(max_num_keypts_) - static_cast(total_num_keypts), 0); + num_keypts_per_level_.at(orb_params_->num_levels_ - 1) = std::max(static_cast(max_num_keypts_) - static_cast(total_num_keypts), 0); // Preparate for computation of orientation u_max_.resize(fast_half_patch_size_ + 1); @@ -240,19 +183,12 @@ void orb_extractor::initialize() { } } -void orb_extractor::calc_scale_factors() { - scale_factors_ = orb_params::calc_scale_factors(orb_params_.num_levels_, orb_params_.scale_factor_); - inv_scale_factors_ = orb_params::calc_inv_scale_factors(orb_params_.num_levels_, orb_params_.scale_factor_); - level_sigma_sq_ = orb_params::calc_level_sigma_sq(orb_params_.num_levels_, orb_params_.scale_factor_); - inv_level_sigma_sq_ = orb_params::calc_inv_level_sigma_sq(orb_params_.num_levels_, orb_params_.scale_factor_); -} - void orb_extractor::create_rectangle_mask(const unsigned int cols, const unsigned int rows) { if (rect_mask_.empty()) { rect_mask_ = cv::Mat(rows, cols, CV_8UC1, cv::Scalar(255)); } // draw masks - for (const auto& mask_rect : orb_params_.mask_rects_) { + for (const auto& mask_rect : mask_rects_) { // draw black rectangle const unsigned int x_min = std::round(cols * mask_rect.at(0)); const unsigned int x_max = std::round(cols * mask_rect.at(1)); @@ -264,9 +200,9 @@ void orb_extractor::create_rectangle_mask(const unsigned int cols, const unsigne void orb_extractor::compute_image_pyramid(const cv::Mat& image) { image_pyramid_.at(0) = image; - for (unsigned int level = 1; level < orb_params_.num_levels_; ++level) { + for (unsigned int level = 1; level < orb_params_->num_levels_; ++level) { // determine the size of an image - const double scale = scale_factors_.at(level); + const double scale = orb_params_->scale_factors_.at(level); const cv::Size size(std::round(image.cols * 1.0 / scale), std::round(image.rows * 1.0 / scale)); // resize cv::resize(image_pyramid_.at(level - 1), image_pyramid_.at(level), size, 0, 0, cv::INTER_LINEAR); @@ -274,7 +210,7 @@ void orb_extractor::compute_image_pyramid(const cv::Mat& image) { } void orb_extractor::compute_fast_keypoints(std::vector>& all_keypts, const cv::Mat& mask) const { - all_keypts.resize(orb_params_.num_levels_); + all_keypts.resize(orb_params_->num_levels_); // An anonymous function which checks mask(image or rectangle) auto is_in_mask = [&mask](const unsigned int y, const unsigned int x, const float scale_factor) { @@ -287,8 +223,8 @@ void orb_extractor::compute_fast_keypoints(std::vector #ifdef USE_OPENMP #pragma omp parallel for #endif - for (unsigned int level = 0; level < orb_params_.num_levels_; ++level) { - const float scale_factor = scale_factors_.at(level); + for (unsigned int level = 0; level < orb_params_->num_levels_; ++level) { + const float scale_factor = orb_params_->scale_factors_.at(level); constexpr unsigned int min_border_x = orb_patch_radius_; constexpr unsigned int min_border_y = orb_patch_radius_; @@ -340,12 +276,12 @@ void orb_extractor::compute_fast_keypoints(std::vector std::vector keypts_in_cell; cv::FAST(image_pyramid_.at(level).rowRange(min_y, max_y).colRange(min_x, max_x), - keypts_in_cell, orb_params_.ini_fast_thr_, true); + keypts_in_cell, orb_params_->ini_fast_thr_, true); // Re-compute FAST keypoint with reduced threshold if enough keypoint was not got if (keypts_in_cell.empty()) { cv::FAST(image_pyramid_.at(level).rowRange(min_y, max_y).colRange(min_x, max_x), - keypts_in_cell, orb_params_.min_fast_thr, true); + keypts_in_cell, orb_params_->min_fast_thr_, true); } if (keypts_in_cell.empty()) { @@ -379,7 +315,7 @@ void orb_extractor::compute_fast_keypoints(std::vector num_keypts_per_level_.at(level)); // Keypoint size is patch size modified by the scale factor - const unsigned int scaled_patch_size = fast_patch_size_ * scale_factors_.at(level); + const unsigned int scaled_patch_size = fast_patch_size_ * orb_params_->scale_factors_.at(level); for (auto& keypt : keypts_at_level) { // Translation correction (scale will be corrected after ORB description) @@ -392,7 +328,7 @@ void orb_extractor::compute_fast_keypoints(std::vector } // Compute orientations - for (unsigned int level = 0; level < orb_params_.num_levels_; ++level) { + for (unsigned int level = 0; level < orb_params_->num_levels_; ++level) { compute_orientation(image_pyramid_.at(level), all_keypts.at(level)); } } @@ -450,19 +386,28 @@ std::vector orb_extractor::distribute_keypoints_via_tree(const std auto prev_leaf_nodes_pool = leaf_nodes_pool; leaf_nodes_pool.clear(); - // Sort by number of keypoints in the patch of each leaf node - std::sort(prev_leaf_nodes_pool.rbegin(), prev_leaf_nodes_pool.rend()); - // Do processes from the node which has much more keypoints + int max_num_keypts = 0; for (const auto& prev_leaf_node : prev_leaf_nodes_pool) { - // Divide node and assign to the leaf node pool - const auto child_nodes = prev_leaf_node.second->divide_node(); - assign_child_nodes(child_nodes, nodes, leaf_nodes_pool); - // Remove the old node - nodes.erase(prev_leaf_node.second->iter_); + if (max_num_keypts < prev_leaf_node.first) { + max_num_keypts = prev_leaf_node.first; + } + } - if (num_keypts <= nodes.size()) { - is_filled = true; - break; + // Do processes from the node which has much more keypoints + for (int target_num_keypts = max_num_keypts; target_num_keypts > 0; --target_num_keypts) { + for (const auto& prev_leaf_node : prev_leaf_nodes_pool) { + if (prev_leaf_node.first == target_num_keypts) { + // Divide node and assign to the leaf node pool + const auto child_nodes = prev_leaf_node.second->divide_node(); + assign_child_nodes(child_nodes, nodes, leaf_nodes_pool); + // Remove the old node + nodes.erase(prev_leaf_node.second->iter_); + + if (num_keypts <= nodes.size()) { + is_filled = true; + break; + } + } } } @@ -601,7 +546,7 @@ void orb_extractor::correct_keypoint_scale(std::vector& keypts_at_ if (level == 0) { return; } - const float scale_at_level = scale_factors_.at(level); + const float scale_at_level = orb_params_->scale_factors_.at(level); for (auto& keypt_at_level : keypts_at_level) { keypt_at_level.pt *= scale_at_level; } diff --git a/src/openvslam/feature/orb_extractor.h b/src/openvslam/feature/orb_extractor.h index 4d3c75213..bfa10821b 100644 --- a/src/openvslam/feature/orb_extractor.h +++ b/src/openvslam/feature/orb_extractor.h @@ -15,14 +15,10 @@ class orb_extractor { orb_extractor() = delete; //! Constructor - orb_extractor(const unsigned int max_num_keypts, - const float scale_factor, const unsigned int num_levels, - const unsigned int ini_fast_thr, const unsigned int min_fast_thr, + orb_extractor(const orb_params* orb_params, + const unsigned int max_num_keypts, const std::vector>& mask_rects = {}); - //! Constructor - orb_extractor(const unsigned int max_num_keypts, const orb_params& orb_params); - //! Destructor virtual ~orb_extractor() = default; @@ -36,41 +32,12 @@ class orb_extractor { //! Set the maximum number of keypoints void set_max_num_keypoints(const unsigned int max_num_keypts); - //! Get the scale factor - float get_scale_factor() const; - - //! Set the scale factor - void set_scale_factor(const float scale_factor); - - //! Get the number of scale levels - unsigned int get_num_scale_levels() const; - - //! Set the number of scale levels - void set_num_scale_levels(const unsigned int num_levels); - - //! Get the initial fast threshold - unsigned int get_initial_fast_threshold() const; - - //! Set the initial fast threshold - void set_initial_fast_threshold(const unsigned int initial_fast_threshold); - - //! Get the minimum fast threshold - unsigned int get_minimum_fast_threshold() const; - - //! Set the minimum fast threshold - void set_minimum_fast_threshold(const unsigned int minimum_fast_threshold); - - //! Get scale factors - std::vector get_scale_factors() const; - - //! Set scale factors - std::vector get_inv_scale_factors() const; - - //! Get sigma square for all levels - std::vector get_level_sigma_sq() const; + //! parameters for ORB extraction + const orb_params* orb_params_; - //! Get inverted sigma square for all levels - std::vector get_inv_level_sigma_sq() const; + //! A vector of keypoint area represents mask area + //! Each areas are denoted as form of [x_min / cols, x_max / cols, y_min / rows, y_max / rows] + std::vector> mask_rects_; //! Image pyramid std::vector image_pyramid_; @@ -125,9 +92,6 @@ class orb_extractor { //! Number of feature points to be extracted unsigned int max_num_keypts_; - //! parameters for ORB extraction - orb_params orb_params_; - //! BRIEF orientation static constexpr unsigned int fast_patch_size_ = 31; //! half size of FAST patch @@ -140,13 +104,6 @@ class orb_extractor { bool mask_is_initialized_ = false; cv::Mat rect_mask_; - //! A list of the scale factor of each pyramid layer - std::vector scale_factors_; - std::vector inv_scale_factors_; - //! A list of the sigma of each pyramid layer - std::vector level_sigma_sq_; - std::vector inv_level_sigma_sq_; - //! Maximum number of keypoint of each level std::vector num_keypts_per_level_; //! Index limitation that used for calculating of keypoint orientation diff --git a/src/openvslam/feature/orb_params.cc b/src/openvslam/feature/orb_params.cc index 163d35b09..028f4dacb 100644 --- a/src/openvslam/feature/orb_params.cc +++ b/src/openvslam/feature/orb_params.cc @@ -1,35 +1,38 @@ #include "openvslam/feature/orb_params.h" +#include #include namespace openvslam { namespace feature { -orb_params::orb_params(const float scale_factor, const unsigned int num_levels, - const unsigned int ini_fast_thr, const unsigned int min_fast_thr, - const std::vector>& mask_rects) - : scale_factor_(scale_factor), num_levels_(num_levels), - ini_fast_thr_(ini_fast_thr), min_fast_thr(min_fast_thr), - mask_rects_(mask_rects) { - for (const auto& v : mask_rects_) { - if (v.size() != 4) { - throw std::runtime_error("mask rectangle must contain four parameters"); - } - if (v.at(0) >= v.at(1)) { - throw std::runtime_error("x_max must be greater than x_min"); - } - if (v.at(2) >= v.at(3)) { - throw std::runtime_error("y_max must be greater than x_min"); - } - } +orb_params::orb_params(const std::string& name) + : orb_params(name, 1.2, 8, 20, 7) {} + +orb_params::orb_params(const std::string& name, const float scale_factor, const unsigned int num_levels, + const unsigned int ini_fast_thr, const unsigned int min_fast_thr) + : name_(name), scale_factor_(scale_factor), log_scale_factor_(std::log(scale_factor)), + num_levels_(num_levels), ini_fast_thr_(ini_fast_thr), min_fast_thr_(min_fast_thr) { + scale_factors_ = calc_scale_factors(num_levels_, scale_factor_); + inv_scale_factors_ = calc_inv_scale_factors(num_levels_, scale_factor_); + level_sigma_sq_ = calc_level_sigma_sq(num_levels_, scale_factor_); + inv_level_sigma_sq_ = calc_inv_level_sigma_sq(num_levels_, scale_factor_); } orb_params::orb_params(const YAML::Node& yaml_node) - : orb_params(yaml_node["scale_factor"].as(1.2), + : orb_params(yaml_node["name"].as(), + yaml_node["scale_factor"].as(1.2), yaml_node["num_levels"].as(8), yaml_node["ini_fast_threshold"].as(20), - yaml_node["min_fast_threshold"].as(7), - yaml_node["mask_rectangles"].as>>(std::vector>())) {} + yaml_node["min_fast_threshold"].as(7)) {} + +nlohmann::json orb_params::to_json() const { + return {{"name", name_}, + {"scale_factor", scale_factor_}, + {"num_levels", num_levels_}, + {"ini_fast_threshold", ini_fast_thr_}, + {"min_fast_threshold", min_fast_thr_}}; +} std::vector orb_params::calc_scale_factors(const unsigned int num_scale_levels, const float scale_factor) { std::vector scale_factors(num_scale_levels, 1.0); @@ -71,17 +74,7 @@ std::ostream& operator<<(std::ostream& os, const orb_params& oparam) { os << "- scale factor: " << oparam.scale_factor_ << std::endl; os << "- number of levels: " << oparam.num_levels_ << std::endl; os << "- initial fast threshold: " << oparam.ini_fast_thr_ << std::endl; - os << "- minimum fast threshold: " << oparam.min_fast_thr << std::endl; - if (!oparam.mask_rects_.empty()) { - os << "- mask rectangles:" << std::endl; - for (const auto& mask_rect : oparam.mask_rects_) { - os << " - [" - << mask_rect.at(0) << ", " - << mask_rect.at(1) << ", " - << mask_rect.at(2) << ", " - << mask_rect.at(3) << "]" << std::endl; - } - } + os << "- minimum fast threshold: " << oparam.min_fast_thr_ << std::endl; return os; } diff --git a/src/openvslam/feature/orb_params.h b/src/openvslam/feature/orb_params.h index 332d60cc8..b0edab9c9 100644 --- a/src/openvslam/feature/orb_params.h +++ b/src/openvslam/feature/orb_params.h @@ -1,18 +1,20 @@ #ifndef OPENVSLAM_FEATURE_ORB_PARAMS_H #define OPENVSLAM_FEATURE_ORB_PARAMS_H +#include #include +#include namespace openvslam { namespace feature { struct orb_params { - orb_params() = default; + orb_params() = delete; //! Constructor - orb_params(const float scale_factor, const unsigned int num_levels, - const unsigned int ini_fast_thr, const unsigned int min_fast_thr, - const std::vector>& mask_rects = {}); + orb_params(const std::string& name, const float scale_factor, const unsigned int num_levels, + const unsigned int ini_fast_thr, const unsigned int min_fast_thr); + orb_params(const std::string& name); //! Constructor explicit orb_params(const YAML::Node& yaml_node); @@ -20,14 +22,23 @@ struct orb_params { //! Destructor virtual ~orb_params() = default; - float scale_factor_ = 1.2; - unsigned int num_levels_ = 8; - unsigned int ini_fast_thr_ = 20; - unsigned int min_fast_thr = 7; + nlohmann::json to_json() const; - //! A vector of keypoint area represents mask area - //! Each areas are denoted as form of [x_min / cols, x_max / cols, y_min / rows, y_max / rows] - std::vector> mask_rects_; + //! name (id for saving) + const std::string name_; + + const float scale_factor_ = 1.2; + const float log_scale_factor_ = std::log(1.2); + const unsigned int num_levels_ = 8; + const unsigned int ini_fast_thr_ = 20; + const unsigned int min_fast_thr_ = 7; + + //! A list of the scale factor of each pyramid layer + std::vector scale_factors_; + std::vector inv_scale_factors_; + //! A list of the sigma of each pyramid layer + std::vector level_sigma_sq_; + std::vector inv_level_sigma_sq_; //! Calculate scale factors static std::vector calc_scale_factors(const unsigned int num_scale_levels, const float scale_factor); diff --git a/src/openvslam/global_optimization_module.cc b/src/openvslam/global_optimization_module.cc index 1afed0d35..7982090e9 100644 --- a/src/openvslam/global_optimization_module.cc +++ b/src/openvslam/global_optimization_module.cc @@ -132,7 +132,7 @@ void global_optimization_module::queue_keyframe(const std::shared_ptr lock(mtx_keyfrm_queue_); - return (!keyfrms_queue_.empty()); + return !keyfrms_queue_.empty(); } void global_optimization_module::correct_loop() { @@ -146,15 +146,13 @@ void global_optimization_module::correct_loop() { // 0-1. stop the mapping module and the previous loop bundle adjuster // pause the mapping module - mapper_->request_pause(); + auto future_pause = mapper_->async_pause(); // abort the previous loop bundle adjuster if (thread_for_loop_BA_ || loop_bundle_adjuster_->is_running()) { abort_loop_BA(); } // wait till the mapping module pauses - while (!mapper_->is_paused()) { - std::this_thread::sleep_for(std::chrono::milliseconds(2)); - } + future_pause.get(); // 0-2. update the graph @@ -173,6 +171,7 @@ void global_optimization_module::correct_loop() { // Sim3 camera poses AFTER loop correction module::keyframe_Sim3_pairs_t Sim3s_nw_after_correction; + std::unordered_map found_lm_to_ref_keyfrm_id; const auto g2o_Sim3_cw_after_correction = loop_detector_->get_Sim3_world_to_current(); { std::lock_guard lock(data::map_database::mtx_database_); @@ -186,7 +185,7 @@ void global_optimization_module::correct_loop() { Sim3s_nw_after_correction = get_Sim3s_after_loop_correction(cam_pose_wc_before_correction, g2o_Sim3_cw_after_correction, curr_neighbors); // correct covibisibility landmark positions - correct_covisibility_landmarks(Sim3s_nw_before_correction, Sim3s_nw_after_correction); + correct_covisibility_landmarks(Sim3s_nw_before_correction, Sim3s_nw_after_correction, found_lm_to_ref_keyfrm_id); // correct covisibility keyframe camera poses correct_covisibility_keyframes(Sim3s_nw_after_correction); } @@ -202,7 +201,7 @@ void global_optimization_module::correct_loop() { // 4. pose graph optimization - graph_optimizer_->optimize(final_candidate_keyfrm, cur_keyfrm_, Sim3s_nw_before_correction, Sim3s_nw_after_correction, new_connections); + graph_optimizer_->optimize(final_candidate_keyfrm, cur_keyfrm_, Sim3s_nw_before_correction, Sim3s_nw_after_correction, new_connections, found_lm_to_ref_keyfrm_id); // add a loop edge final_candidate_keyfrm->graph_node_->add_loop_edge(cur_keyfrm_); @@ -217,7 +216,7 @@ void global_optimization_module::correct_loop() { thread_for_loop_BA_->join(); thread_for_loop_BA_.reset(nullptr); } - thread_for_loop_BA_ = std::unique_ptr(new std::thread(&module::loop_bundle_adjuster::optimize, loop_bundle_adjuster_.get(), cur_keyfrm_->id_)); + thread_for_loop_BA_ = std::unique_ptr(new std::thread(&module::loop_bundle_adjuster::optimize, loop_bundle_adjuster_.get())); // 6. post-processing @@ -266,7 +265,8 @@ module::keyframe_Sim3_pairs_t global_optimization_module::get_Sim3s_after_loop_c } void global_optimization_module::correct_covisibility_landmarks(const module::keyframe_Sim3_pairs_t& Sim3s_nw_before_correction, - const module::keyframe_Sim3_pairs_t& Sim3s_nw_after_correction) const { + const module::keyframe_Sim3_pairs_t& Sim3s_nw_after_correction, + std::unordered_map& found_lm_to_ref_keyfrm_id) const { for (const auto& t : Sim3s_nw_after_correction) { auto neighbor = t.first; // neighbor->world AFTER loop correction @@ -284,20 +284,18 @@ void global_optimization_module::correct_covisibility_landmarks(const module::ke } // avoid duplication - if (lm->loop_fusion_identifier_ == cur_keyfrm_->id_) { + if (found_lm_to_ref_keyfrm_id.count(lm->id_)) { continue; } - lm->loop_fusion_identifier_ = cur_keyfrm_->id_; + // record the reference keyframe used in loop fusion of landmarks + found_lm_to_ref_keyfrm_id[lm->id_] = neighbor->id_; // correct position of `lm` const Vec3_t pos_w_before_correction = lm->get_pos_in_world(); const Vec3_t pos_w_after_correction = Sim3_wn_after_correction.map(Sim3_nw_before_correction.map(pos_w_before_correction)); lm->set_pos_in_world(pos_w_after_correction); // update geometry - lm->update_normal_and_depth(); - - // record the reference keyframe used in loop fusion of landmarks - lm->ref_keyfrm_id_in_loop_fusion_ = neighbor->id_; + lm->update_mean_normal_and_obs_scale_variance(); } } } @@ -324,7 +322,7 @@ void global_optimization_module::replace_duplicated_landmarks(const std::vector< { std::lock_guard lock(data::map_database::mtx_database_); - for (unsigned int idx = 0; idx < cur_keyfrm_->num_keypts_; ++idx) { + for (unsigned int idx = 0; idx < cur_keyfrm_->frm_obs_.num_keypts_; ++idx) { auto curr_match_lm_in_cand = curr_match_lms_observed_in_cand.at(idx); if (!curr_match_lm_in_cand) { continue; @@ -395,22 +393,11 @@ auto global_optimization_module::extract_new_connections(const std::vector lock(mtx_reset_); - reset_is_requested_ = true; - } - - // BLOCK until reset - while (true) { - { - std::lock_guard lock(mtx_reset_); - if (!reset_is_requested_) { - break; - } - } - std::this_thread::sleep_for(std::chrono::microseconds(3000)); - } +std::future global_optimization_module::async_reset() { + std::lock_guard lock(mtx_reset_); + reset_is_requested_ = true; + promises_reset_.emplace_back(); + return promises_reset_.back().get_future(); } bool global_optimization_module::reset_is_requested() const { @@ -424,11 +411,17 @@ void global_optimization_module::reset() { keyfrms_queue_.clear(); loop_detector_->set_loop_correct_keyframe_id(0); reset_is_requested_ = false; + for (auto& promise : promises_reset_) { + promise.set_value(); + } + promises_reset_.clear(); } -void global_optimization_module::request_pause() { +std::future global_optimization_module::async_pause() { std::lock_guard lock1(mtx_pause_); pause_is_requested_ = true; + promises_pause_.emplace_back(); + return promises_pause_.back().get_future(); } bool global_optimization_module::pause_is_requested() const { @@ -445,6 +438,10 @@ void global_optimization_module::pause() { std::lock_guard lock(mtx_pause_); spdlog::info("pause global optimization module"); is_paused_ = true; + for (auto& promise : promises_pause_) { + promise.set_value(); + } + promises_pause_.clear(); } void global_optimization_module::resume() { @@ -462,9 +459,11 @@ void global_optimization_module::resume() { spdlog::info("resume global optimization module"); } -void global_optimization_module::request_terminate() { +std::future global_optimization_module::async_terminate() { std::lock_guard lock(mtx_terminate_); terminate_is_requested_ = true; + promises_terminate_.emplace_back(); + return promises_terminate_.back().get_future(); } bool global_optimization_module::is_terminated() const { @@ -480,6 +479,10 @@ bool global_optimization_module::terminate_is_requested() const { void global_optimization_module::terminate() { std::lock_guard lock(mtx_terminate_); is_terminated_ = true; + for (auto& promise : promises_terminate_) { + promise.set_value(); + } + promises_terminate_.clear(); } bool global_optimization_module::loop_BA_is_running() const { diff --git a/src/openvslam/global_optimization_module.h b/src/openvslam/global_optimization_module.h index 779924980..8066c474e 100644 --- a/src/openvslam/global_optimization_module.h +++ b/src/openvslam/global_optimization_module.h @@ -12,6 +12,7 @@ #include #include #include +#include namespace openvslam { @@ -63,15 +64,13 @@ class global_optimization_module { // management for reset process //! Request to reset the global optimization module - //! (NOTE: this function waits for reset) - void request_reset(); + std::future async_reset(); //----------------------------------------- // management for pause process //! Request to pause the global optimization module - //! (NOTE: this function does not wait for pause) - void request_pause(); + std::future async_pause(); //! Check if the global optimization module is requested to be paused or not bool pause_is_requested() const; @@ -86,8 +85,7 @@ class global_optimization_module { // management for terminate process //! Request to terminate the global optimization module - //! (NOTE: this function does not wait for terminate) - void request_terminate(); + std::future async_terminate(); //! Check if the global optimization module is terminated or not bool is_terminated() const; @@ -118,7 +116,8 @@ class global_optimization_module { //! Correct the positions of the landmarks which are seen in covisibilities void correct_covisibility_landmarks(const module::keyframe_Sim3_pairs_t& Sim3s_nw_before_correction, - const module::keyframe_Sim3_pairs_t& Sim3s_nw_after_correction) const; + const module::keyframe_Sim3_pairs_t& Sim3s_nw_after_correction, + std::unordered_map& found_lm_to_ref_keyfrm_id) const; //! Correct the camera poses of the covisibilities void correct_covisibility_keyframes(const module::keyframe_Sim3_pairs_t& Sim3s_nw_after_correction) const; @@ -136,6 +135,9 @@ class global_optimization_module { //! mutex for access to reset procedure mutable std::mutex mtx_reset_; + //! promises for reset + std::vector> promises_reset_; + //! Check and execute reset bool reset_is_requested() const; @@ -151,6 +153,9 @@ class global_optimization_module { //! mutex for access to pause procedure mutable std::mutex mtx_pause_; + //! promises for pause + std::vector> promises_pause_; + //! Pause the global optimizer void pause(); @@ -165,6 +170,9 @@ class global_optimization_module { //! mutex for access to terminate procedure mutable std::mutex mtx_terminate_; + //! promises for terminate + std::vector> promises_terminate_; + //! Check if termination is requested or not bool terminate_is_requested() const; diff --git a/src/openvslam/initialize/base.cc b/src/openvslam/initialize/base.cc index a5db5fb2e..bd73f1c9c 100644 --- a/src/openvslam/initialize/base.cc +++ b/src/openvslam/initialize/base.cc @@ -8,7 +8,7 @@ namespace initialize { base::base(const data::frame& ref_frm, const unsigned int num_ransac_iters, const unsigned int min_num_triangulated, const float parallax_deg_thr, const float reproj_err_thr) - : ref_camera_(ref_frm.camera_), ref_undist_keypts_(ref_frm.undist_keypts_), ref_bearings_(ref_frm.bearings_), + : ref_camera_(ref_frm.camera_), ref_undist_keypts_(ref_frm.frm_obs_.undist_keypts_), ref_bearings_(ref_frm.frm_obs_.bearings_), num_ransac_iters_(num_ransac_iters), min_num_triangulated_(min_num_triangulated), parallax_deg_thr_(parallax_deg_thr), reproj_err_thr_(reproj_err_thr) {} diff --git a/src/openvslam/initialize/bearing_vector.cc b/src/openvslam/initialize/bearing_vector.cc index fa65bc245..95a3b2106 100644 --- a/src/openvslam/initialize/bearing_vector.cc +++ b/src/openvslam/initialize/bearing_vector.cc @@ -24,11 +24,11 @@ bool bearing_vector::initialize(const data::frame& cur_frm, const std::vector& // set the current camera model cur_camera_ = cur_frm.camera_; // store the keypoints and bearings - cur_undist_keypts_ = cur_frm.undist_keypts_; - cur_bearings_ = cur_frm.bearings_; + cur_undist_keypts_ = cur_frm.frm_obs_.undist_keypts_; + cur_bearings_ = cur_frm.frm_obs_.bearings_; // align matching information ref_cur_matches_.clear(); - ref_cur_matches_.reserve(cur_frm.undist_keypts_.size()); + ref_cur_matches_.reserve(cur_frm.frm_obs_.undist_keypts_.size()); for (unsigned int ref_idx = 0; ref_idx < ref_matches_with_cur.size(); ++ref_idx) { const auto cur_idx = ref_matches_with_cur.at(ref_idx); if (0 <= cur_idx) { diff --git a/src/openvslam/io/map_database_io.cc b/src/openvslam/io/map_database_io.cc index 6f3f99936..f0e2c4ddf 100644 --- a/src/openvslam/io/map_database_io.cc +++ b/src/openvslam/io/map_database_io.cc @@ -2,6 +2,7 @@ #include "openvslam/data/keyframe.h" #include "openvslam/data/landmark.h" #include "openvslam/data/camera_database.h" +#include "openvslam/data/orb_params_database.h" #include "openvslam/data/bow_database.h" #include "openvslam/data/map_database.h" #include "openvslam/io/map_database_io.h" @@ -14,20 +15,22 @@ namespace openvslam { namespace io { -map_database_io::map_database_io(data::camera_database* cam_db, data::map_database* map_db, +map_database_io::map_database_io(data::camera_database* cam_db, data::orb_params_database* orb_params_db, data::map_database* map_db, data::bow_database* bow_db, data::bow_vocabulary* bow_vocab) - : cam_db_(cam_db), map_db_(map_db), bow_db_(bow_db), bow_vocab_(bow_vocab) {} + : cam_db_(cam_db), orb_params_db_(orb_params_db), map_db_(map_db), bow_db_(bow_db), bow_vocab_(bow_vocab) {} void map_database_io::save_message_pack(const std::string& path) { std::lock_guard lock(data::map_database::mtx_database_); - assert(cam_db_ && map_db_); + assert(cam_db_ && orb_params_db_ && map_db_); const auto cameras = cam_db_->to_json(); + const auto orb_params = orb_params_db_->to_json(); nlohmann::json keyfrms; nlohmann::json landmarks; map_db_->to_json(keyfrms, landmarks); nlohmann::json json{{"cameras", cameras}, + {"orb_params", orb_params}, {"keyframes", keyfrms}, {"landmarks", landmarks}, {"frame_next_id", static_cast(data::frame::next_id_)}, @@ -52,7 +55,7 @@ void map_database_io::load_message_pack(const std::string& path) { // 1. initialize database - assert(cam_db_ && map_db_ && bow_db_ && bow_vocab_); + assert(cam_db_ && orb_params_db_ && map_db_ && bow_db_ && bow_vocab_); map_db_->clear(); bow_db_->clear(); @@ -89,9 +92,11 @@ void map_database_io::load_message_pack(const std::string& path) { // load database const auto json_cameras = json.at("cameras"); cam_db_->from_json(json_cameras); + const auto json_orb_params = json.at("orb_params"); + orb_params_db_->from_json(json_orb_params); const auto json_keyfrms = json.at("keyframes"); const auto json_landmarks = json.at("landmarks"); - map_db_->from_json(cam_db_, bow_vocab_, bow_db_, json_keyfrms, json_landmarks); + map_db_->from_json(cam_db_, orb_params_db_, bow_vocab_, json_keyfrms, json_landmarks); const auto keyfrms = map_db_->get_all_keyframes(); for (const auto& keyfrm : keyfrms) { bow_db_->add_keyframe(keyfrm); diff --git a/src/openvslam/io/map_database_io.h b/src/openvslam/io/map_database_io.h index 4d37c1737..bfc998b8f 100644 --- a/src/openvslam/io/map_database_io.h +++ b/src/openvslam/io/map_database_io.h @@ -20,8 +20,8 @@ class map_database_io { /** * Constructor */ - map_database_io(data::camera_database* cam_db, data::map_database* map_db, - data::bow_database* bow_db, data::bow_vocabulary* bow_vocab); + map_database_io(data::camera_database* cam_db, data::orb_params_database* orb_params_db, + data::map_database* map_db, data::bow_database* bow_db, data::bow_vocabulary* bow_vocab); /** * Destructor @@ -41,6 +41,8 @@ class map_database_io { private: //! camera database data::camera_database* const cam_db_ = nullptr; + //! orb_params database + data::orb_params_database* const orb_params_db_ = nullptr; //! map_database data::map_database* const map_db_ = nullptr; //! BoW database diff --git a/src/openvslam/io/trajectory_io.cc b/src/openvslam/io/trajectory_io.cc index a920fb89c..9c3d80066 100644 --- a/src/openvslam/io/trajectory_io.cc +++ b/src/openvslam/io/trajectory_io.cc @@ -1,6 +1,7 @@ #include "openvslam/data/keyframe.h" #include "openvslam/data/map_database.h" #include "openvslam/io/trajectory_io.h" +#include "openvslam/util/converter.h" #include #include @@ -77,14 +78,7 @@ void trajectory_io::save_frame_trajectory(const std::string& path, const std::st const Mat44_t rel_cam_pose_cr = rc_itr->second; const Mat44_t cam_pose_cw = rel_cam_pose_cr * cam_pose_rw; - - const Mat33_t rot_cw = cam_pose_cw.block<3, 3>(0, 0); - const Vec3_t trans_cw = cam_pose_cw.block<3, 1>(0, 3); - const Mat33_t rot_wc = rot_cw.transpose(); - const Vec3_t cam_center = -rot_wc * trans_cw; - Mat44_t cam_pose_wc = Mat44_t::Identity(); - cam_pose_wc.block<3, 3>(0, 0) = rot_wc; - cam_pose_wc.block<3, 1>(0, 3) = cam_center; + Mat44_t cam_pose_wc = util::converter::inverse_pose(cam_pose_cw); if (format == "KITTI") { ofs << std::setprecision(9) diff --git a/src/openvslam/mapping_module.cc b/src/openvslam/mapping_module.cc index a7bb8ac9b..a741aa9e5 100644 --- a/src/openvslam/mapping_module.cc +++ b/src/openvslam/mapping_module.cc @@ -16,8 +16,9 @@ namespace openvslam { -mapping_module::mapping_module(const YAML::Node& yaml_node, data::map_database* map_db) - : local_map_cleaner_(new module::local_map_cleaner(yaml_node["redundant_obs_ratio_thr"].as(0.9))), map_db_(map_db), +mapping_module::mapping_module(const YAML::Node& yaml_node, data::map_database* map_db, data::bow_database* bow_db, data::bow_vocabulary* bow_vocab) + : local_map_cleaner_(new module::local_map_cleaner(map_db, bow_db, yaml_node["redundant_obs_ratio_thr"].as(0.9))), + map_db_(map_db), bow_db_(bow_db), bow_vocab_(bow_vocab), local_bundle_adjuster_(new optimize::local_bundle_adjuster()) { spdlog::debug("CONSTRUCT: mapping_module"); spdlog::debug("load mapping parameters"); @@ -54,14 +55,12 @@ void mapping_module::run() { spdlog::info("start mapping module"); is_terminated_ = false; + set_is_idle(true); while (true) { // waiting time for the other threads std::this_thread::sleep_for(std::chrono::milliseconds(5)); - // LOCK - set_keyframe_acceptability(false); - // check if termination is requested if (terminate_is_requested()) { // terminate and break @@ -69,8 +68,9 @@ void mapping_module::run() { break; } - // check if pause is requested - if (pause_is_requested()) { + // check if pause is requested and not prevented + if (pause_is_requested_and_not_prevented()) { + set_is_idle(false); // if any keyframe is queued, all of them must be processed before the pause while (keyframe_is_queued()) { // create and extend the map with the new keyframe @@ -78,6 +78,7 @@ void mapping_module::run() { // send the new keyframe to the global optimization module global_optimizer_->queue_keyframe(cur_keyfrm_); } + set_is_idle(true); // pause and wait pause(); // check if termination or reset is requested during pause @@ -88,26 +89,23 @@ void mapping_module::run() { // check if reset is requested if (reset_is_requested()) { - // reset, UNLOCK and continue + // reset and continue reset(); - set_keyframe_acceptability(true); + set_is_idle(true); continue; } // if the queue is empty, the following process is not needed if (!keyframe_is_queued()) { - // UNLOCK and continue - set_keyframe_acceptability(true); + set_is_idle(true); continue; } + set_is_idle(false); // create and extend the map with the new keyframe mapping_with_new_keyframe(); // send the new keyframe to the global optimization module global_optimizer_->queue_keyframe(cur_keyfrm_); - - // LOCK end - set_keyframe_acceptability(true); } spdlog::info("terminate mapping module"); @@ -129,12 +127,17 @@ bool mapping_module::keyframe_is_queued() const { return !keyfrms_queue_.empty(); } -bool mapping_module::get_keyframe_acceptability() const { - return keyfrm_acceptability_; +bool mapping_module::is_idle() const { + return is_idle_; +} + +void mapping_module::set_is_idle(const bool is_idle) { + is_idle_ = is_idle; } -void mapping_module::set_keyframe_acceptability(const bool acceptability) { - keyfrm_acceptability_ = acceptability; +bool mapping_module::is_skipping_localBA() const { + auto queued_keyframes = get_num_queued_keyframes(); + return queued_keyframes >= queue_threshold_; } void mapping_module::abort_local_BA() { @@ -175,15 +178,23 @@ void mapping_module::mapping_with_new_keyframe() { // local bundle adjustment abort_local_BA_ = false; + // If the processing speed is insufficient, skip localBA. if (2 < map_db_->get_num_keyframes()) { - local_bundle_adjuster_->optimize(cur_keyfrm_, &abort_local_BA_); + if (is_skipping_localBA()) { + spdlog::debug("Skipped localBA due to insufficient performance"); + } + else { + local_bundle_adjuster_->optimize(map_db_, cur_keyfrm_, &abort_local_BA_); + } } local_map_cleaner_->remove_redundant_keyframes(cur_keyfrm_); } void mapping_module::store_new_keyframe() { // compute BoW feature vector - cur_keyfrm_->compute_bow(); + if (!cur_keyfrm_->bow_is_available()) { + cur_keyfrm_->compute_bow(bow_vocab_); + } // update graph const auto cur_lms = cur_keyfrm_->get_landmarks(); @@ -207,7 +218,7 @@ void mapping_module::store_new_keyframe() { // update connection lm->add_observation(cur_keyfrm_, idx); // update geometry - lm->update_normal_and_depth(); + lm->update_mean_normal_and_obs_scale_variance(); lm->compute_descriptor(); } cur_keyfrm_->graph_node_->update_connections(); @@ -304,7 +315,7 @@ void mapping_module::triangulate_with_two_keyframes(const std::shared_ptradd_landmark(lm, idx_2); lm->compute_descriptor(); - lm->update_normal_and_depth(); + lm->update_mean_normal_and_obs_scale_variance(); map_db_->add_landmark(lm); // wait for redundancy check @@ -335,7 +346,7 @@ void mapping_module::update_new_keyframe() { continue; } lm->compute_descriptor(); - lm->update_normal_and_depth(); + lm->update_mean_normal_and_obs_scale_variance(); } // update the graph @@ -399,7 +410,7 @@ void mapping_module::fuse_landmark_duplication(const std::unordered_set> candidate_landmarks_to_fuse; - candidate_landmarks_to_fuse.reserve(fuse_tgt_keyfrms.size() * cur_keyfrm_->num_keypts_); + candidate_landmarks_to_fuse.reserve(fuse_tgt_keyfrms.size() * cur_keyfrm_->frm_obs_.num_keypts_); for (const auto& fuse_tgt_keyfrm : fuse_tgt_keyfrms) { const auto fuse_tgt_landmarks = fuse_tgt_keyfrm->get_landmarks(); @@ -423,22 +434,11 @@ void mapping_module::fuse_landmark_duplication(const std::unordered_set lock(mtx_reset_); - reset_is_requested_ = true; - } - - // BLOCK until reset - while (true) { - { - std::lock_guard lock(mtx_reset_); - if (!reset_is_requested_) { - break; - } - } - std::this_thread::sleep_for(std::chrono::microseconds(3000)); - } +std::future mapping_module::async_reset() { + std::lock_guard lock(mtx_reset_); + reset_is_requested_ = true; + promises_reset_.emplace_back(); + return promises_reset_.back().get_future(); } bool mapping_module::reset_is_requested() const { @@ -452,13 +452,19 @@ void mapping_module::reset() { keyfrms_queue_.clear(); local_map_cleaner_->reset(); reset_is_requested_ = false; + for (auto& promise : promises_reset_) { + promise.set_value(); + } + promises_reset_.clear(); } -void mapping_module::request_pause() { +std::future mapping_module::async_pause() { std::lock_guard lock1(mtx_pause_); pause_is_requested_ = true; std::lock_guard lock2(mtx_keyfrm_queue_); abort_local_BA_ = true; + promises_pause_.emplace_back(); + return promises_pause_.back().get_future(); } bool mapping_module::is_paused() const { @@ -466,26 +472,37 @@ bool mapping_module::is_paused() const { return is_paused_; } +bool mapping_module::pause_is_requested_and_not_prevented() const { + std::lock_guard lock(mtx_pause_); + return pause_is_requested_ && !prevent_pause_; +} + bool mapping_module::pause_is_requested() const { std::lock_guard lock(mtx_pause_); - return pause_is_requested_ && !force_to_run_; + return pause_is_requested_; } void mapping_module::pause() { std::lock_guard lock(mtx_pause_); spdlog::info("pause mapping module"); is_paused_ = true; + for (auto& promise : promises_pause_) { + promise.set_value(); + } + promises_pause_.clear(); } -bool mapping_module::set_force_to_run(const bool force_to_run) { +bool mapping_module::prevent_pause_if_not_paused() { std::lock_guard lock(mtx_pause_); - - if (force_to_run && is_paused_) { - return false; + if (!is_paused_) { + prevent_pause_ = true; } + return prevent_pause_; +} - force_to_run_ = force_to_run; - return true; +void mapping_module::stop_prevent_pause() { + std::lock_guard lock(mtx_pause_); + prevent_pause_ = false; } void mapping_module::resume() { @@ -497,18 +514,19 @@ void mapping_module::resume() { return; } + assert(keyfrms_queue_.empty()); + is_paused_ = false; pause_is_requested_ = false; - // clear the queue - keyfrms_queue_.clear(); - spdlog::info("resume mapping module"); } -void mapping_module::request_terminate() { +std::future mapping_module::async_terminate() { std::lock_guard lock(mtx_terminate_); terminate_is_requested_ = true; + promises_terminate_.emplace_back(); + return promises_terminate_.back().get_future(); } bool mapping_module::is_terminated() const { @@ -526,6 +544,11 @@ void mapping_module::terminate() { std::lock_guard lock2(mtx_terminate_); is_paused_ = true; is_terminated_ = true; + set_is_idle(true); + for (auto& promise : promises_terminate_) { + promise.set_value(); + } + promises_terminate_.clear(); } } // namespace openvslam diff --git a/src/openvslam/mapping_module.h b/src/openvslam/mapping_module.h index d73f63a00..0ec6d47a7 100644 --- a/src/openvslam/mapping_module.h +++ b/src/openvslam/mapping_module.h @@ -5,10 +5,12 @@ #include "openvslam/camera/base.h" #include "openvslam/module/local_map_cleaner.h" #include "openvslam/optimize/local_bundle_adjuster.h" +#include "openvslam/data/bow_vocabulary_fwd.h" #include #include #include +#include #include @@ -24,13 +26,14 @@ class base; namespace data { class keyframe; +class bow_database; class map_database; } // namespace data class mapping_module { public: //! Constructor - mapping_module(const YAML::Node& yaml_node, data::map_database* map_db); + mapping_module(const YAML::Node& yaml_node, data::map_database* map_db, data::bow_database* bow_db, data::bow_vocabulary* bow_vocab); //! Destructor ~mapping_module(); @@ -53,25 +56,23 @@ class mapping_module { //! Get the number of queued keyframes unsigned int get_num_queued_keyframes() const; - //! Get keyframe acceptability - bool get_keyframe_acceptability() const; + //! True when no keyframes are being processed + bool is_idle() const; - //! Set keyframe acceptability - void set_keyframe_acceptability(const bool acceptability); + //! If the size of the queue exceeds this threshold, skip the localBA + bool is_skipping_localBA() const; //----------------------------------------- // management for reset process //! Request to reset the mapping module - //! (NOTE: this function waits for reset) - void request_reset(); + std::future async_reset(); //----------------------------------------- // management for pause process //! Request to pause the mapping module - //! (NOTE: this function does not wait for reset) - void request_pause(); + std::future async_pause(); //! Check if the mapping module is requested to be paused or not bool pause_is_requested() const; @@ -79,8 +80,11 @@ class mapping_module { //! Check if the mapping module is paused or not bool is_paused() const; - //! Set the flag to force to run the mapping module - bool set_force_to_run(const bool force_to_run); + //! If it is not paused, prevent it from being paused + bool prevent_pause_if_not_paused(); + + //! Stop preventing it from pausing + void stop_prevent_pause(); //! Resume the mapping module void resume(); @@ -89,8 +93,7 @@ class mapping_module { // management for terminate process //! Request to terminate the mapping module - //! (NOTE: this function does not wait for terminate) - void request_terminate(); + std::future async_terminate(); //! Check if the mapping module is terminated or not bool is_terminated() const; @@ -129,12 +132,21 @@ class mapping_module { //! Fuse duplicated landmarks between current keyframe and covisibility keyframes void fuse_landmark_duplication(const std::unordered_set>& fuse_tgt_keyfrms); + //! Check if pause is requested and not prevented + bool pause_is_requested_and_not_prevented() const; + + //! Set is_idle (True when no keyframes are being processed.) + void set_is_idle(const bool is_idle); + //----------------------------------------- // management for reset process //! mutex for access to reset procedure mutable std::mutex mtx_reset_; + //! promises for reset + std::vector> promises_reset_; + //! Check and execute reset bool reset_is_requested() const; @@ -150,6 +162,9 @@ class mapping_module { //! mutex for access to pause procedure mutable std::mutex mtx_pause_; + //! promises for pause + std::vector> promises_pause_; + //! Pause the mapping module void pause(); @@ -158,7 +173,7 @@ class mapping_module { //! flag which indicates whether the main loop is paused or not bool is_paused_ = false; //! flag to force the mapping module to be run - bool force_to_run_ = false; + bool prevent_pause_ = false; //----------------------------------------- // management for terminate process @@ -166,6 +181,9 @@ class mapping_module { //! mutex for access to terminate procedure mutable std::mutex mtx_terminate_; + //! promises for terminate + std::vector> promises_terminate_; + //! Check if termination is requested or not bool terminate_is_requested() const; @@ -194,6 +212,12 @@ class mapping_module { //! map database data::map_database* map_db_ = nullptr; + //! BoW database + data::bow_database* bow_db_ = nullptr; + + //! BoW vocabulary + data::bow_vocabulary* bow_vocab_ = nullptr; + //----------------------------------------- // keyframe queue @@ -218,8 +242,8 @@ class mapping_module { //----------------------------------------- // others - //! flag for keyframe acceptability - std::atomic keyfrm_acceptability_{true}; + //! True when no keyframes are being processed + std::atomic is_idle_{true}; //! current keyframe which is used in the current mapping std::shared_ptr cur_keyfrm_ = nullptr; @@ -235,6 +259,9 @@ class mapping_module { //! Create new landmarks if the baseline distance is greater than baseline_dist_thr_ of the reference keyframe. double baseline_dist_thr_ = 1.0; + + //! If the size of the queue exceeds this threshold, skip the localBA + const unsigned int queue_threshold_ = 2; }; } // namespace openvslam diff --git a/src/openvslam/match/area.cc b/src/openvslam/match/area.cc index 2316ee10a..c67bd3c0b 100644 --- a/src/openvslam/match/area.cc +++ b/src/openvslam/match/area.cc @@ -11,13 +11,13 @@ unsigned int area::match_in_consistent_area(data::frame& frm_1, data::frame& frm angle_checker angle_checker; - matched_indices_2_in_frm_1 = std::vector(frm_1.undist_keypts_.size(), -1); + matched_indices_2_in_frm_1 = std::vector(frm_1.frm_obs_.undist_keypts_.size(), -1); - std::vector matched_dists_in_frm_2(frm_2.undist_keypts_.size(), MAX_HAMMING_DIST); - std::vector matched_indices_1_in_frm_2(frm_2.undist_keypts_.size(), -1); + std::vector matched_dists_in_frm_2(frm_2.frm_obs_.undist_keypts_.size(), MAX_HAMMING_DIST); + std::vector matched_indices_1_in_frm_2(frm_2.frm_obs_.undist_keypts_.size(), -1); - for (unsigned int idx_1 = 0; idx_1 < frm_1.undist_keypts_.size(); ++idx_1) { - const auto& undist_keypt_1 = frm_1.undist_keypts_.at(idx_1); + for (unsigned int idx_1 = 0; idx_1 < frm_1.frm_obs_.undist_keypts_.size(); ++idx_1) { + const auto& undist_keypt_1 = frm_1.frm_obs_.undist_keypts_.at(idx_1); const auto scale_level_1 = undist_keypt_1.octave; // Use only keypoints with the 0-th scale @@ -32,14 +32,14 @@ unsigned int area::match_in_consistent_area(data::frame& frm_1, data::frame& frm continue; } - const auto& desc_1 = frm_1.descriptors_.row(idx_1); + const auto& desc_1 = frm_1.frm_obs_.descriptors_.row(idx_1); unsigned int best_hamm_dist = MAX_HAMMING_DIST; unsigned int second_best_hamm_dist = MAX_HAMMING_DIST; int best_idx_2 = -1; for (const auto idx_2 : indices) { - const auto& desc_2 = frm_2.descriptors_.row(idx_2); + const auto& desc_2 = frm_2.frm_obs_.descriptors_.row(idx_2); const auto hamm_dist = compute_descriptor_distance_32(desc_1, desc_2); @@ -86,7 +86,7 @@ unsigned int area::match_in_consistent_area(data::frame& frm_1, data::frame& frm if (check_orientation_) { const auto delta_angle - = frm_1.undist_keypts_.at(idx_1).angle - frm_2.undist_keypts_.at(best_idx_2).angle; + = frm_1.frm_obs_.undist_keypts_.at(idx_1).angle - frm_2.frm_obs_.undist_keypts_.at(best_idx_2).angle; angle_checker.append_delta_angle(delta_angle, idx_1); } } @@ -104,7 +104,7 @@ unsigned int area::match_in_consistent_area(data::frame& frm_1, data::frame& frm // Update the previous matches for (unsigned int idx_1 = 0; idx_1 < matched_indices_2_in_frm_1.size(); ++idx_1) { if (0 <= matched_indices_2_in_frm_1.at(idx_1)) { - prev_matched_pts.at(idx_1) = frm_2.undist_keypts_.at(matched_indices_2_in_frm_1.at(idx_1)).pt; + prev_matched_pts.at(idx_1) = frm_2.frm_obs_.undist_keypts_.at(matched_indices_2_in_frm_1.at(idx_1)).pt; } } diff --git a/src/openvslam/match/bow_tree.cc b/src/openvslam/match/bow_tree.cc index d0f8412c8..767879390 100644 --- a/src/openvslam/match/bow_tree.cc +++ b/src/openvslam/match/bow_tree.cc @@ -18,7 +18,7 @@ unsigned int bow_tree::match_frame_and_keyframe(const std::shared_ptr angle_checker; - matched_lms_in_frm = std::vector>(frm.num_keypts_, nullptr); + matched_lms_in_frm = std::vector>(frm.frm_obs_.num_keypts_, nullptr); const auto keyfrm_lms = keyfrm->get_landmarks(); @@ -52,7 +52,7 @@ unsigned int bow_tree::match_frame_and_keyframe(const std::shared_ptrdescriptors_.row(keyfrm_idx); + const auto& keyfrm_desc = keyfrm->frm_obs_.descriptors_.row(keyfrm_idx); unsigned int best_hamm_dist = MAX_HAMMING_DIST; int best_frm_idx = -1; @@ -63,7 +63,7 @@ unsigned int bow_tree::match_frame_and_keyframe(const std::shared_ptrkeypts_.at(keyfrm_idx).angle - frm.keypts_.at(best_frm_idx).angle; + = keyfrm->frm_obs_.keypts_.at(keyfrm_idx).angle - frm.frm_obs_.keypts_.at(best_frm_idx).angle; angle_checker.append_delta_angle(delta_angle, best_frm_idx); } @@ -166,7 +166,7 @@ unsigned int bow_tree::match_keyframes(const std::shared_ptr& ke continue; } - const auto& desc_1 = keyfrm_1->descriptors_.row(idx_1); + const auto& desc_1 = keyfrm_1->frm_obs_.descriptors_.row(idx_1); unsigned int best_hamm_dist = MAX_HAMMING_DIST; int best_idx_2 = -1; @@ -187,7 +187,7 @@ unsigned int bow_tree::match_keyframes(const std::shared_ptr& ke continue; } - const auto& desc_2 = keyfrm_2->descriptors_.row(idx_2); + const auto& desc_2 = keyfrm_2->frm_obs_.descriptors_.row(idx_2); const auto hamm_dist = compute_descriptor_distance_32(desc_1, desc_2); @@ -220,7 +220,7 @@ unsigned int bow_tree::match_keyframes(const std::shared_ptr& ke if (check_orientation_) { const auto delta_angle - = keyfrm_1->keypts_.at(idx_1).angle - keyfrm_2->keypts_.at(best_idx_2).angle; + = keyfrm_1->frm_obs_.keypts_.at(idx_1).angle - keyfrm_2->frm_obs_.keypts_.at(best_idx_2).angle; angle_checker.append_delta_angle(delta_angle, idx_1); } } diff --git a/src/openvslam/match/fuse.cc b/src/openvslam/match/fuse.cc index 6798a7c37..27fee01a0 100644 --- a/src/openvslam/match/fuse.cc +++ b/src/openvslam/match/fuse.cc @@ -66,8 +66,8 @@ unsigned int fuse::detect_duplication(const std::shared_ptr& key } // Acquire keypoints in the cell where the reprojected 3D points exist - const int pred_scale_level = lm->predict_scale_level(cam_to_lm_dist, keyfrm); - const auto indices = keyfrm->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm->scale_factors_.at(pred_scale_level)); + const int pred_scale_level = lm->predict_scale_level(cam_to_lm_dist, keyfrm->orb_params_->num_levels_, keyfrm->orb_params_->log_scale_factor_); + const auto indices = keyfrm->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm->orb_params_->scale_factors_.at(pred_scale_level)); if (indices.empty()) { continue; @@ -80,14 +80,14 @@ unsigned int fuse::detect_duplication(const std::shared_ptr& key int best_idx = -1; for (const auto idx : indices) { - const auto scale_level = keyfrm->keypts_.at(idx).octave; + const auto scale_level = keyfrm->frm_obs_.keypts_.at(idx).octave; // TODO: shoud determine the scale with 'keyfrm-> get_keypts_in_cell ()' if (scale_level < pred_scale_level - 1 || pred_scale_level < scale_level) { continue; } - const auto& desc = keyfrm->descriptors_.row(idx); + const auto& desc = keyfrm->frm_obs_.descriptors_.row(idx); const auto hamm_dist = compute_descriptor_distance_32(lm_desc, desc); @@ -173,8 +173,8 @@ unsigned int fuse::replace_duplication(const std::shared_ptr& ke } // Acquire keypoints in the cell where the reprojected 3D points exist - const auto pred_scale_level = lm->predict_scale_level(cam_to_lm_dist, keyfrm); - const auto indices = keyfrm->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm->scale_factors_.at(pred_scale_level)); + const auto pred_scale_level = lm->predict_scale_level(cam_to_lm_dist, keyfrm->orb_params_->num_levels_, keyfrm->orb_params_->log_scale_factor_); + const auto indices = keyfrm->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm->orb_params_->scale_factors_.at(pred_scale_level)); if (indices.empty()) { continue; @@ -187,7 +187,7 @@ unsigned int fuse::replace_duplication(const std::shared_ptr& ke int best_idx = -1; for (const auto idx : indices) { - const auto& keypt = keyfrm->undist_keypts_.at(idx); + const auto& keypt = keyfrm->frm_obs_.undist_keypts_.at(idx); const auto scale_level = static_cast(keypt.octave); @@ -196,16 +196,16 @@ unsigned int fuse::replace_duplication(const std::shared_ptr& ke continue; } - if (keyfrm->stereo_x_right_.at(idx) >= 0) { + if (keyfrm->frm_obs_.stereo_x_right_.at(idx) >= 0) { // Compute reprojection error with 3 degrees of freedom if a stereo match exists const auto e_x = reproj(0) - keypt.pt.x; const auto e_y = reproj(1) - keypt.pt.y; - const auto e_x_right = x_right - keyfrm->stereo_x_right_.at(idx); + const auto e_x_right = x_right - keyfrm->frm_obs_.stereo_x_right_.at(idx); const auto reproj_error_sq = e_x * e_x + e_y * e_y + e_x_right * e_x_right; // n=3 constexpr float chi_sq_3D = 7.81473; - if (chi_sq_3D < reproj_error_sq * keyfrm->inv_level_sigma_sq_.at(scale_level)) { + if (chi_sq_3D < reproj_error_sq * keyfrm->orb_params_->inv_level_sigma_sq_.at(scale_level)) { continue; } } @@ -217,12 +217,12 @@ unsigned int fuse::replace_duplication(const std::shared_ptr& ke // n=2 constexpr float chi_sq_2D = 5.99146; - if (chi_sq_2D < reproj_error_sq * keyfrm->inv_level_sigma_sq_.at(scale_level)) { + if (chi_sq_2D < reproj_error_sq * keyfrm->orb_params_->inv_level_sigma_sq_.at(scale_level)) { continue; } } - const auto& desc = keyfrm->descriptors_.row(idx); + const auto& desc = keyfrm->frm_obs_.descriptors_.row(idx); const auto hamm_dist = compute_descriptor_distance_32(lm_desc, desc); diff --git a/src/openvslam/match/projection.cc b/src/openvslam/match/projection.cc index 70a3bd3fa..32f3846cb 100644 --- a/src/openvslam/match/projection.cc +++ b/src/openvslam/match/projection.cc @@ -8,23 +8,29 @@ namespace openvslam { namespace match { -unsigned int projection::match_frame_and_landmarks(data::frame& frm, const std::vector>& local_landmarks, const float margin) const { +unsigned int projection::match_frame_and_landmarks(data::frame& frm, + const std::vector>& local_landmarks, + eigen_alloc_unord_map& lm_to_reproj, + std::unordered_map& lm_to_x_right, + std::unordered_map& lm_to_scale, + const float margin) const { unsigned int num_matches = 0; // Reproject the 3D points to the frame, then acquire the 2D-3D matches for (auto local_lm : local_landmarks) { - if (!local_lm->is_observable_in_tracking_) { + if (!lm_to_reproj.count(local_lm->id_)) { continue; } if (local_lm->will_be_erased()) { continue; } - const auto pred_scale_level = local_lm->scale_level_in_tracking_; + const auto pred_scale_level = lm_to_scale.at(local_lm->id_); // Acquire keypoints in the cell where the reprojected 3D points exist - const auto indices_in_cell = frm.get_keypoints_in_cell(local_lm->reproj_in_tracking_(0), local_lm->reproj_in_tracking_(1), - margin * frm.scale_factors_.at(pred_scale_level), + Vec2_t reproj = lm_to_reproj.at(local_lm->id_); + const auto indices_in_cell = frm.get_keypoints_in_cell(reproj(0), reproj(1), + margin * frm.orb_params_->scale_factors_.at(pred_scale_level), pred_scale_level - 1, pred_scale_level); if (indices_in_cell.empty()) { continue; @@ -43,14 +49,14 @@ unsigned int projection::match_frame_and_landmarks(data::frame& frm, const std:: continue; } - if (0 < frm.stereo_x_right_.at(idx)) { - const auto reproj_error = std::abs(local_lm->x_right_in_tracking_ - frm.stereo_x_right_.at(idx)); - if (margin * frm.scale_factors_.at(pred_scale_level) < reproj_error) { + if (0 < frm.frm_obs_.stereo_x_right_.at(idx)) { + const auto reproj_error = std::abs(lm_to_x_right.at(local_lm->id_) - frm.frm_obs_.stereo_x_right_.at(idx)); + if (margin * frm.orb_params_->scale_factors_.at(pred_scale_level) < reproj_error) { continue; } } - const cv::Mat& desc = frm.descriptors_.row(idx); + const cv::Mat& desc = frm.frm_obs_.descriptors_.row(idx); const auto dist = compute_descriptor_distance_32(lm_desc, desc); @@ -58,11 +64,11 @@ unsigned int projection::match_frame_and_landmarks(data::frame& frm, const std:: second_best_hamm_dist = best_hamm_dist; best_hamm_dist = dist; second_best_scale_level = best_scale_level; - best_scale_level = frm.undist_keypts_.at(idx).octave; + best_scale_level = frm.frm_obs_.undist_keypts_.at(idx).octave; best_idx = idx; } else if (dist < second_best_hamm_dist) { - second_best_scale_level = frm.undist_keypts_.at(idx).octave; + second_best_scale_level = frm.frm_obs_.undist_keypts_.at(idx).octave; second_best_hamm_dist = dist; } } @@ -109,7 +115,7 @@ unsigned int projection::match_current_and_last_frames(data::frame& curr_frm, co // Reproject the 3D points associated to the keypoints of the last frame, // then acquire the 2D-3D matches - for (unsigned int idx_last = 0; idx_last < last_frm.num_keypts_; ++idx_last) { + for (unsigned int idx_last = 0; idx_last < last_frm.frm_obs_.num_keypts_; ++idx_last) { auto& lm = last_frm.landmarks_.at(idx_last); if (!lm) { continue; @@ -133,23 +139,24 @@ unsigned int projection::match_current_and_last_frames(data::frame& curr_frm, co } // Acquire keypoints in the cell where the reprojected 3D points exist - const auto last_scale_level = last_frm.keypts_.at(idx_last).octave; - std::vector indices; + const auto last_scale_level = last_frm.frm_obs_.keypts_.at(idx_last).octave; + int min_level; + int max_level; if (assume_forward) { - indices = curr_frm.get_keypoints_in_cell(reproj(0), reproj(1), - margin * curr_frm.scale_factors_.at(last_scale_level), - last_scale_level, last_frm.num_scale_levels_ - 1); + min_level = last_scale_level; + max_level = last_frm.orb_params_->num_levels_ - 1; } else if (assume_backward) { - indices = curr_frm.get_keypoints_in_cell(reproj(0), reproj(1), - margin * curr_frm.scale_factors_.at(last_scale_level), - 0, last_scale_level); + min_level = 0; + max_level = last_scale_level; } else { - indices = curr_frm.get_keypoints_in_cell(reproj(0), reproj(1), - margin * curr_frm.scale_factors_.at(last_scale_level), - last_scale_level - 1, last_scale_level + 1); + min_level = last_scale_level - 1; + max_level = last_scale_level + 1; } + auto indices = curr_frm.get_keypoints_in_cell(reproj(0), reproj(1), + margin * curr_frm.orb_params_->scale_factors_.at(last_scale_level), + min_level, max_level); if (indices.empty()) { continue; } @@ -164,14 +171,14 @@ unsigned int projection::match_current_and_last_frames(data::frame& curr_frm, co continue; } - if (curr_frm.stereo_x_right_.at(curr_idx) > 0) { - const float reproj_error = std::fabs(x_right - curr_frm.stereo_x_right_.at(curr_idx)); - if (margin * curr_frm.scale_factors_.at(last_scale_level) < reproj_error) { + if (curr_frm.frm_obs_.stereo_x_right_.at(curr_idx) > 0) { + const float reproj_error = std::fabs(x_right - curr_frm.frm_obs_.stereo_x_right_.at(curr_idx)); + if (margin * curr_frm.orb_params_->scale_factors_.at(last_scale_level) < reproj_error) { continue; } } - const auto& desc = curr_frm.descriptors_.row(curr_idx); + const auto& desc = curr_frm.frm_obs_.descriptors_.row(curr_idx); const auto hamm_dist = compute_descriptor_distance_32(lm_desc, desc); @@ -191,7 +198,7 @@ unsigned int projection::match_current_and_last_frames(data::frame& curr_frm, co if (check_orientation_) { const auto delta_angle - = last_frm.undist_keypts_.at(idx_last).angle - curr_frm.undist_keypts_.at(best_idx).angle; + = last_frm.frm_obs_.undist_keypts_.at(idx_last).angle - curr_frm.frm_obs_.undist_keypts_.at(best_idx).angle; angle_checker.append_delta_angle(delta_angle, best_idx); } } @@ -258,10 +265,10 @@ unsigned int projection::match_frame_and_keyframe(data::frame& curr_frm, const s } // Acquire keypoints in the cell where the reprojected 3D points exist - const auto pred_scale_level = lm->predict_scale_level(cam_to_lm_dist, &curr_frm); + const auto pred_scale_level = lm->predict_scale_level(cam_to_lm_dist, curr_frm.orb_params_->num_levels_, curr_frm.orb_params_->log_scale_factor_); const auto indices = curr_frm.get_keypoints_in_cell(reproj(0), reproj(1), - margin * curr_frm.scale_factors_.at(pred_scale_level), + margin * curr_frm.orb_params_->scale_factors_.at(pred_scale_level), pred_scale_level - 1, pred_scale_level + 1); if (indices.empty()) { @@ -278,7 +285,7 @@ unsigned int projection::match_frame_and_keyframe(data::frame& curr_frm, const s continue; } - const auto& desc = curr_frm.descriptors_.row(curr_idx); + const auto& desc = curr_frm.frm_obs_.descriptors_.row(curr_idx); const auto hamm_dist = compute_descriptor_distance_32(lm_desc, desc); @@ -298,7 +305,7 @@ unsigned int projection::match_frame_and_keyframe(data::frame& curr_frm, const s if (check_orientation_) { const auto delta_angle - = keyfrm->undist_keypts_.at(idx).angle - curr_frm.undist_keypts_.at(best_idx).angle; + = keyfrm->frm_obs_.undist_keypts_.at(idx).angle - curr_frm.frm_obs_.undist_keypts_.at(best_idx).angle; angle_checker.append_delta_angle(delta_angle, best_idx); } } @@ -368,8 +375,8 @@ unsigned int projection::match_by_Sim3_transform(const std::shared_ptrpredict_scale_level(cam_to_lm_dist, keyfrm); - const auto indices = keyfrm->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm->scale_factors_.at(pred_scale_level)); + const auto pred_scale_level = lm->predict_scale_level(cam_to_lm_dist, keyfrm->orb_params_->num_levels_, keyfrm->orb_params_->log_scale_factor_); + const auto indices = keyfrm->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm->orb_params_->scale_factors_.at(pred_scale_level)); if (indices.empty()) { continue; @@ -386,14 +393,14 @@ unsigned int projection::match_by_Sim3_transform(const std::shared_ptr(keyfrm->keypts_.at(idx).octave); + const auto scale_level = static_cast(keyfrm->frm_obs_.keypts_.at(idx).octave); // TODO: should determine the scale with 'keyfrm-> get_keypts_in_cell ()' if (scale_level < pred_scale_level - 1 || pred_scale_level < scale_level) { continue; } - const auto& desc = keyfrm->descriptors_.row(idx); + const auto& desc = keyfrm->frm_obs_.descriptors_.row(idx); const auto hamm_dist = compute_descriptor_distance_32(lm_desc, desc); @@ -496,8 +503,8 @@ unsigned int projection::match_keyframes_mutually(const std::shared_ptrpredict_scale_level(cam_to_lm_dist, keyfrm_2); - const auto indices = keyfrm_2->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm_2->scale_factors_.at(pred_scale_level)); + const auto pred_scale_level = lm->predict_scale_level(cam_to_lm_dist, keyfrm_2->orb_params_->num_levels_, keyfrm_2->orb_params_->log_scale_factor_); + const auto indices = keyfrm_2->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm_2->orb_params_->scale_factors_.at(pred_scale_level)); if (indices.empty()) { continue; @@ -510,14 +517,14 @@ unsigned int projection::match_keyframes_mutually(const std::shared_ptr(keyfrm_2->keypts_.at(idx_2).octave); + const auto scale_level = static_cast(keyfrm_2->frm_obs_.keypts_.at(idx_2).octave); // TODO: should determine the scale with 'keyfrm-> get_keypts_in_cell ()' if (scale_level < pred_scale_level - 1 || pred_scale_level < scale_level) { continue; } - const auto& desc = keyfrm_2->descriptors_.row(idx_2); + const auto& desc = keyfrm_2->frm_obs_.descriptors_.row(idx_2); const auto hamm_dist = compute_descriptor_distance_32(lm_desc, desc); @@ -578,9 +585,9 @@ unsigned int projection::match_keyframes_mutually(const std::shared_ptrpredict_scale_level(cam_to_lm_dist, keyfrm_1); + const auto pred_scale_level = lm->predict_scale_level(cam_to_lm_dist, keyfrm_1->orb_params_->num_levels_, keyfrm_1->orb_params_->log_scale_factor_); - const auto indices = keyfrm_1->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm_1->scale_factors_.at(pred_scale_level)); + const auto indices = keyfrm_1->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm_1->orb_params_->scale_factors_.at(pred_scale_level)); if (indices.empty()) { continue; @@ -593,14 +600,14 @@ unsigned int projection::match_keyframes_mutually(const std::shared_ptr(keyfrm_1->keypts_.at(idx_1).octave); + const auto scale_level = static_cast(keyfrm_1->frm_obs_.keypts_.at(idx_1).octave); // TODO: should determine the scale with 'keyfrm-> get_keypts_in_cell ()' if (scale_level < pred_scale_level - 1 || pred_scale_level < scale_level) { continue; } - const auto& desc = keyfrm_1->descriptors_.row(idx_1); + const auto& desc = keyfrm_1->frm_obs_.descriptors_.row(idx_1); const auto hamm_dist = compute_descriptor_distance_32(lm_desc, desc); diff --git a/src/openvslam/match/projection.h b/src/openvslam/match/projection.h index 086ed06e2..0f09d35c9 100644 --- a/src/openvslam/match/projection.h +++ b/src/openvslam/match/projection.h @@ -25,7 +25,12 @@ class projection final : public base { ~projection() final = default; //! frameの2次元点と3次元の対応を求め，frame.landmarks_に対応情報を記録する - unsigned int match_frame_and_landmarks(data::frame& frm, const std::vector>& local_landmarks, const float margin = 5.0) const; + unsigned int match_frame_and_landmarks(data::frame& frm, + const std::vector>& local_landmarks, + eigen_alloc_unord_map& lm_to_reproj, + std::unordered_map& lm_to_x_right, + std::unordered_map& lm_to_scale, + const float margin = 5.0) const; //! last frameで観測している3次元点をcurrent frameに再投影し，frame.landmarks_に対応情報を記録する unsigned int match_current_and_last_frames(data::frame& curr_frm, const data::frame& last_frm, const float margin) const; diff --git a/src/openvslam/match/robust.cc b/src/openvslam/match/robust.cc index 5037695e4..19295709f 100644 --- a/src/openvslam/match/robust.cc +++ b/src/openvslam/match/robust.cc @@ -36,9 +36,9 @@ unsigned int robust::match_for_triangulation(const std::shared_ptr is_already_matched_in_keyfrm_2(keyfrm_2->num_keypts_, false); + std::vector is_already_matched_in_keyfrm_2(keyfrm_2->frm_obs_.num_keypts_, false); // Save the keypoint idx in keyframe 2 which is already associated to the keypoint idx in keyframe 1 - std::vector matched_indices_2_in_keyfrm_1(keyfrm_1->num_keypts_, -1); + std::vector matched_indices_2_in_keyfrm_1(keyfrm_1->frm_obs_.num_keypts_, -1); #ifdef USE_DBOW2 DBoW2::FeatureVector::const_iterator itr_1 = keyfrm_1->bow_feat_vec_.begin(); @@ -68,12 +68,12 @@ unsigned int robust::match_for_triangulation(const std::shared_ptrstereo_x_right_.at(idx_1); + const bool is_stereo_keypt_1 = 0 <= keyfrm_1->frm_obs_.stereo_x_right_.at(idx_1); // Acquire the keypoints and ORB feature vectors - const auto& keypt_1 = keyfrm_1->undist_keypts_.at(idx_1); - const Vec3_t& bearing_1 = keyfrm_1->bearings_.at(idx_1); - const auto& desc_1 = keyfrm_1->descriptors_.row(idx_1); + const auto& keypt_1 = keyfrm_1->frm_obs_.undist_keypts_.at(idx_1); + const Vec3_t& bearing_1 = keyfrm_1->frm_obs_.bearings_.at(idx_1); + const auto& desc_1 = keyfrm_1->frm_obs_.descriptors_.row(idx_1); // Find a keypoint in keyframe 2 that has the minimum hamming distance unsigned int best_hamm_dist = HAMMING_DIST_THR_LOW; @@ -93,11 +93,11 @@ unsigned int robust::match_for_triangulation(const std::shared_ptrstereo_x_right_.at(idx_2); + const bool is_stereo_keypt_2 = 0 <= keyfrm_2->frm_obs_.stereo_x_right_.at(idx_2); // Acquire the keypoints and ORB feature vectors - const Vec3_t& bearing_2 = keyfrm_2->bearings_.at(idx_2); - const auto& desc_2 = keyfrm_2->descriptors_.row(idx_2); + const Vec3_t& bearing_2 = keyfrm_2->frm_obs_.bearings_.at(idx_2); + const auto& desc_2 = keyfrm_2->frm_obs_.descriptors_.row(idx_2); // Compute the distance const auto hamm_dist = compute_descriptor_distance_32(desc_1, desc_2); @@ -120,7 +120,7 @@ unsigned int robust::match_for_triangulation(const std::shared_ptrscale_factors_.at(keypt_1.octave)); + keyfrm_1->orb_params_->scale_factors_.at(keypt_1.octave)); if (is_inlier) { best_idx_2 = idx_2; best_hamm_dist = hamm_dist; @@ -137,7 +137,7 @@ unsigned int robust::match_for_triangulation(const std::shared_ptrundist_keypts_.at(best_idx_2).angle; + = keypt_1.angle - keyfrm_2->frm_obs_.undist_keypts_.at(best_idx_2).angle; angle_checker.append_delta_angle(delta_angle, idx_1); } } @@ -179,7 +179,7 @@ unsigned int robust::match_for_triangulation(const std::shared_ptr& keyfrm, std::vector>& matched_lms_in_frm) const { // Initialization - const auto num_frm_keypts = frm.num_keypts_; + const auto num_frm_keypts = frm.frm_obs_.num_keypts_; const auto keyfrm_lms = keyfrm->get_landmarks(); unsigned int num_inlier_matches = 0; matched_lms_in_frm = std::vector>(num_frm_keypts, nullptr); @@ -189,7 +189,7 @@ unsigned int robust::match_frame_and_keyframe(data::frame& frm, const std::share brute_force_match(frm, keyfrm, matches); // Extract only inliers with eight-point RANSAC - solve::essential_solver solver(frm.bearings_, keyfrm->bearings_, matches); + solve::essential_solver solver(frm.frm_obs_.bearings_, keyfrm->frm_obs_.bearings_, matches); solver.find_via_ransac(50, false); if (!solver.solution_is_valid()) { return 0; @@ -218,13 +218,13 @@ unsigned int robust::brute_force_match(data::frame& frm, const std::shared_ptrnum_keypts_; - const auto keypts_1 = frm.keypts_; - const auto keypts_2 = keyfrm->keypts_; + const auto num_keypts_1 = frm.frm_obs_.num_keypts_; + const auto num_keypts_2 = keyfrm->frm_obs_.num_keypts_; + const auto keypts_1 = frm.frm_obs_.keypts_; + const auto keypts_2 = keyfrm->frm_obs_.keypts_; const auto lms_2 = keyfrm->get_landmarks(); - const auto& descs_1 = frm.descriptors_; - const auto& descs_2 = keyfrm->descriptors_; + const auto& descs_1 = frm.frm_obs_.descriptors_; + const auto& descs_2 = keyfrm->frm_obs_.descriptors_; // 2. Acquire ORB descriptors in the keyframe which are the first and second closest to the descriptors in the frame // it is assumed that keypoint in the keyframe are associated to 3D points diff --git a/src/openvslam/module/frame_tracker.cc b/src/openvslam/module/frame_tracker.cc index 9c0cf93c1..801b3a6f3 100644 --- a/src/openvslam/module/frame_tracker.cc +++ b/src/openvslam/module/frame_tracker.cc @@ -15,7 +15,7 @@ namespace module { frame_tracker::frame_tracker(camera::base* camera, const unsigned int num_matches_thr) : camera_(camera), num_matches_thr_(num_matches_thr), pose_optimizer_() {} -bool frame_tracker::motion_based_track(data::frame& curr_frm, const data::frame& last_frm, const Mat44_t& velocity) const { +bool frame_tracker::motion_based_track(data::frame& curr_frm, const data::frame& last_frm, const Mat44_t& velocity, std::unordered_set& outlier_ids) const { match::projection projection_matcher(0.9, true); // Set the initial pose by using the motion model @@ -43,7 +43,7 @@ bool frame_tracker::motion_based_track(data::frame& curr_frm, const data::frame& pose_optimizer_.optimize(curr_frm); // Discard the outliers - const auto num_valid_matches = discard_outliers(curr_frm); + const auto num_valid_matches = discard_outliers(curr_frm, outlier_ids); if (num_valid_matches < num_matches_thr_) { spdlog::debug("motion based tracking failed: {} inlier matches < {}", num_valid_matches, num_matches_thr_); @@ -54,12 +54,10 @@ bool frame_tracker::motion_based_track(data::frame& curr_frm, const data::frame& } } -bool frame_tracker::bow_match_based_track(data::frame& curr_frm, const data::frame& last_frm, const std::shared_ptr& ref_keyfrm) const { +bool frame_tracker::bow_match_based_track(data::frame& curr_frm, const data::frame& last_frm, const std::shared_ptr& ref_keyfrm, + std::unordered_set& outlier_ids) const { match::bow_tree bow_matcher(0.7, true); - // Compute the BoW representations to perform the BoW match - curr_frm.compute_bow(); - // Search 2D-2D matches between the ref keyframes and the current frame // to acquire 2D-3D matches between the frame keypoints and 3D points observed in the ref keyframe std::vector> matched_lms_in_curr; @@ -79,7 +77,7 @@ bool frame_tracker::bow_match_based_track(data::frame& curr_frm, const data::fra pose_optimizer_.optimize(curr_frm); // Discard the outliers - const auto num_valid_matches = discard_outliers(curr_frm); + const auto num_valid_matches = discard_outliers(curr_frm, outlier_ids); if (num_valid_matches < num_matches_thr_) { spdlog::debug("bow match based tracking failed: {} inlier matches < {}", num_valid_matches, num_matches_thr_); @@ -90,7 +88,8 @@ bool frame_tracker::bow_match_based_track(data::frame& curr_frm, const data::fra } } -bool frame_tracker::robust_match_based_track(data::frame& curr_frm, const data::frame& last_frm, const std::shared_ptr& ref_keyfrm) const { +bool frame_tracker::robust_match_based_track(data::frame& curr_frm, const data::frame& last_frm, const std::shared_ptr& ref_keyfrm, + std::unordered_set& outlier_ids) const { match::robust robust_matcher(0.8, false); // Search 2D-2D matches between the ref keyframes and the current frame @@ -112,7 +111,7 @@ bool frame_tracker::robust_match_based_track(data::frame& curr_frm, const data:: pose_optimizer_.optimize(curr_frm); // Discard the outliers - const auto num_valid_matches = discard_outliers(curr_frm); + const auto num_valid_matches = discard_outliers(curr_frm, outlier_ids); if (num_valid_matches < num_matches_thr_) { spdlog::debug("robust match based tracking failed: {} inlier matches < {}", num_valid_matches, num_matches_thr_); @@ -123,10 +122,10 @@ bool frame_tracker::robust_match_based_track(data::frame& curr_frm, const data:: } } -unsigned int frame_tracker::discard_outliers(data::frame& curr_frm) const { +unsigned int frame_tracker::discard_outliers(data::frame& curr_frm, std::unordered_set& outlier_ids) const { unsigned int num_valid_matches = 0; - for (unsigned int idx = 0; idx < curr_frm.num_keypts_; ++idx) { + for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) { if (!curr_frm.landmarks_.at(idx)) { continue; } @@ -135,8 +134,7 @@ unsigned int frame_tracker::discard_outliers(data::frame& curr_frm) const { if (curr_frm.outlier_flags_.at(idx)) { curr_frm.outlier_flags_.at(idx) = false; - lm->is_observable_in_tracking_ = false; - lm->identifier_in_local_lm_search_ = curr_frm.id_; + outlier_ids.insert(lm->id_); lm = nullptr; continue; } diff --git a/src/openvslam/module/frame_tracker.h b/src/openvslam/module/frame_tracker.h index 7dd614f33..d2dfd8fbe 100644 --- a/src/openvslam/module/frame_tracker.h +++ b/src/openvslam/module/frame_tracker.h @@ -15,6 +15,7 @@ class base; namespace data { class frame; class keyframe; +class bow_database; } // namespace data namespace module { @@ -23,14 +24,17 @@ class frame_tracker { public: explicit frame_tracker(camera::base* camera, const unsigned int num_matches_thr = 20); - bool motion_based_track(data::frame& curr_frm, const data::frame& last_frm, const Mat44_t& velocity) const; + bool motion_based_track(data::frame& curr_frm, const data::frame& last_frm, const Mat44_t& velocity, + std::unordered_set& outlier_ids) const; - bool bow_match_based_track(data::frame& curr_frm, const data::frame& last_frm, const std::shared_ptr& ref_keyfrm) const; + bool bow_match_based_track(data::frame& curr_frm, const data::frame& last_frm, const std::shared_ptr& ref_keyfrm, + std::unordered_set& outlier_ids) const; - bool robust_match_based_track(data::frame& curr_frm, const data::frame& last_frm, const std::shared_ptr& ref_keyfrm) const; + bool robust_match_based_track(data::frame& curr_frm, const data::frame& last_frm, const std::shared_ptr& ref_keyfrm, + std::unordered_set& outlier_ids) const; private: - unsigned int discard_outliers(data::frame& curr_frm) const; + unsigned int discard_outliers(data::frame& curr_frm, std::unordered_set& outlier_ids) const; const camera::base* camera_; const unsigned int num_matches_thr_; diff --git a/src/openvslam/module/initializer.cc b/src/openvslam/module/initializer.cc index dae83673e..cabb35090 100644 --- a/src/openvslam/module/initializer.cc +++ b/src/openvslam/module/initializer.cc @@ -13,10 +13,9 @@ namespace openvslam { namespace module { -initializer::initializer(const camera::setup_type_t setup_type, - data::map_database* map_db, data::bow_database* bow_db, +initializer::initializer(data::map_database* map_db, data::bow_database* bow_db, const YAML::Node& yaml_node) - : setup_type_(setup_type), map_db_(map_db), bow_db_(bow_db), + : map_db_(map_db), bow_db_(bow_db), num_ransac_iters_(yaml_node["num_ransac_iterations"].as(100)), min_num_triangulated_(yaml_node["num_min_triangulated_pts"].as(50)), parallax_deg_thr_(yaml_node["parallax_deg_threshold"].as(1.0)), @@ -35,6 +34,7 @@ void initializer::reset() { initializer_.reset(nullptr); state_ = initializer_state_t::NotReady; init_frm_id_ = 0; + init_frm_stamp_ = 0.0; } initializer_state_t initializer::get_state() const { @@ -42,7 +42,7 @@ initializer_state_t initializer::get_state() const { } std::vector initializer::get_initial_keypoints() const { - return init_frm_.keypts_; + return init_frm_.frm_obs_.keypts_; } std::vector initializer::get_initial_matches() const { @@ -53,8 +53,13 @@ unsigned int initializer::get_initial_frame_id() const { return init_frm_id_; } -bool initializer::initialize(data::frame& curr_frm) { - switch (setup_type_) { +double initializer::get_initial_frame_timestamp() const { + return init_frm_stamp_; +} + +bool initializer::initialize(const camera::setup_type_t setup_type, + data::bow_vocabulary* bow_vocab, data::frame& curr_frm) { + switch (setup_type) { case camera::setup_type_t::Monocular: { // construct an initializer if not constructed if (state_ == initializer_state_t::NotReady) { @@ -69,7 +74,7 @@ bool initializer::initialize(data::frame& curr_frm) { } // create new map if succeeded - create_map_for_monocular(curr_frm); + create_map_for_monocular(bow_vocab, curr_frm); break; } case camera::setup_type_t::Stereo: @@ -83,7 +88,7 @@ bool initializer::initialize(data::frame& curr_frm) { } // create new map if succeeded - create_map_for_stereo(curr_frm); + create_map_for_stereo(bow_vocab, curr_frm); break; } default: { @@ -94,6 +99,7 @@ bool initializer::initialize(data::frame& curr_frm) { // check the state is succeeded or not if (state_ == initializer_state_t::Succeeded) { init_frm_id_ = curr_frm.id_; + init_frm_stamp_ = curr_frm.timestamp_; return true; } else { @@ -106,9 +112,9 @@ void initializer::create_initializer(data::frame& curr_frm) { init_frm_ = data::frame(curr_frm); // initialize the previously matched coordinates - prev_matched_coords_.resize(init_frm_.undist_keypts_.size()); - for (unsigned int i = 0; i < init_frm_.undist_keypts_.size(); ++i) { - prev_matched_coords_.at(i) = init_frm_.undist_keypts_.at(i).pt; + prev_matched_coords_.resize(init_frm_.frm_obs_.undist_keypts_.size()); + for (unsigned int i = 0; i < init_frm_.frm_obs_.undist_keypts_.size(); ++i) { + prev_matched_coords_.at(i) = init_frm_.frm_obs_.undist_keypts_.at(i).pt; } // initialize matchings (init_idx -> curr_idx) @@ -156,7 +162,7 @@ bool initializer::try_initialize_for_monocular(data::frame& curr_frm) { return initializer_->initialize(curr_frm, init_matches_); } -bool initializer::create_map_for_monocular(data::frame& curr_frm) { +bool initializer::create_map_for_monocular(data::bow_vocabulary* bow_vocab, data::frame& curr_frm) { assert(state_ == initializer_state_t::Initializing); eigen_alloc_vector init_triangulated_pts; @@ -188,12 +194,12 @@ bool initializer::create_map_for_monocular(data::frame& curr_frm) { } // create initial keyframes - auto init_keyfrm = data::keyframe::make_keyframe(init_frm_, map_db_, bow_db_); - auto curr_keyfrm = data::keyframe::make_keyframe(curr_frm, map_db_, bow_db_); + auto init_keyfrm = data::keyframe::make_keyframe(init_frm_); + auto curr_keyfrm = data::keyframe::make_keyframe(curr_frm); // compute BoW representations - init_keyfrm->compute_bow(); - curr_keyfrm->compute_bow(); + init_keyfrm->compute_bow(bow_vocab); + curr_keyfrm->compute_bow(bow_vocab); // add the keyframes to the map DB map_db_->add_keyframe(init_keyfrm); @@ -224,7 +230,7 @@ bool initializer::create_map_for_monocular(data::frame& curr_frm) { // update the descriptor lm->compute_descriptor(); // update the geometry - lm->update_normal_and_depth(); + lm->update_mean_normal_and_obs_scale_variance(); // set the 2D-3D assocications to the current frame curr_frm.landmarks_.at(curr_idx) = lm; @@ -236,7 +242,7 @@ bool initializer::create_map_for_monocular(data::frame& curr_frm) { // global bundle adjustment const auto global_bundle_adjuster = optimize::global_bundle_adjuster(map_db_, num_ba_iters_, true); - global_bundle_adjuster.optimize(); + global_bundle_adjuster.optimize_for_initialization(); // scale the map so that the median of depths is 1.0 const auto median_depth = init_keyfrm->compute_median_depth(init_keyfrm->camera_->model_type_ == camera::model_type_t::Equirectangular); @@ -278,22 +284,22 @@ void initializer::scale_map(const std::shared_ptr& init_keyfrm, bool initializer::try_initialize_for_stereo(data::frame& curr_frm) { assert(state_ == initializer_state_t::Initializing); // count the number of valid depths - unsigned int num_valid_depths = std::count_if(curr_frm.depths_.begin(), curr_frm.depths_.end(), + unsigned int num_valid_depths = std::count_if(curr_frm.frm_obs_.depths_.begin(), curr_frm.frm_obs_.depths_.end(), [](const float depth) { return 0 < depth; }); return min_num_triangulated_ <= num_valid_depths; } -bool initializer::create_map_for_stereo(data::frame& curr_frm) { +bool initializer::create_map_for_stereo(data::bow_vocabulary* bow_vocab, data::frame& curr_frm) { assert(state_ == initializer_state_t::Initializing); // create an initial keyframe curr_frm.set_cam_pose(Mat44_t::Identity()); - auto curr_keyfrm = data::keyframe::make_keyframe(curr_frm, map_db_, bow_db_); + auto curr_keyfrm = data::keyframe::make_keyframe(curr_frm); // compute BoW representation - curr_keyfrm->compute_bow(); + curr_keyfrm->compute_bow(bow_vocab); // add to the map DB map_db_->add_keyframe(curr_keyfrm); @@ -302,9 +308,9 @@ bool initializer::create_map_for_stereo(data::frame& curr_frm) { curr_frm.ref_keyfrm_ = curr_keyfrm; map_db_->update_frame_statistics(curr_frm, false); - for (unsigned int idx = 0; idx < curr_frm.num_keypts_; ++idx) { + for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) { // add a new landmark if tht corresponding depth is valid - const auto z = curr_frm.depths_.at(idx); + const auto z = curr_frm.frm_obs_.depths_.at(idx); if (z <= 0) { continue; } @@ -320,7 +326,7 @@ bool initializer::create_map_for_stereo(data::frame& curr_frm) { // update the descriptor lm->compute_descriptor(); // update the geometry - lm->update_normal_and_depth(); + lm->update_mean_normal_and_obs_scale_variance(); // set the 2D-3D associations to the current frame curr_frm.landmarks_.at(idx) = lm; diff --git a/src/openvslam/module/initializer.h b/src/openvslam/module/initializer.h index 41d2e047a..76c171ec8 100644 --- a/src/openvslam/module/initializer.h +++ b/src/openvslam/module/initializer.h @@ -3,6 +3,7 @@ #include "openvslam/data/frame.h" #include "openvslam/initialize/base.h" +#include "openvslam/data/bow_vocabulary_fwd.h" #include @@ -31,8 +32,7 @@ class initializer { initializer() = delete; //! Constructor - initializer(const camera::setup_type_t setup_type, - data::map_database* map_db, data::bow_database* bow_db, + initializer(data::map_database* map_db, data::bow_database* bow_db, const YAML::Node& yaml_node); //! Destructor @@ -53,12 +53,14 @@ class initializer { //! Get the initial frame ID which succeeded in initialization unsigned int get_initial_frame_id() const; + //! Get the initial frame stamp which succeeded in initialization + double get_initial_frame_timestamp() const; + //! Initialize with the current frame - bool initialize(data::frame& curr_frm); + bool initialize(const camera::setup_type_t setup_type, + data::bow_vocabulary* bow_vocab, data::frame& curr_frm); private: - //! camera setup type - const camera::setup_type_t setup_type_; //! map database data::map_database* map_db_ = nullptr; //! BoW database @@ -66,8 +68,10 @@ class initializer { //! initializer status initializer_state_t state_ = initializer_state_t::NotReady; - //! frame ID used for initialization (will be set after succeeded) + //! ID of frame used for initialization (will be set after succeeded) unsigned int init_frm_id_ = 0; + //! timestamp of frame used for initialization (will be set after succeeded) + double init_frm_stamp_ = 0.0; //----------------------------------------- // parameters @@ -97,7 +101,7 @@ class initializer { bool try_initialize_for_monocular(data::frame& curr_frm); //! Create an initial map with monocular camera setup - bool create_map_for_monocular(data::frame& curr_frm); + bool create_map_for_monocular(data::bow_vocabulary* bow_vocab, data::frame& curr_frm); //! Scaling up or down a initial map void scale_map(const std::shared_ptr& init_keyfrm, const std::shared_ptr& curr_keyfrm, const double scale); @@ -118,7 +122,7 @@ class initializer { bool try_initialize_for_stereo(data::frame& curr_frm); //! Create an initial map with stereo or RGBD camera setup - bool create_map_for_stereo(data::frame& curr_frm); + bool create_map_for_stereo(data::bow_vocabulary* bow_vocab, data::frame& curr_frm); }; } // namespace module diff --git a/src/openvslam/module/keyframe_inserter.cc b/src/openvslam/module/keyframe_inserter.cc index b2dc98fea..dd5d5a4fc 100644 --- a/src/openvslam/module/keyframe_inserter.cc +++ b/src/openvslam/module/keyframe_inserter.cc @@ -1,28 +1,34 @@ #include "openvslam/mapping_module.h" #include "openvslam/data/landmark.h" -#include "openvslam/data/bow_database.h" #include "openvslam/data/map_database.h" #include "openvslam/module/keyframe_inserter.h" +#include + namespace openvslam { namespace module { -keyframe_inserter::keyframe_inserter(const camera::setup_type_t setup_type, const float true_depth_thr, - data::map_database* map_db, data::bow_database* bow_db, - const unsigned int min_num_frms, const unsigned int max_num_frms) - : setup_type_(setup_type), true_depth_thr_(true_depth_thr), - map_db_(map_db), bow_db_(bow_db), - min_num_frms_(min_num_frms), max_num_frms_(max_num_frms) {} +keyframe_inserter::keyframe_inserter(const double max_interval, + const double lms_ratio_thr_almost_all_lms_are_tracked, + const double lms_ratio_thr_view_changed) + : max_interval_(max_interval), + lms_ratio_thr_almost_all_lms_are_tracked_(lms_ratio_thr_almost_all_lms_are_tracked), + lms_ratio_thr_view_changed_(lms_ratio_thr_view_changed) {} + +keyframe_inserter::keyframe_inserter(const YAML::Node& yaml_node) + : keyframe_inserter(yaml_node["max_interval"].as(1.0), + yaml_node["lms_ratio_thr_almost_all_lms_are_tracked"].as(0.95), + yaml_node["lms_ratio_thr_view_changed"].as(0.9)) {} void keyframe_inserter::set_mapping_module(mapping_module* mapper) { mapper_ = mapper; } void keyframe_inserter::reset() { - frm_id_of_last_keyfrm_ = 0; } -bool keyframe_inserter::new_keyframe_is_needed(const data::frame& curr_frm, const unsigned int num_tracked_lms, +bool keyframe_inserter::new_keyframe_is_needed(data::map_database* map_db, const data::frame& curr_frm, + const unsigned int num_tracked_lms, const data::keyframe& ref_keyfrm) const { assert(mapper_); // Any keyframes are not able to be added when the mapping module stops @@ -30,79 +36,54 @@ bool keyframe_inserter::new_keyframe_is_needed(const data::frame& curr_frm, cons return false; } - const auto num_keyfrms = map_db_->get_num_keyframes(); + const auto num_keyfrms = map_db->get_num_keyframes(); + auto last_inserted_keyfrm = map_db->get_last_inserted_keyframe(); // Count the number of the 3D points that are observed from more than two keyframes const unsigned int min_obs_thr = (3 <= num_keyfrms) ? 3 : 2; const auto num_reliable_lms = ref_keyfrm.get_num_tracked_landmarks(min_obs_thr); - // Check if the mapping is in progress or not - const bool mapper_is_idle = mapper_->get_keyframe_acceptability(); + // When the mapping module skips localBA, it does not insert keyframes + const auto mapper_is_skipping_localBA = mapper_->is_skipping_localBA(); - // Ratio-threshold of "the number of 3D points observed in the current frame" / "that of 3D points observed in the last keyframe" - constexpr unsigned int num_tracked_lms_thr = 15; - const float lms_ratio_thr = 0.9; + constexpr unsigned int num_tracked_lms_thr_unstable = 15; - // Condition A1: Add a keyframe if the number of frames added after the previous keyframe insertion reaches the threshold - const bool cond_a1 = frm_id_of_last_keyfrm_ + max_num_frms_ <= curr_frm.id_; - // Condition A2: Add a keyframe if the number of added frames exceeds the minimum, - // and concurrently the mapping module remains standing-by - const bool cond_a2 = (frm_id_of_last_keyfrm_ + min_num_frms_ <= curr_frm.id_) && mapper_is_idle; - // Condition A3: Add a keyframe if the field-of-view of the current frame is changed a lot - const bool cond_a3 = num_tracked_lms < num_reliable_lms * 0.25; + // New keyframe is needed if the time elapsed since the last keyframe insertion reaches the threshold + const bool max_interval_elapsed = last_inserted_keyfrm && last_inserted_keyfrm->timestamp_ + max_interval_ <= curr_frm.timestamp_; + // New keyframe is needed if the field-of-view of the current frame is changed a lot + const bool view_changed = num_tracked_lms < num_reliable_lms * lms_ratio_thr_view_changed_; - // Condition B: (Mandatory for keyframe insertion) - // Add a keyframe if the number of 3D points exceeds the threshold, - // and concurrently the ratio of the reliable 3D points larger than the threshold ratio - const bool cond_b = (num_tracked_lms_thr <= num_tracked_lms) && (num_tracked_lms < num_reliable_lms * lms_ratio_thr); + // (Mandatory for keyframe insertion) + // New keyframe is needed if the number of 3D points exceeds the threshold, + // and concurrently the ratio of the reliable 3D points larger than the threshold ratio + bool tracking_is_unstable = num_tracked_lms < num_tracked_lms_thr_unstable; + bool almost_all_lms_are_tracked = num_tracked_lms > num_reliable_lms * lms_ratio_thr_almost_all_lms_are_tracked_; - // Do not add any kerframes if the condition B is not satisfied - if (!cond_b) { - return false; - } - - // Do not add any kerframes if all the conditions A are not satisfied - if (!cond_a1 && !cond_a2 && !cond_a3) { - return false; - } - - // Add the keyframe if the mapping module isn't in the process - if (mapper_is_idle) { - return true; - } - - // Stop the local bundle adjustment if the mapping module is in the process, then add a new keyframe - if (setup_type_ != camera::setup_type_t::Monocular - && mapper_->get_num_queued_keyframes() <= 2) { - mapper_->abort_local_BA(); - return true; - } - - return false; + return (max_interval_elapsed || view_changed) && !tracking_is_unstable && !almost_all_lms_are_tracked && !mapper_is_skipping_localBA; } -std::shared_ptr keyframe_inserter::insert_new_keyframe(data::frame& curr_frm) { - // Force the mapping module to run - if (!mapper_->set_force_to_run(true)) { +std::shared_ptr keyframe_inserter::insert_new_keyframe(data::map_database* map_db, + data::frame& curr_frm) { + // Do not pause mapping_module to let this keyframe process + if (!mapper_->prevent_pause_if_not_paused()) { + // If it is already paused, exit return nullptr; } curr_frm.update_pose_params(); - auto keyfrm = data::keyframe::make_keyframe(curr_frm, map_db_, bow_db_); - - frm_id_of_last_keyfrm_ = curr_frm.id_; + auto keyfrm = data::keyframe::make_keyframe(curr_frm); // Queue up the keyframe to the mapping module - if (setup_type_ == camera::setup_type_t::Monocular) { + if (!keyfrm->depth_is_avaliable()) { queue_keyframe(keyfrm); return keyfrm; } // Save the valid depth and index pairs std::vector> depth_idx_pairs; - depth_idx_pairs.reserve(curr_frm.num_keypts_); - for (unsigned int idx = 0; idx < curr_frm.num_keypts_; ++idx) { - const auto depth = curr_frm.depths_.at(idx); + depth_idx_pairs.reserve(curr_frm.frm_obs_.num_keypts_); + for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) { + const auto depth = curr_frm.frm_obs_.depths_.at(idx); // Add if the depth is valid if (0 < depth) { depth_idx_pairs.emplace_back(std::make_pair(depth, idx)); @@ -126,7 +107,7 @@ std::shared_ptr keyframe_inserter::insert_new_keyframe(data::fra // Stop adding a keyframe if the number of 3D points exceeds the minimal threshold, // and concurrently the depth value exceeds the threshold - if (min_num_to_create < count && true_depth_thr_ < depth) { + if (min_num_to_create < count && keyfrm->camera_->depth_thr_ < depth) { break; } @@ -141,16 +122,16 @@ std::shared_ptr keyframe_inserter::insert_new_keyframe(data::fra // Stereo-triangulation can be performed if the 3D point is not yet associated to the keypoint index const Vec3_t pos_w = curr_frm.triangulate_stereo(idx); - auto lm = std::make_shared(pos_w, keyfrm, map_db_); + auto lm = std::make_shared(pos_w, keyfrm, map_db); lm->add_observation(keyfrm, idx); keyfrm->add_landmark(lm, idx); curr_frm.landmarks_.at(idx) = lm; lm->compute_descriptor(); - lm->update_normal_and_depth(); + lm->update_mean_normal_and_obs_scale_variance(); - map_db_->add_landmark(lm); + map_db->add_landmark(lm); } // Queue up the keyframe to the mapping module @@ -160,7 +141,7 @@ std::shared_ptr keyframe_inserter::insert_new_keyframe(data::fra void keyframe_inserter::queue_keyframe(const std::shared_ptr& keyfrm) { mapper_->queue_keyframe(keyfrm); - mapper_->set_force_to_run(false); + mapper_->stop_prevent_pause(); } } // namespace module diff --git a/src/openvslam/module/keyframe_inserter.h b/src/openvslam/module/keyframe_inserter.h index e403578e9..093d4ee56 100644 --- a/src/openvslam/module/keyframe_inserter.h +++ b/src/openvslam/module/keyframe_inserter.h @@ -19,9 +19,11 @@ namespace module { class keyframe_inserter { public: - keyframe_inserter(const camera::setup_type_t setup_type, const float true_depth_thr, - data::map_database* map_db, data::bow_database* bow_db, - const unsigned int min_num_frms, const unsigned int max_num_frms); + explicit keyframe_inserter(const double max_interval = 1.0, + const double lms_ratio_thr_almost_all_lms_are_tracked = 0.95, + const double lms_ratio_thr_view_changed = 0.9); + + explicit keyframe_inserter(const YAML::Node& yaml_node); virtual ~keyframe_inserter() = default; @@ -32,13 +34,15 @@ class keyframe_inserter { /** * Check the new keyframe is needed or not */ - bool new_keyframe_is_needed(const data::frame& curr_frm, const unsigned int num_tracked_lms, + bool new_keyframe_is_needed(data::map_database* map_db, + const data::frame& curr_frm, + const unsigned int num_tracked_lms, const data::keyframe& ref_keyfrm) const; /** * Insert the new keyframe derived from the current frame */ - std::shared_ptr insert_new_keyframe(data::frame& curr_frm); + std::shared_ptr insert_new_keyframe(data::map_database* map_db, data::frame& curr_frm); private: /** @@ -46,26 +50,15 @@ class keyframe_inserter { */ void queue_keyframe(const std::shared_ptr& keyfrm); - //! setup type of the tracking camera - const camera::setup_type_t setup_type_; - //! depth threshold in metric scale - const float true_depth_thr_; - - //! map database - data::map_database* map_db_ = nullptr; - //! BoW database - data::bow_database* bow_db_ = nullptr; - //! mapping module mapping_module* mapper_ = nullptr; - //! min number of frames to insert keyframe - const unsigned int min_num_frms_; - //! max number of frames to insert keyframe - const unsigned int max_num_frms_; + //! max interval to insert keyframe + const double max_interval_; - //! frame ID of the last keyframe - unsigned int frm_id_of_last_keyfrm_ = 0; + //! Ratio-threshold of "the number of 3D points observed in the current frame" / "that of 3D points observed in the last keyframe" + const double lms_ratio_thr_almost_all_lms_are_tracked_ = 0.95; + const double lms_ratio_thr_view_changed_ = 0.9; }; } // namespace module diff --git a/src/openvslam/module/local_map_cleaner.cc b/src/openvslam/module/local_map_cleaner.cc index 0125036ed..2143011cd 100644 --- a/src/openvslam/module/local_map_cleaner.cc +++ b/src/openvslam/module/local_map_cleaner.cc @@ -5,8 +5,8 @@ namespace openvslam { namespace module { -local_map_cleaner::local_map_cleaner(double redundant_obs_ratio_thr) - : redundant_obs_ratio_thr_(redundant_obs_ratio_thr) {} +local_map_cleaner::local_map_cleaner(data::map_database* map_db, data::bow_database* bow_db, double redundant_obs_ratio_thr) + : map_db_(map_db), bow_db_(bow_db), redundant_obs_ratio_thr_(redundant_obs_ratio_thr) {} void local_map_cleaner::reset() { fresh_landmarks_.clear(); @@ -57,7 +57,7 @@ unsigned int local_map_cleaner::remove_redundant_landmarks(const unsigned int cu } else if (lm_state == lm_state_t::Invalid) { ++num_removed; - lm->prepare_for_erasing(); + lm->prepare_for_erasing(map_db_); iter = fresh_landmarks_.erase(iter); } else { @@ -97,7 +97,7 @@ unsigned int local_map_cleaner::remove_redundant_keyframes(const std::shared_ptr // if the redundant observation ratio of `covisibility` is larger than the threshold, it will be removed if (redundant_obs_ratio_thr_ <= static_cast(num_redundant_obs) / num_valid_obs) { ++num_removed; - covisibility->prepare_for_erasing(); + covisibility->prepare_for_erasing(map_db_, bow_db_); } } @@ -123,8 +123,8 @@ void local_map_cleaner::count_redundant_observations(const std::shared_ptrdepths_.at(idx); - if (keyfrm->depth_is_avaliable() && (depth < 0.0 || keyfrm->depth_thr_ < depth)) { + const auto depth = keyfrm->frm_obs_.depths_.at(idx); + if (keyfrm->depth_is_avaliable() && (depth < 0.0 || keyfrm->camera_->depth_thr_ < depth)) { continue; } @@ -136,7 +136,7 @@ void local_map_cleaner::count_redundant_observations(const std::shared_ptrundist_keypts_.at(idx).octave; + const auto scale_level = keyfrm->frm_obs_.undist_keypts_.at(idx).octave; // get observers of `lm` const auto observations = lm->get_observations(); @@ -152,7 +152,7 @@ void local_map_cleaner::count_redundant_observations(const std::shared_ptrundist_keypts_.at(obs.second).octave; + const auto ngh_scale_level = ngh_keyfrm->frm_obs_.undist_keypts_.at(obs.second).octave; // compare the scale levels if (ngh_scale_level <= scale_level + 1) { diff --git a/src/openvslam/module/local_map_cleaner.h b/src/openvslam/module/local_map_cleaner.h index d33a15a39..8c6f7fd8e 100644 --- a/src/openvslam/module/local_map_cleaner.h +++ b/src/openvslam/module/local_map_cleaner.h @@ -9,6 +9,8 @@ namespace openvslam { namespace data { class keyframe; class landmark; +class bow_database; +class map_database; } // namespace data namespace module { @@ -18,7 +20,7 @@ class local_map_cleaner { /** * Constructor */ - explicit local_map_cleaner(double redundant_obs_ratio_thr = 0.9); + explicit local_map_cleaner(data::map_database* map_db, data::bow_database* bow_db, double redundant_obs_ratio_thr = 0.9); /** * Destructor @@ -60,6 +62,11 @@ class local_map_cleaner { void count_redundant_observations(const std::shared_ptr& keyfrm, unsigned int& num_valid_obs, unsigned int& num_redundant_obs) const; private: + //! map database + data::map_database* map_db_ = nullptr; + //! BoW database + data::bow_database* bow_db_ = nullptr; + //! double redundant_obs_ratio_thr_; diff --git a/src/openvslam/module/local_map_updater.cc b/src/openvslam/module/local_map_updater.cc index 07762f174..884f0a16a 100644 --- a/src/openvslam/module/local_map_updater.cc +++ b/src/openvslam/module/local_map_updater.cc @@ -7,7 +7,7 @@ namespace openvslam { namespace module { local_map_updater::local_map_updater(const data::frame& curr_frm, const unsigned int max_num_local_keyfrms) - : frm_id_(curr_frm.id_), frm_lms_(curr_frm.landmarks_), num_keypts_(curr_frm.num_keypts_), + : frm_id_(curr_frm.id_), frm_lms_(curr_frm.landmarks_), num_keypts_(curr_frm.frm_obs_.num_keypts_), max_num_local_keyfrms_(max_num_local_keyfrms) {} std::vector> local_map_updater::get_local_keyframes() const { @@ -22,9 +22,9 @@ std::shared_ptr local_map_updater::get_nearest_covisibility() co return nearest_covisibility_; } -bool local_map_updater::acquire_local_map() { +bool local_map_updater::acquire_local_map(std::unordered_set& outlier_ids) { const auto local_keyfrms_was_found = find_local_keyframes(); - const auto local_lms_was_found = find_local_landmarks(); + const auto local_lms_was_found = find_local_landmarks(outlier_ids); return local_keyfrms_was_found && local_lms_was_found; } @@ -33,8 +33,9 @@ bool local_map_updater::find_local_keyframes() { if (keyfrm_weights.empty()) { return false; } - const auto first_local_keyfrms = find_first_local_keyframes(keyfrm_weights); - const auto second_local_keyfrms = find_second_local_keyframes(first_local_keyfrms); + std::unordered_set already_found_ids; + const auto first_local_keyfrms = find_first_local_keyframes(keyfrm_weights, already_found_ids); + const auto second_local_keyfrms = find_second_local_keyframes(first_local_keyfrms, already_found_ids); local_keyfrms_ = first_local_keyfrms; std::copy(second_local_keyfrms.begin(), second_local_keyfrms.end(), std::back_inserter(local_keyfrms_)); return true; @@ -57,7 +58,8 @@ local_map_updater::keyframe_weights_t local_map_updater::count_keyframe_weights( return keyfrm_weights; } -auto local_map_updater::find_first_local_keyframes(const keyframe_weights_t& keyfrm_weights) +auto local_map_updater::find_first_local_keyframes(const keyframe_weights_t& keyfrm_weights, + std::unordered_set& already_found_ids) -> std::vector> { std::vector> first_local_keyfrms; first_local_keyfrms.reserve(2 * keyfrm_weights.size()); @@ -74,7 +76,7 @@ auto local_map_updater::find_first_local_keyframes(const keyframe_weights_t& key first_local_keyfrms.push_back(keyfrm); // avoid duplication - keyfrm->local_map_update_identifier = frm_id_; + already_found_ids.insert(keyfrm->id_); // update the nearest keyframe if (max_weight < weight) { @@ -86,13 +88,14 @@ auto local_map_updater::find_first_local_keyframes(const keyframe_weights_t& key return first_local_keyfrms; } -auto local_map_updater::find_second_local_keyframes(const std::vector>& first_local_keyframes) const +auto local_map_updater::find_second_local_keyframes(const std::vector>& first_local_keyframes, + std::unordered_set& already_found_ids) const -> std::vector> { std::vector> second_local_keyfrms; second_local_keyfrms.reserve(4 * first_local_keyframes.size()); // add the second-order keyframes to the local landmarks - auto add_second_local_keyframe = [this, &second_local_keyfrms](const std::shared_ptr& keyfrm) { + auto add_second_local_keyframe = [this, &second_local_keyfrms, &already_found_ids](const std::shared_ptr& keyfrm) { if (!keyfrm) { return false; } @@ -100,10 +103,10 @@ auto local_map_updater::find_second_local_keyframes(const std::vectorlocal_map_update_identifier == frm_id_) { + if (already_found_ids.count(keyfrm->id_)) { return false; } - keyfrm->local_map_update_identifier = frm_id_; + already_found_ids.insert(keyfrm->id_); second_local_keyfrms.push_back(keyfrm); return true; }; @@ -138,10 +141,11 @@ auto local_map_updater::find_second_local_keyframes(const std::vector& outlier_ids) { local_lms_.clear(); local_lms_.reserve(50 * local_keyfrms_.size()); + std::unordered_set already_found_ids; for (const auto& keyfrm : local_keyfrms_) { const auto& lms = keyfrm->get_landmarks(); @@ -153,11 +157,14 @@ bool local_map_updater::find_local_landmarks() { continue; } + if (outlier_ids.count(lm->id_)) { + continue; + } // avoid duplication - if (lm->identifier_in_local_map_update_ == frm_id_) { + if (already_found_ids.count(lm->id_)) { continue; } - lm->identifier_in_local_map_update_ = frm_id_; + already_found_ids.insert(lm->id_); local_lms_.push_back(lm); } diff --git a/src/openvslam/module/local_map_updater.h b/src/openvslam/module/local_map_updater.h index 6dd7c7d4f..2326c4e55 100644 --- a/src/openvslam/module/local_map_updater.h +++ b/src/openvslam/module/local_map_updater.h @@ -33,7 +33,7 @@ class local_map_updater { std::shared_ptr get_nearest_covisibility() const; //! Acquire the new local map - bool acquire_local_map(); + bool acquire_local_map(std::unordered_set& outlier_ids); private: //! Find the local keyframes @@ -43,15 +43,17 @@ class local_map_updater { keyframe_weights_t count_keyframe_weights() const; //! Find the first-order local keyframes - auto find_first_local_keyframes(const keyframe_weights_t& keyfrm_weights) + auto find_first_local_keyframes(const keyframe_weights_t& keyfrm_weights, + std::unordered_set& already_found_ids) -> std::vector>; //! Find the second-order local keyframes - auto find_second_local_keyframes(const std::vector>& first_local_keyframes) const + auto find_second_local_keyframes(const std::vector>& first_local_keyframes, + std::unordered_set& already_found_ids) const -> std::vector>; //! Find the local landmarks - bool find_local_landmarks(); + bool find_local_landmarks(std::unordered_set& outlier_ids); // frame ID const unsigned int frm_id_; diff --git a/src/openvslam/module/loop_bundle_adjuster.cc b/src/openvslam/module/loop_bundle_adjuster.cc index 75f7451ce..08b4bb2d4 100644 --- a/src/openvslam/module/loop_bundle_adjuster.cc +++ b/src/openvslam/module/loop_bundle_adjuster.cc @@ -34,7 +34,7 @@ bool loop_bundle_adjuster::is_running() const { return loop_BA_is_running_; } -void loop_bundle_adjuster::optimize(const unsigned int identifier) { +void loop_bundle_adjuster::optimize() { spdlog::info("start loop bundle adjustment"); unsigned int num_exec_loop_BA = 0; @@ -45,8 +45,14 @@ void loop_bundle_adjuster::optimize(const unsigned int identifier) { num_exec_loop_BA = num_exec_loop_BA_; } - const auto global_bundle_adjuster = optimize::global_bundle_adjuster(map_db_, num_iter_, false); - global_bundle_adjuster.optimize(identifier, &abort_loop_BA_); + std::unordered_set optimized_keyfrm_ids; + std::unordered_set optimized_landmark_ids; + eigen_alloc_unord_map lm_to_pos_w_after_global_BA; + eigen_alloc_unord_map keyfrm_to_pose_cw_after_global_BA; + const auto global_BA = optimize::global_bundle_adjuster(map_db_, num_iter_, false); + global_BA.optimize(optimized_keyfrm_ids, optimized_landmark_ids, + lm_to_pos_w_after_global_BA, + keyfrm_to_pose_cw_after_global_BA, &abort_loop_BA_); { std::lock_guard lock1(mtx_thread_); @@ -64,13 +70,16 @@ void loop_bundle_adjuster::optimize(const unsigned int identifier) { spdlog::info("updating the map with pose propagation"); // stop mapping module - mapper_->request_pause(); - while (!mapper_->is_paused() && !mapper_->is_terminated()) { - std::this_thread::sleep_for(std::chrono::microseconds(1000)); + auto future_pause = mapper_->async_pause(); + while (future_pause.wait_for(std::chrono::milliseconds(1)) == std::future_status::timeout) { + while (mapper_->is_terminated()) { + break; + } } std::lock_guard lock2(data::map_database::mtx_database_); + eigen_alloc_unord_map keyfrm_to_cam_pose_cw_before_BA; // update the camera pose along the spanning tree from the origin std::list> keyfrms_to_check; keyfrms_to_check.push_back(map_db_->origin_keyfrm_); @@ -80,16 +89,16 @@ void loop_bundle_adjuster::optimize(const unsigned int identifier) { const auto children = parent->graph_node_->get_spanning_children(); for (auto child : children) { - if (child->loop_BA_identifier_ != identifier) { + if (!optimized_keyfrm_ids.count(child->id_)) { // if `child` is NOT optimized by the loop BA // propagate the pose correction from the spanning parent // parent->child const Mat44_t cam_pose_cp = child->get_cam_pose() * cam_pose_wp; // world->child AFTER correction = parent->child * world->parent AFTER correction - child->cam_pose_cw_after_loop_BA_ = cam_pose_cp * parent->cam_pose_cw_after_loop_BA_; + keyfrm_to_pose_cw_after_global_BA[child->id_] = cam_pose_cp * keyfrm_to_pose_cw_after_global_BA.at(parent->id_); // check as `child` has been corrected - child->loop_BA_identifier_ = identifier; + optimized_keyfrm_ids.insert(child->id_); } // need updating @@ -97,9 +106,9 @@ void loop_bundle_adjuster::optimize(const unsigned int identifier) { } // temporally store the camera pose BEFORE correction (for correction of landmark positions) - parent->cam_pose_cw_before_BA_ = parent->get_cam_pose(); + keyfrm_to_cam_pose_cw_before_BA[parent->id_] = parent->get_cam_pose(); // update the camera pose - parent->set_cam_pose(parent->cam_pose_cw_after_loop_BA_); + parent->set_cam_pose(keyfrm_to_pose_cw_after_global_BA.at(parent->id_)); // finish updating keyfrms_to_check.pop_front(); } @@ -111,11 +120,11 @@ void loop_bundle_adjuster::optimize(const unsigned int identifier) { continue; } - if (lm->loop_BA_identifier_ == identifier) { + if (optimized_landmark_ids.count(lm->id_)) { // if `lm` is optimized by the loop BA // update with the optimized position - lm->set_pos_in_world(lm->pos_w_after_global_BA_); + lm->set_pos_in_world(lm_to_pos_w_after_global_BA.at(lm->id_)); } else { // if `lm` is NOT optimized by the loop BA @@ -123,11 +132,12 @@ void loop_bundle_adjuster::optimize(const unsigned int identifier) { // correct the position according to the move of the camera pose of the reference keyframe auto ref_keyfrm = lm->get_ref_keyframe(); - assert(ref_keyfrm->loop_BA_identifier_ == identifier); + assert(optimized_keyfrm_ids.count(ref_keyfrm->id_)); // convert the position to the camera-reference using the camera pose BEFORE the correction - const Mat33_t rot_cw_before_BA = ref_keyfrm->cam_pose_cw_before_BA_.block<3, 3>(0, 0); - const Vec3_t trans_cw_before_BA = ref_keyfrm->cam_pose_cw_before_BA_.block<3, 1>(0, 3); + const Mat44_t pose_cw_before_BA = keyfrm_to_cam_pose_cw_before_BA.at(ref_keyfrm->id_); + const Mat33_t rot_cw_before_BA = pose_cw_before_BA.block<3, 3>(0, 0); + const Vec3_t trans_cw_before_BA = pose_cw_before_BA.block<3, 1>(0, 3); const Vec3_t pos_c = rot_cw_before_BA * lm->get_pos_in_world() + trans_cw_before_BA; // convert the position to the world-reference using the camera pose AFTER the correction diff --git a/src/openvslam/module/loop_bundle_adjuster.h b/src/openvslam/module/loop_bundle_adjuster.h index 39b7d817a..567018ddd 100644 --- a/src/openvslam/module/loop_bundle_adjuster.h +++ b/src/openvslam/module/loop_bundle_adjuster.h @@ -46,7 +46,7 @@ class loop_bundle_adjuster { /** * Run loop BA */ - void optimize(const unsigned int identifier); + void optimize(); private: //! map database diff --git a/src/openvslam/module/loop_detector.cc b/src/openvslam/module/loop_detector.cc index fe86ac063..92acbd8e2 100644 --- a/src/openvslam/module/loop_detector.cc +++ b/src/openvslam/module/loop_detector.cc @@ -38,11 +38,16 @@ void loop_detector::set_current_keyframe(const std::shared_ptr& } bool loop_detector::detect_loop_candidates() { + auto succeeded = detect_loop_candidates_impl(); + // register to the BoW database + bow_db_->add_keyframe(cur_keyfrm_); + return succeeded; +} + +bool loop_detector::detect_loop_candidates_impl() { // if the loop detector is disabled or the loop has been corrected recently, // cannot perfrom the loop correction if (!loop_detector_is_enabled_ || cur_keyfrm_->id_ < prev_loop_correct_keyfrm_id_ + 10) { - // register to the BoW database - bow_db_->add_keyframe(cur_keyfrm_); return false; } @@ -61,8 +66,6 @@ bool loop_detector::detect_loop_candidates() { if (init_loop_candidates.empty()) { // clear the buffer because any candidates are not found cont_detected_keyfrm_sets_.clear(); - // register to the BoW database - bow_db_->add_keyframe(cur_keyfrm_); return false; } @@ -91,9 +94,6 @@ bool loop_detector::detect_loop_candidates() { cont_detected_keyfrm_sets_ = curr_cont_detected_keyfrm_sets; - // register to the BoW database - bow_db_->add_keyframe(cur_keyfrm_); - // return any candidate is found or not return !loop_candidates_to_validate_.empty(); } @@ -104,6 +104,26 @@ bool loop_detector::validate_candidates() { candidate->set_not_to_be_erased(); } + auto succeeded = validate_candidates_impl(); + if (succeeded) { + // allow the removal of the candidates except for the selected one + for (const auto& loop_candidate : loop_candidates_to_validate_) { + if (*loop_candidate == *selected_candidate_) { + continue; + } + loop_candidate->set_to_be_erased(); + } + } + else { + // allow the removal of all of the candidates + for (const auto& loop_candidate : loop_candidates_to_validate_) { + loop_candidate->set_to_be_erased(); + } + } + return succeeded; +} + +bool loop_detector::validate_candidates_impl() { // 1. for each of the candidates, estimate and validate the Sim3 between it and the current keyframe using the observed landmarks // then, select ONE candaite @@ -112,9 +132,6 @@ bool loop_detector::validate_candidates() { Sim3_world_to_curr_ = util::converter::to_eigen_mat(g2o_Sim3_world_to_curr_); if (!candidate_is_found) { - for (const auto& loop_candidate : loop_candidates_to_validate_) { - loop_candidate->set_to_be_erased(); - } return false; } @@ -169,21 +186,10 @@ bool loop_detector::validate_candidates() { spdlog::debug("acquired {} matches after projection-match", num_final_matches); if (num_final_matches_thr_ <= num_final_matches) { - // allow the removal of the candidates except for the selected one - for (const auto& loop_candidate : loop_candidates_to_validate_) { - if (*loop_candidate == *selected_candidate_) { - continue; - } - loop_candidate->set_to_be_erased(); - } return true; } else { spdlog::debug("destruct loop candidate because enough matches not acquired (< {})", num_final_matches_thr_); - // allow the removal of all of the candidates - for (const auto& loop_candidate : loop_candidates_to_validate_) { - loop_candidate->set_to_be_erased(); - } return false; } } diff --git a/src/openvslam/module/loop_detector.h b/src/openvslam/module/loop_detector.h index e457f5666..3c5f476fb 100644 --- a/src/openvslam/module/loop_detector.h +++ b/src/openvslam/module/loop_detector.h @@ -82,6 +82,16 @@ class loop_detector { void set_loop_correct_keyframe_id(const unsigned int loop_correct_keyfrm_id); private: + /** + * called by detect_loop_candidates + */ + bool detect_loop_candidates_impl(); + + /** + * called by validate_candidates + */ + bool validate_candidates_impl(); + /** * Compute the minimum score among covisibilities */ diff --git a/src/openvslam/module/relocalizer.cc b/src/openvslam/module/relocalizer.cc index 6259b2040..76fb2a806 100644 --- a/src/openvslam/module/relocalizer.cc +++ b/src/openvslam/module/relocalizer.cc @@ -10,21 +10,18 @@ namespace openvslam { namespace module { -relocalizer::relocalizer(data::bow_database* bow_db, - const double bow_match_lowe_ratio, const double proj_match_lowe_ratio, +relocalizer::relocalizer(const double bow_match_lowe_ratio, const double proj_match_lowe_ratio, const double robust_match_lowe_ratio, const unsigned int min_num_bow_matches, const unsigned int min_num_valid_obs) - : bow_db_(bow_db), - min_num_bow_matches_(min_num_bow_matches), min_num_valid_obs_(min_num_valid_obs), + : min_num_bow_matches_(min_num_bow_matches), min_num_valid_obs_(min_num_valid_obs), bow_matcher_(bow_match_lowe_ratio, true), proj_matcher_(proj_match_lowe_ratio, true), robust_matcher_(robust_match_lowe_ratio, false), pose_optimizer_() { spdlog::debug("CONSTRUCT: module::relocalizer"); } -relocalizer::relocalizer(data::bow_database* bow_db, const YAML::Node& yaml_node) - : relocalizer(bow_db, - yaml_node["bow_match_lowe_ratio"].as(0.75), +relocalizer::relocalizer(const YAML::Node& yaml_node) + : relocalizer(yaml_node["bow_match_lowe_ratio"].as(0.75), yaml_node["proj_match_lowe_ratio"].as(0.9), yaml_node["robust_match_lowe_ratio"].as(0.8), yaml_node["min_num_bow_matches"].as(20), @@ -35,11 +32,9 @@ relocalizer::~relocalizer() { spdlog::debug("DESTRUCT: module::relocalizer"); } -bool relocalizer::relocalize(data::frame& curr_frm) { - curr_frm.compute_bow(); - +bool relocalizer::relocalize(data::bow_database* bow_db, data::frame& curr_frm) { // Acquire relocalization candidates - const auto reloc_candidates = bow_db_->acquire_relocalization_candidates(&curr_frm); + const auto reloc_candidates = bow_db->acquire_relocalization_candidates(&curr_frm); if (reloc_candidates.empty()) { return false; } @@ -74,8 +69,8 @@ bool relocalizer::reloc_by_candidates(data::frame& curr_frm, // Setup an PnP solver with the current 2D-3D matches const auto valid_indices = extract_valid_indices(matched_landmarks.at(i)); - auto pnp_solver = setup_pnp_solver(valid_indices, curr_frm.bearings_, curr_frm.keypts_, - matched_landmarks.at(i), curr_frm.scale_factors_); + auto pnp_solver = setup_pnp_solver(valid_indices, curr_frm.frm_obs_.bearings_, curr_frm.frm_obs_.keypts_, + matched_landmarks.at(i), curr_frm.orb_params_->scale_factors_); // 1. Estimate the camera pose using EPnP (+ RANSAC) @@ -94,7 +89,7 @@ bool relocalizer::reloc_by_candidates(data::frame& curr_frm, const auto inlier_indices = util::resample_by_indices(valid_indices, pnp_solver->get_inlier_flags()); // Set 2D-3D matches for the pose optimization - curr_frm.landmarks_ = std::vector>(curr_frm.num_keypts_, nullptr); + curr_frm.landmarks_ = std::vector>(curr_frm.frm_obs_.num_keypts_, nullptr); std::set> already_found_landmarks; for (const auto idx : inlier_indices) { // Set only the valid 3D points to the current frame @@ -112,7 +107,7 @@ bool relocalizer::reloc_by_candidates(data::frame& curr_frm, } // Reject outliers - for (unsigned int idx = 0; idx < curr_frm.num_keypts_; idx++) { + for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; idx++) { if (!curr_frm.outlier_flags_.at(idx)) { continue; } @@ -137,7 +132,7 @@ bool relocalizer::reloc_by_candidates(data::frame& curr_frm, if (num_valid_obs < min_num_valid_obs_) { // Exclude the already-associated landmarks already_found_landmarks.clear(); - for (unsigned int idx = 0; idx < curr_frm.num_keypts_; ++idx) { + for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) { if (!curr_frm.landmarks_.at(idx)) { continue; } @@ -167,7 +162,7 @@ bool relocalizer::reloc_by_candidates(data::frame& curr_frm, // TODO: should set the reference keyframe of the current frame // Reject outliers - for (unsigned int idx = 0; idx < curr_frm.num_keypts_; ++idx) { + for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) { if (!curr_frm.outlier_flags_.at(idx)) { continue; } diff --git a/src/openvslam/module/relocalizer.h b/src/openvslam/module/relocalizer.h index 102b408c9..683c61f1d 100644 --- a/src/openvslam/module/relocalizer.h +++ b/src/openvslam/module/relocalizer.h @@ -21,18 +21,17 @@ namespace module { class relocalizer { public: //! Constructor - explicit relocalizer(data::bow_database* bow_db, - const double bow_match_lowe_ratio = 0.75, const double proj_match_lowe_ratio = 0.9, + explicit relocalizer(const double bow_match_lowe_ratio = 0.75, const double proj_match_lowe_ratio = 0.9, const double robust_match_lowe_ratio = 0.8, const unsigned int min_num_bow_matches = 20, const unsigned int min_num_valid_obs = 50); - relocalizer(data::bow_database* bow_db, const YAML::Node& yaml_node); + explicit relocalizer(const YAML::Node& yaml_node); //! Destructor virtual ~relocalizer(); //! Relocalize the specified frame - bool relocalize(data::frame& curr_frm); + bool relocalize(data::bow_database* bow_db, data::frame& curr_frm); //! Relocalize the specified frame by given candidates list bool reloc_by_candidates(data::frame& curr_frm, @@ -50,9 +49,6 @@ class relocalizer { const std::vector>& matched_landmarks, const std::vector& scale_factors) const; - //! BoW database - data::bow_database* bow_db_; - //! minimum threshold of the number of BoW matches const unsigned int min_num_bow_matches_; //! minimum threshold of the number of valid (= inlier after pose optimization) matches diff --git a/src/openvslam/module/two_view_triangulator.cc b/src/openvslam/module/two_view_triangulator.cc index aede89518..d43db17a0 100644 --- a/src/openvslam/module/two_view_triangulator.cc +++ b/src/openvslam/module/two_view_triangulator.cc @@ -12,21 +12,21 @@ two_view_triangulator::two_view_triangulator(const std::shared_ptrget_cam_pose()), cam_center_1_(keyfrm_1->get_cam_center()), camera_1_(keyfrm_1->camera_), rot_2w_(keyfrm_2->get_rotation()), rot_w2_(rot_2w_.transpose()), trans_2w_(keyfrm_2->get_translation()), cam_pose_2w_(keyfrm_2->get_cam_pose()), cam_center_2_(keyfrm_2->get_cam_center()), camera_2_(keyfrm_2->camera_), - ratio_factor_(2.0f * std::max(keyfrm_1->scale_factor_, keyfrm_2->scale_factor_)), + ratio_factor_(2.0f * std::max(keyfrm_1->orb_params_->scale_factor_, keyfrm_2->orb_params_->scale_factor_)), cos_rays_parallax_thr_(std::cos(rays_parallax_deg_thr * M_PI / 180.0)) {} bool two_view_triangulator::triangulate(const unsigned idx_1, const unsigned int idx_2, Vec3_t& pos_w) const { - const auto& keypt_1 = keyfrm_1_->undist_keypts_.at(idx_1); - const float keypt_1_x_right = keyfrm_1_->stereo_x_right_.at(idx_1); + const auto& keypt_1 = keyfrm_1_->frm_obs_.undist_keypts_.at(idx_1); + const float keypt_1_x_right = keyfrm_1_->frm_obs_.stereo_x_right_.at(idx_1); const bool is_stereo_1 = 0 <= keypt_1_x_right; - const auto& keypt_2 = keyfrm_2_->undist_keypts_.at(idx_2); - const float keypt_2_x_right = keyfrm_2_->stereo_x_right_.at(idx_2); + const auto& keypt_2 = keyfrm_2_->frm_obs_.undist_keypts_.at(idx_2); + const float keypt_2_x_right = keyfrm_2_->frm_obs_.stereo_x_right_.at(idx_2); const bool is_stereo_2 = 0 <= keypt_2_x_right; // rays with reference of each camera - const Vec3_t ray_c_1 = keyfrm_1_->bearings_.at(idx_1); - const Vec3_t ray_c_2 = keyfrm_2_->bearings_.at(idx_2); + const Vec3_t ray_c_1 = keyfrm_1_->frm_obs_.bearings_.at(idx_1); + const Vec3_t ray_c_2 = keyfrm_2_->frm_obs_.bearings_.at(idx_2); // rays with the world reference const Vec3_t ray_w_1 = rot_w1_ * ray_c_1; const Vec3_t ray_w_2 = rot_w2_ * ray_c_2; @@ -34,10 +34,10 @@ bool two_view_triangulator::triangulate(const unsigned idx_1, const unsigned int // compute the stereo parallax if the keypoint is observed as stereo const auto cos_stereo_parallax_1 = is_stereo_1 - ? std::cos(2.0 * atan2(camera_1_->true_baseline_ / 2.0, keyfrm_1_->depths_.at(idx_1))) + ? std::cos(2.0 * atan2(camera_1_->true_baseline_ / 2.0, keyfrm_1_->frm_obs_.depths_.at(idx_1))) : 2.0; const auto cos_stereo_parallax_2 = is_stereo_2 - ? std::cos(2.0 * atan2(camera_2_->true_baseline_ / 2.0, keyfrm_2_->depths_.at(idx_2))) + ? std::cos(2.0 * atan2(camera_2_->true_baseline_ / 2.0, keyfrm_2_->frm_obs_.depths_.at(idx_2))) : 2.0; const auto cos_stereo_parallax = std::min(cos_stereo_parallax_1, cos_stereo_parallax_2); @@ -71,16 +71,16 @@ bool two_view_triangulator::triangulate(const unsigned idx_1, const unsigned int // reject the point if reprojection errors are larger than reasonable threshold if (!check_reprojection_error(pos_w, rot_1w_, trans_1w_, camera_1_, keypt_1.pt, keypt_1_x_right, - keyfrm_1_->level_sigma_sq_.at(keypt_1.octave), is_stereo_1) + keyfrm_1_->orb_params_->level_sigma_sq_.at(keypt_1.octave), is_stereo_1) || !check_reprojection_error(pos_w, rot_2w_, trans_2w_, camera_2_, keypt_2.pt, keypt_2_x_right, - keyfrm_2_->level_sigma_sq_.at(keypt_2.octave), is_stereo_2)) { + keyfrm_2_->orb_params_->level_sigma_sq_.at(keypt_2.octave), is_stereo_2)) { return false; } // reject the point if the real scale factor and the predicted one are much different if (!check_scale_factors(pos_w, - keyfrm_1_->scale_factors_.at(keypt_1.octave), - keyfrm_2_->scale_factors_.at(keypt_2.octave))) { + keyfrm_1_->orb_params_->scale_factors_.at(keypt_1.octave), + keyfrm_2_->orb_params_->scale_factors_.at(keypt_2.octave))) { return false; } diff --git a/src/openvslam/optimize/global_bundle_adjuster.cc b/src/openvslam/optimize/global_bundle_adjuster.cc index 31075db0c..41f601c73 100644 --- a/src/openvslam/optimize/global_bundle_adjuster.cc +++ b/src/openvslam/optimize/global_bundle_adjuster.cc @@ -19,16 +19,14 @@ namespace openvslam { namespace optimize { -global_bundle_adjuster::global_bundle_adjuster(data::map_database* map_db, const unsigned int num_iter, const bool use_huber_kernel) - : map_db_(map_db), num_iter_(num_iter), use_huber_kernel_(use_huber_kernel) {} - -void global_bundle_adjuster::optimize(const unsigned int lead_keyfrm_id_in_global_BA, bool* const force_stop_flag) const { - // 1. Collect the dataset - - const auto keyfrms = map_db_->get_all_keyframes(); - const auto lms = map_db_->get_all_landmarks(); - std::vector is_optimized_lm(lms.size(), true); - +void optimize_impl(std::vector>& keyfrms, + std::vector>& lms, + std::vector& is_optimized_lm, + internal::se3::shot_vertex_container& keyfrm_vtx_container, + internal::landmark_vertex_container& lm_vtx_container, + unsigned int num_iter, + bool use_huber_kernel, + bool* const force_stop_flag) { // 2. Construct an optimizer auto linear_solver = g2o::make_unique>(); @@ -44,10 +42,6 @@ void global_bundle_adjuster::optimize(const unsigned int lead_keyfrm_id_in_globa // 3. Convert each of the keyframe to the g2o vertex, then set it to the optimizer - // Container of the shot vertices - auto vtx_id_offset = std::make_shared(0); - internal::se3::shot_vertex_container keyfrm_vtx_container(vtx_id_offset, keyfrms.size()); - // Set the keyframes to the optimizer for (const auto& keyfrm : keyfrms) { if (!keyfrm) { @@ -63,9 +57,6 @@ void global_bundle_adjuster::optimize(const unsigned int lead_keyfrm_id_in_globa // 4. Connect the vertices of the keyframe and the landmark by using reprojection edge - // Container of the landmark vertices - internal::landmark_vertex_container lm_vtx_container(vtx_id_offset, lms.size()); - // Container of the reprojection edges using reproj_edge_wrapper = internal::se3::reproj_edge_wrapper; std::vector reproj_edge_wraps; @@ -108,15 +99,15 @@ void global_bundle_adjuster::optimize(const unsigned int lead_keyfrm_id_in_globa } const auto keyfrm_vtx = keyfrm_vtx_container.get_vertex(keyfrm); - const auto& undist_keypt = keyfrm->undist_keypts_.at(idx); - const float x_right = keyfrm->stereo_x_right_.at(idx); - const float inv_sigma_sq = keyfrm->inv_level_sigma_sq_.at(undist_keypt.octave); + const auto& undist_keypt = keyfrm->frm_obs_.undist_keypts_.at(idx); + const float x_right = keyfrm->frm_obs_.stereo_x_right_.at(idx); + const float inv_sigma_sq = keyfrm->orb_params_->inv_level_sigma_sq_.at(undist_keypt.octave); const auto sqrt_chi_sq = (keyfrm->camera_->setup_type_ == camera::setup_type_t::Monocular) ? sqrt_chi_sq_2D : sqrt_chi_sq_3D; auto reproj_edge_wrap = reproj_edge_wrapper(keyfrm, keyfrm_vtx, lm, lm_vtx, idx, undist_keypt.pt.x, undist_keypt.pt.y, x_right, - inv_sigma_sq, sqrt_chi_sq, use_huber_kernel_); + inv_sigma_sq, sqrt_chi_sq, use_huber_kernel); reproj_edge_wraps.push_back(reproj_edge_wrap); optimizer.addEdge(reproj_edge_wrap.edge_); ++num_edges; @@ -131,11 +122,29 @@ void global_bundle_adjuster::optimize(const unsigned int lead_keyfrm_id_in_globa // 5. Perform optimization optimizer.initializeOptimization(); - optimizer.optimize(num_iter_); + optimizer.optimize(num_iter); if (force_stop_flag && *force_stop_flag) { return; } +} + +global_bundle_adjuster::global_bundle_adjuster(data::map_database* map_db, const unsigned int num_iter, const bool use_huber_kernel) + : map_db_(map_db), num_iter_(num_iter), use_huber_kernel_(use_huber_kernel) {} + +void global_bundle_adjuster::optimize_for_initialization(bool* const force_stop_flag) const { + // 1. Collect the dataset + auto keyfrms = map_db_->get_all_keyframes(); + auto lms = map_db_->get_all_landmarks(); + std::vector is_optimized_lm(lms.size(), true); + + auto vtx_id_offset = std::make_shared(0); + // Container of the shot vertices + internal::se3::shot_vertex_container keyfrm_vtx_container(vtx_id_offset, keyfrms.size()); + // Container of the landmark vertices + internal::landmark_vertex_container lm_vtx_container(vtx_id_offset, lms.size()); + + optimize_impl(keyfrms, lms, is_optimized_lm, keyfrm_vtx_container, lm_vtx_container, num_iter_, use_huber_kernel_, force_stop_flag); // 6. Extract the result @@ -145,13 +154,8 @@ void global_bundle_adjuster::optimize(const unsigned int lead_keyfrm_id_in_globa } auto keyfrm_vtx = keyfrm_vtx_container.get_vertex(keyfrm); const auto cam_pose_cw = util::converter::to_eigen_mat(keyfrm_vtx->estimate()); - if (lead_keyfrm_id_in_global_BA == 0) { - keyfrm->set_cam_pose(cam_pose_cw); - } - else { - keyfrm->cam_pose_cw_after_loop_BA_ = cam_pose_cw; - keyfrm->loop_BA_identifier_ = lead_keyfrm_id_in_global_BA; - } + + keyfrm->set_cam_pose(cam_pose_cw); } for (unsigned int i = 0; i < lms.size(); ++i) { @@ -170,14 +174,60 @@ void global_bundle_adjuster::optimize(const unsigned int lead_keyfrm_id_in_globa auto lm_vtx = lm_vtx_container.get_vertex(lm); const Vec3_t pos_w = lm_vtx->estimate(); - if (lead_keyfrm_id_in_global_BA == 0) { - lm->set_pos_in_world(pos_w); - lm->update_normal_and_depth(); + lm->set_pos_in_world(pos_w); + lm->update_mean_normal_and_obs_scale_variance(); + } +} + +void global_bundle_adjuster::optimize(std::unordered_set& optimized_keyfrm_ids, + std::unordered_set& optimized_landmark_ids, + eigen_alloc_unord_map& lm_to_pos_w_after_global_BA, + eigen_alloc_unord_map& keyfrm_to_pose_cw_after_global_BA, + bool* const force_stop_flag) const { + // 1. Collect the dataset + auto keyfrms = map_db_->get_all_keyframes(); + auto lms = map_db_->get_all_landmarks(); + std::vector is_optimized_lm(lms.size(), true); + + auto vtx_id_offset = std::make_shared(0); + // Container of the shot vertices + internal::se3::shot_vertex_container keyfrm_vtx_container(vtx_id_offset, keyfrms.size()); + // Container of the landmark vertices + internal::landmark_vertex_container lm_vtx_container(vtx_id_offset, lms.size()); + + optimize_impl(keyfrms, lms, is_optimized_lm, keyfrm_vtx_container, lm_vtx_container, num_iter_, use_huber_kernel_, force_stop_flag); + + // 6. Extract the result + + for (auto keyfrm : keyfrms) { + if (keyfrm->will_be_erased()) { + continue; + } + auto keyfrm_vtx = keyfrm_vtx_container.get_vertex(keyfrm); + const auto cam_pose_cw = util::converter::to_eigen_mat(keyfrm_vtx->estimate()); + + keyfrm_to_pose_cw_after_global_BA[keyfrm->id_] = cam_pose_cw; + optimized_keyfrm_ids.insert(keyfrm->id_); + } + + for (unsigned int i = 0; i < lms.size(); ++i) { + if (!is_optimized_lm.at(i)) { + continue; } - else { - lm->pos_w_after_global_BA_ = pos_w; - lm->loop_BA_identifier_ = lead_keyfrm_id_in_global_BA; + + const auto& lm = lms.at(i); + if (!lm) { + continue; + } + if (lm->will_be_erased()) { + continue; } + + auto lm_vtx = lm_vtx_container.get_vertex(lm); + const Vec3_t pos_w = lm_vtx->estimate(); + + lm_to_pos_w_after_global_BA[lm->id_] = pos_w; + optimized_landmark_ids.insert(lm->id_); } } diff --git a/src/openvslam/optimize/global_bundle_adjuster.h b/src/openvslam/optimize/global_bundle_adjuster.h index dcf66ff2b..ebbe3b3f5 100644 --- a/src/openvslam/optimize/global_bundle_adjuster.h +++ b/src/openvslam/optimize/global_bundle_adjuster.h @@ -24,12 +24,21 @@ class global_bundle_adjuster { */ virtual ~global_bundle_adjuster() = default; + void optimize_for_initialization(bool* const force_stop_flag = nullptr) const; + /** * Perform optimization - * @param lead_keyfrm_id_in_global_BA + * @param optimized_keyfrm_ids + * @param optimized_landmark_ids + * @param lm_to_pos_w_after_global_BA + * @param keyfrm_to_pose_cw_after_global_BA * @param force_stop_flag */ - void optimize(const unsigned int lead_keyfrm_id_in_global_BA = 0, bool* const force_stop_flag = nullptr) const; + void optimize(std::unordered_set& optimized_keyfrm_ids, + std::unordered_set& optimized_landmark_ids, + eigen_alloc_unord_map& lm_to_pos_w_after_global_BA, + eigen_alloc_unord_map& keyfrm_to_pose_cw_after_global_BA, + bool* const force_stop_flag = nullptr) const; private: //! map database diff --git a/src/openvslam/optimize/graph_optimizer.cc b/src/openvslam/optimize/graph_optimizer.cc index 9cb0def59..904d51beb 100644 --- a/src/openvslam/optimize/graph_optimizer.cc +++ b/src/openvslam/optimize/graph_optimizer.cc @@ -23,7 +23,8 @@ graph_optimizer::graph_optimizer(data::map_database* map_db, const bool fix_scal void graph_optimizer::optimize(const std::shared_ptr& loop_keyfrm, const std::shared_ptr& curr_keyfrm, const module::keyframe_Sim3_pairs_t& non_corrected_Sim3s, const module::keyframe_Sim3_pairs_t& pre_corrected_Sim3s, - const std::map, std::set>>& loop_connections) const { + const std::map, std::set>>& loop_connections, + std::unordered_map& found_lm_to_ref_keyfrm_id) const { // 1. Construct an optimizer auto linear_solver = g2o::make_unique>(); @@ -38,12 +39,10 @@ void graph_optimizer::optimize(const std::shared_ptr& loop_keyfr const auto all_keyfrms = map_db_->get_all_keyframes(); const auto all_lms = map_db_->get_all_landmarks(); - const unsigned int max_keyfrm_id = map_db_->get_max_keyframe_id(); - // Transform the pre-modified poses of all the keyframes to Sim3, and save them - std::vector> Sim3s_cw(max_keyfrm_id + 1); + eigen_alloc_unord_map Sim3s_cw; // Save the added vertices - std::vector vertices(max_keyfrm_id + 1); + std::unordered_map vertices; constexpr int min_weight = 100; @@ -59,7 +58,7 @@ void graph_optimizer::optimize(const std::shared_ptr& loop_keyfr const auto iter = pre_corrected_Sim3s.find(keyfrm); if (iter != pre_corrected_Sim3s.end()) { // BEFORE optimization, set the already-modified poses for verices - Sim3s_cw.at(id) = iter->second; + Sim3s_cw[id] = iter->second; keyfrm_vtx->setEstimate(iter->second); } else { @@ -68,7 +67,7 @@ void graph_optimizer::optimize(const std::shared_ptr& loop_keyfr const Vec3_t trans_cw = keyfrm->get_translation(); const g2o::Sim3 Sim3_cw(rot_cw, trans_cw, 1.0); - Sim3s_cw.at(id) = Sim3_cw; + Sim3s_cw[id] = Sim3_cw; keyfrm_vtx->setEstimate(Sim3_cw); } @@ -82,7 +81,7 @@ void graph_optimizer::optimize(const std::shared_ptr& loop_keyfr keyfrm_vtx->fix_scale_ = fix_scale_; optimizer.addVertex(keyfrm_vtx); - vertices.at(id) = keyfrm_vtx; + vertices[id] = keyfrm_vtx; } // 3. Add edges @@ -240,7 +239,7 @@ void graph_optimizer::optimize(const std::shared_ptr& loop_keyfr std::lock_guard lock(data::map_database::mtx_database_); // For modification of a point-cloud, save the post-modified poses of all the keyframes - std::vector> corrected_Sim3s_wc(max_keyfrm_id + 1); + std::unordered_map corrected_Sim3s_wc; for (auto keyfrm : all_keyfrms) { const auto id = keyfrm->id_; @@ -255,7 +254,7 @@ void graph_optimizer::optimize(const std::shared_ptr& loop_keyfr const Mat44_t cam_pose_cw = util::converter::to_eigen_cam_pose(rot_cw, trans_cw); keyfrm->set_cam_pose(cam_pose_cw); - corrected_Sim3s_wc.at(id) = corrected_Sim3_cw.inverse(); + corrected_Sim3s_wc[id] = corrected_Sim3_cw.inverse(); } // Update the point-cloud @@ -264,8 +263,8 @@ void graph_optimizer::optimize(const std::shared_ptr& loop_keyfr continue; } - const auto id = (lm->loop_fusion_identifier_ == curr_keyfrm->id_) - ? lm->ref_keyfrm_id_in_loop_fusion_ + const auto id = (found_lm_to_ref_keyfrm_id.count(lm->id_)) + ? found_lm_to_ref_keyfrm_id.at(lm->id_) : lm->get_ref_keyframe()->id_; const g2o::Sim3& Sim3_cw = Sim3s_cw.at(id); @@ -275,7 +274,7 @@ void graph_optimizer::optimize(const std::shared_ptr& loop_keyfr const Vec3_t corrected_pos_w = corrected_Sim3_wc.map(Sim3_cw.map(pos_w)); lm->set_pos_in_world(corrected_pos_w); - lm->update_normal_and_depth(); + lm->update_mean_normal_and_obs_scale_variance(); } } } diff --git a/src/openvslam/optimize/graph_optimizer.h b/src/openvslam/optimize/graph_optimizer.h index e24b98d27..db16f604f 100644 --- a/src/openvslam/optimize/graph_optimizer.h +++ b/src/openvslam/optimize/graph_optimizer.h @@ -41,7 +41,8 @@ class graph_optimizer { void optimize(const std::shared_ptr& loop_keyfrm, const std::shared_ptr& curr_keyfrm, const module::keyframe_Sim3_pairs_t& non_corrected_Sim3s, const module::keyframe_Sim3_pairs_t& pre_corrected_Sim3s, - const std::map, std::set>>& loop_connections) const; + const std::map, std::set>>& loop_connections, + std::unordered_map& found_lm_to_ref_keyfrm_id) const; private: //! map database diff --git a/src/openvslam/optimize/internal/sim3/mutual_reproj_edge_wrapper.h b/src/openvslam/optimize/internal/sim3/mutual_reproj_edge_wrapper.h index 203f44d15..53eaa7380 100644 --- a/src/openvslam/optimize/internal/sim3/mutual_reproj_edge_wrapper.h +++ b/src/openvslam/optimize/internal/sim3/mutual_reproj_edge_wrapper.h @@ -68,9 +68,9 @@ inline mutual_reproj_edge_wapper::mutual_reproj_edge_wapper(const std::shared // 3次元点はkeyfrm_2で観測しているもの，カメラモデルと特徴点はkeyfrm_1のもの auto edge_12 = new internal::sim3::perspective_forward_reproj_edge(); // 特徴点情報と再投影誤差分散をセット - const auto& undist_keypt_1 = shot1->undist_keypts_.at(idx1); + const auto& undist_keypt_1 = shot1->frm_obs_.undist_keypts_.at(idx1); const Vec2_t obs_1{undist_keypt_1.pt.x, undist_keypt_1.pt.y}; - const float inv_sigma_sq_1 = shot1->inv_level_sigma_sq_.at(undist_keypt_1.octave); + const float inv_sigma_sq_1 = shot1->orb_params_->inv_level_sigma_sq_.at(undist_keypt_1.octave); edge_12->setMeasurement(obs_1); edge_12->setInformation(Mat22_t::Identity() * inv_sigma_sq_1); // 3次元点をセット @@ -92,9 +92,9 @@ inline mutual_reproj_edge_wapper::mutual_reproj_edge_wapper(const std::shared // 3次元点はkeyfrm_2で観測しているもの，カメラモデルと特徴点はkeyfrm_1のもの auto edge_12 = new internal::sim3::perspective_forward_reproj_edge(); // 特徴点情報と再投影誤差分散をセット - const auto& undist_keypt_1 = shot1->undist_keypts_.at(idx1); + const auto& undist_keypt_1 = shot1->frm_obs_.undist_keypts_.at(idx1); const Vec2_t obs_1{undist_keypt_1.pt.x, undist_keypt_1.pt.y}; - const float inv_sigma_sq_1 = shot1->inv_level_sigma_sq_.at(undist_keypt_1.octave); + const float inv_sigma_sq_1 = shot1->orb_params_->inv_level_sigma_sq_.at(undist_keypt_1.octave); edge_12->setMeasurement(obs_1); edge_12->setInformation(Mat22_t::Identity() * inv_sigma_sq_1); // 3次元点をセット @@ -116,9 +116,9 @@ inline mutual_reproj_edge_wapper::mutual_reproj_edge_wapper(const std::shared // 3次元点はkeyfrm_2で観測しているもの，カメラモデルと特徴点はkeyfrm_1のもの auto edge_12 = new internal::sim3::equirectangular_forward_reproj_edge(); // 特徴点情報と再投影誤差分散をセット - const auto& undist_keypt_1 = shot1->undist_keypts_.at(idx1); + const auto& undist_keypt_1 = shot1->frm_obs_.undist_keypts_.at(idx1); const Vec2_t obs_1{undist_keypt_1.pt.x, undist_keypt_1.pt.y}; - const float inv_sigma_sq_1 = shot1->inv_level_sigma_sq_.at(undist_keypt_1.octave); + const float inv_sigma_sq_1 = shot1->orb_params_->inv_level_sigma_sq_.at(undist_keypt_1.octave); edge_12->setMeasurement(obs_1); edge_12->setInformation(Mat22_t::Identity() * inv_sigma_sq_1); // 3次元点をセット @@ -138,9 +138,9 @@ inline mutual_reproj_edge_wapper::mutual_reproj_edge_wapper(const std::shared // 3次元点はkeyfrm_2で観測しているもの，カメラモデルと特徴点はkeyfrm_1のもの auto edge_12 = new internal::sim3::perspective_forward_reproj_edge(); // 特徴点情報と再投影誤差分散をセット - const auto& undist_keypt_1 = shot1->undist_keypts_.at(idx1); + const auto& undist_keypt_1 = shot1->frm_obs_.undist_keypts_.at(idx1); const Vec2_t obs_1{undist_keypt_1.pt.x, undist_keypt_1.pt.y}; - const float inv_sigma_sq_1 = shot1->inv_level_sigma_sq_.at(undist_keypt_1.octave); + const float inv_sigma_sq_1 = shot1->orb_params_->inv_level_sigma_sq_.at(undist_keypt_1.octave); edge_12->setMeasurement(obs_1); edge_12->setInformation(Mat22_t::Identity() * inv_sigma_sq_1); // 3次元点をセット @@ -175,9 +175,9 @@ inline mutual_reproj_edge_wapper::mutual_reproj_edge_wapper(const std::shared // 3次元点はkeyfrm_1で観測しているもの，カメラモデルと特徴点はkeyfrm_2のもの auto edge_21 = new internal::sim3::perspective_backward_reproj_edge(); // 特徴点情報と再投影誤差分散をセット - const auto& undist_keypt_2 = shot2->undist_keypts_.at(idx2); + const auto& undist_keypt_2 = shot2->frm_obs_.undist_keypts_.at(idx2); const Vec2_t obs_2{undist_keypt_2.pt.x, undist_keypt_2.pt.y}; - const float inv_sigma_sq_2 = shot2->inv_level_sigma_sq_.at(undist_keypt_2.octave); + const float inv_sigma_sq_2 = shot2->orb_params_->inv_level_sigma_sq_.at(undist_keypt_2.octave); edge_21->setMeasurement(obs_2); edge_21->setInformation(Mat22_t::Identity() * inv_sigma_sq_2); // 3次元点をセット @@ -199,9 +199,9 @@ inline mutual_reproj_edge_wapper::mutual_reproj_edge_wapper(const std::shared // 3次元点はkeyfrm_1で観測しているもの，カメラモデルと特徴点はkeyfrm_2のもの auto edge_21 = new internal::sim3::perspective_backward_reproj_edge(); // 特徴点情報と再投影誤差分散をセット - const auto& undist_keypt_2 = shot2->undist_keypts_.at(idx2); + const auto& undist_keypt_2 = shot2->frm_obs_.undist_keypts_.at(idx2); const Vec2_t obs_2{undist_keypt_2.pt.x, undist_keypt_2.pt.y}; - const float inv_sigma_sq_2 = shot2->inv_level_sigma_sq_.at(undist_keypt_2.octave); + const float inv_sigma_sq_2 = shot2->orb_params_->inv_level_sigma_sq_.at(undist_keypt_2.octave); edge_21->setMeasurement(obs_2); edge_21->setInformation(Mat22_t::Identity() * inv_sigma_sq_2); // 3次元点をセット @@ -223,9 +223,9 @@ inline mutual_reproj_edge_wapper::mutual_reproj_edge_wapper(const std::shared // 3次元点はkeyfrm_1で観測しているもの，カメラモデルと特徴点はkeyfrm_2のもの auto edge_21 = new internal::sim3::equirectangular_backward_reproj_edge(); // 特徴点情報と再投影誤差分散をセット - const auto& undist_keypt_2 = shot2->undist_keypts_.at(idx2); + const auto& undist_keypt_2 = shot2->frm_obs_.undist_keypts_.at(idx2); const Vec2_t obs_2{undist_keypt_2.pt.x, undist_keypt_2.pt.y}; - const float inv_sigma_sq_2 = shot2->inv_level_sigma_sq_.at(undist_keypt_2.octave); + const float inv_sigma_sq_2 = shot2->orb_params_->inv_level_sigma_sq_.at(undist_keypt_2.octave); edge_21->setMeasurement(obs_2); edge_21->setInformation(Mat22_t::Identity() * inv_sigma_sq_2); // 3次元点をセット @@ -245,9 +245,9 @@ inline mutual_reproj_edge_wapper::mutual_reproj_edge_wapper(const std::shared // 3次元点はkeyfrm_1で観測しているもの，カメラモデルと特徴点はkeyfrm_2のもの auto edge_21 = new internal::sim3::perspective_backward_reproj_edge(); // 特徴点情報と再投影誤差分散をセット - const auto& undist_keypt_2 = shot2->undist_keypts_.at(idx2); + const auto& undist_keypt_2 = shot2->frm_obs_.undist_keypts_.at(idx2); const Vec2_t obs_2{undist_keypt_2.pt.x, undist_keypt_2.pt.y}; - const float inv_sigma_sq_2 = shot2->inv_level_sigma_sq_.at(undist_keypt_2.octave); + const float inv_sigma_sq_2 = shot2->orb_params_->inv_level_sigma_sq_.at(undist_keypt_2.octave); edge_21->setMeasurement(obs_2); edge_21->setInformation(Mat22_t::Identity() * inv_sigma_sq_2); // 3次元点をセット diff --git a/src/openvslam/optimize/local_bundle_adjuster.cc b/src/openvslam/optimize/local_bundle_adjuster.cc index e6b822008..3bf481514 100644 --- a/src/openvslam/optimize/local_bundle_adjuster.cc +++ b/src/openvslam/optimize/local_bundle_adjuster.cc @@ -27,7 +27,8 @@ local_bundle_adjuster::local_bundle_adjuster(const unsigned int num_first_iter, const unsigned int num_second_iter) : num_first_iter_(num_first_iter), num_second_iter_(num_second_iter) {} -void local_bundle_adjuster::optimize(const std::shared_ptr& curr_keyfrm, bool* const force_stop_flag) const { +void local_bundle_adjuster::optimize(data::map_database* map_db, + const std::shared_ptr& curr_keyfrm, bool* const force_stop_flag) const { // 1. Aggregate the local and fixed keyframes, and local landmarks // Correct the local keyframes of the current keyframe @@ -172,9 +173,9 @@ void local_bundle_adjuster::optimize(const std::shared_ptrundist_keypts_.at(idx); - const float x_right = keyfrm->stereo_x_right_.at(idx); - const float inv_sigma_sq = keyfrm->inv_level_sigma_sq_.at(undist_keypt.octave); + const auto& undist_keypt = keyfrm->frm_obs_.undist_keypts_.at(idx); + const float x_right = keyfrm->frm_obs_.stereo_x_right_.at(idx); + const float inv_sigma_sq = keyfrm->orb_params_->inv_level_sigma_sq_.at(undist_keypt.octave); const auto sqrt_chi_sq = (keyfrm->camera_->setup_type_ == camera::setup_type_t::Monocular) ? sqrt_chi_sq_2D : sqrt_chi_sq_3D; @@ -188,10 +189,8 @@ void local_bundle_adjuster::optimize(const std::shared_ptrerase_landmark(lm); - lm->erase_observation(keyfrm); + lm->erase_observation(map_db, keyfrm); } for (const auto& id_local_keyfrm_pair : local_keyfrms) { @@ -283,7 +280,7 @@ void local_bundle_adjuster::optimize(const std::shared_ptrset_pos_in_world(lm_vtx->estimate()); - local_lm->update_normal_and_depth(); + local_lm->update_mean_normal_and_obs_scale_variance(); } } } diff --git a/src/openvslam/optimize/local_bundle_adjuster.h b/src/openvslam/optimize/local_bundle_adjuster.h index afca2a0e2..0df35f409 100644 --- a/src/openvslam/optimize/local_bundle_adjuster.h +++ b/src/openvslam/optimize/local_bundle_adjuster.h @@ -16,7 +16,6 @@ class local_bundle_adjuster { public: /** * Constructor - * @param map_db * @param num_first_iter * @param num_second_iter */ @@ -30,10 +29,11 @@ class local_bundle_adjuster { /** * Perform optimization + * @param map_db * @param curr_keyfrm * @param force_stop_flag */ - void optimize(const std::shared_ptr& curr_keyfrm, bool* const force_stop_flag) const; + void optimize(data::map_database* map_db, const std::shared_ptr& curr_keyfrm, bool* const force_stop_flag) const; private: //! number of iterations of first optimization diff --git a/src/openvslam/optimize/pose_optimizer.cc b/src/openvslam/optimize/pose_optimizer.cc index a12536caa..acd682e82 100644 --- a/src/openvslam/optimize/pose_optimizer.cc +++ b/src/openvslam/optimize/pose_optimizer.cc @@ -42,7 +42,7 @@ unsigned int pose_optimizer::optimize(data::frame& frm) const { frm_vtx->setFixed(false); optimizer.addVertex(frm_vtx); - const unsigned int num_keypts = frm.num_keypts_; + const unsigned int num_keypts = frm.frm_obs_.num_keypts_; // 3. Connect the landmark vertices by using projection edges @@ -72,9 +72,9 @@ unsigned int pose_optimizer::optimize(data::frame& frm) const { frm.outlier_flags_.at(idx) = false; // Connect the frame and the landmark vertices using the projection edges - const auto& undist_keypt = frm.undist_keypts_.at(idx); - const float x_right = frm.stereo_x_right_.at(idx); - const float inv_sigma_sq = frm.inv_level_sigma_sq_.at(undist_keypt.octave); + const auto& undist_keypt = frm.frm_obs_.undist_keypts_.at(idx); + const float x_right = frm.frm_obs_.stereo_x_right_.at(idx); + const float inv_sigma_sq = frm.orb_params_->inv_level_sigma_sq_.at(undist_keypt.octave); const auto sqrt_chi_sq = (frm.camera_->setup_type_ == camera::setup_type_t::Monocular) ? sqrt_chi_sq_2D : sqrt_chi_sq_3D; diff --git a/src/openvslam/publish/frame_publisher.cc b/src/openvslam/publish/frame_publisher.cc index 1aeb25b24..b8cdedfdb 100644 --- a/src/openvslam/publish/frame_publisher.cc +++ b/src/openvslam/publish/frame_publisher.cc @@ -21,13 +21,12 @@ frame_publisher::~frame_publisher() { spdlog::debug("DESTRUCT: publish::frame_publisher"); } -cv::Mat frame_publisher::draw_frame(const bool draw_text) { +cv::Mat frame_publisher::draw_frame() { cv::Mat img; tracker_state_t tracking_state; std::vector init_keypts; std::vector init_matches; std::vector curr_keypts; - double elapsed_ms; bool mapping_is_enabled; std::vector is_tracked; @@ -47,8 +46,6 @@ cv::Mat frame_publisher::draw_frame(const bool draw_text) { curr_keypts = curr_keypts_; - elapsed_ms = elapsed_ms_; - mapping_is_enabled = mapping_is_enabled_; is_tracked = is_tracked_; @@ -81,10 +78,7 @@ cv::Mat frame_publisher::draw_frame(const bool draw_text) { } } - if (draw_text) { - // draw tracking info - draw_info_text(img, tracking_state, num_tracked, elapsed_ms, mapping_is_enabled); - } + spdlog::trace("num_tracked: {}", num_tracked); return img; } @@ -139,59 +133,16 @@ unsigned int frame_publisher::draw_tracked_points(cv::Mat& img, const std::vecto return num_tracked; } -void frame_publisher::draw_info_text(cv::Mat& img, const tracker_state_t tracking_state, const unsigned int num_tracked, - const double elapsed_ms, const bool mapping_is_enabled) const { - // create text info - std::stringstream ss; - switch (tracking_state) { - case tracker_state_t::NotInitialized: { - ss << "WAITING FOR IMAGES "; - break; - } - case tracker_state_t::Initializing: { - ss << "INITIALIZE | "; - ss << "KP: " << num_tracked << ", "; - ss << "track time: " << std::fixed << std::setprecision(0) << elapsed_ms << "ms"; - break; - } - case tracker_state_t::Tracking: { - ss << (mapping_is_enabled ? "MAPPING | " : "LOCALIZATION | "); - ss << "KF: " << map_db_->get_num_keyframes() << ", "; - ss << "LM: " << map_db_->get_num_landmarks() << ", "; - ss << "KP: " << num_tracked << ", "; - ss << "track time: " << std::fixed << std::setprecision(0) << elapsed_ms << "ms"; - break; - } - case tracker_state_t::Lost: { - ss << "LOST | "; - ss << "track time: " << std::fixed << std::setprecision(0) << elapsed_ms << "ms"; - break; - } - } - - int baseline = 0; - const cv::Size text_size = cv::getTextSize(ss.str(), cv::FONT_HERSHEY_PLAIN, 1, 1, &baseline); - - // create text area - constexpr float alpha = 0.6; - cv::Mat overlay(img.rows, img.cols, img.type()); - img.copyTo(overlay); - cv::rectangle(overlay, cv::Point2i{0, img.rows - text_size.height - 10}, cv::Point2i{img.cols, img.rows}, cv::Scalar{0, 0, 0}, -1); - cv::addWeighted(overlay, alpha, img, 1 - alpha, 0, img); - // put text - cv::putText(img, ss.str(), cv::Point(5, img.rows - 5), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar{255, 255, 255}, 1, 8); -} - -void frame_publisher::update(tracking_module* tracker) { +void frame_publisher::update(tracking_module* tracker, const cv::Mat& img, double elapsed_ms) { std::lock_guard lock(mtx_); - tracker->img_gray_.copyTo(img_); + img.copyTo(img_); - const auto num_curr_keypts = tracker->curr_frm_.num_keypts_; - curr_keypts_ = tracker->curr_frm_.keypts_; - elapsed_ms_ = tracker->elapsed_ms_; + const auto num_curr_keypts = tracker->curr_frm_.frm_obs_.num_keypts_; + curr_keypts_ = tracker->curr_frm_.frm_obs_.keypts_; + elapsed_ms_ = elapsed_ms; mapping_is_enabled_ = tracker->get_mapping_module_status(); - tracking_state_ = tracker->last_tracking_state_; + tracking_state_ = tracker->tracking_state_; is_tracked_ = std::vector(num_curr_keypts, false); diff --git a/src/openvslam/publish/frame_publisher.h b/src/openvslam/publish/frame_publisher.h index e02d1b2c6..eced00a54 100644 --- a/src/openvslam/publish/frame_publisher.h +++ b/src/openvslam/publish/frame_publisher.h @@ -37,13 +37,13 @@ class frame_publisher { * Update tracking information * NOTE: should be accessed from system thread */ - void update(tracking_module* tracker); + void update(tracking_module* tracker, const cv::Mat& img, double elapsed_ms); /** * Get the current image with tracking information * NOTE: should be accessed from viewer thread */ - cv::Mat draw_frame(const bool draw_text = true); + cv::Mat draw_frame(); protected: unsigned int draw_initial_points(cv::Mat& img, const std::vector& init_keypts, @@ -54,9 +54,6 @@ class frame_publisher { const std::vector& is_tracked, const bool mapping_is_enabled, const float mag = 1.0) const; - void draw_info_text(cv::Mat& img, const tracker_state_t tracking_state, const unsigned int num_tracked, - const double elapsed_ms, const bool mapping_is_enabled) const; - // colors (BGR) const cv::Scalar mapping_color_{0, 255, 255}; const cv::Scalar localization_color_{255, 255, 0}; diff --git a/src/openvslam/publish/map_publisher.cc b/src/openvslam/publish/map_publisher.cc index 8511316e2..fb802a70f 100644 --- a/src/openvslam/publish/map_publisher.cc +++ b/src/openvslam/publish/map_publisher.cc @@ -27,16 +27,6 @@ Mat44_t map_publisher::get_current_cam_pose() { return cam_pose_cw_; } -void map_publisher::set_current_cam_pose_wc(const Mat44_t& cam_pose_wc) { - std::lock_guard lock(mtx_cam_pose_); - cam_pose_wc_ = cam_pose_wc; -} - -Mat44_t map_publisher::get_current_cam_pose_wc() { - std::lock_guard lock(mtx_cam_pose_); - return cam_pose_wc_; -} - unsigned int map_publisher::get_keyframes(std::vector>& all_keyfrms) { all_keyfrms = map_db_->get_all_keyframes(); return map_db_->get_num_keyframes(); diff --git a/src/openvslam/publish/map_publisher.h b/src/openvslam/publish/map_publisher.h index 1571f12a4..a29330fa1 100644 --- a/src/openvslam/publish/map_publisher.h +++ b/src/openvslam/publish/map_publisher.h @@ -48,20 +48,6 @@ class map_publisher { */ Mat44_t get_current_cam_pose(); - /** - * Set current camera pose - * NOTE: should be accessed from tracker thread - * @param cam_pose_wc - */ - void set_current_cam_pose_wc(const Mat44_t& cam_pose_wc); - - /** - * Get current camera pose - * NOTE: should be accessed from viewer thread - * @return - */ - Mat44_t get_current_cam_pose_wc(); - /** * Get all keyframes * @param all_keyfrms diff --git a/src/openvslam/solve/sim3_solver.cc b/src/openvslam/solve/sim3_solver.cc index aa8aea38a..9aea82ca4 100644 --- a/src/openvslam/solve/sim3_solver.cc +++ b/src/openvslam/solve/sim3_solver.cc @@ -64,11 +64,11 @@ sim3_solver::sim3_solver(const std::shared_ptr& keyfrm_1, const continue; } - const auto& keypt_1 = keyfrm_1_->undist_keypts_.at(idx1); - const auto& keypt_2 = keyfrm_2_->undist_keypts_.at(idx2); + const auto& keypt_1 = keyfrm_1_->frm_obs_.undist_keypts_.at(idx1); + const auto& keypt_2 = keyfrm_2_->frm_obs_.undist_keypts_.at(idx2); - const float sigma_sq_1 = keyfrm_1_->level_sigma_sq_.at(keypt_1.octave); - const float sigma_sq_2 = keyfrm_2_->level_sigma_sq_.at(keypt_2.octave); + const float sigma_sq_1 = keyfrm_1_->orb_params_->level_sigma_sq_.at(keypt_1.octave); + const float sigma_sq_2 = keyfrm_2_->orb_params_->level_sigma_sq_.at(keypt_2.octave); chi_sq_x_sigma_sq_1_.push_back(chi_sq_2D * sigma_sq_1); chi_sq_x_sigma_sq_2_.push_back(chi_sq_2D * sigma_sq_2); diff --git a/src/openvslam/system.cc b/src/openvslam/system.cc index 32e9e4d86..8bfbf4f51 100644 --- a/src/openvslam/system.cc +++ b/src/openvslam/system.cc @@ -5,23 +5,46 @@ #include "openvslam/global_optimization_module.h" #include "openvslam/camera/base.h" #include "openvslam/data/camera_database.h" +#include "openvslam/data/common.h" +#include "openvslam/data/frame_observation.h" +#include "openvslam/data/orb_params_database.h" #include "openvslam/data/map_database.h" #include "openvslam/data/bow_database.h" #include "openvslam/data/bow_vocabulary.h" +#include "openvslam/match/stereo.h" +#include "openvslam/feature/orb_extractor.h" #include "openvslam/io/trajectory_io.h" #include "openvslam/io/map_database_io.h" #include "openvslam/publish/map_publisher.h" #include "openvslam/publish/frame_publisher.h" +#include "openvslam/util/converter.h" +#include "openvslam/util/image_converter.h" #include "openvslam/util/yaml.h" #include #include +namespace { +using namespace openvslam; + +double get_depthmap_factor(const camera::base* camera, const YAML::Node& yaml_node) { + spdlog::debug("load depthmap factor"); + double depthmap_factor = 1.0; + if (camera->setup_type_ == camera::setup_type_t::RGBD) { + depthmap_factor = yaml_node["depthmap_factor"].as(depthmap_factor); + } + if (depthmap_factor < 0.) { + throw std::runtime_error("depthmap_factor must be greater than 0"); + } + return depthmap_factor; +} +} // namespace + namespace openvslam { system::system(const std::shared_ptr& cfg, const std::string& vocab_file_path) - : cfg_(cfg), camera_(cfg->camera_) { + : cfg_(cfg), camera_(cfg->camera_), orb_params_(cfg->orb_params_) { spdlog::debug("CONSTRUCT: system"); std::ostringstream message_stream; @@ -84,12 +107,40 @@ system::system(const std::shared_ptr& cfg, const std::string& vocab_file map_publisher_ = std::shared_ptr(new publish::map_publisher(cfg_, map_db_)); // tracking module - tracker_ = new tracking_module(cfg_, this, map_db_, bow_vocab_, bow_db_); + tracker_ = new tracking_module(cfg_, map_db_, bow_vocab_, bow_db_); // mapping module - mapper_ = new mapping_module(cfg_->yaml_node_["Mapping"], map_db_); + mapper_ = new mapping_module(cfg_->yaml_node_["Mapping"], map_db_, bow_db_, bow_vocab_); // global optimization module global_optimizer_ = new global_optimization_module(map_db_, bow_db_, bow_vocab_, cfg_->yaml_node_, camera_->setup_type_ != camera::setup_type_t::Monocular); + // preprocessing modules + const auto preprocessing_params = util::yaml_optional_ref(cfg->yaml_node_, "Preprocessing"); + depthmap_factor_ = get_depthmap_factor(camera_, preprocessing_params); + auto mask_rectangles = preprocessing_params["mask_rectangles"].as>>(std::vector>()); + for (const auto& v : mask_rectangles) { + if (v.size() != 4) { + throw std::runtime_error("mask rectangle must contain four parameters"); + } + if (v.at(0) >= v.at(1)) { + throw std::runtime_error("x_max must be greater than x_min"); + } + if (v.at(2) >= v.at(3)) { + throw std::runtime_error("y_max must be greater than x_min"); + } + } + + orb_params_db_ = new data::orb_params_database(orb_params_); + + const auto max_num_keypoints = preprocessing_params["max_num_keypoints"].as(2000); + extractor_left_ = new feature::orb_extractor(orb_params_, max_num_keypoints, mask_rectangles); + if (camera_->setup_type_ == camera::setup_type_t::Monocular) { + const auto ini_max_num_keypoints = preprocessing_params["ini_max_num_keypoints"].as(2 * extractor_left_->get_max_num_keypoints()); + ini_extractor_left_ = new feature::orb_extractor(orb_params_, ini_max_num_keypoints, mask_rectangles); + } + if (camera_->setup_type_ == camera::setup_type_t::Stereo) { + extractor_right_ = new feature::orb_extractor(orb_params_, max_num_keypoints, mask_rectangles); + } + // connect modules each other tracker_->set_mapping_module(mapper_); tracker_->set_global_optimization_module(global_optimizer_); @@ -120,6 +171,16 @@ system::~system() { delete bow_vocab_; bow_vocab_ = nullptr; + delete extractor_left_; + extractor_left_ = nullptr; + delete extractor_right_; + extractor_right_ = nullptr; + delete ini_extractor_left_; + ini_extractor_left_ = nullptr; + + delete orb_params_db_; + orb_params_db_ = nullptr; + spdlog::debug("DESTRUCT: system"); } @@ -137,14 +198,10 @@ void system::startup(const bool need_initialize) { void system::shutdown() { // terminate the other threads - mapper_->request_terminate(); - global_optimizer_->request_terminate(); - // wait until they stop - while (!mapper_->is_terminated() - || !global_optimizer_->is_terminated() - || global_optimizer_->loop_BA_is_running()) { - std::this_thread::sleep_for(std::chrono::microseconds(5000)); - } + auto future_mapper_terminate = mapper_->async_terminate(); + auto future_global_optimizer_terminate = global_optimizer_->async_terminate(); + future_mapper_terminate.get(); + future_global_optimizer_terminate.get(); // wait until the threads stop mapping_thread_->join(); @@ -170,14 +227,14 @@ void system::save_keyframe_trajectory(const std::string& path, const std::string void system::load_map_database(const std::string& path) const { pause_other_threads(); - io::map_database_io map_db_io(cam_db_, map_db_, bow_db_, bow_vocab_); + io::map_database_io map_db_io(cam_db_, orb_params_db_, map_db_, bow_db_, bow_vocab_); map_db_io.load_message_pack(path); resume_other_threads(); } void system::save_map_database(const std::string& path) const { pause_other_threads(); - io::map_database_io map_db_io(cam_db_, map_db_, bow_db_, bow_vocab_); + io::map_database_io map_db_io(cam_db_, orb_params_db_, map_db_, bow_db_, bow_vocab_); map_db_io.save_message_pack(path); resume_other_threads(); } @@ -207,11 +264,9 @@ void system::disable_mapping_module() { spdlog::critical("please call system::disable_mapping_module() after system::startup()"); } // pause the mapping module - mapper_->request_pause(); + auto future_pause = mapper_->async_pause(); // wait until it stops - while (!mapper_->is_paused()) { - std::this_thread::sleep_for(std::chrono::microseconds(1000)); - } + future_pause.get(); // inform to the tracking module tracker_->set_mapping_module_status(false); } @@ -242,80 +297,192 @@ void system::abort_loop_BA() { global_optimizer_->abort_loop_BA(); } -std::shared_ptr system::feed_monocular_frame(const cv::Mat& img, const double timestamp, const cv::Mat& mask) { - assert(camera_->setup_type_ == camera::setup_type_t::Monocular); +data::frame system::create_monocular_frame(const cv::Mat& img, const double timestamp, const cv::Mat& mask) { + // color conversion + cv::Mat img_gray = img; + util::convert_to_grayscale(img_gray, camera_->color_order_); - check_reset_request(); + bool is_init = tracker_->tracking_state_ == tracker_state_t::Initializing; - const auto cam_pose_wc = tracker_->track_monocular_image(img, timestamp, mask); + data::frame_observation frm_obs; - frame_publisher_->update(tracker_); - if (tracker_->tracking_state_ == tracker_state_t::Tracking && cam_pose_wc) { - map_publisher_->set_current_cam_pose(tracker_->curr_frm_.get_cam_pose()); - map_publisher_->set_current_cam_pose_wc(*cam_pose_wc); + // Extract ORB feature + auto extractor = is_init ? ini_extractor_left_ : extractor_left_; + extractor->extract(img_gray, mask, frm_obs.keypts_, frm_obs.descriptors_); + frm_obs.num_keypts_ = frm_obs.keypts_.size(); + if (frm_obs.keypts_.empty()) { + spdlog::warn("preprocess: cannot extract any keypoints"); } - return cam_pose_wc; + // Undistort keypoints + camera_->undistort_keypoints(frm_obs.keypts_, frm_obs.undist_keypts_); + + // Ignore stereo parameters + frm_obs.stereo_x_right_ = std::vector(frm_obs.num_keypts_, -1); + frm_obs.depths_ = std::vector(frm_obs.num_keypts_, -1); + + // Convert to bearing vector + camera_->convert_keypoints_to_bearings(frm_obs.undist_keypts_, frm_obs.bearings_); + + // Assign all the keypoints into grid + data::assign_keypoints_to_grid(camera_, frm_obs.undist_keypts_, frm_obs.keypt_indices_in_cells_); + + return data::frame(timestamp, camera_, orb_params_, frm_obs); +} + +data::frame system::create_stereo_frame(const cv::Mat& left_img, const cv::Mat& right_img, const double timestamp, const cv::Mat& mask) { + // color conversion + cv::Mat img_gray = left_img; + cv::Mat right_img_gray = right_img; + util::convert_to_grayscale(img_gray, camera_->color_order_); + util::convert_to_grayscale(right_img_gray, camera_->color_order_); + + data::frame_observation frm_obs; + //! keypoints of stereo right image + std::vector keypts_right; + //! ORB descriptors of stereo right image + cv::Mat descriptors_right; + + // Extract ORB feature + std::thread thread_left([this, &frm_obs, &img_gray, &mask]() { + extractor_left_->extract(img_gray, mask, frm_obs.keypts_, frm_obs.descriptors_); + }); + std::thread thread_right([this, &frm_obs, &right_img_gray, &mask, &keypts_right, &descriptors_right]() { + extractor_right_->extract(right_img_gray, mask, keypts_right, descriptors_right); + }); + thread_left.join(); + thread_right.join(); + frm_obs.num_keypts_ = frm_obs.keypts_.size(); + if (frm_obs.keypts_.empty()) { + spdlog::warn("preprocess: cannot extract any keypoints"); + } + + // Undistort keypoints + camera_->undistort_keypoints(frm_obs.keypts_, frm_obs.undist_keypts_); + + // Estimate depth with stereo match + match::stereo stereo_matcher(extractor_left_->image_pyramid_, extractor_right_->image_pyramid_, + frm_obs.keypts_, keypts_right, frm_obs.descriptors_, descriptors_right, + orb_params_->scale_factors_, orb_params_->inv_scale_factors_, + camera_->focal_x_baseline_, camera_->true_baseline_); + stereo_matcher.compute(frm_obs.stereo_x_right_, frm_obs.depths_); + + // Convert to bearing vector + camera_->convert_keypoints_to_bearings(frm_obs.undist_keypts_, frm_obs.bearings_); + + // Assign all the keypoints into grid + data::assign_keypoints_to_grid(camera_, frm_obs.undist_keypts_, frm_obs.keypt_indices_in_cells_); + + return data::frame(timestamp, camera_, orb_params_, frm_obs); } -std::shared_ptr system::feed_stereo_frame(const cv::Mat& left_img, const cv::Mat& right_img, const double timestamp, const cv::Mat& mask) { - assert(camera_->setup_type_ == camera::setup_type_t::Stereo); +data::frame system::create_RGBD_frame(const cv::Mat& rgb_img, const cv::Mat& depthmap, const double timestamp, const cv::Mat& mask) { + // color and depth scale conversion + cv::Mat img_gray = rgb_img; + cv::Mat img_depth = depthmap; + util::convert_to_grayscale(img_gray, camera_->color_order_); + util::convert_to_true_depth(img_depth, depthmap_factor_); - check_reset_request(); + data::frame_observation frm_obs; - const auto cam_pose_wc = tracker_->track_stereo_image(left_img, right_img, timestamp, mask); + // Extract ORB feature + extractor_left_->extract(img_gray, mask, frm_obs.keypts_, frm_obs.descriptors_); + frm_obs.num_keypts_ = frm_obs.keypts_.size(); + if (frm_obs.keypts_.empty()) { + spdlog::warn("preprocess: cannot extract any keypoints"); + } - frame_publisher_->update(tracker_); - if (tracker_->tracking_state_ == tracker_state_t::Tracking && cam_pose_wc) { - map_publisher_->set_current_cam_pose(tracker_->curr_frm_.get_cam_pose()); - map_publisher_->set_current_cam_pose_wc(*cam_pose_wc); + // Undistort keypoints + camera_->undistort_keypoints(frm_obs.keypts_, frm_obs.undist_keypts_); + + // Calculate disparity from depth + // Initialize with invalid value + frm_obs.stereo_x_right_ = std::vector(frm_obs.num_keypts_, -1); + frm_obs.depths_ = std::vector(frm_obs.num_keypts_, -1); + + for (unsigned int idx = 0; idx < frm_obs.num_keypts_; idx++) { + const auto& keypt = frm_obs.keypts_.at(idx); + const auto& undist_keypt = frm_obs.undist_keypts_.at(idx); + + const float x = keypt.pt.x; + const float y = keypt.pt.y; + + const float depth = img_depth.at(y, x); + + if (depth <= 0) { + continue; + } + + frm_obs.depths_.at(idx) = depth; + frm_obs.stereo_x_right_.at(idx) = undist_keypt.pt.x - camera_->focal_x_baseline_ / depth; } - return cam_pose_wc; + // Convert to bearing vector + camera_->convert_keypoints_to_bearings(frm_obs.undist_keypts_, frm_obs.bearings_); + + // Assign all the keypoints into grid + data::assign_keypoints_to_grid(camera_, frm_obs.undist_keypts_, frm_obs.keypt_indices_in_cells_); + return data::frame(timestamp, camera_, orb_params_, frm_obs); +} + +std::shared_ptr system::feed_monocular_frame(const cv::Mat& img, const double timestamp, const cv::Mat& mask) { + assert(camera_->setup_type_ == camera::setup_type_t::Monocular); + return feed_frame(create_monocular_frame(img, timestamp, mask), img); +} + +std::shared_ptr system::feed_stereo_frame(const cv::Mat& left_img, const cv::Mat& right_img, const double timestamp, const cv::Mat& mask) { + assert(camera_->setup_type_ == camera::setup_type_t::Stereo); + return feed_frame(create_stereo_frame(left_img, right_img, timestamp, mask), left_img); } std::shared_ptr system::feed_RGBD_frame(const cv::Mat& rgb_img, const cv::Mat& depthmap, const double timestamp, const cv::Mat& mask) { assert(camera_->setup_type_ == camera::setup_type_t::RGBD); + return feed_frame(create_RGBD_frame(rgb_img, depthmap, timestamp, mask), rgb_img); +} +std::shared_ptr system::feed_frame(const data::frame& frm, const cv::Mat& img) { check_reset_request(); - const auto cam_pose_wc = tracker_->track_RGBD_image(rgb_img, depthmap, timestamp, mask); + const auto start = std::chrono::system_clock::now(); + + const auto cam_pose_wc = tracker_->feed_frame(frm); + + const auto end = std::chrono::system_clock::now(); + double elapsed_ms = std::chrono::duration_cast(end - start).count(); - frame_publisher_->update(tracker_); + frame_publisher_->update(tracker_, img, elapsed_ms); if (tracker_->tracking_state_ == tracker_state_t::Tracking && cam_pose_wc) { - map_publisher_->set_current_cam_pose(tracker_->curr_frm_.get_cam_pose()); - map_publisher_->set_current_cam_pose_wc(*cam_pose_wc); + map_publisher_->set_current_cam_pose(util::converter::inverse_pose(*cam_pose_wc)); } return cam_pose_wc; } bool system::relocalize_by_pose(const Mat44_t& cam_pose_wc) { - const Mat44_t cam_pose_cw = cam_pose_wc.inverse(); + const Mat44_t cam_pose_cw = util::converter::inverse_pose(cam_pose_wc); bool status = tracker_->request_relocalize_by_pose(cam_pose_cw); if (status) { // Even if state will be lost, still update the pose in map_publisher_ // to clearly show new camera position map_publisher_->set_current_cam_pose(cam_pose_cw); - map_publisher_->set_current_cam_pose_wc(cam_pose_wc); } return status; } bool system::relocalize_by_pose_2d(const Mat44_t& cam_pose_wc, const Vec3_t& normal_vector) { - const Mat44_t cam_pose_cw = cam_pose_wc.inverse(); + const Mat44_t cam_pose_cw = util::converter::inverse_pose(cam_pose_wc); bool status = tracker_->request_relocalize_by_pose_2d(cam_pose_cw, normal_vector); if (status) { // Even if state will be lost, still update the pose in map_publisher_ // to clearly show new camera position map_publisher_->set_current_cam_pose(cam_pose_cw); - map_publisher_->set_current_cam_pose_wc(cam_pose_wc); } return status; } void system::pause_tracker() { - tracker_->request_pause(); + auto future_pause = tracker_->async_pause(); + future_pause.get(); } bool system::tracker_is_paused() const { @@ -357,16 +524,20 @@ void system::check_reset_request() { void system::pause_other_threads() const { // pause the mapping module if (mapper_ && !mapper_->is_terminated()) { - mapper_->request_pause(); - while (!mapper_->is_paused() && !mapper_->is_terminated()) { - std::this_thread::sleep_for(std::chrono::microseconds(5000)); + auto future_pause = mapper_->async_pause(); + while (future_pause.wait_for(std::chrono::milliseconds(5)) == std::future_status::timeout) { + if (mapper_->is_terminated()) { + break; + } } } // pause the global optimization module if (global_optimizer_ && !global_optimizer_->is_terminated()) { - global_optimizer_->request_pause(); - while (!global_optimizer_->is_paused() && !global_optimizer_->is_terminated()) { - std::this_thread::sleep_for(std::chrono::microseconds(5000)); + auto future_pause = global_optimizer_->async_pause(); + while (future_pause.wait_for(std::chrono::milliseconds(5)) == std::future_status::timeout) { + if (global_optimizer_->is_terminated()) { + break; + } } } } diff --git a/src/openvslam/system.h b/src/openvslam/system.h index 6c97c4108..bb953890b 100644 --- a/src/openvslam/system.h +++ b/src/openvslam/system.h @@ -25,11 +25,18 @@ class base; } // namespace camera namespace data { +class frame; class camera_database; +class orb_params_database; class map_database; class bow_database; } // namespace data +namespace feature { +class orb_extractor; +class orb_params; +} // namespace feature + namespace publish { class map_publisher; class frame_publisher; @@ -103,16 +110,21 @@ class system { //----------------------------------------- // data feeding methods + std::shared_ptr feed_frame(const data::frame& frm, const cv::Mat& img); + //! Feed a monocular frame to SLAM system //! (NOTE: distorted images are acceptable if calibrated) + data::frame create_monocular_frame(const cv::Mat& img, const double timestamp, const cv::Mat& mask = cv::Mat{}); std::shared_ptr feed_monocular_frame(const cv::Mat& img, const double timestamp, const cv::Mat& mask = cv::Mat{}); //! Feed a stereo frame to SLAM system //! (Note: Left and Right images must be stereo-rectified) + data::frame create_stereo_frame(const cv::Mat& left_img, const cv::Mat& right_img, const double timestamp, const cv::Mat& mask = cv::Mat{}); std::shared_ptr feed_stereo_frame(const cv::Mat& left_img, const cv::Mat& right_img, const double timestamp, const cv::Mat& mask = cv::Mat{}); //! Feed an RGBD frame to SLAM system //! (Note: RGB and Depth images must be aligned) + data::frame create_RGBD_frame(const cv::Mat& rgb_img, const cv::Mat& depthmap, const double timestamp, const cv::Mat& mask); std::shared_ptr feed_RGBD_frame(const cv::Mat& rgb_img, const cv::Mat& depthmap, const double timestamp, const cv::Mat& mask = cv::Mat{}); //----------------------------------------- @@ -153,6 +165,12 @@ class system { //!! Termination of the system is requested or not bool terminate_is_requested() const; + //----------------------------------------- + // config + + //! depthmap factor (pixel_value / depthmap_factor = true_depth) + double depthmap_factor_ = 1.0; + private: //! Check reset request of the system void check_reset_request(); @@ -171,6 +189,12 @@ class system { //! camera database data::camera_database* cam_db_ = nullptr; + //! parameters for orb feature extraction + feature::orb_params* orb_params_ = nullptr; + + //! orb_params database + data::orb_params_database* orb_params_db_ = nullptr; + //! map database data::map_database* map_db_ = nullptr; @@ -193,6 +217,14 @@ class system { //! global optimization thread std::unique_ptr global_optimization_thread_ = nullptr; + // ORB extractors + //! ORB extractor for left/monocular image + feature::orb_extractor* extractor_left_ = nullptr; + //! ORB extractor for right image + feature::orb_extractor* extractor_right_ = nullptr; + //! ORB extractor only when used in initializing + feature::orb_extractor* ini_extractor_left_ = nullptr; + //! frame publisher std::shared_ptr frame_publisher_ = nullptr; //! map publisher diff --git a/src/openvslam/tracking_module.cc b/src/openvslam/tracking_module.cc index 0f701acc9..e4278b010 100644 --- a/src/openvslam/tracking_module.cc +++ b/src/openvslam/tracking_module.cc @@ -7,10 +7,8 @@ #include "openvslam/data/landmark.h" #include "openvslam/data/map_database.h" #include "openvslam/data/bow_database.h" -#include "openvslam/feature/orb_extractor.h" #include "openvslam/match/projection.h" #include "openvslam/module/local_map_updater.h" -#include "openvslam/util/image_converter.h" #include "openvslam/util/yaml.h" #include @@ -21,39 +19,6 @@ namespace { using namespace openvslam; -feature::orb_params get_orb_params(const YAML::Node& yaml_node) { - spdlog::debug("load ORB parameters"); - try { - return feature::orb_params(yaml_node); - } - catch (const std::exception& e) { - spdlog::error("failed in loading ORB parameters: {}", e.what()); - throw; - } -} - -double get_true_depth_thr(const camera::base* camera, const YAML::Node& yaml_node) { - spdlog::debug("load depth threshold"); - double true_depth_thr = 40.0; - if (camera->setup_type_ == camera::setup_type_t::Stereo || camera->setup_type_ == camera::setup_type_t::RGBD) { - const auto depth_thr_factor = yaml_node["depth_threshold"].as(40.0); - true_depth_thr = camera->true_baseline_ * depth_thr_factor; - } - return true_depth_thr; -} - -double get_depthmap_factor(const camera::base* camera, const YAML::Node& yaml_node) { - spdlog::debug("load depthmap factor"); - double depthmap_factor = 1.0; - if (camera->setup_type_ == camera::setup_type_t::RGBD) { - depthmap_factor = yaml_node["depthmap_factor"].as(depthmap_factor); - } - if (depthmap_factor < 0.) { - throw std::runtime_error("depthmap_factor must be greater than 0"); - } - return depthmap_factor; -} - double get_reloc_distance_threshold(const YAML::Node& yaml_node) { spdlog::debug("load maximum distance threshold where close keyframes could be found"); return yaml_node["reloc_distance_threshold"].as(0.2); @@ -76,43 +41,23 @@ double get_use_robust_matcher_for_relocalization_request(const YAML::Node& yaml_ namespace openvslam { -tracking_module::tracking_module(const std::shared_ptr& cfg, system* system, data::map_database* map_db, +tracking_module::tracking_module(const std::shared_ptr& cfg, data::map_database* map_db, data::bow_vocabulary* bow_vocab, data::bow_database* bow_db) - : camera_(cfg->camera_), true_depth_thr_(get_true_depth_thr(camera_, util::yaml_optional_ref(cfg->yaml_node_, "Tracking"))), - depthmap_factor_(get_depthmap_factor(camera_, util::yaml_optional_ref(cfg->yaml_node_, "Tracking"))), + : camera_(cfg->camera_), reloc_distance_threshold_(get_reloc_distance_threshold(util::yaml_optional_ref(cfg->yaml_node_, "Tracking"))), reloc_angle_threshold_(get_reloc_angle_threshold(util::yaml_optional_ref(cfg->yaml_node_, "Tracking"))), enable_auto_relocalization_(get_enable_auto_relocalization(util::yaml_optional_ref(cfg->yaml_node_, "Tracking"))), use_robust_matcher_for_relocalization_request_(get_use_robust_matcher_for_relocalization_request(util::yaml_optional_ref(cfg->yaml_node_, "Tracking"))), - system_(system), map_db_(map_db), bow_vocab_(bow_vocab), bow_db_(bow_db), - initializer_(cfg->camera_->setup_type_, map_db, bow_db, util::yaml_optional_ref(cfg->yaml_node_, "Initializer")), + map_db_(map_db), bow_vocab_(bow_vocab), bow_db_(bow_db), + initializer_(map_db, bow_db, util::yaml_optional_ref(cfg->yaml_node_, "Initializer")), frame_tracker_(camera_, 10), - relocalizer_(bow_db_, util::yaml_optional_ref(cfg->yaml_node_, "Relocalizer")), + relocalizer_(util::yaml_optional_ref(cfg->yaml_node_, "Relocalizer")), pose_optimizer_(), - keyfrm_inserter_(cfg->camera_->setup_type_, true_depth_thr_, map_db, bow_db, 0, cfg->camera_->fps_) { + keyfrm_inserter_(util::yaml_optional_ref(cfg->yaml_node_, "KeyframeInserter")) { spdlog::debug("CONSTRUCT: tracking_module"); - - feature::orb_params orb_params = get_orb_params(util::yaml_optional_ref(cfg->yaml_node_, "Feature")); - const auto tracking_params = util::yaml_optional_ref(cfg->yaml_node_, "Tracking"); - const auto max_num_keypoints = tracking_params["max_num_keypoints"].as(2000); - extractor_left_ = new feature::orb_extractor(max_num_keypoints, orb_params); - if (camera_->setup_type_ == camera::setup_type_t::Monocular) { - const auto ini_max_num_keypoints = tracking_params["ini_max_num_keypoints"].as(2 * extractor_left_->get_max_num_keypoints()); - ini_extractor_left_ = new feature::orb_extractor(ini_max_num_keypoints, orb_params); - } - if (camera_->setup_type_ == camera::setup_type_t::Stereo) { - extractor_right_ = new feature::orb_extractor(max_num_keypoints, orb_params); - } } tracking_module::~tracking_module() { - delete extractor_left_; - extractor_left_ = nullptr; - delete extractor_right_; - extractor_right_ = nullptr; - delete ini_extractor_left_; - ini_extractor_left_ = nullptr; - spdlog::debug("DESTRUCT: tracking_module"); } @@ -143,81 +88,6 @@ std::vector tracking_module::get_initial_matches() const { return initializer_.get_initial_matches(); } -std::shared_ptr tracking_module::track_monocular_image(const cv::Mat& img, const double timestamp, const cv::Mat& mask) { - const auto start = std::chrono::system_clock::now(); - - // color conversion - img_gray_ = img; - util::convert_to_grayscale(img_gray_, camera_->color_order_); - - // create current frame object - if (tracking_state_ == tracker_state_t::NotInitialized || tracking_state_ == tracker_state_t::Initializing) { - curr_frm_ = data::frame(img_gray_, timestamp, ini_extractor_left_, bow_vocab_, camera_, true_depth_thr_, mask); - } - else { - curr_frm_ = data::frame(img_gray_, timestamp, extractor_left_, bow_vocab_, camera_, true_depth_thr_, mask); - } - - track(); - - const auto end = std::chrono::system_clock::now(); - elapsed_ms_ = std::chrono::duration_cast(end - start).count(); - - std::shared_ptr cam_pose_wc = nullptr; - if (curr_frm_.cam_pose_cw_is_valid_) { - cam_pose_wc = std::allocate_shared(Eigen::aligned_allocator(), curr_frm_.get_cam_pose_inv()); - } - return cam_pose_wc; -} - -std::shared_ptr tracking_module::track_stereo_image(const cv::Mat& left_img_rect, const cv::Mat& right_img_rect, const double timestamp, const cv::Mat& mask) { - const auto start = std::chrono::system_clock::now(); - - // color conversion - img_gray_ = left_img_rect; - cv::Mat right_img_gray = right_img_rect; - util::convert_to_grayscale(img_gray_, camera_->color_order_); - util::convert_to_grayscale(right_img_gray, camera_->color_order_); - - // create current frame object - curr_frm_ = data::frame(img_gray_, right_img_gray, timestamp, extractor_left_, extractor_right_, bow_vocab_, camera_, true_depth_thr_, mask); - - track(); - - const auto end = std::chrono::system_clock::now(); - elapsed_ms_ = std::chrono::duration_cast(end - start).count(); - - std::shared_ptr cam_pose_wc = nullptr; - if (curr_frm_.cam_pose_cw_is_valid_) { - cam_pose_wc = std::allocate_shared(Eigen::aligned_allocator(), curr_frm_.get_cam_pose_inv()); - } - return cam_pose_wc; -} - -std::shared_ptr tracking_module::track_RGBD_image(const cv::Mat& img, const cv::Mat& depthmap, const double timestamp, const cv::Mat& mask) { - const auto start = std::chrono::system_clock::now(); - - // color and depth scale conversion - img_gray_ = img; - cv::Mat img_depth = depthmap; - util::convert_to_grayscale(img_gray_, camera_->color_order_); - util::convert_to_true_depth(img_depth, depthmap_factor_); - - // create current frame object - curr_frm_ = data::frame(img_gray_, img_depth, timestamp, extractor_left_, bow_vocab_, camera_, true_depth_thr_, mask); - - track(); - - const auto end = std::chrono::system_clock::now(); - elapsed_ms_ = std::chrono::duration_cast(end - start).count(); - - std::shared_ptr cam_pose_wc = nullptr; - if (curr_frm_.cam_pose_cw_is_valid_) { - cam_pose_wc = std::allocate_shared(Eigen::aligned_allocator(), curr_frm_.get_cam_pose_inv()); - } - return cam_pose_wc; -} - bool tracking_module::request_relocalize_by_pose(const Mat44_t& pose) { std::lock_guard lock(mtx_relocalize_by_pose_request_); if (relocalize_by_pose_is_requested_) { @@ -264,8 +134,10 @@ void tracking_module::reset() { initializer_.reset(); keyfrm_inserter_.reset(); - mapper_->request_reset(); - global_optimizer_->request_reset(); + auto future_mapper_reset = mapper_->async_reset(); + auto future_global_optimizer_reset = global_optimizer_->async_reset(); + future_mapper_reset.get(); + future_global_optimizer_reset.get(); bow_db_->clear(); map_db_->clear(); @@ -275,117 +147,135 @@ void tracking_module::reset() { data::landmark::next_id_ = 0; last_reloc_frm_id_ = 0; + last_reloc_frm_timestamp_ = 0.0; - tracking_state_ = tracker_state_t::NotInitialized; + tracking_state_ = tracker_state_t::Initializing; } -void tracking_module::track() { - if (tracking_state_ == tracker_state_t::NotInitialized) { - tracking_state_ = tracker_state_t::Initializing; - } - - last_tracking_state_ = tracking_state_; - +std::shared_ptr tracking_module::feed_frame(data::frame curr_frm) { // check if pause is requested check_and_execute_pause(); while (is_paused()) { std::this_thread::sleep_for(std::chrono::microseconds(5000)); } - // LOCK the map database - std::lock_guard lock(data::map_database::mtx_database_); + curr_frm_ = curr_frm; + bool succeeded = false; if (tracking_state_ == tracker_state_t::Initializing) { - if (!initialize()) { - return; - } + succeeded = initialize(); + } + else { + bool relocalization_is_needed = tracking_state_ == tracker_state_t::Lost; + succeeded = track(relocalization_is_needed); + } - // update the reference keyframe, local keyframes, and local landmarks - update_local_map(); + // state transition + if (succeeded) { + tracking_state_ = tracker_state_t::Tracking; + } + else if (tracking_state_ == tracker_state_t::Tracking) { + tracking_state_ = tracker_state_t::Lost; - // pass all of the keyframes to the mapping module - const auto keyfrms = map_db_->get_all_keyframes(); - for (const auto& keyfrm : keyfrms) { - mapper_->queue_keyframe(keyfrm); + spdlog::info("tracking lost: frame {}", curr_frm_.id_); + // if tracking is failed within 5.0 sec after initialization, reset the system + constexpr float init_retry_thr = 5.0; + if (curr_frm_.timestamp_ - initializer_.get_initial_frame_timestamp() < init_retry_thr) { + spdlog::info("tracking lost within {} sec after initialization", init_retry_thr); + reset(); + return nullptr; } - - // state transition to Tracking mode - tracking_state_ = tracker_state_t::Tracking; } - else { - // apply replace of landmarks observed in the last frame - apply_landmark_replace(); - // update the camera pose of the last frame - // because the mapping module might optimize the camera pose of the last frame's reference keyframe - update_last_frame(); - // set the reference keyframe of the current frame - curr_frm_.ref_keyfrm_ = last_frm_.ref_keyfrm_; + std::shared_ptr cam_pose_wc = nullptr; + // store the relative pose from the reference keyframe to the current frame + // to update the camera pose at the beginning of the next tracking process + if (curr_frm_.cam_pose_cw_is_valid_) { + last_cam_pose_from_ref_keyfrm_ = curr_frm_.cam_pose_cw_ * curr_frm_.ref_keyfrm_->get_cam_pose_inv(); + cam_pose_wc = std::allocate_shared(Eigen::aligned_allocator(), curr_frm_.get_cam_pose_inv()); + } - auto succeeded = track_current_frame(); + // update last frame + last_frm_ = curr_frm_; - // update the local map and optimize the camera pose of the current frame - if (succeeded) { - update_local_map(); - succeeded = optimize_current_frame_with_local_map(); - } + return cam_pose_wc; +} - // update the motion model - if (succeeded) { - update_motion_model(); - } +bool tracking_module::track(bool relocalization_is_needed) { + // LOCK the map database + std::lock_guard lock(data::map_database::mtx_database_); - // state transition - tracking_state_ = succeeded ? tracker_state_t::Tracking : tracker_state_t::Lost; + // apply replace of landmarks observed in the last frame + apply_landmark_replace(); + // update the camera pose of the last frame + // because the mapping module might optimize the camera pose of the last frame's reference keyframe + update_last_frame(); - // update the frame statistics - map_db_->update_frame_statistics(curr_frm_, tracking_state_ == tracker_state_t::Lost); + // set the reference keyframe of the current frame + curr_frm_.ref_keyfrm_ = last_frm_.ref_keyfrm_; - // if tracking is failed within 5.0 sec after initialization, reset the system - constexpr float init_retry_thr = 5.0; - if (tracking_state_ == tracker_state_t::Lost - && curr_frm_.id_ - initializer_.get_initial_frame_id() < camera_->fps_ * init_retry_thr) { - spdlog::info("tracking lost within {} sec after initialization", init_retry_thr); - system_->request_reset(); - return; + bool succeeded = false; + std::unordered_set outlier_ids; + if (relocalize_by_pose_is_requested()) { + // Force relocalization by pose + succeeded = relocalize_by_pose(get_relocalize_by_pose_request()); + } + else if (!relocalization_is_needed) { + succeeded = track_current_frame(outlier_ids); + } + else if (enable_auto_relocalization_) { + // Compute the BoW representations to perform relocalization + if (!curr_frm_.bow_is_available()) { + curr_frm_.compute_bow(bow_vocab_); } - - // show message if tracking has been lost - if (last_tracking_state_ != tracker_state_t::Lost && tracking_state_ == tracker_state_t::Lost) { - spdlog::info("tracking lost: frame {}", curr_frm_.id_); + // try to relocalize + succeeded = relocalizer_.relocalize(bow_db_, curr_frm_); + if (succeeded) { + last_reloc_frm_id_ = curr_frm_.id_; + last_reloc_frm_timestamp_ = curr_frm_.timestamp_; } + } - // check to insert the new keyframe derived from the current frame - if (succeeded && new_keyframe_is_needed()) { - insert_new_keyframe(); - } + // update the local map and optimize the camera pose of the current frame + unsigned int num_tracked_lms = 0; + if (succeeded) { + update_local_map(outlier_ids); + succeeded = optimize_current_frame_with_local_map(num_tracked_lms, outlier_ids); + } - // tidy up observations - for (unsigned int idx = 0; idx < curr_frm_.num_keypts_; ++idx) { - if (curr_frm_.landmarks_.at(idx) && curr_frm_.outlier_flags_.at(idx)) { - curr_frm_.landmarks_.at(idx) = nullptr; - } - } + // update the motion model + if (succeeded) { + update_motion_model(); } - // store the relative pose from the reference keyframe to the current frame - // to update the camera pose at the beginning of the next tracking process - if (curr_frm_.cam_pose_cw_is_valid_) { - last_cam_pose_from_ref_keyfrm_ = curr_frm_.cam_pose_cw_ * curr_frm_.ref_keyfrm_->get_cam_pose_inv(); + // check to insert the new keyframe derived from the current frame + if (succeeded && new_keyframe_is_needed(num_tracked_lms)) { + insert_new_keyframe(); } - // update last frame - last_frm_ = curr_frm_; + // tidy up observations + for (unsigned int idx = 0; idx < curr_frm_.frm_obs_.num_keypts_; ++idx) { + if (curr_frm_.landmarks_.at(idx) && curr_frm_.outlier_flags_.at(idx)) { + curr_frm_.landmarks_.at(idx) = nullptr; + } + } + + // update the frame statistics + map_db_->update_frame_statistics(curr_frm_, !succeeded); + + return succeeded; } bool tracking_module::initialize() { + // LOCK the map database + std::lock_guard lock(data::map_database::mtx_database_); + // try to initialize with the current frame - initializer_.initialize(curr_frm_); + initializer_.initialize(camera_->setup_type_, bow_vocab_, curr_frm_); // if map building was failed -> reset the map database if (initializer_.get_state() == module::initializer_state_t::Wrong) { - // reset - system_->request_reset(); + reset(); return false; } @@ -394,39 +284,33 @@ bool tracking_module::initialize() { return false; } + // pass all of the keyframes to the mapping module + const auto keyfrms = map_db_->get_all_keyframes(); + for (const auto& keyfrm : keyfrms) { + mapper_->queue_keyframe(keyfrm); + } + // succeeded return true; } -bool tracking_module::track_current_frame() { +bool tracking_module::track_current_frame(std::unordered_set& outlier_ids) { bool succeeded = false; - if (relocalize_by_pose_is_requested()) { - // Force relocalization by pose - succeeded = relocalize_by_pose(get_relocalize_by_pose_request()); - return succeeded; + // Tracking mode + if (twist_is_valid_ && last_reloc_frm_id_ + 2 < curr_frm_.id_) { + // if the motion model is valid + succeeded = frame_tracker_.motion_based_track(curr_frm_, last_frm_, twist_, outlier_ids); } - - if (tracking_state_ == tracker_state_t::Tracking) { - // Tracking mode - if (twist_is_valid_ && last_reloc_frm_id_ + 2 < curr_frm_.id_) { - // if the motion model is valid - succeeded = frame_tracker_.motion_based_track(curr_frm_, last_frm_, twist_); - } - if (!succeeded) { - succeeded = frame_tracker_.bow_match_based_track(curr_frm_, last_frm_, curr_frm_.ref_keyfrm_); - } - if (!succeeded) { - succeeded = frame_tracker_.robust_match_based_track(curr_frm_, last_frm_, curr_frm_.ref_keyfrm_); + if (!succeeded) { + // Compute the BoW representations to perform the BoW match + if (!curr_frm_.bow_is_available()) { + curr_frm_.compute_bow(bow_vocab_); } + succeeded = frame_tracker_.bow_match_based_track(curr_frm_, last_frm_, curr_frm_.ref_keyfrm_, outlier_ids); } - else if (enable_auto_relocalization_) { - // Lost mode - // try to relocalize - succeeded = relocalizer_.relocalize(curr_frm_); - if (succeeded) { - last_reloc_frm_id_ = curr_frm_.id_; - } + if (!succeeded) { + succeeded = frame_tracker_.robust_match_based_track(curr_frm_, last_frm_, curr_frm_.ref_keyfrm_, outlier_ids); } return succeeded; @@ -436,13 +320,16 @@ bool tracking_module::relocalize_by_pose(const pose_request& request) { bool succeeded = false; curr_frm_.set_cam_pose(request.pose_); - curr_frm_.compute_bow(); + if (!curr_frm_.bow_is_available()) { + curr_frm_.compute_bow(bow_vocab_); + } const auto candidates = get_close_keyframes(request); if (!candidates.empty()) { succeeded = relocalizer_.reloc_by_candidates(curr_frm_, candidates, use_robust_matcher_for_relocalization_request_); if (succeeded) { last_reloc_frm_id_ = curr_frm_.id_; + last_reloc_frm_timestamp_ = curr_frm_.timestamp_; } } else { @@ -483,7 +370,7 @@ void tracking_module::update_motion_model() { } void tracking_module::apply_landmark_replace() { - for (unsigned int idx = 0; idx < last_frm_.num_keypts_; ++idx) { + for (unsigned int idx = 0; idx < last_frm_.frm_obs_.num_keypts_; ++idx) { auto& lm = last_frm_.landmarks_.at(idx); if (!lm) { continue; @@ -504,16 +391,17 @@ void tracking_module::update_last_frame() { last_frm_.set_cam_pose(last_cam_pose_from_ref_keyfrm_ * last_ref_keyfrm->get_cam_pose()); } -bool tracking_module::optimize_current_frame_with_local_map() { +bool tracking_module::optimize_current_frame_with_local_map(unsigned int& num_tracked_lms, + std::unordered_set& outlier_ids) { // acquire more 2D-3D matches by reprojecting the local landmarks to the current frame - search_local_landmarks(); + search_local_landmarks(outlier_ids); // optimize the pose pose_optimizer_.optimize(curr_frm_); // count up the number of tracked landmarks - num_tracked_lms_ = 0; - for (unsigned int idx = 0; idx < curr_frm_.num_keypts_; ++idx) { + num_tracked_lms = 0; + for (unsigned int idx = 0; idx < curr_frm_.frm_obs_.num_keypts_; ++idx) { const auto& lm = curr_frm_.landmarks_.at(idx); if (!lm) { continue; @@ -523,7 +411,7 @@ bool tracking_module::optimize_current_frame_with_local_map() { // the observation has been considered as inlier in the pose optimization assert(lm->has_observation()); // count up - ++num_tracked_lms_; + ++num_tracked_lms; // increment the number of tracked frame lm->increase_num_observed(); } @@ -537,23 +425,23 @@ bool tracking_module::optimize_current_frame_with_local_map() { constexpr unsigned int num_tracked_lms_thr = 20; // if recently relocalized, use the more strict threshold - if (curr_frm_.id_ < last_reloc_frm_id_ + camera_->fps_ && num_tracked_lms_ < 2 * num_tracked_lms_thr) { - spdlog::debug("local map tracking failed: {} matches < {}", num_tracked_lms_, 2 * num_tracked_lms_thr); + if (curr_frm_.timestamp_ < last_reloc_frm_timestamp_ + 1.0 && num_tracked_lms < 2 * num_tracked_lms_thr) { + spdlog::debug("local map tracking failed: {} matches < {}", num_tracked_lms, 2 * num_tracked_lms_thr); return false; } // check the threshold of the number of tracked landmarks - if (num_tracked_lms_ < num_tracked_lms_thr) { - spdlog::debug("local map tracking failed: {} matches < {}", num_tracked_lms_, num_tracked_lms_thr); + if (num_tracked_lms < num_tracked_lms_thr) { + spdlog::debug("local map tracking failed: {} matches < {}", num_tracked_lms, num_tracked_lms_thr); return false; } return true; } -void tracking_module::update_local_map() { +void tracking_module::update_local_map(std::unordered_set& outlier_ids) { // clean landmark associations - for (unsigned int idx = 0; idx < curr_frm_.num_keypts_; ++idx) { + for (unsigned int idx = 0; idx < curr_frm_.frm_obs_.num_keypts_; ++idx) { const auto& lm = curr_frm_.landmarks_.at(idx); if (!lm) { continue; @@ -567,7 +455,7 @@ void tracking_module::update_local_map() { // acquire the current local map constexpr unsigned int max_num_local_keyfrms = 60; auto local_map_updater = module::local_map_updater(curr_frm_, max_num_local_keyfrms); - if (!local_map_updater.acquire_local_map()) { + if (!local_map_updater.acquire_local_map(outlier_ids)) { return; } // update the variables @@ -583,8 +471,9 @@ void tracking_module::update_local_map() { map_db_->set_local_landmarks(local_landmarks_); } -void tracking_module::search_local_landmarks() { +void tracking_module::search_local_landmarks(std::unordered_set& outlier_ids) { // select the landmarks which can be reprojected from the ones observed in the current frame + std::unordered_set curr_landmark_ids; for (const auto& lm : curr_frm_.landmarks_) { if (!lm) { continue; @@ -595,8 +484,7 @@ void tracking_module::search_local_landmarks() { // this landmark cannot be reprojected // because already observed in the current frame - lm->is_observable_in_tracking_ = false; - lm->identifier_in_local_lm_search_ = curr_frm_.id_; + curr_landmark_ids.insert(lm->id_); // this landmark is observable from the current frame lm->increase_num_observable(); @@ -607,9 +495,14 @@ void tracking_module::search_local_landmarks() { Vec2_t reproj; float x_right; unsigned int pred_scale_level; + eigen_alloc_unord_map lm_to_reproj; + std::unordered_map lm_to_x_right; + std::unordered_map lm_to_scale; for (const auto& lm : local_landmarks_) { - // avoid the landmarks which cannot be reprojected (== observed in the current frame) - if (lm->identifier_in_local_lm_search_ == curr_frm_.id_) { + if (curr_landmark_ids.count(curr_frm_.id_)) { + continue; + } + if (outlier_ids.count(curr_frm_.id_)) { continue; } if (lm->will_be_erased()) { @@ -618,23 +511,15 @@ void tracking_module::search_local_landmarks() { // check the observability if (curr_frm_.can_observe(lm, 0.5, reproj, x_right, pred_scale_level)) { - // pass the temporary variables - lm->reproj_in_tracking_ = reproj; - lm->x_right_in_tracking_ = x_right; - lm->scale_level_in_tracking_ = pred_scale_level; - - // this landmark can be reprojected - lm->is_observable_in_tracking_ = true; + lm_to_reproj[lm->id_] = reproj; + lm_to_x_right[lm->id_] = x_right; + lm_to_scale[lm->id_] = pred_scale_level; // this landmark is observable from the current frame lm->increase_num_observable(); found_proj_candidate = true; } - else { - // this landmark cannot be reprojected - lm->is_observable_in_tracking_ = false; - } } if (!found_proj_candidate) { @@ -648,36 +533,37 @@ void tracking_module::search_local_landmarks() { : ((camera_->setup_type_ == camera::setup_type_t::RGBD) ? 10.0 : 5.0); - projection_matcher.match_frame_and_landmarks(curr_frm_, local_landmarks_, margin); + projection_matcher.match_frame_and_landmarks(curr_frm_, local_landmarks_, lm_to_reproj, lm_to_x_right, lm_to_scale, margin); } -bool tracking_module::new_keyframe_is_needed() const { +bool tracking_module::new_keyframe_is_needed(unsigned int num_tracked_lms) const { if (!mapping_is_enabled_) { return false; } // cannnot insert the new keyframe in a second after relocalization - const auto num_keyfrms = map_db_->get_num_keyframes(); - if (camera_->fps_ < num_keyfrms && curr_frm_.id_ < last_reloc_frm_id_ + camera_->fps_) { + if (curr_frm_.timestamp_ < last_reloc_frm_timestamp_ + 1.0) { return false; } // check the new keyframe is needed - return keyfrm_inserter_.new_keyframe_is_needed(curr_frm_, num_tracked_lms_, *curr_frm_.ref_keyfrm_); + return keyfrm_inserter_.new_keyframe_is_needed(map_db_, curr_frm_, num_tracked_lms, *curr_frm_.ref_keyfrm_); } void tracking_module::insert_new_keyframe() { // insert the new keyframe - const auto ref_keyfrm = keyfrm_inserter_.insert_new_keyframe(curr_frm_); + const auto ref_keyfrm = keyfrm_inserter_.insert_new_keyframe(map_db_, curr_frm_); // set the reference keyframe with the new keyframe if (ref_keyfrm) { curr_frm_.ref_keyfrm_ = ref_keyfrm; } } -void tracking_module::request_pause() { +std::future tracking_module::async_pause() { std::lock_guard lock1(mtx_pause_); pause_is_requested_ = true; + promises_pause_.emplace_back(); + return promises_pause_.back().get_future(); } bool tracking_module::pause_is_requested() const { @@ -704,6 +590,10 @@ bool tracking_module::check_and_execute_pause() { if (pause_is_requested_) { is_paused_ = true; spdlog::info("pause tracking module"); + for (auto& promise : promises_pause_) { + promise.set_value(); + } + promises_pause_.clear(); return true; } else { diff --git a/src/openvslam/tracking_module.h b/src/openvslam/tracking_module.h index 906711dc5..27d608506 100644 --- a/src/openvslam/tracking_module.h +++ b/src/openvslam/tracking_module.h @@ -10,6 +10,7 @@ #include #include +#include #include #include @@ -25,13 +26,8 @@ class map_database; class bow_database; } // namespace data -namespace feature { -class orb_extractor; -} // namespace feature - // tracker state enum class tracker_state_t { - NotInitialized, Initializing, Tracking, Lost @@ -50,7 +46,7 @@ class tracking_module { EIGEN_MAKE_ALIGNED_OPERATOR_NEW //! Constructor - tracking_module(const std::shared_ptr& cfg, system* system, data::map_database* map_db, + tracking_module(const std::shared_ptr& cfg, data::map_database* map_db, data::bow_vocabulary* bow_vocab, data::bow_database* bow_db); //! Destructor @@ -77,17 +73,8 @@ class tracking_module { //! Get the keypoint matches between the initial frame and the current frame std::vector get_initial_matches() const; - //! Track a monocular frame - //! (NOTE: distorted images are acceptable if calibrated) - std::shared_ptr track_monocular_image(const cv::Mat& img, const double timestamp, const cv::Mat& mask = cv::Mat{}); - - //! Track a stereo frame - //! (Note: Left and Right images must be stereo-rectified) - std::shared_ptr track_stereo_image(const cv::Mat& left_img_rect, const cv::Mat& right_img_rect, const double timestamp, const cv::Mat& mask = cv::Mat{}); - - //! Track an RGBD frame - //! (Note: RGB and Depth images must be aligned) - std::shared_ptr track_RGBD_image(const cv::Mat& img, const cv::Mat& depthmap, const double timestamp, const cv::Mat& mask = cv::Mat{}); + //! Main stream of the tracking module + std::shared_ptr feed_frame(data::frame frame); //! Request to update the pose to a given one. //! Return failure in case if previous request was not finished yet. @@ -104,7 +91,7 @@ class tracking_module { // management for pause process //! Request to pause the tracking module - void request_pause(); + std::future async_pause(); //! Check if the pause of the tracking module is requested or not bool pause_is_requested() const; @@ -121,12 +108,6 @@ class tracking_module { //! camera model camera::base* camera_; - //! depth threshold (Ignore depths farther than true_depth_thr_ times the baseline.) - double true_depth_thr_ = 40.0; - - //! depthmap factor (pixel_value / depthmap_factor = true_depth) - double depthmap_factor_ = 1.0; - //! closest keyframes thresholds (by distance and angle) to relocalize with when updating by pose double reloc_distance_threshold_ = 0.2; double reloc_angle_threshold_ = 0.45; @@ -141,30 +122,23 @@ class tracking_module { // variables //! latest tracking state - tracker_state_t tracking_state_ = tracker_state_t::NotInitialized; - //! last tracking state - tracker_state_t last_tracking_state_ = tracker_state_t::NotInitialized; + tracker_state_t tracking_state_ = tracker_state_t::Initializing; //! current frame and its image data::frame curr_frm_; - //! image of the current frame - cv::Mat img_gray_; - - //! elapsed microseconds for each tracking - double elapsed_ms_ = 0.0; protected: //----------------------------------------- // tracking processes - //! Main stream of the tracking module - void track(); - //! Try to initialize with the current frame bool initialize(); + //! Main stream of the tracking module + bool track(bool relocalization_is_needed); + //! Track the current frame - bool track_current_frame(); + bool track_current_frame(std::unordered_set& outlier_ids); //! Relocalization by pose bool relocalize_by_pose(const pose_request& request); @@ -182,35 +156,25 @@ class tracking_module { void update_last_frame(); //! Optimize the camera pose of the current frame - bool optimize_current_frame_with_local_map(); + bool optimize_current_frame_with_local_map(unsigned int& num_tracked_lms, std::unordered_set& outlier_ids); //! Update the local map - void update_local_map(); + void update_local_map(std::unordered_set& outlier_ids); //! Acquire more 2D-3D matches using initial camera pose estimation - void search_local_landmarks(); + void search_local_landmarks(std::unordered_set& outlier_ids); //! Check the new keyframe is needed or not - bool new_keyframe_is_needed() const; + bool new_keyframe_is_needed(unsigned int num_tracked_lms) const; //! Insert the new keyframe derived from the current frame void insert_new_keyframe(); - //! system - system* system_ = nullptr; //! mapping module mapping_module* mapper_ = nullptr; //! global optimization module global_optimization_module* global_optimizer_ = nullptr; - // ORB extractors - //! ORB extractor for left/monocular image - feature::orb_extractor* extractor_left_ = nullptr; - //! ORB extractor for right image - feature::orb_extractor* extractor_right_ = nullptr; - //! ORB extractor only when used in initializing - feature::orb_extractor* ini_extractor_left_ = nullptr; - //! map_database data::map_database* map_db_ = nullptr; @@ -240,14 +204,13 @@ class tracking_module { //! local landmarks std::vector> local_landmarks_; - //! the number of tracked keyframes in the current keyframe - unsigned int num_tracked_lms_ = 0; - //! last frame data::frame last_frm_; - //! latest frame ID which succeeded in relocalization + //! ID of latest frame which succeeded in relocalization unsigned int last_reloc_frm_id_ = 0; + //! timestamp of latest frame which succeeded in relocalization + double last_reloc_frm_timestamp_ = 0.0; //! motion model Mat44_t twist_; @@ -273,6 +236,9 @@ class tracking_module { //! mutex for pause process mutable std::mutex mtx_pause_; + //! promise for pause + std::vector> promises_pause_; + //! Check the request frame and pause the tracking module bool check_and_execute_pause(); diff --git a/src/openvslam/util/converter.cc b/src/openvslam/util/converter.cc index 80087b6d6..e92e8020e 100644 --- a/src/openvslam/util/converter.cc +++ b/src/openvslam/util/converter.cc @@ -38,6 +38,17 @@ Mat44_t converter::to_eigen_cam_pose(const Mat33_t& rot, const Vec3_t& trans) { return cam_pose; } +Mat44_t converter::inverse_pose(const Mat44_t& pose_cw) { + const Mat33_t rot_cw = pose_cw.block<3, 3>(0, 0); + const Vec3_t trans_cw = pose_cw.block<3, 1>(0, 3); + const Mat33_t rot_wc = rot_cw.transpose(); + const Vec3_t cam_center = -rot_wc * trans_cw; + Mat44_t pose_wc = Mat44_t::Identity(); + pose_wc.block<3, 3>(0, 0) = rot_wc; + pose_wc.block<3, 1>(0, 3) = cam_center; + return pose_wc; +} + Vec3_t converter::to_angle_axis(const Mat33_t& rot_mat) { const Eigen::AngleAxisd angle_axis(rot_mat); return angle_axis.axis() * angle_axis.angle(); diff --git a/src/openvslam/util/converter.h b/src/openvslam/util/converter.h index a72e959cc..5be7bcdae 100644 --- a/src/openvslam/util/converter.h +++ b/src/openvslam/util/converter.h @@ -27,6 +27,9 @@ class converter { static Mat44_t to_eigen_mat(const g2o::Sim3& g2o_Sim3); static Mat44_t to_eigen_cam_pose(const Mat33_t& rot, const Vec3_t& trans); + //! inverse pose + static Mat44_t inverse_pose(const Mat44_t& pose_cw); + //! from/to angle axis static Vec3_t to_angle_axis(const Mat33_t& rot_mat); static Mat33_t to_rot_mat(const Vec3_t& angle_axis); diff --git a/src/pangolin_viewer/color_scheme.cc b/src/pangolin_viewer/color_scheme.cc index 64f7c30f5..3c2b47141 100644 --- a/src/pangolin_viewer/color_scheme.cc +++ b/src/pangolin_viewer/color_scheme.cc @@ -9,9 +9,6 @@ color_scheme::color_scheme(const std::string& color_set_str) { if (stricmp(color_set_str, std::string("white"))) { set_color_as_white(); } - else if (stricmp(color_set_str, std::string("black"))) { - set_color_as_black(); - } else if (stricmp(color_set_str, std::string("purple"))) { set_color_as_purple(); } @@ -30,16 +27,6 @@ void color_scheme::set_color_as_white() { local_lm_ = {{1.0f, 0.0f, 0.0f}}; } -void color_scheme::set_color_as_black() { - bg_ = {{0.15f, 0.15f, 0.15f, 1.0f}}; - grid_ = {{0.3f, 0.3f, 0.3f}}; - curr_cam_ = {{0.7f, 0.7f, 1.0f}}; - kf_line_ = {{0.0f, 1.0f, 0.0f}}; - graph_line_ = {{0.7f, 0.7f, 1.0f, 0.4f}}; - lm_ = {{0.9f, 0.9f, 0.9f}}; - local_lm_ = {{1.0f, 0.1f, 0.1f}}; -} - void color_scheme::set_color_as_purple() { bg_ = {{0.05f, 0.05f, 0.3f, 0.0f}}; grid_ = {{0.3f, 0.3f, 0.3f}}; diff --git a/src/pangolin_viewer/color_scheme.h b/src/pangolin_viewer/color_scheme.h index 30791c10c..956cfe037 100644 --- a/src/pangolin_viewer/color_scheme.h +++ b/src/pangolin_viewer/color_scheme.h @@ -31,8 +31,6 @@ class color_scheme { private: void set_color_as_white(); - void set_color_as_black(); - void set_color_as_purple(); static bool stricmp(const std::string& str1, const std::string& str2); diff --git a/src/pangolin_viewer/viewer.cc b/src/pangolin_viewer/viewer.cc index e6ab9a3a4..37904418c 100644 --- a/src/pangolin_viewer/viewer.cc +++ b/src/pangolin_viewer/viewer.cc @@ -27,7 +27,7 @@ viewer::viewer(const YAML::Node& yaml_node, openvslam::system* system, point_size_(yaml_node["point_size"].as(2)), camera_size_(yaml_node["camera_size"].as(0.15)), camera_line_width_(yaml_node["camera_line_width"].as(2)), - cs_(yaml_node["color_scheme"].as("black")), + cs_(yaml_node["color_scheme"].as("white")), mapping_mode_(system->mapping_module_is_enabled()), loop_detection_mode_(system->loop_detector_is_enabled()) {} diff --git a/test/openvslam/data/bow_vocabulary.cc b/test/openvslam/data/bow_vocabulary.cc index 92c148958..0f53a603f 100644 --- a/test/openvslam/data/bow_vocabulary.cc +++ b/test/openvslam/data/bow_vocabulary.cc @@ -10,10 +10,9 @@ using namespace openvslam; float get_score(data::bow_vocabulary* bow_vocab, const std::string& file1, const std::string& file2) { - auto params = feature::orb_params(); + auto params = feature::orb_params("ORB setting for test"); // mask (Mask the top and bottom 20%) - params.mask_rects_ = {{0.0, 0.2, 0.0, 1.0}, {0.8, 1.0, 0.0, 1.0}}; - auto extractor = feature::orb_extractor(1000, params); + auto extractor = feature::orb_extractor(¶ms, 1000, {{0.0, 0.2, 0.0, 1.0}, {0.8, 1.0, 0.0, 1.0}}); // image const auto img1 = cv::imread(file1, cv::IMREAD_GRAYSCALE); diff --git a/test/openvslam/feature/orb_extractor.cc b/test/openvslam/feature/orb_extractor.cc index 846b6b030..aa6b1c57f 100644 --- a/test/openvslam/feature/orb_extractor.cc +++ b/test/openvslam/feature/orb_extractor.cc @@ -22,8 +22,8 @@ cv::Mat draw_lines(const cv::Mat& img, const unsigned int num_segments = 4) { using namespace openvslam; TEST(orb_extractor, extract_toy_sample_1) { - const auto params = feature::orb_params(); - auto extractor = feature::orb_extractor(1000, params); + const auto params = feature::orb_params("ORB setting for test"); + auto extractor = feature::orb_extractor(¶ms, 1000); // image auto img = cv::Mat(600, 600, CV_8UC1); @@ -43,14 +43,14 @@ TEST(orb_extractor, extract_toy_sample_1) { // スケールを考慮して誤差を測定 for (const auto& keypt : keypts) { - EXPECT_NEAR(keypt.pt.x, 300, 2.0 * extractor.get_scale_factors().at(keypt.octave)); - EXPECT_NEAR(keypt.pt.y, 300, 2.0 * extractor.get_scale_factors().at(keypt.octave)); + EXPECT_NEAR(keypt.pt.x, 300, 2.0 * params.scale_factors_.at(keypt.octave)); + EXPECT_NEAR(keypt.pt.y, 300, 2.0 * params.scale_factors_.at(keypt.octave)); } } TEST(orb_extractor, extract_toy_sample_2) { - const auto params = feature::orb_params(); - auto extractor = feature::orb_extractor(1000, params); + const auto params = feature::orb_params("ORB setting for test"); + auto extractor = feature::orb_extractor(¶ms, 1000); // image auto img = cv::Mat(2000, 2000, CV_8UC1); @@ -70,14 +70,14 @@ TEST(orb_extractor, extract_toy_sample_2) { // スケールを考慮して誤差を測定 for (const auto& keypt : keypts) { - EXPECT_NEAR(keypt.pt.x, 1800, 2.0 * extractor.get_scale_factors().at(keypt.octave)); - EXPECT_NEAR(keypt.pt.y, 1800, 2.0 * extractor.get_scale_factors().at(keypt.octave)); + EXPECT_NEAR(keypt.pt.x, 1800, 2.0 * params.scale_factors_.at(keypt.octave)); + EXPECT_NEAR(keypt.pt.y, 1800, 2.0 * params.scale_factors_.at(keypt.octave)); } } TEST(orb_extractor, extract_without_mask_1) { - const auto params = feature::orb_params(); - auto extractor = feature::orb_extractor(1000, params); + const auto params = feature::orb_params("ORB setting for test"); + auto extractor = feature::orb_extractor(¶ms, 1000); // image const auto img = cv::imread(std::string(TEST_DATA_DIR) + "./equirectangular_image_001.jpg", cv::IMREAD_GRAYSCALE); @@ -95,8 +95,8 @@ TEST(orb_extractor, extract_without_mask_1) { } TEST(orb_extractor, extract_without_mask_2) { - const auto params = feature::orb_params(); - auto extractor = feature::orb_extractor(1000, params); + const auto params = feature::orb_params("ORB setting for test"); + auto extractor = feature::orb_extractor(¶ms, 1000); // image const auto img = cv::imread(std::string(TEST_DATA_DIR) + "./equirectangular_image_002.jpg", cv::IMREAD_GRAYSCALE); @@ -114,8 +114,8 @@ TEST(orb_extractor, extract_without_mask_2) { } TEST(orb_extractor, extract_with_image_mask_1) { - const auto params = feature::orb_params(); - auto extractor = feature::orb_extractor(1000, params); + const auto params = feature::orb_params("ORB setting for test"); + auto extractor = feature::orb_extractor(¶ms, 1000); // image const auto img = cv::imread(std::string(TEST_DATA_DIR) + "./equirectangular_image_001.jpg", cv::IMREAD_GRAYSCALE); @@ -151,8 +151,8 @@ TEST(orb_extractor, extract_with_image_mask_1) { } TEST(orb_extractor, extract_with_image_mask_2) { - const auto params = feature::orb_params(); - auto extractor = feature::orb_extractor(1000, params); + const auto params = feature::orb_params("ORB setting for test"); + auto extractor = feature::orb_extractor(¶ms, 1000); // image const auto img = cv::imread(std::string(TEST_DATA_DIR) + "./equirectangular_image_002.jpg", cv::IMREAD_GRAYSCALE); @@ -188,8 +188,8 @@ TEST(orb_extractor, extract_with_image_mask_2) { } TEST(orb_extractor, extract_with_image_mask_3) { - const auto params = feature::orb_params(); - auto extractor = feature::orb_extractor(1000, params); + const auto params = feature::orb_params("ORB setting for test"); + auto extractor = feature::orb_extractor(¶ms, 1000); // image const auto img = cv::imread(std::string(TEST_DATA_DIR) + "./equirectangular_image_001.jpg", cv::IMREAD_GRAYSCALE); @@ -228,10 +228,9 @@ TEST(orb_extractor, extract_with_image_mask_3) { } TEST(orb_extractor, extract_with_rectangle_mask_1) { - auto params = feature::orb_params(); + auto params = feature::orb_params("ORB setting for test"); // mask (上下20%をマスク) - params.mask_rects_ = {{0.0, 1.0, 0.0, 0.2}, {0.0, 1.0, 0.8, 1.0}}; - auto extractor = feature::orb_extractor(1000, params); + auto extractor = feature::orb_extractor(¶ms, 1000, {{0.0, 1.0, 0.0, 0.2}, {0.0, 1.0, 0.8, 1.0}}); // image const auto img = cv::imread(std::string(TEST_DATA_DIR) + "./equirectangular_image_001.jpg", cv::IMREAD_GRAYSCALE); @@ -262,10 +261,9 @@ TEST(orb_extractor, extract_with_rectangle_mask_1) { } TEST(orb_extractor, extract_with_rectangle_mask_2) { - auto params = feature::orb_params(); + auto params = feature::orb_params("ORB setting for test"); // mask (上下20%をマスク) - params.mask_rects_ = {{0.0, 0.2, 0.0, 1.0}, {0.8, 1.0, 0.0, 1.0}}; - auto extractor = feature::orb_extractor(1000, params); + auto extractor = feature::orb_extractor(¶ms, 1000, {{0.0, 0.2, 0.0, 1.0}, {0.8, 1.0, 0.0, 1.0}}); // image const auto img = cv::imread(std::string(TEST_DATA_DIR) + "./equirectangular_image_002.jpg", cv::IMREAD_GRAYSCALE); @@ -296,10 +294,9 @@ TEST(orb_extractor, extract_with_rectangle_mask_2) { } TEST(orb_extractor, extract_with_rectangle_mask_3) { - auto params = feature::orb_params(); + auto params = feature::orb_params("ORB setting for test"); // mask (上下左右20%をマスク) - params.mask_rects_ = {{0.0, 0.2, 0.0, 1.0}, {0.8, 1.0, 0.0, 1.0}, {0.0, 1.0, 0.0, 0.2}, {0.0, 1.0, 0.8, 1.0}}; - auto extractor = feature::orb_extractor(1000, params); + auto extractor = feature::orb_extractor(¶ms, 1000, {{0.0, 0.2, 0.0, 1.0}, {0.8, 1.0, 0.0, 1.0}, {0.0, 1.0, 0.0, 0.2}, {0.0, 1.0, 0.8, 1.0}}); // image const auto img = cv::imread(std::string(TEST_DATA_DIR) + "./equirectangular_image_002.jpg", cv::IMREAD_GRAYSCALE); @@ -332,8 +329,8 @@ TEST(orb_extractor, extract_with_rectangle_mask_3) { } TEST(orb_extractor, extract_toy_sample_3) { - const auto params = feature::orb_params(); - auto extractor = feature::orb_extractor(1000, params); + const auto params = feature::orb_params("ORB setting for test"); + auto extractor = feature::orb_extractor(¶ms, 1000); // image auto img = cv::Mat(1200, 600, CV_8UC1); @@ -353,14 +350,14 @@ TEST(orb_extractor, extract_toy_sample_3) { // スケールを考慮して誤差を測定 for (const auto& keypt : keypts) { - EXPECT_NEAR(keypt.pt.x, 300, 2.0 * extractor.get_scale_factors().at(keypt.octave)); - EXPECT_NEAR(keypt.pt.y, 600, 2.0 * extractor.get_scale_factors().at(keypt.octave)); + EXPECT_NEAR(keypt.pt.x, 300, 2.0 * params.scale_factors_.at(keypt.octave)); + EXPECT_NEAR(keypt.pt.y, 600, 2.0 * params.scale_factors_.at(keypt.octave)); } } TEST(orb_extractor, extract_without_mask_3) { - const auto params = feature::orb_params(); - auto extractor = feature::orb_extractor(1000, params); + const auto params = feature::orb_params("ORB setting for test"); + auto extractor = feature::orb_extractor(¶ms, 1000); // image const auto img_land = cv::imread(std::string(TEST_DATA_DIR) + "./equirectangular_image_001.jpg", cv::IMREAD_GRAYSCALE); @@ -381,8 +378,8 @@ TEST(orb_extractor, extract_without_mask_3) { } TEST(orb_extractor, extract_without_mask_4) { - const auto params = feature::orb_params(); - auto extractor = feature::orb_extractor(1000, params); + const auto params = feature::orb_params("ORB setting for test"); + auto extractor = feature::orb_extractor(¶ms, 1000); // image const auto img_land = cv::imread(std::string(TEST_DATA_DIR) + "./equirectangular_image_002.jpg", cv::IMREAD_GRAYSCALE); diff --git a/test/openvslam/feature/orb_params.cc b/test/openvslam/feature/orb_params.cc index ed034c1b7..f125895e5 100644 --- a/test/openvslam/feature/orb_params.cc +++ b/test/openvslam/feature/orb_params.cc @@ -9,6 +9,7 @@ using namespace openvslam; TEST(orb_params, load_yaml_without_rectangle_mask) { const std::string yaml = "Feature:\n" + " name: \"ORB setting for test\"\n" " scale_factor: 1.3\n" " num_levels: 12\n" " ini_fast_threshold: 25\n" @@ -20,137 +21,7 @@ TEST(orb_params, load_yaml_without_rectangle_mask) { EXPECT_FLOAT_EQ(params.scale_factor_, 1.3); EXPECT_EQ(params.num_levels_, 12); EXPECT_EQ(params.ini_fast_thr_, 25); - EXPECT_EQ(params.min_fast_thr, 9); - EXPECT_EQ(params.mask_rects_.size(), 0); -} - -TEST(orb_params, load_yaml_with_rectangle_mask) { - const std::string yaml = - "Camera:\n" - " cols: 640\n" - " rows: 480\n" - "Feature:\n" - " scale_factor: 1.3\n" - " num_levels: 12\n" - " ini_fast_threshold: 25\n" - " min_fast_threshold: 9\n" - " mask_rectangles:\n" - " - [0.2, 0.5, 0.3, 0.8]\n" - " - [0.28, 0.59, 0.1, 0.2]\n"; - - const auto yaml_node = YAML::Load(yaml); - const auto params = feature::orb_params(yaml_node["Feature"]); - - EXPECT_FLOAT_EQ(params.scale_factor_, 1.3); - EXPECT_EQ(params.num_levels_, 12); - EXPECT_EQ(params.ini_fast_thr_, 25); - EXPECT_EQ(params.min_fast_thr, 9); - EXPECT_EQ(params.mask_rects_.size(), 2); - - EXPECT_FLOAT_EQ(params.mask_rects_.at(0).at(0), 0.2); - EXPECT_FLOAT_EQ(params.mask_rects_.at(0).at(1), 0.5); - EXPECT_FLOAT_EQ(params.mask_rects_.at(0).at(2), 0.3); - EXPECT_FLOAT_EQ(params.mask_rects_.at(0).at(3), 0.8); - - EXPECT_FLOAT_EQ(params.mask_rects_.at(1).at(0), 0.28); - EXPECT_FLOAT_EQ(params.mask_rects_.at(1).at(1), 0.59); - EXPECT_FLOAT_EQ(params.mask_rects_.at(1).at(2), 0.1); - EXPECT_FLOAT_EQ(params.mask_rects_.at(1).at(3), 0.2); -} - -TEST(orb_params, load_yaml_with_rectangle_mask_exception_1) { - // when the size of vector in mask_rectangles is not four - const std::string yaml = - "Camera:\n" - " cols: 640\n" - " rows: 480\n" - "Feature:\n" - " scale_factor: 1.3\n" - " num_levels: 12\n" - " ini_fast_threshold: 25\n" - " min_fast_threshold: 9\n" - " mask_rectangles:\n" - " - [0.2, 0.5, 0.3, 0.8]\n" - " - [0.28, 0.59, 0.1]\n"; - - const auto yaml_node = YAML::Load(yaml); - EXPECT_THROW(const auto params = feature::orb_params(yaml_node["Feature"]), std::runtime_error); -} - -TEST(orb_params, load_yaml_with_rectangle_mask_exception_2) { - // when x_min equals x_max - const std::string yaml = - "Camera:\n" - " cols: 640\n" - " rows: 480\n" - "Feature:\n" - " scale_factor: 1.3\n" - " num_levels: 12\n" - " ini_fast_threshold: 25\n" - " min_fast_threshold: 9\n" - " mask_rectangles:\n" - " - [0.2, 0.5, 0.3, 0.8]\n" - " - [0.28, 0.28, 0.1, 0.2]\n"; - - const auto yaml_node = YAML::Load(yaml); - EXPECT_THROW(const auto params = feature::orb_params(yaml_node["Feature"]), std::runtime_error); -} - -TEST(orb_params, load_yaml_with_rectangle_mask_exception_3) { - // when x_min is greater than x_max - const std::string yaml = - "Camera:\n" - " cols: 640\n" - " rows: 480\n" - "Feature:\n" - " scale_factor: 1.3\n" - " num_levels: 12\n" - " ini_fast_threshold: 25\n" - " min_fast_threshold: 9\n" - " mask_rectangles:\n" - " - [0.2, 0.5, 0.3, 0.8]\n" - " - [0.25, 0.21, 0.1, 0.2]\n"; - - const auto yaml_node = YAML::Load(yaml); - EXPECT_THROW(const auto params = feature::orb_params(yaml_node["Feature"]), std::runtime_error); -} - -TEST(orb_params, load_yaml_with_rectangle_mask_exception_4) { - // when y_min equals y_max - const std::string yaml = - "Camera:\n" - " cols: 640\n" - " rows: 480\n" - "Feature:\n" - " scale_factor: 1.3\n" - " num_levels: 12\n" - " ini_fast_threshold: 25\n" - " min_fast_threshold: 9\n" - " mask_rectangles:\n" - " - [0.2, 0.5, 0.3, 0.8]\n" - " - [0.28, 0.59, 0.8, 0.8]\n"; - - const auto yaml_node = YAML::Load(yaml); - EXPECT_THROW(const auto params = feature::orb_params(yaml_node["Feature"]), std::runtime_error); -} - -TEST(orb_params, load_yaml_with_rectangle_mask_exception_5) { - // when y_min is greater than y_max - const std::string yaml = - "Camera:\n" - " cols: 640\n" - " rows: 480\n" - "Feature:\n" - " scale_factor: 1.3\n" - " num_levels: 12\n" - " ini_fast_threshold: 25\n" - " min_fast_threshold: 9\n" - " mask_rectangles:\n" - " - [0.2, 0.5, 0.3, 0.8]\n" - " - [0.28, 0.59, 0.7, 0.2]\n"; - - const auto yaml_node = YAML::Load(yaml); - EXPECT_THROW(const auto params = feature::orb_params(yaml_node["Feature"]), std::runtime_error); + EXPECT_EQ(params.min_fast_thr_, 9); } TEST(orb_params, calc_scale_factors) {