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] +
36 if (feature[3] == 0) {
46 if (acos(fabs(angle1)) > acos(fabs(angle2))) {
69 const scalar_t v_norm = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
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);
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);
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);
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;
119 const int64_t n =
points.GetLength();
125 bool is_radius_search;
127 if (indices.GetShape().size() == 1) {
128 is_radius_search =
true;
130 is_radius_search =
false;
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>();
146 int64_t idx = 3 * workload_idx;
147 const scalar_t *point = points_ptr + idx;
148 const scalar_t *normal = normals_ptr + idx;
150 const int indice_size =
151 is_radius_search ? (counts_ptr[workload_idx + 1] -
152 counts_ptr[workload_idx])
153 : counts_ptr[workload_idx];
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 =
163 counts_ptr[workload_idx]]
164 : indices_ptr[workload_idx *
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);
184 const int indice_size =
185 is_radius_search ? (counts_ptr[workload_idx + 1] -
186 counts_ptr[workload_idx])
187 : counts_ptr[workload_idx];
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++) {
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;
199 for (
int j = 0; j < 33; j++) {
201 spfhs_ptr[indices_ptr[idx] * 33 + j] /
204 fpfhs_ptr[workload_idx * 33 + j] += val;
207 for (
int j = 0; j < 3; j++) {
208 sum[j] = sum[j] != 0.0 ? 100.0 / sum[j] : 0.0;
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];
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