Make it work without vocabulary files

src/stella_vslam/data/keyframe.cc

@@ -240,7 +240,9 @@ std::shared_ptr<keyframe> keyframe::from_stmt(sqlite3_stmt* stmt,
     // Construct frame_observation
     frame_observation frm_obs{descriptors, undist_keypts, bearings, stereo_x_right, depths};
     // Compute BoW
-    data::bow_vocabulary_util::compute_bow(bow_vocab, descriptors, bow_vec, bow_feat_vec);
+    if (bow_vocab) {
+        data::bow_vocabulary_util::compute_bow(bow_vocab, descriptors, bow_vec, bow_feat_vec);
+    }
     // NOTE: 3D marker info will be filled in later based on loaded markers
     auto keyfrm = data::keyframe::make_keyframe(
         id + next_keyframe_id, timestamp, pose_cw, camera, orb_params,
@@ -660,7 +662,9 @@ void keyframe::prepare_for_erasing(map_database* map_db, bow_database* bow_db) {
     // 4. remove myself from the databased
 
     map_db->erase_keyframe(shared_from_this());
-    bow_db->erase_keyframe(shared_from_this());
+    if (bow_db) {
+        bow_db->erase_keyframe(shared_from_this());
+    }
 }
 
 bool keyframe::will_be_erased() {

src/stella_vslam/data/map_database.cc

@@ -395,7 +395,9 @@ void map_database::register_keyframe(camera_database* cam_db, orb_params_databas
     // Construct frame_observation
     frame_observation frm_obs{descriptors, undist_keypts, bearings, stereo_x_right, depths};
     // Compute BoW
-    data::bow_vocabulary_util::compute_bow(bow_vocab, descriptors, bow_vec, bow_feat_vec);
+    if (bow_vocab) {
+        data::bow_vocabulary_util::compute_bow(bow_vocab, descriptors, bow_vec, bow_feat_vec);
+    }
     auto keyfrm = data::keyframe::make_keyframe(
         id, timestamp, pose_cw, camera, orb_params,
         frm_obs, bow_vec, bow_feat_vec);

src/stella_vslam/io/map_database_io_sqlite3.cc

@@ -53,7 +53,7 @@ bool map_database_io_sqlite3::load(const std::string& path,
                                    data::bow_database* bow_db,
                                    data::bow_vocabulary* bow_vocab) {
     std::lock_guard<std::mutex> lock(data::map_database::mtx_database_);
-    assert(cam_db && map_db && bow_db && bow_vocab);
+    assert(cam_db && map_db);
 
     // Open database
     sqlite3* db = nullptr;
@@ -69,7 +69,7 @@ bool map_database_io_sqlite3::load(const std::string& path,
     ok = ok && load_stats(db, map_db);
 
     // update bow database
-    if (ok) {
+    if (ok && bow_db) {
         const auto keyfrms = map_db->get_all_keyframes();
         for (const auto& keyfrm : keyfrms) {
             bow_db->add_keyframe(keyfrm);

src/stella_vslam/mapping_module.cc

@@ -110,7 +110,7 @@ void mapping_module::run() {
         // create and extend the map with the new keyframe
         mapping_with_new_keyframe();
         // send the new keyframe to the global optimization module
-        if (!cur_keyfrm_->graph_node_->is_spanning_root()) {
+        if (global_optimizer_ && !cur_keyfrm_->graph_node_->is_spanning_root()) {
             global_optimizer_->queue_keyframe(cur_keyfrm_);
         }
     }
@@ -247,7 +247,7 @@ void mapping_module::mapping_with_new_keyframe() {
 
 void mapping_module::store_new_keyframe() {
     // compute BoW feature vector
-    if (!cur_keyfrm_->bow_is_available()) {
+    if (bow_vocab_ && !cur_keyfrm_->bow_is_available()) {
         cur_keyfrm_->compute_bow(bow_vocab_);
     }
 
@@ -277,6 +277,7 @@ void mapping_module::create_new_landmarks(std::atomic<bool>& abort_create_new_la
     // in order to triangulate landmarks between `cur_keyfrm_` and each of the covisibilities
     const auto cur_covisibilities = cur_keyfrm_->graph_node_->get_top_n_covisibilities(num_covisibilities_for_landmark_generation_);
 
+    match::bow_tree bow_tree_matcher(0.95, false);
     match::robust robust_matcher(0.95, false);
 
     // camera center of the current keyframe
@@ -327,7 +328,12 @@ void mapping_module::create_new_landmarks(std::atomic<bool>& abort_create_new_la
 
         // vector of matches (idx in the current, idx in the neighbor)
         std::vector<std::pair<unsigned int, unsigned int>> matches;
-        robust_matcher.match_for_triangulation(cur_keyfrm_, ngh_keyfrm, E_ngh_to_cur, matches, residual_rad_thr_);
+        if (bow_db_ && bow_vocab_) {
+            bow_tree_matcher.match_for_triangulation(cur_keyfrm_, ngh_keyfrm, E_ngh_to_cur, matches, residual_rad_thr_);
+        }
+        else {
+            robust_matcher.match_for_triangulation(cur_keyfrm_, ngh_keyfrm, E_ngh_to_cur, matches, residual_rad_thr_);
+        }
 
         // triangulation
         triangulate_with_two_keyframes(cur_keyfrm_, ngh_keyfrm, matches);

src/stella_vslam/match/base.h

@@ -1,6 +1,8 @@
 #ifndef STELLA_VSLAM_MATCH_BASE_H
 #define STELLA_VSLAM_MATCH_BASE_H
 
+#include "stella_vslam/type.h"
+
 #include <array>
 #include <algorithm>
 #include <numeric>
@@ -62,6 +64,20 @@ inline unsigned int compute_descriptor_distance_64(const cv::Mat& desc_1, const
     return dist;
 }
 
+inline bool check_epipolar_constraint(const Vec3_t& bearing_1, const Vec3_t& bearing_2,
+                                      const Mat33_t& E_12, float residual_rad_thr,
+                                      const float bearing_1_scale_factor) {
+    // Normal vector of the epipolar plane on keyframe 1
+    const Vec3_t epiplane_in_1 = E_12 * bearing_2;
+
+    // Acquire the angle formed by the normal vector and the bearing
+    const auto cos_residual = std::min(1.0, std::max(-1.0, epiplane_in_1.dot(bearing_1) / epiplane_in_1.norm()));
+    const auto residual_rad = std::abs(M_PI / 2.0 - std::acos(cos_residual));
+
+    // The larger keypoint scale permits less constraints
+    return residual_rad < residual_rad_thr * bearing_1_scale_factor;
+}
+
 class base {
 public:
     base(const float lowe_ratio, const bool check_orientation)

src/stella_vslam/match/bow_tree.cc

@@ -8,6 +8,164 @@
 namespace stella_vslam {
 namespace match {
 
+unsigned int bow_tree::match_for_triangulation(const std::shared_ptr<data::keyframe>& keyfrm_1,
+                                               const std::shared_ptr<data::keyframe>& keyfrm_2,
+                                               const Mat33_t& E_12,
+                                               std::vector<std::pair<unsigned int, unsigned int>>& matched_idx_pairs,
+                                               const float residual_rad_thr) const {
+    unsigned int num_matches = 0;
+
+    // Project the center of keyframe 1 to keyframe 2
+    // to acquire the epipole coordinates of the candidate keyframe
+    const Vec3_t cam_center_1 = keyfrm_1->get_trans_wc();
+    const Mat33_t rot_2w = keyfrm_2->get_rot_cw();
+    const Vec3_t trans_2w = keyfrm_2->get_trans_cw();
+    Vec3_t epiplane_in_keyfrm_2;
+    const bool valid_epiplane = keyfrm_2->camera_->reproject_to_bearing(rot_2w, trans_2w, cam_center_1, epiplane_in_keyfrm_2);
+
+    // Acquire the 3D point information of the keframes
+    const auto assoc_lms_in_keyfrm_1 = keyfrm_1->get_landmarks();
+    const auto assoc_lms_in_keyfrm_2 = keyfrm_2->get_landmarks();
+
+    // Save the matching information
+    // Discard the already matched keypoints in keyframe 2
+    // to acquire a unique association to each keypoint in keyframe 1
+    std::vector<bool> is_already_matched_in_keyfrm_2(keyfrm_2->frm_obs_.undist_keypts_.size(), false);
+    // Save the keypoint idx in keyframe 2 which is already associated to the keypoint idx in keyframe 1
+    std::vector<int> matched_indices_2_in_keyfrm_1(keyfrm_1->frm_obs_.undist_keypts_.size(), -1);
+
+    data::bow_feature_vector::const_iterator itr_1 = keyfrm_1->bow_feat_vec_.begin();
+    data::bow_feature_vector::const_iterator itr_2 = keyfrm_2->bow_feat_vec_.begin();
+    const data::bow_feature_vector::const_iterator itr_1_end = keyfrm_1->bow_feat_vec_.end();
+    const data::bow_feature_vector::const_iterator itr_2_end = keyfrm_2->bow_feat_vec_.end();
+
+    while (itr_1 != itr_1_end && itr_2 != itr_2_end) {
+        // Check if the node numbers of BoW tree match
+        if (itr_1->first == itr_2->first) {
+            // If the node numbers of BoW tree match,
+            // Check in practice if matches exist between keyframes
+            const auto& keyfrm_1_indices = itr_1->second;
+            const auto& keyfrm_2_indices = itr_2->second;
+
+            for (const auto idx_1 : keyfrm_1_indices) {
+                const auto& lm_1 = assoc_lms_in_keyfrm_1.at(idx_1);
+                // Ignore if the keypoint of keyframe is associated any 3D points
+                if (lm_1) {
+                    continue;
+                }
+
+                // Check if it's a stereo keypoint or not
+                const bool is_stereo_keypt_1 = !keyfrm_1->frm_obs_.stereo_x_right_.empty() && 0 <= keyfrm_1->frm_obs_.stereo_x_right_.at(idx_1);
+
+                // Acquire the keypoints and ORB feature vectors
+                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;
+                int best_idx_2 = -1;
+                unsigned int second_best_hamm_dist = MAX_HAMMING_DIST;
+
+                for (const auto idx_2 : keyfrm_2_indices) {
+                    // Ignore if the keypoint is associated any 3D points
+                    // (because this function is used for triangulation)
+                    const auto& lm_2 = assoc_lms_in_keyfrm_2.at(idx_2);
+                    if (lm_2) {
+                        continue;
+                    }
+
+                    // Ignore if matches are already aquired
+                    if (is_already_matched_in_keyfrm_2.at(idx_2)) {
+                        continue;
+                    }
+
+                    if (check_orientation_ && std::abs(util::angle::diff(keypt_1.angle, keyfrm_2->frm_obs_.undist_keypts_.at(idx_2).angle)) > 30.0) {
+                        continue;
+                    }
+
+                    // Check if it's a stereo keypoint or not
+                    const bool is_stereo_keypt_2 = !keyfrm_2->frm_obs_.stereo_x_right_.empty() && 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->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);
+
+                    if (HAMMING_DIST_THR_LOW < hamm_dist || best_hamm_dist < hamm_dist) {
+                        continue;
+                    }
+
+                    if (valid_epiplane && !is_stereo_keypt_1 && !is_stereo_keypt_2) {
+                        // Do not use any keypoints near the epipole if both are not stereo keypoints
+                        const auto cos_dist = epiplane_in_keyfrm_2.dot(bearing_2);
+                        // The threshold of the minimum angle formed by the epipole and the bearing vector is 3.0 degree
+                        constexpr double cos_dist_thr = 0.99862953475;
+
+                        // Do not allow to match if the formed angle is narrower that the threshold value
+                        if (cos_dist_thr < cos_dist) {
+                            continue;
+                        }
+                    }
+
+                    // Check consistency in Matrix E
+                    const bool is_inlier = check_epipolar_constraint(bearing_1, bearing_2, E_12,
+                                                                     keyfrm_1->orb_params_->scale_factors_.at(keypt_1.octave),
+                                                                     residual_rad_thr);
+                    if (is_inlier) {
+                        if (hamm_dist < best_hamm_dist) {
+                            second_best_hamm_dist = best_hamm_dist;
+                            best_hamm_dist = hamm_dist;
+                            best_idx_2 = idx_2;
+                        }
+                        else if (hamm_dist < second_best_hamm_dist) {
+                            second_best_hamm_dist = hamm_dist;
+                        }
+                    }
+                }
+
+                if (best_idx_2 < 0) {
+                    continue;
+                }
+
+                // Ratio test
+                if (lowe_ratio_ * second_best_hamm_dist < static_cast<float>(best_hamm_dist)) {
+                    continue;
+                }
+
+                is_already_matched_in_keyfrm_2.at(best_idx_2) = true;
+                matched_indices_2_in_keyfrm_1.at(idx_1) = best_idx_2;
+                ++num_matches;
+            }
+
+            ++itr_1;
+            ++itr_2;
+        }
+        else if (itr_1->first < itr_2->first) {
+            // Since the node number of keyframe 1 is smaller, increment the iterator until the node numbers match
+            itr_1 = keyfrm_1->bow_feat_vec_.lower_bound(itr_2->first);
+        }
+        else {
+            // Since the node number of keyframe 2 is smaller, increment the iterator until the node numbers match
+            itr_2 = keyfrm_2->bow_feat_vec_.lower_bound(itr_1->first);
+        }
+    }
+
+    matched_idx_pairs.clear();
+    matched_idx_pairs.reserve(num_matches);
+
+    for (unsigned int idx_1 = 0; idx_1 < matched_indices_2_in_keyfrm_1.size(); ++idx_1) {
+        if (matched_indices_2_in_keyfrm_1.at(idx_1) < 0) {
+            continue;
+        }
+        matched_idx_pairs.emplace_back(std::make_pair(idx_1, matched_indices_2_in_keyfrm_1.at(idx_1)));
+    }
+
+    return num_matches;
+}
+
 unsigned int bow_tree::match_frame_and_keyframe(const std::shared_ptr<data::keyframe>& keyfrm, data::frame& frm, std::vector<std::shared_ptr<data::landmark>>& matched_lms_in_frm) const {
     unsigned int num_matches = 0;
 

src/stella_vslam/match/bow_tree.h

@@ -22,14 +22,22 @@ class bow_tree final : public base {
 
     ~bow_tree() final = default;
 
-    //! frameで観測している特徴点とkeyframeで観測している特徴点の対応を求め，それを元にframeの特徴点と3次元点の対応情報を得る
-    //! matched_lms_in_frmには，frameの各特徴点に対応する(keyframeで観測された)3次元点が格納される
-    //! NOTE: matched_lms_in_frm.size()はframeの特徴点数と一致
+    unsigned int match_for_triangulation(const std::shared_ptr<data::keyframe>& keyfrm_1,
+                                         const std::shared_ptr<data::keyframe>& keyfrm_2,
+                                         const Mat33_t& E_12,
+                                         std::vector<std::pair<unsigned int, unsigned int>>& matched_idx_pairs,
+                                         const float residual_rad_thr) const;
+
+    //! Find the correspondence between the feature points observed in the frame and the feature points observed in the keyframe,
+    //! and obtain the correspondence between the feature points in the frame and the 3D points.
+    //! Matched_lms_in_frm stores the 3D points (observed in the keyframe) corresponding to each feature point in the frame.
+    //! NOTE: matched_lms_in_frm.size() is equal to the number of feature points in the frame
     unsigned int match_frame_and_keyframe(const std::shared_ptr<data::keyframe>& keyfrm, data::frame& frm, std::vector<std::shared_ptr<data::landmark>>& matched_lms_in_frm) const;
 
-    //! keyframe1で観測している特徴点とkeyframe2で観測している特徴点の対応を求め，それを元にkeyframe1の特徴点と3次元点の対応情報を得る
-    //! matched_lms_in_keyfrm_1には，keyframe1の各特徴点に対応する(keyframe2で観測された)3次元点が格納される
-    //! NOTE: matched_lms_in_keyfrm_1.size()はkeyframe1の特徴点数と一致
+    //! Find the correspondence between the feature point observed in keyframe1 and the feature point observed in keyframe2,
+    //! and obtain the correspondence between the feature point in keyframe1 and the 3D points.
+    //! Matched_lms_in_keyfrm_1 stores the 3D points (observed in keyframe2) corresponding to each feature point in keyframe1
+    //! NOTE: matched_lms_in_keyfrm_1.size() matches the number of feature points in keyframe1
     unsigned int match_keyframes(const std::shared_ptr<data::keyframe>& keyfrm_1, const std::shared_ptr<data::keyframe>& keyfrm_2, std::vector<std::shared_ptr<data::landmark>>& matched_lms_in_keyfrm_1) const;
 };