changed interface of feature extractor

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());