diff --git a/src/stella_vslam/data/common.cc b/src/stella_vslam/data/common.cc index a25ba0ffd..f64a7481b 100644 --- a/src/stella_vslam/data/common.cc +++ b/src/stella_vslam/data/common.cc @@ -143,7 +143,8 @@ std::vector get_keypoints_in_cell(const camera::base* camera, cons return indices; } - const bool check_level = (0 < min_level) || (0 <= max_level); + const bool check_min_level = 0 <= min_level; + const bool check_max_level = 0 <= max_level; for (int cell_idx_x = min_cell_idx_x; cell_idx_x <= max_cell_idx_x; ++cell_idx_x) { for (int cell_idx_y = min_cell_idx_y; cell_idx_y <= max_cell_idx_y; ++cell_idx_y) { @@ -155,13 +156,11 @@ std::vector get_keypoints_in_cell(const camera::base* camera, cons for (unsigned int idx : keypt_indices_in_cell) { const auto& undist_keypt = undist_keypts.at(idx); - if (check_level) { - if (undist_keypt.octave < min_level) { - continue; - } - if (0 <= max_level && max_level < undist_keypt.octave) { - continue; - } + if (check_min_level && undist_keypt.octave < min_level) { + continue; + } + if (check_max_level && max_level < undist_keypt.octave) { + continue; } const float dist_x = undist_keypt.pt.x - ref_x; diff --git a/src/stella_vslam/match/fuse.cc b/src/stella_vslam/match/fuse.cc index 78db494f0..ee95ce36f 100644 --- a/src/stella_vslam/match/fuse.cc +++ b/src/stella_vslam/match/fuse.cc @@ -69,7 +69,9 @@ unsigned int fuse::detect_duplication(const std::shared_ptr& key // Acquire keypoints in the cell where the reprojected 3D points exist const auto pred_scale_level = lm->predict_scale_level(cam_to_lm_dist, keyfrm->orb_params_->num_levels_, keyfrm->orb_params_->log_scale_factor_); - const auto indices = keyfrm->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm->orb_params_->scale_factors_.at(pred_scale_level)); + const int min_level = std::max(0, static_cast(pred_scale_level) - 1); + const int max_level = std::min(keyfrm->orb_params_->num_levels_ - 1, pred_scale_level + 1); + const auto indices = keyfrm->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm->orb_params_->scale_factors_.at(pred_scale_level), min_level, max_level); if (indices.empty()) { continue; @@ -87,14 +89,8 @@ unsigned int fuse::detect_duplication(const std::shared_ptr& key } const auto& undist_keypt = keyfrm->frm_obs_.undist_keypts_.at(idx); - const auto scale_level = static_cast(undist_keypt.octave); - - // TODO: shoud determine the scale with 'keyfrm-> get_keypts_in_cell ()' - if (scale_level + 1 < pred_scale_level || pred_scale_level < scale_level) { - continue; - } - if (do_reprojection_matching) { + const auto scale_level = static_cast(undist_keypt.octave); if (!keyfrm->frm_obs_.stereo_x_right_.empty() && keyfrm->frm_obs_.stereo_x_right_.at(idx) >= 0) { // Compute reprojection error with 3 degrees of freedom if a stereo match exists const auto e_x = reproj(0) - undist_keypt.pt.x; diff --git a/src/stella_vslam/match/projection.cc b/src/stella_vslam/match/projection.cc index 3a4db1cee..ced3610fe 100644 --- a/src/stella_vslam/match/projection.cc +++ b/src/stella_vslam/match/projection.cc @@ -14,7 +14,7 @@ unsigned int projection::match_frame_and_landmarks(data::frame& frm, const std::vector>& local_landmarks, eigen_alloc_unord_map& lm_to_reproj, std::unordered_map& lm_to_x_right, - std::unordered_map& lm_to_scale, + std::unordered_map& lm_to_scale, const float margin) const { unsigned int num_matches = 0; @@ -27,13 +27,14 @@ unsigned int projection::match_frame_and_landmarks(data::frame& frm, continue; } - const auto pred_scale_level = lm_to_scale.at(local_lm->id_); - // Acquire keypoints in the cell where the reprojected 3D points exist Vec2_t reproj = lm_to_reproj.at(local_lm->id_); + const auto pred_scale_level = lm_to_scale.at(local_lm->id_); + const int min_level = std::max(0, static_cast(pred_scale_level) - 1); + const int max_level = std::min(frm.orb_params_->num_levels_ - 1, pred_scale_level + 1); const auto indices_in_cell = frm.get_keypoints_in_cell(reproj(0), reproj(1), margin * frm.orb_params_->scale_factors_.at(pred_scale_level), - pred_scale_level - 1, pred_scale_level); + min_level, max_level); if (indices_in_cell.empty()) { continue; } @@ -139,20 +140,20 @@ unsigned int projection::match_current_and_last_frames(data::frame& curr_frm, co } // Acquire keypoints in the cell where the reprojected 3D points exist - const auto last_scale_level = last_frm.frm_obs_.undist_keypts_.at(idx_last).octave; + const unsigned int last_scale_level = last_frm.frm_obs_.undist_keypts_.at(idx_last).octave; int min_level; int max_level; if (assume_forward) { min_level = last_scale_level; - max_level = last_frm.orb_params_->num_levels_ - 1; + max_level = std::min(last_frm.orb_params_->num_levels_ - 1, last_scale_level + 1); } else if (assume_backward) { - min_level = 0; + min_level = std::max(0, static_cast(last_scale_level) - 1); max_level = last_scale_level; } else { - min_level = last_scale_level - 1; - max_level = last_scale_level + 1; + min_level = std::max(0, static_cast(last_scale_level) - 1); + max_level = std::min(last_frm.orb_params_->num_levels_ - 1, last_scale_level + 1); } auto indices = curr_frm.get_keypoints_in_cell(reproj(0), reproj(1), margin * curr_frm.orb_params_->scale_factors_.at(last_scale_level), @@ -271,10 +272,11 @@ unsigned int projection::match_frame_and_keyframe(const Mat44_t& cam_pose_cw, // Acquire keypoints in the cell where the reprojected 3D points exist const auto pred_scale_level = lm->predict_scale_level(cam_to_lm_dist, orb_params->num_levels_, orb_params->log_scale_factor_); - + const int min_level = std::max(0, static_cast(pred_scale_level) - 1); + const int max_level = std::min(orb_params->num_levels_ - 1, pred_scale_level + 1); const auto indices = data::get_keypoints_in_cell(camera, frm_obs, reproj(0), reproj(1), margin * orb_params->scale_factors_.at(pred_scale_level), - pred_scale_level - 1, pred_scale_level + 1); + min_level, max_level); if (indices.empty()) { continue; @@ -373,7 +375,9 @@ unsigned int projection::match_by_Sim3_transform(const std::shared_ptrpredict_scale_level(cam_to_lm_dist, keyfrm->orb_params_->num_levels_, keyfrm->orb_params_->log_scale_factor_); - const auto indices = keyfrm->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm->orb_params_->scale_factors_.at(pred_scale_level)); + const int min_level = std::max(0, static_cast(pred_scale_level) - 1); + const int max_level = std::min(keyfrm->orb_params_->num_levels_ - 1, pred_scale_level + 1); + const auto indices = keyfrm->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm->orb_params_->scale_factors_.at(pred_scale_level), min_level, max_level); if (indices.empty()) { continue; @@ -390,13 +394,6 @@ unsigned int projection::match_by_Sim3_transform(const std::shared_ptr(keyfrm->frm_obs_.undist_keypts_.at(idx).octave); - - // TODO: should determine the scale with 'keyfrm-> get_keypts_in_cell ()' - if (scale_level < pred_scale_level - 1 || pred_scale_level < scale_level) { - continue; - } - const auto& desc = keyfrm->frm_obs_.descriptors_.row(idx); const auto hamm_dist = compute_descriptor_distance_32(lm_desc, desc); @@ -503,7 +500,9 @@ unsigned int projection::match_keyframes_mutually(const std::shared_ptrpredict_scale_level(cam_to_lm_dist, keyfrm_2->orb_params_->num_levels_, keyfrm_2->orb_params_->log_scale_factor_); - const auto indices = keyfrm_2->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm_2->orb_params_->scale_factors_.at(pred_scale_level)); + const int min_level = std::max(0, static_cast(pred_scale_level) - 1); + const int max_level = std::min(keyfrm_2->orb_params_->num_levels_ - 1, pred_scale_level + 1); + const auto indices = keyfrm_2->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm_2->orb_params_->scale_factors_.at(pred_scale_level), min_level, max_level); if (indices.empty()) { continue; @@ -516,13 +515,6 @@ unsigned int projection::match_keyframes_mutually(const std::shared_ptr(keyfrm_2->frm_obs_.undist_keypts_.at(idx_2).octave); - - // TODO: should determine the scale with 'keyfrm-> get_keypts_in_cell ()' - if (scale_level < pred_scale_level - 1 || pred_scale_level < scale_level) { - continue; - } - const auto& desc = keyfrm_2->frm_obs_.descriptors_.row(idx_2); const auto hamm_dist = compute_descriptor_distance_32(lm_desc, desc); @@ -587,8 +579,9 @@ unsigned int projection::match_keyframes_mutually(const std::shared_ptrpredict_scale_level(cam_to_lm_dist, keyfrm_1->orb_params_->num_levels_, keyfrm_1->orb_params_->log_scale_factor_); - - const auto indices = keyfrm_1->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm_1->orb_params_->scale_factors_.at(pred_scale_level)); + const int min_level = std::max(0, static_cast(pred_scale_level) - 1); + const int max_level = std::min(keyfrm_1->orb_params_->num_levels_ - 1, pred_scale_level + 1); + const auto indices = keyfrm_1->get_keypoints_in_cell(reproj(0), reproj(1), margin * keyfrm_1->orb_params_->scale_factors_.at(pred_scale_level), min_level, max_level); if (indices.empty()) { continue; @@ -601,13 +594,6 @@ unsigned int projection::match_keyframes_mutually(const std::shared_ptr(keyfrm_1->frm_obs_.undist_keypts_.at(idx_1).octave); - - // TODO: should determine the scale with 'keyfrm-> get_keypts_in_cell ()' - if (scale_level < pred_scale_level - 1 || pred_scale_level < scale_level) { - continue; - } - const auto& desc = keyfrm_1->frm_obs_.descriptors_.row(idx_1); const auto hamm_dist = compute_descriptor_distance_32(lm_desc, desc); diff --git a/src/stella_vslam/match/projection.h b/src/stella_vslam/match/projection.h index 744573fcc..fab07e122 100644 --- a/src/stella_vslam/match/projection.h +++ b/src/stella_vslam/match/projection.h @@ -30,7 +30,7 @@ class projection final : public base { const std::vector>& local_landmarks, eigen_alloc_unord_map& lm_to_reproj, std::unordered_map& lm_to_x_right, - std::unordered_map& lm_to_scale, + std::unordered_map& lm_to_scale, const float margin = 5.0) const; //! last frameで観測している3次元点をcurrent frameに再投影し，frame.landmarks_に対応情報を記録する diff --git a/src/stella_vslam/module/relocalizer.cc b/src/stella_vslam/module/relocalizer.cc index 1e7f5798c..191ea8341 100644 --- a/src/stella_vslam/module/relocalizer.cc +++ b/src/stella_vslam/module/relocalizer.cc @@ -332,7 +332,7 @@ bool relocalizer::refine_pose_by_local_map(data::frame& curr_frm, unsigned int pred_scale_level; eigen_alloc_unord_map lm_to_reproj; std::unordered_map lm_to_x_right; - std::unordered_map lm_to_scale; + std::unordered_map lm_to_scale; for (const auto& lm : local_landmarks) { if (curr_landmark_ids.count(lm->id_)) { continue; diff --git a/src/stella_vslam/tracking_module.cc b/src/stella_vslam/tracking_module.cc index d8e66362c..710fe704d 100644 --- a/src/stella_vslam/tracking_module.cc +++ b/src/stella_vslam/tracking_module.cc @@ -543,7 +543,7 @@ bool tracking_module::search_local_landmarks() { unsigned int pred_scale_level; eigen_alloc_unord_map lm_to_reproj; std::unordered_map lm_to_x_right; - std::unordered_map lm_to_scale; + std::unordered_map lm_to_scale; for (const auto& lm : local_landmarks_) { if (curr_landmark_ids.count(lm->id_)) { continue;