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