38 #ifndef PCL_HARRIS_KEYPOINT_6D_IMPL_H_
39 #define PCL_HARRIS_KEYPOINT_6D_IMPL_H_
41 #include <pcl/keypoints/harris_6d.h>
42 #include <pcl/common/io.h>
43 #include <pcl/features/normal_3d.h>
45 #include <pcl/features/intensity_gradient.h>
46 #include <pcl/features/integral_image_normal.h>
48 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
51 threshold_= threshold;
54 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
57 search_radius_ = radius;
60 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
66 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
73 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
76 memset (coefficients, 0,
sizeof (
float) * 21);
78 for (
const int &neighbor : neighbors)
80 if (std::isfinite ((*normals_)[neighbor].normal_x) && std::isfinite ((*intensity_gradients_)[neighbor].gradient [0]))
82 coefficients[ 0] += (*normals_)[neighbor].normal_x * (*normals_)[neighbor].normal_x;
83 coefficients[ 1] += (*normals_)[neighbor].normal_x * (*normals_)[neighbor].normal_y;
84 coefficients[ 2] += (*normals_)[neighbor].normal_x * (*normals_)[neighbor].normal_z;
85 coefficients[ 3] += (*normals_)[neighbor].normal_x * (*intensity_gradients_)[neighbor].gradient [0];
86 coefficients[ 4] += (*normals_)[neighbor].normal_x * (*intensity_gradients_)[neighbor].gradient [1];
87 coefficients[ 5] += (*normals_)[neighbor].normal_x * (*intensity_gradients_)[neighbor].gradient [2];
89 coefficients[ 6] += (*normals_)[neighbor].normal_y * (*normals_)[neighbor].normal_y;
90 coefficients[ 7] += (*normals_)[neighbor].normal_y * (*normals_)[neighbor].normal_z;
91 coefficients[ 8] += (*normals_)[neighbor].normal_y * (*intensity_gradients_)[neighbor].gradient [0];
92 coefficients[ 9] += (*normals_)[neighbor].normal_y * (*intensity_gradients_)[neighbor].gradient [1];
93 coefficients[10] += (*normals_)[neighbor].normal_y * (*intensity_gradients_)[neighbor].gradient [2];
95 coefficients[11] += (*normals_)[neighbor].normal_z * (*normals_)[neighbor].normal_z;
96 coefficients[12] += (*normals_)[neighbor].normal_z * (*intensity_gradients_)[neighbor].gradient [0];
97 coefficients[13] += (*normals_)[neighbor].normal_z * (*intensity_gradients_)[neighbor].gradient [1];
98 coefficients[14] += (*normals_)[neighbor].normal_z * (*intensity_gradients_)[neighbor].gradient [2];
100 coefficients[15] += (*intensity_gradients_)[neighbor].gradient [0] * (*intensity_gradients_)[neighbor].gradient [0];
101 coefficients[16] += (*intensity_gradients_)[neighbor].gradient [0] * (*intensity_gradients_)[neighbor].gradient [1];
102 coefficients[17] += (*intensity_gradients_)[neighbor].gradient [0] * (*intensity_gradients_)[neighbor].gradient [2];
104 coefficients[18] += (*intensity_gradients_)[neighbor].gradient [1] * (*intensity_gradients_)[neighbor].gradient [1];
105 coefficients[19] += (*intensity_gradients_)[neighbor].gradient [1] * (*intensity_gradients_)[neighbor].gradient [2];
107 coefficients[20] += (*intensity_gradients_)[neighbor].gradient [2] * (*intensity_gradients_)[neighbor].gradient [2];
114 float norm = 1.0 / float (count);
115 coefficients[ 0] *= norm;
116 coefficients[ 1] *= norm;
117 coefficients[ 2] *= norm;
118 coefficients[ 3] *= norm;
119 coefficients[ 4] *= norm;
120 coefficients[ 5] *= norm;
121 coefficients[ 6] *= norm;
122 coefficients[ 7] *= norm;
123 coefficients[ 8] *= norm;
124 coefficients[ 9] *= norm;
125 coefficients[10] *= norm;
126 coefficients[11] *= norm;
127 coefficients[12] *= norm;
128 coefficients[13] *= norm;
129 coefficients[14] *= norm;
130 coefficients[15] *= norm;
131 coefficients[16] *= norm;
132 coefficients[17] *= norm;
133 coefficients[18] *= norm;
134 coefficients[19] *= norm;
135 coefficients[20] *= norm;
140 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
143 if (normals_->empty ())
145 normals_->reserve (surface_->size ());
146 if (!surface_->isOrganized ())
151 normal_estimation.
compute (*normals_);
159 normal_estimation.
compute (*normals_);
164 cloud->
resize (surface_->size ());
165 #pragma omp parallel for \
167 num_threads(threads_)
168 for (
unsigned idx = 0; idx < surface_->size (); ++idx)
170 cloud->
points [idx].x = surface_->points [idx].x;
171 cloud->
points [idx].y = surface_->points [idx].y;
172 cloud->
points [idx].z = surface_->points [idx].z;
175 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));
183 grad_est.
compute (*intensity_gradients_);
185 #pragma omp parallel for \
187 num_threads(threads_)
188 for (std::size_t idx = 0; idx < intensity_gradients_->size (); ++idx)
190 float len = intensity_gradients_->points [idx].gradient_x * intensity_gradients_->points [idx].gradient_x +
191 intensity_gradients_->points [idx].gradient_y * intensity_gradients_->points [idx].gradient_y +
192 intensity_gradients_->points [idx].gradient_z * intensity_gradients_->points [idx].gradient_z ;
197 len = 1.0 / sqrt (len);
198 intensity_gradients_->points [idx].gradient_x *= len;
199 intensity_gradients_->points [idx].gradient_y *= len;
200 intensity_gradients_->points [idx].gradient_z *= len;
204 intensity_gradients_->points [idx].gradient_x = 0;
205 intensity_gradients_->points [idx].gradient_y = 0;
206 intensity_gradients_->points [idx].gradient_z = 0;
211 response->
points.reserve (input_->size());
212 responseTomasi(*response);
220 for (std::size_t i = 0; i < response->
size (); ++i)
221 keypoints_indices_->indices.push_back (i);
225 output.points.clear ();
226 output.points.reserve (response->
size());
228 #pragma omp parallel for \
230 num_threads(threads_)
231 for (std::size_t idx = 0; idx < response->
size (); ++idx)
233 if (!
isFinite ((*response)[idx]) || (*response)[idx].intensity < threshold_)
236 std::vector<int> nn_indices;
237 std::vector<float> nn_dists;
238 tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
239 bool is_maxima =
true;
240 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
242 if ((*response)[idx].intensity < (*response)[*iIt].intensity)
251 output.points.push_back ((*response)[idx]);
252 keypoints_indices_->indices.push_back (idx);
257 refineCorners (output);
260 output.width = output.size();
261 output.is_dense =
true;
265 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
270 PCL_ALIGN (16)
float covar [21];
271 Eigen::SelfAdjointEigenSolver <Eigen::Matrix<float, 6, 6> > solver;
272 Eigen::Matrix<float, 6, 6> covariance;
274 #pragma omp parallel for \
276 firstprivate(pointOut, covar, covariance, solver) \
277 num_threads(threads_)
278 for (
unsigned pIdx = 0; pIdx < input_->size (); ++pIdx)
280 const PointInT& pointIn = input_->points [pIdx];
281 pointOut.intensity = 0.0;
284 std::vector<int> nn_indices;
285 std::vector<float> nn_dists;
286 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
287 calculateCombinedCovar (nn_indices, covar);
289 float trace = covar [0] + covar [6] + covar [11] + covar [15] + covar [18] + covar [20];
292 covariance.coeffRef ( 0) = covar [ 0];
293 covariance.coeffRef ( 1) = covar [ 1];
294 covariance.coeffRef ( 2) = covar [ 2];
295 covariance.coeffRef ( 3) = covar [ 3];
296 covariance.coeffRef ( 4) = covar [ 4];
297 covariance.coeffRef ( 5) = covar [ 5];
299 covariance.coeffRef ( 7) = covar [ 6];
300 covariance.coeffRef ( 8) = covar [ 7];
301 covariance.coeffRef ( 9) = covar [ 8];
302 covariance.coeffRef (10) = covar [ 9];
303 covariance.coeffRef (11) = covar [10];
305 covariance.coeffRef (14) = covar [11];
306 covariance.coeffRef (15) = covar [12];
307 covariance.coeffRef (16) = covar [13];
308 covariance.coeffRef (17) = covar [14];
310 covariance.coeffRef (21) = covar [15];
311 covariance.coeffRef (22) = covar [16];
312 covariance.coeffRef (23) = covar [17];
314 covariance.coeffRef (28) = covar [18];
315 covariance.coeffRef (29) = covar [19];
317 covariance.coeffRef (35) = covar [20];
319 covariance.coeffRef ( 6) = covar [ 1];
321 covariance.coeffRef (12) = covar [ 2];
322 covariance.coeffRef (13) = covar [ 7];
324 covariance.coeffRef (18) = covar [ 3];
325 covariance.coeffRef (19) = covar [ 8];
326 covariance.coeffRef (20) = covar [12];
328 covariance.coeffRef (24) = covar [ 4];
329 covariance.coeffRef (25) = covar [ 9];
330 covariance.coeffRef (26) = covar [13];
331 covariance.coeffRef (27) = covar [16];
333 covariance.coeffRef (30) = covar [ 5];
334 covariance.coeffRef (31) = covar [10];
335 covariance.coeffRef (32) = covar [14];
336 covariance.coeffRef (33) = covar [17];
337 covariance.coeffRef (34) = covar [19];
339 solver.compute (covariance);
340 pointOut.intensity = solver.eigenvalues () [3];
344 pointOut.x = pointIn.x;
345 pointOut.y = pointIn.y;
346 pointOut.z = pointIn.z;
349 output.points.push_back(pointOut);
351 output.height = input_->height;
352 output.width = input_->width;
355 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
363 Eigen::Vector3f NNTp;
364 const Eigen::Vector3f* normal;
365 const Eigen::Vector3f* point;
367 const unsigned max_iterations = 10;
368 for (
typename PointCloudOut::iterator cornerIt = corners.begin(); cornerIt != corners.end(); ++cornerIt)
370 unsigned iterations = 0;
375 corner.x = cornerIt->x;
376 corner.y = cornerIt->y;
377 corner.z = cornerIt->z;
378 std::vector<int> nn_indices;
379 std::vector<float> nn_dists;
380 search.
radiusSearch (corner, search_radius_, nn_indices, nn_dists);
381 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
383 normal =
reinterpret_cast<const Eigen::Vector3f*
> (&((*normals_)[*iIt].normal_x));
384 point =
reinterpret_cast<const Eigen::Vector3f*
> (&((*surface_)[*iIt].x));
385 nnT = (*normal) * (normal->transpose());
387 NNTp += nnT * (*point);
389 if (NNT.determinant() != 0)
390 *(
reinterpret_cast<Eigen::Vector3f*
>(&(cornerIt->x))) = NNT.inverse () * NNTp;
392 diff = (cornerIt->x - corner.x) * (cornerIt->x - corner.x) +
393 (cornerIt->y - corner.y) * (cornerIt->y - corner.y) +
394 (cornerIt->z - corner.z) * (cornerIt->z - corner.z);
396 }
while (diff > 1e-6 && ++iterations < max_iterations);
400 #define PCL_INSTANTIATE_HarrisKeypoint6D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint6D<T,U,N>;
401 #endif // #ifndef PCL_HARRIS_KEYPOINT_6D_IMPL_H_