Support initial pose on ROS-wrapper side

src/openvslam_ros.cc

@@ -20,6 +20,10 @@ system::system(const std::shared_ptr<openvslam::config>& cfg, const std::string&
       transform_listener_(std::make_shared<tf2_ros::TransformListener>(*tf_)) {
     custom_qos_.depth = 1;
     exec_.add_node(node_);
+    init_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+        "/initialpose", 1,
+        std::bind(&system::init_pose_callback,
+                  this, std::placeholders::_1));
 }
 
 void system::publish_pose(const Eigen::Matrix4d& cam_pose_wc, const rclcpp::Time& stamp) {
@@ -71,6 +75,9 @@ void system::setParams() {
     map_frame_ = std::string("map");
     map_frame_ = node_->declare_parameter("map_frame", map_frame_);
 
+    base_link_ = std::string("base_footprint");
+    base_link_ = node_->declare_parameter("base_link", base_link_);
+
     // Set publish_tf to false if not using TF
     publish_tf_ = true;
     publish_tf_ = node_->declare_parameter("publish_tf", publish_tf_);
@@ -80,6 +87,71 @@ void system::setParams() {
     transform_tolerance_ = node_->declare_parameter("transform_tolerance", transform_tolerance_);
 }
 
+void system::init_pose_callback(
+    const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
+    if (camera_link_.empty()) {
+        RCLCPP_ERROR(node_->get_logger(),
+                     "Camera link is not set: no images were received yet");
+        return;
+    }
+
+    Eigen::Translation3d trans(
+        msg->pose.pose.position.x,
+        msg->pose.pose.position.y,
+        msg->pose.pose.position.z);
+    Eigen::Quaterniond rot_q(
+        msg->pose.pose.orientation.w,
+        msg->pose.pose.orientation.x,
+        msg->pose.pose.orientation.y,
+        msg->pose.pose.orientation.z);
+    Eigen::Affine3d initialpose_affine(trans * rot_q);
+
+    Eigen::Matrix3d rot_cv_to_ros_map_frame;
+    rot_cv_to_ros_map_frame << 0, -1, 0,
+        0, 0, -1,
+        1, 0, 0;
+
+    Eigen::Affine3d map_to_initialpose_frame_affine;
+    try {
+        auto map_to_initialpose_frame = tf_->lookupTransform(
+            map_frame_, msg->header.frame_id, tf2_ros::fromMsg(msg->header.stamp),
+            tf2::durationFromSec(0.0));
+        map_to_initialpose_frame_affine = tf2::transformToEigen(
+            map_to_initialpose_frame.transform);
+    }
+    catch (tf2::TransformException& ex) {
+        RCLCPP_ERROR(node_->get_logger(), "Transform failed: %s", ex.what());
+        return;
+    }
+
+    Eigen::Affine3d base_link_to_camera_affine;
+    try {
+        auto base_link_to_camera = tf_->lookupTransform(
+            base_link_, camera_link_, tf2_ros::fromMsg(msg->header.stamp),
+            tf2::durationFromSec(0.0));
+        base_link_to_camera_affine = tf2::transformToEigen(base_link_to_camera.transform);
+    }
+    catch (tf2::TransformException& ex) {
+        RCLCPP_ERROR(node_->get_logger(), "Transform failed: %s", ex.what());
+        return;
+    }
+
+    // Target transform is map_cv -> camera_link and known parameters are following:
+    //   rot_cv_to_ros_map_frame: T(map_cv -> map)
+    //   map_to_initialpose_frame_affine: T(map -> `msg->header.frame_id`)
+    //   initialpose_affine: T(`msg->header.frame_id` -> base_link)
+    //   base_link_to_camera_affine: T(base_link -> camera_link)
+    // The flow of the transformation is as follows:
+    //   map_cv -> map -> `msg->header.frame_id` -> base_link -> camera_link
+    Eigen::Matrix4d cam_pose_cv = (rot_cv_to_ros_map_frame * map_to_initialpose_frame_affine
+                                   * initialpose_affine * base_link_to_camera_affine)
+                                      .matrix();
+
+    if (!SLAM_.update_pose(cam_pose_cv)) {
+        RCLCPP_ERROR(node_->get_logger(), "Can not set initial pose");
+    }
+}
+
 mono::mono(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path)
     : system(cfg, vocab_file_path, mask_img_path) {
     sub_ = image_transport::create_subscription(

src/openvslam_ros.h

@@ -20,6 +20,7 @@
 
 #include <opencv2/core/core.hpp>
 #include <sensor_msgs/msg/image.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 
 namespace openvslam_ros {
 class system {
@@ -35,12 +36,17 @@ class system {
     cv::Mat mask_;
     std::vector<double> track_times_;
     std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> pose_pub_;
+    std::shared_ptr<rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>>
+        init_pose_sub_;
     std::shared_ptr<tf2_ros::TransformBroadcaster> map_to_odom_broadcaster_;
-    std::string odom_frame_, map_frame_, camera_link_;
+    std::string odom_frame_, map_frame_, base_link_, camera_link_;
     std::unique_ptr<tf2_ros::Buffer> tf_;
     std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
     bool publish_tf_;
     double transform_tolerance_;
+
+private:
+    void init_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
 };
 
 class mono : public system {