From 780fddc3dc53176f36eed35b1767e8751f8d2b73 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Mon, 18 Mar 2024 10:51:39 +0100 Subject: [PATCH] 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 | 2 +- cpp/kiss_icp/pipeline/KissICP.hpp | 2 -- ros/src/OdometryServer.cpp | 40 +++++++++++++++---------------- ros/src/OdometryServer.hpp | 3 +-- ros/src/Utils.hpp | 22 +++++++++-------- 5 files changed, 34 insertions(+), 35 deletions(-) diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp index e1aaf1d8..283f0b7d 100644 --- a/cpp/kiss_icp/pipeline/KissICP.cpp +++ b/cpp/kiss_icp/pipeline/KissICP.cpp @@ -37,7 +37,7 @@ namespace kiss_icp::pipeline { KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector &frame, const std::vector ×tamps) { const auto &deskew_frame = [&]() -> std::vector { - 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 diff --git a/cpp/kiss_icp/pipeline/KissICP.hpp b/cpp/kiss_icp/pipeline/KissICP.hpp index e43c6093..69b5830e 100644 --- a/cpp/kiss_icp/pipeline/KissICP.hpp +++ b/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 &frame); Vector3dVectorTuple RegisterFrame(const std::vector &frame, diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp index 0ddedb95..cfc68d55 100644 --- a/ros/src/OdometryServer.cpp +++ b/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("base_frame", base_frame_); odom_frame_ = declare_parameter("odom_frame", odom_frame_); publish_odom_tf_ = declare_parameter("publish_odom_tf", publish_odom_tf_); publish_debug_clouds_ = declare_parameter("visualize", publish_debug_clouds_); - config_.max_range = declare_parameter("max_range", config_.max_range); - config_.min_range = declare_parameter("min_range", config_.min_range); - config_.deskew = declare_parameter("deskew", config_.deskew); - config_.voxel_size = declare_parameter("voxel_size", config_.max_range / 100.0); - config_.max_points_per_voxel = declare_parameter("max_points_per_voxel", config_.max_points_per_voxel); - config_.initial_threshold = declare_parameter("initial_threshold", config_.initial_threshold); - config_.min_motion_th = declare_parameter("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("max_range", config.max_range); + config.min_range = declare_parameter("min_range", config.min_range); + config.deskew = declare_parameter("deskew", config.deskew); + config.voxel_size = declare_parameter("voxel_size", config.max_range / 100.0); + config.max_points_per_voxel = + declare_parameter("max_points_per_voxel", config.max_points_per_voxel); + config.initial_threshold = + declare_parameter("initial_threshold", config.initial_threshold); + config.min_motion_th = declare_parameter("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(config); // Initialize subscribers pointcloud_sub_ = create_subscription( @@ -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 { - 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 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 diff --git a/ros/src/OdometryServer.hpp b/ros/src/OdometryServer.hpp index 418e0d2c..3be0c189 100644 --- a/ros/src/OdometryServer.hpp +++ b/ros/src/OdometryServer.hpp @@ -85,8 +85,7 @@ class OdometryServer : public rclcpp::Node { rclcpp::Publisher::SharedPtr traj_publisher_; /// KISS-ICP - kiss_icp::pipeline::KissICP odometry_; - kiss_icp::pipeline::KISSConfig config_; + std::unique_ptr kiss_icp_; /// Global/map coordinate frame. std::string odom_frame_{"odom"}; diff --git a/ros/src/Utils.hpp b/ros/src/Utils.hpp index 0ae32975..3364ac68 100644 --- a/ros/src/Utils.hpp +++ b/ros/src/Utils.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -35,6 +36,8 @@ #include #include #include +#include +#include #include #include @@ -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 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 ×tamps) { } inline auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg, - const PointField &field) { + const PointField ×tamp_field) { auto extract_timestamps = [&msg](sensor_msgs::PointCloud2ConstIterator &&it) -> std::vector { 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 using sensor_msgs::PointCloud2ConstIterator; if (timestamp_field.datatype == PointField::UINT32) { @@ -191,9 +192,10 @@ inline void FillPointCloud2Timestamp(const std::vector ×tamps, Poin inline std::vector GetTimestamps(const PointCloud2::ConstSharedPtr msg) { auto timestamp_field = GetTimestampField(msg); + if (!timestamp_field.has_value()) return {}; // Extract timestamps from cloud_msg - std::vector timestamps = ExtractTimestampsFromMsg(msg, timestamp_field); + std::vector timestamps = ExtractTimestampsFromMsg(msg, timestamp_field.value()); return timestamps; }