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>;
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
void setNonMaxSupression(bool=false)
whether non maxima suppression should be applied or the response for each point should be returned
void setThreshold(float threshold)
set the threshold value for detecting corners.
void setRefine(bool do_refine)
whether the detected key points should be refined or not.
void setRadius(float radius)
set the radius for normal estimation and non maxima supression.
void responseTomasi(PointCloudOut &output) const
void detectKeypoints(PointCloudOut &output)
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
void refineCorners(PointCloudOut &corners) const
void calculateCombinedCovar(const std::vector< int > &neighbors, float *coefficients) const
Surface normal estimation on organized data using integral images.
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
IntensityGradientEstimation estimates the intensity gradient for a point cloud that contains position...
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
void resize(std::size_t count)
Resizes the container to contain count elements.
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
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.
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...