Open3D (C++ API)  0.18.0+252c867
FeatureImpl.h
Go to the documentation of this file.
1 // ----------------------------------------------------------------------------
2 // - Open3D: www.open3d.org -
3 // ----------------------------------------------------------------------------
4 // Copyright (c) 2018-2023 www.open3d.org
5 // SPDX-License-Identifier: MIT
6 // ----------------------------------------------------------------------------
7 
9 #include "open3d/core/Dispatch.h"
13 
14 namespace open3d {
15 namespace t {
16 namespace pipelines {
17 namespace kernel {
18 
19 #ifndef __CUDACC__
20 using std::max;
21 using std::min;
22 #endif
23 
24 template <typename scalar_t>
25 OPEN3D_HOST_DEVICE void ComputePairFeature(const scalar_t *p1,
26  const scalar_t *n1,
27  const scalar_t *p2,
28  const scalar_t *n2,
29  scalar_t *feature) {
30  scalar_t dp2p1[3], n1_copy[3], n2_copy[3];
31  dp2p1[0] = p2[0] - p1[0];
32  dp2p1[1] = p2[1] - p1[1];
33  dp2p1[2] = p2[2] - p1[2];
34  feature[3] = sqrt(dp2p1[0] * dp2p1[0] + dp2p1[1] * dp2p1[1] +
35  dp2p1[2] * dp2p1[2]);
36  if (feature[3] == 0) {
37  feature[0] = 0;
38  feature[1] = 0;
39  feature[2] = 0;
40  feature[3] = 0;
41  return;
42  }
43 
44  scalar_t angle1 = core::linalg::kernel::dot_3x1(n1, dp2p1) / feature[3];
45  scalar_t angle2 = core::linalg::kernel::dot_3x1(n2, dp2p1) / feature[3];
46  if (acos(fabs(angle1)) > acos(fabs(angle2))) {
47  n1_copy[0] = n2[0];
48  n1_copy[1] = n2[1];
49  n1_copy[2] = n2[2];
50  n2_copy[0] = n1[0];
51  n2_copy[1] = n1[1];
52  n2_copy[2] = n1[2];
53  dp2p1[0] *= -1;
54  dp2p1[1] *= -1;
55  dp2p1[2] *= -1;
56  feature[2] = -angle2;
57  } else {
58  n1_copy[0] = n1[0];
59  n1_copy[1] = n1[1];
60  n1_copy[2] = n1[2];
61  n2_copy[0] = n2[0];
62  n2_copy[1] = n2[1];
63  n2_copy[2] = n2[2];
64  feature[2] = angle1;
65  }
66 
67  scalar_t v[3];
68  core::linalg::kernel::cross_3x1(dp2p1, n1_copy, v);
69  const scalar_t v_norm = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
70  if (v_norm == 0.0) {
71  feature[0] = 0.0;
72  feature[1] = 0.0;
73  feature[2] = 0.0;
74  feature[3] = 0.0;
75  return;
76  }
77  v[0] /= v_norm;
78  v[1] /= v_norm;
79  v[2] /= v_norm;
80  scalar_t w[3];
81  core::linalg::kernel::cross_3x1(n1_copy, v, w);
82  feature[1] = core::linalg::kernel::dot_3x1(v, n2_copy);
83  feature[0] = atan2(core::linalg::kernel::dot_3x1(w, n2_copy),
84  core::linalg::kernel::dot_3x1(n1_copy, n2_copy));
85 }
86 
87 template <typename scalar_t>
88 OPEN3D_HOST_DEVICE void UpdateSPFHFeature(const scalar_t *feature,
89  int64_t idx,
90  scalar_t hist_incr,
91  scalar_t *spfh) {
92  int h_index1 =
93  static_cast<int>(floor(11 * (feature[0] + M_PI) / (2.0 * M_PI)));
94  h_index1 = h_index1 >= 11 ? 10 : max(0, h_index1);
95 
96  int h_index2 = static_cast<int>(floor(11 * (feature[1] + 1.0) * 0.5));
97  h_index2 = h_index2 >= 11 ? 10 : max(0, h_index2);
98 
99  int h_index3 = static_cast<int>(floor(11 * (feature[2] + 1.0) * 0.5));
100  h_index3 = h_index3 >= 11 ? 10 : max(0, h_index3);
101 
102  spfh[idx * 33 + h_index1] += hist_incr;
103  spfh[idx * 33 + h_index2 + 11] += hist_incr;
104  spfh[idx * 33 + h_index3 + 22] += hist_incr;
105 }
106 
107 #if defined(__CUDACC__)
108 void ComputeFPFHFeatureCUDA
109 #else
111 #endif
112  (const core::Tensor &points,
113  const core::Tensor &normals,
114  const core::Tensor &indices,
115  const core::Tensor &distance2,
116  const core::Tensor &counts,
117  core::Tensor &fpfhs) {
118  const core::Dtype dtype = points.GetDtype();
119  const int64_t n = points.GetLength();
120 
121  core::Tensor spfhs = fpfhs.Clone();
122 
123  // Check the nns type (knn = hybrid = false, radius = true).
124  // The nns radius search mode will resulting a prefix sum 1D tensor.
125  bool is_radius_search;
126  int nn_size = 0;
127  if (indices.GetShape().size() == 1) {
128  is_radius_search = true;
129  } else {
130  is_radius_search = false;
131  nn_size = indices.GetShape()[1];
132  }
133 
134  DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() {
135  const scalar_t *points_ptr = points.GetDataPtr<scalar_t>();
136  const scalar_t *normals_ptr = normals.GetDataPtr<scalar_t>();
137  const int32_t *indices_ptr = indices.GetDataPtr<int32_t>();
138  const scalar_t *distance2_ptr = distance2.GetDataPtr<scalar_t>();
139  const int32_t *counts_ptr = counts.GetDataPtr<int32_t>();
140  scalar_t *spfhs_ptr = spfhs.GetDataPtr<scalar_t>();
141  scalar_t *fpfhs_ptr = fpfhs.GetDataPtr<scalar_t>();
142 
143  // Compute SPFH features for each point.
145  points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
146  int64_t idx = 3 * workload_idx;
147  const scalar_t *point = points_ptr + idx;
148  const scalar_t *normal = normals_ptr + idx;
149 
150  const int indice_size =
151  is_radius_search ? (counts_ptr[workload_idx + 1] -
152  counts_ptr[workload_idx])
153  : counts_ptr[workload_idx];
154 
155  if (indice_size > 1) {
156  const scalar_t hist_incr =
157  100.0 / static_cast<scalar_t>(indice_size - 1);
158  for (int i = 1; i < indice_size; i++) {
159  const int point_idx =
160  is_radius_search
161  ? indices_ptr
162  [i +
163  counts_ptr[workload_idx]]
164  : indices_ptr[workload_idx *
165  nn_size +
166  i];
167 
168  const scalar_t *point_ref =
169  points_ptr + 3 * point_idx;
170  const scalar_t *normal_ref =
171  normals_ptr + 3 * point_idx;
172  scalar_t fea[4] = {0};
173  ComputePairFeature<scalar_t>(
174  point, normal, point_ref, normal_ref, fea);
175  UpdateSPFHFeature<scalar_t>(fea, workload_idx,
176  hist_incr, spfhs_ptr);
177  }
178  }
179  });
180 
181  // Compute FPFH features for each point.
183  points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) {
184  const int indice_size =
185  is_radius_search ? (counts_ptr[workload_idx + 1] -
186  counts_ptr[workload_idx])
187  : counts_ptr[workload_idx];
188 
189  if (indice_size > 1) {
190  scalar_t sum[3] = {0.0, 0.0, 0.0};
191  for (int i = 1; i < indice_size; i++) {
192  const int idx =
193  is_radius_search
194  ? i + counts_ptr[workload_idx]
195  : workload_idx * nn_size + i;
196  const scalar_t dist = distance2_ptr[idx];
197  if (dist == 0.0) continue;
198 
199  for (int j = 0; j < 33; j++) {
200  const scalar_t val =
201  spfhs_ptr[indices_ptr[idx] * 33 + j] /
202  dist;
203  sum[j / 11] += val;
204  fpfhs_ptr[workload_idx * 33 + j] += val;
205  }
206  }
207  for (int j = 0; j < 3; j++) {
208  sum[j] = sum[j] != 0.0 ? 100.0 / sum[j] : 0.0;
209  }
210  for (int j = 0; j < 33; j++) {
211  fpfhs_ptr[workload_idx * 33 + j] *= sum[j / 11];
212  fpfhs_ptr[workload_idx * 33 + j] +=
213  spfhs_ptr[workload_idx * 33 + j];
214  }
215  }
216  });
217  });
218 } // namespace kernel
219 
220 } // namespace kernel
221 } // namespace pipelines
222 } // namespace t
223 } // namespace open3d
Common CUDA utilities.
#define OPEN3D_HOST_DEVICE
Definition: CUDAUtils.h:44
#define OPEN3D_DEVICE
Definition: CUDAUtils.h:45
#define DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(DTYPE,...)
Definition: Dispatch.h:77
Definition: Dtype.h:20
Definition: Tensor.h:32
Tensor Clone() const
Copy Tensor to the same device.
Definition: Tensor.h:501
T * GetDataPtr()
Definition: Tensor.h:1143
SizeVector GetShape() const
Definition: Tensor.h:1126
int points
Definition: FilePCD.cpp:54
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_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
void ParallelFor(const Device &device, int64_t n, const func_t &func)
Definition: ParallelFor.h:103
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:395
OPEN3D_HOST_DEVICE void ComputePairFeature(const scalar_t *p1, const scalar_t *n1, const scalar_t *p2, const scalar_t *n2, scalar_t *feature)
Definition: FeatureImpl.h:25
OPEN3D_HOST_DEVICE void UpdateSPFHFeature(const scalar_t *feature, int64_t idx, scalar_t hist_incr, scalar_t *spfh)
Definition: FeatureImpl.h:88
void ComputeFPFHFeatureCPU(const core::Tensor &points, const core::Tensor &normals, const core::Tensor &indices, const core::Tensor &distance2, const core::Tensor &counts, core::Tensor &fpfhs)
Definition: FeatureImpl.h:112
FN_SPECIFIERS MiniVec< float, N > floor(const MiniVec< float, N > &a)
Definition: MiniVec.h:75
Definition: PinholeCameraIntrinsic.cpp:16