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);
222 for (
size_t i = 0; i < response->size (); ++i)
223 keypoints_indices_->indices.push_back (i);
228 output.
points.reserve (response->points.size());
231 #pragma omp parallel for num_threads(threads_) default(shared)
233 for (
size_t idx = 0; idx < response->points.size (); ++idx)
235 if (!
isFinite (response->points[idx]) || response->points[idx].intensity < threshold_)
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)
255 output.
points.push_back (response->points[idx]);
256 keypoints_indices_->indices.push_back (idx);
261 refineCorners (output);
264 output.
width =
static_cast<uint32_t
> (output.
points.size());
269 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
274 PCL_ALIGN (16)
float covar [21];
275 Eigen::SelfAdjointEigenSolver <Eigen::Matrix<float, 6, 6> > solver;
276 Eigen::Matrix<float, 6, 6> covariance;
279 #pragma omp parallel for default (shared) private (pointOut, covar, covariance, solver) num_threads(threads_)
281 for (
unsigned pIdx = 0; pIdx < input_->size (); ++pIdx)
283 const PointInT& pointIn = input_->points [pIdx];
284 pointOut.intensity = 0.0;
287 std::vector<int> nn_indices;
288 std::vector<float> nn_dists;
289 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
290 calculateCombinedCovar (nn_indices, covar);
292 float trace = covar [0] + covar [6] + covar [11] + covar [15] + covar [18] + covar [20];
295 covariance.coeffRef ( 0) = covar [ 0];
296 covariance.coeffRef ( 1) = covar [ 1];
297 covariance.coeffRef ( 2) = covar [ 2];
298 covariance.coeffRef ( 3) = covar [ 3];
299 covariance.coeffRef ( 4) = covar [ 4];
300 covariance.coeffRef ( 5) = covar [ 5];
302 covariance.coeffRef ( 7) = covar [ 6];
303 covariance.coeffRef ( 8) = covar [ 7];
304 covariance.coeffRef ( 9) = covar [ 8];
305 covariance.coeffRef (10) = covar [ 9];
306 covariance.coeffRef (11) = covar [10];
308 covariance.coeffRef (14) = covar [11];
309 covariance.coeffRef (15) = covar [12];
310 covariance.coeffRef (16) = covar [13];
311 covariance.coeffRef (17) = covar [14];
313 covariance.coeffRef (21) = covar [15];
314 covariance.coeffRef (22) = covar [16];
315 covariance.coeffRef (23) = covar [17];
317 covariance.coeffRef (28) = covar [18];
318 covariance.coeffRef (29) = covar [19];
320 covariance.coeffRef (35) = covar [20];
322 covariance.coeffRef ( 6) = covar [ 1];
324 covariance.coeffRef (12) = covar [ 2];
325 covariance.coeffRef (13) = covar [ 7];
327 covariance.coeffRef (18) = covar [ 3];
328 covariance.coeffRef (19) = covar [ 8];
329 covariance.coeffRef (20) = covar [12];
331 covariance.coeffRef (24) = covar [ 4];
332 covariance.coeffRef (25) = covar [ 9];
333 covariance.coeffRef (26) = covar [13];
334 covariance.coeffRef (27) = covar [16];
336 covariance.coeffRef (30) = covar [ 5];
337 covariance.coeffRef (31) = covar [10];
338 covariance.coeffRef (32) = covar [14];
339 covariance.coeffRef (33) = covar [17];
340 covariance.coeffRef (34) = covar [19];
342 solver.compute (covariance);
343 pointOut.intensity = solver.eigenvalues () [3];
347 pointOut.x = pointIn.x;
348 pointOut.y = pointIn.y;
349 pointOut.z = pointIn.z;
354 output.
points.push_back(pointOut);
356 output.
height = input_->height;
357 output.
width = input_->width;
360 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void
368 Eigen::Vector3f NNTp;
369 const Eigen::Vector3f* normal;
370 const Eigen::Vector3f* point;
372 const unsigned max_iterations = 10;
375 unsigned iterations = 0;
380 corner.x = cornerIt->x;
381 corner.y = cornerIt->y;
382 corner.z = cornerIt->z;
383 std::vector<int> nn_indices;
384 std::vector<float> nn_dists;
385 search.
radiusSearch (corner, search_radius_, nn_indices, nn_dists);
386 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
388 normal =
reinterpret_cast<const Eigen::Vector3f*
> (&(normals_->points[*iIt].normal_x));
389 point =
reinterpret_cast<const Eigen::Vector3f*
> (&(surface_->points[*iIt].x));
390 nnT = (*normal) * (normal->transpose());
392 NNTp += nnT * (*point);
394 if (NNT.determinant() != 0)
395 *(reinterpret_cast<Eigen::Vector3f*>(&(cornerIt->x))) = NNT.inverse () * NNTp;
397 diff = (cornerIt->x - corner.x) * (cornerIt->x - corner.x) +
398 (cornerIt->y - corner.y) * (cornerIt->y - corner.y) +
399 (cornerIt->z - corner.z) * (cornerIt->z - corner.z);
401 }
while (diff > 1e-6 && ++iterations < max_iterations);
405 #define PCL_INSTANTIATE_HarrisKeypoint6D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint6D<T,U,N>;
406 #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.
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 ...
boost::shared_ptr< PointCloud< PointT > > Ptr
uint32_t width
The point cloud width (if organized as an image-structure).
void responseTomasi(PointCloudOut &output) const
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 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)
Abstract key point detection method.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void resize(size_t n)
Resize the cloud.
Surface normal estimation on organized data using integral images.
VectorType::iterator iterator
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.
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 setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
void setThreshold(float threshold)
set the threshold value for detecting corners.
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
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.
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.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
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.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
uint32_t height
The point cloud height (if organized as an image-structure).