autoware_ndt_scan_matcher

Purpose

autoware_ndt_scan_matcher is a package for position estimation using the NDT scan matching method.

There are two main functions in this package:

• estimate position by scan matching
• estimate initial position via the ROS service using the Monte Carlo method

One optional function is regularization. Please see the regularization chapter in the back for details. It is disabled by default.

Inputs / Outputs

Input

sensing/gnss/pose_with_covariance is required only when regularization is enabled.

Output

Service

Parameters

Core Parameters

Frame

{{ json_to_markdown(“localization/autoware_ndt_scan_matcher/schema/sub/frame.json”) }}

Sensor Points

{{ json_to_markdown(“localization/autoware_ndt_scan_matcher/schema/sub/sensor_points.json”) }}

Ndt

{{ json_to_markdown(“localization/autoware_ndt_scan_matcher/schema/sub/ndt.json”) }}

Initial Pose Estimation

{{ json_to_markdown(“localization/autoware_ndt_scan_matcher/schema/sub/initial_pose_estimation.json”) }}

Validation

{{ json_to_markdown(“localization/autoware_ndt_scan_matcher/schema/sub/validation.json”) }}

Score Estimation

{{ json_to_markdown(“localization/autoware_ndt_scan_matcher/schema/sub/score_estimation.json”) }}

Covariance

{{ json_to_markdown(“localization/autoware_ndt_scan_matcher/schema/sub/covariance.json”) }}

Regularization

Abstract

This is a function that adds the regularization term to the NDT optimization problem as follows.

$$ \begin{align} \min_{\mathbf{R},\mathbf{t}} \mathrm{NDT}(\mathbf{R},\mathbf{t}) +\mathrm{scale\ factor}\cdot \left| \mathbf{R}^\top (\mathbf{t_{base}-\mathbf{t}}) \cdot

File truncated at 100 lines see the full file

Changelog for package autoware_ndt_scan_matcher

1.9.0 (2026-06-24)

• Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base

• test(autoware_ndt_scan_matcher): unit-test covariance/oscillation math and fix no-ground hot loop (#1108)

  * test(autoware_ndt_scan_matcher): unit-test covariance/oscillation math and fix no-ground hot loop Extract the translation-unit-local rotate_covariance free function and the count_oscillation logic into a testable internal seam (ndt_scan_matcher_helper.{hpp,cpp}) and add two fast, dependency-light ament_auto_add_gtest targets:

  - test_estimate_covariance covers the pure Eigen math in ndt_omp/estimate_covariance.cpp (calc_weight_vec, calculate_weighted_mean_and_cov, propose_poses_to_search, estimate_xy_covariance_by_laplace_approximation, rotate_covariance_to_map/base_link round-trip, adjust_diagonal_covariance) with hand-computed expected values.

  - test_ndt_scan_matcher_helper covers rotate_covariance (36-element covariance block rotation) and count_oscillation (consecutive-inversion counting). These functions previously had zero unit tests and were only exercised by the heavyweight 300s launch/integration tests. Behavior-preserving cleanups:

  - Hoist the constant matrix4f_to_pose(ndt_result.pose).position.z out of the per-point no-ground-points removal loop (it equals ndt_result.pose(2,3)), avoiding a full Pose + quaternion extraction per LiDAR point, and reserve() the output cloud. The NDTScanMatcher::count_oscillation private static member is retained and now delegates to the free function, so no public API changes.

  - Remove the dead, never-read Eigen::Matrix4f base_to_sensor_matrix_ member (transform_sensor_measurement uses a local of the same name). Refs: autowarefoundation/autoware_core#1096

  * fix(autoware_ndt_scan_matcher): guard count_oscillation against zero-length steps and make helper header internal count_oscillation normalized motion vectors without guarding against zero-length steps (e.g. repeated/identical poses). Eigen normalized() on a zero vector yields NaNs, making cosine_value NaN and the oscillation logic unreliable. Skip zero-length steps and treat them as non-oscillations, resetting the consecutive-inversion count. Add unit tests pinning the all-identical and interleaved zero-length cases. Move ndt_scan_matcher_helper.hpp out of include/autoware/... into src/ so the helper stays an internal seam rather than part of the installed public header/ABI surface (USE_SCOPED_HEADER_INSTALL_DIR no longer installs it). Includes now use the project-local quoted form and the unit test includes it via ../src, matching the existing private-header convention in this repo. Refs: autowarefoundation/autoware_core#1096

  * test(autoware_ndt_scan_matcher): assert rotation covariance against independent oracle (#81) Replace the mirrored rot * cov * rot.transpose() expected value with an explicit stdlib scalar expansion of R * cov * R\^T, asserted per-entry via EXPECT_NEAR, so a transpose/sign error in rotate_covariance_to_map is caught instead of mirrored. Retain the round-trip equality as a secondary property check. Refs: autowarefoundation/autoware_core#1096 ---------

• fix(ndt_scan_matcher): explicitly include omp.h (#1166)

  • fix(ndt_scan_matcher): explicitly include omp.h

  * style(pre-commit): autofix ---------Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

• feat(autoware_ndt_scan_matcher): publish all map points if publisher has subscribers (#995)

  • publish all map points if publisher has subscribers
  • style(pre-commit): autofix
  • fix error
  • add parameter
  • fix param name and add json
  • add loaded map clear on ndt ptr reset
  • reserve before adding points
  • add new param to autoware_core_lozalization

  * Update localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp Co-authored-by: Mete Fatih Cırıt <<mfc@autoware.org>> ---------Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Mete Fatih Cırıt <<mfc@autoware.org>>

• fix(autoware_ndt_scan_matcher): zero div if particles_num < 20

File truncated at 100 lines see the full file