From 9928d772bb9b793d7a386cfc459ab3a1bde11eca Mon Sep 17 00:00:00 2001 From: mwlasiuk Date: Sat, 28 Mar 2026 20:10:56 +0100 Subject: [PATCH] Small simplifications --- ..._data_and_drop_here-precision_forestry.cpp | 17 +- .../lidar_odometry_gui.cpp | 17 +- core/include/Core/hash_utils.h | 4 + core/include/Core/icp.h | 20 +- core/include/Core/m_estimators.h | 341 +++++++++++------- core/src/hash_utils.cpp | 12 + 6 files changed, 248 insertions(+), 163 deletions(-) diff --git a/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp index e574d07f..5b92a803 100644 --- a/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp +++ b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp @@ -14,6 +14,7 @@ #include "lidar_odometry_utils.h" #include +#include #include #include #include @@ -272,20 +273,6 @@ std::string formatCompletionTime(double remainingSeconds) return std::string(timeStr); } -int get_index_3d(const set& s, int k) -{ - int index = 0; - for (auto u : s) - { - if (u == k) - { - return index; - } - index++; - } - return -1; -} - void find_best_stretch( std::vector points, std::vector timestamps, std::vector poses, std::string fn1, std::string fn2) { @@ -317,7 +304,7 @@ void find_best_stretch( std::vector points_reindexed = points; for (size_t i = 0; i < points_reindexed.size(); i++) { - points_reindexed[i].index_pose = get_index_3d(indexes, points[i].index_pose); + points_reindexed[i].index_pose = get_index_in_set(indexes, points[i].index_pose); } /// std::vector best_trajectory = trajectory; diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 24c2a5a3..961978dc 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -14,6 +14,7 @@ #include "lidar_odometry_utils.h" #include +#include #include #include #include @@ -303,20 +304,6 @@ std::vector> get_batches_of_points(std::string laz_file, i } #endif -int get_index_3d(const set& s, int k) -{ - int index = 0; - for (auto u : s) - { - if (u == k) - { - return index; - } - index++; - } - return -1; -} - void find_best_stretch( std::vector points, std::vector timestamps, std::vector poses, std::string fn1, std::string fn2) { @@ -348,7 +335,7 @@ void find_best_stretch( std::vector points_reindexed = points; for (size_t i = 0; i < points_reindexed.size(); i++) { - points_reindexed[i].index_pose = get_index_3d(indexes, points[i].index_pose); + points_reindexed[i].index_pose = get_index_in_set(indexes, points[i].index_pose); } /// std::vector best_trajectory = trajectory; diff --git a/core/include/Core/hash_utils.h b/core/include/Core/hash_utils.h index de60523f..ed464878 100644 --- a/core/include/Core/hash_utils.h +++ b/core/include/Core/hash_utils.h @@ -2,8 +2,12 @@ #include +#include + uint64_t get_index_2d(const int16_t x, const int16_t y); uint64_t get_rgd_index_2d(const Eigen::Vector3d& p, const Eigen::Vector2d& b); uint64_t get_index_3d(const int16_t x, const int16_t y, const int16_t z); uint64_t get_rgd_index_3d(const Eigen::Vector3d& p, const Eigen::Vector3d& b); + +int32_t get_index_in_set(const std::set& set, const int32_t query); \ No newline at end of file diff --git a/core/include/Core/icp.h b/core/include/Core/icp.h index 08cb9210..b6cbee62 100644 --- a/core/include/Core/icp.h +++ b/core/include/Core/icp.h @@ -29,21 +29,22 @@ class ICP uint64_t index_begin_inclusive; uint64_t index_end_exclusive; }; + std::vector get_jobs(uint64_t size, int num_threads = 8); ICP() + : search_radius(0.1) + , number_of_threads(std::thread::hardware_concurrency()) + , number_of_iterations(6) + , is_adaptive_robust_kernel(false) + , is_ballanced_horizontal_vs_vertical(true) + , barron_c(1.0) { - search_radius = 0.1; - number_of_threads = std::thread::hardware_concurrency(); - number_of_iterations = 6; - is_adaptive_robust_kernel = false; - is_ballanced_horizontal_vs_vertical = true; - barron_c = 1.0; - }; + } + ~ICP() { - ; - }; + } bool optimize_source_to_target_wc(PointClouds& point_clouds_container, bool fix_first_node); bool compute_uncertainty(PointClouds& point_clouds_container); @@ -76,6 +77,7 @@ class ICP compute_covariance_matrices_point_to_point_source_to_target_source_to_target_lie_algebra_right_jacobian( PointClouds& point_clouds_container); + // TODO(mwlasiuk) : private float search_radius; int number_of_threads; int number_of_iterations; diff --git a/core/include/Core/m_estimators.h b/core/include/Core/m_estimators.h index 7bbcbba8..56974fb1 100644 --- a/core/include/Core/m_estimators.h +++ b/core/include/Core/m_estimators.h @@ -2,266 +2,359 @@ #include -inline double get_l1_rho(double r) +// Helper to get sign of a number +inline double get_sign(const double r) { - return fabs(r); + if (r != 0.0) + { + return std::copysign(1.0, r); + } + return 0.0; +} + +// L1 +inline double get_l1_rho(const double r) +{ + return std::fabs(r); } -inline double get_l1_upsilon(double r) +inline double get_l1_upsilon(const double r) { - return (r > 0) ? 1 : ((r < 0) ? -1 : 0); + return get_sign(r); } -inline double get_l1_w(double r) +inline double get_l1_w(const double r) { - return 1 / fabs(r); + return 1.0 / std::fabs(r); } -inline double get_l2_rho(double r) +// L2 +inline double get_l2_rho(const double r) { - return (r * r) / 2; + return 0.5 * r * r; } -inline double get_l2_upsilon(double r) +inline double get_l2_upsilon(const double r) { return r; } -inline double get_l2_w(double r) +inline double get_l2_w(const double r) { - return 1; + return 1.0; } -inline double get_l1l2_rho(double r) +// L1-L2 +inline double get_l1l2_rho(const double r) { - return 2 * (sqrt(1 + (r * r) / 2.0) - 1); + const double rr = r * r; + + return 2.0 * (std::sqrt(1.0 + 0.5 * rr) - 1.0); } -inline double get_l1l2_upsilon(double r) +inline double get_l1l2_upsilon(const double r) { - return r / (sqrt(1 + (r * r) / 2.0)); + const double rr = r * r; + + return r / std::sqrt(1.0 + 0.5 * rr); } -inline double get_l1l2_w(double r) +inline double get_l1l2_w(const double r) { - return 1 / (sqrt(1 + (r * r) / 2.0)); + const double rr = r * r; + + return 1.0 / std::sqrt(1.0 + 0.5 * rr); } -inline double get_lp_rho(double r, double nu) +// Lp +inline double get_lp_rho(const double r, const double nu) { - return (pow(fabs(r), nu)) / nu; + const double ar = std::fabs(r); + + return std::pow(ar, nu) / nu; } -inline double get_lp_upsilon(double r, double nu) +inline double get_lp_upsilon(const double r, const double nu) { - return ((r > 0) ? 1 : ((r < 0) ? -1 : 0)) * pow(fabs(r), nu - 1); + const double ar = std::fabs(r); + + if (r != 0.0) + { + return get_sign(r) * std::pow(ar, nu - 1.0); + } + + return 0.0; } -inline double get_lp_w(double r, double nu) +inline double get_lp_w(const double r, const double nu) { - return pow(fabs(r), nu - 2); + return std::pow(std::fabs(r), nu - 2.0); } -inline double get_fair_rho(double r, double c) +// Fair +inline double get_fair_rho(const double r, const double c) { - return c * c * (fabs(r) / c - log(1 + fabs(r) / c)); + const double ar_c = std::fabs(r) / c; + return c * c * (ar_c - std::log1p(ar_c)); } -inline double get_fair_upsilon(double r, double c) +inline double get_fair_upsilon(const double r, const double c) { - return r / (1 + fabs(r) / c); + return r / (1.0 + std::fabs(r) / c); } -inline double get_fair_w(double r, double c) +inline double get_fair_w(const double r, const double c) { - return 1 / (1 + fabs(r) / c); + return 1.0 / (1.0 + std::fabs(r) / c); } -inline double get_huber_rho(double r, double k) +// Huber +inline double get_huber_rho(const double r, const double k) { - if (fabs(r) < k) - { - return (r * r) / 2; - } - else + const double ar = std::fabs(r); + + if (ar < k) { - return k * (fabs(r) - k / 2); + return 0.5 * r * r; } + + return k * (ar - 0.5 * k); } -inline double get_huber_upsilon(double r, double k) +inline double get_huber_upsilon(const double r, const double k) { - if (fabs(r) < k) + if (std::fabs(r) < k) { return r; } - else - { - return k * ((r > 0) ? 1 : ((r < 0) ? -1 : 0)); - } + + return k * get_sign(r); } -inline double get_huber_w(double r, double k) +inline double get_huber_w(const double r, const double k) { - if (fabs(r) < k) - { - return 1; - } - else + if (std::fabs(r) < k) { - return k / fabs(r); + return 1.0; } + + return k / std::fabs(r); } -inline double get_cauchy_rho(double r, double c) +// Cauchy +inline double get_cauchy_rho(const double r, const double c) { - return (c * c) / 2 * log(1 + (r / c) * (r / c)); + const double cc = c * c; + const double rc = r / c; + const double rcrc = rc * rc; + + return 0.5 * cc * std::log1p(rcrc); } -inline double get_cauchy_upsilon(double r, double c) +inline double get_cauchy_upsilon(const double r, const double c) { - return r / (1 + (r / c) * (r / c)); + const double rc = r / c; + const double rcrc = rc * rc; + + return r / (1.0 + rcrc); } -inline double get_cauchy_w(double r, double c) +inline double get_cauchy_w(const double r, const double c) { - return 1 / (1 + (r / c) * (r / c)); + const double rc = r / c; + const double rcrc = rc * rc; + + return 1.0 / (1.0 + rcrc); } -inline double get_geman_mcclure_rho(double r) +// Geman-McClure +inline double get_geman_mcclure_rho(const double r) { - return ((r * r) / 2) / (1 + r * r); + const double rr = r * r; + + return 0.5 * rr / (1.0 + rr); } -inline double get_geman_mcclure_upsilon(double r) +inline double get_geman_mcclure_upsilon(const double r) { - return r / ((1 + r * r) * (1 + r * r)); + const double rr = r * r; + + return r / ((1.0 + rr) * (1.0 + rr)); } -inline double get_geman_mcclure_w(double r) +inline double get_geman_mcclure_w(const double r) { - return 1 / ((1 + r * r) * (1 + r * r)); + const double rr = r * r; + + return 1.0 / ((1.0 + rr) * (1.0 + rr)); } -inline double get_welsch_rho(double r, double c) +// Welsch +inline double get_welsch_rho(const double r, const double c) { - return (c * c) / 2 * (1 - exp(-(r / c) * (r / c))); + const double rc = r / c; + const double cc = c * c; + const double rcrc = rc * rc; + + return 0.5 * cc * (1.0 - std::exp(-rcrc)); } -inline double get_welsch_upsilon(double r, double c) +inline double get_welsch_upsilon(const double r, const double c) { - return r * exp(-(r / c) * (r / c)); + const double rc = r / c; + const double rc2 = (rc) * (rc); + + return r * std::exp(-rc2); } -inline double get_welsch_w(double r, double c) +inline double get_welsch_w(const double r, const double c) { - return exp(-(r / c) * (r / c)); + const double rc = r / c; + const double rc2 = (rc) * (rc); + + return std::exp(-rc2); } -inline double get_tukey_rho(double r, double c) +// Tukey +inline double get_tukey_rho(const double r, const double c) { - if (fabs(r) < c) - { - double temp = 1 - (r / c) * (r / c); - return (c * c) / 6 * (1 - temp * temp * temp); - } - else + const double ar = std::fabs(r); + const double cc = c * c; + + if (ar >= c) { - return (c * c) / 6; + return cc / 6.0; } + + const double rc = r / c; + const double rc2 = (rc) * (rc); + const double temp = 1.0 - rc2; + const double temp3 = temp * temp * temp; + + return cc / 6.0 * (1.0 - temp3); } -inline double get_tukey_upsilon(double r, double c) +inline double get_tukey_upsilon(const double r, const double c) { - if (fabs(r) < c) - { - double temp = 1 - (r / c) * (r / c); - return r * temp * temp; - } - else + const double ar = std::fabs(r); + + if (ar >= c) { - return 0; + return 0.0; } + + const double rc = r / c; + const double rc2 = (rc) * (rc); + const double temp = 1.0 - rc2; + const double temp2 = temp * temp; + + return r * temp2; } -inline double get_tukey_w(double r, double c) +inline double get_tukey_w(const double r, const double c) { - if (fabs(r) < c) - { - double temp = 1 - (r / c) * (r / c); - return temp * temp; - } - else + const double ar = std::fabs(r); + + if (ar >= c) { - return 0; + return 0.0; } + + const double rc = r / c; + const double rc2 = (rc) * (rc); + const double temp = 1.0 - rc2; + const double temp2 = temp * temp; + + return temp2; } -inline double get_barron_rho(double r, double alpha, double c) +// Barron +inline double get_barron_rho(const double r, const double alpha, const double c) { - if (alpha <= -1000000000000.0) - { - return 1 - exp(-0.5 * (r / c) * (r / c)); - } - else if (alpha == 0) + const double r_over_c = r / c; + const double r_over_c_sq = r_over_c * r_over_c; + const double abs_alpha_minus2 = std::fabs(alpha - 2.0); + + if (alpha <= -1e12) { - return log(0.5 * (r / c) * (r / c) + 1); + return 1.0 - std::exp(-0.5 * r_over_c_sq); } - else if (alpha == 2) + + if (alpha == 0.0) { - return 0.5 * (r / c) * (r / c); + return std::log(0.5 * r_over_c_sq + 1.0); } - else + + if (alpha == 2.0) { - return fabs(alpha - 2) / alpha * ((pow(((r / c) * (r / c)) / fabs(alpha - 2) + 1, alpha / 2)) - 1); + return 0.5 * r_over_c_sq; } + + return (abs_alpha_minus2 / alpha) * (std::pow((r_over_c_sq / abs_alpha_minus2) + 1.0, alpha / 2.0) - 1.0); } -inline double get_barron_upsilon(double r, double alpha, double c) +inline double get_barron_upsilon(const double r, const double alpha, const double c) { - if (alpha <= -1000000000000.0) - { - return r / (c * c) * exp(-0.5 * (r / c) * (r / c)); - } - else if (alpha == 0) + const double r_over_c = r / c; + const double r_over_c_sq = r_over_c * r_over_c; + const double c_sq = c * c; + const double abs_alpha_minus2 = std::fabs(alpha - 2.0); + + if (alpha <= -1e12) { - return 2 * r / (r * r + 2 * c * c); + return r / c_sq * std::exp(-0.5 * r_over_c_sq); } - else if (alpha == 2) + + if (alpha == 0.0) { - return r / (c * c); + return 2.0 * r / (r * r + 2.0 * c_sq); } - else + + if (alpha == 2.0) { - return r / (c * c) * (pow(((r / c) * (r / c)) / fabs(alpha - 2) + 1, alpha / 2 - 1)); + return r / c_sq; } + + return r / c_sq * std::pow((r_over_c_sq / abs_alpha_minus2) + 1.0, alpha / 2.0 - 1.0); } -inline double get_barron_w(double r, double alpha, double c) +inline double get_barron_w(const double r, const double alpha, const double c) { if (r == 0.0) + { return 1.0; + } + return get_barron_upsilon(r, alpha, c) / r; } -inline double get_approximate_partition_function(double tau_begin, double tau_end, double alpha, double c, double num_steps) +// Approximate partition function +inline double get_approximate_partition_function( + const double tau_begin, const double tau_end, const double alpha, const double c, const double num_steps) { - double step = (tau_end - tau_begin) / num_steps; + const double step = (tau_end - tau_begin) / num_steps; double area = 0.0; - for (int i = 0; i < num_steps; i++) + + for (int i = 0; i < static_cast(num_steps); i++) { - area += exp(-get_barron_rho(tau_begin + (i + 0.5) * step, alpha, c)) * step; + const double tau = tau_begin + (i + 0.5) * step; + const double rho = get_barron_rho(tau, alpha, c); + area += std::exp(-rho) * step; } + return area; } -inline double get_truncated_robust_kernel(double r, double alpha, double c, double Z_tilde) +// Truncated robust kernel +inline double get_truncated_robust_kernel(const double r, const double alpha, const double c, const double Z_tilde) { - return get_barron_rho(r, alpha, c) + log(c * Z_tilde); + return get_barron_rho(r, alpha, c) + std::log(c * Z_tilde); } -inline double get_modified_probability_distribution(double r, double alpha, double c, double tau_begin, double tau_end, double num_steps) +// Modified probability distribution +inline double get_modified_probability_distribution( + const double r, const double alpha, const double c, const double tau_begin, const double tau_end, const double num_steps) { - double Z_tilde = get_approximate_partition_function(tau_begin, tau_end, alpha, 1, num_steps); - return (1.0 / (c * Z_tilde)) * exp(-(get_barron_rho(r, alpha, c))); -} + const double Z_tilde = get_approximate_partition_function(tau_begin, tau_end, alpha, 1.0, num_steps); + return 1.0 / (c * Z_tilde) * std::exp(-get_barron_rho(r, alpha, c)); +} \ No newline at end of file diff --git a/core/src/hash_utils.cpp b/core/src/hash_utils.cpp index 3ac5dfde..247ba24f 100644 --- a/core/src/hash_utils.cpp +++ b/core/src/hash_utils.cpp @@ -49,3 +49,15 @@ uint64_t get_rgd_index_3d(const Eigen::Vector3d& p, const Eigen::Vector3d& b) return get_index_3d(x, y, z); } + +int32_t get_index_in_set(const std::set& set, const int32_t query) +{ + auto it = set.find(query); + + if (it == set.end()) + { + return -1; + } + + return std::distance(set.begin(), it); +}