From da839c09db86e0c009c57753c55f157ab925b410 Mon Sep 17 00:00:00 2001 From: JanuszBedkowski Date: Tue, 20 Jan 2026 22:16:49 +0100 Subject: [PATCH] Revert "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, 84 insertions(+), 88 deletions(-) diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 035e7ec9..4511c31d 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_3d(const set& s, int k) +int get_index(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_3d(indexes, points[i].index_pose); + points_reindexed[i].index_pose = get_index(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 f172ddd7..c67c1900 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -2,13 +2,30 @@ #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; @@ -22,7 +39,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_3d(points[i].point, b); + ip[i].index_of_bucket = get_rgd_index(points[i].point, b); } std::sort( ip.begin(), @@ -145,7 +162,7 @@ void update_rgd( for (int i = 0; i < points_global.size(); i++) { - auto index_of_bucket = get_rgd_index_3d(points_global[i].point, b); + auto index_of_bucket = get_rgd_index(points_global[i].point, b); auto bucket_it = buckets.find(index_of_bucket); @@ -228,7 +245,7 @@ void update_rgd_spherical_coordinates( for (int i = 0; i < points_global.size(); i++) { - auto index_of_bucket = get_rgd_index_3d(points_global_spherical[i], b); + auto index_of_bucket = get_rgd_index(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 4c9c86cf..c57a74ce 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -152,6 +152,9 @@ 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 da04d1fb..9303989c 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -41,7 +41,7 @@ std::vector> nns(const std::vector& points_global, for (int i = 0; i < points_global.size(); i++) { - uint64_t index = get_rgd_index_3d(points_global[i].point, search_radious); + uint64_t index = get_rgd_index(points_global[i].point, search_radious); indexes.emplace_back(index, i); } @@ -87,7 +87,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_3d(position_global, search_radious); + uint64_t index_of_bucket = get_rgd_index(position_global, search_radious); if (buckets.contains(index_of_bucket)) { @@ -531,8 +531,8 @@ void optimize_sf( double azimutal_angle_deg = acos(point_global.z() / r) * RAD_TO_DEG; /////////////// - 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 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 bucket_it = buckets.find(index_of_bucket); // no bucket found @@ -814,7 +814,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_3d(point_cloud_global_sc[indexes_i], b); + auto index_of_bucket = get_rgd_index(point_cloud_global_sc[indexes_i], b); auto bucket_it = buckets.find(index_of_bucket); // no bucket found if (bucket_it == buckets.end()) @@ -1201,7 +1201,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_3d( + auto index_of_bucket = get_rgd_index( { 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 }); @@ -1460,7 +1460,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_3d(point_global, b_indoor); + auto index_of_bucket = get_rgd_index(point_global, b_indoor); auto bucket_it = buckets_indoor.find(index_of_bucket); // no bucket found @@ -1598,7 +1598,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_3d(point_global, b_outdoor); + auto index_of_bucket = get_rgd_index(point_global, b_outdoor); auto bucket_it = buckets_outdoor.find(index_of_bucket); // no bucket found @@ -2145,7 +2145,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_3d(point_global, b); + auto index_of_bucket = get_rgd_index(point_global, b); if (!reference_buckets.contains(index_of_bucket)) continue; @@ -2963,7 +2963,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_3d(point_global, b); + auto index_of_bucket = get_rgd_index(point_global, b); auto bucket_it = buckets.find(index_of_bucket); // no bucket found @@ -3343,7 +3343,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_3d(points_global[i].point, b); + auto index_of_bucket = get_rgd_index(points_global[i].point, b); index_pairs.emplace_back(index_of_bucket, i); } std::sort( @@ -3455,7 +3455,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_3d(points_global[i].point, b); + auto index_of_bucket = get_rgd_index(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 7a24469f..3efe5797 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,18 +20,16 @@ #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; @@ -1462,7 +1460,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_3d(point_global, b); + auto index_of_bucket = get_rgd_index(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 f6e436c7..d99d1885 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -33,8 +33,6 @@ #include -#include - #ifdef _WIN32 #include "resource.h" #include // <-- Required for ShellExecuteA @@ -286,8 +284,8 @@ void optimize() for (size_t i = 0; i < point_cloud_global.size(); i++) { - 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 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 bucket_it = buckets.find(index_of_bucket); // no bucket found if (bucket_it == buckets.end()) @@ -754,7 +752,7 @@ std::vector> get_nn() for (size_t i = 0; i < point_cloud_global_sc.size(); i++) { - auto index_of_bucket = get_rgd_index_3d(point_cloud_global_sc[i], b); + auto index_of_bucket = get_rgd_index(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 cd8a7358..3aec690a 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 da163b22..b4887ea7 100644 --- a/core/include/hash_utils.h +++ b/core/include/hash_utils.h @@ -2,8 +2,8 @@ #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); +// 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_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); +// 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); diff --git a/core/include/surface.h b/core/include/surface.h index 1a5065cd..822023b0 100644 --- a/core/include/surface.h +++ b/core/include/surface.h @@ -3,7 +3,21 @@ #include #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); +} class Surface { diff --git a/core/src/hash_utils.cpp b/core/src/hash_utils.cpp index 14a8e0db..3748225c 100644 --- a/core/src/hash_utils.cpp +++ b/core/src/hash_utils.cpp @@ -2,50 +2,16 @@ #include -uint64_t get_index_2d(const int16_t x, const int16_t y) +uint64_t get_index(const int16_t x, const int16_t y, const int16_t z) { - 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); + return ((static_cast(x) << 32) & (0x0000FFFF00000000ull)) | ((static_cast(y) << 16) & (0x00000000FFFF0000ull)) | + ((static_cast(z) << 0) & (0x000000000000FFFFull)); } -uint64_t get_index_3d(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) { - 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); + 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); } diff --git a/core/src/local_shape_features.cpp b/core/src/local_shape_features.cpp index b325a6c2..dfcf1918 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;