changed interface of feature detector. now it returns directly a list of features instead of only corners

svo/src/depth_filter.cpp

@@ -34,7 +34,7 @@ namespace svo {
 int Seed::batch_counter = 0;
 int Seed::seed_counter = 0;
 
-Seed::Seed(Feature* ftr, float angle, float depth_mean, float depth_min) :
+Seed::Seed(Feature* ftr, float depth_mean, float depth_min) :
     batch_id(batch_counter),
     id(seed_counter++),
     ftr(ftr),
@@ -113,29 +113,22 @@ void DepthFilter::addKeyframe(FramePtr frame, double depth_mean, double depth_mi
 
 void DepthFilter::initializeSeeds(FramePtr frame)
 {
-  feature_detection::Corners corners;
-  feature_detector_->detect(frame->img_pyr_, frame->fts_, Config::triangMinCornerScore(), &corners);
+  Features new_features;
+  feature_detector_->setExistingFeatures(frame->fts_);
+  feature_detector_->detect(frame.get(), frame->img_pyr_,
+                            Config::triangMinCornerScore(), new_features);
 
   seeds_updating_halt_ = true;
   lock_t lock(seeds_mut_); // by locking the updateSeeds function stops
-  size_t new_seeds = 0;
   ++Seed::batch_counter;
 
-  // now for all maximum corners, initialize a new seed
-  for(feature_detection::Corners::iterator it=corners.begin(); it!=corners.end(); ++it)
-  {
-    // we must check again if the score is above the threshold because the Corners
-    // vector is initialized with dummy corners
-    if(it->score > Config::triangMinCornerScore())
-    {
-      Feature* new_feature(new Feature(frame.get(), Vector2d(it->x, it->y), it->level));
-      seeds_.push_back(Seed(new_feature, it->angle, new_keyframe_mean_depth_, new_keyframe_min_depth_));
-      ++new_seeds;
-    }
-  }
+  // initialize a seed for every new feature
+  std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){
+    seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));
+  });
 
   if(options_.verbose)
-    SVO_INFO_STREAM("DepthFilter: Initialized "<<new_seeds<<" new seeds");
+    SVO_INFO_STREAM("DepthFilter: Initialized "<<new_features.size()<<" new seeds");
 
   seeds_updating_halt_ = false;
 }

svo/src/feature_detection.cpp

@@ -64,15 +64,12 @@ FastDetector::FastDetector(
 {}
 
 void FastDetector::detect(
+    Frame* frame,
     const ImgPyr& img_pyr,
-    const Features& existing_fts,
     const double detection_threshold,
-    Corners* corners)
+    Features& fts)
 {
-  setExistingFeatures(existing_fts);
-
-  // now, detect features and perform nonmax suppression
-  corners->resize(grid_n_cols_*grid_n_rows_, Corner(0,0,detection_threshold,0,0.0f));
+  Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,detection_threshold,0,0.0f));
   for(int L=0; L<n_pyr_levels_; ++L)
   {
     const int scale = (1<<L);
@@ -102,10 +99,17 @@ void FastDetector::detect(
       if(grid_occupancy_[k])
         continue;
       const float score = vk::shiTomasiScore(img_pyr[L], xy.x, xy.y);
-      if(score > corners->at(k).score)
-        corners->at(k) = Corner(xy.x*scale, xy.y*scale, score, L, 0.0f);
+      if(score > corners.at(k).score)
+        corners.at(k) = Corner(xy.x*scale, xy.y*scale, score, L, 0.0f);
     }
   }
+
+  // Create feature for every corner that has high enough corner score
+  std::for_each(corners.begin(), corners.end(), [&](Corner& c) {
+    if(c.score > detection_threshold)
+      fts.push_back(new Feature(frame, Vector2d(c.x, c.y), c.level));
+  });
+
   resetGrid();
 }
 

svo/src/frame.cpp

@@ -137,13 +137,6 @@ void Frame::removeKeyPoint(Feature* ftr)
     setKeyPoints();
 }
 
-size_t Frame::nObs() const
-{
-  size_t n=0;
-  std::for_each(fts_.begin(), fts_.end(), [&](Feature* i){if(i->point != NULL) ++n; });
-  return n;
-}
-
 bool Frame::isVisible(const Vector3d& xyz_w) const
 {
   Vector3d xyz_f = T_f_w_*xyz_w;

svo/src/initialization.cpp

@@ -109,24 +109,19 @@ void detectFeatures(
     vector<cv::Point2f>& px_vec,
     vector<Vector3d>& f_vec)
 {
-  feature_detection::Corners corners;
+  Features new_features;
   feature_detection::FastDetector detector(
       frame->img().cols, frame->img().rows, Config::gridSize(), Config::nPyrLevels());
-  detector.detect(frame->img_pyr_, frame->fts_, Config::triangMinCornerScore(), &corners);
+  detector.detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), new_features);
 
   // now for all maximum corners, initialize a new seed
-  px_vec.clear(); px_vec.reserve(corners.size());
-  f_vec.clear(); f_vec.reserve(corners.size());
-  for(feature_detection::Corners::iterator it=corners.begin(); it!=corners.end(); ++it)
-  {
-    // we must check again if the score is above the threshold because the Corners
-    // vector is initialized with dummy corners
-    if(it->score > Config::triangMinCornerScore())
-    {
-      px_vec.push_back(cv::Point2f(it->x, it->y));
-      f_vec.push_back(frame->c2f(it->x, it->y));
-    }
-  }
+  px_vec.clear(); px_vec.reserve(new_features.size());
+  f_vec.clear(); f_vec.reserve(new_features.size());
+  std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){
+    px_vec.push_back(cv::Point2f(ftr->px[0], ftr->px[1]));
+    f_vec.push_back(ftr->f);
+    delete ftr;
+  });
 }
 
 void trackKlt(

svo/src/point.cpp

@@ -24,7 +24,7 @@ namespace svo {
 
 int Point::point_counter_ = 0;
 
-Point::Point(Vector3d pos) :
+Point::Point(const Vector3d& pos) :
   id_(point_counter_++),
   pos_(pos),
   normal_(Vector3d::Zero()),
@@ -38,6 +38,22 @@ Point::Point(Vector3d pos) :
   last_structure_optim_(0)
 {}
 
+Point::Point(const Vector3d& pos, Feature* ftr) :
+  id_(point_counter_++),
+  pos_(pos),
+  normal_(Vector3d::Zero()),
+  n_obs_(1),
+  v_pt_(NULL),
+  last_published_ts_(0),
+  last_projected_kf_id_(-1),
+  type_(TYPE_UNKNOWN),
+  n_failed_reproj_(0),
+  n_succeeded_reproj_(0),
+  last_structure_optim_(0)
+{
+  obs_.push_front(ftr);
+}
+
 Point::~Point()
 {}