Open3D (C++ API)  0.12.0
Voxelize.h
Go to the documentation of this file.
1 // ----------------------------------------------------------------------------
2 // - Open3D: www.open3d.org -
3 // ----------------------------------------------------------------------------
4 // The MIT License (MIT)
5 //
6 // Copyright (c) 2019 www.open3d.org
7 //
8 // Permission is hereby granted, free of charge, to any person obtaining a copy
9 // of this software and associated documentation files (the "Software"), to deal
10 // in the Software without restriction, including without limitation the rights
11 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12 // copies of the Software, and to permit persons to whom the Software is
13 // furnished to do so, subject to the following conditions:
14 //
15 // The above copyright notice and this permission notice shall be included in
16 // all copies or substantial portions of the Software.
17 //
18 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
23 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
24 // IN THE SOFTWARE.
25 // ----------------------------------------------------------------------------
26 
27 #pragma once
28 
29 #include <tbb/parallel_for.h>
30 #include <tbb/parallel_sort.h>
31 
32 #include <vector>
33 
34 #include "open3d/core/Atomic.h"
35 #include "open3d/utility/MiniVec.h"
37 
38 namespace open3d {
39 namespace ml {
40 namespace impl {
41 
84 template <class T, int NDIM, class OUTPUT_ALLOCATOR>
85 void VoxelizeCPU(size_t num_points,
86  const T* const points,
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) {
93  using namespace open3d::utility;
94  typedef MiniVec<T, NDIM> Vec_t;
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);
98  MiniVec<int32_t, NDIM> extents =
99  ceil((points_range_max_vec - points_range_min_vec) * inv_voxel_size)
100  .template cast<int32_t>();
101  MiniVec<int64_t, NDIM> strides;
102  for (int i = 0; i < NDIM; ++i) {
103  strides[i] = 1;
104  for (int j = 0; j < i; ++j) {
105  strides[i] *= extents[j];
106  }
107  }
108  const int64_t invalid_hash = strides[NDIM - 1] * extents[NDIM - 1];
109 
110  auto CoordFn = [&](const Vec_t& point) {
111  auto coords = ((point - points_range_min_vec) * inv_voxel_size)
112  .template cast<int64_t>();
113  return coords;
114  };
115 
116  auto HashFn = [&](const Vec_t& point) {
117  if ((point >= points_range_min_vec && point <= points_range_max_vec)
118  .all()) {
119  auto coords = CoordFn(point);
120  int64_t linear_idx = coords.dot(strides);
121  return linear_idx;
122  }
123  return invalid_hash;
124  };
125 
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;
133  }
134  });
135  tbb::parallel_sort(hashes_indices);
136 
137  uint64_t num_voxels = 1;
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) {
143  core::AtomicFetchAddRelaxed(&num_voxels, 1);
144  }
145  }
146  });
147  if (invalid_hash == hashes_indices.back().first) {
148  --num_voxels;
149  }
150  num_voxels = std::min(int64_t(num_voxels), max_voxels);
151 
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,
156  num_voxels + 1);
157 
158  std::vector<int64_t> tmp_point_indices;
159  {
160  int64_t hash_i = 0; // index into the vector hashes_indices
161  for (int64_t voxel_i = 0; voxel_i < num_voxels; ++voxel_i) {
162  // compute voxel coord and the prefix sum value
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];
167  }
168  out_voxel_row_splits[voxel_i] = tmp_point_indices.size();
169 
170  // add up to max_points_per_voxel indices for this voxel
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) {
175  // new voxel starts -> break
176  break;
177  }
178  if (points_per_voxel < max_points_per_voxel) {
179  tmp_point_indices.push_back(hashes_indices[hash_i].second);
180  ++points_per_voxel;
181  }
182  }
183  }
184  out_voxel_row_splits[num_voxels] = tmp_point_indices.size();
185  }
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));
191 }
192 
193 } // namespace impl
194 } // namespace ml
195 } // namespace open3d
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
Definition: MiniVec.h:43