feat: add kiss-icp matcher

.gitmodules

@@ -4,3 +4,6 @@
 [submodule "thirdparty/fast_gicp"]
 	path = thirdparty/fast_gicp
 	url = git@github.com:RyuYamamoto/fast_gicp
+[submodule "thirdparty/kiss-icp"]
+	path = thirdparty/kiss-icp
+	url = git@github.com:PRBonn/kiss-icp.git

lidar_graph_slam/launch/lidar_graph_slam.launch.xml

@@ -6,7 +6,8 @@
 
   <include file="$(find-pkg-share points_prefiltering)/launch/points_prefiltering.launch.xml"/>
 
-  <include file="$(find-pkg-share lidar_scan_matcher)/launch/lidar_scan_matcher.launch.xml"/>
+  <!--include file="$(find-pkg-share lidar_scan_matcher)/launch/lidar_scan_matcher.launch.xml"/-->
+  <include file="$(find-pkg-share lidar_scan_matcher)/launch/kiss_icp_matcher.launch.xml"/>
 
   <include file="$(find-pkg-share graph_based_slam)/launch/graph_based_slam.launch.xml"/>
 

lidar_scan_matcher/CMakeLists.txt

@@ -1,19 +1,21 @@
 cmake_minimum_required(VERSION 3.5)
 project(lidar_scan_matcher)
 
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-  set(CMAKE_CXX_STANDARD_REQUIRED ON)
-  set(CMAKE_CXX_EXTENSIONS OFF)
-endif()
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic )
-endif()
+#if(NOT CMAKE_CXX_STANDARD)
+#  set(CMAKE_CXX_STANDARD 17)
+#  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+#  set(CMAKE_CXX_EXTENSIONS OFF)
+#endif()
+#if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+#  add_compile_options(-Wall -Wextra -Wpedantic )
+#endif()
 
 find_package(PCL REQUIRED)
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
+add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/kiss-icp/cpp/kiss_icp ${CMAKE_CURRENT_BINARY_DIR}/kiss_icp)
+
 include_directories(include ${PCL_INCLUDE_DIRS})
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
@@ -27,6 +29,20 @@ rclcpp_components_register_node(${PROJECT_NAME}
 
 target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES})
 
+
+ament_auto_add_library(kiss_icp_matcher SHARED
+  src/kiss_icp_matcher.cpp
+)
+
+rclcpp_components_register_node(kiss_icp_matcher
+  PLUGIN "KissIcpMatcher"
+  EXECUTABLE kiss_icp_matcher_node
+)
+
+target_compile_features(kiss_icp_matcher PUBLIC cxx_std_20)
+target_include_directories(kiss_icp_matcher PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+target_link_libraries(kiss_icp_matcher ${PCL_LIBRARIES} kiss_icp::pipeline)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()

lidar_scan_matcher/config/kiss_icp_matcher.param.yaml

@@ -0,0 +1,19 @@
+/kiss_icp_matcher_node:
+  ros__parameters:
+    base_frame_id: "base_link"
+
+    displacement: 1.0
+
+    max_range: 100.0
+
+    min_range: 1.0
+
+    deskew: false
+
+    voxel_size: 1.0
+
+    max_points_per_voxel: 20
+
+    initial_threshold: 2.0
+
+    min_motion_th: 0.1

lidar_scan_matcher/include/lidar_scan_matcher/kiss_icp_matcher.hpp

@@ -0,0 +1,116 @@
+// Copyright (c) 2023 Ryu Yamamoto
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+// MIT License
+//
+// Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill
+// Stachniss.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#ifndef _LIDAR_SCAN_MATCHER__KISS_ICP_MATCHER_HPP_
+#define _LIDAR_SCAN_MATCHER__KISS_ICP_MATCHER_HPP_
+
+#include "kiss_icp/pipeline/KissICP.hpp"
+#include "lidar_graph_slam_utils/lidar_graph_slam_utils.hpp"
+#include "lidar_scan_matcher/kiss_icp_utils.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <lidar_graph_slam_msgs/msg/key_frame.hpp>
+#include <lidar_graph_slam_msgs/msg/key_frame_array.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/registration/gicp.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <tf2/transform_datatypes.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
+
+using PointType = pcl::PointXYZI;
+
+class KissIcpMatcher : public rclcpp::Node
+{
+public:
+  KissIcpMatcher(const rclcpp::NodeOptions & node_options);
+  ~KissIcpMatcher() = default;
+
+  void callback_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
+
+  geometry_msgs::msg::TransformStamped get_transform(
+    const std::string source_frame, const std::string target_frame);
+  pcl::PointCloud<PointType>::Ptr transform_point_cloud(
+    const pcl::PointCloud<PointType>::Ptr input_cloud_ptr,
+    const geometry_msgs::msg::TransformStamped transform);
+  pcl::PointCloud<PointType>::Ptr transform_point_cloud(
+    const pcl::PointCloud<PointType>::Ptr input_cloud_ptr, const Eigen::Matrix4f transform_matrix);
+
+  void publish_tf(
+    const geometry_msgs::msg::Pose pose, const rclcpp::Time stamp, const std::string frame_id,
+    const std::string child_frame_id);
+  void publish_key_frame(const lidar_graph_slam_msgs::msg::KeyFrame key_frame);
+
+private:
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sensor_points_subscriber_;
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr scan_matcher_odom_publisher_;
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr scan_matcher_path_publisher_;
+  rclcpp::Publisher<lidar_graph_slam_msgs::msg::KeyFrame>::SharedPtr key_frame_publisher_;
+
+  tf2_ros::Buffer tf_buffer_{get_clock()};
+  tf2_ros::TransformListener tf_listener_{tf_buffer_};
+  std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
+
+  nav_msgs::msg::Path odometry_path_;
+
+  kiss_icp::pipeline::KissICP kiss_icp_;
+  kiss_icp::pipeline::KISSConfig config_;
+
+  Eigen::Vector3d previous_translation_{Eigen::Vector3d::Zero()};
+
+  int key_frame_id_{0};
+  double displacement_;
+  double accumulate_distance_{0.0};
+  std::string base_frame_id_;
+  bool is_initialized_{false};
+};
+
+#endif

lidar_scan_matcher/include/lidar_scan_matcher/kiss_icp_utils.hpp

@@ -0,0 +1,98 @@
+#ifndef _LIDAR_SCAN_MATCHER__UTILS_HPP_
+#define _LIDAR_SCAN_MATCHER__UTILS_HPP_
+
+#include <Eigen/Core>
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+
+#include <vector>
+
+#include "kiss_icp/pipeline/KissICP.hpp"
+
+inline std::vector<Eigen::Vector3d> point_cloud2_to_eigen(
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
+{
+  std::vector<Eigen::Vector3d> points;
+  points.reserve(msg->height * msg->width);
+  sensor_msgs::PointCloud2ConstIterator<float> msg_x(*msg, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> msg_y(*msg, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> msg_z(*msg, "z");
+  for (size_t i = 0; i < msg->height * msg->width; ++i, ++msg_x, ++msg_y, ++msg_z) {
+    points.emplace_back(*msg_x, *msg_y, *msg_z);
+  }
+  return points;
+}
+
+inline auto normalize_timestamps(const std::vector<double> & timestamps)
+{
+  const auto [min_it, max_it] = std::minmax_element(timestamps.cbegin(), timestamps.cend());
+  const double min_timestamp = *min_it;
+  const double max_timestamp = *max_it;
+
+  std::vector<double> timestamps_normalized(timestamps.size());
+  std::transform(
+    timestamps.cbegin(), timestamps.cend(), timestamps_normalized.begin(),
+    [&](const auto & timestamp) {
+      return (timestamp - min_timestamp) / (max_timestamp - min_timestamp);
+    });
+  return timestamps_normalized;
+}
+
+inline auto get_timestamp_field(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
+{
+  sensor_msgs::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;
+}
+
+inline auto extract_timestamps_from_msg(
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg,
+  const sensor_msgs::msg::PointField & field)
+{
+  auto extract_timestamps =
+    [&msg]<typename T>(sensor_msgs::PointCloud2ConstIterator<T> && it) -> std::vector<double> {
+    const size_t n_points = msg->height * msg->width;
+    std::vector<double> timestamps;
+    timestamps.reserve(n_points);
+    for (size_t i = 0; i < n_points; ++i, ++it) {
+      timestamps.emplace_back(static_cast<double>(*it));
+    }
+    return normalize_timestamps(timestamps);
+  };
+
+  // Get timestamp field that must be one of the following : {t, timestamp, time}
+  auto timestamp_field = get_timestamp_field(msg);
+
+  // According to the type of the timestamp == type, return a PointCloud2ConstIterator<type>
+  using sensor_msgs::PointCloud2ConstIterator;
+  if (timestamp_field.datatype == sensor_msgs::msg::PointField::UINT32) {
+    return extract_timestamps(PointCloud2ConstIterator<uint32_t>(*msg, timestamp_field.name));
+  } else if (timestamp_field.datatype == sensor_msgs::msg::PointField::FLOAT32) {
+    return extract_timestamps(PointCloud2ConstIterator<float>(*msg, timestamp_field.name));
+  } else if (timestamp_field.datatype == sensor_msgs::msg::PointField::FLOAT64) {
+    return extract_timestamps(PointCloud2ConstIterator<double>(*msg, timestamp_field.name));
+  }
+
+  // timestamp type not supported, please open an issue :)
+  throw std::runtime_error("timestamp field type not supported");
+}
+
+inline std::vector<double> get_timestamps(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
+{
+  auto timestamp_field = get_timestamp_field(msg);
+
+  // Extract timestamps from cloud_msg
+  std::vector<double> timestamps = extract_timestamps_from_msg(msg, timestamp_field);
+
+  return timestamps;
+}
+
+#endif

lidar_scan_matcher/launch/kiss_icp_matcher.launch.xml

@@ -0,0 +1,9 @@
+<launch>
+  <arg name="kiss_icp_matcher_param_path" default="$(find-pkg-share lidar_scan_matcher)/config/kiss_icp_matcher.param.yaml"/>
+
+  <node pkg="lidar_scan_matcher" exec="kiss_icp_matcher_node" name="kiss_icp_matcher_node" output="screen">
+    <remap from="points_raw" to="filtered_points"/>
+    <param from="$(var kiss_icp_matcher_param_path)"/>
+  </node>
+
+</launch>

lidar_scan_matcher/package.xml

@@ -21,6 +21,7 @@
   <depend>visualization_msgs</depend>
   <depend>ndt_omp</depend>
   <depend>fast_gicp</depend>
+  <depend>kiss_icp</depend>
   <depend>lidar_graph_slam_utils</depend>
   <depend>lidar_graph_slam_msgs</depend>