60 lines
2.2 KiB
C++
60 lines
2.2 KiB
C++
|
// --- utils_voxel.hpp ------------------------------------
|
|||
|
|
#pragma once
|
|||
|
|
#include <unordered_set>
|
|||
|
|
#include <cmath>
|
|||
|
|
#include <cstdint>
|
|||
|
|
#include <limits>
|
|||
|
|
#include <pcl/point_cloud.h>
|
|||
|
|
#include <pcl/point_types.h>
|
|||
|
|
|
|||
|
|
namespace VoxelLite {
|
|||
|
|
|
|||
|
|
// 21 bits por eixo -> m<>scara 0x1FFFFF
|
|||
|
|
static inline uint64_t pack_key_21b(int ix, int iy, int iz) {
|
|||
|
|
constexpr uint64_t MASK = (1ull << 21) - 1ull; // 0x1FFFFF
|
|||
|
|
constexpr int BIAS = 1 << 20; // para lidar com negativos
|
|||
|
|
|
|||
|
|
uint64_t kx = static_cast<uint64_t>(ix + BIAS) & MASK;
|
|||
|
|
uint64_t ky = static_cast<uint64_t>(iy + BIAS) & MASK;
|
|||
|
|
uint64_t kz = static_cast<uint64_t>(iz + BIAS) & MASK;
|
|||
|
|
|
|||
|
|
// [kx | ky | kz] -> 63 bits usados
|
|||
|
|
return (kx << 42) | (ky << 21) | kz;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Downsample para XYZ (mant<6E>m o 1<> ponto de cada voxel)
|
|||
|
|
inline pcl::PointCloud<pcl::PointXYZ>::Ptr
|
|||
|
|
DownsampleVoxelLikeXYZ(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& in, float leaf) {
|
|||
|
|
auto out = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
|
|||
|
|
if (!in || in->empty()) return out;
|
|||
|
|
|
|||
|
|
// guarda <20>voxels j<> vistos<6F>
|
|||
|
|
std::unordered_set<uint64_t> seen;
|
|||
|
|
seen.reserve(in->points.size()); // evita rehash
|
|||
|
|
out->points.reserve(in->points.size()); // upper bound; no final cabe<62>alho ajusta
|
|||
|
|
|
|||
|
|
const float inv_leaf = 1.0f / std::max(leaf, 1e-5f);
|
|||
|
|
|
|||
|
|
for (const auto& p : in->points) {
|
|||
|
|
if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) continue;
|
|||
|
|
|
|||
|
|
// voxel index
|
|||
|
|
const int ix = static_cast<int>(std::floor(p.x * inv_leaf));
|
|||
|
|
const int iy = static_cast<int>(std::floor(p.y * inv_leaf));
|
|||
|
|
const int iz = static_cast<int>(std::floor(p.z * inv_leaf));
|
|||
|
|
|
|||
|
|
const uint64_t key = pack_key_21b(ix, iy, iz);
|
|||
|
|
auto [it, inserted] = seen.insert(key);
|
|||
|
|
if (inserted) {
|
|||
|
|
out->points.push_back(p); // mant<6E>m o primeiro ponto desse voxel
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
out->width = static_cast<uint32_t>(out->points.size());
|
|||
|
|
out->height = 1;
|
|||
|
|
out->is_dense = false;
|
|||
|
|
return out;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
} // namespace VoxelLite
|