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