Open3D (C++ API)  0.15.1
PointCloudImpl.h
Go to the documentation of this file.
1// ----------------------------------------------------------------------------
2// - Open3D: www.open3d.org -
3// ----------------------------------------------------------------------------
4// The MIT License (MIT)
5//
6// Copyright (c) 2018-2021 www.open3d.org
7//
8// Permission is hereby granted, free of charge, to any person obtaining a copy
9// of this software and associated documentation files (the "Software"), to deal
10// in the Software without restriction, including without limitation the rights
11// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12// copies of the Software, and to permit persons to whom the Software is
13// furnished to do so, subject to the following conditions:
14//
15// The above copyright notice and this permission notice shall be included in
16// all copies or substantial portions of the Software.
17//
18// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
23// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
24// IN THE SOFTWARE.
25// ----------------------------------------------------------------------------
26
27#include <atomic>
28#include <vector>
29
32#include "open3d/core/Dtype.h"
36#include "open3d/core/Tensor.h"
44
45namespace open3d {
46namespace t {
47namespace geometry {
48namespace kernel {
49namespace pointcloud {
50
51#ifndef __CUDACC__
52using std::abs;
53using std::max;
54using std::min;
55using std::sqrt;
56#endif
57
58#if defined(__CUDACC__)
59void UnprojectCUDA
60#else
62#endif
63 (const core::Tensor& depth,
65 image_colors,
68 const core::Tensor& intrinsics,
69 const core::Tensor& extrinsics,
70 float depth_scale,
71 float depth_max,
72 int64_t stride) {
73
74 const bool have_colors = image_colors.has_value();
75 NDArrayIndexer depth_indexer(depth, 2);
76 NDArrayIndexer image_colors_indexer;
77
79 TransformIndexer ti(intrinsics, pose, 1.0f);
80
81 // Output
82 int64_t rows_strided = depth_indexer.GetShape(0) / stride;
83 int64_t cols_strided = depth_indexer.GetShape(1) / stride;
84
85 points = core::Tensor({rows_strided * cols_strided, 3}, core::Float32,
86 depth.GetDevice());
87 NDArrayIndexer point_indexer(points, 1);
88 NDArrayIndexer colors_indexer;
89 if (have_colors) {
90 const auto& imcol = image_colors.value().get();
91 image_colors_indexer = NDArrayIndexer{imcol, 2};
92 colors.value().get() = core::Tensor({rows_strided * cols_strided, 3},
93 core::Float32, imcol.GetDevice());
94 colors_indexer = NDArrayIndexer(colors.value().get(), 1);
95 }
96
97 // Counter
98#if defined(__CUDACC__)
99 core::Tensor count(std::vector<int>{0}, {}, core::Int32, depth.GetDevice());
100 int* count_ptr = count.GetDataPtr<int>();
101#else
102 std::atomic<int> count_atomic(0);
103 std::atomic<int>* count_ptr = &count_atomic;
104#endif
105
106 int64_t n = rows_strided * cols_strided;
107
108 DISPATCH_DTYPE_TO_TEMPLATE(depth.GetDtype(), [&]() {
109 core::ParallelFor(
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;
113
114 float d = *depth_indexer.GetDataPtr<scalar_t>(x, y) /
115 depth_scale;
116 if (d > 0 && d < depth_max) {
117 int idx = OPEN3D_ATOMIC_ADD(count_ptr, 1);
118
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,
122 &z_c);
123
124 float* vertex = point_indexer.GetDataPtr<float>(idx);
125 ti.RigidTransform(x_c, y_c, z_c, vertex + 0, vertex + 1,
126 vertex + 2);
127 if (have_colors) {
128 float* pcd_pixel =
129 colors_indexer.GetDataPtr<float>(idx);
130 float* image_pixel =
131 image_colors_indexer.GetDataPtr<float>(x,
132 y);
133 *pcd_pixel = *image_pixel;
134 *(pcd_pixel + 1) = *(image_pixel + 1);
135 *(pcd_pixel + 2) = *(image_pixel + 2);
136 }
137 }
138 });
139 });
140#if defined(__CUDACC__)
141 int total_pts_count = count.Item<int>();
142#else
143 int total_pts_count = (*count_ptr).load();
144#endif
145
146#ifdef __CUDACC__
148#endif
149 points = points.Slice(0, 0, total_pts_count);
150 if (have_colors) {
151 colors.value().get() =
152 colors.value().get().Slice(0, 0, total_pts_count);
153 }
154}
155
156// This is a `two-pass` estimate method for covariance which is numerically more
157// robust than the `textbook` method generally used for covariance computation.
158template <typename scalar_t>
160 const scalar_t* points_ptr,
161 const int32_t* indices_ptr,
162 const int32_t& indices_count,
163 scalar_t* covariance_ptr) {
164 if (indices_count < 3) {
165 covariance_ptr[0] = 1.0;
166 covariance_ptr[1] = 0.0;
167 covariance_ptr[2] = 0.0;
168 covariance_ptr[3] = 0.0;
169 covariance_ptr[4] = 1.0;
170 covariance_ptr[5] = 0.0;
171 covariance_ptr[6] = 0.0;
172 covariance_ptr[7] = 0.0;
173 covariance_ptr[8] = 1.0;
174 return;
175 }
176
177 double centroid[3] = {0};
178 for (int32_t i = 0; i < indices_count; ++i) {
179 int32_t idx = 3 * indices_ptr[i];
180 centroid[0] += points_ptr[idx];
181 centroid[1] += points_ptr[idx + 1];
182 centroid[2] += points_ptr[idx + 2];
183 }
184
185 centroid[0] /= indices_count;
186 centroid[1] /= indices_count;
187 centroid[2] /= indices_count;
188
189 // cumulants must always be Float64 to ensure precision.
190 double cumulants[6] = {0};
191 for (int32_t i = 0; i < indices_count; ++i) {
192 int32_t idx = 3 * indices_ptr[i];
193 const double x = static_cast<double>(points_ptr[idx]) - centroid[0];
194 const double y = static_cast<double>(points_ptr[idx + 1]) - centroid[1];
195 const double z = static_cast<double>(points_ptr[idx + 2]) - centroid[2];
196
197 cumulants[0] += x * x;
198 cumulants[1] += y * y;
199 cumulants[2] += z * z;
200
201 cumulants[3] += x * y;
202 cumulants[4] += x * z;
203 cumulants[5] += y * z;
204 }
205
206 // Using Bessel's correction (dividing by (n - 1) instead of n).
207 // Refer: https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance
208 const double normalization_factor = static_cast<double>(indices_count - 1);
209 for (int i = 0; i < 6; ++i) {
210 cumulants[i] /= normalization_factor;
211 }
212
213 // Covariances(0, 0)
214 covariance_ptr[0] = static_cast<scalar_t>(cumulants[0]);
215 // Covariances(1, 1)
216 covariance_ptr[4] = static_cast<scalar_t>(cumulants[1]);
217 // Covariances(2, 2)
218 covariance_ptr[8] = static_cast<scalar_t>(cumulants[2]);
219
220 // Covariances(0, 1) = Covariances(1, 0)
221 covariance_ptr[1] = static_cast<scalar_t>(cumulants[3]);
222 covariance_ptr[3] = covariance_ptr[1];
223
224 // Covariances(0, 2) = Covariances(2, 0)
225 covariance_ptr[2] = static_cast<scalar_t>(cumulants[4]);
226 covariance_ptr[6] = covariance_ptr[2];
227
228 // Covariances(1, 2) = Covariances(2, 1)
229 covariance_ptr[5] = static_cast<scalar_t>(cumulants[5]);
230 covariance_ptr[7] = covariance_ptr[5];
231}
232
233#if defined(__CUDACC__)
234void EstimateCovariancesUsingHybridSearchCUDA
235#else
237#endif
238 (const core::Tensor& points,
239 core::Tensor& covariances,
240 const double& radius,
241 const int64_t& max_nn) {
242 core::Dtype dtype = points.GetDtype();
243 int64_t n = points.GetLength();
244
246 bool check = tree.HybridIndex(radius);
247 if (!check) {
248 utility::LogError("Building FixedRadiusIndex failed.");
249 }
250
251 core::Tensor indices, distance, counts;
252 std::tie(indices, distance, counts) =
253 tree.HybridSearch(points, radius, max_nn);
254
256 const scalar_t* points_ptr = points.GetDataPtr<scalar_t>();
257 int32_t* neighbour_indices_ptr = indices.GetDataPtr<int32_t>();
258 int32_t* neighbour_counts_ptr = counts.GetDataPtr<int32_t>();
259 scalar_t* covariances_ptr = covariances.GetDataPtr<scalar_t>();
260
262 points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
263 // NNS [Hybrid Search].
264 const int32_t neighbour_offset = max_nn * workload_idx;
265 // Count of valid correspondences per point.
266 const int32_t neighbour_count =
267 neighbour_counts_ptr[workload_idx];
268 // Covariance is of shape {3, 3}, so it has an
269 // offset factor of 9 x workload_idx.
270 const int32_t covariances_offset = 9 * workload_idx;
271
273 points_ptr,
274 neighbour_indices_ptr + neighbour_offset,
275 neighbour_count,
276 covariances_ptr + covariances_offset);
277 });
278 });
279
280 core::cuda::Synchronize(points.GetDevice());
281}
282
283#if defined(__CUDACC__)
284void EstimateCovariancesUsingKNNSearchCUDA
285#else
287#endif
288 (const core::Tensor& points,
289 core::Tensor& covariances,
290 const int64_t& max_nn) {
291 core::Dtype dtype = points.GetDtype();
292 int64_t n = points.GetLength();
293
295 bool check = tree.KnnIndex();
296 if (!check) {
297 utility::LogError("Building KNN-Index failed.");
298 }
299
300 core::Tensor indices, distance;
301 std::tie(indices, distance) = tree.KnnSearch(points, max_nn);
302
303 indices = indices.Contiguous();
304 int32_t nn_count = static_cast<int32_t>(indices.GetShape()[1]);
305
306 if (nn_count < 3) {
308 "Not enought neighbors to compute Covariances / Normals. Try "
309 "increasing the max_nn parameter.");
310 }
311
313 auto points_ptr = points.GetDataPtr<scalar_t>();
314 auto neighbour_indices_ptr = indices.GetDataPtr<int32_t>();
315 auto covariances_ptr = covariances.GetDataPtr<scalar_t>();
316
318 points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
319 // NNS [KNN Search].
320 const int32_t neighbour_offset = nn_count * workload_idx;
321 // Covariance is of shape {3, 3}, so it has an offset factor
322 // of 9 x workload_idx.
323 const int32_t covariances_offset = 9 * workload_idx;
324
326 points_ptr,
327 neighbour_indices_ptr + neighbour_offset, nn_count,
328 covariances_ptr + covariances_offset);
329 });
330 });
331
332 core::cuda::Synchronize(points.GetDevice());
333}
334
335template <typename scalar_t>
337 const scalar_t eval0,
338 scalar_t* eigen_vector0) {
339 scalar_t row0[3] = {A[0] - eval0, A[1], A[2]};
340 scalar_t row1[3] = {A[1], A[4] - eval0, A[5]};
341 scalar_t row2[3] = {A[2], A[5], A[8] - eval0};
342
343 scalar_t r0xr1[3], r0xr2[3], r1xr2[3];
344
345 core::linalg::kernel::cross_3x1(row0, row1, r0xr1);
346 core::linalg::kernel::cross_3x1(row0, row2, r0xr2);
347 core::linalg::kernel::cross_3x1(row1, row2, r1xr2);
348
349 scalar_t d0 = core::linalg::kernel::dot_3x1(r0xr1, r0xr1);
350 scalar_t d1 = core::linalg::kernel::dot_3x1(r0xr2, r0xr2);
351 scalar_t d2 = core::linalg::kernel::dot_3x1(r1xr2, r1xr2);
352
353 scalar_t dmax = d0;
354 int imax = 0;
355 if (d1 > dmax) {
356 dmax = d1;
357 imax = 1;
358 }
359 if (d2 > dmax) {
360 imax = 2;
361 }
362
363 if (imax == 0) {
364 scalar_t sqrt_d = sqrt(d0);
365 eigen_vector0[0] = r0xr1[0] / sqrt_d;
366 eigen_vector0[1] = r0xr1[1] / sqrt_d;
367 eigen_vector0[2] = r0xr1[2] / sqrt_d;
368 return;
369 } else if (imax == 1) {
370 scalar_t sqrt_d = sqrt(d1);
371 eigen_vector0[0] = r0xr2[0] / sqrt_d;
372 eigen_vector0[1] = r0xr2[1] / sqrt_d;
373 eigen_vector0[2] = r0xr2[2] / sqrt_d;
374 return;
375 } else {
376 scalar_t sqrt_d = sqrt(d2);
377 eigen_vector0[0] = r1xr2[0] / sqrt_d;
378 eigen_vector0[1] = r1xr2[1] / sqrt_d;
379 eigen_vector0[2] = r1xr2[2] / sqrt_d;
380 return;
381 }
382}
383
384template <typename scalar_t>
386 const scalar_t* evec0,
387 const scalar_t eval1,
388 scalar_t* eigen_vector1) {
389 scalar_t U[3];
390 if (abs(evec0[0]) > abs(evec0[1])) {
391 scalar_t inv_length =
392 1.0 / sqrt(evec0[0] * evec0[0] + evec0[2] * evec0[2]);
393 U[0] = -evec0[2] * inv_length;
394 U[1] = 0.0;
395 U[2] = evec0[0] * inv_length;
396 } else {
397 scalar_t inv_length =
398 1.0 / sqrt(evec0[1] * evec0[1] + evec0[2] * evec0[2]);
399 U[0] = 0.0;
400 U[1] = evec0[2] * inv_length;
401 U[2] = -evec0[1] * inv_length;
402 }
403 scalar_t V[3], AU[3], AV[3];
405 core::linalg::kernel::matmul3x3_3x1(A, U, AU);
406 core::linalg::kernel::matmul3x3_3x1(A, V, AV);
407
408 scalar_t m00 = core::linalg::kernel::dot_3x1(U, AU) - eval1;
409 scalar_t m01 = core::linalg::kernel::dot_3x1(U, AV);
410 scalar_t m11 = core::linalg::kernel::dot_3x1(V, AV) - eval1;
411
412 scalar_t absM00 = abs(m00);
413 scalar_t absM01 = abs(m01);
414 scalar_t absM11 = abs(m11);
415 scalar_t max_abs_comp;
416
417 if (absM00 >= absM11) {
418 max_abs_comp = max(absM00, absM01);
419 if (max_abs_comp > 0) {
420 if (absM00 >= absM01) {
421 m01 /= m00;
422 m00 = 1 / sqrt(1 + m01 * m01);
423 m01 *= m00;
424 } else {
425 m00 /= m01;
426 m01 = 1 / sqrt(1 + m00 * m00);
427 m00 *= m01;
428 }
429 eigen_vector1[0] = m01 * U[0] - m00 * V[0];
430 eigen_vector1[1] = m01 * U[1] - m00 * V[1];
431 eigen_vector1[2] = m01 * U[2] - m00 * V[2];
432 return;
433 } else {
434 eigen_vector1[0] = U[0];
435 eigen_vector1[1] = U[1];
436 eigen_vector1[2] = U[2];
437 return;
438 }
439 } else {
440 max_abs_comp = max(absM11, absM01);
441 if (max_abs_comp > 0) {
442 if (absM11 >= absM01) {
443 m01 /= m11;
444 m11 = 1 / sqrt(1 + m01 * m01);
445 m01 *= m11;
446 } else {
447 m11 /= m01;
448 m01 = 1 / sqrt(1 + m11 * m11);
449 m11 *= m01;
450 }
451 eigen_vector1[0] = m11 * U[0] - m01 * V[0];
452 eigen_vector1[1] = m11 * U[1] - m01 * V[1];
453 eigen_vector1[2] = m11 * U[2] - m01 * V[2];
454 return;
455 } else {
456 eigen_vector1[0] = U[0];
457 eigen_vector1[1] = U[1];
458 eigen_vector1[2] = U[2];
459 return;
460 }
461 }
462}
463
464template <typename scalar_t>
466 const scalar_t* covariance_ptr, scalar_t* normals_ptr) {
467 // Based on:
468 // https://www.geometrictools.com/Documentation/RobustEigenSymmetric3x3.pdf
469 // which handles edge cases like points on a plane.
470 scalar_t max_coeff = covariance_ptr[0];
471
472 for (int i = 1; i < 9; ++i) {
473 if (max_coeff < covariance_ptr[i]) {
474 max_coeff = covariance_ptr[i];
475 }
476 }
477
478 if (max_coeff == 0) {
479 normals_ptr[0] = 0.0;
480 normals_ptr[1] = 0.0;
481 normals_ptr[2] = 0.0;
482 return;
483 }
484
485 scalar_t A[9] = {0};
486
487 for (int i = 0; i < 9; ++i) {
488 A[i] = covariance_ptr[i] / max_coeff;
489 }
490
491 scalar_t norm = A[1] * A[1] + A[2] * A[2] + A[5] * A[5];
492
493 if (norm > 0) {
494 scalar_t eval[3];
495 scalar_t evec0[3];
496 scalar_t evec1[3];
497 scalar_t evec2[3];
498
499 scalar_t q = (A[0] + A[4] + A[8]) / 3.0;
500
501 scalar_t b00 = A[0] - q;
502 scalar_t b11 = A[4] - q;
503 scalar_t b22 = A[8] - q;
504
505 scalar_t p =
506 sqrt((b00 * b00 + b11 * b11 + b22 * b22 + norm * 2.0) / 6.0);
507
508 scalar_t c00 = b11 * b22 - A[5] * A[5];
509 scalar_t c01 = A[1] * b22 - A[5] * A[2];
510 scalar_t c02 = A[1] * A[5] - b11 * A[2];
511 scalar_t det = (b00 * c00 - A[1] * c01 + A[2] * c02) / (p * p * p);
512
513 scalar_t half_det = det * 0.5;
514 half_det = min(max(half_det, static_cast<scalar_t>(-1.0)),
515 static_cast<scalar_t>(1.0));
516
517 scalar_t angle = acos(half_det) / 3.0;
518 const scalar_t two_thrids_pi = 2.09439510239319549;
519
520 scalar_t beta2 = cos(angle) * 2.0;
521 scalar_t beta0 = cos(angle + two_thrids_pi) * 2.0;
522 scalar_t beta1 = -(beta0 + beta2);
523
524 eval[0] = q + p * beta0;
525 eval[1] = q + p * beta1;
526 eval[2] = q + p * beta2;
527
528 if (half_det >= 0) {
529 ComputeEigenvector0<scalar_t>(A, eval[2], evec2);
530
531 if (eval[2] < eval[0] && eval[2] < eval[1]) {
532 normals_ptr[0] = evec2[0];
533 normals_ptr[1] = evec2[1];
534 normals_ptr[2] = evec2[2];
535
536 return;
537 }
538
539 ComputeEigenvector1<scalar_t>(A, evec2, eval[1], evec1);
540
541 if (eval[1] < eval[0] && eval[1] < eval[2]) {
542 normals_ptr[0] = evec1[0];
543 normals_ptr[1] = evec1[1];
544 normals_ptr[2] = evec1[2];
545
546 return;
547 }
548
549 normals_ptr[0] = evec1[1] * evec2[2] - evec1[2] * evec2[1];
550 normals_ptr[1] = evec1[2] * evec2[0] - evec1[0] * evec2[2];
551 normals_ptr[2] = evec1[0] * evec2[1] - evec1[1] * evec2[0];
552
553 return;
554 } else {
555 ComputeEigenvector0<scalar_t>(A, eval[0], evec0);
556
557 if (eval[0] < eval[1] && eval[0] < eval[2]) {
558 normals_ptr[0] = evec0[0];
559 normals_ptr[1] = evec0[1];
560 normals_ptr[2] = evec0[2];
561 return;
562 }
563
564 ComputeEigenvector1<scalar_t>(A, evec0, eval[1], evec1);
565
566 if (eval[1] < eval[0] && eval[1] < eval[2]) {
567 normals_ptr[0] = evec1[0];
568 normals_ptr[1] = evec1[1];
569 normals_ptr[2] = evec1[2];
570 return;
571 }
572
573 normals_ptr[0] = evec0[1] * evec1[2] - evec0[2] * evec1[1];
574 normals_ptr[1] = evec0[2] * evec1[0] - evec0[0] * evec1[2];
575 normals_ptr[2] = evec0[0] * evec1[1] - evec0[1] * evec1[0];
576 return;
577 }
578 } else {
579 if (covariance_ptr[0] < covariance_ptr[4] &&
580 covariance_ptr[0] < covariance_ptr[8]) {
581 normals_ptr[0] = 1.0;
582 normals_ptr[1] = 0.0;
583 normals_ptr[2] = 0.0;
584 return;
585 } else if (covariance_ptr[0] < covariance_ptr[4] &&
586 covariance_ptr[0] < covariance_ptr[8]) {
587 normals_ptr[0] = 0.0;
588 normals_ptr[1] = 1.0;
589 normals_ptr[2] = 0.0;
590 return;
591 } else {
592 normals_ptr[0] = 0.0;
593 normals_ptr[1] = 0.0;
594 normals_ptr[2] = 1.0;
595 return;
596 }
597 }
598}
599
600#if defined(__CUDACC__)
601void EstimateNormalsFromCovariancesCUDA
602#else
604#endif
605 (const core::Tensor& covariances,
606 core::Tensor& normals,
607 const bool has_normals) {
608 core::Dtype dtype = covariances.GetDtype();
609 int64_t n = covariances.GetLength();
610
612 const scalar_t* covariances_ptr = covariances.GetDataPtr<scalar_t>();
613 scalar_t* normals_ptr = normals.GetDataPtr<scalar_t>();
614
616 covariances.GetDevice(), n,
617 [=] OPEN3D_DEVICE(int64_t workload_idx) {
618 int32_t covariances_offset = 9 * workload_idx;
619 int32_t normals_offset = 3 * workload_idx;
620 scalar_t normals_output[3] = {0};
621 EstimatePointWiseNormalsWithFastEigen3x3<scalar_t>(
622 covariances_ptr + covariances_offset,
623 normals_output);
624
625 if ((normals_output[0] * normals_output[0] +
626 normals_output[1] * normals_output[1] +
627 normals_output[2] * normals_output[2]) == 0.0 &&
628 !has_normals) {
629 normals_output[0] = 0.0;
630 normals_output[1] = 0.0;
631 normals_output[2] = 1.0;
632 }
633 if (has_normals) {
634 if ((normals_ptr[normals_offset] * normals_output[0] +
635 normals_ptr[normals_offset + 1] *
636 normals_output[1] +
637 normals_ptr[normals_offset + 2] *
638 normals_output[2]) < 0.0) {
639 normals_output[0] *= -1;
640 normals_output[1] *= -1;
641 normals_output[2] *= -1;
642 }
643 }
644
645 normals_ptr[normals_offset] = normals_output[0];
646 normals_ptr[normals_offset + 1] = normals_output[1];
647 normals_ptr[normals_offset + 2] = normals_output[2];
648 });
649 });
650
651 core::cuda::Synchronize(covariances.GetDevice());
652}
653
654template <typename scalar_t>
656 const scalar_t* points_ptr,
657 const scalar_t* normals_ptr,
658 const scalar_t* colors_ptr,
659 const int32_t& idx_offset,
660 const int32_t* indices_ptr,
661 const int32_t& indices_count,
662 scalar_t* color_gradients_ptr) {
663 if (indices_count < 4) {
664 color_gradients_ptr[idx_offset] = 0;
665 color_gradients_ptr[idx_offset + 1] = 0;
666 color_gradients_ptr[idx_offset + 2] = 0;
667 } else {
668 scalar_t vt[3] = {points_ptr[idx_offset], points_ptr[idx_offset + 1],
669 points_ptr[idx_offset + 2]};
670
671 scalar_t nt[3] = {normals_ptr[idx_offset], normals_ptr[idx_offset + 1],
672 normals_ptr[idx_offset + 2]};
673
674 scalar_t it = (colors_ptr[idx_offset] + colors_ptr[idx_offset + 1] +
675 colors_ptr[idx_offset + 2]) /
676 3.0;
677
678 scalar_t AtA[9] = {0};
679 scalar_t Atb[3] = {0};
680
681 // approximate image gradient of vt's tangential plane
682 // projection (p') of a point p on a plane defined by
683 // normal n, where o is the closest point to p on the
684 // plane, is given by:
685 // p' = p - [(p - o).dot(n)] * n p'
686 // => p - [(p.dot(n) - s)] * n [where s = o.dot(n)]
687
688 // Computing the scalar s.
689 scalar_t s = vt[0] * nt[0] + vt[1] * nt[1] + vt[2] * nt[2];
690
691 int i = 1;
692 for (; i < indices_count; i++) {
693 int64_t neighbour_idx_offset = 3 * indices_ptr[i];
694
695 if (neighbour_idx_offset == -1) {
696 break;
697 }
698
699 scalar_t vt_adj[3] = {points_ptr[neighbour_idx_offset],
700 points_ptr[neighbour_idx_offset + 1],
701 points_ptr[neighbour_idx_offset + 2]};
702
703 // p' = p - d * n [where d = p.dot(n) - s]
704 // Computing the scalar d.
705 scalar_t d = vt_adj[0] * nt[0] + vt_adj[1] * nt[1] +
706 vt_adj[2] * nt[2] - s;
707
708 // Computing the p' (projection of the point).
709 scalar_t vt_proj[3] = {vt_adj[0] - d * nt[0], vt_adj[1] - d * nt[1],
710 vt_adj[2] - d * nt[2]};
711
712 scalar_t it_adj = (colors_ptr[neighbour_idx_offset + 0] +
713 colors_ptr[neighbour_idx_offset + 1] +
714 colors_ptr[neighbour_idx_offset + 2]) /
715 3.0;
716
717 scalar_t A[3] = {vt_proj[0] - vt[0], vt_proj[1] - vt[1],
718 vt_proj[2] - vt[2]};
719
720 AtA[0] += A[0] * A[0];
721 AtA[1] += A[1] * A[0];
722 AtA[2] += A[2] * A[0];
723 AtA[4] += A[1] * A[1];
724 AtA[5] += A[2] * A[1];
725 AtA[8] += A[2] * A[2];
726
727 scalar_t b = it_adj - it;
728
729 Atb[0] += A[0] * b;
730 Atb[1] += A[1] * b;
731 Atb[2] += A[2] * b;
732 }
733
734 // Orthogonal constraint.
735 scalar_t A[3] = {(i - 1) * nt[0], (i - 1) * nt[1], (i - 1) * nt[2]};
736
737 AtA[0] += A[0] * A[0];
738 AtA[1] += A[0] * A[1];
739 AtA[2] += A[0] * A[2];
740 AtA[4] += A[1] * A[1];
741 AtA[5] += A[1] * A[2];
742 AtA[8] += A[2] * A[2];
743
744 // Symmetry.
745 AtA[3] = AtA[1];
746 AtA[6] = AtA[2];
747 AtA[7] = AtA[5];
748
750 color_gradients_ptr + idx_offset);
751 }
752}
753
754#if defined(__CUDACC__)
755void EstimateColorGradientsUsingHybridSearchCUDA
756#else
758#endif
759 (const core::Tensor& points,
760 const core::Tensor& normals,
761 const core::Tensor& colors,
762 core::Tensor& color_gradients,
763 const double& radius,
764 const int64_t& max_nn) {
765 core::Dtype dtype = points.GetDtype();
766 int64_t n = points.GetLength();
767
769
770 bool check = tree.HybridIndex(radius);
771 if (!check) {
773 "NearestNeighborSearch::FixedRadiusIndex Index is not set.");
774 }
775
776 core::Tensor indices, distance, counts;
777 std::tie(indices, distance, counts) =
778 tree.HybridSearch(points, radius, max_nn);
779
781 auto points_ptr = points.GetDataPtr<scalar_t>();
782 auto normals_ptr = normals.GetDataPtr<scalar_t>();
783 auto colors_ptr = colors.GetDataPtr<scalar_t>();
784 auto neighbour_indices_ptr = indices.GetDataPtr<int32_t>();
785 auto neighbour_counts_ptr = counts.GetDataPtr<int32_t>();
786 auto color_gradients_ptr = color_gradients.GetDataPtr<scalar_t>();
787
789 points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
790 // NNS [Hybrid Search].
791 int32_t neighbour_offset = max_nn * workload_idx;
792 // Count of valid correspondences per point.
793 int32_t neighbour_count =
794 neighbour_counts_ptr[workload_idx];
795 int32_t idx_offset = 3 * workload_idx;
796
798 points_ptr, normals_ptr, colors_ptr, idx_offset,
799 neighbour_indices_ptr + neighbour_offset,
800 neighbour_count, color_gradients_ptr);
801 });
802 });
803
804 core::cuda::Synchronize(points.GetDevice());
805}
806
807#if defined(__CUDACC__)
808void EstimateColorGradientsUsingKNNSearchCUDA
809#else
811#endif
812 (const core::Tensor& points,
813 const core::Tensor& normals,
814 const core::Tensor& colors,
815 core::Tensor& color_gradients,
816 const int64_t& max_nn) {
817 core::Dtype dtype = points.GetDtype();
818 int64_t n = points.GetLength();
819
821
822 bool check = tree.KnnIndex();
823 if (!check) {
824 utility::LogError("KnnIndex is not set.");
825 }
826
827 core::Tensor indices, distance;
828 std::tie(indices, distance) = tree.KnnSearch(points, max_nn);
829
830 indices = indices.To(core::Int32).Contiguous();
831 int64_t nn_count = indices.GetShape()[1];
832
833 if (nn_count < 4) {
835 "Not enought neighbors to compute Covariances / Normals. Try "
836 "changing the search parameter.");
837 }
838
840 auto points_ptr = points.GetDataPtr<scalar_t>();
841 auto normals_ptr = normals.GetDataPtr<scalar_t>();
842 auto colors_ptr = colors.GetDataPtr<scalar_t>();
843 auto neighbour_indices_ptr = indices.GetDataPtr<int32_t>();
844 auto color_gradients_ptr = color_gradients.GetDataPtr<scalar_t>();
845
847 points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
848 int32_t neighbour_offset = max_nn * workload_idx;
849 int32_t idx_offset = 3 * workload_idx;
850
852 points_ptr, normals_ptr, colors_ptr, idx_offset,
853 neighbour_indices_ptr + neighbour_offset, nn_count,
854 color_gradients_ptr);
855 });
856 });
857
858 core::cuda::Synchronize(points.GetDevice());
859}
860
861} // namespace pointcloud
862} // namespace kernel
863} // namespace geometry
864} // namespace t
865} // namespace open3d
Common CUDA utilities.
#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
Definition: Dtype.h:39
Definition: Tensor.h:51
SizeVector GetShape() const
Definition: Tensor.h:1091
T * GetDataPtr()
Definition: Tensor.h:1108
Tensor Contiguous() const
Definition: Tensor.cpp:746
Tensor To(Dtype dtype, bool copy=false) const
Definition: Tensor.cpp:713
A Class for nearest neighbor search.
Definition: NearestNeighborSearch.h:44
std::tuple< Tensor, Tensor, Tensor > HybridSearch(const Tensor &query_points, double radius, int max_knn)
Definition: NearestNeighborSearch.cpp:149
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:331
Helper class for converting coordinates/indices between 3D/3D, 3D/2D, 2D/3D.
Definition: GeometryIndexer.h:44
Definition: Optional.h:278
bool has_normals
Definition: FilePCD.cpp:80
int count
Definition: FilePCD.cpp:61
int points
Definition: FilePCD.cpp:73
void Synchronize()
Definition: CUDAUtils.cpp:78
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:238
void EstimateNormalsFromCovariancesCPU(const core::Tensor &covariances, core::Tensor &normals, const bool has_normals)
Definition: PointCloudImpl.h:605
OPEN3D_HOST_DEVICE void ComputeEigenvector0(const scalar_t *A, const scalar_t eval0, scalar_t *eigen_vector0)
Definition: PointCloudImpl.h:336
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
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:159
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:812
OPEN3D_HOST_DEVICE void ComputeEigenvector1(const scalar_t *A, const scalar_t *evec0, const scalar_t eval1, scalar_t *eigen_vector1)
Definition: PointCloudImpl.h:385
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:655
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:759
OPEN3D_HOST_DEVICE void EstimatePointWiseNormalsWithFastEigen3x3(const scalar_t *covariance_ptr, scalar_t *normals_ptr)
Definition: PointCloudImpl.h:465
void EstimateCovariancesUsingKNNSearchCPU(const core::Tensor &points, core::Tensor &covariances, const int64_t &max_nn)
Definition: PointCloudImpl.h:288
TArrayIndexer< int64_t > NDArrayIndexer
Definition: GeometryIndexer.h:380
core::Tensor InverseTransformation(const core::Tensor &T)
TODO(wei): find a proper place for such functionalities.
Definition: Utility.h:96
Definition: PinholeCameraIntrinsic.cpp:35