diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt
index 0f5b761baf6f0..0a04f1bb60466 100644
--- a/localization/ndt_scan_matcher/CMakeLists.txt
+++ b/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})
diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md
index 81fac59e4b8db..7c63785aebcd2 100644
--- a/localization/ndt_scan_matcher/README.md
+++ b/localization/ndt_scan_matcher/README.md
@@ -64,7 +64,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)
@@ -172,6 +171,67 @@ The color of the trajectory indicates the error (meter) from the reference traje
+## 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 size: 20[m] x 20[m])
+
+Note that the dynamic map loading may FAIL if the map is split into two or more large size map (e.g. 1000[m] x 1000[m]). Please provide either of
+
+- one PCD map file
+- multiple PCD map files divided into small size (~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) |
+
## Scan matching score based on de-grounded LiDAR scan
### Abstract
diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml
index 1fb8fd003b4ac..56c5baa347aaa 100644
--- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml
+++ b/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,9 +61,18 @@
# 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
+
# A flag for using scan matching score based on de-grounded LiDAR scan
estimate_scores_for_degrounded_scan: false
diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp
index 871ff2760acf7..5253879a28937 100644
--- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp
+++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp
@@ -19,9 +19,9 @@
#include
+#include
#include
#include
-#include
#include
@@ -30,7 +30,7 @@ class MapModule
using PointSource = pcl::PointXYZ;
using PointTarget = pcl::PointXYZ;
using NormalDistributionsTransform =
- pclomp::NormalDistributionsTransform;
+ pclomp::MultiGridNormalDistributionsTransform;
public:
MapModule(
diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp
new file mode 100644
index 0000000000000..fb9577ca42934
--- /dev/null
+++ b/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
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+
+#include