77 memset (coefficients, 0,
sizeof (
float) * 21);
79 for (
const auto &neighbor : neighbors)
81 if (std::isfinite ((*normals_)[neighbor].normal_x) && std::isfinite ((*intensity_gradients_)[neighbor].gradient [0]))
83 coefficients[ 0] += (*normals_)[neighbor].normal_x * (*normals_)[neighbor].normal_x;
84 coefficients[ 1] += (*normals_)[neighbor].normal_x * (*normals_)[neighbor].normal_y;
85 coefficients[ 2] += (*normals_)[neighbor].normal_x * (*normals_)[neighbor].normal_z;
86 coefficients[ 3] += (*normals_)[neighbor].normal_x * (*intensity_gradients_)[neighbor].gradient [0];
87 coefficients[ 4] += (*normals_)[neighbor].normal_x * (*intensity_gradients_)[neighbor].gradient [1];
88 coefficients[ 5] += (*normals_)[neighbor].normal_x * (*intensity_gradients_)[neighbor].gradient [2];
90 coefficients[ 6] += (*normals_)[neighbor].normal_y * (*normals_)[neighbor].normal_y;
91 coefficients[ 7] += (*normals_)[neighbor].normal_y * (*normals_)[neighbor].normal_z;
92 coefficients[ 8] += (*normals_)[neighbor].normal_y * (*intensity_gradients_)[neighbor].gradient [0];
93 coefficients[ 9] += (*normals_)[neighbor].normal_y * (*intensity_gradients_)[neighbor].gradient [1];
94 coefficients[10] += (*normals_)[neighbor].normal_y * (*intensity_gradients_)[neighbor].gradient [2];
96 coefficients[11] += (*normals_)[neighbor].normal_z * (*normals_)[neighbor].normal_z;
97 coefficients[12] += (*normals_)[neighbor].normal_z * (*intensity_gradients_)[neighbor].gradient [0];
98 coefficients[13] += (*normals_)[neighbor].normal_z * (*intensity_gradients_)[neighbor].gradient [1];
99 coefficients[14] += (*normals_)[neighbor].normal_z * (*intensity_gradients_)[neighbor].gradient [2];
101 coefficients[15] += (*intensity_gradients_)[neighbor].gradient [0] * (*intensity_gradients_)[neighbor].gradient [0];
102 coefficients[16] += (*intensity_gradients_)[neighbor].gradient [0] * (*intensity_gradients_)[neighbor].gradient [1];
103 coefficients[17] += (*intensity_gradients_)[neighbor].gradient [0] * (*intensity_gradients_)[neighbor].gradient [2];
105 coefficients[18] += (*intensity_gradients_)[neighbor].gradient [1] * (*intensity_gradients_)[neighbor].gradient [1];
106 coefficients[19] += (*intensity_gradients_)[neighbor].gradient [1] * (*intensity_gradients_)[neighbor].gradient [2];
108 coefficients[20] += (*intensity_gradients_)[neighbor].gradient [2] * (*intensity_gradients_)[neighbor].gradient [2];
115 float norm = 1.0 /
float (count);
116 coefficients[ 0] *= norm;
117 coefficients[ 1] *= norm;
118 coefficients[ 2] *= norm;
119 coefficients[ 3] *= norm;
120 coefficients[ 4] *= norm;
121 coefficients[ 5] *= norm;
122 coefficients[ 6] *= norm;
123 coefficients[ 7] *= norm;
124 coefficients[ 8] *= norm;
125 coefficients[ 9] *= norm;
126 coefficients[10] *= norm;
127 coefficients[11] *= norm;
128 coefficients[12] *= norm;
129 coefficients[13] *= norm;
130 coefficients[14] *= norm;
131 coefficients[15] *= norm;
132 coefficients[16] *= norm;
133 coefficients[17] *= norm;
134 coefficients[18] *= norm;
135 coefficients[19] *= norm;
136 coefficients[20] *= norm;
144 if (normals_->empty ())
146 normals_->reserve (surface_->size ());
147 if (!surface_->isOrganized ())
165 cloud->resize (surface_->size ());
166#pragma omp parallel for \
168 num_threads(threads_)
169 for (
unsigned idx = 0; idx < surface_->size (); ++idx)
171 cloud->points [idx].x = surface_->points [idx].x;
172 cloud->points [idx].y = surface_->points [idx].y;
173 cloud->points [idx].z = surface_->points [idx].z;
176 cloud->points [idx].intensity = 0.00390625 * (0.114 *
float(surface_->points [idx].b) + 0.5870 *
float(surface_->points [idx].g) + 0.2989 *
float(surface_->points [idx].r));
182 grad_est.setInputNormals (normals_);
183 grad_est.setRadiusSearch (search_radius_);
184 grad_est.compute (*intensity_gradients_);
186#pragma omp parallel for \
188 num_threads(threads_)
189 for (std::size_t idx = 0; idx < intensity_gradients_->size (); ++idx)
191 float len = intensity_gradients_->points [idx].gradient_x * intensity_gradients_->points [idx].gradient_x +
192 intensity_gradients_->points [idx].gradient_y * intensity_gradients_->points [idx].gradient_y +
193 intensity_gradients_->points [idx].gradient_z * intensity_gradients_->points [idx].gradient_z ;
199 intensity_gradients_->points [idx].gradient_x *=
len;
200 intensity_gradients_->points [idx].gradient_y *=
len;
201 intensity_gradients_->points [idx].gradient_z *=
len;
205 intensity_gradients_->points [idx].gradient_x = 0;
206 intensity_gradients_->points [idx].gradient_y = 0;
207 intensity_gradients_->points [idx].gradient_z = 0;
212 response->points.reserve (input_->size());
213 responseTomasi(*response);
220 output.is_dense = input_->is_dense;
221 for (std::size_t i = 0; i < response->size (); ++i)
222 keypoints_indices_->indices.push_back (i);
227 output.reserve (response->size());
229#pragma omp parallel for \
231 num_threads(threads_)
232 for (std::size_t idx = 0; idx < response->size (); ++idx)
234 if (!
isFinite ((*response)[idx]) || (*response)[idx].intensity < threshold_)
243 if ((*response)[idx].intensity < (*response)[index].intensity)
252 output.push_back ((*response)[idx]);
253 keypoints_indices_->indices.push_back (idx);
272 Eigen::SelfAdjointEigenSolver <Eigen::Matrix<float, 6, 6> >
solver;
273 Eigen::Matrix<float, 6, 6> covariance;
275#pragma omp parallel for \
277 firstprivate(pointOut, covar, covariance, solver) \
278 num_threads(threads_)
293 covariance.coeffRef ( 0) =
covar [ 0];
294 covariance.coeffRef ( 1) =
covar [ 1];
295 covariance.coeffRef ( 2) =
covar [ 2];
296 covariance.coeffRef ( 3) =
covar [ 3];
297 covariance.coeffRef ( 4) =
covar [ 4];
298 covariance.coeffRef ( 5) =
covar [ 5];
300 covariance.coeffRef ( 7) =
covar [ 6];
301 covariance.coeffRef ( 8) =
covar [ 7];
302 covariance.coeffRef ( 9) =
covar [ 8];
303 covariance.coeffRef (10) =
covar [ 9];
304 covariance.coeffRef (11) =
covar [10];
306 covariance.coeffRef (14) =
covar [11];
307 covariance.coeffRef (15) =
covar [12];
308 covariance.coeffRef (16) =
covar [13];
309 covariance.coeffRef (17) =
covar [14];
311 covariance.coeffRef (21) =
covar [15];
312 covariance.coeffRef (22) =
covar [16];
313 covariance.coeffRef (23) =
covar [17];
315 covariance.coeffRef (28) =
covar [18];
316 covariance.coeffRef (29) =
covar [19];
318 covariance.coeffRef (35) =
covar [20];
320 covariance.coeffRef ( 6) =
covar [ 1];
322 covariance.coeffRef (12) =
covar [ 2];
323 covariance.coeffRef (13) =
covar [ 7];
325 covariance.coeffRef (18) =
covar [ 3];
326 covariance.coeffRef (19) =
covar [ 8];
327 covariance.coeffRef (20) =
covar [12];
329 covariance.coeffRef (24) =
covar [ 4];
330 covariance.coeffRef (25) =
covar [ 9];
331 covariance.coeffRef (26) =
covar [13];
332 covariance.coeffRef (27) =
covar [16];
334 covariance.coeffRef (30) =
covar [ 5];
335 covariance.coeffRef (31) =
covar [10];
336 covariance.coeffRef (32) =
covar [14];
337 covariance.coeffRef (33) =
covar [17];
338 covariance.coeffRef (34) =
covar [19];
340 solver.compute (covariance);
352 output.height = input_->height;
353 output.width = input_->width;
int radiusSearch(const PointT &point, double radius, Indices &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const override
Search for all the nearest neighbors of the query point in a given radius.