Fix: Remove bucket linking to avoid regression in "Precise Forestry" preset

apps/lidar_odometry_step_1/lidar_odometry.cpp

@@ -1,5 +1,6 @@
 #include "lidar_odometry.h"
 #include "tbb/tbb.h"
+#include <UTL/profiler.hpp>
 #include <mutex>
 #include <system_info.hpp>
 
@@ -14,6 +15,7 @@ bool load_data(
     Imu& imu_data,
     bool debugMsg)
 {
+    UTL_PROFILER_SCOPE("load_data");
     std::sort(input_file_names.begin(), input_file_names.end());
 
     std::vector<std::string> csv_files, laz_files;
@@ -323,6 +325,7 @@ void calculate_trajectory(
     bool debugMsg,
     bool use_remove_imu_bias_from_first_stationary_scan)
 {
+    UTL_PROFILER_SCOPE("calculate_trajectory");
     FusionAhrs ahrs;
     FusionAhrsInitialise(&ahrs);
 
@@ -470,6 +473,7 @@ bool compute_step_1(
     std::vector<WorkerData>& worker_data,
     const std::atomic<bool>& pause)
 {
+    UTL_PROFILER_SCOPE("compute_step_1");
     int number_of_initial_points = 0;
     double timestamp_begin = 0.0;
 
@@ -706,6 +710,7 @@ void run_consistency(std::vector<WorkerData>& worker_data, const LidarOdometryPa
 
 void save_result(std::vector<WorkerData>& worker_data, LidarOdometryParams& params, fs::path outwd, double elapsed_time_s)
 {
+    UTL_PROFILER_SCOPE("save_result");
     std::filesystem::create_directory(outwd);
     // concatenate data
     std::vector<WorkerData> worker_data_concatenated;
@@ -1053,6 +1058,7 @@ std::string save_results_automatic(
 
 std::vector<WorkerData> run_lidar_odometry(const std::string& input_dir, LidarOdometryParams& params)
 {
+    UTL_PROFILER_SCOPE("run_lidar_odometry");
     Session session;
     std::vector<WorkerData> worker_data;
     std::vector<std::string> input_file_names;

apps/lidar_odometry_step_1/lidar_odometry_utils.cpp

@@ -137,21 +137,6 @@ void limit_covariance(Eigen::Matrix3d& io_cov)
     // std::cout << io_cov << std::endl;
 }
 
-// Check if ratio is close to an integer: |ratio - round(ratio)| < tolerance
-static bool is_close_to_integer(const double ratio, const double tolerance = 1e-6)
-{
-    return std::fabs(ratio - std::round(ratio)) < tolerance;
-}
-
-bool is_integer_bucket_ratio(const NDT::GridParameters& rgd_params_indoor, const NDT::GridParameters& rgd_params_outdoor)
-{
-    const double ratio_x = rgd_params_outdoor.resolution_X / rgd_params_indoor.resolution_X;
-    const double ratio_y = rgd_params_outdoor.resolution_Y / rgd_params_indoor.resolution_Y;
-    const double ratio_z = rgd_params_outdoor.resolution_Z / rgd_params_indoor.resolution_Z;
-
-    return is_close_to_integer(ratio_x) && is_close_to_integer(ratio_y) && is_close_to_integer(ratio_z);
-}
-
 void update_rgd(
     const NDT::GridParameters& rgd_params,
     NDTBucketMapType& buckets,
@@ -242,31 +227,6 @@ void update_rgd(
     }
 }
 
-void link_buckets_to_coarser(
-    const NDT::GridParameters& rgd_params_indoor,
-    NDTBucketMapType& buckets_indoor,
-    const NDT::GridParameters& rgd_params_outdoor,
-    const NDTBucketMapType& buckets_outdoor)
-{
-    if (!is_integer_bucket_ratio(rgd_params_indoor, rgd_params_outdoor))
-        return;
-
-    const Eigen::Vector3d bucket_size_outdoor{ rgd_params_outdoor.resolution_X,
-                                               rgd_params_outdoor.resolution_Y,
-                                               rgd_params_outdoor.resolution_Z };
-
-    for (auto& [key, bucket] : buckets_indoor)
-    {
-        // Skip if already linked
-        if (bucket.coarser_bucket != nullptr)
-            continue;
-
-        const auto outdoor_key = get_rgd_index_3d(bucket.mean, bucket_size_outdoor);
-        const auto outdoor_it = buckets_outdoor.find(outdoor_key);
-        bucket.coarser_bucket = (outdoor_it != buckets_outdoor.end()) ? &outdoor_it->second : nullptr;
-    }
-}
-
 void update_rgd_hierarchy(
     const NDT::GridParameters& rgd_params_indoor,
     NDTBucketMapType& buckets_indoor,
@@ -285,11 +245,6 @@ void update_rgd_hierarchy(
         {
             update_rgd(rgd_params_outdoor, buckets_outdoor, points_global, viewport, &stats.outdoor_lookups);
         });
-
-    const auto link_start = std::chrono::high_resolution_clock::now();
-    link_buckets_to_coarser(rgd_params_indoor, buckets_indoor, rgd_params_outdoor, buckets_outdoor);
-    const auto link_end = std::chrono::high_resolution_clock::now();
-    stats.link_time_seconds += std::chrono::duration<double>(link_end - link_start).count();
 }
 
 void update_rgd_spherical_coordinates(

apps/lidar_odometry_step_1/lidar_odometry_utils.h

@@ -32,9 +32,7 @@
 
 namespace fs = std::filesystem;
 
-// segmented_map provides pointer stability - pointers to values remain valid after insertions.
-// This is required for NDT::Bucket::coarser_bucket pointer optimization in link_buckets_to_coarser().
-using NDTBucketMapType = ankerl::unordered_dense::segmented_map<uint64_t, NDT::Bucket>;
+using NDTBucketMapType = ankerl::unordered_dense::map<uint64_t, NDT::Bucket>;
 using NDTBucketMapType2 = ankerl::unordered_dense::map<uint64_t, NDT::Bucket2>;
 
 // Helper function for getting software version from CMake macros
@@ -163,16 +161,6 @@ Eigen::Matrix4d getInterpolatedPose(const std::map<double, Eigen::Matrix4d>& tra
 // this function reduces number of points by preserving only first point for each bucket {bucket_x, bucket_y, bucket_z}
 std::vector<Point3Di> decimate(const std::vector<Point3Di>& points, double bucket_x, double bucket_y, double bucket_z);
 
-// Check if outdoor bucket size is integer multiple of indoor bucket size
-bool is_integer_bucket_ratio(const NDT::GridParameters& rgd_params_indoor, const NDT::GridParameters& rgd_params_outdoor);
-
-// Link indoor buckets to their corresponding outdoor buckets via coarser_bucket pointer
-void link_buckets_to_coarser(
-    const NDT::GridParameters& rgd_params_indoor,
-    NDTBucketMapType& buckets_indoor,
-    const NDT::GridParameters& rgd_params_outdoor,
-    const NDTBucketMapType& buckets_outdoor);
-
 // this function updates each bucket (mean value, covariance) in regular grid decomposition
 void update_rgd(
     const NDT::GridParameters& rgd_params,
@@ -186,8 +174,6 @@ struct LookupStats
 {
     size_t indoor_lookups = 0;
     size_t outdoor_lookups = 0;
-    size_t outdoor_pointer_hits = 0; // times coarser_bucket pointer was used instead of lookup
-    double link_time_seconds = 0.0; // cumulative time spent in link_buckets_to_coarser
 };
 
 // hierarchical version: updates both indoor and outdoor, then links buckets

apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp

@@ -1710,28 +1710,14 @@ static void compute_hessian(
     // Outdoor contribution (independent, only when hierarchical mode and range >= 5.0)
     if (ablation_study_use_hierarchical_rgd && range_squared >= outdoor_range_squared)
     {
-        // Get outdoor bucket: use coarser_bucket pointer if available, otherwise hash lookup
-        const NDT::Bucket* outdoor_bucket = (indoor_bucket && indoor_bucket->coarser_bucket) ? indoor_bucket->coarser_bucket : nullptr;
+        const auto outdoor_key = get_rgd_index_3d(point_global, bucket_size_outdoor);
+        const auto outdoor_it = buckets_outdoor.find(outdoor_key);
+        ++stats.outdoor_lookups;
 
-        if (outdoor_bucket != nullptr)
-        {
-            ++stats.outdoor_pointer_hits;
-        }
-        else
-        {
-            const auto outdoor_key = get_rgd_index_3d(point_global, bucket_size_outdoor);
-            const auto outdoor_it = buckets_outdoor.find(outdoor_key);
-            ++stats.outdoor_lookups;
-            if (outdoor_it != buckets_outdoor.end())
-            {
-                outdoor_bucket = &outdoor_it->second;
-            }
-        }
-
-        if (outdoor_bucket != nullptr)
+        if (outdoor_it != buckets_outdoor.end())
         {
             add_outdoor_hessian_contribution(
-                *outdoor_bucket,
+                outdoor_it->second,
                 point_source,
                 pose_trig,
                 viewport,
@@ -1861,7 +1847,6 @@ void optimize_lidar_odometry(
             {
                 lookup_stats.indoor_lookups += local.indoor_lookups;
                 lookup_stats.outdoor_lookups += local.outdoor_lookups;
-                lookup_stats.outdoor_pointer_hits += local.outdoor_pointer_hits;
             });
     }
     else
@@ -2244,18 +2229,6 @@ bool compute_step_2(
 
     UTL_PROFILER_SCOPE("compute_step_2");
 
-    if (params.ablation_study_use_hierarchical_rgd)
-    {
-        if (is_integer_bucket_ratio(params.in_out_params_indoor, params.in_out_params_outdoor))
-        {
-            spdlog::info("hierarchical_rgd: integer bucket ratio detected, bucket linking enabled");
-        }
-        else
-        {
-            spdlog::info("hierarchical_rgd: non-integer bucket ratio, bucket linking disabled");
-        }
-    }
-
     if (worker_data.size() != 0)
     {
         spdlog::stopwatch stopwatch_total;
@@ -2761,12 +2734,7 @@ bool compute_step_2(
         spdlog::info("total_optimization_time: {:.2f}s", total_optimization_time_seconds);
         const double avg_iteration_ms = (total_iterations > 0) ? (total_optimization_time_seconds * 1000.0 / total_iterations) : 0.0;
         spdlog::info("avg_iteration_time: {:.3f}ms", avg_iteration_ms);
-        spdlog::debug(
-            "lookup_stats: indoor={} outdoor_lookups={} outdoor_pointer_hits={} link_time={:.3f}s",
-            lookup_stats.indoor_lookups,
-            lookup_stats.outdoor_lookups,
-            lookup_stats.outdoor_pointer_hits,
-            lookup_stats.link_time_seconds);
+        spdlog::debug("lookup_stats: indoor={} outdoor_lookups={}", lookup_stats.indoor_lookups, lookup_stats.outdoor_lookups);
     }
 
     return true;

core/include/ndt.h

@@ -39,7 +39,6 @@ class NDT
         Eigen::Matrix3d cov_inverse; // precomputed inverse of cov, updated in update_rgd
         Eigen::Vector3d mean;
         Eigen::Vector3d normal_vector;
-        const Bucket* coarser_bucket = nullptr; // pointer to coarser level bucket (e.g., outdoor for indoor)
 
         uint64_t number_of_points;
         uint64_t index_begin;