Open3D (C++ API)  0.16.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 
30 #include "open3d/core/CUDAUtils.h"
31 #include "open3d/core/Dispatch.h"
32 #include "open3d/core/Dtype.h"
35 #include "open3d/core/SizeVector.h"
36 #include "open3d/core/Tensor.h"
43 #include "open3d/utility/Logging.h"
44 
45 namespace open3d {
46 namespace t {
47 namespace geometry {
48 namespace kernel {
49 namespace pointcloud {
50 
51 #ifndef __CUDACC__
52 using std::abs;
53 using std::max;
54 using std::min;
55 using std::sqrt;
56 #endif
57 
58 #if defined(__CUDACC__)
59 void 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 #if defined(__CUDACC__)
157 void GetPointMaskWithinAABBCUDA
158 #else
160 #endif
161  (const core::Tensor& points,
162  const core::Tensor& min_bound,
163  const core::Tensor& max_bound,
164  core::Tensor& mask) {
165 
166  DISPATCH_DTYPE_TO_TEMPLATE(points.GetDtype(), [&]() {
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>();
172 
173  core::ParallelFor(
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];
178 
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;
183  } else {
184  mask_ptr[workload_idx] = false;
185  }
186  });
187  });
188 }
189 
190 #if defined(__CUDACC__)
191 void NormalizeNormalsCUDA
192 #else
194 #endif
195  (core::Tensor& normals) {
196  const core::Dtype dtype = normals.GetDtype();
197  const int64_t n = normals.GetLength();
198 
199  DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
200  scalar_t* ptr = normals.GetDataPtr<scalar_t>();
201 
202  core::ParallelFor(normals.GetDevice(), n,
203  [=] OPEN3D_DEVICE(int64_t workload_idx) {
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);
209  if (norm > 0) {
210  x /= norm;
211  y /= norm;
212  z /= norm;
213  }
214  ptr[idx] = x;
215  ptr[idx + 1] = y;
216  ptr[idx + 2] = z;
217  });
218  });
219 }
220 
221 #if defined(__CUDACC__)
222 void OrientNormalsToAlignWithDirectionCUDA
223 #else
225 #endif
226  (core::Tensor& normals, const core::Tensor& direction) {
227  const core::Dtype dtype = normals.GetDtype();
228  const int64_t n = normals.GetLength();
229 
230  DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
231  scalar_t* ptr = normals.GetDataPtr<scalar_t>();
232  const scalar_t* direction_ptr = direction.GetDataPtr<scalar_t>();
233 
234  core::ParallelFor(normals.GetDevice(), n,
235  [=] OPEN3D_DEVICE(int64_t workload_idx) {
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]);
241  if (norm == 0.0) {
242  normal[0] = direction_ptr[0];
243  normal[1] = direction_ptr[1];
244  normal[2] = direction_ptr[2];
246  normal, direction_ptr) < 0) {
247  normal[0] *= -1;
248  normal[1] *= -1;
249  normal[2] *= -1;
250  }
251  });
252  });
253 }
254 
255 #if defined(__CUDACC__)
256 void OrientNormalsTowardsCameraLocationCUDA
257 #else
259 #endif
260  (const core::Tensor& points,
261  core::Tensor& normals,
262  const core::Tensor& camera) {
263  const core::Dtype dtype = points.GetDtype();
264  const int64_t n = normals.GetLength();
265 
266  DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
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>();
270 
272  normals.GetDevice(), n,
273  [=] OPEN3D_DEVICE(int64_t workload_idx) {
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]);
283  if (norm == 0.0) {
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) {
291  normal[0] = 0.0;
292  normal[1] = 0.0;
293  normal[2] = 1.0;
294  } else {
295  normal[0] /= norm_new;
296  normal[1] /= norm_new;
297  normal[2] /= norm_new;
298  }
299  } else if (core::linalg::kernel::dot_3x1(normal,
300  reference) < 0) {
301  normal[0] *= -1;
302  normal[1] *= -1;
303  normal[2] *= -1;
304  }
305  });
306  });
307 }
308 
309 template <typename scalar_t>
311  scalar_t* u,
312  scalar_t* v) {
313  // Unless the x and y coords are both close to zero, we can simply take (
314  // -y, x, 0 ) and normalize it.
315  // If both x and y are close to zero, then the vector is close to the
316  // z-axis, so it's far from colinear to the x-axis for instance. So we
317  // take the crossed product with (1,0,0) and normalize it.
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;
324  v[2] = 0;
325  } else {
326  const scalar_t norm2_inv =
327  1.0 / sqrt(query[1] * query[1] + query[2] * query[2]);
328  v[0] = 0;
329  v[1] = -1 * query[2] * norm2_inv;
330  v[2] = query[1] * norm2_inv;
331  }
332 
333  core::linalg::kernel::cross_3x1(query, v, u);
334 }
335 
336 template <typename scalar_t>
337 inline OPEN3D_HOST_DEVICE void Swap(scalar_t* x, scalar_t* y) {
338  scalar_t tmp = *x;
339  *x = *y;
340  *y = tmp;
341 }
342 
343 template <typename scalar_t>
344 inline OPEN3D_HOST_DEVICE void Heapify(scalar_t* arr, int n, int root) {
345  int largest = root;
346  int l = 2 * root + 1;
347  int r = 2 * root + 2;
348 
349  if (l < n && arr[l] > arr[largest]) {
350  largest = l;
351  }
352  if (r < n && arr[r] > arr[largest]) {
353  largest = r;
354  }
355  if (largest != root) {
356  Swap<scalar_t>(&arr[root], &arr[largest]);
357  Heapify<scalar_t>(arr, n, largest);
358  }
359 }
360 
361 template <typename scalar_t>
362 OPEN3D_HOST_DEVICE void HeapSort(scalar_t* arr, int n) {
363  for (int i = n / 2 - 1; i >= 0; i--) Heapify(arr, n, i);
364 
365  for (int i = n - 1; i > 0; i--) {
366  Swap<scalar_t>(&arr[0], &arr[i]);
367  Heapify<scalar_t>(arr, i, 0);
368  }
369 }
370 
371 template <typename scalar_t>
372 OPEN3D_HOST_DEVICE bool IsBoundaryPoints(const scalar_t* angles,
373  int counts,
374  double angle_threshold) {
375  scalar_t diff;
376  scalar_t max_diff = 0;
377  // Compute the maximal angle difference between two consecutive angles.
378  for (int i = 0; i < counts - 1; i++) {
379  diff = angles[i + 1] - angles[i];
380  max_diff = max(max_diff, diff);
381  }
382 
383  // Get the angle difference between the last and the first.
384  diff = 2 * M_PI - angles[counts - 1] + angles[0];
385  max_diff = max(max_diff, diff);
386 
387  return max_diff > angle_threshold * M_PI / 180.0 ? true : false;
388 }
389 
390 #if defined(__CUDACC__)
391 void ComputeBoundaryPointsCUDA
392 #else
394 #endif
395  (const core::Tensor& points,
396  const core::Tensor& normals,
397  const core::Tensor& indices,
398  const core::Tensor& counts,
399  core::Tensor& mask,
400  double angle_threshold) {
401 
402  const int nn_size = indices.GetShape()[1];
403 
404  DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(points.GetDtype(), [&]() {
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>();
411 
412  core::Tensor angles = core::Tensor::Full(
413  indices.GetShape(), -10, points.GetDtype(), points.GetDevice());
414  scalar_t* angles_ptr = angles.GetDataPtr<scalar_t>();
415 
416  core::ParallelFor(
417  points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
418  scalar_t u[3], v[3];
419  GetCoordinateSystemOnPlane(normals_ptr + 3 * workload_idx,
420  u, v);
421 
422  // Ignore the point itself.
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;
428 
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));
437 
438  angles_ptr[idx] = angle;
439  }
440 
441  // Sort the angles in ascending order.
442  HeapSort<scalar_t>(
443  angles_ptr + workload_idx * nn_size + 1,
444  indices_size);
445 
446  mask_ptr[workload_idx] = IsBoundaryPoints<scalar_t>(
447  angles_ptr + workload_idx * nn_size + 1,
448  indices_size, angle_threshold);
449  }
450  });
451  });
452 }
453 
454 // This is a `two-pass` estimate method for covariance which is numerically more
455 // robust than the `textbook` method generally used for covariance computation.
456 template <typename scalar_t>
458  const scalar_t* points_ptr,
459  const int32_t* indices_ptr,
460  const int32_t& indices_count,
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;
472  return;
473  }
474 
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];
481  }
482 
483  centroid[0] /= indices_count;
484  centroid[1] /= indices_count;
485  centroid[2] /= indices_count;
486 
487  // cumulants must always be Float64 to ensure precision.
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];
494 
495  cumulants[0] += x * x;
496  cumulants[1] += y * y;
497  cumulants[2] += z * z;
498 
499  cumulants[3] += x * y;
500  cumulants[4] += x * z;
501  cumulants[5] += y * z;
502  }
503 
504  // Using Bessel's correction (dividing by (n - 1) instead of n).
505  // Refer:
506  // https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance
507  const double normalization_factor = static_cast<double>(indices_count - 1);
508  for (int i = 0; i < 6; ++i) {
509  cumulants[i] /= normalization_factor;
510  }
511 
512  // Covariances(0, 0)
513  covariance_ptr[0] = static_cast<scalar_t>(cumulants[0]);
514  // Covariances(1, 1)
515  covariance_ptr[4] = static_cast<scalar_t>(cumulants[1]);
516  // Covariances(2, 2)
517  covariance_ptr[8] = static_cast<scalar_t>(cumulants[2]);
518 
519  // Covariances(0, 1) = Covariances(1, 0)
520  covariance_ptr[1] = static_cast<scalar_t>(cumulants[3]);
521  covariance_ptr[3] = covariance_ptr[1];
522 
523  // Covariances(0, 2) = Covariances(2, 0)
524  covariance_ptr[2] = static_cast<scalar_t>(cumulants[4]);
525  covariance_ptr[6] = covariance_ptr[2];
526 
527  // Covariances(1, 2) = Covariances(2, 1)
528  covariance_ptr[5] = static_cast<scalar_t>(cumulants[5]);
529  covariance_ptr[7] = covariance_ptr[5];
530 }
531 
532 #if defined(__CUDACC__)
533 void EstimateCovariancesUsingHybridSearchCUDA
534 #else
536 #endif
537  (const core::Tensor& points,
538  core::Tensor& covariances,
539  const double& radius,
540  const int64_t& max_nn) {
541  core::Dtype dtype = points.GetDtype();
542  int64_t n = points.GetLength();
543 
545  bool check = tree.HybridIndex(radius);
546  if (!check) {
547  utility::LogError("Building FixedRadiusIndex failed.");
548  }
549 
550  core::Tensor indices, distance, counts;
551  std::tie(indices, distance, counts) =
552  tree.HybridSearch(points, radius, max_nn);
553 
554  DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
555  const scalar_t* points_ptr = points.GetDataPtr<scalar_t>();
556  int32_t* neighbour_indices_ptr = indices.GetDataPtr<int32_t>();
557  int32_t* neighbour_counts_ptr = counts.GetDataPtr<int32_t>();
558  scalar_t* covariances_ptr = covariances.GetDataPtr<scalar_t>();
559 
561  points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
562  // NNS [Hybrid Search].
563  const int32_t neighbour_offset = max_nn * workload_idx;
564  // Count of valid correspondences per point.
565  const int32_t neighbour_count =
566  neighbour_counts_ptr[workload_idx];
567  // Covariance is of shape {3, 3}, so it has an
568  // offset factor of 9 x workload_idx.
569  const int32_t covariances_offset = 9 * workload_idx;
570 
572  points_ptr,
573  neighbour_indices_ptr + neighbour_offset,
574  neighbour_count,
575  covariances_ptr + covariances_offset);
576  });
577  });
578 
579  core::cuda::Synchronize(points.GetDevice());
580 }
581 
582 #if defined(__CUDACC__)
583 void EstimateCovariancesUsingRadiusSearchCUDA
584 #else
586 #endif
587  (const core::Tensor& points,
588  core::Tensor& covariances,
589  const double& radius) {
590  core::Dtype dtype = points.GetDtype();
591  int64_t n = points.GetLength();
592 
594  bool check = tree.FixedRadiusIndex(radius);
595  if (!check) {
596  utility::LogError("Building Radius-Index failed.");
597  }
598 
599  core::Tensor indices, distance, counts;
600  std::tie(indices, distance, counts) =
601  tree.FixedRadiusSearch(points, radius);
602 
603  DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
604  const scalar_t* points_ptr = points.GetDataPtr<scalar_t>();
605  const int32_t* neighbour_indices_ptr = indices.GetDataPtr<int32_t>();
606  const int32_t* neighbour_counts_ptr = counts.GetDataPtr<int32_t>();
607  scalar_t* covariances_ptr = covariances.GetDataPtr<scalar_t>();
608 
610  points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
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]);
616  // Covariance is of shape {3, 3}, so it has an offset
617  // factor of 9 x workload_idx.
618  const int32_t covariances_offset = 9 * workload_idx;
619 
621  points_ptr,
622  neighbour_indices_ptr + neighbour_offset,
623  neighbour_count,
624  covariances_ptr + covariances_offset);
625  });
626  });
627 
628  core::cuda::Synchronize(points.GetDevice());
629 }
630 
631 #if defined(__CUDACC__)
632 void EstimateCovariancesUsingKNNSearchCUDA
633 #else
635 #endif
636  (const core::Tensor& points,
637  core::Tensor& covariances,
638  const int64_t& max_nn) {
639  core::Dtype dtype = points.GetDtype();
640  int64_t n = points.GetLength();
641 
643  bool check = tree.KnnIndex();
644  if (!check) {
645  utility::LogError("Building KNN-Index failed.");
646  }
647 
648  core::Tensor indices, distance;
649  std::tie(indices, distance) = tree.KnnSearch(points, max_nn);
650 
651  indices = indices.Contiguous();
652  int32_t nn_count = static_cast<int32_t>(indices.GetShape()[1]);
653 
654  if (nn_count < 3) {
656  "Not enough neighbors to compute Covariances / Normals. "
657  "Try "
658  "increasing the max_nn parameter.");
659  }
660 
661  DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
662  auto points_ptr = points.GetDataPtr<scalar_t>();
663  auto neighbour_indices_ptr = indices.GetDataPtr<int32_t>();
664  auto covariances_ptr = covariances.GetDataPtr<scalar_t>();
665 
667  points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
668  // NNS [KNN Search].
669  const int32_t neighbour_offset = nn_count * workload_idx;
670  // Covariance is of shape {3, 3}, so it has an offset
671  // factor of 9 x workload_idx.
672  const int32_t covariances_offset = 9 * workload_idx;
673 
675  points_ptr,
676  neighbour_indices_ptr + neighbour_offset, nn_count,
677  covariances_ptr + covariances_offset);
678  });
679  });
680 
681  core::cuda::Synchronize(points.GetDevice());
682 }
683 
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};
691 
692  scalar_t r0xr1[3], r0xr2[3], r1xr2[3];
693 
694  core::linalg::kernel::cross_3x1(row0, row1, r0xr1);
695  core::linalg::kernel::cross_3x1(row0, row2, r0xr2);
696  core::linalg::kernel::cross_3x1(row1, row2, r1xr2);
697 
698  scalar_t d0 = core::linalg::kernel::dot_3x1(r0xr1, r0xr1);
699  scalar_t d1 = core::linalg::kernel::dot_3x1(r0xr2, r0xr2);
700  scalar_t d2 = core::linalg::kernel::dot_3x1(r1xr2, r1xr2);
701 
702  scalar_t dmax = d0;
703  int imax = 0;
704  if (d1 > dmax) {
705  dmax = d1;
706  imax = 1;
707  }
708  if (d2 > dmax) {
709  imax = 2;
710  }
711 
712  if (imax == 0) {
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;
717  return;
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;
723  return;
724  } else {
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;
729  return;
730  }
731 }
732 
733 template <typename scalar_t>
735  const scalar_t* evec0,
736  const scalar_t eval1,
737  scalar_t* eigen_vector1) {
738  scalar_t U[3];
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;
743  U[1] = 0.0;
744  U[2] = evec0[0] * inv_length;
745  } else {
746  scalar_t inv_length =
747  1.0 / sqrt(evec0[1] * evec0[1] + evec0[2] * evec0[2]);
748  U[0] = 0.0;
749  U[1] = evec0[2] * inv_length;
750  U[2] = -evec0[1] * inv_length;
751  }
752  scalar_t V[3], AU[3], AV[3];
753  core::linalg::kernel::cross_3x1(evec0, U, V);
754  core::linalg::kernel::matmul3x3_3x1(A, U, AU);
755  core::linalg::kernel::matmul3x3_3x1(A, V, AV);
756 
757  scalar_t m00 = core::linalg::kernel::dot_3x1(U, AU) - eval1;
758  scalar_t m01 = core::linalg::kernel::dot_3x1(U, AV);
759  scalar_t m11 = core::linalg::kernel::dot_3x1(V, AV) - eval1;
760 
761  scalar_t absM00 = abs(m00);
762  scalar_t absM01 = abs(m01);
763  scalar_t absM11 = abs(m11);
764  scalar_t max_abs_comp;
765 
766  if (absM00 >= absM11) {
767  max_abs_comp = max(absM00, absM01);
768  if (max_abs_comp > 0) {
769  if (absM00 >= absM01) {
770  m01 /= m00;
771  m00 = 1 / sqrt(1 + m01 * m01);
772  m01 *= m00;
773  } else {
774  m00 /= m01;
775  m01 = 1 / sqrt(1 + m00 * m00);
776  m00 *= m01;
777  }
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];
781  return;
782  } else {
783  eigen_vector1[0] = U[0];
784  eigen_vector1[1] = U[1];
785  eigen_vector1[2] = U[2];
786  return;
787  }
788  } else {
789  max_abs_comp = max(absM11, absM01);
790  if (max_abs_comp > 0) {
791  if (absM11 >= absM01) {
792  m01 /= m11;
793  m11 = 1 / sqrt(1 + m01 * m01);
794  m01 *= m11;
795  } else {
796  m11 /= m01;
797  m01 = 1 / sqrt(1 + m11 * m11);
798  m11 *= m01;
799  }
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];
803  return;
804  } else {
805  eigen_vector1[0] = U[0];
806  eigen_vector1[1] = U[1];
807  eigen_vector1[2] = U[2];
808  return;
809  }
810  }
811 }
812 
813 template <typename scalar_t>
815  const scalar_t* covariance_ptr, scalar_t* normals_ptr) {
816  // Based on:
817  // https://www.geometrictools.com/Documentation/RobustEigenSymmetric3x3.pdf
818  // which handles edge cases like points on a plane.
819  scalar_t max_coeff = covariance_ptr[0];
820 
821  for (int i = 1; i < 9; ++i) {
822  if (max_coeff < covariance_ptr[i]) {
823  max_coeff = covariance_ptr[i];
824  }
825  }
826 
827  if (max_coeff == 0) {
828  normals_ptr[0] = 0.0;
829  normals_ptr[1] = 0.0;
830  normals_ptr[2] = 0.0;
831  return;
832  }
833 
834  scalar_t A[9] = {0};
835 
836  for (int i = 0; i < 9; ++i) {
837  A[i] = covariance_ptr[i] / max_coeff;
838  }
839 
840  scalar_t norm = A[1] * A[1] + A[2] * A[2] + A[5] * A[5];
841 
842  if (norm > 0) {
843  scalar_t eval[3];
844  scalar_t evec0[3];
845  scalar_t evec1[3];
846  scalar_t evec2[3];
847 
848  scalar_t q = (A[0] + A[4] + A[8]) / 3.0;
849 
850  scalar_t b00 = A[0] - q;
851  scalar_t b11 = A[4] - q;
852  scalar_t b22 = A[8] - q;
853 
854  scalar_t p =
855  sqrt((b00 * b00 + b11 * b11 + b22 * b22 + norm * 2.0) / 6.0);
856 
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);
861 
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));
865 
866  scalar_t angle = acos(half_det) / 3.0;
867  const scalar_t two_thrids_pi = 2.09439510239319549;
868 
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);
872 
873  eval[0] = q + p * beta0;
874  eval[1] = q + p * beta1;
875  eval[2] = q + p * beta2;
876 
877  if (half_det >= 0) {
878  ComputeEigenvector0<scalar_t>(A, eval[2], evec2);
879 
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];
884 
885  return;
886  }
887 
888  ComputeEigenvector1<scalar_t>(A, evec2, eval[1], evec1);
889 
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];
894 
895  return;
896  }
897 
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];
901 
902  return;
903  } else {
904  ComputeEigenvector0<scalar_t>(A, eval[0], evec0);
905 
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];
910  return;
911  }
912 
913  ComputeEigenvector1<scalar_t>(A, evec0, eval[1], evec1);
914 
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];
919  return;
920  }
921 
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];
925  return;
926  }
927  } else {
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;
933  return;
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;
939  return;
940  } else {
941  normals_ptr[0] = 0.0;
942  normals_ptr[1] = 0.0;
943  normals_ptr[2] = 1.0;
944  return;
945  }
946  }
947 }
948 
949 #if defined(__CUDACC__)
950 void EstimateNormalsFromCovariancesCUDA
951 #else
953 #endif
954  (const core::Tensor& covariances,
955  core::Tensor& normals,
956  const bool has_normals) {
957  core::Dtype dtype = covariances.GetDtype();
958  int64_t n = covariances.GetLength();
959 
960  DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
961  const scalar_t* covariances_ptr = covariances.GetDataPtr<scalar_t>();
962  scalar_t* normals_ptr = normals.GetDataPtr<scalar_t>();
963 
965  covariances.GetDevice(), n,
966  [=] OPEN3D_DEVICE(int64_t workload_idx) {
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,
972  normals_output);
973 
974  if ((normals_output[0] * normals_output[0] +
975  normals_output[1] * normals_output[1] +
976  normals_output[2] * normals_output[2]) == 0.0 &&
977  !has_normals) {
978  normals_output[0] = 0.0;
979  normals_output[1] = 0.0;
980  normals_output[2] = 1.0;
981  }
982  if (has_normals) {
983  if ((normals_ptr[normals_offset] * normals_output[0] +
984  normals_ptr[normals_offset + 1] *
985  normals_output[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;
991  }
992  }
993 
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];
997  });
998  });
999 
1000  core::cuda::Synchronize(covariances.GetDevice());
1001 }
1002 
1003 template <typename scalar_t>
1005  const scalar_t* points_ptr,
1006  const scalar_t* normals_ptr,
1007  const scalar_t* colors_ptr,
1008  const int32_t& idx_offset,
1009  const int32_t* indices_ptr,
1010  const int32_t& indices_count,
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;
1016  } else {
1017  scalar_t vt[3] = {points_ptr[idx_offset], points_ptr[idx_offset + 1],
1018  points_ptr[idx_offset + 2]};
1019 
1020  scalar_t nt[3] = {normals_ptr[idx_offset], normals_ptr[idx_offset + 1],
1021  normals_ptr[idx_offset + 2]};
1022 
1023  scalar_t it = (colors_ptr[idx_offset] + colors_ptr[idx_offset + 1] +
1024  colors_ptr[idx_offset + 2]) /
1025  3.0;
1026 
1027  scalar_t AtA[9] = {0};
1028  scalar_t Atb[3] = {0};
1029 
1030  // approximate image gradient of vt's tangential plane
1031  // projection (p') of a point p on a plane defined by
1032  // normal n, where o is the closest point to p on the
1033  // plane, is given by:
1034  // p' = p - [(p - o).dot(n)] * n p'
1035  // => p - [(p.dot(n) - s)] * n [where s = o.dot(n)]
1036 
1037  // Computing the scalar s.
1038  scalar_t s = vt[0] * nt[0] + vt[1] * nt[1] + vt[2] * nt[2];
1039 
1040  int i = 1;
1041  for (; i < indices_count; i++) {
1042  int64_t neighbour_idx_offset = 3 * indices_ptr[i];
1043 
1044  if (neighbour_idx_offset == -1) {
1045  break;
1046  }
1047 
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]};
1051 
1052  // p' = p - d * n [where d = p.dot(n) - s]
1053  // Computing the scalar d.
1054  scalar_t d = vt_adj[0] * nt[0] + vt_adj[1] * nt[1] +
1055  vt_adj[2] * nt[2] - s;
1056 
1057  // Computing the p' (projection of the point).
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]};
1060 
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]) /
1064  3.0;
1065 
1066  scalar_t A[3] = {vt_proj[0] - vt[0], vt_proj[1] - vt[1],
1067  vt_proj[2] - vt[2]};
1068 
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];
1075 
1076  scalar_t b = it_adj - it;
1077 
1078  Atb[0] += A[0] * b;
1079  Atb[1] += A[1] * b;
1080  Atb[2] += A[2] * b;
1081  }
1082 
1083  // Orthogonal constraint.
1084  scalar_t A[3] = {(i - 1) * nt[0], (i - 1) * nt[1], (i - 1) * nt[2]};
1085 
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];
1092 
1093  // Symmetry.
1094  AtA[3] = AtA[1];
1095  AtA[6] = AtA[2];
1096  AtA[7] = AtA[5];
1097 
1099  color_gradients_ptr + idx_offset);
1100  }
1101 }
1102 
1103 #if defined(__CUDACC__)
1104 void EstimateColorGradientsUsingHybridSearchCUDA
1105 #else
1107 #endif
1108  (const core::Tensor& points,
1109  const core::Tensor& normals,
1110  const core::Tensor& colors,
1111  core::Tensor& color_gradients,
1112  const double& radius,
1113  const int64_t& max_nn) {
1114  core::Dtype dtype = points.GetDtype();
1115  int64_t n = points.GetLength();
1116 
1118 
1119  bool check = tree.HybridIndex(radius);
1120  if (!check) {
1121  utility::LogError("NearestNeighborSearch::HybridIndex is not set.");
1122  }
1123 
1124  core::Tensor indices, distance, counts;
1125  std::tie(indices, distance, counts) =
1126  tree.HybridSearch(points, radius, max_nn);
1127 
1128  DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
1129  auto points_ptr = points.GetDataPtr<scalar_t>();
1130  auto normals_ptr = normals.GetDataPtr<scalar_t>();
1131  auto colors_ptr = colors.GetDataPtr<scalar_t>();
1132  auto neighbour_indices_ptr = indices.GetDataPtr<int32_t>();
1133  auto neighbour_counts_ptr = counts.GetDataPtr<int32_t>();
1134  auto color_gradients_ptr = color_gradients.GetDataPtr<scalar_t>();
1135 
1137  points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
1138  // NNS [Hybrid Search].
1139  int32_t neighbour_offset = max_nn * workload_idx;
1140  // Count of valid correspondences per point.
1141  int32_t neighbour_count =
1142  neighbour_counts_ptr[workload_idx];
1143  int32_t idx_offset = 3 * workload_idx;
1144 
1146  points_ptr, normals_ptr, colors_ptr, idx_offset,
1147  neighbour_indices_ptr + neighbour_offset,
1148  neighbour_count, color_gradients_ptr);
1149  });
1150  });
1151 
1152  core::cuda::Synchronize(points.GetDevice());
1153 }
1154 
1155 #if defined(__CUDACC__)
1156 void EstimateColorGradientsUsingKNNSearchCUDA
1157 #else
1159 #endif
1160  (const core::Tensor& points,
1161  const core::Tensor& normals,
1162  const core::Tensor& colors,
1163  core::Tensor& color_gradients,
1164  const int64_t& max_nn) {
1165  core::Dtype dtype = points.GetDtype();
1166  int64_t n = points.GetLength();
1167 
1169 
1170  bool check = tree.KnnIndex();
1171  if (!check) {
1172  utility::LogError("KnnIndex is not set.");
1173  }
1174 
1175  core::Tensor indices, distance;
1176  std::tie(indices, distance) = tree.KnnSearch(points, max_nn);
1177 
1178  indices = indices.To(core::Int32).Contiguous();
1179  int64_t nn_count = indices.GetShape()[1];
1180 
1181  if (nn_count < 4) {
1183  "Not enough neighbors to compute Covariances / Normals. "
1184  "Try "
1185  "changing the search parameter.");
1186  }
1187 
1188  DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
1189  auto points_ptr = points.GetDataPtr<scalar_t>();
1190  auto normals_ptr = normals.GetDataPtr<scalar_t>();
1191  auto colors_ptr = colors.GetDataPtr<scalar_t>();
1192  auto neighbour_indices_ptr = indices.GetDataPtr<int32_t>();
1193  auto color_gradients_ptr = color_gradients.GetDataPtr<scalar_t>();
1194 
1196  points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
1197  int32_t neighbour_offset = max_nn * workload_idx;
1198  int32_t idx_offset = 3 * workload_idx;
1199 
1201  points_ptr, normals_ptr, colors_ptr, idx_offset,
1202  neighbour_indices_ptr + neighbour_offset, nn_count,
1203  color_gradients_ptr);
1204  });
1205  });
1206 
1207  core::cuda::Synchronize(points.GetDevice());
1208 }
1209 
1210 #if defined(__CUDACC__)
1211 void EstimateColorGradientsUsingRadiusSearchCUDA
1212 #else
1214 #endif
1215  (const core::Tensor& points,
1216  const core::Tensor& normals,
1217  const core::Tensor& colors,
1218  core::Tensor& color_gradients,
1219  const double& radius) {
1220  core::Dtype dtype = points.GetDtype();
1221  int64_t n = points.GetLength();
1222 
1224 
1225  bool check = tree.FixedRadiusIndex(radius);
1226  if (!check) {
1227  utility::LogError("RadiusIndex is not set.");
1228  }
1229 
1230  core::Tensor indices, distance, counts;
1231  std::tie(indices, distance, counts) =
1232  tree.FixedRadiusSearch(points, radius);
1233 
1234  indices = indices.To(core::Int32).Contiguous();
1235  counts = counts.Contiguous();
1236 
1237  DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
1238  auto points_ptr = points.GetDataPtr<scalar_t>();
1239  auto normals_ptr = normals.GetDataPtr<scalar_t>();
1240  auto colors_ptr = colors.GetDataPtr<scalar_t>();
1241  auto neighbour_indices_ptr = indices.GetDataPtr<int32_t>();
1242  auto neighbour_counts_ptr = counts.GetDataPtr<int32_t>();
1243  auto color_gradients_ptr = color_gradients.GetDataPtr<scalar_t>();
1244 
1246  points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
1247  int32_t neighbour_offset =
1248  neighbour_counts_ptr[workload_idx];
1249  // Count of valid correspondences per point.
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;
1254 
1256  points_ptr, normals_ptr, colors_ptr, idx_offset,
1257  neighbour_indices_ptr + neighbour_offset,
1258  neighbour_count, color_gradients_ptr);
1259  });
1260  });
1261 
1262  core::cuda::Synchronize(points.GetDevice());
1263 }
1264 
1265 } // namespace pointcloud
1266 } // namespace kernel
1267 } // namespace geometry
1268 } // namespace t
1269 } // 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
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
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: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