Adding map->camera_link tf

CMakeLists.txt

@@ -122,6 +122,10 @@ find_package(cv_bridge REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(message_filters REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 
 # thread library
 find_package(Threads REQUIRED)

src/CMakeLists.txt

@@ -59,7 +59,7 @@ foreach(EXECUTABLE_TARGET IN LISTS EXECUTABLE_TARGETS)
     target_link_libraries(${EXECUTABLE_TARGET}
                           PRIVATE ${openvslam_LIBS} ${rclcpp_LIBRARIES} ${OpenCV_LIBRARIES})
     ament_target_dependencies(${EXECUTABLE_TARGET}
-                              PUBLIC rclcpp cv_bridge image_transport message_filters rcutils nav_msgs)
+                              PUBLIC rclcpp cv_bridge image_transport message_filters rcutils nav_msgs tf2_ros tf2_geometry_msgs)
     install(TARGETS ${EXECUTABLE_TARGET}
             DESTINATION lib/${PROJECT_NAME})
 

src/openvslam_ros.cc

@@ -11,12 +11,16 @@ namespace openvslam_ros {
 system::system(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path)
     : SLAM_(cfg, vocab_file_path), cfg_(cfg), node_(std::make_shared<rclcpp::Node>("run_slam")), custom_qos_(rmw_qos_profile_default),
       mask_(mask_img_path.empty() ? cv::Mat{} : cv::imread(mask_img_path, cv::IMREAD_GRAYSCALE)),
-      pose_pub_(node_->create_publisher<nav_msgs::msg::Odometry>("~/camera_pose", 1)) {
+      pose_pub_(node_->create_publisher<nav_msgs::msg::Odometry>("~/camera_pose", 1)),
+      map_to_odom_broadcaster_(std::make_shared<tf2_ros::TransformBroadcaster>(node_)),
+      tf_(std::make_unique<tf2_ros::Buffer>(node_->get_clock())),
+      transform_listener_(std::make_shared<tf2_ros::TransformListener>(*tf_)) {
     custom_qos_.depth = 1;
     exec_.add_node(node_);
 }
 
 void system::publish_pose(const Eigen::Matrix4d& cam_pose_wc) {
+    const std::lock_guard<std::mutex> lock(camera_link_mutex);
     // Extract rotation matrix and translation vector from
     Eigen::Matrix3d rot = cam_pose_wc.block<3, 3>(0, 0);
     Eigen::Vector3d trans = cam_pose_wc.block<3, 1>(0, 3);
@@ -32,8 +36,8 @@ void system::publish_pose(const Eigen::Matrix4d& cam_pose_wc) {
     // Create odometry message and update it with current camera pose
     nav_msgs::msg::Odometry pose_msg;
     pose_msg.header.stamp = node_->now();
-    pose_msg.header.frame_id = "map";
-    pose_msg.child_frame_id = "camera_link";
+    pose_msg.header.frame_id = map_frame_;
+    pose_msg.child_frame_id = camera_link_;
     pose_msg.pose.pose.orientation.x = quat.x();
     pose_msg.pose.pose.orientation.y = quat.y();
     pose_msg.pose.pose.orientation.z = quat.z();
@@ -42,6 +46,52 @@ void system::publish_pose(const Eigen::Matrix4d& cam_pose_wc) {
     pose_msg.pose.pose.position.y = trans(1);
     pose_msg.pose.pose.position.z = trans(2);
     pose_pub_->publish(pose_msg);
+
+    // Send map->odom transform. Set publish_tf_ to false with not using TF
+    if (publish_tf_) {
+        tf2::Stamped<tf2::Transform> camera_to_map(tf2::Transform(tf2::Quaternion(quat.x(), quat.y(), quat.z(), quat.w()),
+                                                                  tf2::Vector3(trans(0), trans(1), trans(2)))
+                                                       .inverse(),
+                                                   tf2_ros::fromMsg(node_->now()), camera_link_);
+
+        geometry_msgs::msg::TransformStamped camera_to_map_msg, odom_to_map_msg, map_to_odom_msg;
+        tf2::Stamped<tf2::Transform> odom_to_map_stamped;
+
+        // camera_to_map_msg = tf2::toMsg(camera_to_map); - it breaks the execution
+        camera_to_map_msg.header.stamp = tf2_ros::toMsg(camera_to_map.stamp_);
+        camera_to_map_msg.header.frame_id = camera_to_map.frame_id_;
+        camera_to_map_msg.transform.translation.x = camera_to_map.getOrigin().getX();
+        camera_to_map_msg.transform.translation.y = camera_to_map.getOrigin().getY();
+        camera_to_map_msg.transform.translation.z = camera_to_map.getOrigin().getZ();
+        camera_to_map_msg.transform.rotation = tf2::toMsg(camera_to_map.getRotation());
+
+        try {
+            odom_to_map_msg = tf_->transform(camera_to_map_msg, odom_frame_);
+            tf2::fromMsg(odom_to_map_msg, odom_to_map_stamped);
+
+            map_to_odom_msg.transform = tf2::toMsg(tf2::Transform(tf2::Quaternion(odom_to_map_stamped.getRotation()),
+                                                                  tf2::Vector3(odom_to_map_stamped.getOrigin()))
+                                                       .inverse());
+            map_to_odom_msg.header.frame_id = map_frame_;
+            map_to_odom_msg.child_frame_id = odom_frame_;
+            map_to_odom_msg.header.stamp = node_->now();
+            map_to_odom_broadcaster_->sendTransform(map_to_odom_msg);
+        }
+        catch (tf2::TransformException& ex) {
+            RCLCPP_ERROR(node_->get_logger(), "Transform failed: %s", ex.what());
+        }
+    }
+}
+
+void system::setParams() {
+    odom_frame_ = std::string("odom");
+    odom_frame_ = node_->declare_parameter("odom_frame", odom_frame_);
+
+    map_frame_ = std::string("map");
+    map_frame_ = node_->declare_parameter("map_frame", map_frame_);
+
+    publish_tf_ = true;
+    publish_tf_ = node_->declare_parameter("publish_tf_", publish_tf_);
 }
 
 mono::mono(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path)
@@ -50,6 +100,10 @@ mono::mono(const std::shared_ptr<openvslam::config>& cfg, const std::string& voc
         node_.get(), "camera/image_raw", [this](const sensor_msgs::msg::Image::ConstSharedPtr& msg) { callback(msg); }, "raw", custom_qos_);
 }
 void mono::callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) {
+    if (camera_link_.empty()) {
+        const std::lock_guard<std::mutex> lock(camera_link_mutex);
+        camera_link_ = msg->header.frame_id;
+    }
     const rclcpp::Time tp_1 = node_->now();
     const double timestamp = tp_1.seconds();
 
@@ -78,6 +132,10 @@ stereo::stereo(const std::shared_ptr<openvslam::config>& cfg, const std::string&
 }
 
 void stereo::callback(const sensor_msgs::msg::Image::ConstSharedPtr& left, const sensor_msgs::msg::Image::ConstSharedPtr& right) {
+    if (camera_link_.empty()) {
+        const std::lock_guard<std::mutex> lock(camera_link_mutex);
+        camera_link_ = left->header.frame_id;
+    }
     auto leftcv = cv_bridge::toCvShare(left)->image;
     auto rightcv = cv_bridge::toCvShare(right)->image;
     if (leftcv.empty() || rightcv.empty()) {
@@ -114,6 +172,10 @@ rgbd::rgbd(const std::shared_ptr<openvslam::config>& cfg, const std::string& voc
 }
 
 void rgbd::callback(const sensor_msgs::msg::Image::ConstSharedPtr& color, const sensor_msgs::msg::Image::ConstSharedPtr& depth) {
+    if (camera_link_.empty()) {
+        const std::lock_guard<std::mutex> lock(camera_link_mutex);
+        camera_link_ = color->header.frame_id;
+    }
     auto colorcv = cv_bridge::toCvShare(color)->image;
     auto depthcv = cv_bridge::toCvShare(depth)->image;
     if (colorcv.empty() || depthcv.empty()) {

src/openvslam_ros.h

@@ -6,15 +6,21 @@
 #include <openvslam/util/stereo_rectifier.h>
 
 #include <rclcpp/rclcpp.hpp>
-#include <image_transport/image_transport.hpp>
-#include <image_transport/subscriber_filter.hpp>
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
 #include <message_filters/sync_policies/approximate_time.h>
 #include <message_filters/time_synchronizer.h>
 #include <cv_bridge/cv_bridge.h>
 #include <nav_msgs/msg/odometry.hpp>
 
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+#include <geometry_msgs/msg/transform_stamped.h>
+
 #include <opencv2/core/core.hpp>
 #include <sensor_msgs/msg/image.hpp>
 
@@ -23,6 +29,7 @@ class system {
 public:
     system(const std::shared_ptr<openvslam::config>& cfg, const std::string& vocab_file_path, const std::string& mask_img_path);
     void publish_pose(const Eigen::Matrix4d& cam_pose_wc);
+    void setParams();
     openvslam::system SLAM_;
     std::shared_ptr<openvslam::config> cfg_;
     std::shared_ptr<rclcpp::Node> node_;
@@ -31,6 +38,12 @@ class system {
     cv::Mat mask_;
     std::vector<double> track_times_;
     std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> pose_pub_;
+    std::shared_ptr<tf2_ros::TransformBroadcaster> map_to_odom_broadcaster_;
+    std::string odom_frame_, map_frame_, camera_link_;
+    std::unique_ptr<tf2_ros::Buffer> tf_;
+    std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
+    bool publish_tf_;
+    std::mutex camera_link_mutex;
 };
 
 class mono : public system {

src/run_localization.cc

@@ -91,6 +91,7 @@ void localization(const std::shared_ptr<openvslam::config>& cfg, const std::stri
 #endif
 
     rclcpp::Rate rate(50);
+    ros->setParams();
     while (rclcpp::ok()) {
         ros->exec_.spin_some();
         rate.sleep();

src/run_slam.cc

@@ -83,6 +83,7 @@ void tracking(const std::shared_ptr<openvslam::config>& cfg, const std::string&
 #endif
 
     rclcpp::Rate rate(50);
+    ros->setParams();
     while (rclcpp::ok()) {
         ros->exec_.spin_some();
         rate.sleep();