Fix non-deterministic lidar odometry

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)

core/include/ndt.h

@@ -35,30 +35,30 @@ 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<uint64_t> point_indexes;
         bool valid = false;
     };
 
     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<uint64_t, BucketCoef> buckets;
     };