Open3D (C++ API)  0.19.0
Loading...
Searching...
No Matches
PointCloudImpl.h
Go to the documentation of this file.
1// ----------------------------------------------------------------------------
2// - Open3D: www.open3d.org -
3// ----------------------------------------------------------------------------
4// Copyright (c) 2018-2024 www.open3d.org
5// SPDX-License-Identifier: MIT
6// ----------------------------------------------------------------------------
7
8#include <atomic>
9#include <vector>
10
11#include "open3d/core/CUDAUtils.h"
12#include "open3d/core/Dispatch.h"
13#include "open3d/core/Dtype.h"
14#include "open3d/core/MemoryManager.h"
15#include "open3d/core/ParallelFor.h"
16#include "open3d/core/SizeVector.h"
17#include "open3d/core/Tensor.h"
18#include "open3d/core/linalg/kernel/Matrix.h"
19#include "open3d/core/linalg/kernel/SVD3x3.h"
20#include "open3d/core/nns/NearestNeighborSearch.h"
21#include "open3d/t/geometry/Utility.h"
22#include "open3d/t/geometry/kernel/GeometryIndexer.h"
23#include "open3d/t/geometry/kernel/GeometryMacros.h"
24#include "open3d/t/geometry/kernel/PointCloud.h"
25#include "open3d/utility/Logging.h"
26
27namespace open3d {
28namespace t {
29namespace geometry {
30namespace kernel {
31namespace pointcloud {
32
33#ifndef __CUDACC__
34using std::abs;
35using std::max;
36using std::min;
37using std::sqrt;
38#endif
39
40#ifndef OPEN3D_SKIP_POINTCLOUD_MAIN
41
42#if defined(__CUDACC__)
43void UnprojectCUDA
44#elif defined(SYCL_LANGUAGE_VERSION)
45void UnprojectSYCL
46#else
47void UnprojectCPU
48#endif
49 (const core::Tensor& depth,
50 std::optional<std::reference_wrapper<const core::Tensor>> image_colors,
51 core::Tensor& points,
52 std::optional<std::reference_wrapper<core::Tensor>> colors,
53 const core::Tensor& intrinsics,
54 const core::Tensor& extrinsics,
55 float depth_scale,
56 float depth_max,
57 int64_t stride) {
58
59 const bool have_colors = image_colors.has_value();
60 NDArrayIndexer depth_indexer(depth, 2);
61 NDArrayIndexer image_colors_indexer;
62
63 core::Tensor pose = t::geometry::InverseTransformation(extrinsics);
64 TransformIndexer ti(intrinsics, pose, 1.0f);
65
66 // Output
67 int64_t rows_strided = depth_indexer.GetShape(0) / stride;
68 int64_t cols_strided = depth_indexer.GetShape(1) / stride;
69
70 points = core::Tensor({rows_strided * cols_strided, 3}, core::Float32,
71 depth.GetDevice());
72 NDArrayIndexer point_indexer(points, 1);
73 NDArrayIndexer colors_indexer;
74 if (have_colors) {
75 const auto& imcol = image_colors.value().get();
76 image_colors_indexer = NDArrayIndexer{imcol, 2};
77 colors.value().get() = core::Tensor({rows_strided * cols_strided, 3},
78 core::Float32, imcol.GetDevice());
79 colors_indexer = NDArrayIndexer(colors.value().get(), 1);
80 }
81
82 // Counter
83#if defined(__CUDACC__) || defined(SYCL_LANGUAGE_VERSION)
84 core::Tensor count(std::vector<int>{0}, {}, core::Int32, depth.GetDevice());
85 int* count_ptr = count.GetDataPtr<int>();
86#else
87 std::atomic<int> count_atomic(0);
88 std::atomic<int>* count_ptr = &count_atomic;
89#endif
90
91 int64_t n = rows_strided * cols_strided;
92
93 DISPATCH_DTYPE_TO_TEMPLATE(depth.GetDtype(), [&]() {
94 core::ParallelFor(
95 depth.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
96 int64_t y = (workload_idx / cols_strided) * stride;
97 int64_t x = (workload_idx % cols_strided) * stride;
98
99 float d = *depth_indexer.GetDataPtr<scalar_t>(x, y) /
100 depth_scale;
101 if (d > 0 && d < depth_max) {
102 // Use the OPEN3D_ATOMIC_ADD macro (defined outside
103 // this dispatch macro's argument list) instead of an
104 // inline #if/#elif chain: MSVC's preprocessor cannot
105 // parse directives nested inside an open macro-call
106 // argument list (DISPATCH_DTYPE_TO_TEMPLATE above).
107 int idx = OPEN3D_ATOMIC_ADD(count_ptr, 1);
108
109 float x_c = 0, y_c = 0, z_c = 0;
110 ti.Unproject(static_cast<float>(x),
111 static_cast<float>(y), d, &x_c, &y_c,
112 &z_c);
113
114 float* vertex = point_indexer.GetDataPtr<float>(idx);
115 ti.RigidTransform(x_c, y_c, z_c, vertex + 0, vertex + 1,
116 vertex + 2);
117 if (have_colors) {
118 float* pcd_pixel =
119 colors_indexer.GetDataPtr<float>(idx);
120 float* image_pixel =
121 image_colors_indexer.GetDataPtr<float>(x,
122 y);
123 *pcd_pixel = *image_pixel;
124 *(pcd_pixel + 1) = *(image_pixel + 1);
125 *(pcd_pixel + 2) = *(image_pixel + 2);
126 }
127 }
128 });
129 });
130#if defined(__CUDACC__) || defined(SYCL_LANGUAGE_VERSION)
131 int total_pts_count = count.Item<int>();
132#else
133 int total_pts_count = (*count_ptr).load();
134#endif
135
136#ifdef __CUDACC__
137 core::cuda::Synchronize();
138#endif
139 points = points.Slice(0, 0, total_pts_count);
140 if (have_colors) {
141 colors.value().get() =
142 colors.value().get().Slice(0, 0, total_pts_count);
143 }
144}
145
146#if defined(__CUDACC__)
147void GetPointMaskWithinAABBCUDA
148#elif defined(SYCL_LANGUAGE_VERSION)
149void GetPointMaskWithinAABBSYCL
150#else
151void GetPointMaskWithinAABBCPU
152#endif
153 (const core::Tensor& points,
154 const core::Tensor& min_bound,
155 const core::Tensor& max_bound,
156 core::Tensor& mask) {
157
158 DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(points.GetDtype(), [&]() {
159 const scalar_t* points_ptr = points.GetDataPtr<scalar_t>();
160 const int64_t n = points.GetLength();
161 const scalar_t* min_bound_ptr = min_bound.GetDataPtr<scalar_t>();
162 const scalar_t* max_bound_ptr = max_bound.GetDataPtr<scalar_t>();
163 bool* mask_ptr = mask.GetDataPtr<bool>();
164
165 core::ParallelFor(
166 points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
167 const scalar_t x = points_ptr[3 * workload_idx + 0];
168 const scalar_t y = points_ptr[3 * workload_idx + 1];
169 const scalar_t z = points_ptr[3 * workload_idx + 2];
170
171 if (x >= min_bound_ptr[0] && x <= max_bound_ptr[0] &&
172 y >= min_bound_ptr[1] && y <= max_bound_ptr[1] &&
173 z >= min_bound_ptr[2] && z <= max_bound_ptr[2]) {
174 mask_ptr[workload_idx] = true;
175 } else {
176 mask_ptr[workload_idx] = false;
177 }
178 });
179 });
180}
181
182#if defined(__CUDACC__)
183void GetPointMaskWithinOBBCUDA
184#elif defined(SYCL_LANGUAGE_VERSION)
185void GetPointMaskWithinOBBSYCL
186#else
187void GetPointMaskWithinOBBCPU
188#endif
189 (const core::Tensor& points,
190 const core::Tensor& center,
191 const core::Tensor& rotation,
192 const core::Tensor& extent,
193 core::Tensor& mask) {
194 const core::Tensor half_extent = extent.Div(2);
195 // Since we will extract 3 rotation axis from matrix and use it inside
196 // kernel, the transpose is needed.
197 const core::Tensor rotation_t = rotation.Transpose(0, 1).Contiguous();
198 const core::Tensor pd = points - center;
199 const int64_t n = points.GetLength();
200
201 DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(points.GetDtype(), [&]() {
202 const scalar_t* pd_ptr = pd.GetDataPtr<scalar_t>();
203 // const scalar_t* center_ptr = center.GetDataPtr<scalar_t>();
204 const scalar_t* rotation_ptr = rotation_t.GetDataPtr<scalar_t>();
205 const scalar_t* half_extent_ptr = half_extent.GetDataPtr<scalar_t>();
206 bool* mask_ptr = mask.GetDataPtr<bool>();
207
208 core::ParallelFor(points.GetDevice(), n,
209 [=] OPEN3D_DEVICE(int64_t workload_idx) {
210 int64_t idx = 3 * workload_idx;
211 if (abs(core::linalg::kernel::dot_3x1(
212 pd_ptr + idx, rotation_ptr)) <=
213 half_extent_ptr[0] &&
214 abs(core::linalg::kernel::dot_3x1(
215 pd_ptr + idx, rotation_ptr + 3)) <=
216 half_extent_ptr[1] &&
217 abs(core::linalg::kernel::dot_3x1(
218 pd_ptr + idx, rotation_ptr + 6)) <=
219 half_extent_ptr[2]) {
220 mask_ptr[workload_idx] = true;
221 } else {
222 mask_ptr[workload_idx] = false;
223 }
224 });
225 });
226}
227
228#if defined(__CUDACC__)
229void NormalizeNormalsCUDA
230#elif defined(SYCL_LANGUAGE_VERSION)
231void NormalizeNormalsSYCL
232#else
233void NormalizeNormalsCPU
234#endif
235 (core::Tensor& normals) {
236 const core::Dtype dtype = normals.GetDtype();
237 const int64_t n = normals.GetLength();
238
239 DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
240 scalar_t* ptr = normals.GetDataPtr<scalar_t>();
241
242 core::ParallelFor(normals.GetDevice(), n,
243 [=] OPEN3D_DEVICE(int64_t workload_idx) {
244 int64_t idx = 3 * workload_idx;
245 scalar_t x = ptr[idx];
246 scalar_t y = ptr[idx + 1];
247 scalar_t z = ptr[idx + 2];
248 scalar_t norm = sqrt(x * x + y * y + z * z);
249 if (norm > 0) {
250 x /= norm;
251 y /= norm;
252 z /= norm;
253 }
254 ptr[idx] = x;
255 ptr[idx + 1] = y;
256 ptr[idx + 2] = z;
257 });
258 });
259}
260
261#if defined(__CUDACC__)
262void OrientNormalsToAlignWithDirectionCUDA
263#elif defined(SYCL_LANGUAGE_VERSION)
264void OrientNormalsToAlignWithDirectionSYCL
265#else
266void OrientNormalsToAlignWithDirectionCPU
267#endif
268 (core::Tensor& normals, const core::Tensor& direction) {
269 const core::Dtype dtype = normals.GetDtype();
270 const int64_t n = normals.GetLength();
271
272 DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
273 scalar_t* ptr = normals.GetDataPtr<scalar_t>();
274 const scalar_t* direction_ptr = direction.GetDataPtr<scalar_t>();
275
276 core::ParallelFor(normals.GetDevice(), n,
277 [=] OPEN3D_DEVICE(int64_t workload_idx) {
278 int64_t idx = 3 * workload_idx;
279 scalar_t* normal = ptr + idx;
280 const scalar_t norm = sqrt(normal[0] * normal[0] +
281 normal[1] * normal[1] +
282 normal[2] * normal[2]);
283 if (norm == 0.0) {
284 normal[0] = direction_ptr[0];
285 normal[1] = direction_ptr[1];
286 normal[2] = direction_ptr[2];
287 } else if (core::linalg::kernel::dot_3x1(
288 normal, direction_ptr) < 0) {
289 normal[0] *= -1;
290 normal[1] *= -1;
291 normal[2] *= -1;
292 }
293 });
294 });
295}
296
297#if defined(__CUDACC__)
298void OrientNormalsTowardsCameraLocationCUDA
299#elif defined(SYCL_LANGUAGE_VERSION)
300void OrientNormalsTowardsCameraLocationSYCL
301#else
302void OrientNormalsTowardsCameraLocationCPU
303#endif
304 (const core::Tensor& points,
305 core::Tensor& normals,
306 const core::Tensor& camera) {
307 const core::Dtype dtype = points.GetDtype();
308 const int64_t n = normals.GetLength();
309
310 DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
311 scalar_t* normals_ptr = normals.GetDataPtr<scalar_t>();
312 const scalar_t* camera_ptr = camera.GetDataPtr<scalar_t>();
313 const scalar_t* points_ptr = points.GetDataPtr<scalar_t>();
314
315 core::ParallelFor(
316 normals.GetDevice(), n,
317 [=] OPEN3D_DEVICE(int64_t workload_idx) {
318 int64_t idx = 3 * workload_idx;
319 scalar_t* normal = normals_ptr + idx;
320 const scalar_t* point = points_ptr + idx;
321 const scalar_t reference[3] = {camera_ptr[0] - point[0],
322 camera_ptr[1] - point[1],
323 camera_ptr[2] - point[2]};
324 const scalar_t norm =
325 sqrt(normal[0] * normal[0] + normal[1] * normal[1] +
326 normal[2] * normal[2]);
327 if (norm == 0.0) {
328 normal[0] = reference[0];
329 normal[1] = reference[1];
330 normal[2] = reference[2];
331 const scalar_t norm_new = sqrt(normal[0] * normal[0] +
332 normal[1] * normal[1] +
333 normal[2] * normal[2]);
334 if (norm_new == 0.0) {
335 normal[0] = 0.0;
336 normal[1] = 0.0;
337 normal[2] = 1.0;
338 } else {
339 normal[0] /= norm_new;
340 normal[1] /= norm_new;
341 normal[2] /= norm_new;
342 }
343 } else if (core::linalg::kernel::dot_3x1(normal,
344 reference) < 0) {
345 normal[0] *= -1;
346 normal[1] *= -1;
347 normal[2] *= -1;
348 }
349 });
350 });
351}
352
353#endif // OPEN3D_SKIP_POINTCLOUD_MAIN
354
355template <typename scalar_t>
356OPEN3D_HOST_DEVICE void GetCoordinateSystemOnPlane(const scalar_t* query,
357 scalar_t* u,
358 scalar_t* v) {
359 // Unless the x and y coords are both close to zero, we can simply take
360 // ( -y, x, 0 ) and normalize it. If both x and y are close to zero,
361 // then the vector is close to the z-axis, so it's far from colinear to
362 // the x-axis for instance. So we take the crossed product with (1,0,0)
363 // and normalize it.
364 if (!(abs(query[0] - query[2]) < 1e-6) ||
365 !(abs(query[1] - query[2]) < 1e-6)) {
366 const scalar_t norm2_inv =
367 1.0 / sqrt(query[0] * query[0] + query[1] * query[1]);
368 v[0] = -1 * query[1] * norm2_inv;
369 v[1] = query[0] * norm2_inv;
370 v[2] = 0;
371 } else {
372 const scalar_t norm2_inv =
373 1.0 / sqrt(query[1] * query[1] + query[2] * query[2]);
374 v[0] = 0;
375 v[1] = -1 * query[2] * norm2_inv;
376 v[2] = query[1] * norm2_inv;
377 }
378
379 core::linalg::kernel::cross_3x1(query, v, u);
380}
381
382template <typename scalar_t>
383inline OPEN3D_HOST_DEVICE void Swap(scalar_t* x, scalar_t* y) {
384 scalar_t tmp = *x;
385 *x = *y;
386 *y = tmp;
387}
388
389template <typename scalar_t>
390inline OPEN3D_HOST_DEVICE void Heapify(scalar_t* arr, int n, int root) {
391 int largest = root;
392 while (true) {
393 int l = 2 * largest + 1;
394 int r = 2 * largest + 2;
395 int next_largest = largest;
396
397 if (l < n && arr[l] > arr[next_largest]) {
398 next_largest = l;
399 }
400 if (r < n && arr[r] > arr[next_largest]) {
401 next_largest = r;
402 }
403 if (next_largest != largest) {
404 Swap<scalar_t>(&arr[largest], &arr[next_largest]);
405 largest = next_largest;
406 } else {
407 break;
408 }
409 }
410}
411
412template <typename scalar_t>
413OPEN3D_HOST_DEVICE void HeapSort(scalar_t* arr, int n) {
414 for (int i = n / 2 - 1; i >= 0; i--) Heapify(arr, n, i);
415
416 for (int i = n - 1; i > 0; i--) {
417 Swap<scalar_t>(&arr[0], &arr[i]);
418 Heapify<scalar_t>(arr, i, 0);
419 }
420}
421
422template <typename scalar_t>
423OPEN3D_HOST_DEVICE bool IsBoundaryPoints(const scalar_t* angles,
424 int counts,
425 double angle_threshold) {
426 scalar_t diff;
427 scalar_t max_diff = 0;
428 // Compute the maximal angle difference between two consecutive angles.
429 for (int i = 0; i < counts - 1; i++) {
430 diff = angles[i + 1] - angles[i];
431 max_diff = max(max_diff, diff);
432 }
433
434 // Get the angle difference between the last and the first.
435 diff = 2 * M_PI - angles[counts - 1] + angles[0];
436 max_diff = max(max_diff, diff);
437
438 return max_diff > angle_threshold * M_PI / 180.0 ? true : false;
439}
440
441#ifndef OPEN3D_SKIP_POINTCLOUD_MAIN
442
443#if defined(__CUDACC__)
444void ComputeBoundaryPointsCUDA
445#elif defined(SYCL_LANGUAGE_VERSION)
446void ComputeBoundaryPointsSYCL
447#else
448void ComputeBoundaryPointsCPU
449#endif
450 (const core::Tensor& points,
451 const core::Tensor& normals,
452 const core::Tensor& indices,
453 const core::Tensor& counts,
454 core::Tensor& mask,
455 double angle_threshold) {
456 const int nn_size = indices.GetShape()[1];
457
458 DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(points.GetDtype(), [&]() {
459 const scalar_t* points_ptr = points.GetDataPtr<scalar_t>();
460 const scalar_t* normals_ptr = normals.GetDataPtr<scalar_t>();
461 const int64_t n = points.GetLength();
462 const int32_t* indices_ptr = indices.GetDataPtr<int32_t>();
463 const int32_t* counts_ptr = counts.GetDataPtr<int32_t>();
464 bool* mask_ptr = mask.GetDataPtr<bool>();
465
466 core::Tensor angles = core::Tensor::Full(
467 indices.GetShape(), -10, points.GetDtype(), points.GetDevice());
468 scalar_t* angles_ptr = angles.GetDataPtr<scalar_t>();
469
470 core::ParallelFor(
471 points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
472 scalar_t u[3], v[3];
473 GetCoordinateSystemOnPlane(normals_ptr + 3 * workload_idx,
474 u, v);
475
476 // Ignore the point itself.
477 int indices_size = counts_ptr[workload_idx] - 1;
478 if (indices_size > 0) {
479 const scalar_t* query = points_ptr + 3 * workload_idx;
480 for (int i = 1; i < indices_size + 1; i++) {
481 const int idx = workload_idx * nn_size + i;
482
483 const scalar_t* point_ref =
484 points_ptr + 3 * indices_ptr[idx];
485 const scalar_t delta[3] = {point_ref[0] - query[0],
486 point_ref[1] - query[1],
487 point_ref[2] - query[2]};
488 const scalar_t angle = atan2(
489 core::linalg::kernel::dot_3x1(v, delta),
490 core::linalg::kernel::dot_3x1(u, delta));
491
492 angles_ptr[idx] = angle;
493 }
494
495 // Sort the angles in ascending order.
496 HeapSort<scalar_t>(
497 angles_ptr + workload_idx * nn_size + 1,
498 indices_size);
499
500 mask_ptr[workload_idx] = IsBoundaryPoints<scalar_t>(
501 angles_ptr + workload_idx * nn_size + 1,
502 indices_size, angle_threshold);
503 }
504 });
505 });
506}
507
508#endif // OPEN3D_SKIP_POINTCLOUD_MAIN
509
510// This is a `two-pass` estimate method for covariance which is numerically more
511// robust than the `textbook` method generally used for covariance computation.
512template <typename scalar_t>
513OPEN3D_HOST_DEVICE void EstimatePointWiseRobustNormalizedCovarianceKernel(
514 const scalar_t* points_ptr,
515 const int32_t* indices_ptr,
516 const int32_t& indices_count,
517 scalar_t* covariance_ptr) {
518 if (indices_count < 3) {
519 covariance_ptr[0] = 1.0;
520 covariance_ptr[1] = 0.0;
521 covariance_ptr[2] = 0.0;
522 covariance_ptr[3] = 0.0;
523 covariance_ptr[4] = 1.0;
524 covariance_ptr[5] = 0.0;
525 covariance_ptr[6] = 0.0;
526 covariance_ptr[7] = 0.0;
527 covariance_ptr[8] = 1.0;
528 return;
529 }
530
531 double centroid[3] = {0};
532 for (int32_t i = 0; i < indices_count; ++i) {
533 int32_t idx = 3 * indices_ptr[i];
534 centroid[0] += points_ptr[idx];
535 centroid[1] += points_ptr[idx + 1];
536 centroid[2] += points_ptr[idx + 2];
537 }
538
539 centroid[0] /= indices_count;
540 centroid[1] /= indices_count;
541 centroid[2] /= indices_count;
542
543 // cumulants must always be Float64 to ensure precision.
544 double cumulants[6] = {0};
545 for (int32_t i = 0; i < indices_count; ++i) {
546 int32_t idx = 3 * indices_ptr[i];
547 const double x = static_cast<double>(points_ptr[idx]) - centroid[0];
548 const double y = static_cast<double>(points_ptr[idx + 1]) - centroid[1];
549 const double z = static_cast<double>(points_ptr[idx + 2]) - centroid[2];
550
551 cumulants[0] += x * x;
552 cumulants[1] += y * y;
553 cumulants[2] += z * z;
554
555 cumulants[3] += x * y;
556 cumulants[4] += x * z;
557 cumulants[5] += y * z;
558 }
559
560 // Using Bessel's correction (dividing by (n - 1) instead of n).
561 // Refer:
562 // https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance
563 const double normalization_factor = static_cast<double>(indices_count - 1);
564 for (int i = 0; i < 6; ++i) {
565 cumulants[i] /= normalization_factor;
566 }
567
568 // Covariances(0, 0)
569 covariance_ptr[0] = static_cast<scalar_t>(cumulants[0]);
570 // Covariances(1, 1)
571 covariance_ptr[4] = static_cast<scalar_t>(cumulants[1]);
572 // Covariances(2, 2)
573 covariance_ptr[8] = static_cast<scalar_t>(cumulants[2]);
574
575 // Covariances(0, 1) = Covariances(1, 0)
576 covariance_ptr[1] = static_cast<scalar_t>(cumulants[3]);
577 covariance_ptr[3] = covariance_ptr[1];
578
579 // Covariances(0, 2) = Covariances(2, 0)
580 covariance_ptr[2] = static_cast<scalar_t>(cumulants[4]);
581 covariance_ptr[6] = covariance_ptr[2];
582
583 // Covariances(1, 2) = Covariances(2, 1)
584 covariance_ptr[5] = static_cast<scalar_t>(cumulants[5]);
585 covariance_ptr[7] = covariance_ptr[5];
586}
587
588#if defined(__CUDACC__)
589void EstimateCovariancesUsingHybridSearchCUDA
590#elif defined(SYCL_LANGUAGE_VERSION)
591void EstimateCovariancesUsingHybridSearchSYCL
592#else
593void EstimateCovariancesUsingHybridSearchCPU
594#endif
595 (const core::Tensor& points,
596 core::Tensor& covariances,
597 const double& radius,
598 const int64_t& max_nn) {
599 core::Dtype dtype = points.GetDtype();
600 int64_t n = points.GetLength();
601
602 core::nns::NearestNeighborSearch tree(points, core::Int32);
603 bool check = tree.HybridIndex(radius);
604 if (!check) {
605 utility::LogError("Building FixedRadiusIndex failed.");
606 }
607
608 core::Tensor indices, distance, counts;
609 std::tie(indices, distance, counts) =
610 tree.HybridSearch(points, radius, max_nn);
611
612 DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
613 const scalar_t* points_ptr = points.GetDataPtr<scalar_t>();
614 int32_t* neighbour_indices_ptr = indices.GetDataPtr<int32_t>();
615 int32_t* neighbour_counts_ptr = counts.GetDataPtr<int32_t>();
616 scalar_t* covariances_ptr = covariances.GetDataPtr<scalar_t>();
617
618 core::ParallelFor(
619 points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
620 // NNS [Hybrid Search].
621 const int32_t neighbour_offset = max_nn * workload_idx;
622 // Count of valid correspondences per point.
623 const int32_t neighbour_count =
624 neighbour_counts_ptr[workload_idx];
625 // Covariance is of shape {3, 3}, so it has an
626 // offset factor of 9 x workload_idx.
627 const int32_t covariances_offset = 9 * workload_idx;
628
629 EstimatePointWiseRobustNormalizedCovarianceKernel(
630 points_ptr,
631 neighbour_indices_ptr + neighbour_offset,
632 neighbour_count,
633 covariances_ptr + covariances_offset);
634 });
635 });
636
637 core::cuda::Synchronize(points.GetDevice());
638}
639
640#if defined(__CUDACC__)
641void EstimateCovariancesUsingRadiusSearchCUDA
642#elif defined(SYCL_LANGUAGE_VERSION)
643void EstimateCovariancesUsingRadiusSearchSYCL
644#else
645void EstimateCovariancesUsingRadiusSearchCPU
646#endif
647 (const core::Tensor& points,
648 core::Tensor& covariances,
649 const double& radius) {
650 core::Dtype dtype = points.GetDtype();
651 int64_t n = points.GetLength();
652
653 core::nns::NearestNeighborSearch tree(points, core::Int32);
654 bool check = tree.FixedRadiusIndex(radius);
655 if (!check) {
656 utility::LogError("Building Radius-Index failed.");
657 }
658
659 core::Tensor indices, distance, counts;
660 std::tie(indices, distance, counts) =
661 tree.FixedRadiusSearch(points, radius);
662
663 DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
664 const scalar_t* points_ptr = points.GetDataPtr<scalar_t>();
665 const int32_t* neighbour_indices_ptr = indices.GetDataPtr<int32_t>();
666 const int32_t* neighbour_counts_ptr = counts.GetDataPtr<int32_t>();
667 scalar_t* covariances_ptr = covariances.GetDataPtr<scalar_t>();
668
669 core::ParallelFor(
670 points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
671 const int32_t neighbour_offset =
672 neighbour_counts_ptr[workload_idx];
673 const int32_t neighbour_count =
674 (neighbour_counts_ptr[workload_idx + 1] -
675 neighbour_counts_ptr[workload_idx]);
676 // Covariance is of shape {3, 3}, so it has an offset
677 // factor of 9 x workload_idx.
678 const int32_t covariances_offset = 9 * workload_idx;
679
680 EstimatePointWiseRobustNormalizedCovarianceKernel(
681 points_ptr,
682 neighbour_indices_ptr + neighbour_offset,
683 neighbour_count,
684 covariances_ptr + covariances_offset);
685 });
686 });
687
688 core::cuda::Synchronize(points.GetDevice());
689}
690
691#if defined(__CUDACC__)
692void EstimateCovariancesUsingKNNSearchCUDA
693#elif defined(SYCL_LANGUAGE_VERSION)
694void EstimateCovariancesUsingKNNSearchSYCL
695#else
696void EstimateCovariancesUsingKNNSearchCPU
697#endif
698 (const core::Tensor& points,
699 core::Tensor& covariances,
700 const int64_t& max_nn) {
701 core::Dtype dtype = points.GetDtype();
702 int64_t n = points.GetLength();
703
704 core::nns::NearestNeighborSearch tree(points, core::Int32);
705 bool check = tree.KnnIndex();
706 if (!check) {
707 utility::LogError("Building KNN-Index failed.");
708 }
709
710 core::Tensor indices, distance;
711 std::tie(indices, distance) = tree.KnnSearch(points, max_nn);
712
713 indices = indices.Contiguous();
714 int32_t nn_count = static_cast<int32_t>(indices.GetShape()[1]);
715
716 if (nn_count < 3) {
717 utility::LogError(
718 "Not enough neighbors to compute Covariances / Normals. "
719 "Try "
720 "increasing the max_nn parameter.");
721 }
722
723 DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
724 auto points_ptr = points.GetDataPtr<scalar_t>();
725 auto neighbour_indices_ptr = indices.GetDataPtr<int32_t>();
726 auto covariances_ptr = covariances.GetDataPtr<scalar_t>();
727
728 core::ParallelFor(
729 points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
730 // NNS [KNN Search].
731 const int32_t neighbour_offset = nn_count * workload_idx;
732 // Covariance is of shape {3, 3}, so it has an offset
733 // factor of 9 x workload_idx.
734 const int32_t covariances_offset = 9 * workload_idx;
735
736 EstimatePointWiseRobustNormalizedCovarianceKernel(
737 points_ptr,
738 neighbour_indices_ptr + neighbour_offset, nn_count,
739 covariances_ptr + covariances_offset);
740 });
741 });
742
743 core::cuda::Synchronize(points.GetDevice());
744}
745
746template <typename scalar_t>
747OPEN3D_HOST_DEVICE void ComputeEigenvector0(const scalar_t* A,
748 const scalar_t eval0,
749 scalar_t* eigen_vector0) {
750 scalar_t row0[3] = {A[0] - eval0, A[1], A[2]};
751 scalar_t row1[3] = {A[1], A[4] - eval0, A[5]};
752 scalar_t row2[3] = {A[2], A[5], A[8] - eval0};
753
754 scalar_t r0xr1[3], r0xr2[3], r1xr2[3];
755
756 core::linalg::kernel::cross_3x1(row0, row1, r0xr1);
757 core::linalg::kernel::cross_3x1(row0, row2, r0xr2);
758 core::linalg::kernel::cross_3x1(row1, row2, r1xr2);
759
760 scalar_t d0 = core::linalg::kernel::dot_3x1(r0xr1, r0xr1);
761 scalar_t d1 = core::linalg::kernel::dot_3x1(r0xr2, r0xr2);
762 scalar_t d2 = core::linalg::kernel::dot_3x1(r1xr2, r1xr2);
763
764 scalar_t dmax = d0;
765 int imax = 0;
766 if (d1 > dmax) {
767 dmax = d1;
768 imax = 1;
769 }
770 if (d2 > dmax) {
771 imax = 2;
772 }
773
774 if (imax == 0) {
775 scalar_t sqrt_d = sqrt(d0);
776 eigen_vector0[0] = r0xr1[0] / sqrt_d;
777 eigen_vector0[1] = r0xr1[1] / sqrt_d;
778 eigen_vector0[2] = r0xr1[2] / sqrt_d;
779 return;
780 } else if (imax == 1) {
781 scalar_t sqrt_d = sqrt(d1);
782 eigen_vector0[0] = r0xr2[0] / sqrt_d;
783 eigen_vector0[1] = r0xr2[1] / sqrt_d;
784 eigen_vector0[2] = r0xr2[2] / sqrt_d;
785 return;
786 } else {
787 scalar_t sqrt_d = sqrt(d2);
788 eigen_vector0[0] = r1xr2[0] / sqrt_d;
789 eigen_vector0[1] = r1xr2[1] / sqrt_d;
790 eigen_vector0[2] = r1xr2[2] / sqrt_d;
791 return;
792 }
793}
794
795template <typename scalar_t>
796OPEN3D_HOST_DEVICE void ComputeEigenvector1(const scalar_t* A,
797 const scalar_t* evec0,
798 const scalar_t eval1,
799 scalar_t* eigen_vector1) {
800 scalar_t U[3];
801 if (abs(evec0[0]) > abs(evec0[1])) {
802 scalar_t inv_length =
803 1.0 / sqrt(evec0[0] * evec0[0] + evec0[2] * evec0[2]);
804 U[0] = -evec0[2] * inv_length;
805 U[1] = 0.0;
806 U[2] = evec0[0] * inv_length;
807 } else {
808 scalar_t inv_length =
809 1.0 / sqrt(evec0[1] * evec0[1] + evec0[2] * evec0[2]);
810 U[0] = 0.0;
811 U[1] = evec0[2] * inv_length;
812 U[2] = -evec0[1] * inv_length;
813 }
814 scalar_t V[3], AU[3], AV[3];
815 core::linalg::kernel::cross_3x1(evec0, U, V);
816 core::linalg::kernel::matmul3x3_3x1(A, U, AU);
817 core::linalg::kernel::matmul3x3_3x1(A, V, AV);
818
819 scalar_t m00 = core::linalg::kernel::dot_3x1(U, AU) - eval1;
820 scalar_t m01 = core::linalg::kernel::dot_3x1(U, AV);
821 scalar_t m11 = core::linalg::kernel::dot_3x1(V, AV) - eval1;
822
823 scalar_t absM00 = abs(m00);
824 scalar_t absM01 = abs(m01);
825 scalar_t absM11 = abs(m11);
826 scalar_t max_abs_comp;
827
828 if (absM00 >= absM11) {
829 max_abs_comp = max(absM00, absM01);
830 if (max_abs_comp > 0) {
831 if (absM00 >= absM01) {
832 m01 /= m00;
833 m00 = 1 / sqrt(1 + m01 * m01);
834 m01 *= m00;
835 } else {
836 m00 /= m01;
837 m01 = 1 / sqrt(1 + m00 * m00);
838 m00 *= m01;
839 }
840 eigen_vector1[0] = m01 * U[0] - m00 * V[0];
841 eigen_vector1[1] = m01 * U[1] - m00 * V[1];
842 eigen_vector1[2] = m01 * U[2] - m00 * V[2];
843 return;
844 } else {
845 eigen_vector1[0] = U[0];
846 eigen_vector1[1] = U[1];
847 eigen_vector1[2] = U[2];
848 return;
849 }
850 } else {
851 max_abs_comp = max(absM11, absM01);
852 if (max_abs_comp > 0) {
853 if (absM11 >= absM01) {
854 m01 /= m11;
855 m11 = 1 / sqrt(1 + m01 * m01);
856 m01 *= m11;
857 } else {
858 m11 /= m01;
859 m01 = 1 / sqrt(1 + m11 * m11);
860 m11 *= m01;
861 }
862 eigen_vector1[0] = m11 * U[0] - m01 * V[0];
863 eigen_vector1[1] = m11 * U[1] - m01 * V[1];
864 eigen_vector1[2] = m11 * U[2] - m01 * V[2];
865 return;
866 } else {
867 eigen_vector1[0] = U[0];
868 eigen_vector1[1] = U[1];
869 eigen_vector1[2] = U[2];
870 return;
871 }
872 }
873}
874
875template <typename scalar_t>
876OPEN3D_HOST_DEVICE void EstimatePointWiseNormalsWithFastEigen3x3(
877 const scalar_t* covariance_ptr, scalar_t* normals_ptr) {
878 // Based on:
879 // https://www.geometrictools.com/Documentation/RobustEigenSymmetric3x3.pdf
880 // which handles edge cases like points on a plane.
881 scalar_t max_coeff = covariance_ptr[0];
882
883 for (int i = 1; i < 9; ++i) {
884 if (max_coeff < covariance_ptr[i]) {
885 max_coeff = covariance_ptr[i];
886 }
887 }
888
889 if (max_coeff == 0) {
890 normals_ptr[0] = 0.0;
891 normals_ptr[1] = 0.0;
892 normals_ptr[2] = 0.0;
893 return;
894 }
895
896 scalar_t A[9] = {0};
897
898 for (int i = 0; i < 9; ++i) {
899 A[i] = covariance_ptr[i] / max_coeff;
900 }
901
902 scalar_t norm = A[1] * A[1] + A[2] * A[2] + A[5] * A[5];
903
904 if (norm > 0) {
905 scalar_t eval[3];
906 scalar_t evec0[3];
907 scalar_t evec1[3];
908 scalar_t evec2[3];
909
910 scalar_t q = (A[0] + A[4] + A[8]) / 3.0;
911
912 scalar_t b00 = A[0] - q;
913 scalar_t b11 = A[4] - q;
914 scalar_t b22 = A[8] - q;
915
916 scalar_t p =
917 sqrt((b00 * b00 + b11 * b11 + b22 * b22 + norm * 2.0) / 6.0);
918
919 scalar_t c00 = b11 * b22 - A[5] * A[5];
920 scalar_t c01 = A[1] * b22 - A[5] * A[2];
921 scalar_t c02 = A[1] * A[5] - b11 * A[2];
922 scalar_t det = (b00 * c00 - A[1] * c01 + A[2] * c02) / (p * p * p);
923
924 scalar_t half_det = det * 0.5;
925 half_det = min(max(half_det, static_cast<scalar_t>(-1.0)),
926 static_cast<scalar_t>(1.0));
927
928 scalar_t angle = acos(half_det) / 3.0;
929 const scalar_t two_thrids_pi = 2.09439510239319549;
930
931 scalar_t beta2 = cos(angle) * 2.0;
932 scalar_t beta0 = cos(angle + two_thrids_pi) * 2.0;
933 scalar_t beta1 = -(beta0 + beta2);
934
935 eval[0] = q + p * beta0;
936 eval[1] = q + p * beta1;
937 eval[2] = q + p * beta2;
938
939 if (half_det >= 0) {
940 ComputeEigenvector0<scalar_t>(A, eval[2], evec2);
941
942 if (eval[2] < eval[0] && eval[2] < eval[1]) {
943 normals_ptr[0] = evec2[0];
944 normals_ptr[1] = evec2[1];
945 normals_ptr[2] = evec2[2];
946
947 return;
948 }
949
950 ComputeEigenvector1<scalar_t>(A, evec2, eval[1], evec1);
951
952 if (eval[1] < eval[0] && eval[1] < eval[2]) {
953 normals_ptr[0] = evec1[0];
954 normals_ptr[1] = evec1[1];
955 normals_ptr[2] = evec1[2];
956
957 return;
958 }
959
960 normals_ptr[0] = evec1[1] * evec2[2] - evec1[2] * evec2[1];
961 normals_ptr[1] = evec1[2] * evec2[0] - evec1[0] * evec2[2];
962 normals_ptr[2] = evec1[0] * evec2[1] - evec1[1] * evec2[0];
963
964 return;
965 } else {
966 ComputeEigenvector0<scalar_t>(A, eval[0], evec0);
967
968 if (eval[0] < eval[1] && eval[0] < eval[2]) {
969 normals_ptr[0] = evec0[0];
970 normals_ptr[1] = evec0[1];
971 normals_ptr[2] = evec0[2];
972 return;
973 }
974
975 ComputeEigenvector1<scalar_t>(A, evec0, eval[1], evec1);
976
977 if (eval[1] < eval[0] && eval[1] < eval[2]) {
978 normals_ptr[0] = evec1[0];
979 normals_ptr[1] = evec1[1];
980 normals_ptr[2] = evec1[2];
981 return;
982 }
983
984 normals_ptr[0] = evec0[1] * evec1[2] - evec0[2] * evec1[1];
985 normals_ptr[1] = evec0[2] * evec1[0] - evec0[0] * evec1[2];
986 normals_ptr[2] = evec0[0] * evec1[1] - evec0[1] * evec1[0];
987 return;
988 }
989 } else {
990 if (covariance_ptr[0] < covariance_ptr[4] &&
991 covariance_ptr[0] < covariance_ptr[8]) {
992 normals_ptr[0] = 1.0;
993 normals_ptr[1] = 0.0;
994 normals_ptr[2] = 0.0;
995 return;
996 } else if (covariance_ptr[4] < covariance_ptr[0] &&
997 covariance_ptr[4] < covariance_ptr[8]) {
998 normals_ptr[0] = 0.0;
999 normals_ptr[1] = 1.0;
1000 normals_ptr[2] = 0.0;
1001 return;
1002 } else {
1003 normals_ptr[0] = 0.0;
1004 normals_ptr[1] = 0.0;
1005 normals_ptr[2] = 1.0;
1006 return;
1007 }
1008 }
1009}
1010
1011#if defined(__CUDACC__)
1012void EstimateNormalsFromCovariancesCUDA
1013#elif defined(SYCL_LANGUAGE_VERSION)
1014void EstimateNormalsFromCovariancesSYCL
1015#else
1016void EstimateNormalsFromCovariancesCPU
1017#endif
1018 (const core::Tensor& covariances,
1019 core::Tensor& normals,
1020 const bool has_normals) {
1021 core::Dtype dtype = covariances.GetDtype();
1022 int64_t n = covariances.GetLength();
1023
1024 DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
1025 const scalar_t* covariances_ptr = covariances.GetDataPtr<scalar_t>();
1026 scalar_t* normals_ptr = normals.GetDataPtr<scalar_t>();
1027
1028 core::ParallelFor(
1029 covariances.GetDevice(), n,
1030 [=] OPEN3D_DEVICE(int64_t workload_idx) {
1031 int32_t covariances_offset = 9 * workload_idx;
1032 int32_t normals_offset = 3 * workload_idx;
1033 scalar_t normals_output[3] = {0};
1034 EstimatePointWiseNormalsWithFastEigen3x3<scalar_t>(
1035 covariances_ptr + covariances_offset,
1036 normals_output);
1037
1038 if ((normals_output[0] * normals_output[0] +
1039 normals_output[1] * normals_output[1] +
1040 normals_output[2] * normals_output[2]) == 0.0 &&
1041 !has_normals) {
1042 normals_output[0] = 0.0;
1043 normals_output[1] = 0.0;
1044 normals_output[2] = 1.0;
1045 }
1046 if (has_normals) {
1047 if ((normals_ptr[normals_offset] * normals_output[0] +
1048 normals_ptr[normals_offset + 1] *
1049 normals_output[1] +
1050 normals_ptr[normals_offset + 2] *
1051 normals_output[2]) < 0.0) {
1052 normals_output[0] *= -1;
1053 normals_output[1] *= -1;
1054 normals_output[2] *= -1;
1055 }
1056 }
1057
1058 normals_ptr[normals_offset] = normals_output[0];
1059 normals_ptr[normals_offset + 1] = normals_output[1];
1060 normals_ptr[normals_offset + 2] = normals_output[2];
1061 });
1062 });
1063
1064 core::cuda::Synchronize(covariances.GetDevice());
1065}
1066
1067template <typename scalar_t>
1068OPEN3D_HOST_DEVICE void EstimatePointWiseColorGradientKernel(
1069 const scalar_t* points_ptr,
1070 const scalar_t* normals_ptr,
1071 const scalar_t* colors_ptr,
1072 const int32_t& idx_offset,
1073 const int32_t* indices_ptr,
1074 const int32_t& indices_count,
1075 scalar_t* color_gradients_ptr) {
1076 if (indices_count < 4) {
1077 color_gradients_ptr[idx_offset] = 0;
1078 color_gradients_ptr[idx_offset + 1] = 0;
1079 color_gradients_ptr[idx_offset + 2] = 0;
1080 } else {
1081 scalar_t vt[3] = {points_ptr[idx_offset], points_ptr[idx_offset + 1],
1082 points_ptr[idx_offset + 2]};
1083
1084 scalar_t nt[3] = {normals_ptr[idx_offset], normals_ptr[idx_offset + 1],
1085 normals_ptr[idx_offset + 2]};
1086
1087 scalar_t it = (colors_ptr[idx_offset] + colors_ptr[idx_offset + 1] +
1088 colors_ptr[idx_offset + 2]) /
1089 3.0;
1090
1091 scalar_t AtA[9] = {0};
1092 scalar_t Atb[3] = {0};
1093
1094 // approximate image gradient of vt's tangential plane
1095 // projection (p') of a point p on a plane defined by
1096 // normal n, where o is the closest point to p on the
1097 // plane, is given by:
1098 // p' = p - [(p - o).dot(n)] * n p'
1099 // => p - [(p.dot(n) - s)] * n [where s = o.dot(n)]
1100
1101 // Computing the scalar s.
1102 scalar_t s = vt[0] * nt[0] + vt[1] * nt[1] + vt[2] * nt[2];
1103
1104 int i = 1;
1105 for (; i < indices_count; i++) {
1106 int64_t neighbour_idx_offset = 3 * indices_ptr[i];
1107
1108 if (neighbour_idx_offset == -1) {
1109 break;
1110 }
1111
1112 scalar_t vt_adj[3] = {points_ptr[neighbour_idx_offset],
1113 points_ptr[neighbour_idx_offset + 1],
1114 points_ptr[neighbour_idx_offset + 2]};
1115
1116 // p' = p - d * n [where d = p.dot(n) - s]
1117 // Computing the scalar d.
1118 scalar_t d = vt_adj[0] * nt[0] + vt_adj[1] * nt[1] +
1119 vt_adj[2] * nt[2] - s;
1120
1121 // Computing the p' (projection of the point).
1122 scalar_t vt_proj[3] = {vt_adj[0] - d * nt[0], vt_adj[1] - d * nt[1],
1123 vt_adj[2] - d * nt[2]};
1124
1125 scalar_t it_adj = (colors_ptr[neighbour_idx_offset + 0] +
1126 colors_ptr[neighbour_idx_offset + 1] +
1127 colors_ptr[neighbour_idx_offset + 2]) /
1128 3.0;
1129
1130 scalar_t A[3] = {vt_proj[0] - vt[0], vt_proj[1] - vt[1],
1131 vt_proj[2] - vt[2]};
1132
1133 AtA[0] += A[0] * A[0];
1134 AtA[1] += A[1] * A[0];
1135 AtA[2] += A[2] * A[0];
1136 AtA[4] += A[1] * A[1];
1137 AtA[5] += A[2] * A[1];
1138 AtA[8] += A[2] * A[2];
1139
1140 scalar_t b = it_adj - it;
1141
1142 Atb[0] += A[0] * b;
1143 Atb[1] += A[1] * b;
1144 Atb[2] += A[2] * b;
1145 }
1146
1147 // Orthogonal constraint.
1148 scalar_t A[3] = {(i - 1) * nt[0], (i - 1) * nt[1], (i - 1) * nt[2]};
1149
1150 AtA[0] += A[0] * A[0];
1151 AtA[1] += A[0] * A[1];
1152 AtA[2] += A[0] * A[2];
1153 AtA[4] += A[1] * A[1];
1154 AtA[5] += A[1] * A[2];
1155 AtA[8] += A[2] * A[2];
1156
1157 // Symmetry.
1158 AtA[3] = AtA[1];
1159 AtA[6] = AtA[2];
1160 AtA[7] = AtA[5];
1161
1162 core::linalg::kernel::solve_svd3x3(AtA, Atb,
1163 color_gradients_ptr + idx_offset);
1164 }
1165}
1166
1167#ifndef OPEN3D_SKIP_POINTCLOUD_MAIN
1168
1169#if defined(__CUDACC__)
1170void EstimateColorGradientsUsingHybridSearchCUDA
1171#elif defined(SYCL_LANGUAGE_VERSION)
1172void EstimateColorGradientsUsingHybridSearchSYCL
1173#else
1174void EstimateColorGradientsUsingHybridSearchCPU
1175#endif
1176 (const core::Tensor& points,
1177 const core::Tensor& normals,
1178 const core::Tensor& colors,
1179 core::Tensor& color_gradients,
1180 const double& radius,
1181 const int64_t& max_nn) {
1182 core::Dtype dtype = points.GetDtype();
1183 int64_t n = points.GetLength();
1184
1185 core::nns::NearestNeighborSearch tree(points, core::Int32);
1186
1187 bool check = tree.HybridIndex(radius);
1188 if (!check) {
1189 utility::LogError("NearestNeighborSearch::HybridIndex is not set.");
1190 }
1191
1192 core::Tensor indices, distance, counts;
1193 std::tie(indices, distance, counts) =
1194 tree.HybridSearch(points, radius, max_nn);
1195
1196 DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
1197 auto points_ptr = points.GetDataPtr<scalar_t>();
1198 auto normals_ptr = normals.GetDataPtr<scalar_t>();
1199 auto colors_ptr = colors.GetDataPtr<scalar_t>();
1200 auto neighbour_indices_ptr = indices.GetDataPtr<int32_t>();
1201 auto neighbour_counts_ptr = counts.GetDataPtr<int32_t>();
1202 auto color_gradients_ptr = color_gradients.GetDataPtr<scalar_t>();
1203
1204 core::ParallelFor(
1205 points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
1206 // NNS [Hybrid Search].
1207 int32_t neighbour_offset = max_nn * workload_idx;
1208 // Count of valid correspondences per point.
1209 int32_t neighbour_count =
1210 neighbour_counts_ptr[workload_idx];
1211 int32_t idx_offset = 3 * workload_idx;
1212
1213 EstimatePointWiseColorGradientKernel(
1214 points_ptr, normals_ptr, colors_ptr, idx_offset,
1215 neighbour_indices_ptr + neighbour_offset,
1216 neighbour_count, color_gradients_ptr);
1217 });
1218 });
1219
1220 core::cuda::Synchronize(points.GetDevice());
1221}
1222
1223#if defined(__CUDACC__)
1224void EstimateColorGradientsUsingKNNSearchCUDA
1225#elif defined(SYCL_LANGUAGE_VERSION)
1226void EstimateColorGradientsUsingKNNSearchSYCL
1227#else
1228void EstimateColorGradientsUsingKNNSearchCPU
1229#endif
1230 (const core::Tensor& points,
1231 const core::Tensor& normals,
1232 const core::Tensor& colors,
1233 core::Tensor& color_gradients,
1234 const int64_t& max_nn) {
1235 core::Dtype dtype = points.GetDtype();
1236 int64_t n = points.GetLength();
1237
1238 core::nns::NearestNeighborSearch tree(points, core::Int32);
1239
1240 bool check = tree.KnnIndex();
1241 if (!check) {
1242 utility::LogError("KnnIndex is not set.");
1243 }
1244
1245 core::Tensor indices, distance;
1246 std::tie(indices, distance) = tree.KnnSearch(points, max_nn);
1247
1248 indices = indices.To(core::Int32).Contiguous();
1249 int64_t nn_count = indices.GetShape()[1];
1250
1251 if (nn_count < 4) {
1252 utility::LogError(
1253 "Not enough neighbors to compute Covariances / Normals. "
1254 "Try "
1255 "changing the search parameter.");
1256 }
1257
1258 DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
1259 auto points_ptr = points.GetDataPtr<scalar_t>();
1260 auto normals_ptr = normals.GetDataPtr<scalar_t>();
1261 auto colors_ptr = colors.GetDataPtr<scalar_t>();
1262 auto neighbour_indices_ptr = indices.GetDataPtr<int32_t>();
1263 auto color_gradients_ptr = color_gradients.GetDataPtr<scalar_t>();
1264
1265 core::ParallelFor(
1266 points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
1267 int32_t neighbour_offset = max_nn * workload_idx;
1268 int32_t idx_offset = 3 * workload_idx;
1269
1270 EstimatePointWiseColorGradientKernel(
1271 points_ptr, normals_ptr, colors_ptr, idx_offset,
1272 neighbour_indices_ptr + neighbour_offset, nn_count,
1273 color_gradients_ptr);
1274 });
1275 });
1276
1277 core::cuda::Synchronize(points.GetDevice());
1278}
1279
1280#if defined(__CUDACC__)
1281void EstimateColorGradientsUsingRadiusSearchCUDA
1282#elif defined(SYCL_LANGUAGE_VERSION)
1283void EstimateColorGradientsUsingRadiusSearchSYCL
1284#else
1285void EstimateColorGradientsUsingRadiusSearchCPU
1286#endif
1287 (const core::Tensor& points,
1288 const core::Tensor& normals,
1289 const core::Tensor& colors,
1290 core::Tensor& color_gradients,
1291 const double& radius) {
1292 core::Dtype dtype = points.GetDtype();
1293 int64_t n = points.GetLength();
1294
1295 core::nns::NearestNeighborSearch tree(points, core::Int32);
1296
1297 bool check = tree.FixedRadiusIndex(radius);
1298 if (!check) {
1299 utility::LogError("RadiusIndex is not set.");
1300 }
1301
1302 core::Tensor indices, distance, counts;
1303 std::tie(indices, distance, counts) =
1304 tree.FixedRadiusSearch(points, radius);
1305
1306 indices = indices.To(core::Int32).Contiguous();
1307 counts = counts.Contiguous();
1308
1309 DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
1310 auto points_ptr = points.GetDataPtr<scalar_t>();
1311 auto normals_ptr = normals.GetDataPtr<scalar_t>();
1312 auto colors_ptr = colors.GetDataPtr<scalar_t>();
1313 auto neighbour_indices_ptr = indices.GetDataPtr<int32_t>();
1314 auto neighbour_counts_ptr = counts.GetDataPtr<int32_t>();
1315 auto color_gradients_ptr = color_gradients.GetDataPtr<scalar_t>();
1316
1317 core::ParallelFor(
1318 points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
1319 int32_t neighbour_offset =
1320 neighbour_counts_ptr[workload_idx];
1321 // Count of valid correspondences per point.
1322 const int32_t neighbour_count =
1323 (neighbour_counts_ptr[workload_idx + 1] -
1324 neighbour_counts_ptr[workload_idx]);
1325 int32_t idx_offset = 3 * workload_idx;
1326
1327 EstimatePointWiseColorGradientKernel(
1328 points_ptr, normals_ptr, colors_ptr, idx_offset,
1329 neighbour_indices_ptr + neighbour_offset,
1330 neighbour_count, color_gradients_ptr);
1331 });
1332 });
1333
1334 core::cuda::Synchronize(points.GetDevice());
1335}
1336
1337#endif // OPEN3D_SKIP_POINTCLOUD_MAIN
1338
1339} // namespace pointcloud
1340} // namespace kernel
1341} // namespace geometry
1342} // namespace t
1343} // namespace open3d
CUDAUtils.h
Common CUDA utilities.
OPEN3D_HOST_DEVICE
#define OPEN3D_HOST_DEVICE
Definition CUDAUtils.h:43
OPEN3D_DEVICE
#define OPEN3D_DEVICE
Definition CUDAUtils.h:44
DISPATCH_DTYPE_TO_TEMPLATE
#define DISPATCH_DTYPE_TO_TEMPLATE(DTYPE,...)
Definition Dispatch.h:30
DISPATCH_FLOAT_DTYPE_TO_TEMPLATE
#define DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(DTYPE,...)
Definition Dispatch.h:77
t
double t
Definition SurfaceReconstructionPoisson.cpp:172
point
Point< Real, 3 > point
Definition SurfaceReconstructionPoisson.cpp:163
tree
FEMTree< Dim, Real > & tree
Definition SurfaceReconstructionPoisson.cpp:171
stride
size_t stride
Definition TriangleMeshBuffers.cpp:163
open3d::core::Dtype
Definition Dtype.h:20
open3d::core::Tensor
Definition Tensor.h:32
open3d::core::Tensor::GetShape
SizeVector GetShape() const
Definition Tensor.h:1184
open3d::core::Tensor::GetLength
int64_t GetLength() const
Definition Tensor.h:1182
open3d::core::Tensor::GetDataPtr
T * GetDataPtr()
Definition Tensor.h:1201
open3d::core::Tensor::Div
Tensor Div(const Tensor &value) const
Divides a tensor and returns the resulting tensor.
Definition Tensor.cpp:1301
open3d::core::Tensor::Contiguous
Tensor Contiguous() const
Definition Tensor.cpp:817
open3d::core::Tensor::Transpose
Tensor Transpose(int64_t dim0, int64_t dim1) const
Transpose a Tensor by swapping dimension dim0 and dim1.
Definition Tensor.cpp:1141
open3d::core::Tensor::To
Tensor To(Dtype dtype, bool copy=false) const
Definition Tensor.cpp:784
open3d::core::nns::NearestNeighborSearch
A Class for nearest neighbor search.
Definition NearestNeighborSearch.h:25
open3d::t::geometry::kernel::TArrayIndexer
Definition GeometryIndexer.h:161
open3d::t::geometry::kernel::TArrayIndexer::GetShape
OPEN3D_HOST_DEVICE index_t GetShape(int i) const
Definition GeometryIndexer.h:311
open3d::t::geometry::kernel::TransformIndexer
Helper class for converting coordinates/indices between 3D/3D, 3D/2D, 2D/3D.
Definition GeometryIndexer.h:25
has_normals
bool has_normals
Definition FilePCD.cpp:62
count
int count
Definition FilePCD.cpp:43
points
int points
Definition FilePCD.cpp:55
M_PI
#define M_PI
Definition mikktspace.c:37
open3d::core::cuda::Synchronize
void Synchronize()
Definition CUDAUtils.cpp:58
open3d::core::linalg::kernel::cross_3x1
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:63
open3d::core::linalg::kernel::solve_svd3x3
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:2171
open3d::core::linalg::kernel::dot_3x1
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:89
open3d::core::Int32
const Dtype Int32
Definition Dtype.cpp:46
open3d::core::ParallelFor
void ParallelFor(const Device &device, int64_t n, const func_t &func)
Definition ParallelFor.h:135
open3d::core::Float32
const Dtype Float32
Definition Dtype.cpp:42
open3d::t::geometry::kernel::pointcloud::EstimateCovariancesUsingHybridSearchCPU
void EstimateCovariancesUsingHybridSearchCPU(const core::Tensor &points, core::Tensor &covariances, const double &radius, const int64_t &max_nn)
Definition PointCloudImpl.h:595
open3d::t::geometry::kernel::pointcloud::EstimateCovariancesUsingRadiusSearchCPU
void EstimateCovariancesUsingRadiusSearchCPU(const core::Tensor &points, core::Tensor &covariances, const double &radius)
Definition PointCloudImpl.h:647
open3d::t::geometry::kernel::pointcloud::GetCoordinateSystemOnPlane
OPEN3D_HOST_DEVICE void GetCoordinateSystemOnPlane(const scalar_t *query, scalar_t *u, scalar_t *v)
Definition PointCloudImpl.h:356
open3d::t::geometry::kernel::pointcloud::EstimateNormalsFromCovariancesCPU
void EstimateNormalsFromCovariancesCPU(const core::Tensor &covariances, core::Tensor &normals, const bool has_normals)
Definition PointCloudImpl.h:1018
open3d::t::geometry::kernel::pointcloud::ComputeEigenvector0
OPEN3D_HOST_DEVICE void ComputeEigenvector0(const scalar_t *A, const scalar_t eval0, scalar_t *eigen_vector0)
Definition PointCloudImpl.h:747
open3d::t::geometry::kernel::pointcloud::OrientNormalsTowardsCameraLocationCPU
void OrientNormalsTowardsCameraLocationCPU(const core::Tensor &points, core::Tensor &normals, const core::Tensor &camera)
Definition PointCloudImpl.h:304
open3d::t::geometry::kernel::pointcloud::EstimatePointWiseRobustNormalizedCovarianceKernel
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:513
open3d::t::geometry::kernel::pointcloud::GetPointMaskWithinAABBCPU
void GetPointMaskWithinAABBCPU(const core::Tensor &points, const core::Tensor &min_bound, const core::Tensor &max_bound, core::Tensor &mask)
Definition PointCloudImpl.h:153
open3d::t::geometry::kernel::pointcloud::Swap
OPEN3D_HOST_DEVICE void Swap(scalar_t *x, scalar_t *y)
Definition PointCloudImpl.h:383
open3d::t::geometry::kernel::pointcloud::IsBoundaryPoints
OPEN3D_HOST_DEVICE bool IsBoundaryPoints(const scalar_t *angles, int counts, double angle_threshold)
Definition PointCloudImpl.h:423
open3d::t::geometry::kernel::pointcloud::ComputeBoundaryPointsCPU
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:450
open3d::t::geometry::kernel::pointcloud::EstimateColorGradientsUsingKNNSearchCPU
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:1230
open3d::t::geometry::kernel::pointcloud::UnprojectCPU
void UnprojectCPU(const core::Tensor &depth, std::optional< std::reference_wrapper< const core::Tensor > > image_colors, core::Tensor &points, std::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:49
open3d::t::geometry::kernel::pointcloud::NormalizeNormalsCPU
void NormalizeNormalsCPU(core::Tensor &normals)
Definition PointCloudImpl.h:235
open3d::t::geometry::kernel::pointcloud::ComputeEigenvector1
OPEN3D_HOST_DEVICE void ComputeEigenvector1(const scalar_t *A, const scalar_t *evec0, const scalar_t eval1, scalar_t *eigen_vector1)
Definition PointCloudImpl.h:796
open3d::t::geometry::kernel::pointcloud::EstimatePointWiseColorGradientKernel
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:1068
open3d::t::geometry::kernel::pointcloud::EstimateColorGradientsUsingRadiusSearchCPU
void EstimateColorGradientsUsingRadiusSearchCPU(const core::Tensor &points, const core::Tensor &normals, const core::Tensor &colors, core::Tensor &color_gradient, const double &radius)
Definition PointCloudImpl.h:1287
open3d::t::geometry::kernel::pointcloud::GetPointMaskWithinOBBCPU
void GetPointMaskWithinOBBCPU(const core::Tensor &points, const core::Tensor &center, const core::Tensor &rotation, const core::Tensor &extent, core::Tensor &mask)
Definition PointCloudImpl.h:189
open3d::t::geometry::kernel::pointcloud::EstimateColorGradientsUsingHybridSearchCPU
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:1176
open3d::t::geometry::kernel::pointcloud::EstimatePointWiseNormalsWithFastEigen3x3
OPEN3D_HOST_DEVICE void EstimatePointWiseNormalsWithFastEigen3x3(const scalar_t *covariance_ptr, scalar_t *normals_ptr)
Definition PointCloudImpl.h:876
open3d::t::geometry::kernel::pointcloud::Heapify
OPEN3D_HOST_DEVICE void Heapify(scalar_t *arr, int n, int root)
Definition PointCloudImpl.h:390
open3d::t::geometry::kernel::pointcloud::OrientNormalsToAlignWithDirectionCPU
void OrientNormalsToAlignWithDirectionCPU(core::Tensor &normals, const core::Tensor &direction)
Definition PointCloudImpl.h:268
open3d::t::geometry::kernel::pointcloud::EstimateCovariancesUsingKNNSearchCPU
void EstimateCovariancesUsingKNNSearchCPU(const core::Tensor &points, core::Tensor &covariances, const int64_t &max_nn)
Definition PointCloudImpl.h:698
open3d::t::geometry::kernel::NDArrayIndexer
TArrayIndexer< int64_t > NDArrayIndexer
Definition GeometryIndexer.h:360
open3d::t::geometry::InverseTransformation
core::Tensor InverseTransformation(const core::Tensor &T)
TODO(wei): find a proper place for such functionalities.
Definition Utility.h:77
open3d
Definition PinholeCameraIntrinsic.cpp:16
normals
const core::Tensor * normals
Definition TriangleMesh.cpp:2126
indices
const core::Tensor * indices
Definition TriangleMesh.cpp:2128
Generated by doxygen 1.9.8