depth filter now also updates old seeds with new keyframes. previously a keyframe was only used to initialize new seeds.

svo/include/svo/depth_filter.h

@@ -62,7 +62,7 @@ public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   typedef boost::unique_lock<boost::mutex> lock_t;
-  typedef boost::function<void ( const Seed&, const Vector3d& )> callback_t;
+  typedef boost::function<void ( Point*, double )> callback_t;
 
   /// Depth-filter config parameters
   struct Options

svo/include/svo/map.h

@@ -48,7 +48,7 @@ public:
   ~MapPointCandidates();
 
   /// Add a candidate point.
-  void newCandidatePoint(const Seed& seed, const Vector3d& xyz_world);
+  void newCandidatePoint(Point* point, double depth_sigma2);
 
   /// Adds the feature to the frame and deletes candidate from list.
   void addCandidatePointToFrame(FramePtr frame);

svo/include/svo/matcher.h

@@ -92,6 +92,7 @@ public:
   double h_inv_;                //!< hessian of 1d image alignment along epipolar line
   int search_level_;
   Feature* ref_ftr_;
+  Vector2d px_cur_;
 
   Matcher();
   ~Matcher();

svo/src/depth_filter.cpp

@@ -178,13 +178,17 @@ void DepthFilter::updateSeedsLoop()
       {
         new_keyframe_set_ = false;
         clearFrameQueue();
-        initializeSeeds(new_keyframe_);
-        continue;
+        frame = new_keyframe_;
+      }
+      else
+      {
+        frame = frame_queue_.front();
+        frame_queue_.pop();
       }
-      frame = frame_queue_.front();
-      frame_queue_.pop();
     }
     updateSeeds(frame);
+    if(frame->isKeyframe())
+      initializeSeeds(frame);
   }
 }
 
@@ -247,13 +251,30 @@ void DepthFilter::updateSeeds(FramePtr frame)
     updateSeed(1./z, tau_inverse*tau_inverse, &*it);
     ++n_updates;
 
+    if(frame->isKeyframe())
+    {
+      // The feature detector should not initialize new seeds close to this location
+      feature_detector_->setGridOccpuancy(matcher_.px_cur_);
+    }
+
     // if the seed has converged, we initialize a new candidate point and remove the seed
     if(sqrt(it->sigma2) < it->z_range/options_.seed_convergence_sigma2_thresh)
     {
-      if(it->ftr->point == NULL)
+      assert(it->ftr->point == NULL); // TODO this should not happen anymore
+      Vector3d xyz_world(it->ftr->frame->T_f_w_.inverse() * (it->ftr->f * (1.0/it->mu)));
+      Point* point = new Point(xyz_world, it->ftr);
+      it->ftr->point = point;
+      if(frame->isKeyframe())
       {
-        Vector3d xyz_world(it->ftr->frame->T_f_w_.inverse() * (it->ftr->f * (1.0/it->mu)));
-        seed_converged_cb_(*it, xyz_world);
+        Feature* ftr = new Feature(frame.get(), matcher_.px_cur_, matcher_.search_level_);
+        ftr->point = point;
+        point->addFrameRef(ftr);
+        frame->addFeature(ftr);
+        it->ftr->frame->addFeature(it->ftr);
+      }
+      else
+      {
+        seed_converged_cb_(point, it->sigma2); // put in candidate list
       }
       it = seeds_.erase(it);
     }

svo/src/map.cpp

@@ -18,7 +18,6 @@
 #include <svo/point.h>
 #include <svo/frame.h>
 #include <svo/feature.h>
-#include <svo/depth_filter.h>
 #include <boost/bind.hpp>
 
 namespace svo {
@@ -193,17 +192,11 @@ MapPointCandidates::~MapPointCandidates()
   reset();
 }
 
-void MapPointCandidates::newCandidatePoint(const Seed& seed, const Vector3d& xyz_world)
+void MapPointCandidates::newCandidatePoint(Point* point, double depth_sigma2)
 {
-  // initalize a new point
-  Point* point = new Point(xyz_world);
-  point->addFrameRef(seed.ftr);
   point->type_ = Point::TYPE_CANDIDATE;
-  seed.ftr->point = point;
-
-  // add candidate to list
   boost::unique_lock<boost::mutex> lock(mut_);
-  candidates_.push_back(PointCandidate(point, seed.ftr));
+  candidates_.push_back(PointCandidate(point, point->obs_.front()));
 }
 
 void MapPointCandidates::addCandidatePointToFrame(FramePtr frame)

svo/src/matcher.cpp

@@ -213,8 +213,8 @@ bool Matcher::findEpipolarMatchDirect(
 
   if(epi_length_ < 2.0)
   {
-    Vector2d px = (px_A+px_B)/2.0;
-    Vector2d px_scaled(px/(1<<search_level_));
+    px_cur_ = (px_A+px_B)/2.0;
+    Vector2d px_scaled(px_cur_/(1<<search_level_));
     bool res;
     if(options_.align_1d)
       res = feature_alignment::align1D(
@@ -226,8 +226,8 @@ bool Matcher::findEpipolarMatchDirect(
           options_.align_max_iter, px_scaled);
     if(res)
     {
-      px = px_scaled*(1<<search_level_);
-      if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px), depth))
+      px_cur_ = px_scaled*(1<<search_level_);
+      if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth))
         return true;
     }
     return false;
@@ -282,8 +282,8 @@ bool Matcher::findEpipolarMatchDirect(
   {
     if(options_.subpix_refinement)
     {
-      Vector2d px(cur_frame.cam_->world2cam(uv_best));
-      Vector2d px_scaled(px/(1<<search_level_));
+      px_cur_ = cur_frame.cam_->world2cam(uv_best);
+      Vector2d px_scaled(px_cur_/(1<<search_level_));
       bool res;
       if(options_.align_1d)
         res = feature_alignment::align1D(
@@ -295,13 +295,13 @@ bool Matcher::findEpipolarMatchDirect(
             options_.align_max_iter, px_scaled);
       if(res)
       {
-        px = px_scaled*(1<<search_level_);
-        if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px), depth))
+        px_cur_ = px_scaled*(1<<search_level_);
+        if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth))
           return true;
       }
       return false;
     }
-
+    px_cur_ = cur_frame.cam_->world2cam(uv_best);
     if(depthFromTriangulation(T_cur_ref, ref_ftr.f, vk::unproject2d(uv_best).normalized(), depth))
       return true;
   }

svo/test/test_depth_filter.cpp

@@ -49,7 +49,7 @@ class DepthFilterTest {
  public:
   DepthFilterTest();
   virtual ~DepthFilterTest();
-  void depthFilterCb(const svo::Seed& seed, const Eigen::Vector3d& xyz_world);
+  void depthFilterCb(svo::Point* point, double depth_sigma2);
   void testReconstruction(std::string dataset_dir, std::string experiment_name);
 
   std::list<ConvergedSeed> results_;
@@ -76,15 +76,16 @@ DepthFilterTest::~DepthFilterTest()
   delete cam_;
 }
 
-void DepthFilterTest::depthFilterCb(
-    const svo::Seed& seed,
-    const Eigen::Vector3d& xyz_world)
+void DepthFilterTest::depthFilterCb(svo::Point* point, double depth_sigma2)
 {
-  double depth = (frame_ref_->pos() - xyz_world).norm();
-  double error = fabs(depth - depth_ref_.at<float>(seed.ftr->px[1], seed.ftr->px[0]));
-  results_.push_back(ConvergedSeed(seed.ftr->px[0], seed.ftr->px[1], depth, error));
+  double depth = (frame_ref_->pos() - point->pos_).norm();
+  double error = fabs(depth - depth_ref_.at<float>(point->obs_.front()->px[1],
+                                                   point->obs_.front()->px[0]));
+  results_.push_back(ConvergedSeed(
+      point->obs_.front()->px[0], point->obs_.front()->px[1], depth, error));
   errors_.push_back(error);
-  delete seed.ftr;
+  delete point->obs_.front();
+  delete point;
   ++n_converged_seeds_;
 }