feat(ndt_scan_matcher): dynamic map loading

localization/ndt_scan_matcher/CMakeLists.txt

@@ -34,6 +34,7 @@ ament_auto_add_executable(ndt_scan_matcher
   src/tf2_listener_module.cpp
   src/map_module.cpp
   src/pose_initialization_module.cpp
+  src/map_update_module.cpp
 )
 
 link_directories(${PCL_LIBRARY_DIRS})

localization/ndt_scan_matcher/README.md

@@ -62,7 +62,6 @@ One optional function is regularization. Please see the regularization chapter i
 | `max_iterations`                        | int    | The number of iterations required to calculate alignment                                        |
 | `converged_param_type`                  | int    | The type of indicators for scan matching score (0: TP, 1: NVTL)                                 |
 | `converged_param_transform_probability` | double | Threshold for deciding whether to trust the estimation result                                   |
-| `neighborhood_search_method`            | int    | neighborhood search method (0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1)                         |
 | `num_threads`                           | int    | Number of threads used for parallel computing                                                   |
 
 (TP: Transform Probability, NVTL: Nearest Voxel Transform Probability)
@@ -169,3 +168,57 @@ The color of the trajectory indicates the error (meter) from the reference traje
 - The right figure shows that the regularization suppresses the longitudinal error.
 
 <img src="./media/trajectory_without_regularization.png" alt="drawing" width="300"/> <img src="./media/trajectory_with_regularization.png" alt="drawing" width="300"/>
+
+## Dynamic map loading
+
+Autoware supports dynamic map loading feature for `ndt_scan_matcher`. Using this feature, NDT dynamically requests for the surrounding pointcloud map to `pointcloud_map_loader`, and then receive and preprocess the map in an online fashion.
+
+Using the feature, `ndt_scan_matcher` can theoretically handle any large size maps in terms of memory usage. (Note that it is still possible that there exists a limitation due to other factors, e.g. floating-point error)
+
+### Additional interfaces
+
+#### Additional inputs
+
+| Name             | Type                      | Description                                                 |
+| ---------------- | ------------------------- | ----------------------------------------------------------- |
+| `input_ekf_odom` | `nav_msgs::msg::Odometry` | Vehicle localization results (used for map update decision) |
+
+#### Additional outputs
+
+| Name                          | Type                            | Description                                       |
+| ----------------------------- | ------------------------------- | ------------------------------------------------- |
+| `debug/loaded_pointcloud_map` | `sensor_msgs::msg::PointCloud2` | pointcloud maps used for localization (for debug) |
+
+#### Additional client
+
+| Name                | Type                                                   | Description        |
+| ------------------- | ------------------------------------------------------ | ------------------ |
+| `client_map_loader` | `autoware_map_msgs::srv::GetDifferentialPointCloudMap` | map loading client |
+
+### Parameters
+
+| Name                                  | Type   | Description                                                          |
+| ------------------------------------- | ------ | -------------------------------------------------------------------- |
+| `use_dynamic_map_loading`             | bool   | Flag to enable dynamic map loading feature for NDT (TRUE by default) |
+| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s)                                 |
+| `dynamic_map_loading_map_radius`      | double | Map loading radius for every update                                  |
+| `lidar_radius`                        | double | LiDAR radius used for localization (only used for diagnosis)         |
+
+### Enabling the dynamic map loading feature
+
+To use dynamic map loading feature for `ndt_scan_matcher`, you also need to appropriately configure some other settings outside of this node.
+Follow the next two instructions.
+
+1. enable dynamic map loading interface in `pointcloud_map_loader` (by setting `enable_differential_load` to true in the package)
+2. split the PCD files into grids (recommended split size: 20[m] x 20[m])
+
+Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample-map-rosbag_split.zip`](https://github.com/autowarefoundation/autoware.universe/files/10349104/sample-map-rosbag_split.zip)
+
+|  PCD files  | `use_dynamic_map_loading` | `enable_differential_load` | How NDT loads map(s) |
+| :---------: | :-----------------------: | :------------------------: | :------------------: |
+| single file |           true            |            true            |  at once (standard)  |
+| single file |           true            |           false            |  **does NOT work**   |
+| single file |           false           |         true/false         |  at once (standard)  |
+|  splitted   |           true            |            true            |     dynamically      |
+|  splitted   |           true            |           false            |  **does NOT work**   |
+|  splitted   |           false           |         true/false         |  at once (standard)  |

localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

@@ -1,5 +1,7 @@
 /**:
   ros__parameters:
+    # Use dynamic map loading
+    use_dynamic_map_loading: true
 
     # Vehicle reference frame
     base_frame: "base_link"
@@ -41,10 +43,6 @@
     # Tolerance of distance difference between two initial poses used for linear interpolation. [m]
     initial_pose_distance_tolerance_m: 10.0
 
-    # neighborhood search method
-    # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1
-    neighborhood_search_method: 0
-
     # Number of threads used for parallel computing
     num_threads: 4
 
@@ -63,5 +61,14 @@
     # Regularization switch
     regularization_enabled: false
 
-    # Regularization scale factor
+    # regularization scale factor
     regularization_scale_factor: 0.01
+
+    # Dynamic map loading distance
+    dynamic_map_loading_update_distance: 20.0
+
+    # Dynamic map loading loading radius
+    dynamic_map_loading_map_radius: 150.0
+
+    # Radius of input LiDAR range (used for diagnostics of dynamic map loading)
+    lidar_radius: 100.0

localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp

@@ -19,9 +19,9 @@
 
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
+#include <multigrid_pclomp/multigrid_ndt_omp.h>
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
-#include <pclomp/ndt_omp.h>
 
 #include <memory>
 
@@ -30,7 +30,7 @@ class MapModule
   using PointSource = pcl::PointXYZ;
   using PointTarget = pcl::PointXYZ;
   using NormalDistributionsTransform =
-    pclomp::NormalDistributionsTransform<PointSource, PointTarget>;
+    pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>;
 
 public:
   MapModule(

localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp

@@ -0,0 +1,107 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_
+#define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_
+
+#include "ndt_scan_matcher/debug.hpp"
+#include "ndt_scan_matcher/particle.hpp"
+#include "ndt_scan_matcher/tf2_listener_module.hpp"
+#include "ndt_scan_matcher/util_func.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/marker_helper.hpp>
+
+#include <autoware_map_msgs/srv/get_differential_point_cloud_map.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <tier4_localization_msgs/srv/pose_with_covariance_stamped.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <fmt/format.h>
+#include <multigrid_pclomp/multigrid_ndt_omp.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <map>
+#include <memory>
+#include <optional>
+#include <string>
+#include <vector>
+
+class MapUpdateModule
+{
+  using PointSource = pcl::PointXYZ;
+  using PointTarget = pcl::PointXYZ;
+  using NormalDistributionsTransform =
+    pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>;
+
+public:
+  MapUpdateModule(
+    rclcpp::Node * node, std::mutex * ndt_ptr_mutex,
+    std::shared_ptr<NormalDistributionsTransform> ndt_ptr,
+    std::shared_ptr<Tf2ListenerModule> tf2_listener_module, std::string map_frame,
+    rclcpp::CallbackGroup::SharedPtr main_callback_group,
+    std::shared_ptr<std::map<std::string, std::string>> state_ptr);
+
+private:
+  void service_ndt_align(
+    const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req,
+    tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res);
+  void callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr);
+  void map_update_timer_callback();
+
+  void update_ndt(
+    const std::vector<autoware_map_msgs::msg::PointCloudMapCellWithID> & maps_to_add,
+    const std::vector<std::string> & map_ids_to_remove);
+  void update_map(const geometry_msgs::msg::Point & position);
+  bool should_update_map(const geometry_msgs::msg::Point & position) const;
+  void publish_partial_pcd_map();
+  geometry_msgs::msg::PoseWithCovarianceStamped align_using_monte_carlo(
+    const std::shared_ptr<NormalDistributionsTransform> & ndt_ptr,
+    const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov);
+  void publish_point_cloud(
+    const rclcpp::Time & sensor_ros_time, const std::string & frame_id,
+    const std::shared_ptr<const pcl::PointCloud<PointSource>> & sensor_points_mapTF_ptr);
+
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr loaded_pcd_pub_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+    ndt_monte_carlo_initial_pose_marker_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr sensor_aligned_pose_pub_;
+
+  rclcpp::Service<tier4_localization_msgs::srv::PoseWithCovarianceStamped>::SharedPtr service_;
+  rclcpp::Client<autoware_map_msgs::srv::GetDifferentialPointCloudMap>::SharedPtr
+    pcd_loader_client_;
+  rclcpp::TimerBase::SharedPtr map_update_timer_;
+
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr ekf_odom_sub_;
+
+  rclcpp::CallbackGroup::SharedPtr map_callback_group_;
+
+  std::shared_ptr<NormalDistributionsTransform> ndt_ptr_;
+  std::mutex * ndt_ptr_mutex_;
+  std::string map_frame_;
+  rclcpp::Logger logger_;
+  rclcpp::Clock::SharedPtr clock_;
+  std::shared_ptr<Tf2ListenerModule> tf2_listener_module_;
+  std::shared_ptr<std::map<std::string, std::string>> state_ptr_;
+
+  int initial_estimate_particles_num_;
+  std::optional<geometry_msgs::msg::Point> last_update_position_ = std::nullopt;
+  std::optional<geometry_msgs::msg::Point> current_position_ = std::nullopt;
+  const double dynamic_map_loading_update_distance_;
+  const double dynamic_map_loading_map_radius_;
+  const double lidar_radius_;
+};
+
+#endif  // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -18,6 +18,7 @@
 #define FMT_HEADER_ONLY
 
 #include "ndt_scan_matcher/map_module.hpp"
+#include "ndt_scan_matcher/map_update_module.hpp"
 #include "ndt_scan_matcher/pose_initialization_module.hpp"
 #include "ndt_scan_matcher/tf2_listener_module.hpp"
 
@@ -34,8 +35,8 @@
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <fmt/format.h>
+#include <multigrid_pclomp/multigrid_ndt_omp.h>
 #include <pcl/point_types.h>
-#include <pclomp/ndt_omp.h>
 #include <tf2/transform_datatypes.h>
 
 #ifdef ROS_DISTRO_GALACTIC
@@ -71,7 +72,7 @@ class NDTScanMatcher : public rclcpp::Node
   using PointSource = pcl::PointXYZ;
   using PointTarget = pcl::PointXYZ;
   using NormalDistributionsTransform =
-    pclomp::NormalDistributionsTransform<PointSource, PointTarget>;
+    pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>;
 
 public:
   NDTScanMatcher();
@@ -195,6 +196,7 @@ class NDTScanMatcher : public rclcpp::Node
   std::shared_ptr<Tf2ListenerModule> tf2_listener_module_;
   std::unique_ptr<MapModule> map_module_;
   std::unique_ptr<PoseInitializationModule> pose_init_module_;
+  std::unique_ptr<MapUpdateModule> map_update_module_;
 };
 
 #endif  // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_

localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp

@@ -27,8 +27,8 @@
 #include <tier4_localization_msgs/srv/pose_with_covariance_stamped.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
+#include <multigrid_pclomp/multigrid_ndt_omp.h>
 #include <pcl_conversions/pcl_conversions.h>
-#include <pclomp/ndt_omp.h>
 
 #include <map>
 #include <memory>
@@ -39,7 +39,7 @@ class PoseInitializationModule
   using PointSource = pcl::PointXYZ;
   using PointTarget = pcl::PointXYZ;
   using NormalDistributionsTransform =
-    pclomp::NormalDistributionsTransform<PointSource, PointTarget>;
+    pclomp::MultiGridNormalDistributionsTransform<PointSource, PointTarget>;
 
 public:
   PoseInitializationModule(

localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml

@@ -7,10 +7,13 @@
   <arg name="input_map_points_topic" default="/pointcloud_map" description="Map points topic"/>
   <arg name="input_regularization_pose_topic" default="/sensing/gnss/pose_with_covariance" description="Regularization pose topic"/>
   <arg name="input_service_trigger_node" default="trigger_node" description="Trigger node service name"/>
+  <arg name="input_ekf_odom" default="/localization/kinematic_state" description="Input EKF odometry"/>
 
   <arg name="output_pose_topic" default="ndt_pose" description="Estimated self position"/>
   <arg name="output_pose_with_covariance_topic" default="ndt_pose_with_covariance" description="Estimated self position with covariance"/>
 
+  <arg name="client_map_loader" default="/map/get_differential_pointcloud_map" description="Trigger node service name"/>
+
   <arg name="node_name" default="ndt_scan_matcher" description="Use a different name for this node"/>
 
   <node pkg="ndt_scan_matcher" exec="ndt_scan_matcher" name="$(var node_name)" output="log">
@@ -23,6 +26,9 @@
     <remap from="ndt_pose_with_covariance" to="$(var output_pose_with_covariance_topic)"/>
     <remap from="regularization_pose_with_covariance" to="$(var input_regularization_pose_topic)"/>
     <remap from="trigger_node_srv" to="$(var input_service_trigger_node)"/>
+    <remap from="pcd_loader_service" to="$(var client_map_loader)"/>
+    <remap from="ekf_odom" to="$(var input_ekf_odom)"/>
+
     <param from="$(var param_file)"/>
   </node>
 </launch>

localization/ndt_scan_matcher/package.xml

@@ -14,6 +14,7 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_map_msgs</depend>
   <depend>diagnostic_msgs</depend>
   <depend>fmt</depend>
   <depend>geometry_msgs</depend>