Skip to content

Revert "Unify hash utils" #323

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
JanuszBedkowski merged 1 commit into from
Jan 20, 2026
Merged

Revert "Unify hash utils" #323

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions apps/lidar_odometry_step_1/lidar_odometry_gui.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -299,7 +299,7 @@ std::vector<std::vector<Point3Di>> get_batches_of_points(std::string laz_file, i
}
#endif

int get_index_3d(const set<int>& s, int k)
int get_index(const set<int>& s, int k)
{
int index = 0;
for (auto u : s)
Expand Down Expand Up @@ -345,7 +345,7 @@ void find_best_stretch(
std::vector<Point3Di> 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<Eigen::Affine3d> best_trajectory = trajectory;
Expand Down
25 changes: 21 additions & 4 deletions apps/lidar_odometry_step_1/lidar_odometry_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,13 +2,30 @@
#include "csv.hpp"
#include <algorithm>
#include <filesystem>
#include <hash_utils.h>
#include <regex>

// #include <toml.hpp>

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<uint64_t>(x) << 32) & (0x0000FFFF00000000ull)) | ((static_cast<uint64_t>(y) << 16) & (0x00000000FFFF0000ull)) |
((static_cast<uint64_t>(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<int16_t>(p.x() / b.x());
int16_t y = static_cast<int16_t>(p.y() / b.y());
int16_t z = static_cast<int16_t>(p.z() / b.z());
return get_index(x, y, z);
}

std::vector<Point3Di> decimate(const std::vector<Point3Di>& points, double bucket_x, double bucket_y, double bucket_z)
{
// std::cout << "points.size before decimation: " << points.size() << std::endl;
Expand All @@ -22,7 +39,7 @@ std::vector<Point3Di> decimate(const std::vector<Point3Di>& 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(),
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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);

Expand Down
3 changes: 3 additions & 0 deletions apps/lidar_odometry_step_1/lidar_odometry_utils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<double, Eigen::Matrix4d>& trajectory, double query_time);

Expand Down
24 changes: 12 additions & 12 deletions apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ std::vector<std::pair<int, int>> nns(const std::vector<Point3Di>& 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);
}

Expand Down Expand Up @@ -87,7 +87,7 @@ std::vector<std::pair<int, int>> nns(const std::vector<Point3Di>& 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))
{
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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())
Expand Down Expand Up @@ -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 });

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -2963,7 +2963,7 @@ void Consistency(std::vector<WorkerData>& 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
Expand Down Expand Up @@ -3343,7 +3343,7 @@ void Consistency2(std::vector<WorkerData>& worker_data, const LidarOdometryParam
std::vector<std::pair<uint64_t, uint32_t>> 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(
Expand Down Expand Up @@ -3455,7 +3455,7 @@ void Consistency2(std::vector<WorkerData>& 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())
Expand Down
10 changes: 4 additions & 6 deletions apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,18 +20,16 @@
#include "pfd_wrapper.hpp"

#include "../lidar_odometry_step_1/lidar_odometry_utils.h"
#include <filesystem>

#include <HDMapping/Version.hpp>

#include <mutex>

#include <pair_wise_iterative_closest_point.h>

#include <export_laz.h>

#include <hash_utils.h>

#include <filesystem>
#include <mutex>

#define SAMPLE_PERIOD (1.0 / 200.0)
namespace fs = std::filesystem;

Expand Down Expand Up @@ -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
Expand Down
8 changes: 3 additions & 5 deletions apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,6 @@

#include <opencv2/opencv.hpp>

#include <hash_utils.h>

#ifdef _WIN32
#include "resource.h"
#include <shellapi.h> // <-- Required for ShellExecuteA
Expand Down Expand Up @@ -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())
Expand Down Expand Up @@ -754,7 +752,7 @@ std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> 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);

Expand Down
2 changes: 1 addition & 1 deletion apps/precision_forestry_tools/precision_forestry_tools.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ std::vector<int> 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());
}

Expand Down
8 changes: 4 additions & 4 deletions core/include/hash_utils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,8 @@

#include <Eigen/Eigen>

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);
16 changes: 15 additions & 1 deletion core/include/surface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,21 @@
#include <Eigen/Eigen>
#include <vector>

#include <hash_utils.h>
inline uint64_t get_index_2D(const int16_t x, const int16_t y /*, const int16_t z*/)
{
// return ((static_cast<uint64_t>(x) << 32) & (0x0000FFFF00000000ull)) |
// ((static_cast<uint64_t>(y) << 16) & (0x00000000FFFF0000ull)) |
// ((static_cast<uint64_t>(z) << 0) & (0x000000000000FFFFull));
return ((static_cast<uint64_t>(x) << 16) & (0x00000000FFFF0000ull)) | ((static_cast<uint64_t>(y) << 0) & (0x000000000000FFFFull));
}

inline uint64_t get_rgd_index_2D(const Eigen::Vector3d p, const Eigen::Vector2d b)
{
int16_t x = static_cast<int16_t>(p.x() / b.x());
int16_t y = static_cast<int16_t>(p.y() / b.y());
// int16_t z = static_cast<int16_t>(p.z() / b.z());
return get_index_2D(x, y);
}

class Surface
{
Expand Down
50 changes: 8 additions & 42 deletions core/src/hash_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,50 +2,16 @@

#include <hash_utils.h>

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<uint64_t>(x) << shift_x) & (mask_x));
const auto y_part = ((static_cast<uint64_t>(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<int16_t>(p.x() / b.x());
const int16_t y = static_cast<int16_t>(p.y() / b.y());

return get_index_2d(x, y);
return ((static_cast<uint64_t>(x) << 32) & (0x0000FFFF00000000ull)) | ((static_cast<uint64_t>(y) << 16) & (0x00000000FFFF0000ull)) |
((static_cast<uint64_t>(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<uint64_t>(x) << shift_x) & (mask_x));
const auto y_part = ((static_cast<uint64_t>(y) << shift_y) & (mask_y));
const auto z_part = ((static_cast<uint64_t>(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<int16_t>(p.x() / b.x());
const int16_t y = static_cast<int16_t>(p.y() / b.y());
const int16_t z = static_cast<int16_t>(p.z() / b.z());

return get_index_3d(x, y, z);
int16_t x = static_cast<int16_t>(p.x() / b.x());
int16_t y = static_cast<int16_t>(p.y() / b.y());
int16_t z = static_cast<int16_t>(p.z() / b.z());
return get_index(x, y, z);
}
4 changes: 2 additions & 2 deletions core/src/local_shape_features.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ bool LocalShapeFeatures::calculate_local_shape_features(std::vector<PointWithLoc

for (int i = 0; i < points.size(); i++)
{
uint64_t index = get_rgd_index_3d(points[i].coordinates_global, params.search_radious);
uint64_t index = get_rgd_index(points[i].coordinates_global, params.search_radious);
indexes.emplace_back(index, i);
}

Expand Down Expand Up @@ -53,7 +53,7 @@ bool LocalShapeFeatures::calculate_local_shape_features(std::vector<PointWithLoc
for (double z = -params.search_radious.z(); z <= params.search_radious.z(); z += params.search_radious.z())
{
Eigen::Vector3d position_global = source + Eigen::Vector3d(x, y, z);
uint64_t index_of_bucket = get_rgd_index_3d(position_global, params.search_radious);
uint64_t index_of_bucket = get_rgd_index(position_global, params.search_radious);

if (buckets.contains(index_of_bucket))
{
Expand Down
Loading