moved feature selection to matcher

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_;
 

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)
   {