From d0e57e05c073936c182fcf5272fd3c68d6c98587 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Mon, 5 Feb 2024 17:14:31 +0900 Subject: [PATCH] refactor(ndt_scan_matcher): rework parameters (#6229) * refactor(ndt_scan_matcher): rework parameters Signed-off-by: Yamato Ando * delete tmp file Signed-off-by: Yamato Ando * style(pre-commit): autofix --------- Signed-off-by: Yamato Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/ndt_scan_matcher/README.md | 67 ++++++++----------- .../schema/ndt_scan_matcher.schema.json | 44 ++++++++++++ .../schema/sub/covariance.json | 25 +++++++ .../sub/covariance_covariance_estimation.json | 28 ++++++++ .../schema/sub/dynamic_map_loading.json | 31 +++++++++ .../ndt_scan_matcher/schema/sub/frame.json | 28 ++++++++ .../schema/sub/initial_pose_estimation.json | 25 +++++++ .../ndt_scan_matcher/schema/sub/ndt.json | 53 +++++++++++++++ .../schema/sub/ndt_regularization.json | 24 +++++++ .../schema/sub/score_estimation.json | 39 +++++++++++ .../score_estimation_no_ground_points.json | 24 +++++++ .../schema/sub/validation.json | 42 ++++++++++++ 12 files changed, 390 insertions(+), 40 deletions(-) create mode 100644 localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json create mode 100644 localization/ndt_scan_matcher/schema/sub/covariance.json create mode 100644 localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json create mode 100644 localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json create mode 100644 localization/ndt_scan_matcher/schema/sub/frame.json create mode 100644 localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json create mode 100644 localization/ndt_scan_matcher/schema/sub/ndt.json create mode 100644 localization/ndt_scan_matcher/schema/sub/ndt_regularization.json create mode 100644 localization/ndt_scan_matcher/schema/sub/score_estimation.json create mode 100644 localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json create mode 100644 localization/ndt_scan_matcher/schema/sub/validation.json diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 7956a663f92d5..74d49e13a4c21 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -56,28 +56,29 @@ One optional function is regularization. Please see the regularization chapter i ### Core Parameters -| Name | Type | Description | -| --------------------------------------------------------- | ---------------------- | -------------------------------------------------------------------------------------------------- | -| `base_frame` | string | Vehicle reference frame | -| `ndt_base_frame` | string | NDT reference frame | -| `map_frame` | string | map frame | -| `input_sensor_points_queue_size` | int | Subscriber queue size | -| `trans_epsilon` | double | The max difference between two consecutive transformations to consider convergence | -| `step_size` | double | The newton line search maximum step length | -| `resolution` | double | The ND voxel grid resolution [m] | -| `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 | TP threshold for deciding whether to trust the estimation result (when converged_param_type = 0) | -| `converged_param_nearest_voxel_transformation_likelihood` | double | NVTL threshold for deciding whether to trust the estimation result (when converged_param_type = 1) | -| `initial_estimate_particles_num` | int | The number of particles to estimate initial pose | -| `n_startup_trials` | int | The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). | -| `lidar_topic_timeout_sec` | double | Tolerance of timestamp difference between current time and sensor pointcloud | -| `initial_pose_timeout_sec` | int | Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] | -| `initial_pose_distance_tolerance_m` | double | Tolerance of distance difference between two initial poses used for linear interpolation. [m] | -| `num_threads` | int | Number of threads used for parallel computing | -| `output_pose_covariance` | std::array | The covariance of output pose | - -(TP: Transform Probability, NVTL: Nearest Voxel Transform Probability) +#### Frame + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/frame.json") }} + +#### Ndt + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/ndt.json") }} + +#### Initial Pose Estimation + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json") }} + +#### Validation + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/validation.json") }} + +#### Score Estimation + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/score_estimation.json") }} + +#### Covariance + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance.json") }} ## Regularization @@ -153,10 +154,7 @@ This is because if the base position is far off from the true value, NDT scan ma ### Parameters -| Name | Type | Description | -| ----------------------------- | ------ | ---------------------------------------------------------------------- | -| `regularization_enabled` | bool | Flag to add regularization term to NDT optimization (FALSE by default) | -| `regularization_scale_factor` | double | Coefficient of the regularization term. | +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/ndt_regularization.json") }} Regularization is disabled by default because GNSS is not always accurate enough to serve the appropriate base position in any scenes. @@ -206,11 +204,7 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### Parameters -| Name | Type | Description | -| ------------------------------------- | ------ | ------------------------------------------------------------ | -| `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) | +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json") }} ### Notes for dynamic map loading @@ -237,10 +231,7 @@ This is a function that uses no ground LiDAR scan to estimate the scan matching ### Parameters -| Name | Type | Description | -| ------------------------------------- | ------ | ----------------------------------------------------------------------------------- | -| `estimate_scores_by_no_ground_points` | bool | Flag for using scan matching score based on no ground LiDAR scan (FALSE by default) | -| `z_margin_for_ground_removal` | double | Z-value margin for removal ground points | +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json") }} ## 2D real-time covariance estimation @@ -259,8 +250,4 @@ Note that this function may spoil healthy system behavior if it consumes much ca initial_pose_offset_model is rotated around (x,y) = (0,0) in the direction of the first principal component of the Hessian matrix. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. -| Name | Type | Description | -| ----------------------------- | ------------------- | ----------------------------------------------------------------- | -| `use_covariance_estimation` | bool | Flag for using real-time covariance estimation (FALSE by default) | -| `initial_pose_offset_model_x` | std::vector | X-axis offset [m] | -| `initial_pose_offset_model_y` | std::vector | Y-axis offset [m] | +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json") }} diff --git a/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json b/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json new file mode 100644 index 0000000000000..a42ee37858f46 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json @@ -0,0 +1,44 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "type": "object", + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "frame": { "$ref": "sub/frame.json#/definitions/frame" }, + "ndt": { "$ref": "sub/ndt.json#/definitions/ndt" }, + "regularization": { "$ref": "ndt_regularization.json#/definitions/ndt/regularization" }, + "initial_pose_estimation": { + "$ref": "sub/initial_pose_estimation.json#/definitions/initial_pose_estimation" + }, + "validation": { "$ref": "sub/validation.json#/definitions/validation" }, + "score_estimation": { + "$ref": "sub/score_estimation.json#/definitions/score_estimation" + }, + "covariance": { "$ref": "sub/covariance.json#/definitions/covariance" }, + "dynamic_map_loading": { + "$ref": "sub/dynamic_map_loading.json#/definitions/dynamic_map_loading" + } + }, + "required": [ + "frame", + "ndt", + "initial_pose_estimation", + "validation", + "score_estimation", + "covariance", + "dynamic_map_loading" + ], + "additionalProperties": false + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/ndt_scan_matcher/schema/sub/covariance.json b/localization/ndt_scan_matcher/schema/sub/covariance.json new file mode 100644 index 0000000000000..655edabbdf871 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/covariance.json @@ -0,0 +1,25 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "covariance": { + "type": "object", + "properties": { + "output_pose_covariance": { + "type": "array", + "description": "The covariance of output pose. Note that this covariance matrix is empirically derived.", + "default": [ + 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0225, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.000625 + ] + }, + "covariance_estimation": { + "$ref": "covariance_covariance_estimation.json#/definitions/covariance_estimation" + } + }, + "required": ["output_pose_covariance", "covariance_estimation"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json b/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json new file mode 100644 index 0000000000000..d978e5bcf7357 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json @@ -0,0 +1,28 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "covariance_estimation": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value).", + "default": false + }, + "initial_pose_offset_model_x": { + "type": "array", + "description": "Offset arrangement in covariance estimation [m]. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.", + "default": [0.0, 0.0, 0.5, -0.5, 1.0, -1.0] + }, + "initial_pose_offset_model_y": { + "type": "array", + "description": "Offset arrangement in covariance estimation [m]. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.", + "default": [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] + } + }, + "required": ["enable", "initial_pose_offset_model_x", "initial_pose_offset_model_y"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json b/localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json new file mode 100644 index 0000000000000..9776fbed350f2 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json @@ -0,0 +1,31 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "dynamic_map_loading": { + "type": "object", + "properties": { + "update_distance": { + "type": "number", + "description": "Dynamic map loading distance.", + "default": 20.0, + "minimum": 0.0 + }, + "map_radius": { + "type": "number", + "description": "Dynamic map loading loading radius.", + "default": 150.0, + "minimum": 0.0 + }, + "lidar_radius": { + "type": "number", + "description": "Radius of input LiDAR range (used for diagnostics of dynamic map loading).", + "default": 100.0, + "minimum": 0.0 + } + }, + "required": ["update_distance", "map_radius", "lidar_radius"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/frame.json b/localization/ndt_scan_matcher/schema/sub/frame.json new file mode 100644 index 0000000000000..98bf7abe711c3 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/frame.json @@ -0,0 +1,28 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "frame": { + "type": "object", + "properties": { + "base_frame": { + "type": "string", + "description": "Vehicle reference frame.", + "default": "base_link" + }, + "ndt_base_frame": { + "type": "string", + "description": "NDT reference frame.", + "default": "ndt_base_link" + }, + "map_frame": { + "type": "string", + "description": "Map frame.", + "default": "map" + } + }, + "required": ["base_frame", "ndt_base_frame", "map_frame"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json b/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json new file mode 100644 index 0000000000000..9817f3145bbd3 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json @@ -0,0 +1,25 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "initial_pose_estimation": { + "type": "object", + "properties": { + "particles_num": { + "type": "number", + "description": "The number of particles to estimate initial pose.", + "default": 200, + "minimum": 1 + }, + "n_startup_trials": { + "type": "number", + "description": "The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). This value should be equal to or less than 'initial_estimate_particles_num' and more than 0. If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search.", + "default": 20, + "minimum": 1 + } + }, + "required": ["particles_num", "n_startup_trials"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/ndt.json b/localization/ndt_scan_matcher/schema/sub/ndt.json new file mode 100644 index 0000000000000..850e48c2db33b --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/ndt.json @@ -0,0 +1,53 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "ndt": { + "type": "object", + "properties": { + "trans_epsilon": { + "type": "number", + "description": "The maximum difference between two consecutive transformations in order to consider convergence.", + "default": 0.01, + "minimum": 0.0 + }, + "step_size": { + "type": "number", + "description": "The newton line search maximum step length.", + "default": 0.1, + "minimum": 0.0 + }, + "resolution": { + "type": "number", + "description": "The ND voxel grid resolution.", + "default": 2.0, + "minimum": 0.0 + }, + "max_iterations": { + "type": "number", + "description": "The number of iterations required to calculate alignment.", + "default": 30, + "minimum": 1 + }, + "num_threads": { + "type": "number", + "description": "Number of threads used for parallel computing.", + "default": 4, + "minimum": 1 + }, + "regularization": { + "$ref": "ndt_regularization.json#/definitions/regularization" + } + }, + "required": [ + "trans_epsilon", + "step_size", + "resolution", + "max_iterations", + "num_threads", + "regularization" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/ndt_regularization.json b/localization/ndt_scan_matcher/schema/sub/ndt_regularization.json new file mode 100644 index 0000000000000..1de74de792700 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/ndt_regularization.json @@ -0,0 +1,24 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "regularization": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "Regularization switch.", + "default": false + }, + "scale_factor": { + "type": "number", + "description": "Regularization scale factor.", + "default": 0.01, + "minimum": 0.0 + } + }, + "required": ["enable", "scale_factor"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/score_estimation.json b/localization/ndt_scan_matcher/schema/sub/score_estimation.json new file mode 100644 index 0000000000000..c8a1195f99080 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/score_estimation.json @@ -0,0 +1,39 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "score_estimation": { + "type": "object", + "properties": { + "converged_param_type": { + "type": "number", + "description": "Converged param type. 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD", + "default": 1, + "minimum": 0, + "maximum": 1 + }, + "converged_param_transform_probability": { + "type": "number", + "description": "If converged_param_type is 0, threshold for deciding whether to trust the estimation result.", + "default": 3.0, + "minimum": 0.0 + }, + "converged_param_nearest_voxel_transformation_likelihood": { + "type": "number", + "description": "If converged_param_type is 1, threshold for deciding whether to trust the estimation result.", + "default": 2.3, + "minimum": 0.0 + }, + "no_ground_points": { + "$ref": "score_estimation_no_ground_points.json#/definitions/no_ground_points" + } + }, + "required": [ + "converged_param_type", + "converged_param_transform_probability", + "no_ground_points" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json b/localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json new file mode 100644 index 0000000000000..aa3f0fe162bc0 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json @@ -0,0 +1,24 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "no_ground_points": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "A flag for using scan matching score based on de-grounded LiDAR scan.", + "default": false + }, + "z_margin_for_ground_removal": { + "type": "number", + "description": "If lidar_point.z - base_link.z <= this threshold , the point will be removed.", + "default": 0.8, + "minimum": 0.0 + } + }, + "required": ["enable", "z_margin_for_ground_removal"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/validation.json b/localization/ndt_scan_matcher/schema/sub/validation.json new file mode 100644 index 0000000000000..5ad40ceb99577 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/validation.json @@ -0,0 +1,42 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "validation": { + "type": "object", + "properties": { + "lidar_topic_timeout_sec": { + "type": "number", + "description": "Tolerance of timestamp difference between current time and sensor pointcloud. [sec]", + "default": 1.0, + "minimum": 0.0 + }, + "initial_pose_timeout_sec": { + "type": "number", + "description": "Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec]", + "default": 1.0, + "minimum": 0.0 + }, + "initial_pose_distance_tolerance_m": { + "type": "number", + "description": "Tolerance of distance difference between two initial poses used for linear interpolation. [m]", + "default": 10.0, + "minimum": 0.0 + }, + "critical_upper_bound_exe_time_ms": { + "type": "number", + "description": "The execution time which means probably NDT cannot matches scans properly. [ms]", + "default": 100.0, + "minimum": 0.0 + } + }, + "required": [ + "lidar_topic_timeout_sec", + "initial_pose_timeout_sec", + "initial_pose_distance_tolerance_m", + "critical_upper_bound_exe_time_ms" + ], + "additionalProperties": false + } + } +}