163 index_t resolution2 = resolution * resolution;
164 index_t resolution3 = resolution2 * resolution;
166 TransformIndexer transform_indexer(depth_intrinsic, extrinsics, voxel_size);
171 ArrayIndexer voxel_indexer({resolution, resolution, resolution});
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.");
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>();
189 bool integrate_color =
190 block_value_map.Contains(
"color") &&
color.NumElements() > 0;
191 color_t* color_base_ptr =
nullptr;
194 float color_multiplier = 1.0;
195 if (integrate_color) {
196 color_base_ptr = block_value_map.at(
"color").
GetDataPtr<color_t>();
201 color_multiplier = 255.0;
205 index_t n = indices.GetLength() * resolution3;
208 index_t block_idx = indices_ptr[workload_idx / resolution3];
209 index_t voxel_idx = workload_idx % resolution3;
214 block_keys_indexer.GetDataPtr<
index_t>(block_idx);
221 voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
224 index_t x = xb * resolution + xv;
225 index_t y = yb * resolution + yv;
226 index_t z = zb * resolution + zv;
229 float xc, yc, zc, u, v;
231 static_cast<float>(y),
232 static_cast<float>(z), &xc, &yc, &zc);
235 transform_indexer.
Project(xc, yc, zc, &u, &v);
246 *depth_indexer.
GetDataPtr<input_depth_t>(ui, vi) / depth_scale;
248 float sdf = depth - zc;
249 if (depth <= 0 || depth > depth_max || zc <= 0 || sdf < -sdf_trunc) {
252 sdf = sdf < sdf_trunc ? sdf : sdf_trunc;
255 index_t linear_idx = block_idx * resolution3 + voxel_idx;
257 tsdf_t* tsdf_ptr = tsdf_base_ptr + linear_idx;
258 weight_t* weight_ptr = weight_base_ptr + linear_idx;
260 float inv_wsum = 1.0f / (*weight_ptr + 1);
261 float weight = *weight_ptr;
262 *tsdf_ptr = (
weight * (*tsdf_ptr) + sdf) * inv_wsum;
264 if (integrate_color) {
265 color_t* color_ptr = color_base_ptr + 3 * linear_idx;
270 transform_indexer.
Unproject(ui, vi, 1.0, &x, &y, &z);
273 colormap_indexer.
Project(x, y, z, &uf, &vf);
278 input_color_t* input_color_ptr =
279 color_indexer.
GetDataPtr<input_color_t>(ui, vi);
281 for (
index_t i = 0; i < 3; ++i) {
282 color_ptr[i] = (
weight * color_ptr[i] +
283 input_color_ptr[i] * color_multiplier) *
291#if defined(__CUDACC__)
308 int64_t block_resolution,
317 int h_down = h / down_factor;
318 int w_down = w / down_factor;
320 block_keys.GetDevice());
324 const int fragment_size = 16;
326 if (fragment_buffer.GetDataPtr() == 0 ||
327 fragment_buffer.NumElements() == 0) {
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},
335 const int frag_buffer_size = fragment_buffer.NumElements() / 6;
340#if defined(__CUDACC__)
342 block_keys.GetDevice());
343 int* count_ptr =
count.GetDataPtr<
int>();
345 std::atomic<int> count_atomic(0);
346 std::atomic<int>* count_ptr = &count_atomic;
356 block_keys.GetDevice(), block_keys.GetLength(),
358 int* key = block_keys_indexer.
GetDataPtr<
int>(workload_idx);
360 int u_min = w_down - 1, v_min = h_down - 1, u_max = 0,
362 float z_min = depth_max, z_max = depth_min;
364 float xc, yc, zc, u, v;
367 for (
int i = 0; i < 8; ++i) {
368 float xw = (key[0] + ((i & 1) > 0)) * block_resolution *
370 float yw = (key[1] + ((i & 2) > 0)) * block_resolution *
372 float zw = (key[2] + ((i & 4) > 0)) * block_resolution *
377 if (zc <= 0)
continue;
380 w2c_transform_indexer.
Project(xc, yc, zc, &u, &v);
384 v_min = min(
static_cast<int>(floorf(v)), v_min);
385 v_max = max(
static_cast<int>(ceilf(v)), v_max);
387 u_min = min(
static_cast<int>(floorf(u)), u_min);
388 u_max = max(
static_cast<int>(ceilf(u)), u_max);
390 z_min = min(z_min, zc);
391 z_max = max(z_max, zc);
394 v_min = max(0, v_min);
395 v_max = min(h_down - 1, v_max);
397 u_min = max(0, u_min);
398 u_max = min(w_down - 1, u_max);
400 if (v_min >= v_max || u_min >= u_max || z_min >= z_max)
return;
404 ceilf(
float(v_max - v_min + 1) /
float(fragment_size));
406 ceilf(
float(u_max - u_min + 1) /
float(fragment_size));
408 int frag_count = frag_v_count * frag_u_count;
410 int frag_count_end = frag_count_start + frag_count;
411 if (frag_count_end >= frag_buffer_size) {
416 for (
int frag_v = 0; frag_v < frag_v_count; ++frag_v) {
417 for (
int frag_u = 0; frag_u < frag_u_count;
419 float* frag_ptr = frag_buffer_indexer.
GetDataPtr<
float>(
420 frag_count_start +
offset);
426 frag_ptr[2] = v_min + frag_v * fragment_size;
427 frag_ptr[3] = u_min + frag_u * fragment_size;
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));
437#if defined(__CUDACC__)
438 int needed_frag_count =
count[0].Item<
int>();
440 int needed_frag_count = (*count_ptr).load();
443 int frag_count = needed_frag_count;
444 if (frag_count >= frag_buffer_size) {
446 "Could not generate full range map; allocated {} fragments but "
448 frag_buffer_size, frag_count);
449 frag_count = frag_buffer_size - 1;
451 utility::LogDebug(
"EstimateRange Allocated {} fragments and needed {}",
452 frag_buffer_size, frag_count);
458 int v = workload_idx / w_down;
459 int u = workload_idx % w_down;
462 range_ptr[0] = depth_max;
463 range_ptr[1] = depth_min;
468 block_keys.GetDevice(), frag_count * fragment_size * fragment_size,
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;
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]);
484 if (v > v_max || u > u_max)
return;
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);
490 atomicMinf(&(range_ptr[0]), z_min);
491 atomicMaxf(&(range_ptr[1]), z_max);
493#pragma omp critical(EstimateRangeCPU)
495 range_ptr[0] = min(z_min, range_ptr[0]);
496 range_ptr[1] = max(z_max, range_ptr[1]);
501#if defined(__CUDACC__)
505 if (needed_frag_count != frag_count) {
506 utility::LogInfo(
"Reallocating {} fragments for EstimateRange (was {})",
507 needed_frag_count, frag_count);
510 block_keys.GetDevice());
541 (std::shared_ptr<core::HashMap>& hashmap,
554 float weight_threshold,
555 float trunc_voxel_multiplier,
556 int range_map_down_factor) {
561 auto device_hashmap = hashmap->GetDeviceHashBackend();
562#if defined(__CUDACC__)
564 std::dynamic_pointer_cast<core::StdGPUHashBackend<Key, Hash, Eq>>(
566 if (cuda_hashmap ==
nullptr) {
568 "Unsupported backend: CUDA raycasting only supports STDGPU.");
570 auto hashmap_impl = cuda_hashmap->GetImpl();
573 std::dynamic_pointer_cast<core::TBBHashBackend<Key, Hash, Eq>>(
575 if (cpu_hashmap ==
nullptr) {
577 "Unsupported backend: CPU raycasting only supports TBB.");
579 auto hashmap_impl = *cpu_hashmap->GetImpl();
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.");
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>();
614 if (renderings_map.Contains(
"depth")) {
615 depth_indexer =
ArrayIndexer(renderings_map.at(
"depth"), 2);
617 if (renderings_map.Contains(
"vertex")) {
618 vertex_indexer =
ArrayIndexer(renderings_map.at(
"vertex"), 2);
620 if (renderings_map.Contains(
"normal")) {
621 normal_indexer =
ArrayIndexer(renderings_map.at(
"normal"), 2);
625 if (renderings_map.Contains(
"index")) {
626 index_indexer =
ArrayIndexer(renderings_map.at(
"index"), 2);
628 if (renderings_map.Contains(
"mask")) {
629 mask_indexer =
ArrayIndexer(renderings_map.at(
"mask"), 2);
631 if (renderings_map.Contains(
"interp_ratio")) {
632 interp_ratio_indexer =
635 if (renderings_map.Contains(
"interp_ratio_dx")) {
636 interp_ratio_dx_indexer =
639 if (renderings_map.Contains(
"interp_ratio_dy")) {
640 interp_ratio_dy_indexer =
643 if (renderings_map.Contains(
"interp_ratio_dz")) {
644 interp_ratio_dz_indexer =
649 bool render_color =
false;
650 if (block_value_map.Contains(
"color") && renderings_map.Contains(
"color")) {
652 color_indexer =
ArrayIndexer(renderings_map.at(
"color"), 2);
654 const color_t* color_base_ptr =
655 render_color ? block_value_map.at(
"color").GetDataPtr<color_t>()
658 bool visit_neighbors = render_color || normal_indexer.
GetDataPtr() ||
674 float block_size = voxel_size * block_resolution;
675 index_t resolution2 = block_resolution * block_resolution;
676 index_t resolution3 = resolution2 * block_resolution;
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;
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;
701 Key key(x_b + dx_b, y_b + dy_b, z_b + dz_b);
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);
711 return block_buf_idx * resolution3 + z_vn * resolution2 +
712 y_vn * block_resolution + x_vn;
717 float x_o,
float y_o,
float z_o,
718 float x_d,
float y_d,
float z_d,
float 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;
729 Key key(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);
743 return block_buf_idx * resolution3 + z_v * resolution2 +
744 y_v * block_resolution + x_v;
747 index_t y = workload_idx / cols;
748 index_t x = workload_idx % cols;
750 const float* range = range_indexer.
GetDataPtr<
float>(
751 x / range_map_down_factor, y / range_map_down_factor);
753 float* depth_ptr =
nullptr;
754 float* vertex_ptr =
nullptr;
755 float* color_ptr =
nullptr;
756 float* normal_ptr =
nullptr;
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;
766 vertex_ptr = vertex_indexer.
GetDataPtr<
float>(x, y);
772 depth_ptr = depth_indexer.
GetDataPtr<
float>(x, y);
776 normal_ptr = normal_indexer.
GetDataPtr<
float>(x, y);
783 mask_ptr = mask_indexer.
GetDataPtr<
bool>(x, y);
787 for (
int i = 0; i < 8; ++i) {
792 index_ptr = index_indexer.
GetDataPtr<int64_t>(x, y);
796 for (
int i = 0; i < 8; ++i) {
801 interp_ratio_ptr = interp_ratio_indexer.
GetDataPtr<
float>(x, y);
805 for (
int i = 0; i < 8; ++i) {
806 interp_ratio_ptr[i] = 0;
810 interp_ratio_dx_ptr =
811 interp_ratio_dx_indexer.
GetDataPtr<
float>(x, y);
815 for (
int i = 0; i < 8; ++i) {
816 interp_ratio_dx_ptr[i] = 0;
820 interp_ratio_dy_ptr =
821 interp_ratio_dy_indexer.
GetDataPtr<
float>(x, y);
825 for (
int i = 0; i < 8; ++i) {
826 interp_ratio_dy_ptr[i] = 0;
830 interp_ratio_dz_ptr =
831 interp_ratio_dz_indexer.
GetDataPtr<
float>(x, y);
835 for (
int i = 0; i < 8; ++i) {
836 interp_ratio_dz_ptr[i] = 0;
841 color_ptr = color_indexer.
GetDataPtr<
float>(x, y);
848 const float t_max = range[1];
849 if (
t >= t_max)
return;
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;
859 float tsdf_prev = -1.0f;
861 float sdf_trunc = voxel_size * trunc_voxel_multiplier;
868 c2w_transform_indexer.
Unproject(
static_cast<float>(x),
869 static_cast<float>(y), 1.0f, &x_c, &y_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);
877 bool surface_found =
false;
880 GetLinearIdxAtT(x_o, y_o, z_o, x_d, y_d, z_d,
t, cache);
882 if (linear_idx < 0) {
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;
894 float delta = tsdf * sdf_trunc;
895 t += delta < voxel_size ? voxel_size : delta;
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;
908 *depth_ptr = t_intersect * depth_scale;
912 x_g, y_g, z_g, vertex_ptr + 0, vertex_ptr + 1,
915 if (!visit_neighbors)
return;
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;
927 Key key(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);
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);
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;
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);
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;
961 if (interp_ratio_ptr) {
962 interp_ratio_ptr[k] = r;
968 index_ptr[k] = linear_idx_k;
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);
976 if (interp_ratio_dx_ptr) {
977 interp_ratio_dx_ptr[k] = interp_ratio_dx;
979 if (interp_ratio_dy_ptr) {
980 interp_ratio_dy_ptr[k] = interp_ratio_dy;
982 if (interp_ratio_dz_ptr) {
983 interp_ratio_dz_ptr[k] = interp_ratio_dz;
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;
993 index_t color_linear_idx = linear_idx_k * 3;
995 r * color_base_ptr[color_linear_idx + 0];
997 r * color_base_ptr[color_linear_idx + 1];
999 r * color_base_ptr[color_linear_idx + 2];
1009 color_ptr[0] /= sum_r;
1010 color_ptr[1] /= sum_r;
1011 color_ptr[2] /= sum_r;
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);
1029#if defined(__CUDACC__)
1050 float weight_threshold,
1055 index_t resolution2 = resolution * resolution;
1056 index_t resolution3 = resolution2 * resolution;
1059 ArrayIndexer voxel_indexer({resolution, resolution, resolution});
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.");
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>();
1084 index_t n_blocks = indices.GetLength();
1085 index_t n = n_blocks * resolution3;
1088#if defined(__CUDACC__)
1090 block_keys.GetDevice());
1093 std::atomic<index_t> count_atomic(0);
1094 std::atomic<index_t>* count_ptr = &count_atomic;
1097 if (valid_size < 0) {
1099 "No estimated max point cloud size provided, using a 2-pass "
1100 "estimation. Surface extraction could be slow.");
1108 resolution, nb_block_masks_indexer,
1109 nb_block_indices_indexer);
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;
1120 voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
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;
1128 for (
index_t i = 0; i < 3; ++i) {
1130 GetLinearIdx(xv + (i == 0), yv + (i == 1),
1131 zv + (i == 2), workload_block_idx);
1132 if (linear_idx_i < 0)
continue;
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) {
1142#if defined(__CUDACC__)
1146 valid_size = (*count_ptr).load();
1151 if (
points.GetLength() == 0) {
1165 if (color_base_ptr) {
1175 nb_block_masks_indexer,
1176 nb_block_indices_indexer);
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);
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;
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];
1201 voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
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;
1208 float no[3] = {0}, ne[3] = {0};
1211 GetNormal(xv, yv, zv, workload_block_idx, no);
1213 index_t x = xb * resolution + xv;
1214 index_t y = yb * resolution + yv;
1215 index_t z = zb * resolution + zv;
1218 for (
index_t i = 0; i < 3; ++i) {
1220 GetLinearIdx(xv + (i == 0), yv + (i == 1), zv + (i == 2),
1221 workload_block_idx);
1222 if (linear_idx_i < 0)
continue;
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);
1230 if (idx >= valid_size) {
1231 printf(
"Point cloud size larger than "
1232 "estimated, please increase the "
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));
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;
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];
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];
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;
1277#if defined(__CUDACC__)
1280 index_t total_count = (*count_ptr).load();
1283 utility::LogDebug(
"{} vertices extracted", total_count);
1284 valid_size = total_count;
1286#if defined(BUILD_CUDA_MODULE) && defined(__CUDACC__)
1309 float weight_threshold,
1313 index_t resolution = block_resolution;
1314 index_t resolution3 = resolution * resolution * resolution;
1317 ArrayIndexer voxel_indexer({resolution, resolution, resolution});
1318 index_t n_blocks =
static_cast<index_t>(block_indices.GetLength());
1326 {n_blocks, resolution, resolution, resolution, 4},
core::Int32,
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);
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);
1343 const index_t* indices_ptr = block_indices.GetDataPtr<
index_t>();
1344 const index_t* inv_indices_ptr = inv_block_indices.GetDataPtr<
index_t>();
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.");
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>();
1361 index_t n = n_blocks * resolution3;
1370 static_cast<index_t>(resolution),
1371 nb_block_masks_indexer,
1372 nb_block_indices_indexer);
1376 index_t workload_block_idx = widx / resolution3;
1377 index_t voxel_idx = widx % resolution3;
1381 voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
1386 for (
index_t i = 0; i < 8; ++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;
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;
1396 table_idx |= ((tsdf_i < 0) ? (1 << i) : 0);
1400 xv, yv, zv, workload_block_idx);
1401 mesh_struct_ptr[3] = table_idx;
1403 if (table_idx == 0 || table_idx == 255)
return;
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];
1414 index_t dxb = xv_i / resolution;
1415 index_t dyb = yv_i / resolution;
1416 index_t dzb = zv_i / resolution;
1418 index_t nb_idx = (dxb + 1) + (dyb + 1) * 3 + (dzb + 1) * 9;
1422 workload_block_idx, nb_idx);
1425 xv_i - dxb * resolution,
1426 yv_i - dyb * resolution,
1427 zv_i - dzb * resolution,
1428 inv_indices_ptr[block_idx_i]);
1431 mesh_ptr_i[edge_i] = -1;
1437#if defined(__CUDACC__)
1442 std::atomic<index_t> count_atomic(0);
1443 std::atomic<index_t>* count_ptr = &count_atomic;
1446 if (vertex_count < 0) {
1449 index_t workload_block_idx = widx / resolution3;
1450 index_t voxel_idx = widx % resolution3;
1454 voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
1459 xv, yv, zv, workload_block_idx);
1462 if (mesh_struct_ptr[0] != -1 && mesh_struct_ptr[1] != -1 &&
1463 mesh_struct_ptr[2] != -1) {
1468 for (
index_t e = 0; e < 3; ++e) {
1469 index_t vertex_idx = mesh_struct_ptr[e];
1470 if (vertex_idx != -1)
continue;
1476#if defined(__CUDACC__)
1479 vertex_count = (*count_ptr).load();
1483 utility::LogDebug(
"Total vertex count = {}", vertex_count);
1490 if (color_base_ptr) {
1498#if defined(__CUDACC__)
1512 nb_block_masks_indexer,
1513 nb_block_indices_indexer);
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);
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;
1531 index_t xb = block_key_ptr[0];
1532 index_t yb = block_key_ptr[1];
1533 index_t zb = block_key_ptr[2];
1537 voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
1540 index_t x = xb * resolution + xv;
1541 index_t y = yb * resolution + yv;
1542 index_t z = zb * resolution + zv;
1546 xv, yv, zv, workload_block_idx);
1549 if (mesh_struct_ptr[0] != -1 && mesh_struct_ptr[1] != -1 &&
1550 mesh_struct_ptr[2] != -1) {
1555 index_t linear_idx = resolution3 * block_idx + voxel_idx;
1556 float tsdf_o = tsdf_base_ptr[linear_idx];
1558 float no[3] = {0}, ne[3] = {0};
1561 GetNormal(xv, yv, zv, workload_block_idx, no);
1564 for (
index_t e = 0; e < 3; ++e) {
1565 index_t vertex_idx = mesh_struct_ptr[e];
1566 if (vertex_idx != -1)
continue;
1569 GetLinearIdx(xv + (e == 0), yv + (e == 1), zv + (e == 2),
1570 workload_block_idx);
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);
1577 mesh_struct_ptr[e] = idx;
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);
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);
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) +
1597 normal_ptr[0] = nx / norm;
1598 normal_ptr[1] = ny / norm;
1599 normal_ptr[2] = nz / norm;
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];
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];
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;
1619 index_t triangle_count = vertex_count * 3;
1623#if defined(__CUDACC__)
1631 index_t workload_block_idx = widx / resolution3;
1632 index_t voxel_idx = widx % resolution3;
1636 voxel_indexer.WorkloadToCoord(voxel_idx, &xv, &yv, &zv);
1640 xv, yv, zv, workload_block_idx);
1642 index_t table_idx = mesh_struct_ptr[3];
1643 if (tri_count[table_idx] == 0)
return;
1645 for (
index_t tri = 0; tri < 16; tri += 3) {
1646 if (tri_table[table_idx][tri] == -1)
return;
1650 for (
index_t vertex = 0; vertex < 3; ++vertex) {
1651 index_t edge = tri_table[table_idx][tri + vertex];
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];
1658 index_t dxb = xv_i / resolution;
1659 index_t dyb = yv_i / resolution;
1660 index_t dzb = zv_i / resolution;
1662 index_t nb_idx = (dxb + 1) + (dyb + 1) * 3 + (dzb + 1) * 9;
1666 workload_block_idx, nb_idx);
1669 xv_i - dxb * resolution,
1670 yv_i - dyb * resolution,
1671 zv_i - dzb * resolution,
1672 inv_indices_ptr[block_idx_i]);
1675 triangle_indexer.GetDataPtr<
index_t>(tri_idx);
1676 triangle_ptr[2 - vertex] = mesh_struct_ptr_i[edge_i];
1681#if defined(__CUDACC__)
1684 triangle_count = (*count_ptr).load();
1686 utility::LogDebug(
"Total triangle count = {}", triangle_count);
1687 triangles = triangles.Slice(0, 0, triangle_count);