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.8.0 (2026-05-01)

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

• fix(ndt_scan_matcher): fix concurrency bugs caused by missing locks by introducing Guarded class (#1030)

  • Removed [setInputSource]{.title-ref}
  • Applied [pre-commit run -a]{.title-ref}
  • Corrected according to cppcheck's indications
  • bug: fix wrongly remaining [getInputSource]{.title-ref} (see below)
  • This bug is caused due to my previous conflict resolve

  * fix(agnocastlib): fix missing lock on latest_ekf_position_ and add Guarded class The [latest_ekf_position_]{.title-ref} field in [NDTScanMatcher]{.title-ref} was intended to be protected by [latest_ekf_position_mutex_]{.title-ref}, but a concurrency bug existed due to missing lock acquisitions. To resolve this issue, this commit introduces a new [Guarded]{.title-ref} class. This class encapsulates both the mutex and the data it protects into a single entity. By enforcing lock acquisition whenever the internal data is accessed, it systematically prevents bugs caused by forgetting to acquire locks.

  • refactor(ndt_scan_matcher): apply Guarded class to last_update_position_

  * fix(ndt_scan_matcher): fix missing lock bugs by wrapping ndt_ptr_ with Guarded Refactored [ndt_ptr_]{.title-ref} and [ndt_ptr_mtx_]{.title-ref} in [NDTScanMatcher]{.title-ref} using [Guarded]{.title-ref} to fix missing lock bugs. This change was also applied to [MapUpdateModule]{.title-ref}. Due to this change, some member functions were updated to take [NDT]{.title-ref} as an argument. Whenever possible, these functions now take a reference to [NDT]{.title-ref} instead of a [shared_ptr]{.title-ref}. This is a best practice to prevent the pointer from leaking outside of [Guarded::with()]{.title-ref}. Note: [MapUpdateModule::secondary_ndt_ptr_]{.title-ref} also has a missing lock bug. This will be fixed in a future commit and is not covered here.

  * refactor(ndt_scan_matcher): pass NDT by reference instead of std::shared_ptr When a function takes [std::shared_ptr<T>]{.title-ref} as an argument, there should be a specific reason to use a pointer (such as sharing ownership). However, functions like [estimate_xy_covariance_by_laplace_approximation()]{.title-ref} do not actually need these characteristics internally. By changing the arguments to take a reference instead, we reduce the risk of leaking pointers outside unnecessarily. Although this commit is not a bug fix, it is important as an example of a best practice when using `Guarded`: preventing internal data from leaking outside as much as possible.

  • refactor(ndt_scan_matcher): pre-commit

  * fix(ndt_scan_matcher): fix concurrency bug in secondary_ndt_ptr_ using Guarded The [secondary_ndt_ptr_]{.title-ref} field in [MapUpdateModule]{.title-ref} previously lacked lock protection. Since it can be accessed by multiple threads at the same time (from [NDTScanMatcher::service_ndt_align()]{.title-ref} and [NDTScanMatcher::callback_timer()]{.title-ref}), it needs to be protected. To fix this, we wrapped it with [Guarded]{.title-ref} to add a lock. Generally, this data is accessed from within a MutuallyExclusive timer callback. Because it is not accessed frequently by multiple threads, we decided to use a larger lock scope for the basic processing.

  * refactor(ndt_scan_matcher): move heavy map update operations outside the lock To prevent the NDT scan matcher from being blocked during dynamic map loading, this commit moves the cloning and destruction of the NDT map out of the [ndt_ptr_]{.title-ref} lock scope. By using only a fast [std::swap]{.title-ref} inside the lock, the align process is no longer delayed.

  * fix(ndt_scan_matcher): fix cppcheck errors ---------Co-authored-by: Shintaro Sakoda <<shintaro.sakoda@tier4.jp>> Co-authored-by: Junya Sasaki <<j2sasaki1990@gmail.com>> Co-authored-by: Junya Sasaki <<junya.sasaki@tier4.jp>>

• fix(ndt_scan_matcher): remove [setInputSource]{.title-ref} (#837)

  • Removed [setInputSource]{.title-ref}
  • Applied [pre-commit run -a]{.title-ref}
  • Corrected according to cppcheck's indications
  • bug: fix wrongly remaining [getInputSource]{.title-ref} (see below)

  * This bug is caused due to my previous conflict resolve ---------Co-authored-by: Junya Sasaki <<j2sasaki1990@gmail.com>> Co-authored-by: Junya Sasaki <<junya.sasaki@tier4.jp>>

File truncated at 100 lines see the full file