implementation of a relocalizer that tries to relocalize at the last keyframe. an interface is available for more elaborated relocalizers

svo/include/svo/frame_handler_base.h

@@ -46,7 +46,8 @@ public:
     STAGE_PAUSED,
     STAGE_FIRST_FRAME,
     STAGE_SECOND_FRAME,
-    STAGE_DEFAULT_FRAME
+    STAGE_DEFAULT_FRAME,
+    STAGE_RELOCALIZING
   };
   enum TrackingQuality {
     TRACKING_INSUFFICIENT,

svo/include/svo/frame_handler_mono.h

@@ -50,6 +50,13 @@ public:
   /// Access the depth filter.
   DepthFilter* depthFilter() const { return depth_filter_; }
 
+  /// An external place recognition module may know where to relocalize.
+  bool relocalizeFrameAtPose(
+      const int keyframe_id,
+      const SE3& T_kf_f,
+      const cv::Mat& img,
+      const double timestamp);
+
 protected:
   vk::AbstractCamera* cam_;                     //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit).
   Reprojector reprojector_;                     //!< Projects points from other keyframes into the current frame
@@ -72,6 +79,11 @@ protected:
   /// Processes all frames after the first two keyframes.
   virtual UpdateResult processFrame();
 
+  /// Try relocalizing the frame at relative position to provided keyframe.
+  virtual UpdateResult relocalizeFrame(
+      const SE3& T_cur_ref,
+      FramePtr ref_keyframe);
+
   /// Reset the frame handler. Implement in derived class.
   virtual void resetAll();
 

svo/include/svo/map.h

@@ -105,6 +105,8 @@ public:
   /// Return the keyframe which is furthest apart from pos.
   FramePtr getFurthestKeyframe(const Vector3d& pos) const;
 
+  bool getKeyframeById(const int id, FramePtr& frame) const;
+
   /// Transform the whole map with rotation R, translation t and scale s.
   void transform(const Matrix3d& R, const Vector3d& t, const double& s);
 

svo/src/frame_handler_base.cpp

@@ -129,9 +129,17 @@ int FrameHandlerBase::finishFrameProcessingCommon(
   }
 #endif
 
-  // reset if failure or requested
-  if(dropout == RESULT_FAILURE || set_reset_)
+  if(dropout == RESULT_FAILURE &&
+      (stage_ == STAGE_DEFAULT_FRAME || stage_ == STAGE_RELOCALIZING ))
+  {
+    stage_ = STAGE_RELOCALIZING;
+    tracking_quality_ = TRACKING_INSUFFICIENT;
+  }
+  else if (dropout == RESULT_FAILURE)
     resetAll();
+  if(set_reset_)
+    resetAll();
+
   return 0;
 }
 

svo/src/frame_handler_mono.cpp

@@ -77,6 +77,9 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp)
     res = processSecondFrame();
   else if(stage_ == STAGE_FIRST_FRAME)
     res = processFirstFrame();
+  else if(stage_ == STAGE_RELOCALIZING)
+    res = relocalizeFrame(SE3(Matrix3d::Identity(), Vector3d::Zero()),
+                          map_.getClosestKeyframe(last_frame_));
 
   // set last frame
   last_frame_ = new_frame_;
@@ -86,15 +89,6 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp)
   finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs());
 }
 
-void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame)
-{
-  resetAll();
-  last_frame_ = first_frame;
-  last_frame_->setKeyframe();
-  map_.addKeyframe(last_frame_);
-  stage_ = STAGE_DEFAULT_FRAME;
-}
-
 FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
 {
   new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero());
@@ -155,7 +149,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   const size_t repr_n_mps = reprojector_.n_trials_;
   SVO_LOG2(repr_n_mps, repr_n_new_references);
   SVO_DEBUG_STREAM("Reprojection:\t nPoints = "<<repr_n_mps<<"\t \t nMatches = "<<repr_n_new_references);
-  if(repr_n_new_references == 0) {
+  if(repr_n_new_references < 20) {
     SVO_ERROR_STREAM("Not enough matched features.");
     return RESULT_FAILURE;
   }
@@ -171,7 +165,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   SVO_LOG4(sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
   SVO_DEBUG_STREAM("PoseOptimizer:\t ErrInit = "<<sfba_error_init<<"px\t thresh = "<<sfba_thresh);
   SVO_DEBUG_STREAM("PoseOptimizer:\t ErrFin. = "<<sfba_error_final<<"px\t nObsFin. = "<<sfba_n_edges_final);
-  if(sfba_n_edges_final < 10)
+  if(sfba_n_edges_final < 20)
     return RESULT_FAILURE;
 
   // structure optimization
@@ -235,6 +229,48 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   return RESULT_IS_KEYFRAME;
 }
 
+FrameHandlerMono::UpdateResult FrameHandlerMono::relocalizeFrame(
+    const SE3& T_cur_ref,
+    FramePtr ref_keyframe)
+{
+  SVO_WARN_STREAM_THROTTLE(0.5, "Relocalizing frame");
+  SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
+                           30, SparseImgAlign::GaussNewton, false, false);
+  size_t img_align_n_tracked = img_align.run(ref_keyframe, new_frame_);
+  if(img_align_n_tracked > 30)
+  {
+    last_frame_ = ref_keyframe;
+    FrameHandlerMono::UpdateResult res = processFrame();
+    if(res != RESULT_FAILURE)
+    {
+      stage_ = STAGE_DEFAULT_FRAME;
+      SVO_INFO_STREAM("Relocalization successful.");
+    }
+    else
+      new_frame_->T_f_w_ = ref_keyframe->T_f_w_;
+    return res;
+  }
+  return RESULT_FAILURE;
+}
+
+bool FrameHandlerMono::relocalizeFrameAtPose(
+    const int keyframe_id,
+    const SE3& T_f_kf,
+    const cv::Mat& img,
+    const double timestamp)
+{
+  FramePtr ref_keyframe;
+  if(!map_.getKeyframeById(keyframe_id, ref_keyframe))
+    return false;
+  new_frame_.reset(new Frame(cam_, img.clone(), timestamp));
+  UpdateResult res = relocalizeFrame(T_f_kf, ref_keyframe);
+  if(res != RESULT_FAILURE) {
+    last_frame_ = new_frame_;
+    return true;
+  }
+  return false;
+}
+
 void FrameHandlerMono::resetAll()
 {
   resetCommon();
@@ -245,6 +281,15 @@ void FrameHandlerMono::resetAll()
   depth_filter_->reset();
 }
 
+void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame)
+{
+  resetAll();
+  last_frame_ = first_frame;
+  last_frame_->setKeyframe();
+  map_.addKeyframe(last_frame_);
+  stage_ = STAGE_DEFAULT_FRAME;
+}
+
 bool FrameHandlerMono::needNewKf(double scene_depth_mean)
 {
   for(auto it=overlap_kfs_.begin(), ite=overlap_kfs_.end(); it!=ite; ++it)

svo/src/map.cpp

@@ -155,6 +155,18 @@ FramePtr Map::getFurthestKeyframe(const Vector3d& pos) const
   return furthest_kf;
 }
 
+bool Map::getKeyframeById(const int id, FramePtr& frame) const
+{
+  bool found = false;
+  for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
+    if((*it)->id_ == id) {
+      found = true;
+      frame = *it;
+      break;
+    }
+  return found;
+}
+
 void Map::transform(const Matrix3d& R, const Vector3d& t, const double& s)
 {
   for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)