Skip to content

Commit

Permalink
Fix scale range in matching modules (#550)
Browse files Browse the repository at this point in the history
  • Loading branch information
@ymd-stella
ymd-stella authored Dec 24, 2023
1 parent fc4af23 commit 6a46619
Show file tree
Hide file tree
Showing 6 changed files with 36 additions and 55 deletions.
15 changes: 7 additions & 8 deletions src/stella_vslam/data/common.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -143,7 +143,8 @@ std::vector<unsigned int> 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) {
Expand All @@ -155,13 +156,11 @@ std::vector<unsigned int> 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;
Expand Down
12 changes: 4 additions & 8 deletions src/stella_vslam/match/fuse.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,9 @@ unsigned int fuse::detect_duplication(const std::shared_ptr<data::keyframe>& 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<int>(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;
Expand All @@ -87,14 +89,8 @@ unsigned int fuse::detect_duplication(const std::shared_ptr<data::keyframe>& key
}
const auto& undist_keypt = keyfrm->frm_obs_.undist_keypts_.at(idx);

const auto scale_level = static_cast<unsigned int>(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<unsigned int>(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;
Expand Down
58 changes: 22 additions & 36 deletions src/stella_vslam/match/projection.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ unsigned int projection::match_frame_and_landmarks(data::frame& frm,
const std::vector<std::shared_ptr<data::landmark>>& local_landmarks,
eigen_alloc_unord_map<unsigned int, Vec2_t>& lm_to_reproj,
std::unordered_map<unsigned int, float>& lm_to_x_right,
std::unordered_map<unsigned int, int>& lm_to_scale,
std::unordered_map<unsigned int, unsigned int>& lm_to_scale,
const float margin) const {
unsigned int num_matches = 0;

Expand All @@ -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<int>(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;
}
Expand Down Expand Up @@ -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<int>(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<int>(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),
Expand Down Expand Up @@ -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<int>(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;
Expand Down Expand Up @@ -373,7 +375,9 @@ unsigned int projection::match_by_Sim3_transform(const std::shared_ptr<data::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<int>(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;
Expand All @@ -390,13 +394,6 @@ unsigned int projection::match_by_Sim3_transform(const std::shared_ptr<data::key
continue;
}

const auto scale_level = static_cast<unsigned int>(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);
Expand Down Expand Up @@ -503,7 +500,9 @@ unsigned int projection::match_keyframes_mutually(const std::shared_ptr<data::ke

// 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_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<int>(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;
Expand All @@ -516,13 +515,6 @@ unsigned int projection::match_keyframes_mutually(const std::shared_ptr<data::ke
int best_idx_2 = -1;

for (const auto idx_2 : indices) {
const auto scale_level = static_cast<unsigned int>(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);
Expand Down Expand Up @@ -587,8 +579,9 @@ unsigned int projection::match_keyframes_mutually(const std::shared_ptr<data::ke

// 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_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<int>(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;
Expand All @@ -601,13 +594,6 @@ unsigned int projection::match_keyframes_mutually(const std::shared_ptr<data::ke
int best_idx_1 = -1;

for (const auto idx_1 : indices) {
const auto scale_level = static_cast<unsigned int>(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);
Expand Down
2 changes: 1 addition & 1 deletion src/stella_vslam/match/projection.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ class projection final : public base {
const std::vector<std::shared_ptr<data::landmark>>& local_landmarks,
eigen_alloc_unord_map<unsigned int, Vec2_t>& lm_to_reproj,
std::unordered_map<unsigned int, float>& lm_to_x_right,
std::unordered_map<unsigned int, int>& lm_to_scale,
std::unordered_map<unsigned int, unsigned int>& lm_to_scale,
const float margin = 5.0) const;

//! last frameで観測している3次元点をcurrent frameに再投影し，frame.landmarks_に対応情報を記録する
Expand Down
2 changes: 1 addition & 1 deletion src/stella_vslam/module/relocalizer.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -332,7 +332,7 @@ bool relocalizer::refine_pose_by_local_map(data::frame& curr_frm,
unsigned int pred_scale_level;
eigen_alloc_unord_map<unsigned int, Vec2_t> lm_to_reproj;
std::unordered_map<unsigned int, float> lm_to_x_right;
std::unordered_map<unsigned int, int> lm_to_scale;
std::unordered_map<unsigned int, unsigned int> lm_to_scale;
for (const auto& lm : local_landmarks) {
if (curr_landmark_ids.count(lm->id_)) {
continue;
Expand Down
2 changes: 1 addition & 1 deletion src/stella_vslam/tracking_module.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -543,7 +543,7 @@ bool tracking_module::search_local_landmarks() {
unsigned int pred_scale_level;
eigen_alloc_unord_map<unsigned int, Vec2_t> lm_to_reproj;
std::unordered_map<unsigned int, float> lm_to_x_right;
std::unordered_map<unsigned int, int> lm_to_scale;
std::unordered_map<unsigned int, unsigned int> lm_to_scale;
for (const auto& lm : local_landmarks_) {
if (curr_landmark_ids.count(lm->id_)) {
continue;
Expand Down

0 comments on commit 6a46619

Please sign in to comment.