refactor(ndt_scan_matcher): rework parameters

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<double, 36> | 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<double> | X-axis offset [m]                                                 |
-| `initial_pose_offset_model_y` | std::vector<double> | Y-axis offset [m]                                                 |
+{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json") }}

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
+}

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
+    }
+  }
+}

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
+    }
+  }
+}

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
+    }
+  }
+}

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
+    }
+  }
+}

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
+    }
+  }
+}

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
+    }
+  }
+}

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
+    }
+  }
+}