29 #include <tbb/parallel_for.h> 30 #include <tbb/parallel_sort.h> 84 template <
class T,
int NDIM,
class OUTPUT_ALLOCATOR>
87 const T*
const voxel_size,
88 const T*
const points_range_min,
89 const T*
const points_range_max,
90 const int64_t max_points_per_voxel,
91 const int64_t max_voxels,
92 OUTPUT_ALLOCATOR& output_allocator) {
95 const Vec_t inv_voxel_size = T(1) / Vec_t(voxel_size);
96 const Vec_t points_range_min_vec(points_range_min);
97 const Vec_t points_range_max_vec(points_range_max);
99 ceil((points_range_max_vec - points_range_min_vec) * inv_voxel_size)
100 .template cast<int32_t>();
102 for (
int i = 0; i < NDIM; ++i) {
104 for (
int j = 0; j < i; ++j) {
105 strides[i] *= extents[j];
108 const int64_t invalid_hash = strides[NDIM - 1] * extents[NDIM - 1];
110 auto CoordFn = [&](
const Vec_t& point) {
111 auto coords = ((point - points_range_min_vec) * inv_voxel_size)
112 .template cast<int64_t>();
116 auto HashFn = [&](
const Vec_t& point) {
117 if ((point >= points_range_min_vec && point <= points_range_max_vec)
119 auto coords = CoordFn(point);
120 int64_t linear_idx = coords.dot(strides);
126 std::vector<std::pair<int64_t, int64_t>> hashes_indices(num_points);
127 tbb::parallel_for(tbb::blocked_range<int64_t>(0, num_points),
128 [&](
const tbb::blocked_range<int64_t>& r) {
129 for (int64_t i = r.begin(); i != r.end(); ++i) {
130 Vec_t pos(points + NDIM * i);
131 hashes_indices[i].first = HashFn(pos);
132 hashes_indices[i].second = i;
135 tbb::parallel_sort(hashes_indices);
138 tbb::parallel_for(tbb::blocked_range<int64_t>(1, hashes_indices.size()),
139 [&](
const tbb::blocked_range<int64_t>& r) {
140 for (int64_t i = r.begin(); i != r.end(); ++i) {
141 if (hashes_indices[i - 1].first !=
142 hashes_indices[i].first) {
147 if (invalid_hash == hashes_indices.back().first) {
150 num_voxels = std::min(int64_t(num_voxels), max_voxels);
152 int32_t* out_voxel_coords =
nullptr;
153 output_allocator.AllocVoxelCoords(&out_voxel_coords, num_voxels, NDIM);
154 int64_t* out_voxel_row_splits =
nullptr;
155 output_allocator.AllocVoxelPointRowSplits(&out_voxel_row_splits,
158 std::vector<int64_t> tmp_point_indices;
161 for (int64_t voxel_i = 0; voxel_i < num_voxels; ++voxel_i) {
163 auto coord = CoordFn(
164 Vec_t(points + hashes_indices[hash_i].second * NDIM));
165 for (
int d = 0; d < NDIM; ++d) {
166 out_voxel_coords[voxel_i * NDIM + d] = coord[d];
168 out_voxel_row_splits[voxel_i] = tmp_point_indices.size();
171 int64_t points_per_voxel = 0;
172 const int64_t current_hash = hashes_indices[hash_i].first;
173 for (; hash_i < hashes_indices.size(); ++hash_i) {
174 if (current_hash != hashes_indices[hash_i].first) {
178 if (points_per_voxel < max_points_per_voxel) {
179 tmp_point_indices.push_back(hashes_indices[hash_i].second);
184 out_voxel_row_splits[num_voxels] = tmp_point_indices.size();
186 int64_t* out_point_indices =
nullptr;
187 output_allocator.AllocVoxelPointIndices(&out_point_indices,
188 tmp_point_indices.size());
189 memcpy(out_point_indices, tmp_point_indices.data(),
190 tmp_point_indices.size() *
sizeof(int64_t));
const char const char value recording_handle imu_sample recording_handle uint8_t size_t data_size k4a_record_configuration_t config target_format k4a_capture_t capture_handle k4a_imu_sample_t imu_sample playback_handle k4a_logging_message_cb_t void min_level device_handle k4a_imu_sample_t int32_t
Definition: K4aPlugin.cpp:398
FN_SPECIFIERS MiniVec< float, N > ceil(const MiniVec< float, N > &a)
Definition: MiniVec.h:108
const char const char value recording_handle imu_sample recording_handle uint8_t size_t data_size k4a_record_configuration_t config target_format k4a_capture_t capture_handle k4a_imu_sample_t imu_sample uint64_t
Definition: K4aPlugin.cpp:352
int points
Definition: FilePCD.cpp:73
Definition: PinholeCameraIntrinsic.cpp:35
void VoxelizeCPU(size_t num_points, const T *const points, const T *const voxel_size, const T *const points_range_min, const T *const points_range_max, const int64_t max_points_per_voxel, const int64_t max_voxels, OUTPUT_ALLOCATOR &output_allocator)
Definition: Voxelize.h:85
uint32_t AtomicFetchAddRelaxed(uint32_t *address, uint32_t val)
Definition: Atomic.h:40
Definition: Console.cpp:51