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