Add fixed covariance to odometry msg

PRBonn · Feb 29, 2024 · 48005a2 · 48005a2
1 parent 13413b7
commit 48005a2
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 0 deletions.
diff --git a/ros/config/kiss_icp.yaml b/ros/config/kiss_icp.yaml
@@ -18,3 +18,7 @@ kiss_icp_node:
     # config.adaptive_threshold
     initial_threshold: 2.0
     min_motion_th: 0.1
+
+    # Covariance diagonal values
+    position_covariance: 0.1
+    orientation_covariance: 0.1
diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp
@@ -59,6 +59,8 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
     odom_frame_ = declare_parameter<std::string>("odom_frame", odom_frame_);
     publish_odom_tf_ = declare_parameter<bool>("publish_odom_tf", publish_odom_tf_);
     publish_debug_clouds_ = declare_parameter<bool>("publish_debug_clouds", publish_debug_clouds_);
+    position_covariance_ = declare_parameter<double>("position_covariance");
+    orientation_covariance_ = declare_parameter<double>("orientation_covariance");
     config_.max_range = declare_parameter<double>("max_range", config_.max_range);
     config_.min_range = declare_parameter<double>("min_range", config_.min_range);
     config_.deskew = declare_parameter<bool>("deskew", config_.deskew);
@@ -174,6 +176,13 @@ void OdometryServer::PublishOdometry(const Sophus::SE3d &pose,
     odom_msg.header.stamp = stamp;
     odom_msg.header.frame_id = odom_frame_;
     odom_msg.pose.pose = tf2::sophusToPose(pose);
+    odom_msg.pose.covariance.fill(0.0);
+    odom_msg.pose.covariance[0] = position_covariance_;
+    odom_msg.pose.covariance[7] = position_covariance_;
+    odom_msg.pose.covariance[14] = position_covariance_;
+    odom_msg.pose.covariance[21] = orientation_covariance_;
+    odom_msg.pose.covariance[28] = orientation_covariance_;
+    odom_msg.pose.covariance[35] = orientation_covariance_;
     odom_publisher_->publish(std::move(odom_msg));
 }
 

diff --git a/ros/src/OdometryServer.hpp b/ros/src/OdometryServer.hpp
@@ -91,6 +91,10 @@ class OdometryServer : public rclcpp::Node {
     /// Global/map coordinate frame.
     std::string odom_frame_{"odom"};
     std::string base_frame_{};
+
+    /// Covariance diagonal
+    double position_covariance_;
+    double orientation_covariance_;
 };
 
 }  // namespace kiss_icp_ros