Auto 2257 fix double construction kiss icp cpp (#294)

* First stop. Remove default constructor

Since we can't guarantee how the pipeline is going to be constructed at
runtime is better to force the user to wrap it in a pointer type to
avoid having a default-constructed object that "should" be modified
later on

* Second stop: make the only kiss_icp::pipeline::KissICP consumer use ptr

This way we guarantee that it's constructed once the `config_` has been
properly populated

* Last stop: rename "odometry_" for "kiss_icp_"

* Remove unnecessary class member

* format

* Remove unnncesary duplicated call

* Update utility function to accoomodate for empty timestamps

cpp/kiss_icp/pipeline/KissICP.cpp

@@ -37,7 +37,7 @@ namespace kiss_icp::pipeline {
 KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vector3d> &frame,
                                                     const std::vector<double> &timestamps) {
     const auto &deskew_frame = [&]() -> std::vector<Eigen::Vector3d> {
-        if (!config_.deskew) return frame;
+        if (!config_.deskew || timestamps.empty()) return frame;
         // TODO(Nacho) Add some asserts here to sanitize the timestamps
 
         //  If not enough poses for the estimation, do not de-skew

cpp/kiss_icp/pipeline/KissICP.hpp

@@ -65,8 +65,6 @@ class KissICP {
           local_map_(config.voxel_size, config.max_range, config.max_points_per_voxel),
           adaptive_threshold_(config.initial_threshold, config.min_motion_th, config.max_range) {}
 
-    KissICP() : KissICP(KISSConfig{}) {}
-
 public:
     Vector3dVectorTuple RegisterFrame(const std::vector<Eigen::Vector3d> &frame);
     Vector3dVectorTuple RegisterFrame(const std::vector<Eigen::Vector3d> &frame,

ros/src/OdometryServer.cpp

@@ -54,26 +54,29 @@ using utils::PointCloud2ToEigen;
 
 OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
     : rclcpp::Node("odometry_node", options) {
-    // clang-format off
     base_frame_ = declare_parameter<std::string>("base_frame", base_frame_);
     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>("visualize", publish_debug_clouds_);
-    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);
-    config_.voxel_size = declare_parameter<double>("voxel_size", config_.max_range / 100.0);
-    config_.max_points_per_voxel = declare_parameter<int>("max_points_per_voxel", config_.max_points_per_voxel);
-    config_.initial_threshold = declare_parameter<double>("initial_threshold", config_.initial_threshold);
-    config_.min_motion_th = declare_parameter<double>("min_motion_th", config_.min_motion_th);
-    if (config_.max_range < config_.min_range) {
-        RCLCPP_WARN(get_logger(), "[WARNING] max_range is smaller than min_range, settng min_range to 0.0");
-        config_.min_range = 0.0;
+
+    kiss_icp::pipeline::KISSConfig config;
+    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);
+    config.voxel_size = declare_parameter<double>("voxel_size", config.max_range / 100.0);
+    config.max_points_per_voxel =
+        declare_parameter<int>("max_points_per_voxel", config.max_points_per_voxel);
+    config.initial_threshold =
+        declare_parameter<double>("initial_threshold", config.initial_threshold);
+    config.min_motion_th = declare_parameter<double>("min_motion_th", config.min_motion_th);
+    if (config.max_range < config.min_range) {
+        RCLCPP_WARN(get_logger(),
+                    "[WARNING] max_range is smaller than min_range, settng min_range to 0.0");
+        config.min_range = 0.0;
     }
-    // clang-format on
 
     // Construct the main KISS-ICP odometry node
-    odometry_ = kiss_icp::pipeline::KissICP(config_);
+    kiss_icp_ = std::make_unique<kiss_icp::pipeline::KissICP>(config);
 
     // Initialize subscribers
     pointcloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
@@ -121,17 +124,14 @@ Sophus::SE3d OdometryServer::LookupTransform(const std::string &target_frame,
 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 = [&]() -> std::vector<double> {
-        if (!config_.deskew) return {};
-        return GetTimestamps(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] = odometry_.RegisterFrame(points, timestamps);
+    const auto &[frame, keypoints] = kiss_icp_->RegisterFrame(points, timestamps);
 
     // Compute the pose using KISS, ego-centric to the LiDAR
-    const Sophus::SE3d kiss_pose = odometry_.poses().back();
+    const Sophus::SE3d kiss_pose = kiss_icp_->poses().back();
 
     // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame
     const auto pose = [&]() -> Sophus::SE3d {
@@ -186,7 +186,7 @@ void OdometryServer::PublishClouds(const std::vector<Eigen::Vector3d> frame,
     odom_header.frame_id = odom_frame_;
 
     // Publish map
-    const auto kiss_map = odometry_.LocalMap();
+    const auto kiss_map = kiss_icp_->LocalMap();
 
     if (!publish_odom_tf_) {
         // debugging happens in an egocentric world

ros/src/OdometryServer.hpp

@@ -85,8 +85,7 @@ class OdometryServer : public rclcpp::Node {
     rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr traj_publisher_;
 
     /// KISS-ICP
-    kiss_icp::pipeline::KissICP odometry_;
-    kiss_icp::pipeline::KISSConfig config_;
+    std::unique_ptr<kiss_icp::pipeline::KissICP> kiss_icp_;
 
     /// Global/map coordinate frame.
     std::string odom_frame_{"odom"};

ros/src/Utils.hpp

@@ -26,6 +26,7 @@
 #include <algorithm>
 #include <cstddef>
 #include <memory>
+#include <optional>
 #include <regex>
 #include <sophus/se3.hpp>
 #include <string>
@@ -35,6 +36,8 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/transform.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
+#include <rclcpp/logger.hpp>
+#include <rclcpp/logging.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 
@@ -88,17 +91,18 @@ inline std::string FixFrameId(const std::string &frame_id) {
     return std::regex_replace(frame_id, std::regex("^/"), "");
 }
 
-inline auto GetTimestampField(const PointCloud2::ConstSharedPtr msg) {
+inline std::optional<PointField> GetTimestampField(const PointCloud2::ConstSharedPtr msg) {
     PointField timestamp_field;
     for (const auto &field : msg->fields) {
         if ((field.name == "t" || field.name == "timestamp" || field.name == "time")) {
             timestamp_field = field;
         }
     }
-    if (!timestamp_field.count) {
-        throw std::runtime_error("Field 't', 'timestamp', or 'time'  does not exist");
-    }
-    return timestamp_field;
+    if (timestamp_field.count) return timestamp_field;
+    RCLCPP_WARN_ONCE(rclcpp::get_logger("odometry_node"),
+                     "Field 't', 'timestamp', or 'time'  does not exist. "
+                     "Disabling scan deskewing");
+    return {};
 }
 
 // Normalize timestamps from 0.0 to 1.0
@@ -116,7 +120,7 @@ inline auto NormalizeTimestamps(const std::vector<double> &timestamps) {
 }
 
 inline auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg,
-                                     const PointField &field) {
+                                     const PointField &timestamp_field) {
     auto extract_timestamps =
         [&msg]<typename T>(sensor_msgs::PointCloud2ConstIterator<T> &&it) -> std::vector<double> {
         const size_t n_points = msg->height * msg->width;
@@ -128,9 +132,6 @@ inline auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg,
         return NormalizeTimestamps(timestamps);
     };
 
-    // Get timestamp field that must be one of the following : {t, timestamp, time}
-    auto timestamp_field = GetTimestampField(msg);
-
     // According to the type of the timestamp == type, return a PointCloud2ConstIterator<type>
     using sensor_msgs::PointCloud2ConstIterator;
     if (timestamp_field.datatype == PointField::UINT32) {
@@ -191,9 +192,10 @@ inline void FillPointCloud2Timestamp(const std::vector<double> &timestamps, Poin
 
 inline std::vector<double> GetTimestamps(const PointCloud2::ConstSharedPtr msg) {
     auto timestamp_field = GetTimestampField(msg);
+    if (!timestamp_field.has_value()) return {};
 
     // Extract timestamps from cloud_msg
-    std::vector<double> timestamps = ExtractTimestampsFromMsg(msg, timestamp_field);
+    std::vector<double> timestamps = ExtractTimestampsFromMsg(msg, timestamp_field.value());
 
     return timestamps;
 }