diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md
index a44db7bbaa4bf..8049104e8f2f5 100644
--- a/localization/ndt_scan_matcher/README.md
+++ b/localization/ndt_scan_matcher/README.md
@@ -262,23 +262,27 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num
-| Name | Description | Transition condition to Warning | Transition condition to Error | Whether to reject the estimation result (affects `skipping_publish_num`) |
-| ----------------------------------------- | -------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ----------------------------- | --------------------------------------------------------------------------------------------------- |
-| `topic_time_stamp` | the time stamp of input topic | none | none | no |
-| `sensor_points_size` | the size of sensor points | the size is 0 | none | yes |
-| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `validation.lidar_topic_timeout_sec` | none | yes |
-| `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | failed | yes |
-| `sensor_points_max_distance` | the max distance of sensor points | the max distance is **shorter** than `sensor_points.required_distance` | none | yes |
-| `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | if `is_activated` is false, then estimation is not executed and `skipping_publish_num` is set to 0. |
-| `is_succeed_interpolate_initial_pose` | whether the interpolate of initial pose is succeed or not | failed.
(1) the size of `initial_pose_buffer_` is **smaller** than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`.
(3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes |
-| `is_set_map_points` | whether the map points is set or not | not set | none | yes |
-| `iteration_num` | the number of times calculate alignment | the number of times is **larger** than `ndt.max_iterations` | none | yes |
-| `local_optimal_solution_oscillation_num` | the number of times the solution is judged to be oscillating | the number of times is **larger** than 10 | none | yes |
-| `transform_probability` | the score of how well the map aligns with the sensor points | the score is **smaller** than`score_estimation.converged_param_transform_probability` (only in the case of `score_estimation.converged_param_type` is 0=TRANSFORM_PROBABILITY) | none | yes |
-| `nearest_voxel_transformation_likelihood` | the score of how well the map aligns with the sensor points | the score is **smaller** than `score_estimation.converged_param_nearest_voxel_transformation_likelihood` (only in the case of `score_estimation.converged_param_type` is 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) | none | yes |
-| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | none | no |
-| `execution_time` | the time for convergence processing | the time is **longer** than `validation.critical_upper_bound_exe_time_ms` | none | no |
-| `skipping_publish_num` | the number of times rejected estimation results consecutively | the number of times is 5 or more | none | - |
+| Name | Description | Transition condition to Warning | Transition condition to Error | Whether to reject the estimation result (affects `skipping_publish_num`) |
+| ------------------------------------------------ | -------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ----------------------------- | --------------------------------------------------------------------------------------------------- |
+| `topic_time_stamp` | the time stamp of input topic | none | none | no |
+| `sensor_points_size` | the size of sensor points | the size is 0 | none | yes |
+| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `validation.lidar_topic_timeout_sec` | none | yes |
+| `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | failed | yes |
+| `sensor_points_max_distance` | the max distance of sensor points | the max distance is **shorter** than `sensor_points.required_distance` | none | yes |
+| `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | if `is_activated` is false, then estimation is not executed and `skipping_publish_num` is set to 0. |
+| `is_succeed_interpolate_initial_pose` | whether the interpolate of initial pose is succeed or not | failed.
(1) the size of `initial_pose_buffer_` is **smaller** than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`.
(3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes |
+| `is_set_map_points` | whether the map points is set or not | not set | none | yes |
+| `iteration_num` | the number of times calculate alignment | the number of times is **larger** than `ndt.max_iterations` | none | yes |
+| `local_optimal_solution_oscillation_num` | the number of times the solution is judged to be oscillating | the number of times is **larger** than 10 | none | yes |
+| `transform_probability` | the score of how well the map aligns with the sensor points | the score is **smaller** than`score_estimation.converged_param_transform_probability` (only in the case of `score_estimation.converged_param_type` is 0=TRANSFORM_PROBABILITY) | none | yes |
+| `transform_probability_diff` | the tp score difference for the current ndt optimization | none | none | no |
+| `transform_probability_before` | the tp score before the current ndt optimization | none | none | no |
+| `nearest_voxel_transformation_likelihood` | the score of how well the map aligns with the sensor points | the score is **smaller** than `score_estimation.converged_param_nearest_voxel_transformation_likelihood` (only in the case of `score_estimation.converged_param_type` is 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) | none | yes |
+| `nearest_voxel_transformation_likelihood_diff` | the nvtl score difference for the current ndt optimization | none | none | no |
+| `nearest_voxel_transformation_likelihood_before` | the nvtl score before the current ndt optimization | none | none | no |
+| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | none | no |
+| `execution_time` | the time for convergence processing | the time is **longer** than `validation.critical_upper_bound_exe_time_ms` | none | no |
+| `skipping_publish_num` | the number of times rejected estimation results consecutively | the number of times is 5 or more | none | - |
※The `sensor_points_callback` shares the same callback group as the `trigger_node_service` and `ndt_align_service`. Consequently, if the initial pose estimation takes too long, this diagnostic may become stale.
diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
index 8acfe3bd5c1ca..349f924019c28 100644
--- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
+++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -489,6 +489,38 @@ bool NDTScanMatcher::callback_sensor_points_main(
return false;
}
+ // check score diff
+ const std::vector & tp_array = ndt_result.transform_probability_array;
+ if (static_cast(tp_array.size()) != ndt_result.iteration_num + 1) {
+ // only publish warning to /diagnostics, not skip publishing pose
+ std::stringstream message;
+ message << "transform_probability_array size is not equal to iteration_num + 1."
+ << " transform_probability_array size: " << tp_array.size()
+ << ", iteration_num: " << ndt_result.iteration_num;
+ diagnostics_scan_points_->updateLevelAndMessage(
+ diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
+ } else {
+ const float diff = tp_array.back() - tp_array.front();
+ diagnostics_scan_points_->addKeyValue("transform_probability_diff", diff);
+ diagnostics_scan_points_->addKeyValue("transform_probability_before", tp_array.front());
+ }
+ const std::vector & nvtl_array = ndt_result.nearest_voxel_transformation_likelihood_array;
+ if (static_cast(nvtl_array.size()) != ndt_result.iteration_num + 1) {
+ // only publish warning to /diagnostics, not skip publishing pose
+ std::stringstream message;
+ message
+ << "nearest_voxel_transformation_likelihood_array size is not equal to iteration_num + 1."
+ << " nearest_voxel_transformation_likelihood_array size: " << nvtl_array.size()
+ << ", iteration_num: " << ndt_result.iteration_num;
+ diagnostics_scan_points_->updateLevelAndMessage(
+ diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
+ } else {
+ const float diff = nvtl_array.back() - nvtl_array.front();
+ diagnostics_scan_points_->addKeyValue("nearest_voxel_transformation_likelihood_diff", diff);
+ diagnostics_scan_points_->addKeyValue(
+ "nearest_voxel_transformation_likelihood_before", nvtl_array.front());
+ }
+
bool is_ok_score = (score > score_threshold);
if (!is_ok_score) {
std::stringstream message;