Point Cloud Library (PCL)  1.11.1-dev
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 <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
42 #include <pcl/keypoints/harris_6d.h>
43 #include <pcl/common/io.h>
44 #include <pcl/features/normal_3d.h>
45 //#include <pcl/features/fast_intensity_gradient.h>
46 #include <pcl/features/intensity_gradient.h>
47 #include <pcl/features/integral_image_normal.h>
48 
49 template <typename PointInT, typename PointOutT, typename NormalT> void
51 {
52  threshold_= threshold;
53 }
54 
55 template <typename PointInT, typename PointOutT, typename NormalT> void
57 {
58  search_radius_ = radius;
59 }
60 
61 template <typename PointInT, typename PointOutT, typename NormalT> void
63 {
64  refine_ = do_refine;
65 }
66 
67 template <typename PointInT, typename PointOutT, typename NormalT> void
69 {
70  nonmax_ = nonmax;
71 }
72 
73 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
74 template <typename PointInT, typename PointOutT, typename NormalT> void
75 pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::calculateCombinedCovar (const std::vector<int>& neighbors, float* coefficients) const
76 {
77  memset (coefficients, 0, sizeof (float) * 21);
78  unsigned count = 0;
79  for (const int &neighbor : neighbors)
80  {
81  if (std::isfinite ((*normals_)[neighbor].normal_x) && std::isfinite ((*intensity_gradients_)[neighbor].gradient [0]))
82  {
83  coefficients[ 0] += (*normals_)[neighbor].normal_x * (*normals_)[neighbor].normal_x;
84  coefficients[ 1] += (*normals_)[neighbor].normal_x * (*normals_)[neighbor].normal_y;
85  coefficients[ 2] += (*normals_)[neighbor].normal_x * (*normals_)[neighbor].normal_z;
86  coefficients[ 3] += (*normals_)[neighbor].normal_x * (*intensity_gradients_)[neighbor].gradient [0];
87  coefficients[ 4] += (*normals_)[neighbor].normal_x * (*intensity_gradients_)[neighbor].gradient [1];
88  coefficients[ 5] += (*normals_)[neighbor].normal_x * (*intensity_gradients_)[neighbor].gradient [2];
89 
90  coefficients[ 6] += (*normals_)[neighbor].normal_y * (*normals_)[neighbor].normal_y;
91  coefficients[ 7] += (*normals_)[neighbor].normal_y * (*normals_)[neighbor].normal_z;
92  coefficients[ 8] += (*normals_)[neighbor].normal_y * (*intensity_gradients_)[neighbor].gradient [0];
93  coefficients[ 9] += (*normals_)[neighbor].normal_y * (*intensity_gradients_)[neighbor].gradient [1];
94  coefficients[10] += (*normals_)[neighbor].normal_y * (*intensity_gradients_)[neighbor].gradient [2];
95 
96  coefficients[11] += (*normals_)[neighbor].normal_z * (*normals_)[neighbor].normal_z;
97  coefficients[12] += (*normals_)[neighbor].normal_z * (*intensity_gradients_)[neighbor].gradient [0];
98  coefficients[13] += (*normals_)[neighbor].normal_z * (*intensity_gradients_)[neighbor].gradient [1];
99  coefficients[14] += (*normals_)[neighbor].normal_z * (*intensity_gradients_)[neighbor].gradient [2];
100 
101  coefficients[15] += (*intensity_gradients_)[neighbor].gradient [0] * (*intensity_gradients_)[neighbor].gradient [0];
102  coefficients[16] += (*intensity_gradients_)[neighbor].gradient [0] * (*intensity_gradients_)[neighbor].gradient [1];
103  coefficients[17] += (*intensity_gradients_)[neighbor].gradient [0] * (*intensity_gradients_)[neighbor].gradient [2];
104 
105  coefficients[18] += (*intensity_gradients_)[neighbor].gradient [1] * (*intensity_gradients_)[neighbor].gradient [1];
106  coefficients[19] += (*intensity_gradients_)[neighbor].gradient [1] * (*intensity_gradients_)[neighbor].gradient [2];
107 
108  coefficients[20] += (*intensity_gradients_)[neighbor].gradient [2] * (*intensity_gradients_)[neighbor].gradient [2];
109 
110  ++count;
111  }
112  }
113  if (count > 0)
114  {
115  float norm = 1.0 / float (count);
116  coefficients[ 0] *= norm;
117  coefficients[ 1] *= norm;
118  coefficients[ 2] *= norm;
119  coefficients[ 3] *= norm;
120  coefficients[ 4] *= norm;
121  coefficients[ 5] *= norm;
122  coefficients[ 6] *= norm;
123  coefficients[ 7] *= norm;
124  coefficients[ 8] *= norm;
125  coefficients[ 9] *= norm;
126  coefficients[10] *= norm;
127  coefficients[11] *= norm;
128  coefficients[12] *= norm;
129  coefficients[13] *= norm;
130  coefficients[14] *= norm;
131  coefficients[15] *= norm;
132  coefficients[16] *= norm;
133  coefficients[17] *= norm;
134  coefficients[18] *= norm;
135  coefficients[19] *= norm;
136  coefficients[20] *= norm;
137  }
138 }
139 
140 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
141 template <typename PointInT, typename PointOutT, typename NormalT> void
143 {
144  if (normals_->empty ())
145  {
146  normals_->reserve (surface_->size ());
147  if (!surface_->isOrganized ())
148  {
150  normal_estimation.setInputCloud (surface_);
151  normal_estimation.setRadiusSearch (search_radius_);
152  normal_estimation.compute (*normals_);
153  }
154  else
155  {
158  normal_estimation.setInputCloud (surface_);
159  normal_estimation.setNormalSmoothingSize (5.0);
160  normal_estimation.compute (*normals_);
161  }
162  }
163 
165  cloud->resize (surface_->size ());
166 #pragma omp parallel for \
167  default(none) \
168  num_threads(threads_)
169  for (unsigned idx = 0; idx < surface_->size (); ++idx)
170  {
171  cloud->points [idx].x = surface_->points [idx].x;
172  cloud->points [idx].y = surface_->points [idx].y;
173  cloud->points [idx].z = surface_->points [idx].z;
174  //grayscale = 0.2989 * R + 0.5870 * G + 0.1140 * B
175 
176  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));
177  }
178  pcl::copyPointCloud (*surface_, *cloud);
179 
181  grad_est.setInputCloud (cloud);
182  grad_est.setInputNormals (normals_);
183  grad_est.setRadiusSearch (search_radius_);
184  grad_est.compute (*intensity_gradients_);
185 
186 #pragma omp parallel for \
187  default(none) \
188  num_threads(threads_)
189  for (std::size_t idx = 0; idx < intensity_gradients_->size (); ++idx)
190  {
191  float len = intensity_gradients_->points [idx].gradient_x * intensity_gradients_->points [idx].gradient_x +
192  intensity_gradients_->points [idx].gradient_y * intensity_gradients_->points [idx].gradient_y +
193  intensity_gradients_->points [idx].gradient_z * intensity_gradients_->points [idx].gradient_z ;
194 
195  // Suat: ToDo: remove this magic number or expose using set/get
196  if (len > 200.0)
197  {
198  len = 1.0 / sqrt (len);
199  intensity_gradients_->points [idx].gradient_x *= len;
200  intensity_gradients_->points [idx].gradient_y *= len;
201  intensity_gradients_->points [idx].gradient_z *= len;
202  }
203  else
204  {
205  intensity_gradients_->points [idx].gradient_x = 0;
206  intensity_gradients_->points [idx].gradient_y = 0;
207  intensity_gradients_->points [idx].gradient_z = 0;
208  }
209  }
210 
212  response->points.reserve (input_->size());
213  responseTomasi(*response);
214 
215  // just return the response
216  if (!nonmax_)
217  {
218  output = *response;
219  // we do not change the denseness in this case
220  output.is_dense = input_->is_dense;
221  for (std::size_t i = 0; i < response->size (); ++i)
222  keypoints_indices_->indices.push_back (i);
223  }
224  else
225  {
226  output.clear ();
227  output.reserve (response->size());
228 
229 #pragma omp parallel for \
230  default(none) \
231  num_threads(threads_)
232  for (std::size_t idx = 0; idx < response->size (); ++idx)
233  {
234  if (!isFinite ((*response)[idx]) || (*response)[idx].intensity < threshold_)
235  continue;
236 
237  std::vector<int> nn_indices;
238  std::vector<float> nn_dists;
239  tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
240  bool is_maxima = true;
241  for (const auto& index : nn_indices)
242  {
243  if ((*response)[idx].intensity < (*response)[index].intensity)
244  {
245  is_maxima = false;
246  break;
247  }
248  }
249  if (is_maxima)
250  #pragma omp critical
251  {
252  output.push_back ((*response)[idx]);
253  keypoints_indices_->indices.push_back (idx);
254  }
255  }
256 
257  if (refine_)
258  refineCorners (output);
259 
260  output.height = 1;
261  output.width = output.size();
262  output.is_dense = true;
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 #pragma omp parallel for \
276  default(none) \
277  firstprivate(pointOut, covar, covariance, solver) \
278  num_threads(threads_)
279  for (unsigned pIdx = 0; pIdx < input_->size (); ++pIdx)
280  {
281  const PointInT& pointIn = input_->points [pIdx];
282  pointOut.intensity = 0.0; //std::numeric_limits<float>::quiet_NaN ();
283  if (isFinite (pointIn))
284  {
285  std::vector<int> nn_indices;
286  std::vector<float> nn_dists;
287  tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
288  calculateCombinedCovar (nn_indices, covar);
289 
290  float trace = covar [0] + covar [6] + covar [11] + covar [15] + covar [18] + covar [20];
291  if (trace != 0)
292  {
293  covariance.coeffRef ( 0) = covar [ 0];
294  covariance.coeffRef ( 1) = covar [ 1];
295  covariance.coeffRef ( 2) = covar [ 2];
296  covariance.coeffRef ( 3) = covar [ 3];
297  covariance.coeffRef ( 4) = covar [ 4];
298  covariance.coeffRef ( 5) = covar [ 5];
299 
300  covariance.coeffRef ( 7) = covar [ 6];
301  covariance.coeffRef ( 8) = covar [ 7];
302  covariance.coeffRef ( 9) = covar [ 8];
303  covariance.coeffRef (10) = covar [ 9];
304  covariance.coeffRef (11) = covar [10];
305 
306  covariance.coeffRef (14) = covar [11];
307  covariance.coeffRef (15) = covar [12];
308  covariance.coeffRef (16) = covar [13];
309  covariance.coeffRef (17) = covar [14];
310 
311  covariance.coeffRef (21) = covar [15];
312  covariance.coeffRef (22) = covar [16];
313  covariance.coeffRef (23) = covar [17];
314 
315  covariance.coeffRef (28) = covar [18];
316  covariance.coeffRef (29) = covar [19];
317 
318  covariance.coeffRef (35) = covar [20];
319 
320  covariance.coeffRef ( 6) = covar [ 1];
321 
322  covariance.coeffRef (12) = covar [ 2];
323  covariance.coeffRef (13) = covar [ 7];
324 
325  covariance.coeffRef (18) = covar [ 3];
326  covariance.coeffRef (19) = covar [ 8];
327  covariance.coeffRef (20) = covar [12];
328 
329  covariance.coeffRef (24) = covar [ 4];
330  covariance.coeffRef (25) = covar [ 9];
331  covariance.coeffRef (26) = covar [13];
332  covariance.coeffRef (27) = covar [16];
333 
334  covariance.coeffRef (30) = covar [ 5];
335  covariance.coeffRef (31) = covar [10];
336  covariance.coeffRef (32) = covar [14];
337  covariance.coeffRef (33) = covar [17];
338  covariance.coeffRef (34) = covar [19];
339 
340  solver.compute (covariance);
341  pointOut.intensity = solver.eigenvalues () [3];
342  }
343  }
344 
345  pointOut.x = pointIn.x;
346  pointOut.y = pointIn.y;
347  pointOut.z = pointIn.z;
348 
349  #pragma omp critical
350  output.push_back(pointOut);
351  }
352  output.height = input_->height;
353  output.width = input_->width;
354 }
355 
356 template <typename PointInT, typename PointOutT, typename NormalT> void
358 {
360  search.setInputCloud(surface_);
361 
362  Eigen::Matrix3f nnT;
363  Eigen::Matrix3f NNT;
364  Eigen::Vector3f NNTp;
365  const Eigen::Vector3f* normal;
366  const Eigen::Vector3f* point;
367  float diff;
368  const unsigned max_iterations = 10;
369  for (typename PointCloudOut::iterator cornerIt = corners.begin(); cornerIt != corners.end(); ++cornerIt)
370  {
371  unsigned iterations = 0;
372  do {
373  NNT.setZero();
374  NNTp.setZero();
375  PointInT corner;
376  corner.x = cornerIt->x;
377  corner.y = cornerIt->y;
378  corner.z = cornerIt->z;
379  std::vector<int> nn_indices;
380  std::vector<float> nn_dists;
381  search.radiusSearch (corner, search_radius_, nn_indices, nn_dists);
382  for (const auto& index : nn_indices)
383  {
384  normal = reinterpret_cast<const Eigen::Vector3f*> (&((*normals_)[index].normal_x));
385  point = reinterpret_cast<const Eigen::Vector3f*> (&((*surface_)[index].x));
386  nnT = (*normal) * (normal->transpose());
387  NNT += nnT;
388  NNTp += nnT * (*point);
389  }
390  if (NNT.determinant() != 0)
391  *(reinterpret_cast<Eigen::Vector3f*>(&(cornerIt->x))) = NNT.inverse () * NNTp;
392 
393  diff = (cornerIt->x - corner.x) * (cornerIt->x - corner.x) +
394  (cornerIt->y - corner.y) * (cornerIt->y - corner.y) +
395  (cornerIt->z - corner.z) * (cornerIt->z - corner.z);
396 
397  } while (diff > 1e-6 && ++iterations < max_iterations);
398  }
399 }
400 
401 #define PCL_INSTANTIATE_HarrisKeypoint6D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint6D<T,U,N>;
402 #endif // #ifndef PCL_HARRIS_KEYPOINT_6D_IMPL_H_
403 
pcl::HarrisKeypoint6D::setRadius
void setRadius(float radius)
set the radius for normal estimation and non maxima supression.
Definition: harris_6d.hpp:56
pcl::NormalEstimation
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
Definition: normal_3d.h:243
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:388
pcl::isFinite
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...
Definition: point_tests.h:55
pcl::IntegralImageNormalEstimation
Surface normal estimation on organized data using integral images.
Definition: integral_image_normal.h:66
pcl::HarrisKeypoint6D::calculateCombinedCovar
void calculateCombinedCovar(const std::vector< int > &neighbors, float *coefficients) const
Definition: harris_6d.hpp:75
pcl::search::KdTree::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
Definition: kdtree.hpp:76
pcl::HarrisKeypoint6D::setRefine
void setRefine(bool do_refine)
whether the detected key points should be refined or not.
Definition: harris_6d.hpp:62
pcl::search::KdTree::radiusSearch
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.
Definition: kdtree.hpp:96
pcl::PointCloud< pcl::PointXYZI >
pcl::IntegralImageNormalEstimation::setInputCloud
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
Definition: integral_image_normal.h:233
pcl::HarrisKeypoint6D::responseTomasi
void responseTomasi(PointCloudOut &output) const
Definition: harris_6d.hpp:267
pcl::IntegralImageNormalEstimation::setNormalEstimationMethod
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
Definition: integral_image_normal.h:215
pcl::PCLBase< PointInT >::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
pcl::copyPointCloud
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.
Definition: io.hpp:121
pcl::HarrisKeypoint6D::PointCloudOut
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: harris_6d.h:56
pcl::NormalEstimation::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition: normal_3d.h:332
pcl::IntegralImageNormalEstimation::setNormalSmoothingSize
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
Definition: integral_image_normal.h:191
pcl::search::KdTree
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:61
pcl::Feature::compute
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:194
pcl::FeatureFromNormals::setInputNormals
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition: feature.h:345
pcl::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:396
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:455
pcl::IntensityGradientEstimation
IntensityGradientEstimation estimates the intensity gradient for a point cloud that contains position...
Definition: intensity_gradient.h:56
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:436
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:406
pcl::HarrisKeypoint6D::refineCorners
void refineCorners(PointCloudOut &corners) const
Definition: harris_6d.hpp:357
pcl::Feature::setRadiusSearch
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:201
pcl::HarrisKeypoint6D::setNonMaxSupression
void setNonMaxSupression(bool=false)
whether non maxima suppression should be applied or the response for each point should be returned
Definition: harris_6d.hpp:68
pcl::HarrisKeypoint6D::detectKeypoints
void detectKeypoints(PointCloudOut &output)
Definition: harris_6d.hpp:142
pcl::HarrisKeypoint6D::setThreshold
void setThreshold(float threshold)
set the threshold value for detecting corners.
Definition: harris_6d.hpp:50