Point Cloud Library (PCL)  1.7.1
harris_6d.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_HARRIS_KEYPOINT_6D_IMPL_H_
39 #define PCL_HARRIS_KEYPOINT_6D_IMPL_H_
40 
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>
46 //#include <pcl/features/fast_intensity_gradient.h>
47 #include <pcl/features/intensity_gradient.h>
48 #include <pcl/features/integral_image_normal.h>
49 
50 template <typename PointInT, typename PointOutT, typename NormalT> void
52 {
53  threshold_= threshold;
54 }
55 
56 template <typename PointInT, typename PointOutT, typename NormalT> void
58 {
59  search_radius_ = radius;
60 }
61 
62 template <typename PointInT, typename PointOutT, typename NormalT> void
64 {
65  refine_ = do_refine;
66 }
67 
68 template <typename PointInT, typename PointOutT, typename NormalT> void
70 {
71  nonmax_ = nonmax;
72 }
73 
74 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
75 template <typename PointInT, typename PointOutT, typename NormalT> void
76 pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::calculateCombinedCovar (const std::vector<int>& neighbors, float* coefficients) const
77 {
78  memset (coefficients, 0, sizeof (float) * 21);
79  unsigned count = 0;
80  for (std::vector<int>::const_iterator iIt = neighbors.begin(); iIt != neighbors.end(); ++iIt)
81  {
82  if (pcl_isfinite (normals_->points[*iIt].normal_x) && pcl_isfinite (intensity_gradients_->points[*iIt].gradient [0]))
83  {
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];
90 
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];
96 
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];
101 
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];
105 
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];
108 
109  coefficients[20] += intensity_gradients_->points[*iIt].gradient [2] * intensity_gradients_->points[*iIt].gradient [2];
110 
111  ++count;
112  }
113  }
114  if (count > 0)
115  {
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;
138  }
139 }
140 
141 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
142 template <typename PointInT, typename PointOutT, typename NormalT> void
144 {
145  if (normals_->empty ())
146  {
147  normals_->reserve (surface_->size ());
148  if (!surface_->isOrganized ())
149  {
151  normal_estimation.setInputCloud (surface_);
152  normal_estimation.setRadiusSearch (search_radius_);
153  normal_estimation.compute (*normals_);
154  }
155  else
156  {
159  normal_estimation.setInputCloud (surface_);
160  normal_estimation.setNormalSmoothingSize (5.0);
161  normal_estimation.compute (*normals_);
162  }
163  }
164 
166  cloud->resize (surface_->size ());
167 #ifdef _OPENMP
168  #pragma omp parallel for num_threads(threads_) default(shared)
169 #endif
170  for (unsigned idx = 0; idx < surface_->size (); ++idx)
171  {
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;
175  //grayscale = 0.2989 * R + 0.5870 * G + 0.1140 * B
176 
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));
178  }
179  pcl::copyPointCloud (*surface_, *cloud);
180 
182  grad_est.setInputCloud (cloud);
183  grad_est.setInputNormals (normals_);
184  grad_est.setRadiusSearch (search_radius_);
185  grad_est.compute (*intensity_gradients_);
186 
187 #ifdef _OPENMP
188  #pragma omp parallel for num_threads(threads_) default (shared)
189 #endif
190  for (unsigned idx = 0; idx < intensity_gradients_->size (); ++idx)
191  {
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 ;
195 
196  // Suat: ToDo: remove this magic number or expose using set/get
197  if (len > 200.0)
198  {
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;
203  }
204  else
205  {
206  intensity_gradients_->points [idx].gradient_x = 0;
207  intensity_gradients_->points [idx].gradient_y = 0;
208  intensity_gradients_->points [idx].gradient_z = 0;
209  }
210  }
211 
212  boost::shared_ptr<pcl::PointCloud<PointOutT> > response (new pcl::PointCloud<PointOutT> ());
213  response->points.reserve (input_->points.size());
214  responseTomasi(*response);
215 
216  // just return the response
217  if (!nonmax_)
218  {
219  output = *response;
220  // we do not change the denseness in this case
221  output.is_dense = input_->is_dense;
222  }
223  else
224  {
225  output.points.clear ();
226  output.points.reserve (response->points.size());
227 
228 #ifdef _OPENMP
229  #pragma omp parallel for num_threads(threads_) default(shared)
230 #endif
231  for (size_t idx = 0; idx < response->points.size (); ++idx)
232  {
233  if (!isFinite (response->points[idx]) || response->points[idx].intensity < threshold_)
234  continue;
235 
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)
241  {
242  if (response->points[idx].intensity < response->points[*iIt].intensity)
243  {
244  is_maxima = false;
245  break;
246  }
247  }
248  if (is_maxima)
249 #ifdef _OPENMP
250  #pragma omp critical
251 #endif
252  output.points.push_back (response->points[idx]);
253  }
254 
255  if (refine_)
256  refineCorners (output);
257 
258  output.height = 1;
259  output.width = static_cast<uint32_t> (output.points.size());
260  output.is_dense = true;
261  }
262 
263 
264 }
265 
266 template <typename PointInT, typename PointOutT, typename NormalT> void
268 {
269  // get the 6x6 covar-mat
270  PointOutT pointOut;
271  PCL_ALIGN (16) float covar [21];
272  Eigen::SelfAdjointEigenSolver <Eigen::Matrix<float, 6, 6> > solver;
273  Eigen::Matrix<float, 6, 6> covariance;
274 
275 #ifdef _OPENMP
276  #pragma omp parallel for default (shared) private (pointOut, covar, covariance, solver) num_threads(threads_)
277 #endif
278  for (unsigned pIdx = 0; pIdx < input_->size (); ++pIdx)
279  {
280  const PointInT& pointIn = input_->points [pIdx];
281  pointOut.intensity = 0.0; //std::numeric_limits<float>::quiet_NaN ();
282  if (isFinite (pointIn))
283  {
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);
288 
289  float trace = covar [0] + covar [6] + covar [11] + covar [15] + covar [18] + covar [20];
290  if (trace != 0)
291  {
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];
298 
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];
304 
305  covariance.coeffRef (14) = covar [11];
306  covariance.coeffRef (15) = covar [12];
307  covariance.coeffRef (16) = covar [13];
308  covariance.coeffRef (17) = covar [14];
309 
310  covariance.coeffRef (21) = covar [15];
311  covariance.coeffRef (22) = covar [16];
312  covariance.coeffRef (23) = covar [17];
313 
314  covariance.coeffRef (28) = covar [18];
315  covariance.coeffRef (29) = covar [19];
316 
317  covariance.coeffRef (35) = covar [20];
318 
319  covariance.coeffRef ( 6) = covar [ 1];
320 
321  covariance.coeffRef (12) = covar [ 2];
322  covariance.coeffRef (13) = covar [ 7];
323 
324  covariance.coeffRef (18) = covar [ 3];
325  covariance.coeffRef (19) = covar [ 8];
326  covariance.coeffRef (20) = covar [12];
327 
328  covariance.coeffRef (24) = covar [ 4];
329  covariance.coeffRef (25) = covar [ 9];
330  covariance.coeffRef (26) = covar [13];
331  covariance.coeffRef (27) = covar [16];
332 
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];
338 
339  solver.compute (covariance);
340  pointOut.intensity = solver.eigenvalues () [3];
341  }
342  }
343 
344  pointOut.x = pointIn.x;
345  pointOut.y = pointIn.y;
346  pointOut.z = pointIn.z;
347 #ifdef _OPENMP
348  #pragma omp critical
349 #endif
350 
351  output.points.push_back(pointOut);
352  }
353  output.height = input_->height;
354  output.width = input_->width;
355 }
356 
357 template <typename PointInT, typename PointOutT, typename NormalT> void
359 {
361  search.setInputCloud(surface_);
362 
363  Eigen::Matrix3f nnT;
364  Eigen::Matrix3f NNT;
365  Eigen::Vector3f NNTp;
366  const Eigen::Vector3f* normal;
367  const Eigen::Vector3f* point;
368  float diff;
369  const unsigned max_iterations = 10;
370  for (typename PointCloudOut::iterator cornerIt = corners.begin(); cornerIt != corners.end(); ++cornerIt)
371  {
372  unsigned iterations = 0;
373  do {
374  NNT.setZero();
375  NNTp.setZero();
376  PointInT corner;
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)
384  {
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());
388  NNT += nnT;
389  NNTp += nnT * (*point);
390  }
391  if (NNT.determinant() != 0)
392  *(reinterpret_cast<Eigen::Vector3f*>(&(cornerIt->x))) = NNT.inverse () * NNTp;
393 
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);
397 
398  } while (diff > 1e-6 && ++iterations < max_iterations);
399  }
400 }
401 
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_
404 
void setThreshold(float threshold)
set the threshold value for detecting corners.
Definition: harris_6d.hpp:51
void detectKeypoints(PointCloudOut &output)
Abstract key point detection method.
Definition: harris_6d.hpp:143
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:447
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: normal_3d.h:286
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
void setRefine(bool do_refine)
whether the detected key points should be refined or not.
Definition: harris_6d.hpp:63
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
Definition: feature.h:200
void setRadius(float radius)
set the radius for normal estimation and non maxima supression.
Definition: harris_6d.hpp:57
void responseTomasi(PointCloudOut &output) const
Definition: harris_6d.hpp:267
VectorType::iterator iterator
Definition: point_cloud.h:432
void refineCorners(PointCloudOut &corners) const
Definition: harris_6d.hpp:358
Surface normal estimation on organized data using integral images.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
IntensityGradientEstimation estimates the intensity gradient for a point cloud that contains position...
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
Definition: point_tests.h:53
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
Definition: kdtree.hpp:77
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.
Definition: kdtree.hpp:97
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.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
iterator end()
Definition: point_cloud.h:435
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:189
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
iterator begin()
Definition: point_cloud.h:434
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
Definition: normal_3d.h:197
void calculateCombinedCovar(const std::vector< int > &neighbors, float *coefficients) const
Definition: harris_6d.hpp:76
void setNonMaxSupression(bool=false)
whether non maxima suppression should be applied or the response for each point should be returned ...
Definition: harris_6d.hpp:69
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
Definition: feature.h:344