Open3D (C++ API)  0.18.0+252c867
VoxelBlockGridImpl.h
Go to the documentation of this file.
1 // ----------------------------------------------------------------------------
2 // - Open3D: www.open3d.org -
3 // ----------------------------------------------------------------------------
4 // Copyright (c) 2018-2023 www.open3d.org
5 // SPDX-License-Identifier: MIT
6 // ----------------------------------------------------------------------------
7 
8 #include <atomic>
9 #include <cmath>
10 
11 #include "open3d/core/Dispatch.h"
12 #include "open3d/core/Dtype.h"
14 #include "open3d/core/SizeVector.h"
15 #include "open3d/core/Tensor.h"
21 #include "open3d/utility/Logging.h"
22 #include "open3d/utility/Timer.h"
23 
24 namespace open3d {
25 namespace t {
26 namespace geometry {
27 namespace kernel {
28 namespace voxel_grid {
29 
30 using index_t = int;
32 
33 #if defined(__CUDACC__)
34 void GetVoxelCoordinatesAndFlattenedIndicesCUDA
35 #else
37 #endif
38  (const core::Tensor& buf_indices,
39  const core::Tensor& block_keys,
40  core::Tensor& voxel_coords,
41  core::Tensor& flattened_indices,
42  index_t resolution,
43  float voxel_size) {
44  core::Device device = buf_indices.GetDevice();
45 
46  const index_t* buf_indices_ptr = buf_indices.GetDataPtr<index_t>();
47  const index_t* block_key_ptr = block_keys.GetDataPtr<index_t>();
48 
49  float* voxel_coords_ptr = voxel_coords.GetDataPtr<float>();
50  int64_t* flattened_indices_ptr = flattened_indices.GetDataPtr<int64_t>();
51 
52  index_t n = flattened_indices.GetLength();
53  ArrayIndexer voxel_indexer({resolution, resolution, resolution});
54  index_t resolution3 = resolution * resolution * resolution;
55 
56  core::ParallelFor(device, n, [=] OPEN3D_DEVICE(index_t workload_idx) {
57  index_t block_idx = buf_indices_ptr[workload_idx / resolution3];
58  index_t voxel_idx = workload_idx % resolution3;
59 
60  index_t block_key_offset = block_idx * 3;
61  index_t xb = block_key_ptr[block_key_offset + 0];
62  index_t yb = block_key_ptr[block_key_offset + 1];
63  index_t zb = block_key_ptr[block_key_offset + 2];
64 
65  index_t xv, yv, zv;
66  voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
67 
68  float x = (xb * resolution + xv) * voxel_size;
69  float y = (yb * resolution + yv) * voxel_size;
70  float z = (zb * resolution + zv) * voxel_size;
71 
72  flattened_indices_ptr[workload_idx] =
73  block_idx * resolution3 + voxel_idx;
74 
75  index_t voxel_coords_offset = workload_idx * 3;
76  voxel_coords_ptr[voxel_coords_offset + 0] = x;
77  voxel_coords_ptr[voxel_coords_offset + 1] = y;
78  voxel_coords_ptr[voxel_coords_offset + 2] = z;
79  });
80 }
81 
84  index_t yo,
85  index_t zo,
86  index_t curr_block_idx,
87  index_t resolution,
88  const ArrayIndexer& nb_block_masks_indexer,
89  const ArrayIndexer& nb_block_indices_indexer) {
90  index_t xn = (xo + resolution) % resolution;
91  index_t yn = (yo + resolution) % resolution;
92  index_t zn = (zo + resolution) % resolution;
93 
94  index_t dxb = Sign(xo - xn);
95  index_t dyb = Sign(yo - yn);
96  index_t dzb = Sign(zo - zn);
97 
98  index_t nb_idx = (dxb + 1) + (dyb + 1) * 3 + (dzb + 1) * 9;
99 
100  bool block_mask_i =
101  *nb_block_masks_indexer.GetDataPtr<bool>(curr_block_idx, nb_idx);
102  if (!block_mask_i) return -1;
103 
104  index_t block_idx_i = *nb_block_indices_indexer.GetDataPtr<index_t>(
105  curr_block_idx, nb_idx);
106 
107  return (((block_idx_i * resolution) + zn) * resolution + yn) * resolution +
108  xn;
109 }
110 
111 template <typename tsdf_t>
113  const tsdf_t* tsdf_base_ptr,
114  index_t xo,
115  index_t yo,
116  index_t zo,
117  index_t curr_block_idx,
118  float* n,
119  index_t resolution,
120  const ArrayIndexer& nb_block_masks_indexer,
121  const ArrayIndexer& nb_block_indices_indexer) {
122  auto GetLinearIdx = [&] OPEN3D_DEVICE(index_t xo, index_t yo,
123  index_t zo) -> index_t {
124  return DeviceGetLinearIdx(xo, yo, zo, curr_block_idx, resolution,
125  nb_block_masks_indexer,
126  nb_block_indices_indexer);
127  };
128  index_t vxp = GetLinearIdx(xo + 1, yo, zo);
129  index_t vxn = GetLinearIdx(xo - 1, yo, zo);
130  index_t vyp = GetLinearIdx(xo, yo + 1, zo);
131  index_t vyn = GetLinearIdx(xo, yo - 1, zo);
132  index_t vzp = GetLinearIdx(xo, yo, zo + 1);
133  index_t vzn = GetLinearIdx(xo, yo, zo - 1);
134  if (vxp >= 0 && vxn >= 0) n[0] = tsdf_base_ptr[vxp] - tsdf_base_ptr[vxn];
135  if (vyp >= 0 && vyn >= 0) n[1] = tsdf_base_ptr[vyp] - tsdf_base_ptr[vyn];
136  if (vzp >= 0 && vzn >= 0) n[2] = tsdf_base_ptr[vzp] - tsdf_base_ptr[vzn];
137 };
138 
139 template <typename input_depth_t,
140  typename input_color_t,
141  typename tsdf_t,
142  typename weight_t,
143  typename color_t>
144 #if defined(__CUDACC__)
145 void IntegrateCUDA
146 #else
148 #endif
149  (const core::Tensor& depth,
150  const core::Tensor& color,
151  const core::Tensor& indices,
152  const core::Tensor& block_keys,
153  TensorMap& block_value_map,
154  const core::Tensor& depth_intrinsic,
155  const core::Tensor& color_intrinsic,
156  const core::Tensor& extrinsics,
157  index_t resolution,
158  float voxel_size,
159  float sdf_trunc,
160  float depth_scale,
161  float depth_max) {
162  // Parameters
163  index_t resolution2 = resolution * resolution;
164  index_t resolution3 = resolution2 * resolution;
165 
166  TransformIndexer transform_indexer(depth_intrinsic, extrinsics, voxel_size);
167  TransformIndexer colormap_indexer(
168  color_intrinsic,
170 
171  ArrayIndexer voxel_indexer({resolution, resolution, resolution});
172 
173  ArrayIndexer block_keys_indexer(block_keys, 1);
174  ArrayIndexer depth_indexer(depth, 2);
175  core::Device device = block_keys.GetDevice();
176 
177  const index_t* indices_ptr = indices.GetDataPtr<index_t>();
178 
179  if (!block_value_map.Contains("tsdf") ||
180  !block_value_map.Contains("weight")) {
182  "TSDF and/or weight not allocated in blocks, please implement "
183  "customized integration.");
184  }
185  tsdf_t* tsdf_base_ptr = block_value_map.at("tsdf").GetDataPtr<tsdf_t>();
186  weight_t* weight_base_ptr =
187  block_value_map.at("weight").GetDataPtr<weight_t>();
188 
189  bool integrate_color =
190  block_value_map.Contains("color") && color.NumElements() > 0;
191  color_t* color_base_ptr = nullptr;
192  ArrayIndexer color_indexer;
193 
194  float color_multiplier = 1.0;
195  if (integrate_color) {
196  color_base_ptr = block_value_map.at("color").GetDataPtr<color_t>();
197  color_indexer = ArrayIndexer(color, 2);
198 
199  // Float32: [0, 1] -> [0, 255]
200  if (color.GetDtype() == core::Float32) {
201  color_multiplier = 255.0;
202  }
203  }
204 
205  index_t n = indices.GetLength() * resolution3;
206  core::ParallelFor(device, n, [=] OPEN3D_DEVICE(index_t workload_idx) {
207  // Natural index (0, N) -> (block_idx, voxel_idx)
208  index_t block_idx = indices_ptr[workload_idx / resolution3];
209  index_t voxel_idx = workload_idx % resolution3;
210 
212  // block_idx -> (x_block, y_block, z_block)
213  index_t* block_key_ptr =
214  block_keys_indexer.GetDataPtr<index_t>(block_idx);
215  index_t xb = block_key_ptr[0];
216  index_t yb = block_key_ptr[1];
217  index_t zb = block_key_ptr[2];
218 
219  // voxel_idx -> (x_voxel, y_voxel, z_voxel)
220  index_t xv, yv, zv;
221  voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
222 
223  // coordinate in world (in voxel)
224  index_t x = xb * resolution + xv;
225  index_t y = yb * resolution + yv;
226  index_t z = zb * resolution + zv;
227 
228  // coordinate in camera (in voxel -> in meter)
229  float xc, yc, zc, u, v;
230  transform_indexer.RigidTransform(static_cast<float>(x),
231  static_cast<float>(y),
232  static_cast<float>(z), &xc, &yc, &zc);
233 
234  // coordinate in image (in pixel)
235  transform_indexer.Project(xc, yc, zc, &u, &v);
236  if (!depth_indexer.InBoundary(u, v)) {
237  return;
238  }
239 
240  index_t ui = static_cast<index_t>(u);
241  index_t vi = static_cast<index_t>(v);
242 
243  // Associate image workload and compute SDF and
244  // TSDF.
245  float depth =
246  *depth_indexer.GetDataPtr<input_depth_t>(ui, vi) / depth_scale;
247 
248  float sdf = depth - zc;
249  if (depth <= 0 || depth > depth_max || zc <= 0 || sdf < -sdf_trunc) {
250  return;
251  }
252  sdf = sdf < sdf_trunc ? sdf : sdf_trunc;
253  sdf /= sdf_trunc;
254 
255  index_t linear_idx = block_idx * resolution3 + voxel_idx;
256 
257  tsdf_t* tsdf_ptr = tsdf_base_ptr + linear_idx;
258  weight_t* weight_ptr = weight_base_ptr + linear_idx;
259 
260  float inv_wsum = 1.0f / (*weight_ptr + 1);
261  float weight = *weight_ptr;
262  *tsdf_ptr = (weight * (*tsdf_ptr) + sdf) * inv_wsum;
263 
264  if (integrate_color) {
265  color_t* color_ptr = color_base_ptr + 3 * linear_idx;
266 
267  // Unproject ui, vi with depth_intrinsic, then project back with
268  // color_intrinsic
269  float x, y, z;
270  transform_indexer.Unproject(ui, vi, 1.0, &x, &y, &z);
271 
272  float uf, vf;
273  colormap_indexer.Project(x, y, z, &uf, &vf);
274  if (color_indexer.InBoundary(uf, vf)) {
275  ui = round(uf);
276  vi = round(vf);
277 
278  input_color_t* input_color_ptr =
279  color_indexer.GetDataPtr<input_color_t>(ui, vi);
280 
281  for (index_t i = 0; i < 3; ++i) {
282  color_ptr[i] = (weight * color_ptr[i] +
283  input_color_ptr[i] * color_multiplier) *
284  inv_wsum;
285  }
286  }
287  }
288  *weight_ptr = weight + 1;
289  });
290 
291 #if defined(__CUDACC__)
293 #endif
294 }
295 
296 #if defined(__CUDACC__)
297 void EstimateRangeCUDA
298 #else
300 #endif
301  (const core::Tensor& block_keys,
302  core::Tensor& range_minmax_map,
303  const core::Tensor& intrinsics,
304  const core::Tensor& extrinsics,
305  int h,
306  int w,
307  int down_factor,
308  int64_t block_resolution,
309  float voxel_size,
310  float depth_min,
311  float depth_max,
312  core::Tensor& fragment_buffer) {
313 
314  // TODO(wei): reserve it in a reusable buffer
315 
316  // Every 2 channels: (min, max)
317  int h_down = h / down_factor;
318  int w_down = w / down_factor;
319  range_minmax_map = core::Tensor({h_down, w_down, 2}, core::Float32,
320  block_keys.GetDevice());
321  NDArrayIndexer range_map_indexer(range_minmax_map, 2);
322 
323  // Every 6 channels: (v_min, u_min, v_max, u_max, z_min, z_max)
324  const int fragment_size = 16;
325 
326  if (fragment_buffer.GetDataPtr() == 0 ||
327  fragment_buffer.NumElements() == 0) {
328  // Rough heuristic; should tend to overallocate
329  const int reserve_frag_buffer_size =
330  h_down * w_down / (fragment_size * fragment_size) / voxel_size;
331  fragment_buffer = core::Tensor({reserve_frag_buffer_size, 6},
332  core::Float32, block_keys.GetDevice());
333  }
334 
335  const int frag_buffer_size = fragment_buffer.NumElements() / 6;
336 
337  NDArrayIndexer frag_buffer_indexer(fragment_buffer, 1);
338  NDArrayIndexer block_keys_indexer(block_keys, 1);
339  TransformIndexer w2c_transform_indexer(intrinsics, extrinsics);
340 #if defined(__CUDACC__)
341  core::Tensor count(std::vector<int>{0}, {1}, core::Int32,
342  block_keys.GetDevice());
343  int* count_ptr = count.GetDataPtr<int>();
344 #else
345  std::atomic<int> count_atomic(0);
346  std::atomic<int>* count_ptr = &count_atomic;
347 #endif
348 
349 #ifndef __CUDACC__
350  using std::max;
351  using std::min;
352 #endif
353 
354  // Pass 0: iterate over blocks, fill-in an rendering fragment array
356  block_keys.GetDevice(), block_keys.GetLength(),
357  [=] OPEN3D_DEVICE(int64_t workload_idx) {
358  int* key = block_keys_indexer.GetDataPtr<int>(workload_idx);
359 
360  int u_min = w_down - 1, v_min = h_down - 1, u_max = 0,
361  v_max = 0;
362  float z_min = depth_max, z_max = depth_min;
363 
364  float xc, yc, zc, u, v;
365 
366  // Project 8 corners to low-res image and form a rectangle
367  for (int i = 0; i < 8; ++i) {
368  float xw = (key[0] + ((i & 1) > 0)) * block_resolution *
369  voxel_size;
370  float yw = (key[1] + ((i & 2) > 0)) * block_resolution *
371  voxel_size;
372  float zw = (key[2] + ((i & 4) > 0)) * block_resolution *
373  voxel_size;
374 
375  w2c_transform_indexer.RigidTransform(xw, yw, zw, &xc, &yc,
376  &zc);
377  if (zc <= 0) continue;
378 
379  // Project to the down sampled image buffer
380  w2c_transform_indexer.Project(xc, yc, zc, &u, &v);
381  u /= down_factor;
382  v /= down_factor;
383 
384  v_min = min(static_cast<int>(floorf(v)), v_min);
385  v_max = max(static_cast<int>(ceilf(v)), v_max);
386 
387  u_min = min(static_cast<int>(floorf(u)), u_min);
388  u_max = max(static_cast<int>(ceilf(u)), u_max);
389 
390  z_min = min(z_min, zc);
391  z_max = max(z_max, zc);
392  }
393 
394  v_min = max(0, v_min);
395  v_max = min(h_down - 1, v_max);
396 
397  u_min = max(0, u_min);
398  u_max = min(w_down - 1, u_max);
399 
400  if (v_min >= v_max || u_min >= u_max || z_min >= z_max) return;
401 
402  // Divide the rectangle into small 16x16 fragments
403  int frag_v_count =
404  ceilf(float(v_max - v_min + 1) / float(fragment_size));
405  int frag_u_count =
406  ceilf(float(u_max - u_min + 1) / float(fragment_size));
407 
408  int frag_count = frag_v_count * frag_u_count;
409  int frag_count_start = OPEN3D_ATOMIC_ADD(count_ptr, frag_count);
410  int frag_count_end = frag_count_start + frag_count;
411  if (frag_count_end >= frag_buffer_size) {
412  return;
413  }
414 
415  int offset = 0;
416  for (int frag_v = 0; frag_v < frag_v_count; ++frag_v) {
417  for (int frag_u = 0; frag_u < frag_u_count;
418  ++frag_u, ++offset) {
419  float* frag_ptr = frag_buffer_indexer.GetDataPtr<float>(
420  frag_count_start + offset);
421  // zmin, zmax
422  frag_ptr[0] = z_min;
423  frag_ptr[1] = z_max;
424 
425  // vmin, umin
426  frag_ptr[2] = v_min + frag_v * fragment_size;
427  frag_ptr[3] = u_min + frag_u * fragment_size;
428 
429  // vmax, umax
430  frag_ptr[4] = min(frag_ptr[2] + fragment_size - 1,
431  static_cast<float>(v_max));
432  frag_ptr[5] = min(frag_ptr[3] + fragment_size - 1,
433  static_cast<float>(u_max));
434  }
435  }
436  });
437 #if defined(__CUDACC__)
438  int needed_frag_count = count[0].Item<int>();
439 #else
440  int needed_frag_count = (*count_ptr).load();
441 #endif
442 
443  int frag_count = needed_frag_count;
444  if (frag_count >= frag_buffer_size) {
446  "Could not generate full range map; allocated {} fragments but "
447  "needed {}",
448  frag_buffer_size, frag_count);
449  frag_count = frag_buffer_size - 1;
450  } else {
451  utility::LogDebug("EstimateRange Allocated {} fragments and needed {}",
452  frag_buffer_size, frag_count);
453  }
454 
455  // Pass 0.5: Fill in range map to prepare for atomic min/max
456  core::ParallelFor(block_keys.GetDevice(), h_down * w_down,
457  [=] OPEN3D_DEVICE(int64_t workload_idx) {
458  int v = workload_idx / w_down;
459  int u = workload_idx % w_down;
460  float* range_ptr =
461  range_map_indexer.GetDataPtr<float>(u, v);
462  range_ptr[0] = depth_max;
463  range_ptr[1] = depth_min;
464  });
465 
466  // Pass 1: iterate over rendering fragment array, fill-in range
468  block_keys.GetDevice(), frag_count * fragment_size * fragment_size,
469  [=] OPEN3D_DEVICE(int64_t workload_idx) {
470  int frag_idx = workload_idx / (fragment_size * fragment_size);
471  int local_idx = workload_idx % (fragment_size * fragment_size);
472  int dv = local_idx / fragment_size;
473  int du = local_idx % fragment_size;
474 
475  float* frag_ptr =
476  frag_buffer_indexer.GetDataPtr<float>(frag_idx);
477  int v_min = static_cast<int>(frag_ptr[2]);
478  int u_min = static_cast<int>(frag_ptr[3]);
479  int v_max = static_cast<int>(frag_ptr[4]);
480  int u_max = static_cast<int>(frag_ptr[5]);
481 
482  int v = v_min + dv;
483  int u = u_min + du;
484  if (v > v_max || u > u_max) return;
485 
486  float z_min = frag_ptr[0];
487  float z_max = frag_ptr[1];
488  float* range_ptr = range_map_indexer.GetDataPtr<float>(u, v);
489 #ifdef __CUDACC__
490  atomicMinf(&(range_ptr[0]), z_min);
491  atomicMaxf(&(range_ptr[1]), z_max);
492 #else
493 #pragma omp critical(EstimateRangeCPU)
494  {
495  range_ptr[0] = min(z_min, range_ptr[0]);
496  range_ptr[1] = max(z_max, range_ptr[1]);
497  }
498 #endif
499  });
500 
501 #if defined(__CUDACC__)
503 #endif
504 
505  if (needed_frag_count != frag_count) {
506  utility::LogInfo("Reallocating {} fragments for EstimateRange (was {})",
507  needed_frag_count, frag_count);
508 
509  fragment_buffer = core::Tensor({needed_frag_count, 6}, core::Float32,
510  block_keys.GetDevice());
511  }
512 }
513 
514 struct MiniVecCache {
519 
521  return (xin == x && yin == y && zin == z) ? block_idx : -1;
522  }
523 
524  inline void OPEN3D_DEVICE Update(index_t xin,
525  index_t yin,
526  index_t zin,
527  index_t block_idx_in) {
528  x = xin;
529  y = yin;
530  z = zin;
531  block_idx = block_idx_in;
532  }
533 };
534 
535 template <typename tsdf_t, typename weight_t, typename color_t>
536 #if defined(__CUDACC__)
537 void RayCastCUDA
538 #else
540 #endif
541  (std::shared_ptr<core::HashMap>& hashmap,
542  const TensorMap& block_value_map,
543  const core::Tensor& range,
544  TensorMap& renderings_map,
545  const core::Tensor& intrinsic,
546  const core::Tensor& extrinsics,
547  index_t h,
548  index_t w,
549  index_t block_resolution,
550  float voxel_size,
551  float depth_scale,
552  float depth_min,
553  float depth_max,
554  float weight_threshold,
555  float trunc_voxel_multiplier,
556  int range_map_down_factor) {
557  using Key = utility::MiniVec<index_t, 3>;
560 
561  auto device_hashmap = hashmap->GetDeviceHashBackend();
562 #if defined(__CUDACC__)
563  auto cuda_hashmap =
564  std::dynamic_pointer_cast<core::StdGPUHashBackend<Key, Hash, Eq>>(
565  device_hashmap);
566  if (cuda_hashmap == nullptr) {
568  "Unsupported backend: CUDA raycasting only supports STDGPU.");
569  }
570  auto hashmap_impl = cuda_hashmap->GetImpl();
571 #else
572  auto cpu_hashmap =
573  std::dynamic_pointer_cast<core::TBBHashBackend<Key, Hash, Eq>>(
574  device_hashmap);
575  if (cpu_hashmap == nullptr) {
577  "Unsupported backend: CPU raycasting only supports TBB.");
578  }
579  auto hashmap_impl = *cpu_hashmap->GetImpl();
580 #endif
581 
582  core::Device device = hashmap->GetDevice();
583 
584  ArrayIndexer range_indexer(range, 2);
585 
586  // Geometry
587  ArrayIndexer depth_indexer;
588  ArrayIndexer vertex_indexer;
589  ArrayIndexer normal_indexer;
590 
591  // Diff rendering
592  ArrayIndexer index_indexer;
593  ArrayIndexer mask_indexer;
594  ArrayIndexer interp_ratio_indexer;
595  ArrayIndexer interp_ratio_dx_indexer;
596  ArrayIndexer interp_ratio_dy_indexer;
597  ArrayIndexer interp_ratio_dz_indexer;
598 
599  // Color
600  ArrayIndexer color_indexer;
601 
602  if (!block_value_map.Contains("tsdf") ||
603  !block_value_map.Contains("weight")) {
605  "TSDF and/or weight not allocated in blocks, please implement "
606  "customized integration.");
607  }
608  const tsdf_t* tsdf_base_ptr =
609  block_value_map.at("tsdf").GetDataPtr<tsdf_t>();
610  const weight_t* weight_base_ptr =
611  block_value_map.at("weight").GetDataPtr<weight_t>();
612 
613  // Geometry
614  if (renderings_map.Contains("depth")) {
615  depth_indexer = ArrayIndexer(renderings_map.at("depth"), 2);
616  }
617  if (renderings_map.Contains("vertex")) {
618  vertex_indexer = ArrayIndexer(renderings_map.at("vertex"), 2);
619  }
620  if (renderings_map.Contains("normal")) {
621  normal_indexer = ArrayIndexer(renderings_map.at("normal"), 2);
622  }
623 
624  // Diff rendering
625  if (renderings_map.Contains("index")) {
626  index_indexer = ArrayIndexer(renderings_map.at("index"), 2);
627  }
628  if (renderings_map.Contains("mask")) {
629  mask_indexer = ArrayIndexer(renderings_map.at("mask"), 2);
630  }
631  if (renderings_map.Contains("interp_ratio")) {
632  interp_ratio_indexer =
633  ArrayIndexer(renderings_map.at("interp_ratio"), 2);
634  }
635  if (renderings_map.Contains("interp_ratio_dx")) {
636  interp_ratio_dx_indexer =
637  ArrayIndexer(renderings_map.at("interp_ratio_dx"), 2);
638  }
639  if (renderings_map.Contains("interp_ratio_dy")) {
640  interp_ratio_dy_indexer =
641  ArrayIndexer(renderings_map.at("interp_ratio_dy"), 2);
642  }
643  if (renderings_map.Contains("interp_ratio_dz")) {
644  interp_ratio_dz_indexer =
645  ArrayIndexer(renderings_map.at("interp_ratio_dz"), 2);
646  }
647 
648  // Color
649  bool render_color = false;
650  if (block_value_map.Contains("color") && renderings_map.Contains("color")) {
651  render_color = true;
652  color_indexer = ArrayIndexer(renderings_map.at("color"), 2);
653  }
654  const color_t* color_base_ptr =
655  render_color ? block_value_map.at("color").GetDataPtr<color_t>()
656  : nullptr;
657 
658  bool visit_neighbors = render_color || normal_indexer.GetDataPtr() ||
659  mask_indexer.GetDataPtr() ||
660  index_indexer.GetDataPtr() ||
661  interp_ratio_indexer.GetDataPtr() ||
662  interp_ratio_dx_indexer.GetDataPtr() ||
663  interp_ratio_dy_indexer.GetDataPtr() ||
664  interp_ratio_dz_indexer.GetDataPtr();
665 
666  TransformIndexer c2w_transform_indexer(
667  intrinsic, t::geometry::InverseTransformation(extrinsics));
668  TransformIndexer w2c_transform_indexer(intrinsic, extrinsics);
669 
670  index_t rows = h;
671  index_t cols = w;
672  index_t n = rows * cols;
673 
674  float block_size = voxel_size * block_resolution;
675  index_t resolution2 = block_resolution * block_resolution;
676  index_t resolution3 = resolution2 * block_resolution;
677 
678 #ifndef __CUDACC__
679  using std::max;
680  using std::sqrt;
681 #endif
682 
683  core::ParallelFor(device, n, [=] OPEN3D_DEVICE(index_t workload_idx) {
684  auto GetLinearIdxAtP = [&] OPEN3D_DEVICE(
685  index_t x_b, index_t y_b, index_t z_b,
686  index_t x_v, index_t y_v, index_t z_v,
687  core::buf_index_t block_buf_idx,
688  MiniVecCache & cache) -> index_t {
689  index_t x_vn = (x_v + block_resolution) % block_resolution;
690  index_t y_vn = (y_v + block_resolution) % block_resolution;
691  index_t z_vn = (z_v + block_resolution) % block_resolution;
692 
693  index_t dx_b = Sign(x_v - x_vn);
694  index_t dy_b = Sign(y_v - y_vn);
695  index_t dz_b = Sign(z_v - z_vn);
696 
697  if (dx_b == 0 && dy_b == 0 && dz_b == 0) {
698  return block_buf_idx * resolution3 + z_v * resolution2 +
699  y_v * block_resolution + x_v;
700  } else {
701  Key key(x_b + dx_b, y_b + dy_b, z_b + dz_b);
702 
703  index_t block_buf_idx = cache.Check(key[0], key[1], key[2]);
704  if (block_buf_idx < 0) {
705  auto iter = hashmap_impl.find(key);
706  if (iter == hashmap_impl.end()) return -1;
707  block_buf_idx = iter->second;
708  cache.Update(key[0], key[1], key[2], block_buf_idx);
709  }
710 
711  return block_buf_idx * resolution3 + z_vn * resolution2 +
712  y_vn * block_resolution + x_vn;
713  }
714  };
715 
716  auto GetLinearIdxAtT = [&] OPEN3D_DEVICE(
717  float x_o, float y_o, float z_o,
718  float x_d, float y_d, float z_d, float t,
719  MiniVecCache& cache) -> index_t {
720  float x_g = x_o + t * x_d;
721  float y_g = y_o + t * y_d;
722  float z_g = z_o + t * z_d;
723 
724  // MiniVec coordinate and look up
725  index_t x_b = static_cast<index_t>(floorf(x_g / block_size));
726  index_t y_b = static_cast<index_t>(floorf(y_g / block_size));
727  index_t z_b = static_cast<index_t>(floorf(z_g / block_size));
728 
729  Key key(x_b, y_b, z_b);
730  index_t block_buf_idx = cache.Check(x_b, y_b, z_b);
731  if (block_buf_idx < 0) {
732  auto iter = hashmap_impl.find(key);
733  if (iter == hashmap_impl.end()) return -1;
734  block_buf_idx = iter->second;
735  cache.Update(x_b, y_b, z_b, block_buf_idx);
736  }
737 
738  // Voxel coordinate and look up
739  index_t x_v = index_t((x_g - x_b * block_size) / voxel_size);
740  index_t y_v = index_t((y_g - y_b * block_size) / voxel_size);
741  index_t z_v = index_t((z_g - z_b * block_size) / voxel_size);
742 
743  return block_buf_idx * resolution3 + z_v * resolution2 +
744  y_v * block_resolution + x_v;
745  };
746 
747  index_t y = workload_idx / cols;
748  index_t x = workload_idx % cols;
749 
750  const float* range = range_indexer.GetDataPtr<float>(
751  x / range_map_down_factor, y / range_map_down_factor);
752 
753  float* depth_ptr = nullptr;
754  float* vertex_ptr = nullptr;
755  float* color_ptr = nullptr;
756  float* normal_ptr = nullptr;
757 
758  int64_t* index_ptr = nullptr;
759  bool* mask_ptr = nullptr;
760  float* interp_ratio_ptr = nullptr;
761  float* interp_ratio_dx_ptr = nullptr;
762  float* interp_ratio_dy_ptr = nullptr;
763  float* interp_ratio_dz_ptr = nullptr;
764 
765  if (vertex_indexer.GetDataPtr()) {
766  vertex_ptr = vertex_indexer.GetDataPtr<float>(x, y);
767  vertex_ptr[0] = 0;
768  vertex_ptr[1] = 0;
769  vertex_ptr[2] = 0;
770  }
771  if (depth_indexer.GetDataPtr()) {
772  depth_ptr = depth_indexer.GetDataPtr<float>(x, y);
773  depth_ptr[0] = 0;
774  }
775  if (normal_indexer.GetDataPtr()) {
776  normal_ptr = normal_indexer.GetDataPtr<float>(x, y);
777  normal_ptr[0] = 0;
778  normal_ptr[1] = 0;
779  normal_ptr[2] = 0;
780  }
781 
782  if (mask_indexer.GetDataPtr()) {
783  mask_ptr = mask_indexer.GetDataPtr<bool>(x, y);
784 #ifdef __CUDACC__
785 #pragma unroll
786 #endif
787  for (int i = 0; i < 8; ++i) {
788  mask_ptr[i] = false;
789  }
790  }
791  if (index_indexer.GetDataPtr()) {
792  index_ptr = index_indexer.GetDataPtr<int64_t>(x, y);
793 #ifdef __CUDACC__
794 #pragma unroll
795 #endif
796  for (int i = 0; i < 8; ++i) {
797  index_ptr[i] = 0;
798  }
799  }
800  if (interp_ratio_indexer.GetDataPtr()) {
801  interp_ratio_ptr = interp_ratio_indexer.GetDataPtr<float>(x, y);
802 #ifdef __CUDACC__
803 #pragma unroll
804 #endif
805  for (int i = 0; i < 8; ++i) {
806  interp_ratio_ptr[i] = 0;
807  }
808  }
809  if (interp_ratio_dx_indexer.GetDataPtr()) {
810  interp_ratio_dx_ptr =
811  interp_ratio_dx_indexer.GetDataPtr<float>(x, y);
812 #ifdef __CUDACC__
813 #pragma unroll
814 #endif
815  for (int i = 0; i < 8; ++i) {
816  interp_ratio_dx_ptr[i] = 0;
817  }
818  }
819  if (interp_ratio_dy_indexer.GetDataPtr()) {
820  interp_ratio_dy_ptr =
821  interp_ratio_dy_indexer.GetDataPtr<float>(x, y);
822 #ifdef __CUDACC__
823 #pragma unroll
824 #endif
825  for (int i = 0; i < 8; ++i) {
826  interp_ratio_dy_ptr[i] = 0;
827  }
828  }
829  if (interp_ratio_dz_indexer.GetDataPtr()) {
830  interp_ratio_dz_ptr =
831  interp_ratio_dz_indexer.GetDataPtr<float>(x, y);
832 #ifdef __CUDACC__
833 #pragma unroll
834 #endif
835  for (int i = 0; i < 8; ++i) {
836  interp_ratio_dz_ptr[i] = 0;
837  }
838  }
839 
840  if (color_indexer.GetDataPtr()) {
841  color_ptr = color_indexer.GetDataPtr<float>(x, y);
842  color_ptr[0] = 0;
843  color_ptr[1] = 0;
844  color_ptr[2] = 0;
845  }
846 
847  float t = range[0];
848  const float t_max = range[1];
849  if (t >= t_max) return;
850 
851  // Coordinates in camera and global
852  float x_c = 0, y_c = 0, z_c = 0;
853  float x_g = 0, y_g = 0, z_g = 0;
854  float x_o = 0, y_o = 0, z_o = 0;
855 
856  // Iterative ray intersection check
857  float t_prev = t;
858 
859  float tsdf_prev = -1.0f;
860  float tsdf = 1.0;
861  float sdf_trunc = voxel_size * trunc_voxel_multiplier;
862  float w = 0.0;
863 
864  // Camera origin
865  c2w_transform_indexer.RigidTransform(0, 0, 0, &x_o, &y_o, &z_o);
866 
867  // Direction
868  c2w_transform_indexer.Unproject(static_cast<float>(x),
869  static_cast<float>(y), 1.0f, &x_c, &y_c,
870  &z_c);
871  c2w_transform_indexer.RigidTransform(x_c, y_c, z_c, &x_g, &y_g, &z_g);
872  float x_d = (x_g - x_o);
873  float y_d = (y_g - y_o);
874  float z_d = (z_g - z_o);
875 
876  MiniVecCache cache{0, 0, 0, -1};
877  bool surface_found = false;
878  while (t < t_max) {
879  index_t linear_idx =
880  GetLinearIdxAtT(x_o, y_o, z_o, x_d, y_d, z_d, t, cache);
881 
882  if (linear_idx < 0) {
883  t_prev = t;
884  t += block_size;
885  } else {
886  tsdf_prev = tsdf;
887  tsdf = tsdf_base_ptr[linear_idx];
888  w = weight_base_ptr[linear_idx];
889  if (tsdf_prev > 0 && w >= weight_threshold && tsdf <= 0) {
890  surface_found = true;
891  break;
892  }
893  t_prev = t;
894  float delta = tsdf * sdf_trunc;
895  t += delta < voxel_size ? voxel_size : delta;
896  }
897  }
898 
899  if (surface_found) {
900  float t_intersect =
901  (t * tsdf_prev - t_prev * tsdf) / (tsdf_prev - tsdf);
902  x_g = x_o + t_intersect * x_d;
903  y_g = y_o + t_intersect * y_d;
904  z_g = z_o + t_intersect * z_d;
905 
906  // Trivial vertex assignment
907  if (depth_ptr) {
908  *depth_ptr = t_intersect * depth_scale;
909  }
910  if (vertex_ptr) {
911  w2c_transform_indexer.RigidTransform(
912  x_g, y_g, z_g, vertex_ptr + 0, vertex_ptr + 1,
913  vertex_ptr + 2);
914  }
915  if (!visit_neighbors) return;
916 
917  // Trilinear interpolation
918  // TODO(wei): simplify the flow by splitting the
919  // functions given what is enabled
920  index_t x_b = static_cast<index_t>(floorf(x_g / block_size));
921  index_t y_b = static_cast<index_t>(floorf(y_g / block_size));
922  index_t z_b = static_cast<index_t>(floorf(z_g / block_size));
923  float x_v = (x_g - float(x_b) * block_size) / voxel_size;
924  float y_v = (y_g - float(y_b) * block_size) / voxel_size;
925  float z_v = (z_g - float(z_b) * block_size) / voxel_size;
926 
927  Key key(x_b, y_b, z_b);
928 
929  index_t block_buf_idx = cache.Check(x_b, y_b, z_b);
930  if (block_buf_idx < 0) {
931  auto iter = hashmap_impl.find(key);
932  if (iter == hashmap_impl.end()) return;
933  block_buf_idx = iter->second;
934  cache.Update(x_b, y_b, z_b, block_buf_idx);
935  }
936 
937  index_t x_v_floor = static_cast<index_t>(floorf(x_v));
938  index_t y_v_floor = static_cast<index_t>(floorf(y_v));
939  index_t z_v_floor = static_cast<index_t>(floorf(z_v));
940 
941  float ratio_x = x_v - float(x_v_floor);
942  float ratio_y = y_v - float(y_v_floor);
943  float ratio_z = z_v - float(z_v_floor);
944 
945  float sum_r = 0.0;
946  for (index_t k = 0; k < 8; ++k) {
947  index_t dx_v = (k & 1) > 0 ? 1 : 0;
948  index_t dy_v = (k & 2) > 0 ? 1 : 0;
949  index_t dz_v = (k & 4) > 0 ? 1 : 0;
950 
951  index_t linear_idx_k = GetLinearIdxAtP(
952  x_b, y_b, z_b, x_v_floor + dx_v, y_v_floor + dy_v,
953  z_v_floor + dz_v, block_buf_idx, cache);
954 
955  if (linear_idx_k >= 0 && weight_base_ptr[linear_idx_k] > 0) {
956  float rx = dx_v * (ratio_x) + (1 - dx_v) * (1 - ratio_x);
957  float ry = dy_v * (ratio_y) + (1 - dy_v) * (1 - ratio_y);
958  float rz = dz_v * (ratio_z) + (1 - dz_v) * (1 - ratio_z);
959  float r = rx * ry * rz;
960 
961  if (interp_ratio_ptr) {
962  interp_ratio_ptr[k] = r;
963  }
964  if (mask_ptr) {
965  mask_ptr[k] = true;
966  }
967  if (index_ptr) {
968  index_ptr[k] = linear_idx_k;
969  }
970 
971  float tsdf_k = tsdf_base_ptr[linear_idx_k];
972  float interp_ratio_dx = ry * rz * (2 * dx_v - 1);
973  float interp_ratio_dy = rx * rz * (2 * dy_v - 1);
974  float interp_ratio_dz = rx * ry * (2 * dz_v - 1);
975 
976  if (interp_ratio_dx_ptr) {
977  interp_ratio_dx_ptr[k] = interp_ratio_dx;
978  }
979  if (interp_ratio_dy_ptr) {
980  interp_ratio_dy_ptr[k] = interp_ratio_dy;
981  }
982  if (interp_ratio_dz_ptr) {
983  interp_ratio_dz_ptr[k] = interp_ratio_dz;
984  }
985 
986  if (normal_ptr) {
987  normal_ptr[0] += interp_ratio_dx * tsdf_k;
988  normal_ptr[1] += interp_ratio_dy * tsdf_k;
989  normal_ptr[2] += interp_ratio_dz * tsdf_k;
990  }
991 
992  if (color_ptr) {
993  index_t color_linear_idx = linear_idx_k * 3;
994  color_ptr[0] +=
995  r * color_base_ptr[color_linear_idx + 0];
996  color_ptr[1] +=
997  r * color_base_ptr[color_linear_idx + 1];
998  color_ptr[2] +=
999  r * color_base_ptr[color_linear_idx + 2];
1000  }
1001 
1002  sum_r += r;
1003  }
1004  } // loop over 8 neighbors
1005 
1006  if (sum_r > 0) {
1007  sum_r *= 255.0;
1008  if (color_ptr) {
1009  color_ptr[0] /= sum_r;
1010  color_ptr[1] /= sum_r;
1011  color_ptr[2] /= sum_r;
1012  }
1013 
1014  if (normal_ptr) {
1015  constexpr float EPSILON = 1e-5f;
1016  float norm = sqrt(normal_ptr[0] * normal_ptr[0] +
1017  normal_ptr[1] * normal_ptr[1] +
1018  normal_ptr[2] * normal_ptr[2]);
1019  norm = std::max(norm, EPSILON);
1020  w2c_transform_indexer.Rotate(
1021  -normal_ptr[0] / norm, -normal_ptr[1] / norm,
1022  -normal_ptr[2] / norm, normal_ptr + 0,
1023  normal_ptr + 1, normal_ptr + 2);
1024  }
1025  }
1026  } // surface-found
1027  });
1028 
1029 #if defined(__CUDACC__)
1031 #endif
1032 }
1033 
1034 template <typename tsdf_t, typename weight_t, typename color_t>
1035 #if defined(__CUDACC__)
1036 void ExtractPointCloudCUDA
1037 #else
1039 #endif
1040  (const core::Tensor& indices,
1041  const core::Tensor& nb_indices,
1042  const core::Tensor& nb_masks,
1043  const core::Tensor& block_keys,
1044  const TensorMap& block_value_map,
1046  core::Tensor& normals,
1047  core::Tensor& colors,
1048  index_t resolution,
1049  float voxel_size,
1050  float weight_threshold,
1051  int& valid_size) {
1052  core::Device device = block_keys.GetDevice();
1053 
1054  // Parameters
1055  index_t resolution2 = resolution * resolution;
1056  index_t resolution3 = resolution2 * resolution;
1057 
1058  // Shape / transform indexers, no data involved
1059  ArrayIndexer voxel_indexer({resolution, resolution, resolution});
1060 
1061  // Real data indexer
1062  ArrayIndexer block_keys_indexer(block_keys, 1);
1063  ArrayIndexer nb_block_masks_indexer(nb_masks, 2);
1064  ArrayIndexer nb_block_indices_indexer(nb_indices, 2);
1065 
1066  // Plain arrays that does not require indexers
1067  const index_t* indices_ptr = indices.GetDataPtr<index_t>();
1068 
1069  if (!block_value_map.Contains("tsdf") ||
1070  !block_value_map.Contains("weight")) {
1072  "TSDF and/or weight not allocated in blocks, please implement "
1073  "customized integration.");
1074  }
1075  const tsdf_t* tsdf_base_ptr =
1076  block_value_map.at("tsdf").GetDataPtr<tsdf_t>();
1077  const weight_t* weight_base_ptr =
1078  block_value_map.at("weight").GetDataPtr<weight_t>();
1079  const color_t* color_base_ptr = nullptr;
1080  if (block_value_map.Contains("color")) {
1081  color_base_ptr = block_value_map.at("color").GetDataPtr<color_t>();
1082  }
1083 
1084  index_t n_blocks = indices.GetLength();
1085  index_t n = n_blocks * resolution3;
1086 
1087  // Output
1088 #if defined(__CUDACC__)
1089  core::Tensor count(std::vector<index_t>{0}, {1}, core::Int32,
1090  block_keys.GetDevice());
1091  index_t* count_ptr = count.GetDataPtr<index_t>();
1092 #else
1093  std::atomic<index_t> count_atomic(0);
1094  std::atomic<index_t>* count_ptr = &count_atomic;
1095 #endif
1096 
1097  if (valid_size < 0) {
1099  "No estimated max point cloud size provided, using a 2-pass "
1100  "estimation. Surface extraction could be slow.");
1101  // This pass determines valid number of points.
1102 
1103  core::ParallelFor(device, n, [=] OPEN3D_DEVICE(index_t workload_idx) {
1104  auto GetLinearIdx = [&] OPEN3D_DEVICE(
1105  index_t xo, index_t yo, index_t zo,
1106  index_t curr_block_idx) -> index_t {
1107  return DeviceGetLinearIdx(xo, yo, zo, curr_block_idx,
1108  resolution, nb_block_masks_indexer,
1109  nb_block_indices_indexer);
1110  };
1111 
1112  // Natural index (0, N) -> (block_idx,
1113  // voxel_idx)
1114  index_t workload_block_idx = workload_idx / resolution3;
1115  index_t block_idx = indices_ptr[workload_block_idx];
1116  index_t voxel_idx = workload_idx % resolution3;
1117 
1118  // voxel_idx -> (x_voxel, y_voxel, z_voxel)
1119  index_t xv, yv, zv;
1120  voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
1121 
1122  index_t linear_idx = block_idx * resolution3 + voxel_idx;
1123  float tsdf_o = tsdf_base_ptr[linear_idx];
1124  float weight_o = weight_base_ptr[linear_idx];
1125  if (weight_o <= weight_threshold) return;
1126 
1127  // Enumerate x-y-z directions
1128  for (index_t i = 0; i < 3; ++i) {
1129  index_t linear_idx_i =
1130  GetLinearIdx(xv + (i == 0), yv + (i == 1),
1131  zv + (i == 2), workload_block_idx);
1132  if (linear_idx_i < 0) continue;
1133 
1134  float tsdf_i = tsdf_base_ptr[linear_idx_i];
1135  float weight_i = weight_base_ptr[linear_idx_i];
1136  if (weight_i > weight_threshold && tsdf_i * tsdf_o < 0) {
1137  OPEN3D_ATOMIC_ADD(count_ptr, 1);
1138  }
1139  }
1140  });
1141 
1142 #if defined(__CUDACC__)
1143  valid_size = count[0].Item<index_t>();
1144  count[0] = 0;
1145 #else
1146  valid_size = (*count_ptr).load();
1147  (*count_ptr) = 0;
1148 #endif
1149  }
1150 
1151  if (points.GetLength() == 0) {
1152  points = core::Tensor({valid_size, 3}, core::Float32, device);
1153  }
1154  ArrayIndexer point_indexer(points, 1);
1155 
1156  // Normals
1157  ArrayIndexer normal_indexer;
1158  normals = core::Tensor({valid_size, 3}, core::Float32, device);
1159  normal_indexer = ArrayIndexer(normals, 1);
1160 
1161  // This pass extracts exact surface points.
1162 
1163  // Colors
1164  ArrayIndexer color_indexer;
1165  if (color_base_ptr) {
1166  colors = core::Tensor({valid_size, 3}, core::Float32, device);
1167  color_indexer = ArrayIndexer(colors, 1);
1168  }
1169 
1170  core::ParallelFor(device, n, [=] OPEN3D_DEVICE(index_t workload_idx) {
1171  auto GetLinearIdx = [&] OPEN3D_DEVICE(
1172  index_t xo, index_t yo, index_t zo,
1173  index_t curr_block_idx) -> index_t {
1174  return DeviceGetLinearIdx(xo, yo, zo, curr_block_idx, resolution,
1175  nb_block_masks_indexer,
1176  nb_block_indices_indexer);
1177  };
1178 
1179  auto GetNormal = [&] OPEN3D_DEVICE(index_t xo, index_t yo, index_t zo,
1180  index_t curr_block_idx, float* n) {
1181  return DeviceGetNormal<tsdf_t>(
1182  tsdf_base_ptr, xo, yo, zo, curr_block_idx, n, resolution,
1183  nb_block_masks_indexer, nb_block_indices_indexer);
1184  };
1185 
1186  // Natural index (0, N) -> (block_idx, voxel_idx)
1187  index_t workload_block_idx = workload_idx / resolution3;
1188  index_t block_idx = indices_ptr[workload_block_idx];
1189  index_t voxel_idx = workload_idx % resolution3;
1190 
1192  // block_idx -> (x_block, y_block, z_block)
1193  index_t* block_key_ptr =
1194  block_keys_indexer.GetDataPtr<index_t>(block_idx);
1195  index_t xb = block_key_ptr[0];
1196  index_t yb = block_key_ptr[1];
1197  index_t zb = block_key_ptr[2];
1198 
1199  // voxel_idx -> (x_voxel, y_voxel, z_voxel)
1200  index_t xv, yv, zv;
1201  voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
1202 
1203  index_t linear_idx = block_idx * resolution3 + voxel_idx;
1204  float tsdf_o = tsdf_base_ptr[linear_idx];
1205  float weight_o = weight_base_ptr[linear_idx];
1206  if (weight_o <= weight_threshold) return;
1207 
1208  float no[3] = {0}, ne[3] = {0};
1209 
1210  // Get normal at origin
1211  GetNormal(xv, yv, zv, workload_block_idx, no);
1212 
1213  index_t x = xb * resolution + xv;
1214  index_t y = yb * resolution + yv;
1215  index_t z = zb * resolution + zv;
1216 
1217  // Enumerate x-y-z axis
1218  for (index_t i = 0; i < 3; ++i) {
1219  index_t linear_idx_i =
1220  GetLinearIdx(xv + (i == 0), yv + (i == 1), zv + (i == 2),
1221  workload_block_idx);
1222  if (linear_idx_i < 0) continue;
1223 
1224  float tsdf_i = tsdf_base_ptr[linear_idx_i];
1225  float weight_i = weight_base_ptr[linear_idx_i];
1226  if (weight_i > weight_threshold && tsdf_i * tsdf_o < 0) {
1227  float ratio = (0 - tsdf_o) / (tsdf_i - tsdf_o);
1228 
1229  index_t idx = OPEN3D_ATOMIC_ADD(count_ptr, 1);
1230  if (idx >= valid_size) {
1231  printf("Point cloud size larger than "
1232  "estimated, please increase the "
1233  "estimation!\n");
1234  return;
1235  }
1236 
1237  float* point_ptr = point_indexer.GetDataPtr<float>(idx);
1238  point_ptr[0] = voxel_size * (x + ratio * int(i == 0));
1239  point_ptr[1] = voxel_size * (y + ratio * int(i == 1));
1240  point_ptr[2] = voxel_size * (z + ratio * int(i == 2));
1241 
1242  // Get normal at edge and interpolate
1243  float* normal_ptr = normal_indexer.GetDataPtr<float>(idx);
1244  GetNormal(xv + (i == 0), yv + (i == 1), zv + (i == 2),
1245  workload_block_idx, ne);
1246  float nx = (1 - ratio) * no[0] + ratio * ne[0];
1247  float ny = (1 - ratio) * no[1] + ratio * ne[1];
1248  float nz = (1 - ratio) * no[2] + ratio * ne[2];
1249  float norm = static_cast<float>(
1250  sqrt(nx * nx + ny * ny + nz * nz) + 1e-5);
1251  normal_ptr[0] = nx / norm;
1252  normal_ptr[1] = ny / norm;
1253  normal_ptr[2] = nz / norm;
1254 
1255  if (color_base_ptr) {
1256  float* color_ptr = color_indexer.GetDataPtr<float>(idx);
1257  const color_t* color_o_ptr =
1258  color_base_ptr + 3 * linear_idx;
1259  float r_o = color_o_ptr[0];
1260  float g_o = color_o_ptr[1];
1261  float b_o = color_o_ptr[2];
1262 
1263  const color_t* color_i_ptr =
1264  color_base_ptr + 3 * linear_idx_i;
1265  float r_i = color_i_ptr[0];
1266  float g_i = color_i_ptr[1];
1267  float b_i = color_i_ptr[2];
1268 
1269  color_ptr[0] = ((1 - ratio) * r_o + ratio * r_i) / 255.0f;
1270  color_ptr[1] = ((1 - ratio) * g_o + ratio * g_i) / 255.0f;
1271  color_ptr[2] = ((1 - ratio) * b_o + ratio * b_i) / 255.0f;
1272  }
1273  }
1274  }
1275  });
1276 
1277 #if defined(__CUDACC__)
1278  index_t total_count = count.Item<index_t>();
1279 #else
1280  index_t total_count = (*count_ptr).load();
1281 #endif
1282 
1283  utility::LogDebug("{} vertices extracted", total_count);
1284  valid_size = total_count;
1285 
1286 #if defined(BUILD_CUDA_MODULE) && defined(__CUDACC__)
1288 #endif
1289 }
1290 
1291 template <typename tsdf_t, typename weight_t, typename color_t>
1292 #if defined(__CUDACC__)
1293 void ExtractTriangleMeshCUDA
1294 #else
1296 #endif
1297  (const core::Tensor& block_indices,
1298  const core::Tensor& inv_block_indices,
1299  const core::Tensor& nb_block_indices,
1300  const core::Tensor& nb_block_masks,
1301  const core::Tensor& block_keys,
1302  const TensorMap& block_value_map,
1303  core::Tensor& vertices,
1304  core::Tensor& triangles,
1305  core::Tensor& vertex_normals,
1306  core::Tensor& vertex_colors,
1307  index_t block_resolution,
1308  float voxel_size,
1309  float weight_threshold,
1310  index_t& vertex_count) {
1311  core::Device device = block_indices.GetDevice();
1312 
1313  index_t resolution = block_resolution;
1314  index_t resolution3 = resolution * resolution * resolution;
1315 
1316  // Shape / transform indexers, no data involved
1317  ArrayIndexer voxel_indexer({resolution, resolution, resolution});
1318  index_t n_blocks = static_cast<index_t>(block_indices.GetLength());
1319 
1320  // TODO(wei): profile performance by replacing the table to a hashmap.
1321  // Voxel-wise mesh info. 4 channels correspond to:
1322  // 3 edges' corresponding vertex index + 1 table index.
1323  core::Tensor mesh_structure;
1324  try {
1325  mesh_structure = core::Tensor::Zeros(
1326  {n_blocks, resolution, resolution, resolution, 4}, core::Int32,
1327  device);
1328  } catch (const std::runtime_error&) {
1330  "Unable to allocate assistance mesh structure for Marching "
1331  "Cubes with {} active voxel blocks. Please consider using a "
1332  "larger voxel size (currently {}) for TSDF integration, or "
1333  "using tsdf_volume.cpu() to perform mesh extraction on CPU.",
1334  n_blocks, voxel_size);
1335  }
1336 
1337  // Real data indexer
1338  ArrayIndexer mesh_structure_indexer(mesh_structure, 4);
1339  ArrayIndexer nb_block_masks_indexer(nb_block_masks, 2);
1340  ArrayIndexer nb_block_indices_indexer(nb_block_indices, 2);
1341 
1342  // Plain arrays that does not require indexers
1343  const index_t* indices_ptr = block_indices.GetDataPtr<index_t>();
1344  const index_t* inv_indices_ptr = inv_block_indices.GetDataPtr<index_t>();
1345 
1346  if (!block_value_map.Contains("tsdf") ||
1347  !block_value_map.Contains("weight")) {
1349  "TSDF and/or weight not allocated in blocks, please implement "
1350  "customized integration.");
1351  }
1352  const tsdf_t* tsdf_base_ptr =
1353  block_value_map.at("tsdf").GetDataPtr<tsdf_t>();
1354  const weight_t* weight_base_ptr =
1355  block_value_map.at("weight").GetDataPtr<weight_t>();
1356  const color_t* color_base_ptr = nullptr;
1357  if (block_value_map.Contains("color")) {
1358  color_base_ptr = block_value_map.at("color").GetDataPtr<color_t>();
1359  }
1360 
1361  index_t n = n_blocks * resolution3;
1362  // Pass 0: analyze mesh structure, set up one-on-one correspondences
1363  // from edges to vertices.
1364 
1365  core::ParallelFor(device, n, [=] OPEN3D_DEVICE(index_t widx) {
1366  auto GetLinearIdx = [&] OPEN3D_DEVICE(
1367  index_t xo, index_t yo, index_t zo,
1368  index_t curr_block_idx) -> index_t {
1369  return DeviceGetLinearIdx(xo, yo, zo, curr_block_idx,
1370  static_cast<index_t>(resolution),
1371  nb_block_masks_indexer,
1372  nb_block_indices_indexer);
1373  };
1374 
1375  // Natural index (0, N) -> (block_idx, voxel_idx)
1376  index_t workload_block_idx = widx / resolution3;
1377  index_t voxel_idx = widx % resolution3;
1378 
1379  // voxel_idx -> (x_voxel, y_voxel, z_voxel)
1380  index_t xv, yv, zv;
1381  voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
1382 
1383  // Check per-vertex sign in the cube to determine cube
1384  // type
1385  index_t table_idx = 0;
1386  for (index_t i = 0; i < 8; ++i) {
1387  index_t linear_idx_i =
1388  GetLinearIdx(xv + vtx_shifts[i][0], yv + vtx_shifts[i][1],
1389  zv + vtx_shifts[i][2], workload_block_idx);
1390  if (linear_idx_i < 0) return;
1391 
1392  float tsdf_i = tsdf_base_ptr[linear_idx_i];
1393  float weight_i = weight_base_ptr[linear_idx_i];
1394  if (weight_i <= weight_threshold) return;
1395 
1396  table_idx |= ((tsdf_i < 0) ? (1 << i) : 0);
1397  }
1398 
1399  index_t* mesh_struct_ptr = mesh_structure_indexer.GetDataPtr<index_t>(
1400  xv, yv, zv, workload_block_idx);
1401  mesh_struct_ptr[3] = table_idx;
1402 
1403  if (table_idx == 0 || table_idx == 255) return;
1404 
1405  // Check per-edge sign determine the cube type
1406  index_t edges_with_vertices = edge_table[table_idx];
1407  for (index_t i = 0; i < 12; ++i) {
1408  if (edges_with_vertices & (1 << i)) {
1409  index_t xv_i = xv + edge_shifts[i][0];
1410  index_t yv_i = yv + edge_shifts[i][1];
1411  index_t zv_i = zv + edge_shifts[i][2];
1412  index_t edge_i = edge_shifts[i][3];
1413 
1414  index_t dxb = xv_i / resolution;
1415  index_t dyb = yv_i / resolution;
1416  index_t dzb = zv_i / resolution;
1417 
1418  index_t nb_idx = (dxb + 1) + (dyb + 1) * 3 + (dzb + 1) * 9;
1419 
1420  index_t block_idx_i =
1421  *nb_block_indices_indexer.GetDataPtr<index_t>(
1422  workload_block_idx, nb_idx);
1423  index_t* mesh_ptr_i =
1424  mesh_structure_indexer.GetDataPtr<index_t>(
1425  xv_i - dxb * resolution,
1426  yv_i - dyb * resolution,
1427  zv_i - dzb * resolution,
1428  inv_indices_ptr[block_idx_i]);
1429 
1430  // Non-atomic write, but we are safe
1431  mesh_ptr_i[edge_i] = -1;
1432  }
1433  }
1434  });
1435 
1436  // Pass 1: determine valid number of vertices (if not preset)
1437 #if defined(__CUDACC__)
1438  core::Tensor count(std::vector<index_t>{0}, {}, core::Int32, device);
1439 
1440  index_t* count_ptr = count.GetDataPtr<index_t>();
1441 #else
1442  std::atomic<index_t> count_atomic(0);
1443  std::atomic<index_t>* count_ptr = &count_atomic;
1444 #endif
1445 
1446  if (vertex_count < 0) {
1447  core::ParallelFor(device, n, [=] OPEN3D_DEVICE(index_t widx) {
1448  // Natural index (0, N) -> (block_idx, voxel_idx)
1449  index_t workload_block_idx = widx / resolution3;
1450  index_t voxel_idx = widx % resolution3;
1451 
1452  // voxel_idx -> (x_voxel, y_voxel, z_voxel)
1453  index_t xv, yv, zv;
1454  voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
1455 
1456  // Obtain voxel's mesh struct ptr
1457  index_t* mesh_struct_ptr =
1458  mesh_structure_indexer.GetDataPtr<index_t>(
1459  xv, yv, zv, workload_block_idx);
1460 
1461  // Early quit -- no allocated vertex to compute
1462  if (mesh_struct_ptr[0] != -1 && mesh_struct_ptr[1] != -1 &&
1463  mesh_struct_ptr[2] != -1) {
1464  return;
1465  }
1466 
1467  // Enumerate 3 edges in the voxel
1468  for (index_t e = 0; e < 3; ++e) {
1469  index_t vertex_idx = mesh_struct_ptr[e];
1470  if (vertex_idx != -1) continue;
1471 
1472  OPEN3D_ATOMIC_ADD(count_ptr, 1);
1473  }
1474  });
1475 
1476 #if defined(__CUDACC__)
1477  vertex_count = count.Item<index_t>();
1478 #else
1479  vertex_count = (*count_ptr).load();
1480 #endif
1481  }
1482 
1483  utility::LogDebug("Total vertex count = {}", vertex_count);
1484  vertices = core::Tensor({vertex_count, 3}, core::Float32, device);
1485 
1486  vertex_normals = core::Tensor({vertex_count, 3}, core::Float32, device);
1487  ArrayIndexer normal_indexer = ArrayIndexer(vertex_normals, 1);
1488 
1489  ArrayIndexer color_indexer;
1490  if (color_base_ptr) {
1491  vertex_colors = core::Tensor({vertex_count, 3}, core::Float32, device);
1492  color_indexer = ArrayIndexer(vertex_colors, 1);
1493  }
1494 
1495  ArrayIndexer block_keys_indexer(block_keys, 1);
1496  ArrayIndexer vertex_indexer(vertices, 1);
1497 
1498 #if defined(__CUDACC__)
1499  count = core::Tensor(std::vector<index_t>{0}, {}, core::Int32, device);
1500  count_ptr = count.GetDataPtr<index_t>();
1501 #else
1502  (*count_ptr) = 0;
1503 #endif
1504 
1505  // Pass 2: extract vertices.
1506 
1507  core::ParallelFor(device, n, [=] OPEN3D_DEVICE(index_t widx) {
1508  auto GetLinearIdx = [&] OPEN3D_DEVICE(
1509  index_t xo, index_t yo, index_t zo,
1510  index_t curr_block_idx) -> index_t {
1511  return DeviceGetLinearIdx(xo, yo, zo, curr_block_idx, resolution,
1512  nb_block_masks_indexer,
1513  nb_block_indices_indexer);
1514  };
1515 
1516  auto GetNormal = [&] OPEN3D_DEVICE(index_t xo, index_t yo, index_t zo,
1517  index_t curr_block_idx, float* n) {
1518  return DeviceGetNormal<tsdf_t>(
1519  tsdf_base_ptr, xo, yo, zo, curr_block_idx, n, resolution,
1520  nb_block_masks_indexer, nb_block_indices_indexer);
1521  };
1522 
1523  // Natural index (0, N) -> (block_idx, voxel_idx)
1524  index_t workload_block_idx = widx / resolution3;
1525  index_t block_idx = indices_ptr[workload_block_idx];
1526  index_t voxel_idx = widx % resolution3;
1527 
1528  // block_idx -> (x_block, y_block, z_block)
1529  index_t* block_key_ptr =
1530  block_keys_indexer.GetDataPtr<index_t>(block_idx);
1531  index_t xb = block_key_ptr[0];
1532  index_t yb = block_key_ptr[1];
1533  index_t zb = block_key_ptr[2];
1534 
1535  // voxel_idx -> (x_voxel, y_voxel, z_voxel)
1536  index_t xv, yv, zv;
1537  voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
1538 
1539  // global coordinate (in voxels)
1540  index_t x = xb * resolution + xv;
1541  index_t y = yb * resolution + yv;
1542  index_t z = zb * resolution + zv;
1543 
1544  // Obtain voxel's mesh struct ptr
1545  index_t* mesh_struct_ptr = mesh_structure_indexer.GetDataPtr<index_t>(
1546  xv, yv, zv, workload_block_idx);
1547 
1548  // Early quit -- no allocated vertex to compute
1549  if (mesh_struct_ptr[0] != -1 && mesh_struct_ptr[1] != -1 &&
1550  mesh_struct_ptr[2] != -1) {
1551  return;
1552  }
1553 
1554  // Obtain voxel ptr
1555  index_t linear_idx = resolution3 * block_idx + voxel_idx;
1556  float tsdf_o = tsdf_base_ptr[linear_idx];
1557 
1558  float no[3] = {0}, ne[3] = {0};
1559 
1560  // Get normal at origin
1561  GetNormal(xv, yv, zv, workload_block_idx, no);
1562 
1563  // Enumerate 3 edges in the voxel
1564  for (index_t e = 0; e < 3; ++e) {
1565  index_t vertex_idx = mesh_struct_ptr[e];
1566  if (vertex_idx != -1) continue;
1567 
1568  index_t linear_idx_e =
1569  GetLinearIdx(xv + (e == 0), yv + (e == 1), zv + (e == 2),
1570  workload_block_idx);
1571  OPEN3D_ASSERT(linear_idx_e > 0 &&
1572  "Internal error: GetVoxelAt returns nullptr.");
1573  float tsdf_e = tsdf_base_ptr[linear_idx_e];
1574  float ratio = (0 - tsdf_o) / (tsdf_e - tsdf_o);
1575 
1576  index_t idx = OPEN3D_ATOMIC_ADD(count_ptr, 1);
1577  mesh_struct_ptr[e] = idx;
1578 
1579  float ratio_x = ratio * index_t(e == 0);
1580  float ratio_y = ratio * index_t(e == 1);
1581  float ratio_z = ratio * index_t(e == 2);
1582 
1583  float* vertex_ptr = vertex_indexer.GetDataPtr<float>(idx);
1584  vertex_ptr[0] = voxel_size * (x + ratio_x);
1585  vertex_ptr[1] = voxel_size * (y + ratio_y);
1586  vertex_ptr[2] = voxel_size * (z + ratio_z);
1587 
1588  // Get normal at edge and interpolate
1589  float* normal_ptr = normal_indexer.GetDataPtr<float>(idx);
1590  GetNormal(xv + (e == 0), yv + (e == 1), zv + (e == 2),
1591  workload_block_idx, ne);
1592  float nx = (1 - ratio) * no[0] + ratio * ne[0];
1593  float ny = (1 - ratio) * no[1] + ratio * ne[1];
1594  float nz = (1 - ratio) * no[2] + ratio * ne[2];
1595  float norm = static_cast<float>(sqrt(nx * nx + ny * ny + nz * nz) +
1596  1e-5);
1597  normal_ptr[0] = nx / norm;
1598  normal_ptr[1] = ny / norm;
1599  normal_ptr[2] = nz / norm;
1600 
1601  if (color_base_ptr) {
1602  float* color_ptr = color_indexer.GetDataPtr<float>(idx);
1603  float r_o = color_base_ptr[linear_idx * 3 + 0];
1604  float g_o = color_base_ptr[linear_idx * 3 + 1];
1605  float b_o = color_base_ptr[linear_idx * 3 + 2];
1606 
1607  float r_e = color_base_ptr[linear_idx_e * 3 + 0];
1608  float g_e = color_base_ptr[linear_idx_e * 3 + 1];
1609  float b_e = color_base_ptr[linear_idx_e * 3 + 2];
1610 
1611  color_ptr[0] = ((1 - ratio) * r_o + ratio * r_e) / 255.0f;
1612  color_ptr[1] = ((1 - ratio) * g_o + ratio * g_e) / 255.0f;
1613  color_ptr[2] = ((1 - ratio) * b_o + ratio * b_e) / 255.0f;
1614  }
1615  }
1616  });
1617 
1618  // Pass 3: connect vertices and form triangles.
1619  index_t triangle_count = vertex_count * 3;
1620  triangles = core::Tensor({triangle_count, 3}, core::Int32, device);
1621  ArrayIndexer triangle_indexer(triangles, 1);
1622 
1623 #if defined(__CUDACC__)
1624  count = core::Tensor(std::vector<index_t>{0}, {}, core::Int32, device);
1625  count_ptr = count.GetDataPtr<index_t>();
1626 #else
1627  (*count_ptr) = 0;
1628 #endif
1629  core::ParallelFor(device, n, [=] OPEN3D_DEVICE(index_t widx) {
1630  // Natural index (0, N) -> (block_idx, voxel_idx)
1631  index_t workload_block_idx = widx / resolution3;
1632  index_t voxel_idx = widx % resolution3;
1633 
1634  // voxel_idx -> (x_voxel, y_voxel, z_voxel)
1635  index_t xv, yv, zv;
1636  voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
1637 
1638  // Obtain voxel's mesh struct ptr
1639  index_t* mesh_struct_ptr = mesh_structure_indexer.GetDataPtr<index_t>(
1640  xv, yv, zv, workload_block_idx);
1641 
1642  index_t table_idx = mesh_struct_ptr[3];
1643  if (tri_count[table_idx] == 0) return;
1644 
1645  for (index_t tri = 0; tri < 16; tri += 3) {
1646  if (tri_table[table_idx][tri] == -1) return;
1647 
1648  index_t tri_idx = OPEN3D_ATOMIC_ADD(count_ptr, 1);
1649 
1650  for (index_t vertex = 0; vertex < 3; ++vertex) {
1651  index_t edge = tri_table[table_idx][tri + vertex];
1652 
1653  index_t xv_i = xv + edge_shifts[edge][0];
1654  index_t yv_i = yv + edge_shifts[edge][1];
1655  index_t zv_i = zv + edge_shifts[edge][2];
1656  index_t edge_i = edge_shifts[edge][3];
1657 
1658  index_t dxb = xv_i / resolution;
1659  index_t dyb = yv_i / resolution;
1660  index_t dzb = zv_i / resolution;
1661 
1662  index_t nb_idx = (dxb + 1) + (dyb + 1) * 3 + (dzb + 1) * 9;
1663 
1664  index_t block_idx_i =
1665  *nb_block_indices_indexer.GetDataPtr<index_t>(
1666  workload_block_idx, nb_idx);
1667  index_t* mesh_struct_ptr_i =
1668  mesh_structure_indexer.GetDataPtr<index_t>(
1669  xv_i - dxb * resolution,
1670  yv_i - dyb * resolution,
1671  zv_i - dzb * resolution,
1672  inv_indices_ptr[block_idx_i]);
1673 
1674  index_t* triangle_ptr =
1675  triangle_indexer.GetDataPtr<index_t>(tri_idx);
1676  triangle_ptr[2 - vertex] = mesh_struct_ptr_i[edge_i];
1677  }
1678  }
1679  });
1680 
1681 #if defined(__CUDACC__)
1682  triangle_count = count.Item<index_t>();
1683 #else
1684  triangle_count = (*count_ptr).load();
1685 #endif
1686  utility::LogDebug("Total triangle count = {}", triangle_count);
1687  triangles = triangles.Slice(0, 0, triangle_count);
1688 }
1689 
1690 } // namespace voxel_grid
1691 } // namespace kernel
1692 } // namespace geometry
1693 } // namespace t
1694 } // namespace open3d
#define OPEN3D_DEVICE
Definition: CUDAUtils.h:45
OPEN3D_HOST_DEVICE int Sign(int x)
Definition: GeometryMacros.h:77
#define OPEN3D_ATOMIC_ADD(X, Y)
Definition: GeometryMacros.h:39
math::float4 color
Definition: LineSetBuffers.cpp:45
#define LogWarning(...)
Definition: Logging.h:60
#define LogInfo(...)
Definition: Logging.h:70
#define LogError(...)
Definition: Logging.h:48
#define LogDebug(...)
Definition: Logging.h:79
#define OPEN3D_ASSERT(...)
Definition: Macro.h:48
Definition: Device.h:18
static const Dtype Float64
Definition: Dtype.h:24
Definition: Tensor.h:32
static Tensor Zeros(const SizeVector &shape, Dtype dtype, const Device &device=Device("CPU:0"))
Create a tensor fill with zeros.
Definition: Tensor.cpp:374
static Tensor Eye(int64_t n, Dtype dtype, const Device &device)
Create an identity matrix of size n x n.
Definition: Tensor.cpp:386
Definition: TensorMap.h:31
Definition: GeometryIndexer.h:161
OPEN3D_HOST_DEVICE bool InBoundary(float x, float y) const
Definition: GeometryIndexer.h:294
OPEN3D_HOST_DEVICE void * GetDataPtr() const
Definition: GeometryIndexer.h:315
Helper class for converting coordinates/indices between 3D/3D, 3D/2D, 2D/3D.
Definition: GeometryIndexer.h:25
OPEN3D_HOST_DEVICE void Project(float x_in, float y_in, float z_in, float *u_out, float *v_out) const
Project a 3D coordinate in camera coordinate to a 2D uv coordinate.
Definition: GeometryIndexer.h:100
OPEN3D_HOST_DEVICE void Rotate(float x_in, float y_in, float z_in, float *x_out, float *y_out, float *z_out) const
Transform a 3D coordinate in camera coordinate to world coordinate.
Definition: GeometryIndexer.h:81
OPEN3D_HOST_DEVICE void RigidTransform(float x_in, float y_in, float z_in, float *x_out, float *y_out, float *z_out) const
Transform a 3D coordinate in camera coordinate to world coordinate.
Definition: GeometryIndexer.h:62
OPEN3D_HOST_DEVICE void Unproject(float u_in, float v_in, float d_in, float *x_out, float *y_out, float *z_out) const
Unproject a 2D uv coordinate with depth to 3D in camera coordinate.
Definition: GeometryIndexer.h:111
int count
Definition: FilePCD.cpp:42
int offset
Definition: FilePCD.cpp:45
int points
Definition: FilePCD.cpp:54
void Synchronize()
Definition: CUDAUtils.cpp:58
uint32_t buf_index_t
Definition: HashBackendBuffer.h:44
const Dtype Int32
Definition: Dtype.cpp:46
void ParallelFor(const Device &device, int64_t n, const func_t &func)
Definition: ParallelFor.h:103
const Dtype Float32
Definition: Dtype.cpp:42
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 timeout_in_ms capture_handle capture_handle capture_handle image_handle float
Definition: K4aPlugin.cpp:460
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 timeout_in_ms capture_handle capture_handle capture_handle image_handle temperature_c int
Definition: K4aPlugin.cpp:474
void ExtractPointCloudCPU(const core::Tensor &block_indices, const core::Tensor &nb_block_indices, const core::Tensor &nb_block_masks, const core::Tensor &block_keys, const TensorMap &block_value_map, core::Tensor &points, core::Tensor &normals, core::Tensor &colors, index_t block_resolution, float voxel_size, float weight_threshold, index_t &valid_size)
Definition: VoxelBlockGridImpl.h:1040
void GetVoxelCoordinatesAndFlattenedIndicesCPU(const core::Tensor &buf_indices, const core::Tensor &block_keys, core::Tensor &voxel_coords, core::Tensor &flattened_indices, index_t block_resolution, float voxel_size)
Definition: VoxelBlockGridImpl.h:38
OPEN3D_DEVICE index_t DeviceGetLinearIdx(index_t xo, index_t yo, index_t zo, index_t curr_block_idx, index_t resolution, const ArrayIndexer &nb_block_masks_indexer, const ArrayIndexer &nb_block_indices_indexer)
Definition: VoxelBlockGridImpl.h:83
void IntegrateCPU(const core::Tensor &depth, const core::Tensor &color, const core::Tensor &block_indices, const core::Tensor &block_keys, TensorMap &block_value_map, const core::Tensor &depth_intrinsic, const core::Tensor &color_intrinsic, const core::Tensor &extrinsic, index_t resolution, float voxel_size, float sdf_trunc, float depth_scale, float depth_max)
Definition: VoxelBlockGridImpl.h:149
TArrayIndexer< index_t > ArrayIndexer
Definition: VoxelBlockGridImpl.h:31
void ExtractTriangleMeshCPU(const core::Tensor &block_indices, const core::Tensor &inv_block_indices, const core::Tensor &nb_block_indices, const core::Tensor &nb_block_masks, const core::Tensor &block_keys, const TensorMap &block_value_map, core::Tensor &vertices, core::Tensor &triangles, core::Tensor &vertex_normals, core::Tensor &vertex_colors, index_t block_resolution, float voxel_size, float weight_threshold, index_t &vertex_count)
Definition: VoxelBlockGridImpl.h:1297
OPEN3D_DEVICE void DeviceGetNormal(const tsdf_t *tsdf_base_ptr, index_t xo, index_t yo, index_t zo, index_t curr_block_idx, float *n, index_t resolution, const ArrayIndexer &nb_block_masks_indexer, const ArrayIndexer &nb_block_indices_indexer)
Definition: VoxelBlockGridImpl.h:112
void RayCastCPU(std::shared_ptr< core::HashMap > &hashmap, const TensorMap &block_value_map, const core::Tensor &range_map, TensorMap &renderings_map, const core::Tensor &intrinsic, const core::Tensor &extrinsic, index_t h, index_t w, index_t block_resolution, float voxel_size, float depth_scale, float depth_min, float depth_max, float weight_threshold, float trunc_voxel_multiplier, int range_map_down_factor)
Definition: VoxelBlockGridImpl.h:541
int index_t
Definition: VoxelBlockGrid.h:22
void EstimateRangeCPU(const core::Tensor &block_keys, core::Tensor &range_minmax_map, const core::Tensor &intrinsics, const core::Tensor &extrinsics, int h, int w, int down_factor, int64_t block_resolution, float voxel_size, float depth_min, float depth_max, core::Tensor &fragment_buffer)
Definition: VoxelBlockGridImpl.h:301
core::Tensor InverseTransformation(const core::Tensor &T)
TODO(wei): find a proper place for such functionalities.
Definition: Utility.h:77
Definition: PinholeCameraIntrinsic.cpp:16
Definition: VoxelBlockGridImpl.h:514
void OPEN3D_DEVICE Update(index_t xin, index_t yin, index_t zin, index_t block_idx_in)
Definition: VoxelBlockGridImpl.h:524
index_t x
Definition: VoxelBlockGridImpl.h:515
index_t block_idx
Definition: VoxelBlockGridImpl.h:518
index_t z
Definition: VoxelBlockGridImpl.h:517
index_t y
Definition: VoxelBlockGridImpl.h:516
index_t OPEN3D_DEVICE Check(index_t xin, index_t yin, index_t zin)
Definition: VoxelBlockGridImpl.h:520
Definition: Dispatch.h:110
Definition: Dispatch.h:94
Definition: MiniVec.h:24