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 (std::vector<int>::const_iterator iIt = neighbors.begin(); iIt != neighbors.end(); ++iIt)
82 if (pcl_isfinite (normals_->points[*iIt].normal_x) && pcl_isfinite (intensity_gradients_->points[*iIt].gradient [0]))
84 coefficients[ 0] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_x;
85 coefficients[ 1] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_y;
86 coefficients[ 2] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_z;
87 coefficients[ 3] += normals_->points[*iIt].normal_x * intensity_gradients_->points[*iIt].gradient [0];
88 coefficients[ 4] += normals_->points[*iIt].normal_x * intensity_gradients_->points[*iIt].gradient [1];
89 coefficients[ 5] += normals_->points[*iIt].normal_x * intensity_gradients_->points[*iIt].gradient [2];
91 coefficients[ 6] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_y;
92 coefficients[ 7] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_z;
93 coefficients[ 8] += normals_->points[*iIt].normal_y * intensity_gradients_->points[*iIt].gradient [0];
94 coefficients[ 9] += normals_->points[*iIt].normal_y * intensity_gradients_->points[*iIt].gradient [1];
95 coefficients[10] += normals_->points[*iIt].normal_y * intensity_gradients_->points[*iIt].gradient [2];
97 coefficients[11] += normals_->points[*iIt].normal_z * normals_->points[*iIt].normal_z;
98 coefficients[12] += normals_->points[*iIt].normal_z * intensity_gradients_->points[*iIt].gradient [0];
99 coefficients[13] += normals_->points[*iIt].normal_z * intensity_gradients_->points[*iIt].gradient [1];
100 coefficients[14] += normals_->points[*iIt].normal_z * intensity_gradients_->points[*iIt].gradient [2];
102 coefficients[15] += intensity_gradients_->points[*iIt].gradient [0] * intensity_gradients_->points[*iIt].gradient [0];
103 coefficients[16] += intensity_gradients_->points[*iIt].gradient [0] * intensity_gradients_->points[*iIt].gradient [1];
104 coefficients[17] += intensity_gradients_->points[*iIt].gradient [0] * intensity_gradients_->points[*iIt].gradient [2];
106 coefficients[18] += intensity_gradients_->points[*iIt].gradient [1] * intensity_gradients_->points[*iIt].gradient [1];
107 coefficients[19] += intensity_gradients_->points[*iIt].gradient [1] * intensity_gradients_->points[*iIt].gradient [2];
109 coefficients[20] += intensity_gradients_->points[*iIt].gradient [2] * intensity_gradients_->points[*iIt].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 ());
168 #pragma omp parallel for num_threads(threads_) default(shared)
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_);
188 #pragma omp parallel for num_threads(threads_) default (shared)
190 for (
unsigned 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);
226 output.
points.reserve (response->points.size());
229 #pragma omp parallel for num_threads(threads_) default(shared)
231 for (
size_t idx = 0; idx < response->points.size (); ++idx)
233 if (!
isFinite (response->points[idx]) || response->points[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->points[idx].intensity < response->points[*iIt].intensity)
252 output.
points.push_back (response->points[idx]);
256 refineCorners (output);
259 output.
width =
static_cast<uint32_t
> (output.
points.size());
266 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
271 PCL_ALIGN (16)
float covar [21];
272 Eigen::SelfAdjointEigenSolver <Eigen::Matrix<float, 6, 6> > solver;
273 Eigen::Matrix<float, 6, 6> covariance;
276 #pragma omp parallel for default (shared) private (pointOut, covar, covariance, solver) 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;
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;
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 setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
void setNonMaxSupression(bool=false)
whether non maxima suppression should be applied or the response for each point should be returned ...
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
void responseTomasi(PointCloudOut &output) const
void setRefine(bool do_refine)
whether the detected key points should be refined or not.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
void resize(size_t n)
Resize the cloud.
void setRadius(float radius)
set the radius for normal estimation and non maxima supression.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
uint32_t width
The point cloud width (if organized as an image-structure).
int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const
Search for all the nearest neighbors of the query point in a given radius.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
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...
VectorType::iterator iterator
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in using th...
uint32_t height
The point cloud height (if organized as an image-structure).
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
void setThreshold(float threshold)
set the threshold value for detecting corners.
Surface normal estimation on organized data using integral images.
IntensityGradientEstimation estimates the intensity gradient for a point cloud that contains position...
void refineCorners(PointCloudOut &corners) const
void detectKeypoints(PointCloudOut &output)
Abstract key point detection method.
boost::shared_ptr< PointCloud< PointT > > Ptr
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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...