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

Jump to bottom

Fix check_epipolar_constraint #565

Merged
merged 1 commit into from
Feb 24, 2024
Merged

Fix check_epipolar_constraint #565

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
5 changes: 3 additions & 2 deletions src/stella_vslam/mapping_module.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,8 @@ mapping_module::mapping_module(const YAML::Node& yaml_node, data::map_database*
num_covisibilities_for_landmark_generation_(yaml_node["num_covisibilities_for_landmark_generation"].as<unsigned int>(10)),
num_covisibilities_for_landmark_fusion_(yaml_node["num_covisibilities_for_landmark_fusion"].as<unsigned int>(10)),
erase_temporal_keyframes_(yaml_node["erase_temporal_keyframes"].as<bool>(false)),
num_temporal_keyframes_(yaml_node["num_temporal_keyframes"].as<unsigned int>(15)) {
num_temporal_keyframes_(yaml_node["num_temporal_keyframes"].as<unsigned int>(15)),
residual_rad_thr_(yaml_node["residual_deg_thr"].as<float>(0.2) * M_PI / 180.0) {
spdlog::debug("CONSTRUCT: mapping_module");

spdlog::debug("load mapping parameters");
Expand Down Expand Up @@ -326,7 +327,7 @@ void mapping_module::create_new_landmarks(std::atomic<bool>& abort_create_new_la

// vector of matches (idx in the current, idx in the neighbor)
std::vector<std::pair<unsigned int, unsigned int>> matches;
robust_matcher.match_for_triangulation(cur_keyfrm_, ngh_keyfrm, E_ngh_to_cur, matches);
robust_matcher.match_for_triangulation(cur_keyfrm_, ngh_keyfrm, E_ngh_to_cur, matches, residual_rad_thr_);

// triangulation
triangulate_with_two_keyframes(cur_keyfrm_, ngh_keyfrm, matches);
Expand Down
4 changes: 4 additions & 0 deletions src/stella_vslam/mapping_module.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -269,6 +269,10 @@ class mapping_module {

//! Number of temporal keyframes
const unsigned int num_temporal_keyframes_ = 15;

// The default inlier threshold value is 0.2 degree
// (e.g. for the camera with width of 900-pixel and 90-degree FOV, 0.2 degree is equivalent to 2 pixel in the horizontal direction)
float residual_rad_thr_ = 0.2 * M_PI / 180.0;
};

} // namespace stella_vslam
Expand Down
24 changes: 11 additions & 13 deletions src/stella_vslam/match/robust.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,11 @@
namespace stella_vslam {
namespace match {

unsigned int robust::match_for_triangulation(const std::shared_ptr<data::keyframe>& keyfrm_1, const std::shared_ptr<data::keyframe>& keyfrm_2, const Mat33_t& E_12,
std::vector<std::pair<unsigned int, unsigned int>>& matched_idx_pairs) const {
unsigned int robust::match_for_triangulation(const std::shared_ptr<data::keyframe>& keyfrm_1,
const std::shared_ptr<data::keyframe>& keyfrm_2,
const Mat33_t& E_12,
std::vector<std::pair<unsigned int, unsigned int>>& matched_idx_pairs,
const float residual_rad_thr) const {
unsigned int num_matches = 0;

// Project the center of keyframe 1 to keyframe 2
Expand Down Expand Up @@ -112,7 +115,8 @@ unsigned int robust::match_for_triangulation(const std::shared_ptr<data::keyfram

// Check consistency in Matrix E
const bool is_inlier = check_epipolar_constraint(bearing_1, bearing_2, E_12,
keyfrm_1->orb_params_->scale_factors_.at(keypt_1.octave));
keyfrm_1->orb_params_->scale_factors_.at(keypt_1.octave),
residual_rad_thr);
if (is_inlier) {
if (hamm_dist < best_hamm_dist) {
second_best_hamm_dist = best_hamm_dist;
Expand Down Expand Up @@ -348,22 +352,16 @@ unsigned int robust::brute_force_match(const data::frame_observation& frm_obs,
}

bool robust::check_epipolar_constraint(const Vec3_t& bearing_1, const Vec3_t& bearing_2,
const Mat33_t& E_12, const float bearing_1_scale_factor) const {
const Mat33_t& E_12, float residual_rad_thr,
const float bearing_1_scale_factor) const {
// Normal vector of the epipolar plane on keyframe 1
const Vec3_t epiplane_in_1 = E_12 * bearing_2;

// Acquire the angle formed by the normal vector and the bearing
const auto cos_residual = epiplane_in_1.dot(bearing_1) / epiplane_in_1.norm();
const auto residual_rad = M_PI / 2.0 - std::abs(std::acos(cos_residual));

// The Inlier threshold value is 0.2 degree
// (e.g. for the camera with width of 900-pixel and 90-degree FOV, 0.2 degree is equivalent to 2 pixel in the horizontal direction)
// TODO: should prameterize the threshold
constexpr double residual_deg_thr = 0.2;
constexpr double residual_rad_thr = residual_deg_thr * M_PI / 180.0;
const auto cos_residual = std::min(1.0, std::max(-1.0, epiplane_in_1.dot(bearing_1) / epiplane_in_1.norm()));
const auto residual_rad = std::abs(M_PI / 2.0 - std::acos(cos_residual));

// The larger keypoint scale permits less constraints
// TODO: should consider the threshold weighting
return residual_rad < residual_rad_thr * bearing_1_scale_factor;
}

Expand Down
10 changes: 7 additions & 3 deletions src/stella_vslam/match/robust.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,8 +24,11 @@ class robust final : public base {

~robust() final = default;

unsigned int match_for_triangulation(const std::shared_ptr<data::keyframe>& keyfrm_1, const std::shared_ptr<data::keyframe>& keyfrm_2, const Mat33_t& E_12,
std::vector<std::pair<unsigned int, unsigned int>>& matched_idx_pairs) const;
unsigned int match_for_triangulation(const std::shared_ptr<data::keyframe>& keyfrm_1,
const std::shared_ptr<data::keyframe>& keyfrm_2,
const Mat33_t& E_12,
std::vector<std::pair<unsigned int, unsigned int>>& matched_idx_pairs,
const float residual_rad_thr) const;

unsigned int match_keyframes(const std::shared_ptr<data::keyframe>& keyfrm1, const std::shared_ptr<data::keyframe>& keyfrm2,
std::vector<std::shared_ptr<data::landmark>>& matched_lms_in_frm,
Expand All @@ -39,7 +42,8 @@ class robust final : public base {

private:
bool check_epipolar_constraint(const Vec3_t& bearing_1, const Vec3_t& bearing_2,
const Mat33_t& E_12, const float bearing_1_scale_factor = 1.0) const;
const Mat33_t& E_12, float residual_rad_thr,
const float bearing_1_scale_factor = 1.0) const;
};

} // namespace match
Expand Down
Loading