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