changed interface of feature detector. now it returns directly a list…

… of features instead of only corners

svo/include/svo/depth_filter.h

@@ -46,7 +46,7 @@ struct Seed
   float z_range;               //!< Max range of the possible depth.
   float sigma2;                //!< Variance of normal distribution.
   Matrix2d patch_cov;          //!< Patch covariance in reference image.
-  Seed(Feature* ftr, float angle, float depth_mean, float depth_min);
+  Seed(Feature* ftr, float depth_mean, float depth_min);
 };
 
 /// Depth filter implements the Bayesian Update proposed in:

svo/include/svo/feature_detection.h

@@ -52,15 +52,19 @@ class AbstractDetector
   virtual ~AbstractDetector() {};
 
   virtual void detect(
+      Frame* frame,
       const ImgPyr& img_pyr,
-      const Features& fts,
       const double detection_threshold,
-      Corners* corners) = 0;
+      Features& fts) = 0;
 
-  /// Notify the detector that a cell already has a feature before detection.
+  /// Flag the grid cell as occupied
   void setGridOccpuancy(const Vector2d& px);
 
+  /// Set grid cells of existing features as occupied
+  void setExistingFeatures(const Features& fts);
+
 protected:
+
   static const int border_ = 8; //!< no feature should be within 8px of border.
   const int cell_size_;
   const int n_pyr_levels_;
@@ -70,14 +74,11 @@ class AbstractDetector
 
   void resetGrid();
 
-  void setExistingFeatures(const Features& fts);
-
   inline int getCellIndex(int x, int y, int level)
   {
     const int scale = (1<<level);
     return (scale*y)/cell_size_*grid_n_cols_ + (scale*x)/cell_size_;
   }
-
 };
 typedef boost::shared_ptr<AbstractDetector> DetectorPtr;
 
@@ -94,10 +95,10 @@ class FastDetector : public AbstractDetector
   virtual ~FastDetector() {}
 
   virtual void detect(
+      Frame* frame,
       const ImgPyr& img_pyr,
-      const Features& fts,
       const double detection_threshold,
-      Corners* corners);
+      Features& fts);
 };
 
 } // namespace feature_detection

svo/include/svo/frame.h

@@ -79,7 +79,7 @@ class Frame : boost::noncopyable
   void removeKeyPoint(Feature* ftr);
 
   /// Return number of point observations.
-  size_t nObs() const;
+  inline size_t nObs() const { return fts_.size(); }
 
   /// Check if a point in (w)orld coordinate frame is visible in the image.
   bool isVisible(const Vector3d& xyz_w) const;

svo/include/svo/point.h

@@ -56,7 +56,8 @@ class Point : boost::noncopyable
   int                         n_succeeded_reproj_;      //!< Number of succeeded reprojections. Used to assess the quality of the point.
   int                         last_structure_optim_;    //!< Timestamp of last point optimization
 
-  Point(Vector3d pos);
+  Point(const Vector3d& pos);
+  Point(const Vector3d& pos, Feature* ftr);
   ~Point();
 
   /// Add a reference to a frame.

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

svo/test/test_feature_detection.cpp

@@ -17,8 +17,10 @@
 #include <string.h>
 #include <svo/global.h>
 #include <svo/config.h>
+#include <svo/frame.h>
 #include <svo/feature_detection.h>
 #include <svo/depth_filter.h>
+#include <svo/feature.h>
 #include <vikit/timer.h>
 #include <vikit/vision.h>
 #include <vikit/abstract_camera.h>
@@ -37,38 +39,29 @@ void testCornerDetector()
   cv::Mat img(cv::imread(img_name, 0));
   assert(img.type() == CV_8UC1 && !img.empty());
 
-  svo::ImgPyr img_pyr;
-  svo::frame_utils::createImgPyramid(
-      img, max(svo::Config::nPyrLevels(), svo::Config::kltMaxLevel()+1), img_pyr);
-  svo::Features fts;
+  vk::AbstractCamera* cam = new vk::ATANCamera(752, 480, 0.511496, 0.802603, 0.530199, 0.496011, 0.934092);
+  svo::FramePtr frame(new svo::Frame(cam, img, 0.0));
 
   // Corner detection
   vk::Timer t;
-  svo::feature_detection::Corners corners;
+  svo::Features fts;
   svo::feature_detection::FastDetector fast_detector(
       img.cols, img.rows, svo::Config::gridSize(), svo::Config::nPyrLevels());
   for(int i=0; i<100; ++i)
   {
-    fast_detector.detect(img_pyr, fts, svo::Config::triangMinCornerScore(), &corners);
+    fast_detector.detect(frame.get(), frame->img_pyr_, svo::Config::triangMinCornerScore(), fts);
   }
-  printf("Fast corner detection took %f ms, %zu corners detected (ref i7-W520: 8.06878ms, 416)\n", t.stop()*10, corners.size());
+  printf("Fast corner detection took %f ms, %zu corners detected (ref i7-W520: 8.06878ms, 416)\n", t.stop()*10, fts.size());
+  printf("Note, in this case, feature detection also contains the cam2world projection of the feature.\n");
+  cv::Mat img_rgb = cv::Mat(img.size(), CV_8UC3);
+  cv::cvtColor(img, img_rgb, CV_GRAY2RGB);
+  std::for_each(fts.begin(), fts.end(), [&](svo::Feature* i){
+    cv::circle(img_rgb, cv::Point2f(i->px[0], i->px[1]), 4*(i->level+1), cv::Scalar(0,255,0), 1);
+  });
+  cv::imshow("ref_img", img_rgb);
+  cv::waitKey(0);
 
-  if(false)
-  {
-    cv::Mat img_rgb = cv::Mat(img.size(), CV_8UC3);
-    cv::cvtColor(img, img_rgb, CV_GRAY2RGB);
-    for(svo::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 > svo::Config::triangMinCornerScore())
-      {
-        cv::circle(img_rgb, cv::Point2f(it->x, it->y), 4*(it->level+1), cv::Scalar(0,255,0), 1);
-      }
-    }
-    cv::imshow("ref_img", img_rgb);
-    cv::waitKey(0);
-  }
+  std::for_each(fts.begin(), fts.end(), [&](svo::Feature* i){ delete i; });
 }
 
 } // namespace

svo/test/test_pose_optimizer.cpp

@@ -58,25 +58,15 @@ class PoseOptimizerTest {
     vk::blender_utils::loadBlenderDepthmap(dataset_dir + "/depth/frame_000001_0.depth", *cam_, depthmap_);
 
     // detect features
-    feature_detection::Corners corners;
     feature_detection::FastDetector detector(
         cam_->width(), cam_->height(), Config::gridSize(), Config::nPyrLevels());
-    detector.detect(frame_->img_pyr_, frame_->fts_, Config::triangMinCornerScore(), &corners);
+    detector.detect(frame_.get(), frame_->img_pyr_, Config::triangMinCornerScore(), frame_->fts_);
     size_t n_fts = 0;
-    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* ftr(new Feature(frame_.get(), Vector2d(it->x, it->y), it->level));
-        Point* point(new Point(frame_->f2w(ftr->f*depthmap_.at<float>(it->y, it->x))));
-        ftr->point = point;
-        ftr->frame = frame_.get();
-        frame_->addFeature(ftr);
-        ++n_fts;
-      }
-    }
+    std::for_each(frame_->fts_.begin(), frame_->fts_.end(), [&](Feature* i){
+      Point* point(new Point(frame_->f2w(i->f*depthmap_.at<float>(i->px[1], i->px[0])), i));
+      i->point = point;
+      ++n_fts;
+    });
     printf("Added %zu features to frame.\n", n_fts);
   }
 

svo/test/test_sparse_img_align.cpp

@@ -97,20 +97,16 @@ void SparseImgAlignTest::testSequence(
           dataset_dir+"/depth/"+iter->image_name_+"_0.depth", *cam_, depthmap);
 
       // extract features, generate features with 3D points
-      svo::feature_detection::Corners corners;
       feature_detector->detect(
-          frame_ref_->img_pyr_, frame_ref_->fts_, svo::Config::triangMinCornerScore(), &corners);
-      for(svo::feature_detection::Corners::iterator corner=corners.begin(); corner!=corners.end(); ++corner)
-      {
-        svo::Feature* ftr = new svo::Feature(frame_ref_.get(), Eigen::Vector2d(corner->x, corner->y), corner->level);
-        Eigen::Vector3d pt_pos_cur = ftr->f*depthmap.at<float>(corner->y, corner->x);
+          frame_ref_.get(), frame_ref_->img_pyr_, svo::Config::triangMinCornerScore(), frame_ref_->fts_);
+      std::for_each(frame_ref_->fts_.begin(), frame_ref_->fts_.end(), [&](svo::Feature* i) {
+        Eigen::Vector3d pt_pos_cur = i->f*depthmap.at<float>(i->px[1], i->px[0]);
         Eigen::Vector3d pt_pos_w = frame_ref_->T_f_w_.inverse()*pt_pos_cur;
-        svo::Point* pt = new svo::Point(pt_pos_w);
-        ftr->point = pt;
-        frame_ref_->addFeature(ftr);
-      }
+        svo::Point* pt = new svo::Point(pt_pos_w, i);
+        i->point = pt;
+      });
 
-      printf("Added %zu 3d pts to the reference frame.\n", corners.size());
+      printf("Added %zu 3d pts to the reference frame.\n", frame_ref_->nObs());
       T_prev_w = frame_ref_->T_f_w_;
       T_prevgt_w = T_gt_w;
       continue;

svo_ros/src/benchmark_node.cpp

@@ -190,21 +190,14 @@ void BenchmarkNode::runBlenderBenchmark(const std::string& dataset_dir)
       // extract features, generate features with 3D points
       svo::feature_detection::FastDetector detector(
           cam_->width(), cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels());
-      svo::feature_detection::Corners corners;
-      detector.detect(frame_ref->img_pyr_, frame_ref->fts_, svo::Config::triangMinCornerScore(), &corners);
-      for(auto corner=corners.begin(); corner!=corners.end(); ++corner)
-      {
-        if(corner->score < Config::triangMinCornerScore())
-          continue;
-        svo::Feature* ftr = new svo::Feature(frame_ref.get(), Eigen::Vector2d(corner->x, corner->y), corner->level);
-        Eigen::Vector3d pt_pos_cur = ftr->f*depthmap.at<float>(corner->y, corner->x);
+      detector.detect(frame_ref.get(), frame_ref->img_pyr_, svo::Config::triangMinCornerScore(), frame_ref->fts_);
+      std::for_each(frame_ref->fts_.begin(), frame_ref->fts_.end(), [&](Feature* ftr) {
+        Eigen::Vector3d pt_pos_cur = ftr->f*depthmap.at<float>(ftr->px[1], ftr->px[0]);
         Eigen::Vector3d pt_pos_world = frame_ref->T_f_w_.inverse()*pt_pos_cur;
-        svo::Point* point = new svo::Point(pt_pos_world);
+        svo::Point* point = new svo::Point(pt_pos_world, ftr);
         ftr->point = point;
-        ftr->point->addFrameRef(ftr);
-        frame_ref->addFeature(ftr);
-      }
-      SVO_INFO_STREAM("Added "<<corners.size()<<" 3d pts to the reference frame.");
+      });
+      SVO_INFO_STREAM("Added "<<frame_ref->nObs()<<" 3d pts to the reference frame.");
       vo_->setFirstFrame(frame_ref);
       SVO_INFO_STREAM("Set reference frame.");
     }