Deskewing

CMakeLists.txt

@@ -11,10 +11,12 @@ find_package(geometry_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2 REQUIRED)
-find_package(libpointmatcher_ros REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(libpointmatcher CONFIG 1.4.2 REQUIRED)
-find_package(norlab_icp_mapper CONFIG 2.0.0 REQUIRED)
+find_package(norlab_icp_mapper CONFIG 2.1.0 REQUIRED)
+find_package(libpointmatcher_ros 2.0.1 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(OpenMP REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
     "srv/SaveTrajectory.srv"
@@ -27,7 +29,11 @@ include_directories(
     ${norlab_icp_mapper_INCLUDE_DIRS}
 )
 
-add_executable(mapper_node src/mapper_node.cpp src/NodeParameters.cpp)
+add_executable(mapper_node
+    src/mapper_node.cpp
+    src/NodeParameters.cpp
+    src/Deskewer.cpp
+)
 
 ament_target_dependencies(mapper_node
     rclcpp
@@ -38,9 +44,14 @@ ament_target_dependencies(mapper_node
     tf2_ros
     tf2
     libpointmatcher_ros
+    tf2_geometry_msgs
 )
+
+# Add OpenMP flags and link OpenMP library
+target_compile_options(mapper_node PRIVATE ${OpenMP_CXX_FLAGS})
 target_link_libraries(mapper_node
     ${norlab_icp_mapper_LIBRARIES}
+    OpenMP::OpenMP_CXX
 )
 
 rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
@@ -51,5 +62,10 @@ install(TARGETS mapper_node
     DESTINATION lib/${PROJECT_NAME}
 )
 
-ament_package()
+install(DIRECTORY launch
+        DESTINATION share/${PROJECT_NAME})
 
+install(DIRECTORY config
+        DESTINATION share/${PROJECT_NAME})
+
+ament_package()

README.md

@@ -1,10 +1,12 @@
 # norlab_icp_mapper_ros
+
 A bridge between [norlab_icp_mapper](https://github.com/norlab-ulaval/norlab_icp_mapper/) and ROS.
 Check the [mapper's documentation](https://norlab-icp-mapper.readthedocs.io/en/latest/UsingInRos/) for a detailed guide.
 
 ## Node Parameters
+
 |               Name                |                                                 Description                                                 |        Possible values         |                       Default Value                        |
-|:---------------------------------:|:-----------------------------------------------------------------------------------------------------------:|:------------------------------:|:----------------------------------------------------------:|
+| :-------------------------------: | :---------------------------------------------------------------------------------------------------------: | :----------------------------: | :--------------------------------------------------------: |
 |            odom_frame             |                                          Frame used for odometry.                                           |           Any string           |                           "odom"                           |
 |            robot_frame            |                                        Frame centered on the robot.                                         |           Any string           |                        "base_link"                         |
 |          mapping_config           |                               Path to the file containing the mapping config.                               |         Any file path          |                             ""                             |
@@ -20,18 +22,24 @@ Check the [mapper's documentation](https://norlab-icp-mapper.readthedocs.io/en/l
 |               is_3D               |                       true when a 3D sensor is used, false when a 2D sensor is used.                        |         {true, false}          |                            true                            |
 |   save_map_cells_on_hard_drive    |      true when map cell storage on hard drive is wanted, false when map cell storage in RAM is wanted.      |         {true, false}          |                            true                            |
 | publish_tfs_between_registrations |      When false, the map tf is published only after registration. Otherwise with map_tf_publish_rate.       |         {true, false}          |                            true                            |
-
+|              deskew               |               Set to true if you want to apply motion compensation on the input point cloud.                |         {true, false}          |                            true                            |
+|  expectedUniqueDeskewingTFNumber  |                    How much memory should we reserve for the per-point transformations.                     |       A positive integer       |                            4000                            |
+|       deskewingRoundToNSecs       |           How much should each point's timestamp be rounded for deskewing transformation search.            |       A positive integer       |                           50000                            |
 
 ## Node Topics
-|   Name    |                     Description                     |
-|:---------:|:---------------------------------------------------:|
-| points_in |  Topic from which the input points are retrieved.   |
-|    map    |        Topic in which the map is published.         |
-| icp_odom  | Topic in which the corrected odometry is published. |
+
+|           Name           |                            Description                             |
+| :----------------------: | :----------------------------------------------------------------: |
+|        points_in         |          Topic from which the input points are retrieved.          |
+|           map            |                Topic in which the map is published.                |
+|         icp_odom         |        Topic in which the corrected odometry is published.         |
+| scan_after_input_filters |        The input scan, after all input filters are applied.        |
+|    scan_after_deskew     | The input scan, after all input filters and deskewing are applied. |
 
 ## Node Services
+
 |        Name        |          Description          | Parameter Name |               Parameter Description                |
-|:------------------:|:-----------------------------:|:--------------:|:--------------------------------------------------:|
+| :----------------: | :---------------------------: | :------------: | :------------------------------------------------: |
 |      save_map      |    Saves the current map.     |    filename    |    Path of the file in which the map is saved.     |
 |  save_trajectory   | Saves the current trajectory. |    filename    | Path of the file in which the trajectory is saved. |
 | reload_yaml_config | Reload the YAML config file.  |                |                                                    |
@@ -46,6 +54,8 @@ flowchart LR
 /points_in([ /points_in<br>sensor_msgs/msg/PointCloud2 ]):::bugged
 /icp_odom([ /icp_odom<br>nav_msgs/msg/Odometry ]):::bugged
 /map([ /map<br>sensor_msgs/msg/PointCloud2 ]):::bugged
+/scan_after_input_filters([ /map<br>sensor_msgs/msg/PointCloud2 ]):::bugged
+/scan_after_deskew([ /map<br>sensor_msgs/msg/PointCloud2 ]):::bugged
 /disable_mapping[/ /disable_mapping<br>std_srvs/srv/Empty \]:::bugged
 /enable_mapping[/ /enable_mapping<br>std_srvs/srv/Empty \]:::bugged
 /load_map[/ /load_map<br>norlab_icp_mapper_ros/srv/LoadMap \]:::bugged
@@ -92,4 +102,3 @@ style nodes opacity:0.15,fill:#FFF
 style connection opacity:0.15,fill:#FFF
 
 ```
-

config/mapper.yaml

@@ -0,0 +1,66 @@
+mapper:
+  updateCondition:
+    type: delay
+    value: 0.5
+
+  sensorMaxRange: 100
+
+  mapperModule:
+    - PointDistanceMapperModule:
+        minDistNewPoint: 0.1
+
+input:
+  - RandomSamplingDataPointsFilter:
+      prob: 0.8
+
+  - AddDescriptorDataPointsFilter:
+      descriptorName: probabilityDynamic
+      descriptorDimension: 1
+      descriptorValues: [0.6]
+
+  - SamplingSurfaceNormalDataPointsFilter:
+      ratio: 0.5
+      knn: 7
+      samplingMethod: 0
+      maxBoxDim: inf
+      averageExistingDescriptors: 1
+      keepNormals: 1
+      keepDensities: 0
+      keepEigenValues: 0
+      keepEigenVectors: 0
+
+icp:
+  readingDataPointsFilters:
+    - RandomSamplingDataPointsFilter:
+        prob: 0.8
+
+  referenceDataPointsFilters:
+
+  outlierFilters:
+    - TrimmedDistOutlierFilter:
+        ratio: 0.75
+
+  matcher:
+    KDTreeMatcher:
+      knn: 3
+      epsilon: 1
+      searchType: 1
+      maxDist: 200.0
+
+  errorMinimizer:
+    PointToPlaneErrorMinimizer:
+      force4DOF: 1
+
+  transformationCheckers:
+    - CounterTransformationChecker:
+        maxIterationCount: 40
+    - DifferentialTransformationChecker:
+        minDiffRotErr: 0.001
+        minDiffTransErr: 0.001
+        smoothLength: 3
+
+  inspector: NullInspector
+
+  logger: NullLogger
+
+post:

launch/mapper.launch.py

@@ -0,0 +1,48 @@
+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "use_sim_time", default_value="true", description="Use simulation time"
+            ),
+            Node(
+                package="norlab_icp_mapper_ros",
+                executable="mapper_node",
+                name="mapper_node",
+                output="screen",
+                parameters=[
+                    {
+                        "use_sim_time": True,
+                        "odom_frame": "odom",
+                        "robot_frame": "base_link",
+                        "mapping_config": os.path.join(
+                            get_package_share_directory("norlab_icp_mapper_ros"),
+                            "config",
+                            "mapper.yaml",
+                        ),
+                        "initial_map_file_name": "",
+                        "initial_robot_pose": "[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]",
+                        "final_map_file_name": "map.vtk",
+                        "final_trajectory_file_name": "trajectory.vtk",
+                        "map_publish_rate": 10.0,
+                        "map_tf_publish_rate": 10.0,
+                        "max_idle_time": 10.0,
+                        "is_mapping": True,
+                        "is_online": True,
+                        "is_3D": True,
+                        "save_map_cells_on_hard_drive": True,
+                        "publish_tfs_between_registrations": True,
+                    }
+                ],
+                remappings=[
+                    ("points_in", "lslidar_point_cloud"),
+                ],
+            ),
+        ]
+    )

package.xml

@@ -2,28 +2,30 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>norlab_icp_mapper_ros</name>
-  <version>2.0.0</version>
+  <version>2.1.0</version>
   <description>A bridge between norlab_icp_mapper and ROS.</description>
   <maintainer email="[email protected]">Simon-Pierre Deschenes</maintainer>
+  <maintainer email="[email protected]">Matej Boxan</maintainer>
   <license>new BSD</license>
-  
+
   <buildtool_depend>ament_cmake</buildtool_depend>
-  
+
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>std_srvs</depend>
   <depend>tf2_ros</depend>
   <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>libpointmatcher_ros</depend>
 
   <build_depend>rosidl_default_generators</build_depend>
 
   <exec_depend>rosidl_default_runtime</exec_depend>
 
   <member_of_group>rosidl_interface_packages</member_of_group>
-  
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/Deskewer.cpp

@@ -0,0 +1,79 @@
+#include "Deskewer.h"
+#include <cstdint>
+#include <chrono>
+#include <pointmatcher_ros/PointMatcher_ROS.h>
+
+#include <tf2_ros/transform_listener.h>
+#include <tf2/utils.h>
+#include <tf2_ros/buffer.h>
+#include <omp.h>
+
+Deskewer::Deskewer(const rclcpp::Logger& logger, rclcpp::Clock::SharedPtr clock, uint expectedUniqueDeskewingTFNumber, uint deskewingRoundToNSecs)
+    : logger(logger),
+    expectedUniqueDeskewingTFNumber(expectedUniqueDeskewingTFNumber),
+    deskewingRoundToNSecs(deskewingRoundToNSecs)
+{
+    tfBuffer = std::unique_ptr<tf2_ros::Buffer>(new tf2_ros::Buffer(clock));
+    tfListener = std::unique_ptr<tf2_ros::TransformListener>(new tf2_ros::TransformListener(*tfBuffer));
+    tfsCache.reserve(expectedUniqueDeskewingTFNumber);
+}
+
+bool Deskewer::deskewCloud(Deskewer::DP &cloud, const std::string &sensorFrame)
+{
+   	std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
+
+    tfsCache.clear();
+    std::unordered_map<int64_t, int64_t> timeCache;
+
+    if (!cloud.timeExists(timeFieldName))
+    {
+        RCLCPP_WARN(logger, "The input pointcloud does not contain the 'time' field. Skipping.");
+        return false;
+    }
+
+    int64_t latestTime = 0;
+    for (int i=0; i<cloud.getNbPoints(); ++i)
+    {
+        latestTime = std::max(latestTime, cloud.times(i));
+    }
+    rclcpp::Time latestTimeRos(latestTime);
+
+    //iterate over the pointcloud, lookup ROS transforms
+    // and fill the lookup table with the transforms
+    for (int i=0; i<cloud.getNbPoints(); ++i)
+    {
+        int64_t cachedTfTime = cloud.times(i) / deskewingRoundToNSecs;
+        timeCache[cloud.times(i)] = cachedTfTime;
+        if(tfsCache.count(cachedTfTime) == 0)
+        {
+            rclcpp::Time laserTimeRos(cloud.times(i));
+            try{
+                geometry_msgs::msg::TransformStamped transform = tfBuffer->lookupTransform(sensorFrame,
+                                                        latestTimeRos,
+                                                        sensorFrame,
+                                                        laserTimeRos,
+                                                        fixedFrameForLaser,
+                                                        rclcpp::Duration(0, 2.5e8));
+                tfsCache[cachedTfTime] = transform;
+            }
+            catch(tf2::TransformException &ex){
+                RCLCPP_ERROR(logger, "Pointcloud callback failed because: %s", ex.what());
+                return false;
+            }
+        }
+    }
+
+    // apply the transforms to the pointcloud in parallel
+    #pragma omp parallel for
+    for (int i=0; i<cloud.getNbPoints(); ++i) {
+        // transform the point
+        int64_t cachedTfTime = timeCache[cloud.times(i)];
+        auto transform = tfsCache[cachedTfTime];
+        auto transformationParameters = PointMatcher_ROS::rosTfToPointMatcherTransformation<float>(transform, 4);
+        cloud.features.col(i) = transformationParameters * cloud.features.col(i);
+    }
+
+	std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
+    RCLCPP_DEBUG_STREAM(logger, "Point cloud deskewed in " << std::chrono::duration_cast<std::chrono::milliseconds>(end - begin).count() << " [ms]");
+    return true;
+}

src/Deskewer.h

@@ -0,0 +1,36 @@
+#ifndef DESKEWER_H
+#define DESKEWER_H
+
+#include <pointmatcher/PointMatcher.h>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+class Deskewer
+{
+  private:
+    typedef PointMatcher<float> PM;
+    typedef PM::DataPoints DP;
+
+    uint expectedUniqueDeskewingTFNumber;
+    uint deskewingRoundToNSecs;
+    std::unique_ptr<tf2_ros::Buffer> tfBuffer = nullptr;
+    std::unique_ptr<tf2_ros::TransformListener> tfListener = nullptr;
+    rclcpp::Logger logger;
+
+    const std::string timeFieldName = "time";
+    std::string fixedFrameForLaser = "odom";
+
+    // Dictionary to store already looked up transforms
+    std::unordered_map<int64_t, geometry_msgs::msg::TransformStamped> tfsCache;
+
+  public:
+    // Constructor
+    Deskewer(const rclcpp::Logger &logger, rclcpp::Clock::SharedPtr clock, uint expectedUniqueDeskewingTFNumber, uint deskewingRoundToNSecs);
+
+    bool deskewCloud(DP &cloud, const std::string &sensor_frame);
+};
+
+#endif