Skip to content

Commit

Permalink
Fix check_epipolar_constraint
Browse files Browse the repository at this point in the history
  • Loading branch information
@ymd-stella
ymd-stella committed Feb 24, 2024
1 parent 07b4e82 commit dc24df1
Show file tree
Hide file tree
Showing 4 changed files with 25 additions and 18 deletions.
5 changes: 3 additions & 2 deletions src/stella_vslam/mapping_module.cc
View file
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
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
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
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

0 comments on commit dc24df1

Please sign in to comment.