Transform the internal LiDAR odometry map to lidar frame

I honestly do not know why this is not working, but we can see in the
future. It's just to save so computation in the ROS node

ros/src/OdometryServer.cpp

@@ -194,15 +194,13 @@ void OdometryServer::PublishOdometry(const Sophus::SE3d &kiss_pose,
 
 void OdometryServer::PublishClouds(const std::vector<Eigen::Vector3d> frame,
                                    const std::vector<Eigen::Vector3d> keypoints,
-                                   const std_msgs::msg::Header &lidar_header) {
-    // The internal map representation is in the lidar_odom_frame_
-    std_msgs::msg::Header map_header;
-    map_header.frame_id = lidar_odom_frame_;
-    map_header.stamp = lidar_header.stamp;
-
-    frame_publisher_->publish(std::move(EigenToPointCloud2(frame, lidar_header)));
-    kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, lidar_header)));
-    map_publisher_->publish(std::move(EigenToPointCloud2(kiss_icp_->LocalMap(), map_header)));
+                                   const std_msgs::msg::Header &header) {
+    const auto kiss_map = kiss_icp_->LocalMap();
+    const auto kiss_pose = kiss_icp_->pose().inverse();
+
+    frame_publisher_->publish(std::move(EigenToPointCloud2(frame, header)));
+    kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, header)));
+    map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, kiss_pose, header)));
 }
 }  // namespace kiss_icp_ros
 

ros/src/Utils.hpp

@@ -219,6 +219,16 @@ inline std::unique_ptr<PointCloud2> EigenToPointCloud2(const std::vector<Eigen::
     return msg;
 }
 
+inline std::unique_ptr<PointCloud2> EigenToPointCloud2(const std::vector<Eigen::Vector3d> &points,
+                                                       const Sophus::SE3d &T,
+                                                       const Header &header) {
+    std::vector<Eigen::Vector3d> points_t;
+    points_t.resize(points.size());
+    std::transform(points.cbegin(), points.cend(), points_t.begin(),
+                   [&](const auto &point) { return T * point; });
+    return EigenToPointCloud2(points_t, header);
+}
+
 inline std::unique_ptr<PointCloud2> EigenToPointCloud2(const std::vector<Eigen::Vector3d> &points,
                                                        const std::vector<double> &timestamps,
                                                        const Header &header) {