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/filters/passthrough.h>
44 #include <pcl/filters/extract_indices.h>
45 #include <pcl/features/normal_3d.h>
47 #include <pcl/features/intensity_gradient.h>
48 #include <pcl/features/integral_image_normal.h>
50 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
53 threshold_= threshold;
56 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
59 search_radius_ = radius;
62 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
68 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
75 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
78 memset (coefficients, 0,
sizeof (
float) * 21);
80 for (
const int &neighbor : neighbors)
82 if (std::isfinite (normals_->points[neighbor].normal_x) && std::isfinite (intensity_gradients_->points[neighbor].gradient [0]))
84 coefficients[ 0] += normals_->points[neighbor].normal_x * normals_->points[neighbor].normal_x;
85 coefficients[ 1] += normals_->points[neighbor].normal_x * normals_->points[neighbor].normal_y;
86 coefficients[ 2] += normals_->points[neighbor].normal_x * normals_->points[neighbor].normal_z;
87 coefficients[ 3] += normals_->points[neighbor].normal_x * intensity_gradients_->points[neighbor].gradient [0];
88 coefficients[ 4] += normals_->points[neighbor].normal_x * intensity_gradients_->points[neighbor].gradient [1];
89 coefficients[ 5] += normals_->points[neighbor].normal_x * intensity_gradients_->points[neighbor].gradient [2];
91 coefficients[ 6] += normals_->points[neighbor].normal_y * normals_->points[neighbor].normal_y;
92 coefficients[ 7] += normals_->points[neighbor].normal_y * normals_->points[neighbor].normal_z;
93 coefficients[ 8] += normals_->points[neighbor].normal_y * intensity_gradients_->points[neighbor].gradient [0];
94 coefficients[ 9] += normals_->points[neighbor].normal_y * intensity_gradients_->points[neighbor].gradient [1];
95 coefficients[10] += normals_->points[neighbor].normal_y * intensity_gradients_->points[neighbor].gradient [2];
97 coefficients[11] += normals_->points[neighbor].normal_z * normals_->points[neighbor].normal_z;
98 coefficients[12] += normals_->points[neighbor].normal_z * intensity_gradients_->points[neighbor].gradient [0];
99 coefficients[13] += normals_->points[neighbor].normal_z * intensity_gradients_->points[neighbor].gradient [1];
100 coefficients[14] += normals_->points[neighbor].normal_z * intensity_gradients_->points[neighbor].gradient [2];
102 coefficients[15] += intensity_gradients_->points[neighbor].gradient [0] * intensity_gradients_->points[neighbor].gradient [0];
103 coefficients[16] += intensity_gradients_->points[neighbor].gradient [0] * intensity_gradients_->points[neighbor].gradient [1];
104 coefficients[17] += intensity_gradients_->points[neighbor].gradient [0] * intensity_gradients_->points[neighbor].gradient [2];
106 coefficients[18] += intensity_gradients_->points[neighbor].gradient [1] * intensity_gradients_->points[neighbor].gradient [1];
107 coefficients[19] += intensity_gradients_->points[neighbor].gradient [1] * intensity_gradients_->points[neighbor].gradient [2];
109 coefficients[20] += intensity_gradients_->points[neighbor].gradient [2] * intensity_gradients_->points[neighbor].gradient [2];
116 float norm = 1.0 / float (count);
117 coefficients[ 0] *= norm;
118 coefficients[ 1] *= norm;
119 coefficients[ 2] *= norm;
120 coefficients[ 3] *= norm;
121 coefficients[ 4] *= norm;
122 coefficients[ 5] *= norm;
123 coefficients[ 6] *= norm;
124 coefficients[ 7] *= norm;
125 coefficients[ 8] *= norm;
126 coefficients[ 9] *= norm;
127 coefficients[10] *= norm;
128 coefficients[11] *= norm;
129 coefficients[12] *= norm;
130 coefficients[13] *= norm;
131 coefficients[14] *= norm;
132 coefficients[15] *= norm;
133 coefficients[16] *= norm;
134 coefficients[17] *= norm;
135 coefficients[18] *= norm;
136 coefficients[19] *= norm;
137 coefficients[20] *= norm;
142 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
145 if (normals_->empty ())
147 normals_->reserve (surface_->size ());
148 if (!surface_->isOrganized ())
153 normal_estimation.
compute (*normals_);
161 normal_estimation.
compute (*normals_);
166 cloud->
resize (surface_->size ());
167 #pragma omp parallel for \
169 num_threads(threads_)
170 for (
unsigned idx = 0; idx < surface_->size (); ++idx)
172 cloud->
points [idx].x = surface_->points [idx].x;
173 cloud->
points [idx].y = surface_->points [idx].y;
174 cloud->
points [idx].z = surface_->points [idx].z;
177 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));
185 grad_est.
compute (*intensity_gradients_);
187 #pragma omp parallel for \
189 num_threads(threads_)
190 for (std::size_t idx = 0; idx < intensity_gradients_->size (); ++idx)
192 float len = intensity_gradients_->points [idx].gradient_x * intensity_gradients_->points [idx].gradient_x +
193 intensity_gradients_->points [idx].gradient_y * intensity_gradients_->points [idx].gradient_y +
194 intensity_gradients_->points [idx].gradient_z * intensity_gradients_->points [idx].gradient_z ;
199 len = 1.0 / sqrt (len);
200 intensity_gradients_->points [idx].gradient_x *= len;
201 intensity_gradients_->points [idx].gradient_y *= len;
202 intensity_gradients_->points [idx].gradient_z *= len;
206 intensity_gradients_->points [idx].gradient_x = 0;
207 intensity_gradients_->points [idx].gradient_y = 0;
208 intensity_gradients_->points [idx].gradient_z = 0;
213 response->
points.reserve (input_->points.size());
214 responseTomasi(*response);
222 for (std::size_t i = 0; i < response->
size (); ++i)
223 keypoints_indices_->indices.push_back (i);
227 output.points.clear ();
228 output.points.reserve (response->
points.size());
230 #pragma omp parallel for \
232 num_threads(threads_)
233 for (std::size_t idx = 0; idx < response->
points.size (); ++idx)
238 std::vector<int> nn_indices;
239 std::vector<float> nn_dists;
240 tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
241 bool is_maxima =
true;
242 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
244 if (response->
points[idx].intensity < response->
points[*iIt].intensity)
253 output.points.push_back (response->
points[idx]);
254 keypoints_indices_->indices.push_back (idx);
259 refineCorners (output);
262 output.width =
static_cast<std::uint32_t
> (output.points.size());
263 output.is_dense =
true;
267 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
272 PCL_ALIGN (16)
float covar [21];
273 Eigen::SelfAdjointEigenSolver <Eigen::Matrix<float, 6, 6> > solver;
274 Eigen::Matrix<float, 6, 6> covariance;
276 #pragma omp parallel for \
278 private(pointOut, covar, covariance, solver) \
279 num_threads(threads_)
280 for (
unsigned pIdx = 0; pIdx < input_->size (); ++pIdx)
282 const PointInT& pointIn = input_->points [pIdx];
283 pointOut.intensity = 0.0;
286 std::vector<int> nn_indices;
287 std::vector<float> nn_dists;
288 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
289 calculateCombinedCovar (nn_indices, covar);
291 float trace = covar [0] + covar [6] + covar [11] + covar [15] + covar [18] + covar [20];
294 covariance.coeffRef ( 0) = covar [ 0];
295 covariance.coeffRef ( 1) = covar [ 1];
296 covariance.coeffRef ( 2) = covar [ 2];
297 covariance.coeffRef ( 3) = covar [ 3];
298 covariance.coeffRef ( 4) = covar [ 4];
299 covariance.coeffRef ( 5) = covar [ 5];
301 covariance.coeffRef ( 7) = covar [ 6];
302 covariance.coeffRef ( 8) = covar [ 7];
303 covariance.coeffRef ( 9) = covar [ 8];
304 covariance.coeffRef (10) = covar [ 9];
305 covariance.coeffRef (11) = covar [10];
307 covariance.coeffRef (14) = covar [11];
308 covariance.coeffRef (15) = covar [12];
309 covariance.coeffRef (16) = covar [13];
310 covariance.coeffRef (17) = covar [14];
312 covariance.coeffRef (21) = covar [15];
313 covariance.coeffRef (22) = covar [16];
314 covariance.coeffRef (23) = covar [17];
316 covariance.coeffRef (28) = covar [18];
317 covariance.coeffRef (29) = covar [19];
319 covariance.coeffRef (35) = covar [20];
321 covariance.coeffRef ( 6) = covar [ 1];
323 covariance.coeffRef (12) = covar [ 2];
324 covariance.coeffRef (13) = covar [ 7];
326 covariance.coeffRef (18) = covar [ 3];
327 covariance.coeffRef (19) = covar [ 8];
328 covariance.coeffRef (20) = covar [12];
330 covariance.coeffRef (24) = covar [ 4];
331 covariance.coeffRef (25) = covar [ 9];
332 covariance.coeffRef (26) = covar [13];
333 covariance.coeffRef (27) = covar [16];
335 covariance.coeffRef (30) = covar [ 5];
336 covariance.coeffRef (31) = covar [10];
337 covariance.coeffRef (32) = covar [14];
338 covariance.coeffRef (33) = covar [17];
339 covariance.coeffRef (34) = covar [19];
341 solver.compute (covariance);
342 pointOut.intensity = solver.eigenvalues () [3];
346 pointOut.x = pointIn.x;
347 pointOut.y = pointIn.y;
348 pointOut.z = pointIn.z;
351 output.points.push_back(pointOut);
353 output.height = input_->height;
354 output.width = input_->width;
357 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
365 Eigen::Vector3f NNTp;
366 const Eigen::Vector3f* normal;
367 const Eigen::Vector3f* point;
369 const unsigned max_iterations = 10;
370 for (
typename PointCloudOut::iterator cornerIt = corners.begin(); cornerIt != corners.end(); ++cornerIt)
372 unsigned iterations = 0;
377 corner.x = cornerIt->x;
378 corner.y = cornerIt->y;
379 corner.z = cornerIt->z;
380 std::vector<int> nn_indices;
381 std::vector<float> nn_dists;
382 search.
radiusSearch (corner, search_radius_, nn_indices, nn_dists);
383 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
385 normal =
reinterpret_cast<const Eigen::Vector3f*
> (&(normals_->points[*iIt].normal_x));
386 point =
reinterpret_cast<const Eigen::Vector3f*
> (&(surface_->points[*iIt].x));
387 nnT = (*normal) * (normal->transpose());
389 NNTp += nnT * (*point);
391 if (NNT.determinant() != 0)
392 *(reinterpret_cast<Eigen::Vector3f*>(&(cornerIt->x))) = NNT.inverse () * NNTp;
394 diff = (cornerIt->x - corner.x) * (cornerIt->x - corner.x) +
395 (cornerIt->y - corner.y) * (cornerIt->y - corner.y) +
396 (cornerIt->z - corner.z) * (cornerIt->z - corner.z);
398 }
while (diff > 1e-6 && ++iterations < max_iterations);
402 #define PCL_INSTANTIATE_HarrisKeypoint6D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint6D<T,U,N>;
403 #endif // #ifndef PCL_HARRIS_KEYPOINT_6D_IMPL_H_
void calculateCombinedCovar(const std::vector< int > &neighbors, float *coefficients) const
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
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...
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void refineCorners(PointCloudOut &corners) const
void setNonMaxSupression(bool=false)
whether non maxima suppression should be applied or the response for each point should be returned ...
void resize(std::size_t n)
Resize the cloud.
void responseTomasi(PointCloudOut &output) const
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
void detectKeypoints(PointCloudOut &output)
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.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
Surface normal estimation on organized data using integral images.
void setRadius(float radius)
set the radius for normal estimation and non maxima supression.
void setRefine(bool do_refine)
whether the detected key points should be refined or not.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
IntensityGradientEstimation estimates the intensity gradient for a point cloud that contains position...
void setThreshold(float threshold)
set the threshold value for detecting corners.
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
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 compute(PointCloudOut &output)
Base method for feature estimation for all points given in using th...
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
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.