From a39d3bb5f4061a38a75c0f539ea0e26988cc3a55 Mon Sep 17 00:00:00 2001 From: ymd-stella <7959916+ymd-stella@users.noreply.github.com> Date: Sun, 28 Jan 2024 14:12:30 +0900 Subject: [PATCH] Remove num_keypts from frame_observation (#554) --- src/stella_vslam/data/frame.cc | 2 +- src/stella_vslam/data/frame_observation.h | 6 ++---- src/stella_vslam/data/keyframe.cc | 8 ++++---- src/stella_vslam/data/map_database.cc | 4 ++-- src/stella_vslam/global_optimization_module.cc | 2 +- src/stella_vslam/match/bow_tree.cc | 2 +- src/stella_vslam/match/projection.cc | 2 +- src/stella_vslam/match/robust.cc | 16 +++++++++------- src/stella_vslam/module/frame_tracker.cc | 2 +- src/stella_vslam/module/initializer.cc | 2 +- src/stella_vslam/module/keyframe_inserter.cc | 4 ++-- src/stella_vslam/module/local_map_updater.cc | 14 +++++--------- src/stella_vslam/module/local_map_updater.h | 4 ---- src/stella_vslam/module/loop_detector.cc | 8 ++++---- src/stella_vslam/module/relocalizer.cc | 10 +++++----- src/stella_vslam/optimize/pose_optimizer_g2o.cc | 2 +- .../optimize/pose_optimizer_gtsam.cc | 2 +- src/stella_vslam/system.cc | 9 +++------ src/stella_vslam/tracking_module.cc | 10 +++++----- 19 files changed, 49 insertions(+), 60 deletions(-) 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>(frm_obs_.num_keypts_, nullptr)) {} + landmarks_(std::vector>(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& 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), 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>(frm_obs_.num_keypts_, nullptr)) { + landmarks_(std::vector>(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::from_stmt(sqlite3_stmt* stmt, 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, 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 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>> 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 lm_ids(keyframes_.at(keyfrm_id)->frm_obs_.num_keypts_, -1); + std::vector lm_ids(keyframes_.at(keyfrm_id)->frm_obs_.undist_keypts_.size(), -1); p = reinterpret_cast(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 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& keyfrm, data::frame& frm, std::vector>& matched_lms_in_frm) const { unsigned int num_matches = 0; - matched_lms_in_frm = std::vector>(frm.frm_obs_.num_keypts_, nullptr); + matched_lms_in_frm = std::vector>(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 is_already_matched_in_keyfrm_2(keyfrm_2->frm_obs_.num_keypts_, false); + std::vector 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 matched_indices_2_in_keyfrm_1(keyfrm_1->frm_obs_.num_keypts_, -1); + std::vector 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& keyf std::vector>& 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>(num_frm_keypts, nullptr); @@ -205,7 +205,7 @@ unsigned int robust::match_frame_and_keyframe(data::frame& frm, const std::share std::vector>& 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>(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& keyfrm, std::vector>& matches) const { +unsigned int robust::brute_force_match(const data::frame_observation& frm_obs, + const std::shared_ptr& keyfrm, + std::vector>& 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& 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 keyframe_inserter::create_new_keyframe( // Save the valid depth and index pairs std::vector> 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 local_map_updater::get_nearest_covisibility() co } bool local_map_updater::acquire_local_map(const std::vector>& 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>& 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>& frm_lms, - const unsigned int num_keypts, unsigned int keyframe_id_threshold) const -> std::vector>> { std::vector>> 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>& 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 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>& 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>& 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>& frm_lms, - const unsigned int num_keypts, unsigned int keyframe_id_threshold) const -> std::vector>>; @@ -64,7 +61,6 @@ class local_map_updater { //! Find the local landmarks bool find_local_landmarks(const std::vector>& 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_setget_inlier_flags()); // Set 2D-3D matches for the pose optimization - auto lms_in_cand = std::vector>(cur_keyfrm_->frm_obs_.num_keypts_, nullptr); + auto lms_in_cand = std::vector>(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_setfrm_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> 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_setfrm_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> 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& 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(frm_obs.num_keypts_, -1); - frm_obs.depths_ = std::vector(frm_obs.num_keypts_, -1); + frm_obs.stereo_x_right_ = std::vector(frm_obs.undist_keypts_.size(), -1); + frm_obs.depths_ = std::vector(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>& replaced_lms) { std::lock_guard 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