Skip to content

Commit d0e57e0

Browse files
YamatoAndopre-commit-ci[bot]
authored and
zulfaqar-azmi-t4
committed
refactor(ndt_scan_matcher): rework parameters (autowarefoundation#6229)
* refactor(ndt_scan_matcher): rework parameters Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * delete tmp file Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent b21df01 commit d0e57e0

12 files changed

+390
-40
lines changed

‎localization/ndt_scan_matcher/README.md

+27-40
Original file line numberDiff line numberDiff line change
@@ -56,28 +56,29 @@ One optional function is regularization. Please see the regularization chapter i
5656

5757
### Core Parameters
5858

59-
| Name | Type | Description |
60-
| --------------------------------------------------------- | ---------------------- | -------------------------------------------------------------------------------------------------- |
61-
| `base_frame` | string | Vehicle reference frame |
62-
| `ndt_base_frame` | string | NDT reference frame |
63-
| `map_frame` | string | map frame |
64-
| `input_sensor_points_queue_size` | int | Subscriber queue size |
65-
| `trans_epsilon` | double | The max difference between two consecutive transformations to consider convergence |
66-
| `step_size` | double | The newton line search maximum step length |
67-
| `resolution` | double | The ND voxel grid resolution [m] |
68-
| `max_iterations` | int | The number of iterations required to calculate alignment |
69-
| `converged_param_type` | int | The type of indicators for scan matching score (0: TP, 1: NVTL) |
70-
| `converged_param_transform_probability` | double | TP threshold for deciding whether to trust the estimation result (when converged_param_type = 0) |
71-
| `converged_param_nearest_voxel_transformation_likelihood` | double | NVTL threshold for deciding whether to trust the estimation result (when converged_param_type = 1) |
72-
| `initial_estimate_particles_num` | int | The number of particles to estimate initial pose |
73-
| `n_startup_trials` | int | The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). |
74-
| `lidar_topic_timeout_sec` | double | Tolerance of timestamp difference between current time and sensor pointcloud |
75-
| `initial_pose_timeout_sec` | int | Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] |
76-
| `initial_pose_distance_tolerance_m` | double | Tolerance of distance difference between two initial poses used for linear interpolation. [m] |
77-
| `num_threads` | int | Number of threads used for parallel computing |
78-
| `output_pose_covariance` | std::array<double, 36> | The covariance of output pose |
79-
80-
(TP: Transform Probability, NVTL: Nearest Voxel Transform Probability)
59+
#### Frame
60+
61+
{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/frame.json") }}
62+
63+
#### Ndt
64+
65+
{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/ndt.json") }}
66+
67+
#### Initial Pose Estimation
68+
69+
{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json") }}
70+
71+
#### Validation
72+
73+
{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/validation.json") }}
74+
75+
#### Score Estimation
76+
77+
{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/score_estimation.json") }}
78+
79+
#### Covariance
80+
81+
{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance.json") }}
8182

8283
## Regularization
8384

@@ -153,10 +154,7 @@ This is because if the base position is far off from the true value, NDT scan ma
153154

154155
### Parameters
155156

156-
| Name | Type | Description |
157-
| ----------------------------- | ------ | ---------------------------------------------------------------------- |
158-
| `regularization_enabled` | bool | Flag to add regularization term to NDT optimization (FALSE by default) |
159-
| `regularization_scale_factor` | double | Coefficient of the regularization term. |
157+
{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/ndt_regularization.json") }}
160158

161159
Regularization is disabled by default because GNSS is not always accurate enough to serve the appropriate base position in any scenes.
162160

@@ -206,11 +204,7 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma
206204

207205
### Parameters
208206

209-
| Name | Type | Description |
210-
| ------------------------------------- | ------ | ------------------------------------------------------------ |
211-
| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) |
212-
| `dynamic_map_loading_map_radius` | double | Map loading radius for every update |
213-
| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) |
207+
{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json") }}
214208

215209
### Notes for dynamic map loading
216210

@@ -237,10 +231,7 @@ This is a function that uses no ground LiDAR scan to estimate the scan matching
237231

238232
### Parameters
239233

240-
| Name | Type | Description |
241-
| ------------------------------------- | ------ | ----------------------------------------------------------------------------------- |
242-
| `estimate_scores_by_no_ground_points` | bool | Flag for using scan matching score based on no ground LiDAR scan (FALSE by default) |
243-
| `z_margin_for_ground_removal` | double | Z-value margin for removal ground points |
234+
{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json") }}
244235

245236
## 2D real-time covariance estimation
246237

@@ -259,8 +250,4 @@ Note that this function may spoil healthy system behavior if it consumes much ca
259250
initial_pose_offset_model is rotated around (x,y) = (0,0) in the direction of the first principal component of the Hessian matrix.
260251
initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.
261252

262-
| Name | Type | Description |
263-
| ----------------------------- | ------------------- | ----------------------------------------------------------------- |
264-
| `use_covariance_estimation` | bool | Flag for using real-time covariance estimation (FALSE by default) |
265-
| `initial_pose_offset_model_x` | std::vector<double> | X-axis offset [m] |
266-
| `initial_pose_offset_model_y` | std::vector<double> | Y-axis offset [m] |
253+
{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json") }}

‎localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json

+44
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"type": "object",
4+
"properties": {
5+
"/**": {
6+
"type": "object",
7+
"properties": {
8+
"ros__parameters": {
9+
"type": "object",
10+
"properties": {
11+
"frame": { "$ref": "sub/frame.json#/definitions/frame" },
12+
"ndt": { "$ref": "sub/ndt.json#/definitions/ndt" },
13+
"regularization": { "$ref": "ndt_regularization.json#/definitions/ndt/regularization" },
14+
"initial_pose_estimation": {
15+
"$ref": "sub/initial_pose_estimation.json#/definitions/initial_pose_estimation"
16+
},
17+
"validation": { "$ref": "sub/validation.json#/definitions/validation" },
18+
"score_estimation": {
19+
"$ref": "sub/score_estimation.json#/definitions/score_estimation"
20+
},
21+
"covariance": { "$ref": "sub/covariance.json#/definitions/covariance" },
22+
"dynamic_map_loading": {
23+
"$ref": "sub/dynamic_map_loading.json#/definitions/dynamic_map_loading"
24+
}
25+
},
26+
"required": [
27+
"frame",
28+
"ndt",
29+
"initial_pose_estimation",
30+
"validation",
31+
"score_estimation",
32+
"covariance",
33+
"dynamic_map_loading"
34+
],
35+
"additionalProperties": false
36+
}
37+
},
38+
"required": ["ros__parameters"],
39+
"additionalProperties": false
40+
}
41+
},
42+
"required": ["/**"],
43+
"additionalProperties": false
44+
}

‎localization/ndt_scan_matcher/schema/sub/covariance.json

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "Parameters for Ndt Scan Matcher Node",
4+
"definitions": {
5+
"covariance": {
6+
"type": "object",
7+
"properties": {
8+
"output_pose_covariance": {
9+
"type": "array",
10+
"description": "The covariance of output pose. Note that this covariance matrix is empirically derived.",
11+
"default": [
12+
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,
13+
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,
14+
0.0, 0.0, 0.0, 0.0, 0.000625
15+
]
16+
},
17+
"covariance_estimation": {
18+
"$ref": "covariance_covariance_estimation.json#/definitions/covariance_estimation"
19+
}
20+
},
21+
"required": ["output_pose_covariance", "covariance_estimation"],
22+
"additionalProperties": false
23+
}
24+
}
25+
}

‎localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json

+28
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "Parameters for Ndt Scan Matcher Node",
4+
"definitions": {
5+
"covariance_estimation": {
6+
"type": "object",
7+
"properties": {
8+
"enable": {
9+
"type": "boolean",
10+
"description": "2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value).",
11+
"default": false
12+
},
13+
"initial_pose_offset_model_x": {
14+
"type": "array",
15+
"description": "Offset arrangement in covariance estimation [m]. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.",
16+
"default": [0.0, 0.0, 0.5, -0.5, 1.0, -1.0]
17+
},
18+
"initial_pose_offset_model_y": {
19+
"type": "array",
20+
"description": "Offset arrangement in covariance estimation [m]. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.",
21+
"default": [0.5, -0.5, 0.0, 0.0, 0.0, 0.0]
22+
}
23+
},
24+
"required": ["enable", "initial_pose_offset_model_x", "initial_pose_offset_model_y"],
25+
"additionalProperties": false
26+
}
27+
}
28+
}

‎localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json

+31
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "Parameters for Ndt Scan Matcher Node",
4+
"definitions": {
5+
"dynamic_map_loading": {
6+
"type": "object",
7+
"properties": {
8+
"update_distance": {
9+
"type": "number",
10+
"description": "Dynamic map loading distance.",
11+
"default": 20.0,
12+
"minimum": 0.0
13+
},
14+
"map_radius": {
15+
"type": "number",
16+
"description": "Dynamic map loading loading radius.",
17+
"default": 150.0,
18+
"minimum": 0.0
19+
},
20+
"lidar_radius": {
21+
"type": "number",
22+
"description": "Radius of input LiDAR range (used for diagnostics of dynamic map loading).",
23+
"default": 100.0,
24+
"minimum": 0.0
25+
}
26+
},
27+
"required": ["update_distance", "map_radius", "lidar_radius"],
28+
"additionalProperties": false
29+
}
30+
}
31+
}

‎localization/ndt_scan_matcher/schema/sub/frame.json

+28
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "Parameters for Ndt Scan Matcher Node",
4+
"definitions": {
5+
"frame": {
6+
"type": "object",
7+
"properties": {
8+
"base_frame": {
9+
"type": "string",
10+
"description": "Vehicle reference frame.",
11+
"default": "base_link"
12+
},
13+
"ndt_base_frame": {
14+
"type": "string",
15+
"description": "NDT reference frame.",
16+
"default": "ndt_base_link"
17+
},
18+
"map_frame": {
19+
"type": "string",
20+
"description": "Map frame.",
21+
"default": "map"
22+
}
23+
},
24+
"required": ["base_frame", "ndt_base_frame", "map_frame"],
25+
"additionalProperties": false
26+
}
27+
}
28+
}

‎localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "Parameters for Ndt Scan Matcher Node",
4+
"definitions": {
5+
"initial_pose_estimation": {
6+
"type": "object",
7+
"properties": {
8+
"particles_num": {
9+
"type": "number",
10+
"description": "The number of particles to estimate initial pose.",
11+
"default": 200,
12+
"minimum": 1
13+
},
14+
"n_startup_trials": {
15+
"type": "number",
16+
"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.",
17+
"default": 20,
18+
"minimum": 1
19+
}
20+
},
21+
"required": ["particles_num", "n_startup_trials"],
22+
"additionalProperties": false
23+
}
24+
}
25+
}

‎localization/ndt_scan_matcher/schema/sub/ndt.json

+53
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "Parameters for Ndt Scan Matcher Node",
4+
"definitions": {
5+
"ndt": {
6+
"type": "object",
7+
"properties": {
8+
"trans_epsilon": {
9+
"type": "number",
10+
"description": "The maximum difference between two consecutive transformations in order to consider convergence.",
11+
"default": 0.01,
12+
"minimum": 0.0
13+
},
14+
"step_size": {
15+
"type": "number",
16+
"description": "The newton line search maximum step length.",
17+
"default": 0.1,
18+
"minimum": 0.0
19+
},
20+
"resolution": {
21+
"type": "number",
22+
"description": "The ND voxel grid resolution.",
23+
"default": 2.0,
24+
"minimum": 0.0
25+
},
26+
"max_iterations": {
27+
"type": "number",
28+
"description": "The number of iterations required to calculate alignment.",
29+
"default": 30,
30+
"minimum": 1
31+
},
32+
"num_threads": {
33+
"type": "number",
34+
"description": "Number of threads used for parallel computing.",
35+
"default": 4,
36+
"minimum": 1
37+
},
38+
"regularization": {
39+
"$ref": "ndt_regularization.json#/definitions/regularization"
40+
}
41+
},
42+
"required": [
43+
"trans_epsilon",
44+
"step_size",
45+
"resolution",
46+
"max_iterations",
47+
"num_threads",
48+
"regularization"
49+
],
50+
"additionalProperties": false
51+
}
52+
}
53+
}

‎localization/ndt_scan_matcher/schema/sub/ndt_regularization.json

+24
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "Parameters for Ndt Scan Matcher Node",
4+
"definitions": {
5+
"regularization": {
6+
"type": "object",
7+
"properties": {
8+
"enable": {
9+
"type": "boolean",
10+
"description": "Regularization switch.",
11+
"default": false
12+
},
13+
"scale_factor": {
14+
"type": "number",
15+
"description": "Regularization scale factor.",
16+
"default": 0.01,
17+
"minimum": 0.0
18+
}
19+
},
20+
"required": ["enable", "scale_factor"],
21+
"additionalProperties": false
22+
}
23+
}
24+
}

0 commit comments

Comments
 (0)