diff --git a/src/stella_vslam/CMakeLists.txt b/src/stella_vslam/CMakeLists.txt
index b1e1e2b6e..84b0ce69d 100644
--- a/src/stella_vslam/CMakeLists.txt
+++ b/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/)
diff --git a/src/stella_vslam/data/frame.cc b/src/stella_vslam/data/frame.cc
index df6af5ec8..5a9a2829c 100644
--- a/src/stella_vslam/data/frame.cc
+++ b/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;
diff --git a/src/stella_vslam/data/frame_observation.h b/src/stella_vslam/data/frame_observation.h
index 405178b28..4cf9191f2 100644
--- a/src/stella_vslam/data/frame_observation.h
+++ b/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
diff --git a/src/stella_vslam/data/keyframe.cc b/src/stella_vslam/data/keyframe.cc
index bd72cc390..961ebc67c 100644
--- a/src/stella_vslam/data/keyframe.cc
+++ b/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);
 
diff --git a/src/stella_vslam/data/map_database.cc b/src/stella_vslam/data/map_database.cc
index 1232f8761..13c7195a1 100644
--- a/src/stella_vslam/data/map_database.cc
+++ b/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++;
diff --git a/src/stella_vslam/global_optimization_module.cc b/src/stella_vslam/global_optimization_module.cc
index e2d0647a2..34d1ef792 100644
--- a/src/stella_vslam/global_optimization_module.cc
+++ b/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;
diff --git a/src/stella_vslam/match/bow_tree.cc b/src/stella_vslam/match/bow_tree.cc
index d83c836cd..faae61ffd 100644
--- a/src/stella_vslam/match/bow_tree.cc
+++ b/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();
 
diff --git a/src/stella_vslam/match/projection.cc b/src/stella_vslam/match/projection.cc
index ced3610fe..ec39060f5 100644
--- a/src/stella_vslam/match/projection.cc
+++ b/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;
diff --git a/src/stella_vslam/match/robust.cc b/src/stella_vslam/match/robust.cc
index c659fb2ad..4f039e05f 100644
--- a/src/stella_vslam/match/robust.cc
+++ b/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();
diff --git a/src/stella_vslam/module/frame_tracker.cc b/src/stella_vslam/module/frame_tracker.cc
index fc8ff8874..61b215f5d 100644
--- a/src/stella_vslam/module/frame_tracker.cc
+++ b/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;
         }
diff --git a/src/stella_vslam/module/initializer.cc b/src/stella_vslam/module/initializer.cc
index 6c6baa4c1..6eab1ed8a 100644
--- a/src/stella_vslam/module/initializer.cc
+++ b/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) {
diff --git a/src/stella_vslam/module/keyframe_inserter.cc b/src/stella_vslam/module/keyframe_inserter.cc
index 2fe2ed269..cfdfb42ed 100644
--- a/src/stella_vslam/module/keyframe_inserter.cc
+++ b/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
diff --git a/src/stella_vslam/module/local_map_updater.cc b/src/stella_vslam/module/local_map_updater.cc
index 3e0efc3ed..505243971 100644
--- a/src/stella_vslam/module/local_map_updater.cc
+++ b/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,7 +48,6 @@ 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;
@@ -58,7 +55,7 @@ auto local_map_updater::count_num_shared_lms(
     // 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;
diff --git a/src/stella_vslam/module/local_map_updater.h b/src/stella_vslam/module/local_map_updater.h
index d730413ca..2be2c6077 100644
--- a/src/stella_vslam/module/local_map_updater.h
+++ b/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
diff --git a/src/stella_vslam/module/loop_detector.cc b/src/stella_vslam/module/loop_detector.cc
index ab620ceda..39f4a2314 100644
--- a/src/stella_vslam/module/loop_detector.cc
+++ b/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;
             }
diff --git a/src/stella_vslam/module/relocalizer.cc b/src/stella_vslam/module/relocalizer.cc
index 191ea8341..456a08503 100644
--- a/src/stella_vslam/module/relocalizer.cc
+++ b/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;
             }
diff --git a/src/stella_vslam/optimize/pose_optimizer_g2o.cc b/src/stella_vslam/optimize/pose_optimizer_g2o.cc
index 3a10ce9b0..95e2a3f94 100644
--- a/src/stella_vslam/optimize/pose_optimizer_g2o.cc
+++ b/src/stella_vslam/optimize/pose_optimizer_g2o.cc
@@ -63,7 +63,7 @@ unsigned int pose_optimizer_g2o::optimize(const Mat44_t& cam_pose_cw, const data
     frm_vtx->setFixed(false);
     optimizer.addVertex(frm_vtx);
 
-    const unsigned int num_keypts = frm_obs.num_keypts_;
+    const unsigned int num_keypts = frm_obs.undist_keypts_.size();
     outlier_flags.resize(num_keypts);
     std::fill(outlier_flags.begin(), outlier_flags.end(), false);
 
diff --git a/src/stella_vslam/optimize/pose_optimizer_gtsam.cc b/src/stella_vslam/optimize/pose_optimizer_gtsam.cc
index 9305489cc..ed0d4b007 100644
--- a/src/stella_vslam/optimize/pose_optimizer_gtsam.cc
+++ b/src/stella_vslam/optimize/pose_optimizer_gtsam.cc
@@ -68,7 +68,7 @@ unsigned int pose_optimizer_gtsam::optimize(const Mat44_t& cam_pose_cw, const da
     values.insert(gtsam::Symbol('x', 0),
                   gtsam::Pose3(util::converter::inverse_pose(normalized_cam_pose_cw)));
 
-    const unsigned int num_keypts = frm_obs.num_keypts_;
+    const unsigned int num_keypts = frm_obs.undist_keypts_.size();
     outlier_flags.resize(num_keypts);
     std::fill(outlier_flags.begin(), outlier_flags.end(), false);
 
diff --git a/src/stella_vslam/system.cc b/src/stella_vslam/system.cc
index cbc5cfed4..e1186e724 100644
--- a/src/stella_vslam/system.cc
+++ b/src/stella_vslam/system.cc
@@ -291,7 +291,6 @@ data::frame system::create_monocular_frame(const cv::Mat& img, const double time
     // Extract ORB feature
     keypts_.clear();
     extractor_left_->extract(img_gray, mask, keypts_, frm_obs.descriptors_);
-    frm_obs.num_keypts_ = keypts_.size();
     if (keypts_.empty()) {
         spdlog::warn("preprocess: cannot extract any keypoints");
     }
@@ -343,7 +342,6 @@ data::frame system::create_stereo_frame(const cv::Mat& left_img, const cv::Mat&
     });
     thread_left.join();
     thread_right.join();
-    frm_obs.num_keypts_ = keypts_.size();
     if (keypts_.empty()) {
         spdlog::warn("preprocess: cannot extract any keypoints");
     }
@@ -391,7 +389,6 @@ data::frame system::create_RGBD_frame(const cv::Mat& rgb_img, const cv::Mat& dep
     // Extract ORB feature
     keypts_.clear();
     extractor_left_->extract(img_gray, mask, keypts_, frm_obs.descriptors_);
-    frm_obs.num_keypts_ = keypts_.size();
     if (keypts_.empty()) {
         spdlog::warn("preprocess: cannot extract any keypoints");
     }
@@ -401,10 +398,10 @@ data::frame system::create_RGBD_frame(const cv::Mat& rgb_img, const cv::Mat& dep
 
     // Calculate disparity from depth
     // Initialize with invalid value
-    frm_obs.stereo_x_right_ = std::vector<float>(frm_obs.num_keypts_, -1);
-    frm_obs.depths_ = std::vector<float>(frm_obs.num_keypts_, -1);
+    frm_obs.stereo_x_right_ = std::vector<float>(frm_obs.undist_keypts_.size(), -1);
+    frm_obs.depths_ = std::vector<float>(frm_obs.undist_keypts_.size(), -1);
 
-    for (unsigned int idx = 0; idx < frm_obs.num_keypts_; idx++) {
+    for (unsigned int idx = 0; idx < frm_obs.undist_keypts_.size(); idx++) {
         const auto& keypt = keypts_.at(idx);
         const auto& undist_keypt = frm_obs.undist_keypts_.at(idx);
 
diff --git a/src/stella_vslam/tracking_module.cc b/src/stella_vslam/tracking_module.cc
index 710fe704d..b7141a52f 100644
--- a/src/stella_vslam/tracking_module.cc
+++ b/src/stella_vslam/tracking_module.cc
@@ -402,7 +402,7 @@ void tracking_module::update_motion_model() {
 
 void tracking_module::replace_landmarks_in_last_frm(nondeterministic::unordered_map<std::shared_ptr<data::landmark>, std::shared_ptr<data::landmark>>& replaced_lms) {
     std::lock_guard<std::mutex> lock(mtx_last_frm_);
-    for (unsigned int idx = 0; idx < last_frm_.frm_obs_.num_keypts_; ++idx) {
+    for (unsigned int idx = 0; idx < last_frm_.frm_obs_.undist_keypts_.size(); ++idx) {
         const auto& lm = last_frm_.get_landmark(idx);
         if (!lm) {
             continue;
@@ -436,7 +436,7 @@ bool tracking_module::optimize_current_frame_with_local_map(unsigned int& num_tr
     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;
         }
@@ -446,7 +446,7 @@ bool tracking_module::optimize_current_frame_with_local_map(unsigned int& num_tr
     // count up the number of tracked landmarks
     num_tracked_lms = 0;
     num_reliable_lms = 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) {
         const auto& lm = curr_frm_.get_landmark(idx);
         if (!lm) {
             continue;
@@ -487,7 +487,7 @@ bool tracking_module::optimize_current_frame_with_local_map(unsigned int& num_tr
 
 bool tracking_module::update_local_map(unsigned int fixed_keyframe_id_threshold) {
     // clean landmark associations
-    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;
@@ -501,7 +501,7 @@ bool tracking_module::update_local_map(unsigned int fixed_keyframe_id_threshold)
     // acquire the current local map
     local_landmarks_.clear();
     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_, fixed_keyframe_id_threshold)) {
+    if (!local_map_updater.acquire_local_map(curr_frm_.get_landmarks(), fixed_keyframe_id_threshold)) {
         return false;
     }
     // update the variables