diff --git a/svo/include/svo/matcher.h b/svo/include/svo/matcher.h index 368e182..d0d1134 100644 --- a/svo/include/svo/matcher.h +++ b/svo/include/svo/matcher.h @@ -76,21 +76,27 @@ public: double max_epi_length_optim;//!< max length of epipolar line to skip epipolar search and directly go to img align size_t max_epi_search_steps;//!< max number of evaluations along epipolar line bool subpix_refinement; //!< do gauss newton feature patch alignment after epipolar search + bool epi_search_edgelet_filtering; + double epi_search_edgelet_max_angle; Options() : align_1d(false), align_max_iter(10), max_epi_length_optim(2.0), max_epi_search_steps(1000), - subpix_refinement(true) + subpix_refinement(true), + epi_search_edgelet_filtering(true), + epi_search_edgelet_max_angle(0.7) {} } options_; uint8_t* patch_; uint8_t* patch_with_border_; Matrix2d A_cur_ref_; //!< affine warp matrix + Vector2d epi_dir_; double epi_length_; //!< length of epipolar line segment in pixels (only used for epipolar search) double h_inv_; //!< hessian of 1d image alignment along epipolar line int search_level_; + bool reject_; Feature* ref_ftr_; Vector2d px_cur_; diff --git a/svo/src/matcher.cpp b/svo/src/matcher.cpp index f3098e7..0ec7714 100644 --- a/svo/src/matcher.cpp +++ b/svo/src/matcher.cpp @@ -206,12 +206,25 @@ bool Matcher::findEpipolarMatchDirect( // Compute start and end of epipolar line in old_kf for match search, on unit plane! Vector2d A = vk::project2d(T_cur_ref * (ref_ftr.f*d_min)); Vector2d B = vk::project2d(T_cur_ref * (ref_ftr.f*d_max)); - Vector2d epi_dir = A - B; + epi_dir_ = A - B; // Compute affine warp matrix warp::getWarpMatrixAffine( *ref_frame.cam_, *cur_frame.cam_, ref_ftr.px, ref_ftr.f, d_estimate, T_cur_ref, ref_ftr.level, A_cur_ref_); + + // feature pre-selection + reject_ = false; + if(ref_ftr.type == Feature::EDGELET && options_.epi_search_edgelet_filtering) + { + const Vector2d grad_cur = (A_cur_ref_ * ref_ftr.grad).normalized(); + const double cosangle = fabs(grad_cur.dot(epi_dir_.normalized())); + if(cosangle < options_.epi_search_edgelet_max_angle) { + reject_ = true; + return false; + } + } + search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1); // Find length of search range on epipolar line @@ -247,7 +260,7 @@ bool Matcher::findEpipolarMatchDirect( } size_t n_steps = epi_length_/0.7; // one step per pixel - Vector2d step = epi_dir/n_steps; + Vector2d step = epi_dir_/n_steps; if(n_steps > options_.max_epi_search_steps) {