diff --git a/svo/include/svo/frame_handler_base.h b/svo/include/svo/frame_handler_base.h index bbde3ce..ad8dd56 100644 --- a/svo/include/svo/frame_handler_base.h +++ b/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, diff --git a/svo/include/svo/frame_handler_mono.h b/svo/include/svo/frame_handler_mono.h index 01f7b45..546e501 100644 --- a/svo/include/svo/frame_handler_mono.h +++ b/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(); diff --git a/svo/include/svo/map.h b/svo/include/svo/map.h index d8e5a71..458622b 100644 --- a/svo/include/svo/map.h +++ b/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); diff --git a/svo/src/frame_handler_base.cpp b/svo/src/frame_handler_base.cpp index 41b03ce..fb6d490 100644 --- a/svo/src/frame_handler_base.cpp +++ b/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; } diff --git a/svo/src/frame_handler_mono.cpp b/svo/src/frame_handler_mono.cpp index 419f21f..3fd1a7a 100644 --- a/svo/src/frame_handler_mono.cpp +++ b/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 = "< 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) diff --git a/svo/src/map.cpp b/svo/src/map.cpp index 69839ad..aa63648 100644 --- a/svo/src/map.cpp +++ b/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)