Skip to content

Commit

Permalink
refactor(ndt_scan_matcher): delete default parameters from ndt_scan_m…
Browse files Browse the repository at this point in the history 
…atcher_core.cpp (autowarefoundation#5155)

* Removed default paramters in the constructor in ndt_scan_matcher_core.cpp
Extracted paramter map_frame to ndt_scan_matcher.param.yaml

Signed-off-by: TaikiYamada4 <[email protected]>

* Updated README.md to match ndt_scan_matcher.param.yaml
Fixed minor grammar mistakes.

Signed-off-by: TaikiYamada4 <[email protected]>

* style(pre-commit): autofix

* style(pre-commit): autofix

* Fixed minor grammar mistakes

Signed-off-by: TaikiYamada4 <[email protected]>

* Removed unnecessary value initialization for parameters that should be defined in ndt_scan_matcher.param.yaml

Signed-off-by: TaikiYamada4 <[email protected]>

---------

Signed-off-by: TaikiYamada4 <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@TaikiYamada4 @pre-commit-ci @shmpwk
2 people authored and shmpwk committed Nov 16, 2023
1 parent db0987a commit dfa86dd
Show file tree
Hide file tree
Showing 3 changed files with 40 additions and 37 deletions.
34 changes: 20 additions & 14 deletions localization/ndt_scan_matcher/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,19 +54,25 @@ 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 |
| `input_sensor_points_queue_size` | int | Subscriber queue size |
| `trans_epsilon` | double | The maximum difference between two consecutive transformations in order 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 | Threshold for deciding whether to trust the estimation result |
| `lidar_topic_timeout_sec` | double | Tolerance of timestamp difference between current time and sensor pointcloud |
| `num_threads` | int | Number of threads used for parallel computing |
| 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 |
| `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)

Expand Down Expand Up @@ -238,7 +244,7 @@ Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample

### Abstract

This is a function that using de-grounded LiDAR scan estimate scan matching score.This score can more accurately reflect the current localization performance.
This is a function that uses de-grounded LiDAR scan to estimate the scan matching score. This score can reflect the current localization performance more accurately.
[related issue](https://github.com/autowarefoundation/autoware.universe/issues/2044).

### Parameters
Expand Down
11 changes: 7 additions & 4 deletions localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,9 @@
# NDT reference frame
ndt_base_frame: "ndt_base_link"

# map frame
map_frame: "map"

# Subscriber queue size
input_sensor_points_queue_size: 1

Expand Down Expand Up @@ -53,7 +56,7 @@
num_threads: 4

# The covariance of output pose
# Do note that this covariance matrix is empirically derived
# Note that this covariance matrix is empirically derived
output_pose_covariance:
[
0.0225, 0.0, 0.0, 0.0, 0.0, 0.0,
Expand All @@ -67,7 +70,7 @@
# Regularization switch
regularization_enabled: false

# regularization scale factor
# Regularization scale factor
regularization_scale_factor: 0.01

# Dynamic map loading distance
Expand All @@ -86,5 +89,5 @@
# If lidar_point.z - base_link.z <= this threshold , the point will be removed
z_margin_for_ground_removal: 0.8

# The execution time which means probably NDT cannot matches scans properly
critical_upper_bound_exe_time_ms: 100 # [ms]
# The execution time which means probably NDT cannot matches scans properly. [ms]
critical_upper_bound_exe_time_ms: 100
32 changes: 13 additions & 19 deletions localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,24 +84,9 @@ NDTScanMatcher::NDTScanMatcher()
tf2_broadcaster_(*this),
ndt_ptr_(new NormalDistributionsTransform),
state_ptr_(new std::map<std::string, std::string>),
base_frame_("base_link"),
ndt_base_frame_("ndt_base_link"),
map_frame_("map"),
converged_param_type_(ConvergedParamType::TRANSFORM_PROBABILITY),
converged_param_transform_probability_(4.5),
converged_param_nearest_voxel_transformation_likelihood_(2.3),
initial_estimate_particles_num_(100),
lidar_topic_timeout_sec_(1.0),
initial_pose_timeout_sec_(1.0),
initial_pose_distance_tolerance_m_(10.0),
inversion_vector_threshold_(-0.9),
oscillation_threshold_(10),
output_pose_covariance_(),
regularization_enabled_(declare_parameter<bool>("regularization_enabled")),
estimate_scores_for_degrounded_scan_(
declare_parameter("estimate_scores_for_degrounded_scan", false)),
z_margin_for_ground_removal_(declare_parameter("z_margin_for_ground_removal", 0.8)),
critical_upper_bound_exe_time_ms_(100)
inversion_vector_threshold_(-0.9), // Not necessary to extract to ndt_scan_matcher.param.yaml
oscillation_threshold_(10), // Not necessary to extract to ndt_scan_matcher.param.yaml
regularization_enabled_(declare_parameter<bool>("regularization_enabled"))
{
(*state_ptr_)["state"] = "Initializing";
is_activated_ = false;
Expand All @@ -116,6 +101,9 @@ NDTScanMatcher::NDTScanMatcher()
ndt_base_frame_ = this->declare_parameter<std::string>("ndt_base_frame");
RCLCPP_INFO(get_logger(), "ndt_base_frame_id: %s", ndt_base_frame_.c_str());

map_frame_ = this->declare_parameter<std::string>("map_frame");
RCLCPP_INFO(get_logger(), "map_frame_id: %s", map_frame_.c_str());

pclomp::NdtParams ndt_params{};
ndt_params.trans_epsilon = this->declare_parameter<double>("trans_epsilon");
ndt_params.step_size = this->declare_parameter<double>("step_size");
Expand All @@ -141,8 +129,9 @@ NDTScanMatcher::NDTScanMatcher()
this->declare_parameter<double>("converged_param_nearest_voxel_transformation_likelihood");

lidar_topic_timeout_sec_ = this->declare_parameter<double>("lidar_topic_timeout_sec");

critical_upper_bound_exe_time_ms_ =
this->declare_parameter("critical_upper_bound_exe_time_ms", critical_upper_bound_exe_time_ms_);
this->declare_parameter<int>("critical_upper_bound_exe_time_ms");

initial_pose_timeout_sec_ =
this->declare_parameter("initial_pose_timeout_sec", initial_pose_timeout_sec_);
Expand All @@ -160,6 +149,11 @@ NDTScanMatcher::NDTScanMatcher()

initial_estimate_particles_num_ = this->declare_parameter<int>("initial_estimate_particles_num");

estimate_scores_for_degrounded_scan_ =
this->declare_parameter<bool>("estimate_scores_for_degrounded_scan");

z_margin_for_ground_removal_ = this->declare_parameter<double>("z_margin_for_ground_removal");

rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group;
initial_pose_callback_group =
this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
Expand Down

0 comments on commit dfa86dd

Please sign in to comment.