Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix scale range in matching modules #550

Merged
merged 1 commit into from
Dec 24, 2023
Merged

Fix scale range in matching modules #550

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 7 additions & 8 deletions src/stella_vslam/data/common.cc
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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