changed interface of feature extractor

svo/CMakeLists.txt

@@ -1,8 +1,8 @@
 ################################################################################
 # user build settings
 
-SET(DEBUG FALSE)
-SET(TRACE FALSE)
+SET(DEBUG TRUE)
+SET(TRACE TRUE)
 SET(HAVE_G2O FALSE) 
 
 ################################################################################

svo/include/svo/feature_detection.h

@@ -26,7 +26,6 @@ namespace svo {
 namespace feature_detection {
 
 /// Temporary container used for corner detection. Features are initialized from these.
-// TODO: why not use feature struct?
 struct Corner
 {
   int x;        //!< x-coordinate of corner in the image.
@@ -44,14 +43,31 @@ typedef vector<Corner> Corners;
 class AbstractDetector
 {
 public:
+  AbstractDetector(
+      const int img_width,
+      const int img_height,
+      const int cell_size,
+      const int n_pyr_levels);
+
   virtual ~AbstractDetector() {};
+
   virtual void detect(
       const ImgPyr& img_pyr,
       const Features& fts,
-      const int cell_size,
-      const int n_levels,
       const double detection_threshold,
-      Corners* corners) const = 0;
+      Corners* corners) = 0;
+
+  /// Notify the detector that a cell already has a feature before detection.
+  void setGridOccpuancy(const Vector2d& px);
+
+protected:
+  const int cell_size_;
+  const int n_pyr_levels_;
+  const int grid_n_cols_;
+  const int grid_n_rows_;
+  vector<bool> grid_occupancy_;
+  void resetGrid();
+  void setExistingFeatures(const Features& fts);
 };
 typedef boost::shared_ptr<AbstractDetector> DetectorPtr;
 
@@ -59,15 +75,19 @@ typedef boost::shared_ptr<AbstractDetector> DetectorPtr;
 class FastDetector : public AbstractDetector
 {
 public:
-  FastDetector() {}
+  FastDetector(
+      const int img_width,
+      const int img_height,
+      const int cell_size,
+      const int n_pyr_levels);
+
   virtual ~FastDetector() {}
+
   virtual void detect(
-        const ImgPyr& img_pyr,
-        const Features& fts,
-        const int cell_size,
-        const int n_levels,
-        const double detection_threshold,
-        Corners* corners) const;
+      const ImgPyr& img_pyr,
+      const Features& fts,
+      const double detection_threshold,
+      Corners* corners);
 };
 
 } // namespace feature_detection

svo/src/depth_filter.cpp

@@ -114,9 +114,7 @@ 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_, svo::Config::gridSize(),
-                            svo::Config::nPyrLevels(), Config::triangMinCornerScore(),
-                            &corners);
+  feature_detector_->detect(frame->img_pyr_, frame->fts_, Config::triangMinCornerScore(), &corners);
 
   seeds_updating_halt_ = true;
   lock_t lock(seeds_mut_); // by locking the updateSeeds function stops

svo/src/feature_detection.cpp

@@ -22,54 +22,91 @@
 namespace svo {
 namespace feature_detection {
 
+AbstractDetector::AbstractDetector(
+    const int img_width,
+    const int img_height,
+    const int cell_size,
+    const int n_pyr_levels) :
+        cell_size_(cell_size),
+        n_pyr_levels_(n_pyr_levels),
+        grid_n_cols_(ceil(static_cast<double>(img_width)/cell_size_)),
+        grid_n_rows_(ceil(static_cast<double>(img_height)/cell_size_)),
+        grid_occupancy_(grid_n_cols_*grid_n_rows_, false)
+{}
+
+void AbstractDetector::resetGrid()
+{
+  std::fill(grid_occupancy_.begin(), grid_occupancy_.end(), false);
+}
+
+void AbstractDetector::setExistingFeatures(const Features& fts)
+{
+  std::for_each(fts.begin(), fts.end(), [&](Feature* i){
+    grid_occupancy_.at(
+        static_cast<int>(i->px[1]/cell_size_)*grid_n_cols_
+        + static_cast<int>(i->px[0]/cell_size_)) = true;
+  });
+}
+
+void AbstractDetector::setGridOccpuancy(const Vector2d& px)
+{
+  grid_occupancy_.at(
+      static_cast<int>(px[1]/cell_size_)*grid_n_cols_
+    + static_cast<int>(px[0]/cell_size_)) = true;
+}
+
+FastDetector::FastDetector(
+    const int img_width,
+    const int img_height,
+    const int cell_size,
+    const int n_pyr_levels) :
+        AbstractDetector(img_width, img_height, cell_size, n_pyr_levels)
+{}
+
 void FastDetector::detect(
     const ImgPyr& img_pyr,
     const Features& existing_fts,
-    const int cell_size,
-    const int n_levels,
     const double detection_threshold,
-    Corners* corners) const
+    Corners* corners)
 {
-  // first, find which parts of the image already have a matched feature
-  const int grid_n_cols = ceil(static_cast<double>(img_pyr[0].cols)/cell_size);
-  const int grid_n_rows = ceil(static_cast<double>(img_pyr[0].rows)/cell_size);
-  vector<bool> occupancy(grid_n_cols*grid_n_rows, false);
-  for(Features::const_iterator it=existing_fts.begin(); it!=existing_fts.end(); ++it)
-  {
-    if((*it)->point == false)
-      continue;
-    const int k = static_cast<int>((*it)->px[1]/cell_size)*grid_n_cols + static_cast<int>((*it)->px[0]/cell_size);
-    occupancy.at(k) = true;
-  }
+  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));
-  for(int L=0; L<n_levels; ++L)
+  corners->resize(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);
     vector<fast::fast_xy> fast_corners;
 #if __SSE2__
-      fast::fast_corner_detect_10_sse2((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
+      fast::fast_corner_detect_10_sse2(
+          (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
+          img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
 #elif HAVE_FAST_NEON
-      fast::fast_corner_detect_9_neon((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
+      fast::fast_corner_detect_9_neon(
+          (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
+          img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
 #else
-      fast::fast_corner_detect_10((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
+      fast::fast_corner_detect_10(
+          (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
+          img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
 #endif
     vector<int> scores, nm_corners;
     fast::fast_corner_score_10((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, fast_corners, 20, scores);
     fast::fast_nonmax_3x3(fast_corners, scores, nm_corners);
 
-    for(vector<int>::iterator it=nm_corners.begin(); it!=nm_corners.end(); ++it)
+    for(auto it=nm_corners.begin(), ite=nm_corners.end(); it!=ite; ++it)
     {
       fast::fast_xy& xy = fast_corners.at(*it);
-      const int k = static_cast<int>((xy.y*scale)/cell_size)*grid_n_cols + static_cast<int>((xy.x*scale)/cell_size);
-      if(occupancy[k])
+      const int k = static_cast<int>((xy.y*scale)/cell_size_)*grid_n_cols_
+                  + static_cast<int>((xy.x*scale)/cell_size_);
+      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);
     }
   }
+  resetGrid();
 }
 
 } // namespace feature_detection

svo/src/frame_handler_mono.cpp

@@ -37,7 +37,8 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam) :
   depth_filter_(NULL)
 {
   // create depth filter and set callback
-  feature_detection::DetectorPtr feature_detector(new feature_detection::FastDetector());
+  feature_detection::DetectorPtr feature_detector(
+      new feature_detection::FastDetector(cam_->width(), cam_->height(), Config::gridSize(), Config::nPyrLevels()));
   DepthFilter::callback_t depth_filter_cb = boost::bind(&MapPointCandidates::newCandidatePoint, &map_.point_candidates_, _1, _2);
   depth_filter_ = new DepthFilter(feature_detector, depth_filter_cb);
   depth_filter_->startThread();
@@ -183,6 +184,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
     depth_filter_->addFrame(new_frame_);
     return RESULT_NO_KEYFRAME;
   }
+  new_frame_->setKeyframe();
   SVO_DEBUG_STREAM("New keyframe selected.");
 
   // new keyframe selected
@@ -222,7 +224,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   }
 
   // add keyframe to map
-  new_frame_->setKeyframe();
   map_.addKeyframe(new_frame_);
 
   return RESULT_IS_KEYFRAME;

svo/src/initialization.cpp

@@ -110,10 +110,9 @@ void detectFeatures(
     vector<Vector3d>& f_vec)
 {
   feature_detection::Corners corners;
-  feature_detection::FastDetector detector;
-  detector.detect(frame->img_pyr_, frame->fts_, Config::gridSize(),
-                  Config::nPyrLevels(), Config::triangMinCornerScore(),
-                  &corners);
+  feature_detection::FastDetector detector(
+      frame->img().cols, frame->img().rows, Config::gridSize(), Config::nPyrLevels());
+  detector.detect(frame->img_pyr_, frame->fts_, Config::triangMinCornerScore(), &corners);
 
   // now for all maximum corners, initialize a new seed
   px_vec.clear(); px_vec.reserve(corners.size());

svo/test/test_depth_filter.cpp

@@ -90,6 +90,7 @@ void DepthFilterTest::depthFilterCb(
 
 void DepthFilterTest::testReconstruction(
     std::string dataset_dir,
+
     std::string experiment_name)
 {
   vk::FileReader<vk::blender_utils::file_format::ImageNameAndPose> sequence_file_reader(dataset_dir+"/trajectory.txt");
@@ -103,7 +104,9 @@ void DepthFilterTest::testReconstruction(
   vk::Timer t;
   std::list<size_t> n_converged_per_iteration;
 
-  svo::feature_detection::DetectorPtr feature_detector(new svo::feature_detection::FastDetector());
+  svo::feature_detection::DetectorPtr feature_detector(
+      new svo::feature_detection::FastDetector(
+          cam_->width(), cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels()));
   svo::DepthFilter::callback_t depth_filter_cb = boost::bind(&DepthFilterTest::depthFilterCb, this, _1, _2);
   depth_filter_ = new svo::DepthFilter(feature_detector, depth_filter_cb);
 

svo/test/test_feature_detection.cpp

@@ -45,11 +45,11 @@ void testCornerDetector()
   // Corner detection
   vk::Timer t;
   svo::feature_detection::Corners corners;
-  svo::feature_detection::FastDetector fast_detector;
+  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::gridSize(), svo::Config::nPyrLevels(),
-                         svo::Config::triangMinCornerScore(), &corners);
+    fast_detector.detect(img_pyr, fts, svo::Config::triangMinCornerScore(), &corners);
   }
   printf("Fast corner detection took %f ms, %zu corners detected (ref i7-W520: 8.06878ms, 416)\n", t.stop()*10, corners.size());
 

svo/test/test_pose_optimizer.cpp

@@ -59,9 +59,9 @@ class PoseOptimizerTest {
 
     // detect features
     feature_detection::Corners corners;
-    feature_detection::FastDetector detector;
-    detector.detect(frame_->img_pyr_, frame_->fts_, Config::gridSize(),
-                    Config::nPyrLevels(), Config::triangMinCornerScore(), &corners);
+    feature_detection::FastDetector detector(
+        cam_->width(), cam_->height(), Config::gridSize(), Config::nPyrLevels());
+    detector.detect(frame_->img_pyr_, frame_->fts_, Config::triangMinCornerScore(), &corners);
     size_t n_fts = 0;
     for(feature_detection::Corners::iterator it=corners.begin(); it!=corners.end(); ++it)
     {

svo/test/test_sparse_img_align.cpp

@@ -99,8 +99,7 @@ void SparseImgAlignTest::testSequence(
       // extract features, generate features with 3D points
       svo::feature_detection::Corners corners;
       feature_detector->detect(
-          frame_ref_->img_pyr_, frame_ref_->fts_, svo::Config::gridSize(),
-          svo::Config::nPyrLevels(), svo::Config::triangMinCornerScore(), &corners);
+          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);
@@ -158,10 +157,11 @@ int main(int argc, char** argv)
 {
   std::string experiment_name("flying_room_1_rig_1_fast_minlev0");
   std::string dataset_dir(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
-  svo::feature_detection::FastDetector detector;
   svo::Config::triangMinCornerScore() = 20;
   svo::Config::kltMinLevel() = 0;
   SparseImgAlignTest test;
+  svo::feature_detection::FastDetector detector(
+      test.cam_->width(), test.cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels());
   test.testSequence(dataset_dir, experiment_name, &detector);
   return 0;
 }

svo_analysis/experiments/synthetic.yaml

@@ -1,7 +1,10 @@
 experiment_label: 'svo fast'
 param_settings: vo_fast
 datasets:
+ - 1404102247_fr1_straight
+ - 1404102220_fr1_straight
+ - 1404102129_fr1_straight
  - 1404091756_fr1_straight
  - 1404091807_fr1_straight_rot
  - 1404091819_fr1_sine_2d
- - 1404091831_fr1_sine_3d
+ - 1404091831_fr1_sine_3d

svo_ros/src/benchmark_node.cpp

@@ -188,10 +188,10 @@ void BenchmarkNode::runBlenderBenchmark(const std::string& dataset_dir)
           dataset_dir+"/depth/"+it->image_name_+"_0.depth", *cam_, depthmap);
 
       // extract features, generate features with 3D points
-      svo::feature_detection::FastDetector detector;
+      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::gridSize(),
-                      svo::Config::nPyrLevels(), svo::Config::triangMinCornerScore(), &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())