Add update_pose() API call

src/openvslam/data/map_database.cc

@@ -71,6 +71,32 @@ std::vector<keyframe*> map_database::get_all_keyframes() const {
     return keyframes;
 }
 
+std::vector<keyframe*> map_database::get_close_keyframes(const Mat44_t& pose,
+                                                         const double distance_threshold,
+                                                         const double cos_angle_threshold) const {
+    std::lock_guard<std::mutex> lock(mtx_map_access_);
+
+    // Close (within given thresholds) keyframes
+    std::vector<keyframe*> filtered_keyframes;
+
+    // Calculate angles and distances between given pose and all keyframes
+    Mat33_t M = pose.block<3, 3>(0, 0);
+    Vec3_t Mt = pose.block<3, 1>(0, 3);
+    for (const auto id_keyframe : keyframes_) {
+        Mat33_t N = id_keyframe.second->get_cam_pose().block<3, 3>(0, 0);
+        Vec3_t Nt = id_keyframe.second->get_cam_pose().block<3, 1>(0, 3);
+        // Angle between two cameras related to given pose and selected keyframe
+        const double cos_angle = ((M * N.transpose()).trace() - 1) / 2;
+        // Distance between given pose and selected keyframe
+        const double dist = (Nt - Mt).norm();
+        if (dist < distance_threshold && cos_angle > cos_angle_threshold) {
+            filtered_keyframes.push_back(id_keyframe.second);
+        }
+    }
+
+    return filtered_keyframes;
+}
+
 unsigned int map_database::get_num_keyframes() const {
     std::lock_guard<std::mutex> lock(mtx_map_access_);
     return keyframes_.size();

src/openvslam/data/map_database.h

@@ -78,6 +78,17 @@ class map_database {
      */
     std::vector<keyframe*> get_all_keyframes() const;
 
+    /**
+     * Get closest keyframes to a given pose
+     * @param pose Given pose
+     * @param distance_threshold Maximum distance where close keyframes could be found
+     * @param cos_angle_threshold Minimum cosine of angle between given pose and close keyframes
+     * @return Vector closest keyframes
+     */
+    std::vector<keyframe*> get_close_keyframes(const Mat44_t& pose,
+                                               const double distance_threshold,
+                                               const double cos_angle_threshold) const;
+
     /**
      * Get the number of keyframes
      * @return

src/openvslam/module/relocalizer.cc

@@ -32,6 +32,12 @@ bool relocalizer::relocalize(data::frame& curr_frm) {
     if (reloc_candidates.empty()) {
         return false;
     }
+
+    return reloc_by_candidates(curr_frm, reloc_candidates);
+}
+
+bool relocalizer::reloc_by_candidates(data::frame& curr_frm,
+                                      const std::vector<openvslam::data::keyframe*>& reloc_candidates) {
     const auto num_candidates = reloc_candidates.size();
 
     std::vector<std::vector<data::landmark*>> matched_landmarks(num_candidates);

src/openvslam/module/relocalizer.h

@@ -28,6 +28,10 @@ class relocalizer {
     //! Relocalize the specified frame
     bool relocalize(data::frame& curr_frm);
 
+    //! Relocalize the specified frame by given candidates list
+    bool reloc_by_candidates(data::frame& curr_frm,
+                             const std::vector<openvslam::data::keyframe*>& reloc_candidates);
+
 private:
     //! Extract valid (non-deleted) landmarks from landmark vector
     std::vector<unsigned int> extract_valid_indices(const std::vector<data::landmark*>& landmarks) const;

src/openvslam/system.cc

@@ -286,6 +286,14 @@ Mat44_t system::feed_RGBD_frame(const cv::Mat& rgb_img, const cv::Mat& depthmap,
     return cam_pose_cw;
 }
 
+void system::update_pose(const Mat44_t& pose) {
+    tracker_->request_update_pose(pose);
+    // Even if state will be lost, still update the pose in map_publisher_
+    // to clearly show new camera position
+    map_publisher_->set_current_cam_pose(pose);
+    map_publisher_->set_current_cam_pose_wc(pose.inverse());
+}
+
 void system::pause_tracker() {
     tracker_->request_pause();
 }

src/openvslam/system.h

@@ -113,6 +113,12 @@ class system {
     //! (Note: RGB and Depth images must be aligned)
     Mat44_t feed_RGBD_frame(const cv::Mat& rgb_img, const cv::Mat& depthmap, const double timestamp, const cv::Mat& mask = cv::Mat{});
 
+    //-----------------------------------------
+    // pose initializing/updating
+
+    //! Try to update the pose to a given one
+    void update_pose(const Mat44_t& pose);
+
     //-----------------------------------------
     // management for pause
 

src/openvslam/tracking_module.cc

@@ -53,6 +53,17 @@ double get_depthmap_factor(const camera::base* camera, const YAML::Node& yaml_no
     }
     return depthmap_factor;
 }
+
+double get_pose_reloc_distance_threshold(const YAML::Node& yaml_node) {
+    spdlog::debug("load maximum distance threshold where close keyframes could be found");
+    return yaml_node["reloc_distance_threshold"].as<double>(0.2);
+}
+
+double get_cos_angle_reloc_threshold(const YAML::Node& yaml_node) {
+    spdlog::debug("load minimum cosine of angle between given pose and close keyframes threshold");
+    return yaml_node["angle_reloc_threshold"].as<double>(0.9);
+}
+
 } // unnamed namespace
 
 namespace openvslam {
@@ -61,6 +72,8 @@ tracking_module::tracking_module(const std::shared_ptr<config>& cfg, system* sys
                                  data::bow_vocabulary* bow_vocab, data::bow_database* bow_db)
     : camera_(cfg->camera_), true_depth_thr_(get_true_depth_thr(camera_, cfg->yaml_node_)),
       depthmap_factor_(get_depthmap_factor(camera_, cfg->yaml_node_)),
+      pose_reloc_distance_threshold_(get_pose_reloc_distance_threshold(cfg->yaml_node_)),
+      cos_angle_reloc_threshold_(get_cos_angle_reloc_threshold(cfg->yaml_node_)),
       system_(system), map_db_(map_db), bow_vocab_(bow_vocab), bow_db_(bow_db),
       initializer_(cfg->camera_->setup_type_, map_db, bow_db, util::yaml_optional_ref(cfg->yaml_node_, "Initializer")),
       frame_tracker_(camera_, 10), relocalizer_(bow_db_), pose_optimizer_(),
@@ -179,6 +192,27 @@ Mat44_t tracking_module::track_RGBD_image(const cv::Mat& img, const cv::Mat& dep
     return curr_frm_.cam_pose_cw_;
 }
 
+void tracking_module::request_update_pose(const Mat44_t& pose) {
+    std::lock_guard<std::mutex> lock(mtx_update_pose_);
+    update_pose_is_requested_ = true;
+    requested_pose_ = pose;
+}
+
+bool tracking_module::update_pose_is_requested() {
+    std::lock_guard<std::mutex> lock(mtx_update_pose_);
+    return update_pose_is_requested_;
+}
+
+Mat44_t& tracking_module::get_requested_pose() {
+    std::lock_guard<std::mutex> lock(mtx_update_pose_);
+    return requested_pose_;
+}
+
+void tracking_module::finish_update_pose_request() {
+    std::lock_guard<std::mutex> lock(mtx_update_pose_);
+    update_pose_is_requested_ = false;
+}
+
 void tracking_module::reset() {
     spdlog::info("resetting system");
 
@@ -321,6 +355,28 @@ bool tracking_module::initialize() {
 
 bool tracking_module::track_current_frame() {
     bool succeeded = false;
+
+    if (update_pose_is_requested()) {
+        // Force relocalization by pose
+        curr_frm_.set_cam_pose(get_requested_pose());
+
+        curr_frm_.compute_bow();
+        const auto candidates = map_db_->get_close_keyframes(get_requested_pose(),
+                                                             pose_reloc_distance_threshold_,
+                                                             cos_angle_reloc_threshold_);
+        if (!candidates.empty()) {
+            succeeded = relocalizer_.reloc_by_candidates(curr_frm_, candidates);
+            if (succeeded) {
+                last_reloc_frm_id_ = curr_frm_.id_;
+            }
+        }
+        else {
+            curr_frm_.cam_pose_cw_is_valid_ = false;
+        }
+        finish_update_pose_request();
+        return succeeded;
+    }
+
     if (tracking_state_ == tracker_state_t::Tracking) {
         // Tracking mode
         if (velocity_is_valid_ && last_reloc_frm_id_ + 2 < curr_frm_.id_) {
@@ -342,6 +398,7 @@ bool tracking_module::track_current_frame() {
             last_reloc_frm_id_ = curr_frm_.id_;
         }
     }
+
     return succeeded;
 }
 

src/openvslam/tracking_module.h

@@ -80,6 +80,9 @@ class tracking_module {
     //! (Note: RGB and Depth images must be aligned)
     Mat44_t track_RGBD_image(const cv::Mat& img, const cv::Mat& depthmap, const double timestamp, const cv::Mat& mask = cv::Mat{});
 
+    //! Request to update the pose to a given one
+    void request_update_pose(const Mat44_t& pose);
+
     //-----------------------------------------
     // management for reset process
 
@@ -113,6 +116,10 @@ class tracking_module {
     //! depthmap factor (pixel_value / depthmap_factor = true_depth)
     double depthmap_factor_ = 1.0;
 
+    //! closest keyframes thresholds (by distance and angle) to relocalize with when updating by pose
+    double pose_reloc_distance_threshold_ = 0.2;
+    double cos_angle_reloc_threshold_ = 0.9;
+
     //-----------------------------------------
     // variables
 
@@ -253,6 +260,19 @@ class tracking_module {
 
     //! Pause of the tracking module is requested or not
     bool pause_is_requested_ = false;
+
+    //! Mutex for update pose into a given position
+    mutable std::mutex mtx_update_pose_;
+    //! Update into a given position is requested or not
+    bool update_pose_is_requested();
+    //! Get requested for relocalization pose
+    Mat44_t& get_requested_pose();
+    //! Finish update request. Returns true in case of request was made.
+    void finish_update_pose_request();
+    //! Indicator of update pose request
+    bool update_pose_is_requested_ = false;
+    //! Requested pose to update
+    Mat44_t requested_pose_;
 };
 
 } // namespace openvslam