From 0c05de6b2f634081503416054de372a9ab8632a5 Mon Sep 17 00:00:00 2001 From: mwlasiuk Date: Tue, 20 Jan 2026 20:26:57 +0100 Subject: [PATCH] Unify hash utils --- .../lidar_odometry_gui.cpp | 4 +- .../lidar_odometry_utils.cpp | 25 ++-------- .../lidar_odometry_utils.h | 3 -- .../lidar_odometry_utils_optimizers.cpp | 24 ++++----- .../livox_mid_360_intrinsic_calibration.cpp | 10 ++-- .../mandeye_raw_data_viewer.cpp | 8 +-- .../precision_forestry_tools.cpp | 2 +- core/include/hash_utils.h | 8 +-- core/include/surface.h | 16 +----- core/src/hash_utils.cpp | 50 ++++++++++++++++--- core/src/local_shape_features.cpp | 4 +- .../src/pair_wise_iterative_closest_point.cpp | 8 +-- core/src/surface.cpp | 10 ++-- 13 files changed, 88 insertions(+), 84 deletions(-) diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 4511c31d..035e7ec9 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -299,7 +299,7 @@ std::vector> get_batches_of_points(std::string laz_file, i } #endif -int get_index(const set& s, int k) +int get_index_3d(const set& s, int k) { int index = 0; for (auto u : s) @@ -345,7 +345,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(indexes, points[i].index_pose); + points_reindexed[i].index_pose = get_index_3d(indexes, points[i].index_pose); } /// std::vector best_trajectory = trajectory; diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index c67c1900..f172ddd7 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -2,30 +2,13 @@ #include "csv.hpp" #include #include +#include #include // #include namespace fs = std::filesystem; -// TODO(m.wlasiuk) : these 2 functions - core/utils - -// this function provides unique index -uint64_t get_index(const int16_t x, const int16_t y, const int16_t z) -{ - return ((static_cast(x) << 32) & (0x0000FFFF00000000ull)) | ((static_cast(y) << 16) & (0x00000000FFFF0000ull)) | - ((static_cast(z) << 0) & (0x000000000000FFFFull)); -} - -// this function provides unique index for input point p and 3D space decomposition into buckets b -uint64_t get_rgd_index(const Eigen::Vector3d p, const Eigen::Vector3d b) -{ - int16_t x = static_cast(p.x() / b.x()); - int16_t y = static_cast(p.y() / b.y()); - int16_t z = static_cast(p.z() / b.z()); - return get_index(x, y, z); -} - std::vector decimate(const std::vector& points, double bucket_x, double bucket_y, double bucket_z) { // std::cout << "points.size before decimation: " << points.size() << std::endl; @@ -39,7 +22,7 @@ std::vector decimate(const std::vector& points, double bucke for (int i = 0; i < points.size(); i++) { ip[i].index_of_point = i; - ip[i].index_of_bucket = get_rgd_index(points[i].point, b); + ip[i].index_of_bucket = get_rgd_index_3d(points[i].point, b); } std::sort( ip.begin(), @@ -162,7 +145,7 @@ void update_rgd( for (int i = 0; i < points_global.size(); i++) { - auto index_of_bucket = get_rgd_index(points_global[i].point, b); + auto index_of_bucket = get_rgd_index_3d(points_global[i].point, b); auto bucket_it = buckets.find(index_of_bucket); @@ -245,7 +228,7 @@ void update_rgd_spherical_coordinates( for (int i = 0; i < points_global.size(); i++) { - auto index_of_bucket = get_rgd_index(points_global_spherical[i], b); + auto index_of_bucket = get_rgd_index_3d(points_global_spherical[i], b); auto bucket_it = buckets.find(index_of_bucket); diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index c57a74ce..4c9c86cf 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -152,9 +152,6 @@ struct LidarOdometryParams bool save_index_pose = false; }; -uint64_t get_index(const int16_t x, const int16_t y, const int16_t z); -uint64_t get_rgd_index(const Eigen::Vector3d p, const Eigen::Vector3d b); - // this function finds interpolated pose between two poses according to query_time Eigen::Matrix4d getInterpolatedPose(const std::map& trajectory, double query_time); 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 22eee87e..29a0f313 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -39,7 +39,7 @@ std::vector> nns(const std::vector& points_global, for (int i = 0; i < points_global.size(); i++) { - uint64_t index = get_rgd_index(points_global[i].point, search_radious); + uint64_t index = get_rgd_index_3d(points_global[i].point, search_radious); indexes.emplace_back(index, i); } @@ -85,7 +85,7 @@ std::vector> nns(const std::vector& points_global, for (double z = -search_radious.z(); z <= search_radious.z(); z += search_radious.z()) { Eigen::Vector3d position_global = source.point + Eigen::Vector3d(x, y, z); - uint64_t index_of_bucket = get_rgd_index(position_global, search_radious); + uint64_t index_of_bucket = get_rgd_index_3d(position_global, search_radious); if (buckets.contains(index_of_bucket)) { @@ -529,8 +529,8 @@ void optimize_sf( double azimutal_angle_deg = acos(point_global.z() / r) * RAD_TO_DEG; /////////////// - auto index_of_bucket = get_rgd_index({ r, polar_angle_deg, azimutal_angle_deg }, b); - // auto index_of_bucket = get_rgd_index(point_global, b); + auto index_of_bucket = get_rgd_index_3d({ r, polar_angle_deg, azimutal_angle_deg }, b); + // auto index_of_bucket = get_rgd_index_3d(point_global, b); auto bucket_it = buckets.find(index_of_bucket); // no bucket found @@ -812,7 +812,7 @@ void optimize_sf2( const auto hessian_fun = [&](const int& indexes_i) -> Blocks { int c = intermediate_points[indexes_i].index_pose * 6; - auto index_of_bucket = get_rgd_index(point_cloud_global_sc[indexes_i], b); + auto index_of_bucket = get_rgd_index_3d(point_cloud_global_sc[indexes_i], b); auto bucket_it = buckets.find(index_of_bucket); // no bucket found if (bucket_it == buckets.end()) @@ -1199,7 +1199,7 @@ void optimize_rigid_sf( double polar_angle_deg_l = atan2(point_global.y(), point_global.x()) * RAD_TO_DEG; double azimutal_angle_deg_l = acos(point_global.z() / r_l) * RAD_TO_DEG; - auto index_of_bucket = get_rgd_index( + auto index_of_bucket = get_rgd_index_3d( { r_l, polar_angle_deg_l, azimutal_angle_deg_l }, { rgd_params_sc.resolution_X, rgd_params_sc.resolution_Y, rgd_params_sc.resolution_Z }); @@ -1459,7 +1459,7 @@ static void compute_hessian_indoor( return; Eigen::Vector3d point_global = intermediate_trajectory[intermediate_points_i.index_pose] * intermediate_points_i.point; - auto index_of_bucket = get_rgd_index(point_global, b_indoor); + auto index_of_bucket = get_rgd_index_3d(point_global, b_indoor); auto bucket_it = buckets_indoor.find(index_of_bucket); // no bucket found @@ -1600,7 +1600,7 @@ static void compute_hessian_outdoor( return; Eigen::Vector3d point_global = intermediate_trajectory[intermediate_points_i.index_pose] * intermediate_points_i.point; - auto index_of_bucket = get_rgd_index(point_global, b_outdoor); + auto index_of_bucket = get_rgd_index_3d(point_global, b_outdoor); auto bucket_it = buckets_outdoor.find(index_of_bucket); // no bucket found @@ -2080,7 +2080,7 @@ void align_to_reference( for (int i = 0; i < initial_points.size(); i += 1) { Eigen::Vector3d point_global = m_g * initial_points[i].point; - auto index_of_bucket = get_rgd_index(point_global, b); + auto index_of_bucket = get_rgd_index_3d(point_global, b); if (!reference_buckets.contains(index_of_bucket)) continue; @@ -2898,7 +2898,7 @@ void Consistency(std::vector& worker_data, const LidarOdometryParams return; Eigen::Vector3d point_global = trajectory[intermediate_points_i.index_pose] * intermediate_points_i.point; - auto index_of_bucket = get_rgd_index(point_global, b); + auto index_of_bucket = get_rgd_index_3d(point_global, b); auto bucket_it = buckets.find(index_of_bucket); // no bucket found @@ -3278,7 +3278,7 @@ void Consistency2(std::vector& worker_data, const LidarOdometryParam std::vector> index_pairs; for (int i = 0; i < points_global.size(); i++) { - auto index_of_bucket = get_rgd_index(points_global[i].point, b); + auto index_of_bucket = get_rgd_index_3d(points_global[i].point, b); index_pairs.emplace_back(index_of_bucket, i); } std::sort( @@ -3390,7 +3390,7 @@ void Consistency2(std::vector& worker_data, const LidarOdometryParam if (points_local[i].point.norm() < 1.0) continue; - auto index_of_bucket = get_rgd_index(points_global[i].point, b); + auto index_of_bucket = get_rgd_index_3d(points_global[i].point, b); auto bucket_it = buckets.find(index_of_bucket); // no bucket found if (bucket_it == buckets.end()) diff --git a/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp b/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp index 3efe5797..7a24469f 100644 --- a/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp +++ b/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp @@ -20,16 +20,18 @@ #include "pfd_wrapper.hpp" #include "../lidar_odometry_step_1/lidar_odometry_utils.h" -#include #include -#include - #include #include +#include + +#include +#include + #define SAMPLE_PERIOD (1.0 / 200.0) namespace fs = std::filesystem; @@ -1460,7 +1462,7 @@ void calibrate_intrinsics() Eigen::Vector3d point_local(intermediate_points_i.point.x(), intermediate_points_i.point.y(), intermediate_points_i.point.z()); Eigen::Vector3d point_global = m_pose * point_local; - auto index_of_bucket = get_rgd_index(point_global, b); + auto index_of_bucket = get_rgd_index_3d(point_global, b); auto bucket_it = buckets.find(index_of_bucket); // no bucket found diff --git a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp index d99d1885..f6e436c7 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -33,6 +33,8 @@ #include +#include + #ifdef _WIN32 #include "resource.h" #include // <-- Required for ShellExecuteA @@ -284,8 +286,8 @@ void optimize() for (size_t i = 0; i < point_cloud_global.size(); i++) { - auto index_of_bucket = get_rgd_index(point_cloud_global_sc[i], b); - //auto index_of_bucket = get_rgd_index(point_cloud_global[i].point, b); + auto index_of_bucket = get_rgd_index_3d(point_cloud_global_sc[i], b); + //auto index_of_bucket = get_rgd_index_3d(point_cloud_global[i].point, b); auto bucket_it = buckets.find(index_of_bucket); // no bucket found if (bucket_it == buckets.end()) @@ -752,7 +754,7 @@ std::vector> get_nn() for (size_t i = 0; i < point_cloud_global_sc.size(); i++) { - auto index_of_bucket = get_rgd_index(point_cloud_global_sc[i], b); + auto index_of_bucket = get_rgd_index_3d(point_cloud_global_sc[i], b); auto bucket_it = buckets.find(index_of_bucket); diff --git a/apps/precision_forestry_tools/precision_forestry_tools.cpp b/apps/precision_forestry_tools/precision_forestry_tools.cpp index 3aec690a..cd8a7358 100644 --- a/apps/precision_forestry_tools/precision_forestry_tools.cpp +++ b/apps/precision_forestry_tools/precision_forestry_tools.cpp @@ -76,7 +76,7 @@ std::vector get_lowest_points_indexes(const PointCloud& pc, Eigen::Vector2d for (size_t i = 0; i < pc.points_local.size(); i++) { - uint64_t index = get_rgd_index_2D(pc.points_local[i], bucket_dim_xy); + uint64_t index = get_rgd_index_2d(pc.points_local[i], bucket_dim_xy); indexes_tuple.emplace_back(index, i, pc.points_local[i].z()); } diff --git a/core/include/hash_utils.h b/core/include/hash_utils.h index b4887ea7..da163b22 100644 --- a/core/include/hash_utils.h +++ b/core/include/hash_utils.h @@ -2,8 +2,8 @@ #include -// this function provides unique index -uint64_t get_index(const int16_t x, const int16_t y, const int16_t z); +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); -// this function provides unique index for input point p and 3D space decomposition into buckets b -uint64_t get_rgd_index(const Eigen::Vector3d p, const Eigen::Vector3d 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); diff --git a/core/include/surface.h b/core/include/surface.h index 822023b0..1a5065cd 100644 --- a/core/include/surface.h +++ b/core/include/surface.h @@ -3,21 +3,7 @@ #include #include -inline uint64_t get_index_2D(const int16_t x, const int16_t y /*, const int16_t z*/) -{ - // return ((static_cast(x) << 32) & (0x0000FFFF00000000ull)) | - // ((static_cast(y) << 16) & (0x00000000FFFF0000ull)) | - // ((static_cast(z) << 0) & (0x000000000000FFFFull)); - return ((static_cast(x) << 16) & (0x00000000FFFF0000ull)) | ((static_cast(y) << 0) & (0x000000000000FFFFull)); -} - -inline uint64_t get_rgd_index_2D(const Eigen::Vector3d p, const Eigen::Vector2d b) -{ - int16_t x = static_cast(p.x() / b.x()); - int16_t y = static_cast(p.y() / b.y()); - // int16_t z = static_cast(p.z() / b.z()); - return get_index_2D(x, y); -} +#include class Surface { diff --git a/core/src/hash_utils.cpp b/core/src/hash_utils.cpp index 3748225c..14a8e0db 100644 --- a/core/src/hash_utils.cpp +++ b/core/src/hash_utils.cpp @@ -2,16 +2,50 @@ #include -uint64_t get_index(const int16_t x, const int16_t y, const int16_t z) +uint64_t get_index_2d(const int16_t x, const int16_t y) { - return ((static_cast(x) << 32) & (0x0000FFFF00000000ull)) | ((static_cast(y) << 16) & (0x00000000FFFF0000ull)) | - ((static_cast(z) << 0) & (0x000000000000FFFFull)); + static constexpr auto shift_x = 16 * 1; + static constexpr auto shift_y = 16 * 0; + + static constexpr auto mask_x = 0x00000000FFFF0000ull; + static constexpr auto mask_y = 0x000000000000FFFFull; + + const auto x_part = ((static_cast(x) << shift_x) & (mask_x)); + const auto y_part = ((static_cast(y) << shift_y) & (mask_y)); + + return x_part | y_part; +} + +uint64_t get_rgd_index_2d(const Eigen::Vector3d p, const Eigen::Vector2d b) +{ + const int16_t x = static_cast(p.x() / b.x()); + const int16_t y = static_cast(p.y() / b.y()); + + return get_index_2d(x, y); } -uint64_t get_rgd_index(const Eigen::Vector3d p, const Eigen::Vector3d b) +uint64_t get_index_3d(const int16_t x, const int16_t y, const int16_t z) { - int16_t x = static_cast(p.x() / b.x()); - int16_t y = static_cast(p.y() / b.y()); - int16_t z = static_cast(p.z() / b.z()); - return get_index(x, y, z); + static constexpr auto shift_x = 16 * 2; + static constexpr auto shift_y = 16 * 2; + static constexpr auto shift_z = 16 * 0; + + static constexpr auto mask_x = 0x0000FFFF00000000ull; + static constexpr auto mask_y = 0x00000000FFFF0000ull; + static constexpr auto mask_z = 0x000000000000FFFFull; + + const auto x_part = ((static_cast(x) << shift_x) & (mask_x)); + const auto y_part = ((static_cast(y) << shift_y) & (mask_y)); + const auto z_part = ((static_cast(z) << shift_z) & (mask_z)); + + return x_part | y_part | z_part; +} + +uint64_t get_rgd_index_3d(const Eigen::Vector3d p, const Eigen::Vector3d b) +{ + const int16_t x = static_cast(p.x() / b.x()); + const int16_t y = static_cast(p.y() / b.y()); + const int16_t z = static_cast(p.z() / b.z()); + + return get_index_3d(x, y, z); } diff --git a/core/src/local_shape_features.cpp b/core/src/local_shape_features.cpp index dfcf1918..b325a6c2 100644 --- a/core/src/local_shape_features.cpp +++ b/core/src/local_shape_features.cpp @@ -9,7 +9,7 @@ bool LocalShapeFeatures::calculate_local_shape_features(std::vector Surface::get_filtered_indexes( for (int i = 0; i < lowest_points.size(); i++) { - uint64_t index = get_rgd_index_2D(lowest_points[i].first, bucket_dim_xy); + uint64_t index = get_rgd_index_2d(lowest_points[i].first, bucket_dim_xy); indexes.emplace_back(index, i); } @@ -491,7 +491,7 @@ std::vector Surface::get_filtered_indexes( //{ Eigen::Vector3d position_global = source + Eigen::Vector3d(x, y, 0); - uint64_t index_of_bucket = get_rgd_index_2D(position_global, bucket_dim_xy); + uint64_t index_of_bucket = get_rgd_index_2d(position_global, bucket_dim_xy); if (buckets.contains(index_of_bucket)) { @@ -599,7 +599,7 @@ std::vector Surface::get_filtered_indexes( for (int i = 0; i < lowest_points.size(); i++) { - uint64_t index = get_rgd_index_2D(lowest_points[i], bucket_dim_xy); + uint64_t index = get_rgd_index_2d(lowest_points[i], bucket_dim_xy); indexes.emplace_back(index, i); }*/ @@ -630,7 +630,7 @@ std::vector Surface::get_points_without_surface( for (int i = 0; i < vertices.size(); i++) { - uint64_t index = get_rgd_index_2D(vertices[i].translation(), bucket_dim_xy); + uint64_t index = get_rgd_index_2d(vertices[i].translation(), bucket_dim_xy); indexes.emplace_back(index, i); } @@ -660,7 +660,7 @@ std::vector Surface::get_points_without_surface( for (int i = 0; i < points.size(); i++) { - uint64_t index_of_bucket = get_rgd_index_2D(points[i], bucket_dim_xy); + uint64_t index_of_bucket = get_rgd_index_2d(points[i], bucket_dim_xy); if (buckets.contains(index_of_bucket)) { int index = buckets[index_of_bucket].first;