diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index 6928c1b1..9c9c6cf4 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -1,5 +1,6 @@ #include "lidar_odometry.h" #include "tbb/tbb.h" +#include #include #include @@ -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 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& worker_data, const std::atomic& 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& worker_data, const LidarOdometryPa void save_result(std::vector& 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 worker_data_concatenated; @@ -1053,6 +1058,7 @@ std::string save_results_automatic( std::vector run_lidar_odometry(const std::string& input_dir, LidarOdometryParams& params) { + UTL_PROFILER_SCOPE("run_lidar_odometry"); Session session; std::vector worker_data; std::vector input_file_names; diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index 1642814c..fcf111c7 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/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(link_end - link_start).count(); } void update_rgd_spherical_coordinates( diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index 84e95346..a1dc0065 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/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; +using NDTBucketMapType = ankerl::unordered_dense::map; using NDTBucketMapType2 = ankerl::unordered_dense::map; // Helper function for getting software version from CMake macros @@ -163,16 +161,6 @@ Eigen::Matrix4d getInterpolatedPose(const std::map& tra // this function reduces number of points by preserving only first point for each bucket {bucket_x, bucket_y, bucket_z} std::vector decimate(const std::vector& 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 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 23289a5d..5444958f 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/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; diff --git a/core/include/ndt.h b/core/include/ndt.h index 053fedec..e34c88b6 100644 --- a/core/include/ndt.h +++ b/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;