49 namespace pointcloud {
58 #if defined(__CUDACC__)
74 const bool have_colors = image_colors.has_value();
90 const auto& imcol = image_colors.value().get();
92 colors.value().get() =
core::Tensor({rows_strided * cols_strided, 3},
98 #if defined(__CUDACC__)
100 int* count_ptr =
count.GetDataPtr<
int>();
102 std::atomic<int> count_atomic(0);
103 std::atomic<int>* count_ptr = &count_atomic;
106 int64_t n = rows_strided * cols_strided;
110 depth.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
111 int64_t y = (workload_idx / cols_strided) * stride;
112 int64_t x = (workload_idx % cols_strided) * stride;
114 float d = *depth_indexer.GetDataPtr<scalar_t>(x, y) /
116 if (d > 0 && d < depth_max) {
117 int idx = OPEN3D_ATOMIC_ADD(count_ptr, 1);
119 float x_c = 0, y_c = 0, z_c = 0;
120 ti.Unproject(static_cast<float>(x),
121 static_cast<float>(y), d, &x_c, &y_c,
124 float* vertex = point_indexer.GetDataPtr<float>(idx);
125 ti.RigidTransform(x_c, y_c, z_c, vertex + 0, vertex + 1,
129 colors_indexer.GetDataPtr<float>(idx);
131 image_colors_indexer.GetDataPtr<float>(x,
133 *pcd_pixel = *image_pixel;
134 *(pcd_pixel + 1) = *(image_pixel + 1);
135 *(pcd_pixel + 2) = *(image_pixel + 2);
140 #if defined(__CUDACC__)
141 int total_pts_count =
count.Item<
int>();
143 int total_pts_count = (*count_ptr).load();
151 colors.value().get() =
152 colors.value().get().Slice(0, 0, total_pts_count);
156 #if defined(__CUDACC__)
157 void GetPointMaskWithinAABBCUDA
167 const scalar_t* points_ptr = points.GetDataPtr<scalar_t>();
168 const int64_t n = points.GetLength();
169 const scalar_t* min_bound_ptr = min_bound.GetDataPtr<scalar_t>();
170 const scalar_t* max_bound_ptr = max_bound.GetDataPtr<scalar_t>();
171 bool* mask_ptr = mask.GetDataPtr<bool>();
174 points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
175 const scalar_t x = points_ptr[3 * workload_idx + 0];
176 const scalar_t y = points_ptr[3 * workload_idx + 1];
177 const scalar_t z = points_ptr[3 * workload_idx + 2];
179 if (x >= min_bound_ptr[0] && x <= max_bound_ptr[0] &&
180 y >= min_bound_ptr[1] && y <= max_bound_ptr[1] &&
181 z >= min_bound_ptr[2] && z <= max_bound_ptr[2]) {
182 mask_ptr[workload_idx] = true;
184 mask_ptr[workload_idx] = false;
190 #if defined(__CUDACC__)
191 void NormalizeNormalsCUDA
197 const int64_t n = normals.GetLength();
200 scalar_t* ptr = normals.GetDataPtr<scalar_t>();
204 int64_t idx = 3 * workload_idx;
205 scalar_t x = ptr[idx];
206 scalar_t y = ptr[idx + 1];
207 scalar_t z = ptr[idx + 2];
208 scalar_t norm = sqrt(x * x + y * y + z * z);
221 #if defined(__CUDACC__)
222 void OrientNormalsToAlignWithDirectionCUDA
228 const int64_t n = normals.GetLength();
231 scalar_t* ptr = normals.GetDataPtr<scalar_t>();
232 const scalar_t* direction_ptr = direction.GetDataPtr<scalar_t>();
236 int64_t idx = 3 * workload_idx;
237 scalar_t* normal = ptr + idx;
238 const scalar_t norm = sqrt(normal[0] * normal[0] +
239 normal[1] * normal[1] +
240 normal[2] * normal[2]);
242 normal[0] = direction_ptr[0];
243 normal[1] = direction_ptr[1];
244 normal[2] = direction_ptr[2];
246 normal, direction_ptr) < 0) {
255 #if defined(__CUDACC__)
256 void OrientNormalsTowardsCameraLocationCUDA
264 const int64_t n = normals.GetLength();
267 scalar_t* normals_ptr = normals.GetDataPtr<scalar_t>();
268 const scalar_t* camera_ptr = camera.GetDataPtr<scalar_t>();
269 const scalar_t* points_ptr =
points.GetDataPtr<scalar_t>();
272 normals.GetDevice(), n,
274 int64_t idx = 3 * workload_idx;
275 scalar_t* normal = normals_ptr + idx;
276 const scalar_t* point = points_ptr + idx;
277 const scalar_t reference[3] = {camera_ptr[0] - point[0],
278 camera_ptr[1] - point[1],
279 camera_ptr[2] - point[2]};
280 const scalar_t norm =
281 sqrt(normal[0] * normal[0] + normal[1] * normal[1] +
282 normal[2] * normal[2]);
284 normal[0] = reference[0];
285 normal[1] = reference[1];
286 normal[2] = reference[2];
287 const scalar_t norm_new = sqrt(normal[0] * normal[0] +
288 normal[1] * normal[1] +
289 normal[2] * normal[2]);
290 if (norm_new == 0.0) {
295 normal[0] /= norm_new;
296 normal[1] /= norm_new;
297 normal[2] /= norm_new;
309 template <
typename scalar_t>
318 if (!(abs(query[0] - query[2]) < 1e-6) ||
319 !(abs(query[1] - query[2]) < 1e-6)) {
320 const scalar_t norm2_inv =
321 1.0 / sqrt(query[0] * query[0] + query[1] * query[1]);
322 v[0] = -1 * query[1] * norm2_inv;
323 v[1] = query[0] * norm2_inv;
326 const scalar_t norm2_inv =
327 1.0 / sqrt(query[1] * query[1] + query[2] * query[2]);
329 v[1] = -1 * query[2] * norm2_inv;
330 v[2] = query[1] * norm2_inv;
336 template <
typename scalar_t>
343 template <
typename scalar_t>
346 int l = 2 * root + 1;
347 int r = 2 * root + 2;
349 if (l < n && arr[l] > arr[largest]) {
352 if (r < n && arr[r] > arr[largest]) {
355 if (largest != root) {
356 Swap<scalar_t>(&arr[root], &arr[largest]);
357 Heapify<scalar_t>(arr, n, largest);
361 template <
typename scalar_t>
363 for (
int i = n / 2 - 1; i >= 0; i--)
Heapify(arr, n, i);
365 for (
int i = n - 1; i > 0; i--) {
366 Swap<scalar_t>(&arr[0], &arr[i]);
367 Heapify<scalar_t>(arr, i, 0);
371 template <
typename scalar_t>
374 double angle_threshold) {
376 scalar_t max_diff = 0;
378 for (
int i = 0; i < counts - 1; i++) {
379 diff = angles[i + 1] - angles[i];
380 max_diff = max(max_diff, diff);
384 diff = 2 * M_PI - angles[counts - 1] + angles[0];
385 max_diff = max(max_diff, diff);
387 return max_diff > angle_threshold * M_PI / 180.0 ? true :
false;
390 #if defined(__CUDACC__)
391 void ComputeBoundaryPointsCUDA
400 double angle_threshold) {
402 const int nn_size = indices.GetShape()[1];
405 const scalar_t* points_ptr = points.GetDataPtr<scalar_t>();
406 const scalar_t* normals_ptr = normals.GetDataPtr<scalar_t>();
407 const int64_t n = points.GetLength();
408 const int32_t* indices_ptr = indices.GetDataPtr<int32_t>();
409 const int32_t* counts_ptr = counts.GetDataPtr<int32_t>();
410 bool* mask_ptr = mask.GetDataPtr<bool>();
412 core::Tensor angles = core::Tensor::Full(
413 indices.GetShape(), -10, points.GetDtype(), points.GetDevice());
414 scalar_t* angles_ptr = angles.GetDataPtr<scalar_t>();
417 points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
419 GetCoordinateSystemOnPlane(normals_ptr + 3 * workload_idx,
423 int indices_size = counts_ptr[workload_idx] - 1;
424 if (indices_size > 0) {
425 const scalar_t* query = points_ptr + 3 * workload_idx;
426 for (int i = 1; i < indices_size + 1; i++) {
427 const int idx = workload_idx * nn_size + i;
429 const scalar_t* point_ref =
430 points_ptr + 3 * indices_ptr[idx];
431 const scalar_t delta[3] = {point_ref[0] - query[0],
432 point_ref[1] - query[1],
433 point_ref[2] - query[2]};
434 const scalar_t angle = atan2(
435 core::linalg::kernel::dot_3x1(v, delta),
436 core::linalg::kernel::dot_3x1(u, delta));
438 angles_ptr[idx] = angle;
443 angles_ptr + workload_idx * nn_size + 1,
446 mask_ptr[workload_idx] = IsBoundaryPoints<scalar_t>(
447 angles_ptr + workload_idx * nn_size + 1,
448 indices_size, angle_threshold);
456 template <
typename scalar_t>
458 const scalar_t* points_ptr,
461 scalar_t* covariance_ptr) {
462 if (indices_count < 3) {
463 covariance_ptr[0] = 1.0;
464 covariance_ptr[1] = 0.0;
465 covariance_ptr[2] = 0.0;
466 covariance_ptr[3] = 0.0;
467 covariance_ptr[4] = 1.0;
468 covariance_ptr[5] = 0.0;
469 covariance_ptr[6] = 0.0;
470 covariance_ptr[7] = 0.0;
471 covariance_ptr[8] = 1.0;
475 double centroid[3] = {0};
476 for (
int32_t i = 0; i < indices_count; ++i) {
477 int32_t idx = 3 * indices_ptr[i];
478 centroid[0] += points_ptr[idx];
479 centroid[1] += points_ptr[idx + 1];
480 centroid[2] += points_ptr[idx + 2];
483 centroid[0] /= indices_count;
484 centroid[1] /= indices_count;
485 centroid[2] /= indices_count;
488 double cumulants[6] = {0};
489 for (
int32_t i = 0; i < indices_count; ++i) {
490 int32_t idx = 3 * indices_ptr[i];
491 const double x =
static_cast<double>(points_ptr[idx]) - centroid[0];
492 const double y =
static_cast<double>(points_ptr[idx + 1]) - centroid[1];
493 const double z =
static_cast<double>(points_ptr[idx + 2]) - centroid[2];
495 cumulants[0] += x * x;
496 cumulants[1] += y * y;
497 cumulants[2] += z * z;
499 cumulants[3] += x * y;
500 cumulants[4] += x * z;
501 cumulants[5] += y * z;
507 const double normalization_factor =
static_cast<double>(indices_count - 1);
508 for (
int i = 0; i < 6; ++i) {
509 cumulants[i] /= normalization_factor;
513 covariance_ptr[0] =
static_cast<scalar_t
>(cumulants[0]);
515 covariance_ptr[4] =
static_cast<scalar_t
>(cumulants[1]);
517 covariance_ptr[8] =
static_cast<scalar_t
>(cumulants[2]);
520 covariance_ptr[1] =
static_cast<scalar_t
>(cumulants[3]);
521 covariance_ptr[3] = covariance_ptr[1];
524 covariance_ptr[2] =
static_cast<scalar_t
>(cumulants[4]);
525 covariance_ptr[6] = covariance_ptr[2];
528 covariance_ptr[5] =
static_cast<scalar_t
>(cumulants[5]);
529 covariance_ptr[7] = covariance_ptr[5];
532 #if defined(__CUDACC__)
533 void EstimateCovariancesUsingHybridSearchCUDA
539 const double& radius,
540 const int64_t& max_nn) {
542 int64_t n =
points.GetLength();
551 std::tie(indices, distance, counts) =
555 const scalar_t* points_ptr =
points.GetDataPtr<scalar_t>();
558 scalar_t* covariances_ptr = covariances.GetDataPtr<scalar_t>();
563 const int32_t neighbour_offset = max_nn * workload_idx;
565 const int32_t neighbour_count =
566 neighbour_counts_ptr[workload_idx];
569 const int32_t covariances_offset = 9 * workload_idx;
573 neighbour_indices_ptr + neighbour_offset,
575 covariances_ptr + covariances_offset);
582 #if defined(__CUDACC__)
583 void EstimateCovariancesUsingRadiusSearchCUDA
589 const double& radius) {
591 int64_t n =
points.GetLength();
600 std::tie(indices, distance, counts) =
604 const scalar_t* points_ptr =
points.GetDataPtr<scalar_t>();
607 scalar_t* covariances_ptr = covariances.GetDataPtr<scalar_t>();
611 const int32_t neighbour_offset =
612 neighbour_counts_ptr[workload_idx];
613 const int32_t neighbour_count =
614 (neighbour_counts_ptr[workload_idx + 1] -
615 neighbour_counts_ptr[workload_idx]);
618 const int32_t covariances_offset = 9 * workload_idx;
622 neighbour_indices_ptr + neighbour_offset,
624 covariances_ptr + covariances_offset);
631 #if defined(__CUDACC__)
632 void EstimateCovariancesUsingKNNSearchCUDA
638 const int64_t& max_nn) {
640 int64_t n =
points.GetLength();
656 "Not enough neighbors to compute Covariances / Normals. "
658 "increasing the max_nn parameter.");
662 auto points_ptr =
points.GetDataPtr<scalar_t>();
664 auto covariances_ptr = covariances.GetDataPtr<scalar_t>();
669 const int32_t neighbour_offset = nn_count * workload_idx;
672 const int32_t covariances_offset = 9 * workload_idx;
676 neighbour_indices_ptr + neighbour_offset, nn_count,
677 covariances_ptr + covariances_offset);
684 template <
typename scalar_t>
686 const scalar_t eval0,
687 scalar_t* eigen_vector0) {
688 scalar_t row0[3] = {A[0] - eval0, A[1], A[2]};
689 scalar_t row1[3] = {A[1], A[4] - eval0, A[5]};
690 scalar_t row2[3] = {A[2], A[5], A[8] - eval0};
692 scalar_t r0xr1[3], r0xr2[3], r1xr2[3];
713 scalar_t sqrt_d = sqrt(d0);
714 eigen_vector0[0] = r0xr1[0] / sqrt_d;
715 eigen_vector0[1] = r0xr1[1] / sqrt_d;
716 eigen_vector0[2] = r0xr1[2] / sqrt_d;
718 }
else if (imax == 1) {
719 scalar_t sqrt_d = sqrt(d1);
720 eigen_vector0[0] = r0xr2[0] / sqrt_d;
721 eigen_vector0[1] = r0xr2[1] / sqrt_d;
722 eigen_vector0[2] = r0xr2[2] / sqrt_d;
725 scalar_t sqrt_d = sqrt(d2);
726 eigen_vector0[0] = r1xr2[0] / sqrt_d;
727 eigen_vector0[1] = r1xr2[1] / sqrt_d;
728 eigen_vector0[2] = r1xr2[2] / sqrt_d;
733 template <
typename scalar_t>
735 const scalar_t* evec0,
736 const scalar_t eval1,
737 scalar_t* eigen_vector1) {
739 if (abs(evec0[0]) > abs(evec0[1])) {
740 scalar_t inv_length =
741 1.0 / sqrt(evec0[0] * evec0[0] + evec0[2] * evec0[2]);
742 U[0] = -evec0[2] * inv_length;
744 U[2] = evec0[0] * inv_length;
746 scalar_t inv_length =
747 1.0 / sqrt(evec0[1] * evec0[1] + evec0[2] * evec0[2]);
749 U[1] = evec0[2] * inv_length;
750 U[2] = -evec0[1] * inv_length;
752 scalar_t V[3], AU[3], AV[3];
754 core::linalg::kernel::matmul3x3_3x1(A, U, AU);
755 core::linalg::kernel::matmul3x3_3x1(A, V, AV);
761 scalar_t absM00 = abs(m00);
762 scalar_t absM01 = abs(m01);
763 scalar_t absM11 = abs(m11);
764 scalar_t max_abs_comp;
766 if (absM00 >= absM11) {
767 max_abs_comp = max(absM00, absM01);
768 if (max_abs_comp > 0) {
769 if (absM00 >= absM01) {
771 m00 = 1 / sqrt(1 + m01 * m01);
775 m01 = 1 / sqrt(1 + m00 * m00);
778 eigen_vector1[0] = m01 * U[0] - m00 * V[0];
779 eigen_vector1[1] = m01 * U[1] - m00 * V[1];
780 eigen_vector1[2] = m01 * U[2] - m00 * V[2];
783 eigen_vector1[0] = U[0];
784 eigen_vector1[1] = U[1];
785 eigen_vector1[2] = U[2];
789 max_abs_comp = max(absM11, absM01);
790 if (max_abs_comp > 0) {
791 if (absM11 >= absM01) {
793 m11 = 1 / sqrt(1 + m01 * m01);
797 m01 = 1 / sqrt(1 + m11 * m11);
800 eigen_vector1[0] = m11 * U[0] - m01 * V[0];
801 eigen_vector1[1] = m11 * U[1] - m01 * V[1];
802 eigen_vector1[2] = m11 * U[2] - m01 * V[2];
805 eigen_vector1[0] = U[0];
806 eigen_vector1[1] = U[1];
807 eigen_vector1[2] = U[2];
813 template <
typename scalar_t>
815 const scalar_t* covariance_ptr, scalar_t* normals_ptr) {
819 scalar_t max_coeff = covariance_ptr[0];
821 for (
int i = 1; i < 9; ++i) {
822 if (max_coeff < covariance_ptr[i]) {
823 max_coeff = covariance_ptr[i];
827 if (max_coeff == 0) {
828 normals_ptr[0] = 0.0;
829 normals_ptr[1] = 0.0;
830 normals_ptr[2] = 0.0;
836 for (
int i = 0; i < 9; ++i) {
837 A[i] = covariance_ptr[i] / max_coeff;
840 scalar_t norm = A[1] * A[1] + A[2] * A[2] + A[5] * A[5];
848 scalar_t q = (A[0] + A[4] + A[8]) / 3.0;
850 scalar_t b00 = A[0] - q;
851 scalar_t b11 = A[4] - q;
852 scalar_t b22 = A[8] - q;
855 sqrt((b00 * b00 + b11 * b11 + b22 * b22 + norm * 2.0) / 6.0);
857 scalar_t c00 = b11 * b22 - A[5] * A[5];
858 scalar_t c01 = A[1] * b22 - A[5] * A[2];
859 scalar_t c02 = A[1] * A[5] - b11 * A[2];
860 scalar_t det = (b00 * c00 - A[1] * c01 + A[2] * c02) / (p * p * p);
862 scalar_t half_det = det * 0.5;
863 half_det = min(max(half_det,
static_cast<scalar_t
>(-1.0)),
864 static_cast<scalar_t
>(1.0));
866 scalar_t angle = acos(half_det) / 3.0;
867 const scalar_t two_thrids_pi = 2.09439510239319549;
869 scalar_t beta2 = cos(angle) * 2.0;
870 scalar_t beta0 = cos(angle + two_thrids_pi) * 2.0;
871 scalar_t beta1 = -(beta0 + beta2);
873 eval[0] = q + p * beta0;
874 eval[1] = q + p * beta1;
875 eval[2] = q + p * beta2;
878 ComputeEigenvector0<scalar_t>(A, eval[2], evec2);
880 if (eval[2] < eval[0] && eval[2] < eval[1]) {
881 normals_ptr[0] = evec2[0];
882 normals_ptr[1] = evec2[1];
883 normals_ptr[2] = evec2[2];
888 ComputeEigenvector1<scalar_t>(A, evec2, eval[1], evec1);
890 if (eval[1] < eval[0] && eval[1] < eval[2]) {
891 normals_ptr[0] = evec1[0];
892 normals_ptr[1] = evec1[1];
893 normals_ptr[2] = evec1[2];
898 normals_ptr[0] = evec1[1] * evec2[2] - evec1[2] * evec2[1];
899 normals_ptr[1] = evec1[2] * evec2[0] - evec1[0] * evec2[2];
900 normals_ptr[2] = evec1[0] * evec2[1] - evec1[1] * evec2[0];
904 ComputeEigenvector0<scalar_t>(A, eval[0], evec0);
906 if (eval[0] < eval[1] && eval[0] < eval[2]) {
907 normals_ptr[0] = evec0[0];
908 normals_ptr[1] = evec0[1];
909 normals_ptr[2] = evec0[2];
913 ComputeEigenvector1<scalar_t>(A, evec0, eval[1], evec1);
915 if (eval[1] < eval[0] && eval[1] < eval[2]) {
916 normals_ptr[0] = evec1[0];
917 normals_ptr[1] = evec1[1];
918 normals_ptr[2] = evec1[2];
922 normals_ptr[0] = evec0[1] * evec1[2] - evec0[2] * evec1[1];
923 normals_ptr[1] = evec0[2] * evec1[0] - evec0[0] * evec1[2];
924 normals_ptr[2] = evec0[0] * evec1[1] - evec0[1] * evec1[0];
928 if (covariance_ptr[0] < covariance_ptr[4] &&
929 covariance_ptr[0] < covariance_ptr[8]) {
930 normals_ptr[0] = 1.0;
931 normals_ptr[1] = 0.0;
932 normals_ptr[2] = 0.0;
934 }
else if (covariance_ptr[0] < covariance_ptr[4] &&
935 covariance_ptr[0] < covariance_ptr[8]) {
936 normals_ptr[0] = 0.0;
937 normals_ptr[1] = 1.0;
938 normals_ptr[2] = 0.0;
941 normals_ptr[0] = 0.0;
942 normals_ptr[1] = 0.0;
943 normals_ptr[2] = 1.0;
949 #if defined(__CUDACC__)
950 void EstimateNormalsFromCovariancesCUDA
958 int64_t n = covariances.GetLength();
961 const scalar_t* covariances_ptr = covariances.GetDataPtr<scalar_t>();
962 scalar_t* normals_ptr = normals.GetDataPtr<scalar_t>();
965 covariances.GetDevice(), n,
967 int32_t covariances_offset = 9 * workload_idx;
968 int32_t normals_offset = 3 * workload_idx;
969 scalar_t normals_output[3] = {0};
970 EstimatePointWiseNormalsWithFastEigen3x3<scalar_t>(
971 covariances_ptr + covariances_offset,
974 if ((normals_output[0] * normals_output[0] +
975 normals_output[1] * normals_output[1] +
976 normals_output[2] * normals_output[2]) == 0.0 &&
978 normals_output[0] = 0.0;
979 normals_output[1] = 0.0;
980 normals_output[2] = 1.0;
983 if ((normals_ptr[normals_offset] * normals_output[0] +
984 normals_ptr[normals_offset + 1] *
986 normals_ptr[normals_offset + 2] *
987 normals_output[2]) < 0.0) {
988 normals_output[0] *= -1;
989 normals_output[1] *= -1;
990 normals_output[2] *= -1;
994 normals_ptr[normals_offset] = normals_output[0];
995 normals_ptr[normals_offset + 1] = normals_output[1];
996 normals_ptr[normals_offset + 2] = normals_output[2];
1003 template <
typename scalar_t>
1005 const scalar_t* points_ptr,
1006 const scalar_t* normals_ptr,
1007 const scalar_t* colors_ptr,
1011 scalar_t* color_gradients_ptr) {
1012 if (indices_count < 4) {
1013 color_gradients_ptr[idx_offset] = 0;
1014 color_gradients_ptr[idx_offset + 1] = 0;
1015 color_gradients_ptr[idx_offset + 2] = 0;
1017 scalar_t vt[3] = {points_ptr[idx_offset], points_ptr[idx_offset + 1],
1018 points_ptr[idx_offset + 2]};
1020 scalar_t nt[3] = {normals_ptr[idx_offset], normals_ptr[idx_offset + 1],
1021 normals_ptr[idx_offset + 2]};
1023 scalar_t it = (colors_ptr[idx_offset] + colors_ptr[idx_offset + 1] +
1024 colors_ptr[idx_offset + 2]) /
1027 scalar_t AtA[9] = {0};
1028 scalar_t Atb[3] = {0};
1038 scalar_t s = vt[0] * nt[0] + vt[1] * nt[1] + vt[2] * nt[2];
1041 for (; i < indices_count; i++) {
1042 int64_t neighbour_idx_offset = 3 * indices_ptr[i];
1044 if (neighbour_idx_offset == -1) {
1048 scalar_t vt_adj[3] = {points_ptr[neighbour_idx_offset],
1049 points_ptr[neighbour_idx_offset + 1],
1050 points_ptr[neighbour_idx_offset + 2]};
1054 scalar_t d = vt_adj[0] * nt[0] + vt_adj[1] * nt[1] +
1055 vt_adj[2] * nt[2] - s;
1058 scalar_t vt_proj[3] = {vt_adj[0] - d * nt[0], vt_adj[1] - d * nt[1],
1059 vt_adj[2] - d * nt[2]};
1061 scalar_t it_adj = (colors_ptr[neighbour_idx_offset + 0] +
1062 colors_ptr[neighbour_idx_offset + 1] +
1063 colors_ptr[neighbour_idx_offset + 2]) /
1066 scalar_t A[3] = {vt_proj[0] - vt[0], vt_proj[1] - vt[1],
1067 vt_proj[2] - vt[2]};
1069 AtA[0] += A[0] * A[0];
1070 AtA[1] += A[1] * A[0];
1071 AtA[2] += A[2] * A[0];
1072 AtA[4] += A[1] * A[1];
1073 AtA[5] += A[2] * A[1];
1074 AtA[8] += A[2] * A[2];
1076 scalar_t b = it_adj - it;
1084 scalar_t A[3] = {(i - 1) * nt[0], (i - 1) * nt[1], (i - 1) * nt[2]};
1086 AtA[0] += A[0] * A[0];
1087 AtA[1] += A[0] * A[1];
1088 AtA[2] += A[0] * A[2];
1089 AtA[4] += A[1] * A[1];
1090 AtA[5] += A[1] * A[2];
1091 AtA[8] += A[2] * A[2];
1099 color_gradients_ptr + idx_offset);
1103 #if defined(__CUDACC__)
1104 void EstimateColorGradientsUsingHybridSearchCUDA
1112 const double& radius,
1113 const int64_t& max_nn) {
1115 int64_t n =
points.GetLength();
1125 std::tie(indices, distance, counts) =
1129 auto points_ptr =
points.GetDataPtr<scalar_t>();
1130 auto normals_ptr = normals.GetDataPtr<scalar_t>();
1131 auto colors_ptr = colors.GetDataPtr<scalar_t>();
1134 auto color_gradients_ptr = color_gradients.GetDataPtr<scalar_t>();
1139 int32_t neighbour_offset = max_nn * workload_idx;
1142 neighbour_counts_ptr[workload_idx];
1143 int32_t idx_offset = 3 * workload_idx;
1146 points_ptr, normals_ptr, colors_ptr, idx_offset,
1147 neighbour_indices_ptr + neighbour_offset,
1148 neighbour_count, color_gradients_ptr);
1155 #if defined(__CUDACC__)
1156 void EstimateColorGradientsUsingKNNSearchCUDA
1164 const int64_t& max_nn) {
1166 int64_t n =
points.GetLength();
1179 int64_t nn_count = indices.
GetShape()[1];
1183 "Not enough neighbors to compute Covariances / Normals. "
1185 "changing the search parameter.");
1189 auto points_ptr =
points.GetDataPtr<scalar_t>();
1190 auto normals_ptr = normals.GetDataPtr<scalar_t>();
1191 auto colors_ptr = colors.GetDataPtr<scalar_t>();
1193 auto color_gradients_ptr = color_gradients.GetDataPtr<scalar_t>();
1197 int32_t neighbour_offset = max_nn * workload_idx;
1198 int32_t idx_offset = 3 * workload_idx;
1201 points_ptr, normals_ptr, colors_ptr, idx_offset,
1202 neighbour_indices_ptr + neighbour_offset, nn_count,
1203 color_gradients_ptr);
1210 #if defined(__CUDACC__)
1211 void EstimateColorGradientsUsingRadiusSearchCUDA
1219 const double& radius) {
1221 int64_t n =
points.GetLength();
1231 std::tie(indices, distance, counts) =
1238 auto points_ptr =
points.GetDataPtr<scalar_t>();
1239 auto normals_ptr = normals.GetDataPtr<scalar_t>();
1240 auto colors_ptr = colors.GetDataPtr<scalar_t>();
1243 auto color_gradients_ptr = color_gradients.GetDataPtr<scalar_t>();
1248 neighbour_counts_ptr[workload_idx];
1250 const int32_t neighbour_count =
1251 (neighbour_counts_ptr[workload_idx + 1] -
1252 neighbour_counts_ptr[workload_idx]);
1253 int32_t idx_offset = 3 * workload_idx;
1256 points_ptr, normals_ptr, colors_ptr, idx_offset,
1257 neighbour_indices_ptr + neighbour_offset,
1258 neighbour_count, color_gradients_ptr);
#define OPEN3D_HOST_DEVICE
Definition: CUDAUtils.h:63
#define OPEN3D_DEVICE
Definition: CUDAUtils.h:64
#define DISPATCH_DTYPE_TO_TEMPLATE(DTYPE,...)
Definition: Dispatch.h:49
#define DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(DTYPE,...)
Definition: Dispatch.h:96
#define LogError(...)
Definition: Logging.h:67
size_t stride
Definition: TriangleMeshBuffers.cpp:184
T * GetDataPtr()
Definition: Tensor.h:1149
SizeVector GetShape() const
Definition: Tensor.h:1132
Tensor Contiguous() const
Definition: Tensor.cpp:758
Tensor To(Dtype dtype, bool copy=false) const
Definition: Tensor.cpp:725
A Class for nearest neighbor search.
Definition: NearestNeighborSearch.h:44
std::tuple< Tensor, Tensor, Tensor > HybridSearch(const Tensor &query_points, const double radius, const int max_knn) const
Definition: NearestNeighborSearch.cpp:149
bool FixedRadiusIndex(utility::optional< double > radius={})
Definition: NearestNeighborSearch.cpp:59
std::tuple< Tensor, Tensor, Tensor > FixedRadiusSearch(const Tensor &query_points, double radius, bool sort=true)
Definition: NearestNeighborSearch.cpp:117
bool KnnIndex()
Definition: NearestNeighborSearch.cpp:42
bool HybridIndex(utility::optional< double > radius={})
Definition: NearestNeighborSearch.cpp:79
std::pair< Tensor, Tensor > KnnSearch(const Tensor &query_points, int knn)
Definition: NearestNeighborSearch.cpp:98
Definition: GeometryIndexer.h:180
OPEN3D_HOST_DEVICE index_t GetShape(int i) const
Definition: GeometryIndexer.h:330
Definition: Optional.h:278
void Synchronize()
Definition: CUDAUtils.cpp:77
OPEN3D_HOST_DEVICE OPEN3D_FORCE_INLINE void cross_3x1(const scalar_t *A_3x1_input, const scalar_t *B_3x1_input, scalar_t *C_3x1_output)
Definition: Matrix.h:82
OPEN3D_DEVICE OPEN3D_FORCE_INLINE void solve_svd3x3(const scalar_t *A_3x3, const scalar_t *B_3x1, scalar_t *X_3x1)
Definition: SVD3x3.h:2190
OPEN3D_HOST_DEVICE OPEN3D_FORCE_INLINE scalar_t dot_3x1(const scalar_t *A_3x1_input, const scalar_t *B_3x1_input)
Definition: Matrix.h:96
const Dtype Int32
Definition: Dtype.cpp:65
void ParallelFor(const Device &device, int64_t n, const func_t &func)
Definition: ParallelFor.h:122
const Dtype Float32
Definition: Dtype.cpp:61
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 int32_t
Definition: K4aPlugin.cpp:414
void EstimateCovariancesUsingHybridSearchCPU(const core::Tensor &points, core::Tensor &covariances, const double &radius, const int64_t &max_nn)
Definition: PointCloudImpl.h:537
void EstimateCovariancesUsingRadiusSearchCPU(const core::Tensor &points, core::Tensor &covariances, const double &radius)
Definition: PointCloudImpl.h:587
OPEN3D_HOST_DEVICE void GetCoordinateSystemOnPlane(const scalar_t *query, scalar_t *u, scalar_t *v)
Definition: PointCloudImpl.h:310
void UnprojectCPU(const core::Tensor &depth, utility::optional< std::reference_wrapper< const core::Tensor >> image_colors, core::Tensor &points, utility::optional< std::reference_wrapper< core::Tensor >> colors, const core::Tensor &intrinsics, const core::Tensor &extrinsics, float depth_scale, float depth_max, int64_t stride)
Definition: PointCloudImpl.h:63
void EstimateNormalsFromCovariancesCPU(const core::Tensor &covariances, core::Tensor &normals, const bool has_normals)
Definition: PointCloudImpl.h:954
OPEN3D_HOST_DEVICE void ComputeEigenvector0(const scalar_t *A, const scalar_t eval0, scalar_t *eigen_vector0)
Definition: PointCloudImpl.h:685
void OrientNormalsTowardsCameraLocationCPU(const core::Tensor &points, core::Tensor &normals, const core::Tensor &camera)
Definition: PointCloudImpl.h:260
OPEN3D_HOST_DEVICE void EstimatePointWiseRobustNormalizedCovarianceKernel(const scalar_t *points_ptr, const int32_t *indices_ptr, const int32_t &indices_count, scalar_t *covariance_ptr)
Definition: PointCloudImpl.h:457
void GetPointMaskWithinAABBCPU(const core::Tensor &points, const core::Tensor &min_bound, const core::Tensor &max_bound, core::Tensor &mask)
Definition: PointCloudImpl.h:161
OPEN3D_HOST_DEVICE void Swap(scalar_t *x, scalar_t *y)
Definition: PointCloudImpl.h:337
OPEN3D_HOST_DEVICE bool IsBoundaryPoints(const scalar_t *angles, int counts, double angle_threshold)
Definition: PointCloudImpl.h:372
void ComputeBoundaryPointsCPU(const core::Tensor &points, const core::Tensor &normals, const core::Tensor &indices, const core::Tensor &counts, core::Tensor &mask, double angle_threshold)
Definition: PointCloudImpl.h:395
void EstimateColorGradientsUsingKNNSearchCPU(const core::Tensor &points, const core::Tensor &normals, const core::Tensor &colors, core::Tensor &color_gradient, const int64_t &max_nn)
Definition: PointCloudImpl.h:1160
void NormalizeNormalsCPU(core::Tensor &normals)
Definition: PointCloudImpl.h:195
OPEN3D_HOST_DEVICE void ComputeEigenvector1(const scalar_t *A, const scalar_t *evec0, const scalar_t eval1, scalar_t *eigen_vector1)
Definition: PointCloudImpl.h:734
OPEN3D_HOST_DEVICE void EstimatePointWiseColorGradientKernel(const scalar_t *points_ptr, const scalar_t *normals_ptr, const scalar_t *colors_ptr, const int32_t &idx_offset, const int32_t *indices_ptr, const int32_t &indices_count, scalar_t *color_gradients_ptr)
Definition: PointCloudImpl.h:1004
void EstimateColorGradientsUsingRadiusSearchCPU(const core::Tensor &points, const core::Tensor &normals, const core::Tensor &colors, core::Tensor &color_gradient, const double &radius)
Definition: PointCloudImpl.h:1215
void EstimateColorGradientsUsingHybridSearchCPU(const core::Tensor &points, const core::Tensor &normals, const core::Tensor &colors, core::Tensor &color_gradient, const double &radius, const int64_t &max_nn)
Definition: PointCloudImpl.h:1108
OPEN3D_HOST_DEVICE void EstimatePointWiseNormalsWithFastEigen3x3(const scalar_t *covariance_ptr, scalar_t *normals_ptr)
Definition: PointCloudImpl.h:814
OPEN3D_HOST_DEVICE void Heapify(scalar_t *arr, int n, int root)
Definition: PointCloudImpl.h:344
void OrientNormalsToAlignWithDirectionCPU(core::Tensor &normals, const core::Tensor &direction)
Definition: PointCloudImpl.h:226
void EstimateCovariancesUsingKNNSearchCPU(const core::Tensor &points, core::Tensor &covariances, const int64_t &max_nn)
Definition: PointCloudImpl.h:636
OPEN3D_HOST_DEVICE void HeapSort(scalar_t *arr, int n)
Definition: PointCloudImpl.h:362
TArrayIndexer< int64_t > NDArrayIndexer
Definition: GeometryIndexer.h:379
core::Tensor InverseTransformation(const core::Tensor &T)
TODO(wei): find a proper place for such functionalities.
Definition: Utility.h:96
Definition: PinholeCameraIntrinsic.cpp:35