Remove num_keypts from frame_observation

src/stella_vslam/CMakeLists.txt

@@ -24,7 +24,7 @@ find_package(g2o REQUIRED
              g2o::csparse)
 
 # gtsam
-set(USE_GTSAM OFF CACHE BOOL "Enable gtsam")
+set(USE_GTSAM ON CACHE BOOL "Enable gtsam")
 unset(GTSAM_INCLUDE_DIRS CACHE)
 find_path(GTSAM_INCLUDE_DIRS NAMES gtsam
           PATHS /usr/local/include/)

src/stella_vslam/data/frame.cc

@@ -19,7 +19,7 @@ frame::frame(const double timestamp, camera::base* camera, feature::orb_params*
     : id_(next_id_++), timestamp_(timestamp), camera_(camera), orb_params_(orb_params), frm_obs_(frm_obs),
       markers_2d_(markers_2d),
       // Initialize association with 3D points
-      landmarks_(std::vector<std::shared_ptr<landmark>>(frm_obs_.num_keypts_, nullptr)) {}
+      landmarks_(std::vector<std::shared_ptr<landmark>>(frm_obs_.undist_keypts_.size(), nullptr)) {}
 
 void frame::set_pose_cw(const Mat44_t& pose_cw) {
     pose_is_valid_ = true;

src/stella_vslam/data/frame_observation.h

@@ -13,15 +13,13 @@ struct frame_observation {
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
     frame_observation() = default;
-    frame_observation(unsigned int num_keypts, const cv::Mat& descriptors,
+    frame_observation(const cv::Mat& descriptors,
                       const std::vector<cv::KeyPoint>& undist_keypts, const eigen_alloc_vector<Vec3_t>& bearings,
                       const std::vector<float>& stereo_x_right, const std::vector<float>& depths,
                       const std::vector<std::vector<std::vector<unsigned int>>>& keypt_indices_in_cells)
-        : num_keypts_(num_keypts), descriptors_(descriptors), undist_keypts_(undist_keypts), bearings_(bearings),
+        : 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;
     //! descriptors
     cv::Mat descriptors_;
     //! undistorted keypoints of monocular or stereo left image

src/stella_vslam/data/keyframe.cc

@@ -35,7 +35,7 @@ keyframe::keyframe(const unsigned int id, const double timestamp,
       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<std::shared_ptr<landmark>>(frm_obs_.num_keypts_, nullptr)) {
+      landmarks_(std::vector<std::shared_ptr<landmark>>(frm_obs_.undist_keypts_.size(), nullptr)) {
     // set pose parameters (pose_wc_, trans_wc_) using pose_cw_
     set_pose_cw(pose_cw);
 
@@ -137,7 +137,7 @@ std::shared_ptr<keyframe> keyframe::from_stmt(sqlite3_stmt* stmt,
     std::vector<std::vector<std::vector<unsigned int>>> 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, descriptors, undist_keypts, bearings, stereo_x_right, depths, keypt_indices_in_cells};
+    frame_observation frm_obs{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(
@@ -180,7 +180,7 @@ nlohmann::json keyframe::to_json() const {
             {"rot_cw", convert_rotation_to_json(pose_cw_.block<3, 3>(0, 0))},
             {"trans_cw", convert_translation_to_json(pose_cw_.block<3, 1>(0, 3))},
             // features and observations
-            {"n_keypts", frm_obs_.num_keypts_},
+            {"n_keypts", frm_obs_.undist_keypts_.size()},
             {"undist_keypts", convert_keypoints_to_json(frm_obs_.undist_keypts_)},
             {"x_rights", frm_obs_.stereo_x_right_},
             {"depths", frm_obs_.depths_},
@@ -415,7 +415,7 @@ float keyframe::compute_median_depth(const bool abs) const {
     }
 
     std::vector<float> depths;
-    depths.reserve(frm_obs_.num_keypts_);
+    depths.reserve(frm_obs_.undist_keypts_.size());
     const Vec3_t rot_cw_z_row = pose_cw.block<1, 3>(2, 0);
     const float trans_cw_z = pose_cw(2, 3);
 

src/stella_vslam/data/map_database.cc

@@ -395,7 +395,7 @@ void map_database::register_keyframe(camera_database* cam_db, orb_params_databas
     std::vector<std::vector<std::vector<unsigned int>>> 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, descriptors, undist_keypts, bearings, stereo_x_right, depths, keypt_indices_in_cells};
+    frame_observation frm_obs{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(
@@ -603,7 +603,7 @@ void map_database::load_association_from_stmt(sqlite3_stmt* stmt) {
     auto keyfrm_id = sqlite3_column_int64(stmt, column_id);
     assert(keyframes_.count(keyfrm_id));
     column_id++;
-    std::vector<int> lm_ids(keyframes_.at(keyfrm_id)->frm_obs_.num_keypts_, -1);
+    std::vector<int> lm_ids(keyframes_.at(keyfrm_id)->frm_obs_.undist_keypts_.size(), -1);
     p = reinterpret_cast<const char*>(sqlite3_column_blob(stmt, column_id));
     std::memcpy(lm_ids.data(), p, sqlite3_column_bytes(stmt, column_id));
     column_id++;

src/stella_vslam/global_optimization_module.cc

@@ -399,7 +399,7 @@ void global_optimization_module::replace_duplicated_landmarks(const std::vector<
     {
         std::lock_guard<std::mutex> lock(data::map_database::mtx_database_);
 
-        for (unsigned int idx = 0; idx < cur_keyfrm_->frm_obs_.num_keypts_; ++idx) {
+        for (unsigned int idx = 0; idx < cur_keyfrm_->frm_obs_.undist_keypts_.size(); ++idx) {
             auto curr_match_lm_in_cand = curr_match_lms_observed_in_cand.at(idx);
             if (!curr_match_lm_in_cand) {
                 continue;

src/stella_vslam/match/bow_tree.cc

@@ -11,7 +11,7 @@ namespace match {
 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;
 
-    matched_lms_in_frm = std::vector<std::shared_ptr<data::landmark>>(frm.frm_obs_.num_keypts_, nullptr);
+    matched_lms_in_frm = std::vector<std::shared_ptr<data::landmark>>(frm.frm_obs_.undist_keypts_.size(), nullptr);
 
     const auto keyfrm_lms = keyfrm->get_landmarks();
 

src/stella_vslam/match/projection.cc

@@ -117,7 +117,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.frm_obs_.num_keypts_; ++idx_last) {
+    for (unsigned int idx_last = 0; idx_last < last_frm.frm_obs_.undist_keypts_.size(); ++idx_last) {
         const auto& lm = last_frm.get_landmark(idx_last);
         if (!lm) {
             continue;

src/stella_vslam/match/robust.cc

@@ -30,9 +30,9 @@ unsigned int robust::match_for_triangulation(const std::shared_ptr<data::keyfram
     // 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_.num_keypts_, false);
+    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_.num_keypts_, -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();
@@ -157,7 +157,7 @@ unsigned int robust::match_keyframes(const std::shared_ptr<data::keyframe>& keyf
                                      std::vector<std::shared_ptr<data::landmark>>& matched_lms_in_frm,
                                      bool validate_with_essential_solver, bool use_fixed_seed) const {
     // Initialization
-    const auto num_frm_keypts = keyfrm1->frm_obs_.num_keypts_;
+    const auto num_frm_keypts = keyfrm1->frm_obs_.undist_keypts_.size();
     const auto keyfrm_lms = keyfrm2->get_landmarks();
     unsigned int num_inlier_matches = 0;
     matched_lms_in_frm = std::vector<std::shared_ptr<data::landmark>>(num_frm_keypts, nullptr);
@@ -205,7 +205,7 @@ unsigned int robust::match_frame_and_keyframe(data::frame& frm, const std::share
                                               std::vector<std::shared_ptr<data::landmark>>& matched_lms_in_frm,
                                               bool use_fixed_seed) const {
     // Initialization
-    const auto num_frm_keypts = frm.frm_obs_.num_keypts_;
+    const auto num_frm_keypts = frm.frm_obs_.undist_keypts_.size();
     const auto keyfrm_lms = keyfrm->get_landmarks();
     unsigned int num_inlier_matches = 0;
     matched_lms_in_frm = std::vector<std::shared_ptr<data::landmark>>(num_frm_keypts, nullptr);
@@ -237,13 +237,15 @@ unsigned int robust::match_frame_and_keyframe(data::frame& frm, const std::share
     return num_inlier_matches;
 }
 
-unsigned int robust::brute_force_match(const data::frame_observation& frm_obs, const std::shared_ptr<data::keyframe>& keyfrm, std::vector<std::pair<int, int>>& matches) const {
+unsigned int robust::brute_force_match(const data::frame_observation& frm_obs,
+                                       const std::shared_ptr<data::keyframe>& keyfrm,
+                                       std::vector<std::pair<int, int>>& matches) const {
     unsigned int num_matches = 0;
 
     // 1. Acquire the frame and keyframe information
 
-    const auto num_keypts_1 = frm_obs.num_keypts_;
-    const auto num_keypts_2 = keyfrm->frm_obs_.num_keypts_;
+    const auto num_keypts_1 = frm_obs.undist_keypts_.size();
+    const auto num_keypts_2 = keyfrm->frm_obs_.undist_keypts_.size();
     const auto keypts_1 = frm_obs.undist_keypts_;
     const auto keypts_2 = keyfrm->frm_obs_.undist_keypts_;
     const auto lms_2 = keyfrm->get_landmarks();

src/stella_vslam/module/frame_tracker.cc

@@ -134,7 +134,7 @@ bool frame_tracker::robust_match_based_track(data::frame& curr_frm, const data::
 unsigned int frame_tracker::discard_outliers(const std::vector<bool>& outlier_flags, data::frame& curr_frm) const {
     unsigned int num_valid_matches = 0;
 
-    for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) {
+    for (unsigned int idx = 0; idx < curr_frm.frm_obs_.undist_keypts_.size(); ++idx) {
         if (curr_frm.get_landmark(idx) == nullptr) {
             continue;
         }

src/stella_vslam/module/initializer.cc

@@ -343,7 +343,7 @@ bool initializer::create_map_for_stereo(data::bow_vocabulary* bow_vocab, data::f
     curr_frm.ref_keyfrm_ = curr_keyfrm;
     map_db_->update_frame_statistics(curr_frm, false);
 
-    for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) {
+    for (unsigned int idx = 0; idx < curr_frm.frm_obs_.undist_keypts_.size(); ++idx) {
         // add a new landmark if tht corresponding depth is valid
         const auto z = curr_frm.frm_obs_.depths_.at(idx);
         if (z <= 0) {

src/stella_vslam/module/keyframe_inserter.cc

@@ -143,8 +143,8 @@ std::shared_ptr<data::keyframe> keyframe_inserter::create_new_keyframe(
 
     // Save the valid depth and index pairs
     std::vector<std::pair<float, unsigned int>> depth_idx_pairs;
-    depth_idx_pairs.reserve(curr_frm.frm_obs_.num_keypts_);
-    for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) {
+    depth_idx_pairs.reserve(curr_frm.frm_obs_.undist_keypts_.size());
+    for (unsigned int idx = 0; idx < curr_frm.frm_obs_.undist_keypts_.size(); ++idx) {
         assert(!curr_frm.frm_obs_.depths_.empty());
         const auto depth = curr_frm.frm_obs_.depths_.at(idx);
         // Add if the depth is valid

src/stella_vslam/module/local_map_updater.cc

@@ -24,17 +24,15 @@ std::shared_ptr<data::keyframe> local_map_updater::get_nearest_covisibility() co
 }
 
 bool local_map_updater::acquire_local_map(const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
-                                          const unsigned int num_keypts,
                                           unsigned int keyframe_id_threshold) {
-    const auto local_keyfrms_was_found = find_local_keyframes(frm_lms, num_keypts, keyframe_id_threshold);
-    const auto local_lms_was_found = find_local_landmarks(frm_lms, num_keypts, keyframe_id_threshold);
+    const auto local_keyfrms_was_found = find_local_keyframes(frm_lms, keyframe_id_threshold);
+    const auto local_lms_was_found = find_local_landmarks(frm_lms, keyframe_id_threshold);
     return local_keyfrms_was_found && local_lms_was_found;
 }
 
 bool local_map_updater::find_local_keyframes(const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
-                                             const unsigned int num_keypts,
                                              unsigned int keyframe_id_threshold) {
-    const auto num_shared_lms_and_keyfrm = count_num_shared_lms(frm_lms, num_keypts, keyframe_id_threshold);
+    const auto num_shared_lms_and_keyfrm = count_num_shared_lms(frm_lms, keyframe_id_threshold);
     if (num_shared_lms_and_keyfrm.empty()) {
         SPDLOG_TRACE("find_local_keyframes: empty");
         return false;
@@ -50,15 +48,14 @@ bool local_map_updater::find_local_keyframes(const std::vector<std::shared_ptr<d
 
 auto local_map_updater::count_num_shared_lms(
     const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
-    const unsigned int num_keypts,
     unsigned int keyframe_id_threshold) const
     -> std::vector<std::pair<unsigned int, std::shared_ptr<data::keyframe>>> {
     std::vector<std::pair<unsigned int, std::shared_ptr<data::keyframe>>> num_shared_lms_and_keyfrm;
 
     // count the number of sharing landmarks between the current frame and each of the neighbor keyframes
     // key: keyframe, value: number of sharing landmarks
     keyframe_to_num_shared_lms_t keyfrm_to_num_shared_lms;
-    for (unsigned int idx = 0; idx < num_keypts; ++idx) {
+    for (unsigned int idx = 0; idx < frm_lms.size(); ++idx) {
         auto& lm = frm_lms.at(idx);
         if (!lm) {
             continue;
@@ -179,13 +176,12 @@ auto local_map_updater::find_second_local_keyframes(const std::vector<std::share
 }
 
 bool local_map_updater::find_local_landmarks(const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
-                                             const unsigned int num_keypts,
                                              unsigned int keyframe_id_threshold) {
     local_lms_.clear();
     local_lms_.reserve(50 * local_keyfrms_.size());
 
     std::unordered_set<unsigned int> already_found_lms_ids;
-    for (unsigned int idx = 0; idx < num_keypts; ++idx) {
+    for (unsigned int idx = 0; idx < frm_lms.size(); ++idx) {
         auto& lm = frm_lms.at(idx);
         if (!lm) {
             continue;

src/stella_vslam/module/local_map_updater.h

@@ -34,19 +34,16 @@ class local_map_updater {
 
     //! Acquire the new local map
     bool acquire_local_map(const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
-                           const unsigned int num_keypts,
                            unsigned int keyframe_id_threshold = 0);
 
 private:
     //! Find the local keyframes
     bool find_local_keyframes(const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
-                              const unsigned int num_keypts,
                               unsigned int keyframe_id_threshold);
 
     //! Count the number of shared landmarks between the current frame and each of the neighbor keyframes
     auto count_num_shared_lms(
         const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
-        const unsigned int num_keypts,
         unsigned int keyframe_id_threshold) const
         -> std::vector<std::pair<unsigned int, std::shared_ptr<data::keyframe>>>;
 
@@ -64,7 +61,6 @@ class local_map_updater {
 
     //! Find the local landmarks
     bool find_local_landmarks(const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
-                              const unsigned int num_keypts,
                               unsigned int keyframe_id_threshold);
 
     // maximum number of the local keyframes

src/stella_vslam/module/loop_detector.cc

@@ -424,7 +424,7 @@ bool loop_detector::select_loop_candidate_via_Sim3(const std::unordered_set<std:
         const auto inlier_indices = util::resample_by_indices(valid_indices, pnp_solver->get_inlier_flags());
 
         // Set 2D-3D matches for the pose optimization
-        auto lms_in_cand = std::vector<std::shared_ptr<data::landmark>>(cur_keyfrm_->frm_obs_.num_keypts_, nullptr);
+        auto lms_in_cand = std::vector<std::shared_ptr<data::landmark>>(cur_keyfrm_->frm_obs_.undist_keypts_.size(), nullptr);
         for (const auto idx : inlier_indices) {
             // Set only the valid 3D points to the current frame
             lms_in_cand.at(idx) = curr_match_lms_observed_in_cand.at(idx);
@@ -445,7 +445,7 @@ bool loop_detector::select_loop_candidate_via_Sim3(const std::unordered_set<std:
         }
 
         // Reject outliers
-        for (unsigned int idx = 0; idx < cur_keyfrm_->frm_obs_.num_keypts_; idx++) {
+        for (unsigned int idx = 0; idx < cur_keyfrm_->frm_obs_.undist_keypts_.size(); idx++) {
             if (!outlier_flags.at(idx)) {
                 continue;
             }
@@ -486,7 +486,7 @@ bool loop_detector::select_loop_candidate_via_Sim3(const std::unordered_set<std:
 
         // Exclude the already-associated landmarks
         std::set<std::shared_ptr<data::landmark>> already_found_landmarks1;
-        for (unsigned int idx = 0; idx < cur_keyfrm_->frm_obs_.num_keypts_; ++idx) {
+        for (unsigned int idx = 0; idx < cur_keyfrm_->frm_obs_.undist_keypts_.size(); ++idx) {
             if (!curr_match_lms_observed_in_cand.at(idx)) {
                 continue;
             }
@@ -518,7 +518,7 @@ bool loop_detector::select_loop_candidate_via_Sim3(const std::unordered_set<std:
         }
 
         // Reject outliers
-        for (unsigned int idx = 0; idx < cur_keyfrm_->frm_obs_.num_keypts_; ++idx) {
+        for (unsigned int idx = 0; idx < cur_keyfrm_->frm_obs_.undist_keypts_.size(); ++idx) {
             if (!outlier_flags2.at(idx)) {
                 continue;
             }

src/stella_vslam/module/relocalizer.cc

@@ -223,7 +223,7 @@ bool relocalizer::optimize_pose(data::frame& curr_frm,
     }
 
     // Reject outliers
-    for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; idx++) {
+    for (unsigned int idx = 0; idx < curr_frm.frm_obs_.undist_keypts_.size(); idx++) {
         if (!outlier_flags.at(idx)) {
             continue;
         }
@@ -256,7 +256,7 @@ bool relocalizer::refine_pose(data::frame& curr_frm,
 
     // Exclude the already-associated landmarks
     std::set<std::shared_ptr<data::landmark>> already_found_landmarks1;
-    for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) {
+    for (unsigned int idx = 0; idx < curr_frm.frm_obs_.undist_keypts_.size(); ++idx) {
         const auto& lm = curr_frm.get_landmark(idx);
         if (!lm) {
             continue;
@@ -286,7 +286,7 @@ bool relocalizer::refine_pose(data::frame& curr_frm,
     }
 
     // Reject outliers
-    for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) {
+    for (unsigned int idx = 0; idx < curr_frm.frm_obs_.undist_keypts_.size(); ++idx) {
         if (!outlier_flags2.at(idx)) {
             continue;
         }
@@ -300,7 +300,7 @@ bool relocalizer::refine_pose_by_local_map(data::frame& curr_frm,
                                            const std::shared_ptr<stella_vslam::data::keyframe>& candidate_keyfrm) const {
     // Create local map
     auto local_map_updater = module::local_map_updater(max_num_local_keyfrms_);
-    if (!local_map_updater.acquire_local_map(curr_frm.get_landmarks(), curr_frm.frm_obs_.num_keypts_)) {
+    if (!local_map_updater.acquire_local_map(curr_frm.get_landmarks())) {
         return false;
     }
     auto local_keyfrms = local_map_updater.get_local_keyframes();
@@ -367,7 +367,7 @@ bool relocalizer::refine_pose_by_local_map(data::frame& curr_frm,
         curr_frm.set_pose_cw(optimized_pose);
 
         // Reject outliers
-        for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) {
+        for (unsigned int idx = 0; idx < curr_frm.frm_obs_.undist_keypts_.size(); ++idx) {
             if (!outlier_flags.at(idx)) {
                 continue;
             }