Loading [MathJax]/extensions/TeX/AMSsymbols.js
Open3D (C++ API)  0.14.1
•All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
TSDFVoxelGridImpl.h
Go to the documentation of this file.
1 // ----------------------------------------------------------------------------
2 // - Open3D: www.open3d.org -
3 // ----------------------------------------------------------------------------
4 // The MIT License (MIT)
5 //
6 // Copyright (c) 2018-2021 www.open3d.org
7 //
8 // Permission is hereby granted, free of charge, to any person obtaining a copy
9 // of this software and associated documentation files (the "Software"), to deal
10 // in the Software without restriction, including without limitation the rights
11 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12 // copies of the Software, and to permit persons to whom the Software is
13 // furnished to do so, subject to the following conditions:
14 //
15 // The above copyright notice and this permission notice shall be included in
16 // all copies or substantial portions of the Software.
17 //
18 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
23 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
24 // IN THE SOFTWARE.
25 // ----------------------------------------------------------------------------
26 
27 #include <atomic>
28 #include <cmath>
29 
30 #include "open3d/core/Dispatch.h"
31 #include "open3d/core/Dtype.h"
32 #include "open3d/core/MemoryManager.h"
33 #include "open3d/core/SizeVector.h"
34 #include "open3d/core/Tensor.h"
35 #include "open3d/t/geometry/Utility.h"
36 #include "open3d/t/geometry/kernel/GeometryIndexer.h"
37 #include "open3d/t/geometry/kernel/GeometryMacros.h"
38 #include "open3d/t/geometry/kernel/TSDFVoxel.h"
39 #include "open3d/t/geometry/kernel/TSDFVoxelGrid.h"
40 #include "open3d/utility/Logging.h"
41 #include "open3d/utility/MiniVec.h"
42 #include "open3d/utility/Timer.h"
43 
44 namespace open3d {
45 namespace t {
46 namespace geometry {
47 namespace kernel {
48 namespace tsdf {
49 
50 #if defined(__CUDACC__)
51 void IntegrateCUDA
52 #else
53 void IntegrateCPU
54 #endif
55  (const core::Tensor& depth,
56  const core::Tensor& color,
57  const core::Tensor& indices,
58  const core::Tensor& block_keys,
59  core::Tensor& block_values,
60  // Transforms
61  const core::Tensor& intrinsics,
62  const core::Tensor& extrinsics,
63  // Parameters
64  int64_t resolution,
65  float voxel_size,
66  float sdf_trunc,
67  float depth_scale,
68  float depth_max) {
69  // Parameters
70  int64_t resolution3 = resolution * resolution * resolution;
71 
72  // Shape / transform indexers, no data involved
73  NDArrayIndexer voxel_indexer({resolution, resolution, resolution});
74  TransformIndexer transform_indexer(intrinsics, extrinsics, voxel_size);
75 
76  // Real data indexer
77  NDArrayIndexer depth_indexer(depth, 2);
78  NDArrayIndexer block_keys_indexer(block_keys, 1);
79  NDArrayIndexer voxel_block_buffer_indexer(block_values, 4);
80 
81  // Optional color integration
82  NDArrayIndexer color_indexer;
83  bool integrate_color = false;
84  if (color.NumElements() != 0) {
85  color_indexer = NDArrayIndexer(color, 2);
86  integrate_color = true;
87  }
88 
89  // Plain arrays that does not require indexers
90  const int* indices_ptr = indices.GetDataPtr<int>();
91 
92  int64_t n = indices.GetLength() * resolution3;
93 
94  DISPATCH_BYTESIZE_TO_VOXEL(
95  voxel_block_buffer_indexer.ElementByteSize(), [&]() {
96  core::ParallelFor(
97  depth.GetDevice(), n,
98  [=] OPEN3D_DEVICE(int64_t workload_idx) {
99  // Natural index (0, N) -> (block_idx, voxel_idx)
100  int block_idx =
101  indices_ptr[workload_idx / resolution3];
102  int voxel_idx = workload_idx % resolution3;
103 
105  // block_idx -> (x_block, y_block, z_block)
106  int* block_key_ptr =
107  block_keys_indexer.GetDataPtr<int>(
108  block_idx);
109  int64_t xb = static_cast<int64_t>(block_key_ptr[0]);
110  int64_t yb = static_cast<int64_t>(block_key_ptr[1]);
111  int64_t zb = static_cast<int64_t>(block_key_ptr[2]);
112 
113  // voxel_idx -> (x_voxel, y_voxel, z_voxel)
114  int64_t xv, yv, zv;
115  voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv,
116  &zv);
117 
118  // coordinate in world (in voxel)
119  int64_t x = (xb * resolution + xv);
120  int64_t y = (yb * resolution + yv);
121  int64_t z = (zb * resolution + zv);
122 
123  // coordinate in camera (in voxel -> in meter)
124  float xc, yc, zc, u, v;
125  transform_indexer.RigidTransform(
126  static_cast<float>(x),
127  static_cast<float>(y),
128  static_cast<float>(z), &xc, &yc, &zc);
129 
130  // coordinate in image (in pixel)
131  transform_indexer.Project(xc, yc, zc, &u, &v);
132  if (!depth_indexer.InBoundary(u, v)) {
133  return;
134  }
135 
136  // Associate image workload and compute SDF and
137  // TSDF.
138  float depth = *depth_indexer.GetDataPtr<float>(
139  static_cast<int64_t>(u),
140  static_cast<int64_t>(v)) /
141  depth_scale;
142 
143  float sdf = (depth - zc);
144  if (depth <= 0 || depth > depth_max || zc <= 0 ||
145  sdf < -sdf_trunc) {
146  return;
147  }
148  sdf = sdf < sdf_trunc ? sdf : sdf_trunc;
149  sdf /= sdf_trunc;
150 
151  // Associate voxel workload and update TSDF/Weights
152  voxel_t* voxel_ptr =
153  voxel_block_buffer_indexer
154  .GetDataPtr<voxel_t>(xv, yv, zv,
155  block_idx);
156 
157  if (integrate_color) {
158  float* color_ptr =
159  color_indexer.GetDataPtr<float>(
160  static_cast<int64_t>(u),
161  static_cast<int64_t>(v));
162 
163  voxel_ptr->Integrate(sdf, color_ptr[0],
164  color_ptr[1],
165  color_ptr[2]);
166  } else {
167  voxel_ptr->Integrate(sdf);
168  }
169  });
170  });
171 #if defined(__CUDACC__)
172  core::cuda::Synchronize();
173 #endif
174 }
175 
176 #if defined(__CUDACC__)
177 void ExtractSurfacePointsCUDA
178 #else
179 void ExtractSurfacePointsCPU
180 #endif
181  (const core::Tensor& indices,
182  const core::Tensor& nb_indices,
183  const core::Tensor& nb_masks,
184  const core::Tensor& block_keys,
185  const core::Tensor& block_values,
186  core::Tensor& points,
187  utility::optional<std::reference_wrapper<core::Tensor>> normals,
188  utility::optional<std::reference_wrapper<core::Tensor>> colors,
189  int64_t resolution,
190  float voxel_size,
191  float weight_threshold,
192  int& valid_size) {
193  // Parameters
194  int64_t resolution3 = resolution * resolution * resolution;
195 
196  // Shape / transform indexers, no data involved
197  NDArrayIndexer voxel_indexer({resolution, resolution, resolution});
198 
199  // Real data indexer
200  NDArrayIndexer voxel_block_buffer_indexer(block_values, 4);
201  NDArrayIndexer block_keys_indexer(block_keys, 1);
202  NDArrayIndexer nb_block_masks_indexer(nb_masks, 2);
203  NDArrayIndexer nb_block_indices_indexer(nb_indices, 2);
204 
205  // Plain arrays that does not require indexers
206  const int64_t* indices_ptr = indices.GetDataPtr<int64_t>();
207 
208  int64_t n_blocks = indices.GetLength();
209  int64_t n = n_blocks * resolution3;
210 
211  // Output
212 #if defined(__CUDACC__)
213  core::Tensor count(std::vector<int>{0}, {1}, core::Int32,
214  block_values.GetDevice());
215  int* count_ptr = count.GetDataPtr<int>();
216 #else
217  std::atomic<int> count_atomic(0);
218  std::atomic<int>* count_ptr = &count_atomic;
219 #endif
220 
221  if (valid_size < 0) {
222  utility::LogWarning(
223  "No estimated max point cloud size provided, using a 2-pass "
224  "estimation. Surface extraction could be slow.");
225  // This pass determines valid number of points.
226  DISPATCH_BYTESIZE_TO_VOXEL(
227  voxel_block_buffer_indexer.ElementByteSize(), [&]() {
228  core::ParallelFor(
229  indices.GetDevice(), n,
230  [=] OPEN3D_DEVICE(int64_t workload_idx) {
231  auto GetVoxelAt = [&] OPEN3D_DEVICE(
232  int xo, int yo,
233  int zo,
234  int curr_block_idx)
235  -> voxel_t* {
236  return DeviceGetVoxelAt<voxel_t>(
237  xo, yo, zo, curr_block_idx,
238  static_cast<int>(resolution),
239  nb_block_masks_indexer,
240  nb_block_indices_indexer,
241  voxel_block_buffer_indexer);
242  };
243 
244  // Natural index (0, N) -> (block_idx,
245  // voxel_idx)
246  int64_t workload_block_idx =
247  workload_idx / resolution3;
248  int64_t block_idx =
249  indices_ptr[workload_block_idx];
250  int64_t voxel_idx = workload_idx % resolution3;
251 
252  // voxel_idx -> (x_voxel, y_voxel, z_voxel)
253  int64_t xv, yv, zv;
254  voxel_indexer.WorkloadToCoord(voxel_idx, &xv,
255  &yv, &zv);
256 
257  voxel_t* voxel_ptr =
258  voxel_block_buffer_indexer
259  .GetDataPtr<voxel_t>(xv, yv, zv,
260  block_idx);
261  float tsdf_o = voxel_ptr->GetTSDF();
262  float weight_o = voxel_ptr->GetWeight();
263  if (weight_o <= weight_threshold) return;
264 
265  // Enumerate x-y-z directions
266  for (int i = 0; i < 3; ++i) {
267  voxel_t* ptr = GetVoxelAt(
268  static_cast<int>(xv) + (i == 0),
269  static_cast<int>(yv) + (i == 1),
270  static_cast<int>(zv) + (i == 2),
271  static_cast<int>(
272  workload_block_idx));
273  if (ptr == nullptr) continue;
274 
275  float tsdf_i = ptr->GetTSDF();
276  float weight_i = ptr->GetWeight();
277 
278  if (weight_i > weight_threshold &&
279  tsdf_i * tsdf_o < 0) {
280  OPEN3D_ATOMIC_ADD(count_ptr, 1);
281  }
282  }
283  });
284  });
285 
286 #if defined(__CUDACC__)
287  valid_size = count[0].Item<int>();
288  count[0] = 0;
289 #else
290  valid_size = (*count_ptr).load();
291  (*count_ptr) = 0;
292 #endif
293  }
294 
295  int max_count = valid_size;
296  if (points.GetLength() == 0) {
297  points = core::Tensor({max_count, 3}, core::Float32,
298  block_values.GetDevice());
299  }
300  NDArrayIndexer point_indexer(points, 1);
301 
302  // Normals
303  bool extract_normal = false;
304  NDArrayIndexer normal_indexer;
305  if (normals.has_value()) {
306  extract_normal = true;
307  if (normals.value().get().GetLength() == 0) {
308  normals.value().get() = core::Tensor({max_count, 3}, core::Float32,
309  block_values.GetDevice());
310  }
311  normal_indexer = NDArrayIndexer(normals.value().get(), 1);
312  }
313 
314  // This pass extracts exact surface points.
315  DISPATCH_BYTESIZE_TO_VOXEL(
316  voxel_block_buffer_indexer.ElementByteSize(), [&]() {
317  // Colors
318  bool extract_color = false;
319  NDArrayIndexer color_indexer;
320  if (voxel_t::HasColor() && colors.has_value()) {
321  extract_color = true;
322  if (colors.value().get().GetLength() == 0) {
323  colors.value().get() =
324  core::Tensor({max_count, 3}, core::Float32,
325  block_values.GetDevice());
326  }
327  color_indexer = NDArrayIndexer(colors.value().get(), 1);
328  }
329 
330  core::ParallelFor(
331  indices.GetDevice(), n,
332  [=] OPEN3D_DEVICE(int64_t workload_idx) {
333  auto GetVoxelAt =
334  [&] OPEN3D_DEVICE(
335  int xo, int yo, int zo,
336  int curr_block_idx) -> voxel_t* {
337  return DeviceGetVoxelAt<voxel_t>(
338  xo, yo, zo, curr_block_idx,
339  static_cast<int>(resolution),
340  nb_block_masks_indexer,
341  nb_block_indices_indexer,
342  voxel_block_buffer_indexer);
343  };
344  auto GetNormalAt = [&] OPEN3D_DEVICE(
345  int xo, int yo, int zo,
346  int curr_block_idx,
347  float* n) {
348  return DeviceGetNormalAt<voxel_t>(
349  xo, yo, zo, curr_block_idx, n,
350  static_cast<int>(resolution),
351  voxel_size, nb_block_masks_indexer,
352  nb_block_indices_indexer,
353  voxel_block_buffer_indexer);
354  };
355 
356  // Natural index (0, N) -> (block_idx, voxel_idx)
357  int64_t workload_block_idx =
358  workload_idx / resolution3;
359  int64_t block_idx = indices_ptr[workload_block_idx];
360  int64_t voxel_idx = workload_idx % resolution3;
361 
363  // block_idx -> (x_block, y_block, z_block)
364  int* block_key_ptr =
365  block_keys_indexer.GetDataPtr<int>(
366  block_idx);
367  int64_t xb = static_cast<int64_t>(block_key_ptr[0]);
368  int64_t yb = static_cast<int64_t>(block_key_ptr[1]);
369  int64_t zb = static_cast<int64_t>(block_key_ptr[2]);
370 
371  // voxel_idx -> (x_voxel, y_voxel, z_voxel)
372  int64_t xv, yv, zv;
373  voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv,
374  &zv);
375 
376  voxel_t* voxel_ptr =
377  voxel_block_buffer_indexer
378  .GetDataPtr<voxel_t>(xv, yv, zv,
379  block_idx);
380  float tsdf_o = voxel_ptr->GetTSDF();
381  float weight_o = voxel_ptr->GetWeight();
382 
383  if (weight_o <= weight_threshold) return;
384 
385  int64_t x = xb * resolution + xv;
386  int64_t y = yb * resolution + yv;
387  int64_t z = zb * resolution + zv;
388 
389  float no[3] = {0}, ni[3] = {0};
390  if (extract_normal) {
391  GetNormalAt(
392  static_cast<int>(xv),
393  static_cast<int>(yv),
394  static_cast<int>(zv),
395  static_cast<int>(workload_block_idx),
396  no);
397  }
398 
399  // Enumerate x-y-z axis
400  for (int i = 0; i < 3; ++i) {
401  voxel_t* ptr = GetVoxelAt(
402  static_cast<int>(xv) + (i == 0),
403  static_cast<int>(yv) + (i == 1),
404  static_cast<int>(zv) + (i == 2),
405  static_cast<int>(workload_block_idx));
406  if (ptr == nullptr) continue;
407 
408  float tsdf_i = ptr->GetTSDF();
409  float weight_i = ptr->GetWeight();
410 
411  if (weight_i > weight_threshold &&
412  tsdf_i * tsdf_o < 0) {
413  float ratio =
414  (0 - tsdf_o) / (tsdf_i - tsdf_o);
415 
416  int idx = OPEN3D_ATOMIC_ADD(count_ptr, 1);
417  if (idx >= valid_size) {
418  printf("Point cloud size larger than "
419  "estimated, please increase the "
420  "estimation!\n");
421  return;
422  }
423 
424  float* point_ptr =
425  point_indexer.GetDataPtr<float>(
426  idx);
427  point_ptr[0] = voxel_size *
428  (x + ratio * int(i == 0));
429  point_ptr[1] = voxel_size *
430  (y + ratio * int(i == 1));
431  point_ptr[2] = voxel_size *
432  (z + ratio * int(i == 2));
433 
434  if (extract_color) {
435  float* color_ptr =
436  color_indexer.GetDataPtr<float>(
437  idx);
438 
439  float r_o = voxel_ptr->GetR();
440  float g_o = voxel_ptr->GetG();
441  float b_o = voxel_ptr->GetB();
442 
443  float r_i = ptr->GetR();
444  float g_i = ptr->GetG();
445  float b_i = ptr->GetB();
446 
447  color_ptr[0] = ((1 - ratio) * r_o +
448  ratio * r_i) /
449  255.0f;
450  color_ptr[1] = ((1 - ratio) * g_o +
451  ratio * g_i) /
452  255.0f;
453  color_ptr[2] = ((1 - ratio) * b_o +
454  ratio * b_i) /
455  255.0f;
456  }
457 
458  if (extract_normal) {
459  GetNormalAt(
460  static_cast<int>(xv) + (i == 0),
461  static_cast<int>(yv) + (i == 1),
462  static_cast<int>(zv) + (i == 2),
463  static_cast<int>(
464  workload_block_idx),
465  ni);
466 
467  float* normal_ptr =
468  normal_indexer
469  .GetDataPtr<float>(idx);
470  float nx = (1 - ratio) * no[0] +
471  ratio * ni[0];
472  float ny = (1 - ratio) * no[1] +
473  ratio * ni[1];
474  float nz = (1 - ratio) * no[2] +
475  ratio * ni[2];
476  float norm = static_cast<float>(
477  sqrt(nx * nx + ny * ny +
478  nz * nz) +
479  1e-5);
480  normal_ptr[0] = nx / norm;
481  normal_ptr[1] = ny / norm;
482  normal_ptr[2] = nz / norm;
483  }
484  }
485  }
486  });
487  });
488 #if defined(__CUDACC__)
489  int total_count = count.Item<int>();
490 #else
491  int total_count = (*count_ptr).load();
492 #endif
493 
494  utility::LogDebug("{} vertices extracted", total_count);
495  valid_size = total_count;
496 
497 #if defined(BUILD_CUDA_MODULE) && defined(__CUDACC__)
498  core::cuda::Synchronize();
499 #endif
500 }
501 
502 #if defined(__CUDACC__)
503 void ExtractSurfaceMeshCUDA
504 #else
505 void ExtractSurfaceMeshCPU
506 #endif
507  (const core::Tensor& indices,
508  const core::Tensor& inv_indices,
509  const core::Tensor& nb_indices,
510  const core::Tensor& nb_masks,
511  const core::Tensor& block_keys,
512  const core::Tensor& block_values,
513  core::Tensor& vertices,
514  core::Tensor& triangles,
515  utility::optional<std::reference_wrapper<core::Tensor>> normals,
516  utility::optional<std::reference_wrapper<core::Tensor>> colors,
517  int64_t resolution,
518  float voxel_size,
519  float weight_threshold,
520  int& vertex_count) {
521 
522  int64_t resolution3 = resolution * resolution * resolution;
523 
524  // Shape / transform indexers, no data involved
525  NDArrayIndexer voxel_indexer({resolution, resolution, resolution});
526  int n_blocks = static_cast<int>(indices.GetLength());
527 
528  // TODO(wei): profile performance by replacing the table to a hashmap.
529  // Voxel-wise mesh info. 4 channels correspond to:
530  // 3 edges' corresponding vertex index + 1 table index.
531  core::Tensor mesh_structure;
532  try {
533  mesh_structure = core::Tensor::Zeros(
534  {n_blocks, resolution, resolution, resolution, 4}, core::Int32,
535  block_keys.GetDevice());
536  } catch (const std::runtime_error&) {
537  utility::LogError(
538  "[MeshExtractionKernel] Unable to allocate assistance mesh "
539  "structure for Marching "
540  "Cubes with {} active voxel blocks. Please consider using a "
541  "larger voxel size (currently {}) for TSDF "
542  "integration, or using tsdf_volume.cpu() to perform mesh "
543  "extraction on CPU.",
544  n_blocks, voxel_size);
545  }
546 
547  // Real data indexer
548  NDArrayIndexer voxel_block_buffer_indexer(block_values, 4);
549  NDArrayIndexer mesh_structure_indexer(mesh_structure, 4);
550  NDArrayIndexer nb_block_masks_indexer(nb_masks, 2);
551  NDArrayIndexer nb_block_indices_indexer(nb_indices, 2);
552 
553  // Plain arrays that does not require indexers
554  const int64_t* indices_ptr = indices.GetDataPtr<int64_t>();
555  const int64_t* inv_indices_ptr = inv_indices.GetDataPtr<int64_t>();
556  int64_t n = n_blocks * resolution3;
557 
558  int64_t voxel_bytesize = voxel_block_buffer_indexer.ElementByteSize();
559  // Pass 0: analyze mesh structure, set up one-on-one correspondences
560  // from edges to vertices.
561  DISPATCH_BYTESIZE_TO_VOXEL(voxel_bytesize, [&]() {
562  core::ParallelFor(
563  indices.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t widx) {
564  auto GetVoxelAt = [&] OPEN3D_DEVICE(
565  int xo, int yo, int zo,
566  int curr_block_idx) -> voxel_t* {
567  return DeviceGetVoxelAt<voxel_t>(
568  xo, yo, zo, curr_block_idx,
569  static_cast<int>(resolution),
570  nb_block_masks_indexer,
571  nb_block_indices_indexer,
572  voxel_block_buffer_indexer);
573  };
574 
575  // Natural index (0, N) -> (block_idx, voxel_idx)
576  int64_t workload_block_idx = widx / resolution3;
577  int64_t voxel_idx = widx % resolution3;
578 
579  // voxel_idx -> (x_voxel, y_voxel, z_voxel)
580  int64_t xv, yv, zv;
581  voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
582 
583  // Check per-vertex sign in the cube to determine cube
584  // type
585  int table_idx = 0;
586  for (int i = 0; i < 8; ++i) {
587  voxel_t* voxel_ptr_i = GetVoxelAt(
588  static_cast<int>(xv) + vtx_shifts[i][0],
589  static_cast<int>(yv) + vtx_shifts[i][1],
590  static_cast<int>(zv) + vtx_shifts[i][2],
591  static_cast<int>(workload_block_idx));
592  if (voxel_ptr_i == nullptr) return;
593 
594  float tsdf_i = voxel_ptr_i->GetTSDF();
595  float weight_i = voxel_ptr_i->GetWeight();
596  if (weight_i <= weight_threshold) return;
597 
598  table_idx |= ((tsdf_i < 0) ? (1 << i) : 0);
599  }
600 
601  int* mesh_struct_ptr =
602  mesh_structure_indexer.GetDataPtr<int>(
603  xv, yv, zv, workload_block_idx);
604  mesh_struct_ptr[3] = table_idx;
605 
606  if (table_idx == 0 || table_idx == 255) return;
607 
608  // Check per-edge sign determine the cube type
609  int edges_with_vertices = edge_table[table_idx];
610  for (int i = 0; i < 12; ++i) {
611  if (edges_with_vertices & (1 << i)) {
612  int64_t xv_i = xv + edge_shifts[i][0];
613  int64_t yv_i = yv + edge_shifts[i][1];
614  int64_t zv_i = zv + edge_shifts[i][2];
615  int edge_i = edge_shifts[i][3];
616 
617  int dxb = static_cast<int>(xv_i / resolution);
618  int dyb = static_cast<int>(yv_i / resolution);
619  int dzb = static_cast<int>(zv_i / resolution);
620 
621  int nb_idx =
622  (dxb + 1) + (dyb + 1) * 3 + (dzb + 1) * 9;
623 
624  int64_t block_idx_i =
625  *nb_block_indices_indexer
626  .GetDataPtr<int64_t>(
627  workload_block_idx,
628  nb_idx);
629  int* mesh_ptr_i =
630  mesh_structure_indexer.GetDataPtr<int>(
631  xv_i - dxb * resolution,
632  yv_i - dyb * resolution,
633  zv_i - dzb * resolution,
634  inv_indices_ptr[block_idx_i]);
635 
636  // Non-atomic write, but we are safe
637  mesh_ptr_i[edge_i] = -1;
638  }
639  }
640  });
641  });
642 
643  // Pass 1: determine valid number of vertices (if not preset)
644 #if defined(__CUDACC__)
645  core::Tensor count(std::vector<int>{0}, {}, core::Int32,
646  block_values.GetDevice());
647 
648  int* count_ptr = count.GetDataPtr<int>();
649 #else
650  std::atomic<int> count_atomic(0);
651  std::atomic<int>* count_ptr = &count_atomic;
652 #endif
653 
654  if (vertex_count < 0) {
655  core::ParallelFor(
656  indices.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t widx) {
657  // Natural index (0, N) -> (block_idx, voxel_idx)
658  int64_t workload_block_idx = widx / resolution3;
659  int64_t voxel_idx = widx % resolution3;
660 
661  // voxel_idx -> (x_voxel, y_voxel, z_voxel)
662  int64_t xv, yv, zv;
663  voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
664 
665  // Obtain voxel's mesh struct ptr
666  int* mesh_struct_ptr =
667  mesh_structure_indexer.GetDataPtr<int>(
668  xv, yv, zv, workload_block_idx);
669 
670  // Early quit -- no allocated vertex to compute
671  if (mesh_struct_ptr[0] != -1 && mesh_struct_ptr[1] != -1 &&
672  mesh_struct_ptr[2] != -1) {
673  return;
674  }
675 
676  // Enumerate 3 edges in the voxel
677  for (int e = 0; e < 3; ++e) {
678  int vertex_idx = mesh_struct_ptr[e];
679  if (vertex_idx != -1) continue;
680 
681  OPEN3D_ATOMIC_ADD(count_ptr, 1);
682  }
683  });
684 
685 #if defined(__CUDACC__)
686  vertex_count = count.Item<int>();
687 #else
688  vertex_count = (*count_ptr).load();
689 #endif
690  }
691 
692  utility::LogDebug("Total vertex count = {}", vertex_count);
693  vertices = core::Tensor({vertex_count, 3}, core::Float32,
694  block_values.GetDevice());
695 
696  bool extract_normal = false;
697  NDArrayIndexer normal_indexer;
698  if (normals.has_value()) {
699  extract_normal = true;
700  normals.value().get() = core::Tensor({vertex_count, 3}, core::Float32,
701  block_values.GetDevice());
702  normal_indexer = NDArrayIndexer(normals.value().get(), 1);
703  }
704 
705  NDArrayIndexer block_keys_indexer(block_keys, 1);
706  NDArrayIndexer vertex_indexer(vertices, 1);
707 
708 #if defined(__CUDACC__)
709  count = core::Tensor(std::vector<int>{0}, {}, core::Int32,
710  block_values.GetDevice());
711  count_ptr = count.GetDataPtr<int>();
712 #else
713  (*count_ptr) = 0;
714 #endif
715 
716  // Pass 2: extract vertices.
717  DISPATCH_BYTESIZE_TO_VOXEL(voxel_bytesize, [&]() {
718  bool extract_color = false;
719  NDArrayIndexer color_indexer;
720  if (voxel_t::HasColor() && colors.has_value()) {
721  extract_color = true;
722  colors.value().get() = core::Tensor(
723  {vertex_count, 3}, core::Float32, block_values.GetDevice());
724  color_indexer = NDArrayIndexer(colors.value().get(), 1);
725  }
726 
727  core::ParallelFor(
728  indices.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t widx) {
729  auto GetVoxelAt = [&] OPEN3D_DEVICE(
730  int xo, int yo, int zo,
731  int curr_block_idx) -> voxel_t* {
732  return DeviceGetVoxelAt<voxel_t>(
733  xo, yo, zo, curr_block_idx,
734  static_cast<int>(resolution),
735  nb_block_masks_indexer,
736  nb_block_indices_indexer,
737  voxel_block_buffer_indexer);
738  };
739 
740  auto GetNormalAt = [&] OPEN3D_DEVICE(int xo, int yo, int zo,
741  int curr_block_idx,
742  float* n) {
743  return DeviceGetNormalAt<voxel_t>(
744  xo, yo, zo, curr_block_idx, n,
745  static_cast<int>(resolution), voxel_size,
746  nb_block_masks_indexer,
747  nb_block_indices_indexer,
748  voxel_block_buffer_indexer);
749  };
750 
751  // Natural index (0, N) -> (block_idx, voxel_idx)
752  int64_t workload_block_idx = widx / resolution3;
753  int64_t block_idx = indices_ptr[workload_block_idx];
754  int64_t voxel_idx = widx % resolution3;
755 
756  // block_idx -> (x_block, y_block, z_block)
757  int* block_key_ptr =
758  block_keys_indexer.GetDataPtr<int>(block_idx);
759  int64_t xb = static_cast<int64_t>(block_key_ptr[0]);
760  int64_t yb = static_cast<int64_t>(block_key_ptr[1]);
761  int64_t zb = static_cast<int64_t>(block_key_ptr[2]);
762 
763  // voxel_idx -> (x_voxel, y_voxel, z_voxel)
764  int64_t xv, yv, zv;
765  voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
766 
767  // global coordinate (in voxels)
768  int64_t x = xb * resolution + xv;
769  int64_t y = yb * resolution + yv;
770  int64_t z = zb * resolution + zv;
771 
772  // Obtain voxel's mesh struct ptr
773  int* mesh_struct_ptr =
774  mesh_structure_indexer.GetDataPtr<int>(
775  xv, yv, zv, workload_block_idx);
776 
777  // Early quit -- no allocated vertex to compute
778  if (mesh_struct_ptr[0] != -1 && mesh_struct_ptr[1] != -1 &&
779  mesh_struct_ptr[2] != -1) {
780  return;
781  }
782 
783  // Obtain voxel ptr
784  voxel_t* voxel_ptr =
785  voxel_block_buffer_indexer.GetDataPtr<voxel_t>(
786  xv, yv, zv, block_idx);
787  float tsdf_o = voxel_ptr->GetTSDF();
788  float no[3] = {0}, ne[3] = {0};
789 
790  if (extract_normal) {
791  GetNormalAt(static_cast<int>(xv), static_cast<int>(yv),
792  static_cast<int>(zv),
793  static_cast<int>(workload_block_idx), no);
794  }
795 
796  // Enumerate 3 edges in the voxel
797  for (int e = 0; e < 3; ++e) {
798  int vertex_idx = mesh_struct_ptr[e];
799  if (vertex_idx != -1) continue;
800 
801  voxel_t* voxel_ptr_e = GetVoxelAt(
802  static_cast<int>(xv) + (e == 0),
803  static_cast<int>(yv) + (e == 1),
804  static_cast<int>(zv) + (e == 2),
805  static_cast<int>(workload_block_idx));
806  OPEN3D_ASSERT(
807  voxel_ptr_e != nullptr &&
808  "Internal error: GetVoxelAt returns nullptr.");
809  float tsdf_e = voxel_ptr_e->GetTSDF();
810  float ratio = (0 - tsdf_o) / (tsdf_e - tsdf_o);
811 
812  int idx = OPEN3D_ATOMIC_ADD(count_ptr, 1);
813  mesh_struct_ptr[e] = idx;
814 
815  float ratio_x = ratio * int(e == 0);
816  float ratio_y = ratio * int(e == 1);
817  float ratio_z = ratio * int(e == 2);
818 
819  float* vertex_ptr =
820  vertex_indexer.GetDataPtr<float>(idx);
821  vertex_ptr[0] = voxel_size * (x + ratio_x);
822  vertex_ptr[1] = voxel_size * (y + ratio_y);
823  vertex_ptr[2] = voxel_size * (z + ratio_z);
824 
825  if (extract_normal) {
826  float* normal_ptr =
827  normal_indexer.GetDataPtr<float>(idx);
828  GetNormalAt(static_cast<int>(xv) + (e == 0),
829  static_cast<int>(yv) + (e == 1),
830  static_cast<int>(zv) + (e == 2),
831  static_cast<int>(workload_block_idx),
832  ne);
833  float nx = (1 - ratio) * no[0] + ratio * ne[0];
834  float ny = (1 - ratio) * no[1] + ratio * ne[1];
835  float nz = (1 - ratio) * no[2] + ratio * ne[2];
836  float norm = static_cast<float>(
837  sqrt(nx * nx + ny * ny + nz * nz) + 1e-5);
838  normal_ptr[0] = nx / norm;
839  normal_ptr[1] = ny / norm;
840  normal_ptr[2] = nz / norm;
841  }
842 
843  if (extract_color) {
844  float* color_ptr =
845  color_indexer.GetDataPtr<float>(idx);
846  float r_o = voxel_ptr->GetR();
847  float g_o = voxel_ptr->GetG();
848  float b_o = voxel_ptr->GetB();
849 
850  float r_e = voxel_ptr_e->GetR();
851  float g_e = voxel_ptr_e->GetG();
852  float b_e = voxel_ptr_e->GetB();
853  color_ptr[0] =
854  ((1 - ratio) * r_o + ratio * r_e) / 255.0f;
855  color_ptr[1] =
856  ((1 - ratio) * g_o + ratio * g_e) / 255.0f;
857  color_ptr[2] =
858  ((1 - ratio) * b_o + ratio * b_e) / 255.0f;
859  }
860  }
861  });
862  });
863 
864  // Pass 3: connect vertices and form triangles.
865  int triangle_count = vertex_count * 3;
866  triangles = core::Tensor({triangle_count, 3}, core::Int64,
867  block_values.GetDevice());
868  NDArrayIndexer triangle_indexer(triangles, 1);
869 
870 #if defined(__CUDACC__)
871  count = core::Tensor(std::vector<int>{0}, {}, core::Int32,
872  block_values.GetDevice());
873  count_ptr = count.GetDataPtr<int>();
874 #else
875  (*count_ptr) = 0;
876 #endif
877  core::ParallelFor(indices.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t widx) {
878  // Natural index (0, N) -> (block_idx, voxel_idx)
879  int64_t workload_block_idx = widx / resolution3;
880  int64_t voxel_idx = widx % resolution3;
881 
882  // voxel_idx -> (x_voxel, y_voxel, z_voxel)
883  int64_t xv, yv, zv;
884  voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
885 
886  // Obtain voxel's mesh struct ptr
887  int* mesh_struct_ptr = mesh_structure_indexer.GetDataPtr<int>(
888  xv, yv, zv, workload_block_idx);
889 
890  int table_idx = mesh_struct_ptr[3];
891  if (tri_count[table_idx] == 0) return;
892 
893  for (size_t tri = 0; tri < 16; tri += 3) {
894  if (tri_table[table_idx][tri] == -1) return;
895 
896  int tri_idx = OPEN3D_ATOMIC_ADD(count_ptr, 1);
897 
898  for (size_t vertex = 0; vertex < 3; ++vertex) {
899  int edge = tri_table[table_idx][tri + vertex];
900 
901  int64_t xv_i = xv + edge_shifts[edge][0];
902  int64_t yv_i = yv + edge_shifts[edge][1];
903  int64_t zv_i = zv + edge_shifts[edge][2];
904  int64_t edge_i = edge_shifts[edge][3];
905 
906  int dxb = static_cast<int>(xv_i / resolution);
907  int dyb = static_cast<int>(yv_i / resolution);
908  int dzb = static_cast<int>(zv_i / resolution);
909 
910  int nb_idx = (dxb + 1) + (dyb + 1) * 3 + (dzb + 1) * 9;
911 
912  int64_t block_idx_i =
913  *nb_block_indices_indexer.GetDataPtr<int64_t>(
914  workload_block_idx, nb_idx);
915  int* mesh_struct_ptr_i = mesh_structure_indexer.GetDataPtr<int>(
916  xv_i - dxb * resolution, yv_i - dyb * resolution,
917  zv_i - dzb * resolution, inv_indices_ptr[block_idx_i]);
918 
919  int64_t* triangle_ptr =
920  triangle_indexer.GetDataPtr<int64_t>(tri_idx);
921  triangle_ptr[2 - vertex] = mesh_struct_ptr_i[edge_i];
922  }
923  }
924  });
925 
926 #if defined(__CUDACC__)
927  triangle_count = count.Item<int>();
928 #else
929  triangle_count = (*count_ptr).load();
930 #endif
931  utility::LogInfo("Total triangle count = {}", triangle_count);
932  triangles = triangles.Slice(0, 0, triangle_count);
933 }
934 
935 #if defined(__CUDACC__)
936 void EstimateRangeCUDA
937 #else
938 void EstimateRangeCPU
939 #endif
940  (const core::Tensor& block_keys,
941  core::Tensor& range_minmax_map,
942  const core::Tensor& intrinsics,
943  const core::Tensor& extrinsics,
944  int h,
945  int w,
946  int down_factor,
947  int64_t block_resolution,
948  float voxel_size,
949  float depth_min,
950  float depth_max) {
951 
952  // TODO(wei): reserve it in a reusable buffer
953 
954  // Every 2 channels: (min, max)
955  int h_down = h / down_factor;
956  int w_down = w / down_factor;
957  range_minmax_map = core::Tensor({h_down, w_down, 2}, core::Float32,
958  block_keys.GetDevice());
959  NDArrayIndexer range_map_indexer(range_minmax_map, 2);
960 
961  // Every 6 channels: (v_min, u_min, v_max, u_max, z_min, z_max)
962  const int fragment_size = 16;
963  const int frag_buffer_size = 65535;
964 
965  // TODO(wei): explicit buffer
966  core::Tensor fragment_buffer = core::Tensor(
967  {frag_buffer_size, 6}, core::Float32, block_keys.GetDevice());
968 
969  NDArrayIndexer frag_buffer_indexer(fragment_buffer, 1);
970  NDArrayIndexer block_keys_indexer(block_keys, 1);
971  TransformIndexer w2c_transform_indexer(intrinsics, extrinsics);
972 #if defined(__CUDACC__)
973  core::Tensor count(std::vector<int>{0}, {1}, core::Int32,
974  block_keys.GetDevice());
975  int* count_ptr = count.GetDataPtr<int>();
976 #else
977  std::atomic<int> count_atomic(0);
978  std::atomic<int>* count_ptr = &count_atomic;
979 #endif
980 
981 #ifndef __CUDACC__
982  using std::max;
983  using std::min;
984 #endif
985 
986  // Pass 0: iterate over blocks, fill-in an rendering fragment array
987  core::ParallelFor(
988  block_keys.GetDevice(), block_keys.GetLength(),
989  [=] OPEN3D_DEVICE(int64_t workload_idx) {
990  int* key = block_keys_indexer.GetDataPtr<int>(workload_idx);
991 
992  int u_min = w_down - 1, v_min = h_down - 1, u_max = 0,
993  v_max = 0;
994  float z_min = depth_max, z_max = depth_min;
995 
996  float xc, yc, zc, u, v;
997 
998  // Project 8 corners to low-res image and form a rectangle
999  for (int i = 0; i < 8; ++i) {
1000  float xw = (key[0] + ((i & 1) > 0)) * block_resolution *
1001  voxel_size;
1002  float yw = (key[1] + ((i & 2) > 0)) * block_resolution *
1003  voxel_size;
1004  float zw = (key[2] + ((i & 4) > 0)) * block_resolution *
1005  voxel_size;
1006 
1007  w2c_transform_indexer.RigidTransform(xw, yw, zw, &xc, &yc,
1008  &zc);
1009  if (zc <= 0) continue;
1010 
1011  // Project to the down sampled image buffer
1012  w2c_transform_indexer.Project(xc, yc, zc, &u, &v);
1013  u /= down_factor;
1014  v /= down_factor;
1015 
1016  v_min = min(static_cast<int>(floorf(v)), v_min);
1017  v_max = max(static_cast<int>(ceilf(v)), v_max);
1018 
1019  u_min = min(static_cast<int>(floorf(u)), u_min);
1020  u_max = max(static_cast<int>(ceilf(u)), u_max);
1021 
1022  z_min = min(z_min, zc);
1023  z_max = max(z_max, zc);
1024  }
1025 
1026  v_min = max(0, v_min);
1027  v_max = min(h_down - 1, v_max);
1028 
1029  u_min = max(0, u_min);
1030  u_max = min(w_down - 1, u_max);
1031 
1032  if (v_min >= v_max || u_min >= u_max || z_min >= z_max) return;
1033 
1034  // Divide the rectangle into small 16x16 fragments
1035  int frag_v_count =
1036  ceilf(float(v_max - v_min + 1) / float(fragment_size));
1037  int frag_u_count =
1038  ceilf(float(u_max - u_min + 1) / float(fragment_size));
1039 
1040  int frag_count = frag_v_count * frag_u_count;
1041  int frag_count_start = OPEN3D_ATOMIC_ADD(count_ptr, 1);
1042  int frag_count_end = frag_count_start + frag_count;
1043  if (frag_count_end >= frag_buffer_size) {
1044  printf("Fragment count exceeding buffer size, abort!\n");
1045  }
1046 
1047  int offset = 0;
1048  for (int frag_v = 0; frag_v < frag_v_count; ++frag_v) {
1049  for (int frag_u = 0; frag_u < frag_u_count;
1050  ++frag_u, ++offset) {
1051  float* frag_ptr = frag_buffer_indexer.GetDataPtr<float>(
1052  frag_count_start + offset);
1053  // zmin, zmax
1054  frag_ptr[0] = z_min;
1055  frag_ptr[1] = z_max;
1056 
1057  // vmin, umin
1058  frag_ptr[2] = v_min + frag_v * fragment_size;
1059  frag_ptr[3] = u_min + frag_u * fragment_size;
1060 
1061  // vmax, umax
1062  frag_ptr[4] = min(frag_ptr[2] + fragment_size - 1,
1063  static_cast<float>(v_max));
1064  frag_ptr[5] = min(frag_ptr[3] + fragment_size - 1,
1065  static_cast<float>(u_max));
1066  }
1067  }
1068  });
1069 #if defined(__CUDACC__)
1070  int frag_count = count[0].Item<int>();
1071 #else
1072  int frag_count = (*count_ptr).load();
1073 #endif
1074 
1075  // Pass 0.5: Fill in range map to prepare for atomic min/max
1076  core::ParallelFor(block_keys.GetDevice(), h_down * w_down,
1077  [=] OPEN3D_DEVICE(int64_t workload_idx) {
1078  int v = workload_idx / w_down;
1079  int u = workload_idx % w_down;
1080  float* range_ptr =
1081  range_map_indexer.GetDataPtr<float>(u, v);
1082  range_ptr[0] = depth_max;
1083  range_ptr[1] = depth_min;
1084  });
1085 
1086  // Pass 1: iterate over rendering fragment array, fill-in range
1087  core::ParallelFor(
1088  block_keys.GetDevice(), frag_count * fragment_size * fragment_size,
1089  [=] OPEN3D_DEVICE(int64_t workload_idx) {
1090  int frag_idx = workload_idx / (fragment_size * fragment_size);
1091  int local_idx = workload_idx % (fragment_size * fragment_size);
1092  int dv = local_idx / fragment_size;
1093  int du = local_idx % fragment_size;
1094 
1095  float* frag_ptr =
1096  frag_buffer_indexer.GetDataPtr<float>(frag_idx);
1097  int v_min = static_cast<int>(frag_ptr[2]);
1098  int u_min = static_cast<int>(frag_ptr[3]);
1099  int v_max = static_cast<int>(frag_ptr[4]);
1100  int u_max = static_cast<int>(frag_ptr[5]);
1101 
1102  int v = v_min + dv;
1103  int u = u_min + du;
1104  if (v > v_max || u > u_max) return;
1105 
1106  float z_min = frag_ptr[0];
1107  float z_max = frag_ptr[1];
1108  float* range_ptr = range_map_indexer.GetDataPtr<float>(u, v);
1109 #ifdef __CUDACC__
1110  atomicMinf(&(range_ptr[0]), z_min);
1111  atomicMaxf(&(range_ptr[1]), z_max);
1112 #else
1113 #pragma omp critical(EstimateRangeCPU)
1114  {
1115  range_ptr[0] = min(z_min, range_ptr[0]);
1116  range_ptr[1] = max(z_max, range_ptr[1]);
1117  }
1118 #endif
1119  });
1120 #if defined(__CUDACC__)
1121  core::cuda::Synchronize();
1122 #endif
1123 }
1124 
1125 struct MiniVecCache {
1126  int x;
1127  int y;
1128  int z;
1129  int block_idx;
1130 
1131  inline int OPEN3D_DEVICE Check(int xin, int yin, int zin) {
1132  return (xin == x && yin == y && zin == z) ? block_idx : -1;
1133  }
1134 
1135  inline void OPEN3D_DEVICE Update(int xin,
1136  int yin,
1137  int zin,
1138  int block_idx_in) {
1139  x = xin;
1140  y = yin;
1141  z = zin;
1142  block_idx = block_idx_in;
1143  }
1144 };
1145 
1146 #if defined(__CUDACC__)
1147 void RayCastCUDA
1148 #else
1149 void RayCastCPU
1150 #endif
1151  (std::shared_ptr<core::DeviceHashBackend>& hashmap,
1152  const core::Tensor& block_values,
1153  const core::Tensor& range_map,
1154  core::Tensor& vertex_map,
1155  core::Tensor& depth_map,
1156  core::Tensor& color_map,
1157  core::Tensor& normal_map,
1158  const core::Tensor& intrinsics,
1159  const core::Tensor& extrinsics,
1160  int h,
1161  int w,
1162  int64_t block_resolution,
1163  float voxel_size,
1164  float sdf_trunc,
1165  float depth_scale,
1166  float depth_min,
1167  float depth_max,
1168  float weight_threshold) {
1169  using Key = utility::MiniVec<int, 3>;
1170  using Hash = utility::MiniVecHash<int, 3>;
1171  using Eq = utility::MiniVecEq<int, 3>;
1172 
1173 #if defined(BUILD_CUDA_MODULE) && defined(__CUDACC__)
1174  auto cuda_hashmap =
1175  std::dynamic_pointer_cast<core::StdGPUHashBackend<Key, Hash, Eq>>(
1176  hashmap);
1177  if (cuda_hashmap == nullptr) {
1178  utility::LogError(
1179  "Unsupported backend: CUDA raycasting only supports STDGPU.");
1180  }
1181  auto hashmap_impl = cuda_hashmap->GetImpl();
1182 #else
1183  auto cpu_hashmap =
1184  std::dynamic_pointer_cast<core::TBBHashBackend<Key, Hash, Eq>>(
1185  hashmap);
1186  auto hashmap_impl = *cpu_hashmap->GetImpl();
1187 #endif
1188 
1189  NDArrayIndexer voxel_block_buffer_indexer(block_values, 4);
1190  NDArrayIndexer range_map_indexer(range_map, 2);
1191 
1192  NDArrayIndexer vertex_map_indexer;
1193  NDArrayIndexer depth_map_indexer;
1194  NDArrayIndexer color_map_indexer;
1195  NDArrayIndexer normal_map_indexer;
1196 
1197  bool enable_vertex = (vertex_map.GetLength() != 0);
1198  bool enable_depth = (depth_map.GetLength() != 0);
1199  bool enable_color = (color_map.GetLength() != 0);
1200  bool enable_normal = (normal_map.GetLength() != 0);
1201  if (!enable_vertex && !enable_depth && !enable_color && !enable_normal) {
1202  utility::LogWarning("No output specified for ray casting, exit.");
1203  return;
1204  }
1205 
1206  if (enable_vertex) {
1207  vertex_map_indexer = NDArrayIndexer(vertex_map, 2);
1208  }
1209  if (enable_depth) {
1210  depth_map_indexer = NDArrayIndexer(depth_map, 2);
1211  }
1212  if (enable_color) {
1213  color_map_indexer = NDArrayIndexer(color_map, 2);
1214  }
1215  if (enable_normal) {
1216  normal_map_indexer = NDArrayIndexer(normal_map, 2);
1217  }
1218 
1219  TransformIndexer c2w_transform_indexer(
1220  intrinsics, t::geometry::InverseTransformation(extrinsics));
1221  TransformIndexer w2c_transform_indexer(intrinsics, extrinsics);
1222 
1223  int64_t rows = h;
1224  int64_t cols = w;
1225 
1226  float block_size = voxel_size * block_resolution;
1227 
1228 #ifndef __CUDACC__
1229  using std::max;
1230 #endif
1231 
1232  DISPATCH_BYTESIZE_TO_VOXEL(voxel_block_buffer_indexer.ElementByteSize(), [&]() {
1233  core::ParallelFor(
1234  hashmap->GetDevice(), rows * cols,
1235  [=] OPEN3D_DEVICE(int64_t workload_idx) {
1236  auto GetVoxelAtP =
1237  [&] OPEN3D_DEVICE(int x_b, int y_b, int z_b,
1238  int x_v, int y_v, int z_v,
1239  core::buf_index_t block_addr,
1240  MiniVecCache& cache) -> voxel_t* {
1241  int x_vn = (x_v + block_resolution) % block_resolution;
1242  int y_vn = (y_v + block_resolution) % block_resolution;
1243  int z_vn = (z_v + block_resolution) % block_resolution;
1244 
1245  int dx_b = Sign(x_v - x_vn);
1246  int dy_b = Sign(y_v - y_vn);
1247  int dz_b = Sign(z_v - z_vn);
1248 
1249  if (dx_b == 0 && dy_b == 0 && dz_b == 0) {
1250  return voxel_block_buffer_indexer
1251  .GetDataPtr<voxel_t>(x_v, y_v, z_v,
1252  block_addr);
1253  } else {
1254  Key key(x_b + dx_b, y_b + dy_b, z_b + dz_b);
1255  int block_addr =
1256  cache.Check(key[0], key[1], key[2]);
1257  if (block_addr < 0) {
1258  auto iter = hashmap_impl.find(key);
1259  if (iter == hashmap_impl.end()) return nullptr;
1260  block_addr = iter->second;
1261  cache.Update(key[0], key[1], key[2],
1262  block_addr);
1263  }
1264 
1265  return voxel_block_buffer_indexer
1266  .GetDataPtr<voxel_t>(x_vn, y_vn, z_vn,
1267  block_addr);
1268  }
1269  };
1270 
1271  auto GetVoxelAtT =
1272  [&] OPEN3D_DEVICE(float x_o, float y_o, float z_o,
1273  float x_d, float y_d, float z_d,
1274  float t,
1275  MiniVecCache& cache) -> voxel_t* {
1276  float x_g = x_o + t * x_d;
1277  float y_g = y_o + t * y_d;
1278  float z_g = z_o + t * z_d;
1279 
1280  // MiniVec coordinate and look up
1281  int x_b = static_cast<int>(floorf(x_g / block_size));
1282  int y_b = static_cast<int>(floorf(y_g / block_size));
1283  int z_b = static_cast<int>(floorf(z_g / block_size));
1284 
1285  Key key(x_b, y_b, z_b);
1286  int block_addr = cache.Check(x_b, y_b, z_b);
1287  if (block_addr < 0) {
1288  auto iter = hashmap_impl.find(key);
1289  if (iter == hashmap_impl.end()) return nullptr;
1290  block_addr = iter->second;
1291  cache.Update(x_b, y_b, z_b, block_addr);
1292  }
1293 
1294  // Voxel coordinate and look up
1295  int x_v = int((x_g - x_b * block_size) / voxel_size);
1296  int y_v = int((y_g - y_b * block_size) / voxel_size);
1297  int z_v = int((z_g - z_b * block_size) / voxel_size);
1298  return voxel_block_buffer_indexer.GetDataPtr<voxel_t>(
1299  x_v, y_v, z_v, block_addr);
1300  };
1301 
1302  int64_t y = workload_idx / cols;
1303  int64_t x = workload_idx % cols;
1304 
1305  float *depth_ptr = nullptr, *vertex_ptr = nullptr,
1306  *normal_ptr = nullptr, *color_ptr = nullptr;
1307  if (enable_depth) {
1308  depth_ptr = depth_map_indexer.GetDataPtr<float>(x, y);
1309  *depth_ptr = 0;
1310  }
1311  if (enable_vertex) {
1312  vertex_ptr = vertex_map_indexer.GetDataPtr<float>(x, y);
1313  vertex_ptr[0] = 0;
1314  vertex_ptr[1] = 0;
1315  vertex_ptr[2] = 0;
1316  }
1317  if (enable_color) {
1318  color_ptr = color_map_indexer.GetDataPtr<float>(x, y);
1319  color_ptr[0] = 0;
1320  color_ptr[1] = 0;
1321  color_ptr[2] = 0;
1322  }
1323  if (enable_normal) {
1324  normal_ptr = normal_map_indexer.GetDataPtr<float>(x, y);
1325  normal_ptr[0] = 0;
1326  normal_ptr[1] = 0;
1327  normal_ptr[2] = 0;
1328  }
1329 
1330  const float* range =
1331  range_map_indexer.GetDataPtr<float>(x / 8, y / 8);
1332  float t = range[0];
1333  const float t_max = range[1];
1334  if (t >= t_max) return;
1335 
1336  // Coordinates in camera and global
1337  float x_c = 0, y_c = 0, z_c = 0;
1338  float x_g = 0, y_g = 0, z_g = 0;
1339  float x_o = 0, y_o = 0, z_o = 0;
1340 
1341  // Iterative ray intersection check
1342  float t_prev = t;
1343 
1344  float tsdf_prev = -1.0f;
1345  float tsdf = 1.0;
1346  float w = 0.0;
1347 
1348  // Camera origin
1349  c2w_transform_indexer.RigidTransform(0, 0, 0, &x_o, &y_o,
1350  &z_o);
1351 
1352  // Direction
1353  c2w_transform_indexer.Unproject(static_cast<float>(x),
1354  static_cast<float>(y), 1.0f,
1355  &x_c, &y_c, &z_c);
1356  c2w_transform_indexer.RigidTransform(x_c, y_c, z_c, &x_g,
1357  &y_g, &z_g);
1358  float x_d = (x_g - x_o);
1359  float y_d = (y_g - y_o);
1360  float z_d = (z_g - z_o);
1361 
1362  MiniVecCache cache{0, 0, 0, -1};
1363  bool surface_found = false;
1364  while (t < t_max) {
1365  voxel_t* voxel_ptr = GetVoxelAtT(x_o, y_o, z_o, x_d,
1366  y_d, z_d, t, cache);
1367 
1368  if (!voxel_ptr) {
1369  t_prev = t;
1370  t += block_size;
1371  } else {
1372  tsdf_prev = tsdf;
1373  tsdf = voxel_ptr->GetTSDF();
1374  w = voxel_ptr->GetWeight();
1375  if (tsdf_prev > 0 && w >= weight_threshold &&
1376  tsdf <= 0) {
1377  surface_found = true;
1378  break;
1379  }
1380  t_prev = t;
1381  float delta = tsdf * sdf_trunc;
1382  t += delta < voxel_size ? voxel_size : delta;
1383  }
1384  }
1385 
1386  if (surface_found) {
1387  float t_intersect = (t * tsdf_prev - t_prev * tsdf) /
1388  (tsdf_prev - tsdf);
1389  x_g = x_o + t_intersect * x_d;
1390  y_g = y_o + t_intersect * y_d;
1391  z_g = z_o + t_intersect * z_d;
1392 
1393  // Trivial vertex assignment
1394  if (enable_depth) {
1395  *depth_ptr = t_intersect * depth_scale;
1396  }
1397  if (enable_vertex) {
1398  w2c_transform_indexer.RigidTransform(
1399  x_g, y_g, z_g, vertex_ptr + 0,
1400  vertex_ptr + 1, vertex_ptr + 2);
1401  }
1402 
1403  // Trilinear interpolation
1404  // TODO(wei): simplify the flow by splitting the
1405  // functions given what is enabled
1406  if (enable_color || enable_normal) {
1407  int x_b =
1408  static_cast<int>(floorf(x_g / block_size));
1409  int y_b =
1410  static_cast<int>(floorf(y_g / block_size));
1411  int z_b =
1412  static_cast<int>(floorf(z_g / block_size));
1413  float x_v = (x_g - float(x_b) * block_size) /
1414  voxel_size;
1415  float y_v = (y_g - float(y_b) * block_size) /
1416  voxel_size;
1417  float z_v = (z_g - float(z_b) * block_size) /
1418  voxel_size;
1419 
1420  Key key(x_b, y_b, z_b);
1421 
1422  int block_addr = cache.Check(x_b, y_b, z_b);
1423  if (block_addr < 0) {
1424  auto iter = hashmap_impl.find(key);
1425  if (iter == hashmap_impl.end()) return;
1426  block_addr = iter->second;
1427  cache.Update(x_b, y_b, z_b, block_addr);
1428  }
1429 
1430  int x_v_floor = static_cast<int>(floorf(x_v));
1431  int y_v_floor = static_cast<int>(floorf(y_v));
1432  int z_v_floor = static_cast<int>(floorf(z_v));
1433 
1434  float ratio_x = x_v - float(x_v_floor);
1435  float ratio_y = y_v - float(y_v_floor);
1436  float ratio_z = z_v - float(z_v_floor);
1437 
1438  float sum_weight_color = 0.0;
1439  float sum_weight_normal = 0.0;
1440  for (int k = 0; k < 8; ++k) {
1441  int dx_v = (k & 1) > 0 ? 1 : 0;
1442  int dy_v = (k & 2) > 0 ? 1 : 0;
1443  int dz_v = (k & 4) > 0 ? 1 : 0;
1444  float ratio = (dx_v * (ratio_x) +
1445  (1 - dx_v) * (1 - ratio_x)) *
1446  (dy_v * (ratio_y) +
1447  (1 - dy_v) * (1 - ratio_y)) *
1448  (dz_v * (ratio_z) +
1449  (1 - dz_v) * (1 - ratio_z));
1450 
1451  voxel_t* voxel_ptr_k = GetVoxelAtP(
1452  x_b, y_b, z_b, x_v_floor + dx_v,
1453  y_v_floor + dy_v, z_v_floor + dz_v,
1454  block_addr, cache);
1455 
1456  if (enable_color && voxel_ptr_k &&
1457  voxel_ptr_k->GetWeight() > 0) {
1458  sum_weight_color += ratio;
1459  color_ptr[0] += ratio * voxel_ptr_k->GetR();
1460  color_ptr[1] += ratio * voxel_ptr_k->GetG();
1461  color_ptr[2] += ratio * voxel_ptr_k->GetB();
1462  }
1463 
1464  if (enable_normal) {
1465  for (int dim = 0; dim < 3; ++dim) {
1466  voxel_t* voxel_ptr_k_plus = GetVoxelAtP(
1467  x_b, y_b, z_b,
1468  x_v_floor + dx_v + (dim == 0),
1469  y_v_floor + dy_v + (dim == 1),
1470  z_v_floor + dz_v + (dim == 2),
1471  block_addr, cache);
1472  voxel_t* voxel_ptr_k_minus =
1473  GetVoxelAtP(x_b, y_b, z_b,
1474  x_v_floor + dx_v -
1475  (dim == 0),
1476  y_v_floor + dy_v -
1477  (dim == 1),
1478  z_v_floor + dz_v -
1479  (dim == 2),
1480  block_addr, cache);
1481 
1482  bool valid = false;
1483  if (voxel_ptr_k_plus &&
1484  voxel_ptr_k_plus->GetWeight() > 0) {
1485  normal_ptr[dim] +=
1486  ratio *
1487  voxel_ptr_k_plus
1488  ->GetTSDF() /
1489  (2 * voxel_size);
1490  valid = true;
1491  }
1492 
1493  if (voxel_ptr_k_minus &&
1494  voxel_ptr_k_minus->GetWeight() >
1495  0) {
1496  normal_ptr[dim] -=
1497  ratio *
1498  voxel_ptr_k_minus
1499  ->GetTSDF() /
1500  (2 * voxel_size);
1501  valid = true;
1502  }
1503  sum_weight_normal += valid ? ratio : 0;
1504  }
1505  } // if (enable_normal)
1506  } // loop over 8 neighbors
1507 
1508  if (enable_color && sum_weight_color > 0) {
1509  sum_weight_color *= 255.0;
1510  color_ptr[0] /= sum_weight_color;
1511  color_ptr[1] /= sum_weight_color;
1512  color_ptr[2] /= sum_weight_color;
1513  }
1514  if (enable_normal && sum_weight_normal > 0) {
1515  normal_ptr[0] /= sum_weight_normal;
1516  normal_ptr[1] /= sum_weight_normal;
1517  normal_ptr[2] /= sum_weight_normal;
1518  float norm =
1519  sqrt(normal_ptr[0] * normal_ptr[0] +
1520  normal_ptr[1] * normal_ptr[1] +
1521  normal_ptr[2] * normal_ptr[2]);
1522  w2c_transform_indexer.Rotate(
1523  normal_ptr[0] / norm,
1524  normal_ptr[1] / norm,
1525  normal_ptr[2] / norm, normal_ptr + 0,
1526  normal_ptr + 1, normal_ptr + 2);
1527  }
1528  } // if (color or normal)
1529  } // if (tsdf < 0)
1530  });
1531  });
1532 
1533 #if defined(__CUDACC__)
1534  core::cuda::Synchronize();
1535 #endif
1536 }
1537 
1538 } // namespace tsdf
1539 } // namespace kernel
1540 } // namespace geometry
1541 } // namespace t
1542 } // namespace open3d
open3d::core::TBBHashBackend::GetImpl
std::shared_ptr< tbb::concurrent_unordered_map< Key, buf_index_t, Hash, Eq > > GetImpl() const
Definition: TBBHashBackend.h:76
open3d::t::geometry::kernel::NDArrayIndexer
TArrayIndexer< int64_t > NDArrayIndexer
Definition: GeometryIndexer.h:380
open3d::t::geometry::kernel::TransformIndexer::Rotate
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:100
open3d::core::StdGPUHashBackend
Definition: StdGPUHashBackend.h:134
open3d::t::geometry::kernel::tsdf::MiniVecCache::x
int x
Definition: TSDFVoxelGridImpl.h:1126
open3d::t::geometry::kernel::TArrayIndexer
Definition: GeometryIndexer.h:180
open3d::t::geometry::kernel::tsdf::MiniVecCache
Definition: TSDFVoxelGridImpl.h:1125
open3d::core::Int64
const Dtype Int64
Definition: Dtype.cpp:66
open3d::t::geometry::kernel::TransformIndexer::Project
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:119
open3d::t::geometry::kernel::tsdf::ExtractSurfaceMeshCPU
void ExtractSurfaceMeshCPU(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 core::Tensor &block_values, core::Tensor &vertices, core::Tensor &triangles, utility::optional< std::reference_wrapper< core::Tensor >> vertex_normals, utility::optional< std::reference_wrapper< core::Tensor >> vertex_colors, int64_t block_resolution, float voxel_size, float weight_threshold, int &vertex_count)
Definition: TSDFVoxelGridImpl.h:507
open3d::t::geometry::kernel::TArrayIndexer::GetDataPtr
OPEN3D_HOST_DEVICE void * GetDataPtr() const
Definition: GeometryIndexer.h:335
Sign
OPEN3D_HOST_DEVICE int Sign(int x)
Definition: GeometryMacros.h:96
Utility.h
GeometryMacros.h
open3d::t::geometry::kernel::tsdf::MiniVecCache::Update
void OPEN3D_DEVICE Update(int xin, int yin, int zin, int block_idx_in)
Definition: TSDFVoxelGridImpl.h:1135
open3d::utility::MiniVecEq
Definition: Dispatch.h:129
open3d::t::geometry::kernel::TransformIndexer
Helper class for converting coordinates/indices between 3D/3D, 3D/2D, 2D/3D.
Definition: GeometryIndexer.h:44
open3d::core::ParallelFor
void ParallelFor(const Device &device, int64_t n, const func_t &func)
Definition: ParallelFor.h:122
TSDFVoxel.h
open3d::core::Float32
const Dtype Float32
Definition: Dtype.cpp:61
SizeVector.h
open3d::t::geometry::kernel::tsdf::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)
Definition: TSDFVoxelGridImpl.h:940
points
int points
Definition: FilePCD.cpp:73
open3d::core::Tensor::GetDevice
Device GetDevice() const
Definition: Tensor.cpp:1365
open3d::core::buf_index_t
uint32_t buf_index_t
Definition: HashBackendBuffer.h:63
OPEN3D_DEVICE
#define OPEN3D_DEVICE
Definition: CUDAUtils.h:64
count
int count
Definition: FilePCD.cpp:61
open3d::t::geometry::kernel::TransformIndexer::Unproject
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:130
OPEN3D_ATOMIC_ADD
#define OPEN3D_ATOMIC_ADD(X, Y)
Definition: GeometryMacros.h:58
open3d::t::geometry::kernel::TransformIndexer::RigidTransform
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:81
LogWarning
#define LogWarning(...)
Definition: Logging.h:84
open3d::core::cuda::Synchronize
void Synchronize()
Definition: CUDAUtils.cpp:78
open3d::utility::MiniVecHash
Definition: Dispatch.h:113
LogDebug
#define LogDebug(...)
Definition: Logging.h:103
color
math::float4 color
Definition: LineSetBuffers.cpp:64
open3d::t::geometry::InverseTransformation
core::Tensor InverseTransformation(const core::Tensor &T)
TODO(wei): find a proper place for such functionalities.
Definition: Utility.h:96
open3d::core::Int32
const Dtype Int32
Definition: Dtype.cpp:65
Dispatch.h
Timer.h
open3d::t::geometry::kernel::tsdf::MiniVecCache::z
int z
Definition: TSDFVoxelGridImpl.h:1128
open3d::core::TBBHashBackend
Definition: TBBHashBackend.h:41
offset
int offset
Definition: FilePCD.cpp:64
open3d::t::geometry::kernel::tsdf::MiniVecCache::y
int y
Definition: TSDFVoxelGridImpl.h:1127
open3d::io::k4a_plugin::int
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:479
MemoryManager.h
open3d::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:380
open3d::t::geometry::kernel::TArrayIndexer::ElementByteSize
OPEN3D_HOST_DEVICE index_t ElementByteSize()
Definition: GeometryIndexer.h:246
open3d::t::geometry::kernel::tsdf::ExtractSurfacePointsCPU
void ExtractSurfacePointsCPU(const core::Tensor &block_indices, const core::Tensor &nb_block_indices, const core::Tensor &nb_block_masks, const core::Tensor &block_keys, const core::Tensor &block_values, core::Tensor &points, utility::optional< std::reference_wrapper< core::Tensor >> normals, utility::optional< std::reference_wrapper< core::Tensor >> colors, int64_t block_resolution, float voxel_size, float weight_threshold, int &valid_size)
Definition: TSDFVoxelGridImpl.h:181
open3d::utility::optional
Definition: Optional.h:79
GeometryIndexer.h
open3d::t::geometry::kernel::tsdf::MiniVecCache::block_idx
int block_idx
Definition: TSDFVoxelGridImpl.h:1129
DISPATCH_BYTESIZE_TO_VOXEL
#define DISPATCH_BYTESIZE_TO_VOXEL(BYTESIZE,...)
Definition: TSDFVoxel.h:33
open3d
Definition: PinholeCameraIntrinsic.cpp:35
open3d::io::k4a_plugin::float
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:465
Tensor.h
open3d::core::Tensor::GetDataPtr
T * GetDataPtr()
Definition: Tensor.h:1074
OPEN3D_ASSERT
#define OPEN3D_ASSERT(...)
Definition: Macro.h:67
open3d::utility::MiniVec
Definition: MiniVec.h:43
open3d::t::geometry::kernel::tsdf::IntegrateCPU
void IntegrateCPU(const core::Tensor &depth, const core::Tensor &color, const core::Tensor &block_indices, const core::Tensor &block_keys, core::Tensor &block_values, const core::Tensor &intrinsics, const core::Tensor &extrinsics, int64_t resolution, float voxel_size, float sdf_trunc, float depth_scale, float depth_max)
Definition: TSDFVoxelGridImpl.h:55
Logging.h
MiniVec.h
open3d::core::Tensor::GetLength
int64_t GetLength() const
Definition: Tensor.h:1055
open3d::t::geometry::kernel::TArrayIndexer::InBoundary
OPEN3D_HOST_DEVICE bool InBoundary(float x, float y) const
Definition: GeometryIndexer.h:314
open3d::t::geometry::kernel::tsdf::MiniVecCache::Check
int OPEN3D_DEVICE Check(int xin, int yin, int zin)
Definition: TSDFVoxelGridImpl.h:1131
Dtype.h
LogInfo
#define LogInfo(...)
Definition: Logging.h:94
LogError
#define LogError(...)
Definition: Logging.h:72
open3d::t::geometry::kernel::tsdf::RayCastCPU
void RayCastCPU(std::shared_ptr< core::DeviceHashBackend > &hashmap, const core::Tensor &block_values, const core::Tensor &range_map, core::Tensor &vertex_map, core::Tensor &depth_map, core::Tensor &color_map, core::Tensor &normal_map, const core::Tensor &intrinsics, const core::Tensor &extrinsics, int h, int w, int64_t block_resolution, float voxel_size, float sdf_trunc, float depth_scale, float depth_min, float depth_max, float weight_threshold)
Definition: TSDFVoxelGridImpl.h:1151
Generated by   doxygen 1.8.13