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