From 6cd68cfa98bf615086749aebc11fb435f12178d5 Mon Sep 17 00:00:00 2001 From: Danil An Date: Fri, 20 Feb 2026 17:14:27 +0400 Subject: [PATCH] Zero-initialize NDT bucket structs to fix single-threaded non-determinism --- .../lidar_odometry_utils_optimizers.cpp | 4 +++ core/include/ndt.h | 26 +++++++++---------- 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp index c49f367c..7ccc926d 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -1466,6 +1466,8 @@ static void add_indoor_hessian_contribution( if ((infm.array() > threshold).any() || (infm.array() < -threshold).any()) return; + // Buckets with < 3 points have normal_vector == Zero (default-initialized), + // so dot product is 0 and this check passes - they still contribute to the Hessian. if (ablation_study_use_view_point_and_normal_vectors) { if (indoor_bucket.normal_vector.dot(viewport - indoor_bucket.mean) < 0) @@ -1572,6 +1574,8 @@ static void add_outdoor_hessian_contribution( if ((infm.array() > threshold).any() || (infm.array() < -threshold).any()) return; + // Buckets with < 3 points have normal_vector == Zero (default-initialized), + // so dot product is 0 and this check passes - they still contribute to the Hessian. if (ablation_study_use_view_point_and_normal_vectors) { if (outdoor_bucket.normal_vector.dot(viewport - outdoor_bucket.mean) < 0) diff --git a/core/include/ndt.h b/core/include/ndt.h index e34c88b6..4a11b207 100644 --- a/core/include/ndt.h +++ b/core/include/ndt.h @@ -35,21 +35,21 @@ class NDT struct Bucket { - Eigen::Matrix3d cov; - Eigen::Matrix3d cov_inverse; // precomputed inverse of cov, updated in update_rgd - Eigen::Vector3d mean; - Eigen::Vector3d normal_vector; - - uint64_t number_of_points; - uint64_t index_begin; - uint64_t index_end; + Eigen::Matrix3d cov = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d cov_inverse = Eigen::Matrix3d::Zero(); // precomputed inverse of cov, updated in update_rgd + Eigen::Vector3d mean = Eigen::Vector3d::Zero(); + Eigen::Vector3d normal_vector = Eigen::Vector3d::Zero(); + + uint64_t number_of_points = 0; + uint64_t index_begin = 0; + uint64_t index_end = 0; }; struct BucketCoef { - Eigen::Vector3d mean; - Eigen::Matrix3d cov; - Eigen::Vector3d normal_vector; + Eigen::Vector3d mean = Eigen::Vector3d::Zero(); + Eigen::Matrix3d cov = Eigen::Matrix3d::Zero(); + Eigen::Vector3d normal_vector = Eigen::Vector3d::Zero(); std::vector point_indexes; bool valid = false; }; @@ -57,8 +57,8 @@ class NDT struct Bucket2 { int classification = 0; // 1 - ceiling, 2 - floor - uint64_t index_begin_inclusive; - uint64_t index_end_exclusive; + uint64_t index_begin_inclusive = 0; + uint64_t index_end_exclusive = 0; // uint64_t number_of_points; std::unordered_map buckets; };