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

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;
   }