-
Notifications
You must be signed in to change notification settings - Fork 672
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
refactor(ndt_scan_matcher): rework parameters
Signed-off-by: Yamato Ando <[email protected]>
- Loading branch information
1 parent
47f265d
commit c470440
Showing
13 changed files
with
637 additions
and
40 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
38 changes: 38 additions & 0 deletions
38
localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,38 @@ | ||
| { | ||
| "$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 | ||
| } |
232 changes: 232 additions & 0 deletions
232
localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json.tmp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,232 @@ | ||
| { | ||
| "$schema": "http://json-schema.org/draft-07/schema#", | ||
| "title": "Parameters for Ndt Scan Matcher Node", | ||
| "type": "object", | ||
| "definitions": { | ||
| "ndt_scan_matcher": { | ||
| "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" | ||
| }, | ||
| "input_sensor_points_queue_size": { | ||
| "type": "number", | ||
| "description": "Subscriber queue size.", | ||
| "default": 1, | ||
| "minimum": 1 | ||
| }, | ||
| "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 | ||
| }, | ||
| "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 | ||
| }, | ||
| "initial_estimate_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 | ||
| }, | ||
| "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 | ||
| }, | ||
| "num_threads": { | ||
| "type": "number", | ||
| "description": "Number of threads used for parallel computing.", | ||
| "default": 4, | ||
| "minimum": 1 | ||
| }, | ||
| "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 | ||
| ] | ||
| }, | ||
| "use_covariance_estimation": { | ||
| "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] | ||
| }, | ||
| "regularization_enabled": { | ||
| "type": "boolean", | ||
| "description": "Regularization switch.", | ||
| "default": false | ||
| }, | ||
| "regularization_scale_factor": { | ||
| "type": "number", | ||
| "description": "Regularization scale factor.", | ||
| "default": 0.01, | ||
| "minimum": 0.0 | ||
| }, | ||
| "dynamic_map_loading_update_distance": { | ||
| "type": "number", | ||
| "description": "Dynamic map loading distance.", | ||
| "default": 20.0, | ||
| "minimum": 0.0 | ||
| }, | ||
| "dynamic_map_loading_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 | ||
| }, | ||
| "estimate_scores_for_degrounded_scan": { | ||
| "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": [ | ||
| "base_frame", | ||
| "ndt_base_frame", | ||
| "map_frame", | ||
| "input_sensor_points_queue_size", | ||
| "trans_epsilon", | ||
| "step_size", | ||
| "resolution", | ||
| "max_iterations", | ||
| "converged_param_type", | ||
| "converged_param_transform_probability", | ||
| "converged_param_nearest_voxel_transformation_likelihood", | ||
| "initial_estimate_particles_num", | ||
| "n_startup_trials", | ||
| "lidar_topic_timeout_sec", | ||
| "initial_pose_timeout_sec", | ||
| "initial_pose_distance_tolerance_m", | ||
| "critical_upper_bound_exe_time_ms", | ||
| "num_threads", | ||
| "output_pose_covariance", | ||
| "use_covariance_estimation", | ||
| "initial_pose_offset_model_x", | ||
| "initial_pose_offset_model_y", | ||
| "regularization_enabled", | ||
| "regularization_scale_factor", | ||
| "dynamic_map_loading_update_distance", | ||
| "dynamic_map_loading_map_radius", | ||
| "lidar_radius", | ||
| "estimate_scores_for_degrounded_scan", | ||
| "z_margin_for_ground_removal" | ||
| ], | ||
| "additionalProperties": false | ||
| } | ||
| }, | ||
| "properties": { | ||
| "/**": { | ||
| "type": "object", | ||
| "properties": { | ||
| "ros__parameters": { | ||
| "$ref": "#/definitions/ndt_scan_matcher" | ||
| } | ||
| }, | ||
| "required": ["ros__parameters"], | ||
| "additionalProperties": false | ||
| } | ||
| }, | ||
| "required": ["/**"], | ||
| "additionalProperties": false | ||
| } | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,31 @@ | ||
| { | ||
| "$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 | ||
| } | ||
| } | ||
| } |
Oops, something went wrong.