Fix and improve ROS visualization (#285)

* Fix this madness Simplify implementation by debugging in an ego-centric way always. Since the KISS-ICP internal map is on the global coordinates, fetch the last ego-centric pose and apply it to the map. Seen from the cloud_frame_id (which is the sensor frame) everything should always work in terms of visualization, no matter the multi-sensor setup. * Now is safe to disable this by default * Simplify, borrow the header from the input pointcloud msg This actually makes the visualization closer to the Python visualizer * Disable path/odom visualization by default In the case where we are not computing the poses in an egocentric world (base_frame != "") and we are not publishing to the TF tree, then the visualization wouldn't make sense. Therefore, disable it by default * Changed my mind If someone doesn't have that particular frame defined, then the visualization won't work. Leave this default * Move responsability of handling tf frames out of Registration (#288) * Move responsability of handling tf frames out of Registration Since with this new changes PublishOdometry is the only member that requieres to know the user specified target-frame, it is not necesary to handle all this bits. This makes the implementation cleaner, easier to read and reduces the lines of code. Now RegisterFrame is a simple callback as few months ago one can read in seconds. * typo * Easier to read * We need this for LookupTransform * Remove unused variable * Revert "Remove unused variable" This reverts commit 424ee90. * Remove unnecessary check * Remove unnecessary exposed utility function from ROS API With this change this function is not exposed (which was never the intention to) to the header. This way we can also "hide" this into a private unnamed namesapces and benefit from inlining the simple function into the translation unit * Revert "Remove unnecessary exposed utility function from ROS API" This reverts commit 23cd7ef. * Revert "Remove unnecessary check" This reverts commit d1dcb48. * merge conflicts :0 * Remove unnecessary exposed utility function from ROS API (#289) * Move responsability of handling tf frames out of Registration Since with this new changes PublishOdometry is the only member that requieres to know the user specified target-frame, it is not necesary to handle all this bits. This makes the implementation cleaner, easier to read and reduces the lines of code. Now RegisterFrame is a simple callback as few months ago one can read in seconds. * typo * Easier to read * We need this for LookupTransform * Remove unused variable * Revert "Remove unused variable" This reverts commit 424ee90. * Remove unnecessary check * Remove unnecessary exposed utility function from ROS API With this change this function is not exposed (which was never the intention to) to the header. This way we can also "hide" this into a private unnamed namesapces and benefit from inlining the simple function into the translation unit * Revert "Remove unnecessary exposed utility function from ROS API" This reverts commit 23cd7ef. * Remove unnecessary exposed utility function from ROS API With this change this function is not exposed (which was never the intention to) to the header. This way we can also "hide" this into a private unnamed namesapces and benefit from inlining the simple function into the translation unit * Revert "Remove unnecessary check" This reverts commit d1dcb48. --------- Co-authored-by: tizianoGuadagnino <[email protected]> * too many merges * Merge Rviz and Python colors * Just make the default construction more clear --------- Co-authored-by: tizianoGuadagnino <[email protected]>
PRBonn · Mar 25, 2024 · 20797de · 20797de
1 parent 41d76f1
commit 20797de
Show file tree

Hide file tree

Showing 4 changed files with 63 additions and 92 deletions.
diff --git a/python/kiss_icp/tools/visualizer.py b/python/kiss_icp/tools/visualizer.py
@@ -29,7 +29,7 @@
 
 import numpy as np
 
-YELLOW = np.array([1, 0.706, 0])
+CYAN = np.array([0.24, 0.898, 1])
 RED = np.array([128, 0, 0]) / 255.0
 BLACK = np.array([0, 0, 0]) / 255.0
 BLUE = np.array([0.4, 0.5, 0.9])
@@ -198,7 +198,7 @@ def _update_geometries(self, source, keypoints, target, pose):
         # Source hot frame
         if self.render_source:
             self.source.points = self.o3d.utility.Vector3dVector(source)
-            self.source.paint_uniform_color(YELLOW)
+            self.source.paint_uniform_color(CYAN)
             if self.global_view:
                 self.source.transform(pose)
         else:
@@ -207,7 +207,7 @@ def _update_geometries(self, source, keypoints, target, pose):
         # Keypoints
         if self.render_keypoints:
             self.keypoints.points = self.o3d.utility.Vector3dVector(keypoints)
-            self.keypoints.paint_uniform_color(YELLOW)
+            self.keypoints.paint_uniform_color(CYAN)
             if self.global_view:
                 self.keypoints.transform(pose)
         else:

diff --git a/ros/rviz/kiss_icp.rviz b/ros/rviz/kiss_icp.rviz
@@ -23,7 +23,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: frame_deskew
+    SyncSource: ""
 Visualization Manager:
   Class: ""
   Displays:
@@ -38,7 +38,7 @@ Visualization Manager:
           Axis: Z
           Channel Name: intensity
           Class: rviz_default_plugins/PointCloud2
-          Color: 255; 255; 0
+          Color: 61; 229; 255
           Color Transformer: FlatColor
           Decay Time: 0
           Enabled: true
@@ -52,7 +52,7 @@ Visualization Manager:
           Selectable: true
           Size (Pixels): 3
           Size (m): 0.019999999552965164
-          Style: Flat Squares
+          Style: Points
           Topic:
             Depth: 5
             Durability Policy: Volatile
@@ -84,8 +84,8 @@ Visualization Manager:
           Name: kiss_keypoints
           Position Transformer: XYZ
           Selectable: true
-          Size (Pixels): 10
-          Size (m): 1
+          Size (Pixels): 3
+          Size (m): 0.20000000298023224
           Style: Points
           Topic:
             Depth: 5
@@ -100,14 +100,14 @@ Visualization Manager:
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
-            Max Value: 98.31531524658203
-            Min Value: 0.6587954163551331
+            Max Value: 14.369325637817383
+            Min Value: 0.20239250361919403
             Value: true
           Axis: Z
           Channel Name: intensity
           Class: rviz_default_plugins/PointCloud2
-          Color: 255; 255; 255
-          Color Transformer: AxisColor
+          Color: 102; 102; 102
+          Color Transformer: FlatColor
           Decay Time: 0
           Enabled: true
           Invert Rainbow: false
@@ -174,7 +174,7 @@ Visualization Manager:
             Reliability Policy: Best Effort
             Value: /kiss/odometry
           Value: true
-      Enabled: true
+      Enabled: false
       Name: odometry
     - Class: rviz_default_plugins/Axes
       Enabled: true
@@ -185,7 +185,7 @@ Visualization Manager:
       Value: true
   Enabled: true
   Global Options:
-    Background Color: 48; 48; 48
+    Background Color: 0; 0; 0
     Fixed Frame: odom
     Frame Rate: 30
   Name: root
@@ -229,16 +229,16 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 39.488563537597656
+      Distance: 178.319580078125
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 3.482842445373535
-        Y: 4.3972296714782715
-        Z: 10.242613792419434
+        X: -68.01193237304688
+        Y: 258.05126953125
+        Z: -27.22064781188965
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false

diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp
@@ -45,6 +45,26 @@
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <std_msgs/msg/string.hpp>
 
+namespace {
+Sophus::SE3d LookupTransform(const std::string &target_frame,
+                             const std::string &source_frame,
+                             const std::unique_ptr<tf2_ros::Buffer> &tf2_buffer) {
+    std::string err_msg;
+    if (tf2_buffer->canTransform(target_frame, source_frame, tf2::TimePointZero, &err_msg)) {
+        try {
+            auto tf = tf2_buffer->lookupTransform(target_frame, source_frame, tf2::TimePointZero);
+            return tf2::transformToSophus(tf);
+        } catch (tf2::TransformException &ex) {
+            RCLCPP_WARN(rclcpp::get_logger("LookupTransform"), "%s", ex.what());
+        }
+    }
+    RCLCPP_WARN(rclcpp::get_logger("LookupTransform"), "Failed to find tf. Reason=%s",
+                err_msg.c_str());
+    // default construction is the identity
+    return Sophus::SE3d();
+}
+}  // namespace
+
 namespace kiss_icp_ros {
 
 using utils::EigenToPointCloud2;
@@ -101,106 +121,63 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
     RCLCPP_INFO(this->get_logger(), "KISS-ICP ROS 2 odometry node initialized");
 }
 
-Sophus::SE3d OdometryServer::LookupTransform(const std::string &target_frame,
-                                             const std::string &source_frame) const {
-    std::string err_msg;
-    if (tf2_buffer_->_frameExists(source_frame) &&  //
-        tf2_buffer_->_frameExists(target_frame) &&  //
-        tf2_buffer_->canTransform(target_frame, source_frame, tf2::TimePointZero, &err_msg)) {
-        try {
-            auto tf = tf2_buffer_->lookupTransform(target_frame, source_frame, tf2::TimePointZero);
-            return tf2::transformToSophus(tf);
-        } catch (tf2::TransformException &ex) {
-            RCLCPP_WARN(this->get_logger(), "%s", ex.what());
-        }
-    }
-    RCLCPP_WARN(this->get_logger(), "Failed to find tf. Reason=%s", err_msg.c_str());
-    return {};
-}
-
 void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) {
     const auto cloud_frame_id = msg->header.frame_id;
     const auto points = PointCloud2ToEigen(msg);
     const auto timestamps = GetTimestamps(msg);
-    const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id);
 
     // Register frame, main entry point to KISS-ICP pipeline
     const auto &[frame, keypoints] = kiss_icp_->RegisterFrame(points, timestamps);
 
-    // Compute the pose using KISS, ego-centric to the LiDAR
+    // Extract the last KISS-ICP pose, ego-centric to the LiDAR
     const Sophus::SE3d kiss_pose = kiss_icp_->poses().back();
 
+    // Spit the current estimated pose to ROS msgs handling the desired target frame
+    PublishOdometry(kiss_pose, msg->header);
+    // Publishing these clouds is a bit costly, so do it only if we are debugging
+    if (publish_debug_clouds_) {
+        PublishClouds(frame, keypoints, msg->header);
+    }
+}
+
+void OdometryServer::PublishOdometry(const Sophus::SE3d &kiss_pose,
+                                     const std_msgs::msg::Header &header) {
     // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame
+    const auto cloud_frame_id = header.frame_id;
+    const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id);
     const auto pose = [&]() -> Sophus::SE3d {
         if (egocentric_estimation) return kiss_pose;
-        const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id);
+        const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id, tf2_buffer_);
         return cloud2base * kiss_pose * cloud2base.inverse();
     }();
 
-    // Spit the current estimated pose to ROS msgs
-    PublishOdometry(pose, msg->header.stamp, cloud_frame_id);
-    // Publishing this clouds is a bit costly, so do it only if we are debugging
-    if (publish_debug_clouds_) {
-        PublishClouds(frame, keypoints, msg->header.stamp, cloud_frame_id);
-    }
-}
-
-void OdometryServer::PublishOdometry(const Sophus::SE3d &pose,
-                                     const rclcpp::Time &stamp,
-                                     const std::string &cloud_frame_id) {
     // Broadcast the tf ---
     if (publish_odom_tf_) {
         geometry_msgs::msg::TransformStamped transform_msg;
-        transform_msg.header.stamp = stamp;
+        transform_msg.header.stamp = header.stamp;
         transform_msg.header.frame_id = odom_frame_;
-        transform_msg.child_frame_id = base_frame_.empty() ? cloud_frame_id : base_frame_;
+        transform_msg.child_frame_id = egocentric_estimation ? cloud_frame_id : base_frame_;
         transform_msg.transform = tf2::sophusToTransform(pose);
         tf_broadcaster_->sendTransform(transform_msg);
     }
 
     // publish odometry msg
     nav_msgs::msg::Odometry odom_msg;
-    odom_msg.header.stamp = stamp;
+    odom_msg.header.stamp = header.stamp;
     odom_msg.header.frame_id = odom_frame_;
     odom_msg.pose.pose = tf2::sophusToPose(pose);
     odom_publisher_->publish(std::move(odom_msg));
 }
 
 void OdometryServer::PublishClouds(const std::vector<Eigen::Vector3d> frame,
                                    const std::vector<Eigen::Vector3d> keypoints,
-                                   const rclcpp::Time &stamp,
-                                   const std::string &cloud_frame_id) {
-    std_msgs::msg::Header odom_header;
-    odom_header.stamp = stamp;
-    odom_header.frame_id = odom_frame_;
-
-    // Publish map
+                                   const std_msgs::msg::Header &header) {
     const auto kiss_map = kiss_icp_->LocalMap();
+    const auto kiss_pose = kiss_icp_->poses().back().inverse();
 
-    if (!publish_odom_tf_) {
-        // debugging happens in an egocentric world
-        std_msgs::msg::Header cloud_header;
-        cloud_header.stamp = stamp;
-        cloud_header.frame_id = cloud_frame_id;
-
-        frame_publisher_->publish(std::move(EigenToPointCloud2(frame, cloud_header)));
-        kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, cloud_header)));
-        map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, odom_header)));
-
-        return;
-    }
-
-    // If transmitting to tf tree we know where the clouds are exactly
-    const auto cloud2odom = LookupTransform(odom_frame_, cloud_frame_id);
-    frame_publisher_->publish(std::move(EigenToPointCloud2(frame, cloud2odom, odom_header)));
-    kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, cloud2odom, odom_header)));
-
-    if (!base_frame_.empty()) {
-        const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id);
-        map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, cloud2base, odom_header)));
-    } else {
-        map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, odom_header)));
-    }
+    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
 

diff --git a/ros/src/OdometryServer.hpp b/ros/src/OdometryServer.hpp
@@ -33,6 +33,7 @@
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <std_msgs/msg/header.hpp>
 #include <string>
 
 namespace kiss_icp_ros {
@@ -48,19 +49,12 @@ class OdometryServer : public rclcpp::Node {
     void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
 
     /// Stream the estimated pose to ROS
-    void PublishOdometry(const Sophus::SE3d &pose,
-                         const rclcpp::Time &stamp,
-                         const std::string &cloud_frame_id);
+    void PublishOdometry(const Sophus::SE3d &kiss_pose, const std_msgs::msg::Header &header);
 
     /// Stream the debugging point clouds for visualization (if required)
     void PublishClouds(const std::vector<Eigen::Vector3d> frame,
                        const std::vector<Eigen::Vector3d> keypoints,
-                       const rclcpp::Time &stamp,
-                       const std::string &cloud_frame_id);
-
-    /// Utility function to compute transformation using tf tree
-    Sophus::SE3d LookupTransform(const std::string &target_frame,
-                                 const std::string &source_frame) const;
+                       const std_msgs::msg::Header &header);
 
 private:
     /// Tools for broadcasting TFs.