Point Cloud Library (PCL)  1.7.1
integral_image_normal.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
39 #ifndef PCL_FEATURES_INTEGRALIMAGE_BASED_IMPL_NORMAL_ESTIMATOR_H_
40 #define PCL_FEATURES_INTEGRALIMAGE_BASED_IMPL_NORMAL_ESTIMATOR_H_
41 
42 #include <pcl/features/integral_image_normal.h>
43 
44 //////////////////////////////////////////////////////////////////////////////////////////
45 template <typename PointInT, typename PointOutT>
47 {
48  if (diff_x_ != NULL) delete[] diff_x_;
49  if (diff_y_ != NULL) delete[] diff_y_;
50  if (depth_data_ != NULL) delete[] depth_data_;
51  if (distance_map_ != NULL) delete[] distance_map_;
52 }
53 
54 //////////////////////////////////////////////////////////////////////////////////////////
55 template <typename PointInT, typename PointOutT> void
57 {
58  if (border_policy_ != BORDER_POLICY_IGNORE &&
59  border_policy_ != BORDER_POLICY_MIRROR)
60  PCL_THROW_EXCEPTION (InitFailedException,
61  "[pcl::IntegralImageNormalEstimation::initData] unknown border policy.");
62 
63  if (normal_estimation_method_ != COVARIANCE_MATRIX &&
64  normal_estimation_method_ != AVERAGE_3D_GRADIENT &&
65  normal_estimation_method_ != AVERAGE_DEPTH_CHANGE &&
66  normal_estimation_method_ != SIMPLE_3D_GRADIENT)
67  PCL_THROW_EXCEPTION (InitFailedException,
68  "[pcl::IntegralImageNormalEstimation::initData] unknown normal estimation method.");
69 
70  // compute derivatives
71  if (diff_x_ != NULL) delete[] diff_x_;
72  if (diff_y_ != NULL) delete[] diff_y_;
73  if (depth_data_ != NULL) delete[] depth_data_;
74  if (distance_map_ != NULL) delete[] distance_map_;
75  diff_x_ = NULL;
76  diff_y_ = NULL;
77  depth_data_ = NULL;
78  distance_map_ = NULL;
79 
80  if (normal_estimation_method_ == COVARIANCE_MATRIX)
81  initCovarianceMatrixMethod ();
82  else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
83  initAverage3DGradientMethod ();
84  else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
85  initAverageDepthChangeMethod ();
86  else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
87  initSimple3DGradientMethod ();
88 }
89 
90 
91 //////////////////////////////////////////////////////////////////////////////////////////
92 template <typename PointInT, typename PointOutT> void
94 {
95  rect_width_ = width;
96  rect_width_2_ = width/2;
97  rect_width_4_ = width/4;
98  rect_height_ = height;
99  rect_height_2_ = height/2;
100  rect_height_4_ = height/4;
101 }
102 
103 //////////////////////////////////////////////////////////////////////////////////////////
104 template <typename PointInT, typename PointOutT> void
106 {
107  // number of DataType entries per element (equal or bigger than dimensions)
108  int element_stride = sizeof (PointInT) / sizeof (float);
109  // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
110  int row_stride = element_stride * input_->width;
111 
112  const float *data_ = reinterpret_cast<const float*> (&input_->points[0]);
113 
114  integral_image_XYZ_.setSecondOrderComputation (false);
115  integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride);
116 
117  init_simple_3d_gradient_ = true;
118  init_covariance_matrix_ = init_average_3d_gradient_ = init_depth_change_ = false;
119 }
120 
121 //////////////////////////////////////////////////////////////////////////////////////////
122 template <typename PointInT, typename PointOutT> void
124 {
125  // number of DataType entries per element (equal or bigger than dimensions)
126  int element_stride = sizeof (PointInT) / sizeof (float);
127  // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
128  int row_stride = element_stride * input_->width;
129 
130  const float *data_ = reinterpret_cast<const float*> (&input_->points[0]);
131 
132  integral_image_XYZ_.setSecondOrderComputation (true);
133  integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride);
134 
135  init_covariance_matrix_ = true;
136  init_average_3d_gradient_ = init_depth_change_ = init_simple_3d_gradient_ = false;
137 }
138 
139 //////////////////////////////////////////////////////////////////////////////////////////
140 template <typename PointInT, typename PointOutT> void
142 {
143  size_t data_size = (input_->points.size () << 2);
144  diff_x_ = new float[data_size];
145  diff_y_ = new float[data_size];
146 
147  memset (diff_x_, 0, sizeof(float) * data_size);
148  memset (diff_y_, 0, sizeof(float) * data_size);
149 
150  // x u x
151  // l x r
152  // x d x
153  const PointInT* point_up = &(input_->points [1]);
154  const PointInT* point_dn = point_up + (input_->width << 1);//&(input_->points [1 + (input_->width << 1)]);
155  const PointInT* point_lf = &(input_->points [input_->width]);
156  const PointInT* point_rg = point_lf + 2; //&(input_->points [input_->width + 2]);
157  float* diff_x_ptr = diff_x_ + ((input_->width + 1) << 2);
158  float* diff_y_ptr = diff_y_ + ((input_->width + 1) << 2);
159  unsigned diff_skip = 8; // skip last element in row and the first in the next row
160 
161  for (size_t ri = 1; ri < input_->height - 1; ++ri
162  , point_up += input_->width
163  , point_dn += input_->width
164  , point_lf += input_->width
165  , point_rg += input_->width
166  , diff_x_ptr += diff_skip
167  , diff_y_ptr += diff_skip)
168  {
169  for (size_t ci = 0; ci < input_->width - 2; ++ci, diff_x_ptr += 4, diff_y_ptr += 4)
170  {
171  diff_x_ptr[0] = point_rg[ci].x - point_lf[ci].x;
172  diff_x_ptr[1] = point_rg[ci].y - point_lf[ci].y;
173  diff_x_ptr[2] = point_rg[ci].z - point_lf[ci].z;
174 
175  diff_y_ptr[0] = point_dn[ci].x - point_up[ci].x;
176  diff_y_ptr[1] = point_dn[ci].y - point_up[ci].y;
177  diff_y_ptr[2] = point_dn[ci].z - point_up[ci].z;
178  }
179  }
180 
181  // Compute integral images
182  integral_image_DX_.setInput (diff_x_, input_->width, input_->height, 4, input_->width << 2);
183  integral_image_DY_.setInput (diff_y_, input_->width, input_->height, 4, input_->width << 2);
184  init_covariance_matrix_ = init_depth_change_ = init_simple_3d_gradient_ = false;
185  init_average_3d_gradient_ = true;
186 }
187 
188 //////////////////////////////////////////////////////////////////////////////////////////
189 template <typename PointInT, typename PointOutT> void
191 {
192  // number of DataType entries per element (equal or bigger than dimensions)
193  int element_stride = sizeof (PointInT) / sizeof (float);
194  // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
195  int row_stride = element_stride * input_->width;
196 
197  const float *data_ = reinterpret_cast<const float*> (&input_->points[0]);
198 
199  // integral image over the z - value
200  integral_image_depth_.setInput (&(data_[2]), input_->width, input_->height, element_stride, row_stride);
201  init_depth_change_ = true;
202  init_covariance_matrix_ = init_average_3d_gradient_ = init_simple_3d_gradient_ = false;
203 }
204 
205 //////////////////////////////////////////////////////////////////////////////////////////
206 template <typename PointInT, typename PointOutT> void
208  const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
209 {
210  float bad_point = std::numeric_limits<float>::quiet_NaN ();
211 
212  if (normal_estimation_method_ == COVARIANCE_MATRIX)
213  {
214  if (!init_covariance_matrix_)
215  initCovarianceMatrixMethod ();
216 
217  unsigned count = integral_image_XYZ_.getFiniteElementsCount (pos_x - (rect_width_2_), pos_y - (rect_height_2_), rect_width_, rect_height_);
218 
219  // no valid points within the rectangular reagion?
220  if (count == 0)
221  {
222  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
223  return;
224  }
225 
226  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
227  Eigen::Vector3f center;
228  typename IntegralImage2D<float, 3>::SecondOrderType so_elements;
229  center = integral_image_XYZ_.getFirstOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_).template cast<float> ();
230  so_elements = integral_image_XYZ_.getSecondOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
231 
232  covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
233  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
234  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
235  covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
236  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
237  covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
238  covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
239  float eigen_value;
240  Eigen::Vector3f eigen_vector;
241  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
242  flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
243  normal.getNormalVector3fMap () = eigen_vector;
244 
245  // Compute the curvature surface change
246  if (eigen_value > 0.0)
247  normal.curvature = fabsf (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
248  else
249  normal.curvature = 0;
250 
251  return;
252  }
253  else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
254  {
255  if (!init_average_3d_gradient_)
256  initAverage3DGradientMethod ();
257 
258  unsigned count_x = integral_image_DX_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
259  unsigned count_y = integral_image_DY_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
260  if (count_x == 0 || count_y == 0)
261  {
262  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
263  return;
264  }
265  Eigen::Vector3d gradient_x = integral_image_DX_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
266  Eigen::Vector3d gradient_y = integral_image_DY_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
267 
268  Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
269  double normal_length = normal_vector.squaredNorm ();
270  if (normal_length == 0.0f)
271  {
272  normal.getNormalVector3fMap ().setConstant (bad_point);
273  normal.curvature = bad_point;
274  return;
275  }
276 
277  normal_vector /= sqrt (normal_length);
278 
279  float nx = static_cast<float> (normal_vector [0]);
280  float ny = static_cast<float> (normal_vector [1]);
281  float nz = static_cast<float> (normal_vector [2]);
282 
283  flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
284 
285  normal.normal_x = nx;
286  normal.normal_y = ny;
287  normal.normal_z = nz;
288  normal.curvature = bad_point;
289  return;
290  }
291  else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
292  {
293  if (!init_depth_change_)
294  initAverageDepthChangeMethod ();
295 
296  // width and height are at least 3 x 3
297  unsigned count_L_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
298  unsigned count_R_z = integral_image_depth_.getFiniteElementsCount (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
299  unsigned count_U_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_);
300  unsigned count_D_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_);
301 
302  if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
303  {
304  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
305  return;
306  }
307 
308  float mean_L_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_) / count_L_z);
309  float mean_R_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_) / count_R_z);
310  float mean_U_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_) / count_U_z);
311  float mean_D_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_) / count_D_z);
312 
313  PointInT pointL = input_->points[point_index - rect_width_4_ - 1];
314  PointInT pointR = input_->points[point_index + rect_width_4_ + 1];
315  PointInT pointU = input_->points[point_index - rect_height_4_ * input_->width - 1];
316  PointInT pointD = input_->points[point_index + rect_height_4_ * input_->width + 1];
317 
318  const float mean_x_z = mean_R_z - mean_L_z;
319  const float mean_y_z = mean_D_z - mean_U_z;
320 
321  const float mean_x_x = pointR.x - pointL.x;
322  const float mean_x_y = pointR.y - pointL.y;
323  const float mean_y_x = pointD.x - pointU.x;
324  const float mean_y_y = pointD.y - pointU.y;
325 
326  float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
327  float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
328  float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
329 
330  const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
331 
332  if (normal_length == 0.0f)
333  {
334  normal.getNormalVector3fMap ().setConstant (bad_point);
335  normal.curvature = bad_point;
336  return;
337  }
338 
339  flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
340 
341  const float scale = 1.0f / sqrtf (normal_length);
342 
343  normal.normal_x = normal_x * scale;
344  normal.normal_y = normal_y * scale;
345  normal.normal_z = normal_z * scale;
346  normal.curvature = bad_point;
347 
348  return;
349  }
350  else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
351  {
352  if (!init_simple_3d_gradient_)
353  initSimple3DGradientMethod ();
354 
355  // this method does not work if lots of NaNs are in the neighborhood of the point
356  Eigen::Vector3d gradient_x = integral_image_XYZ_.getFirstOrderSum (pos_x + rect_width_2_, pos_y - rect_height_2_, 1, rect_height_) -
357  integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, 1, rect_height_);
358 
359  Eigen::Vector3d gradient_y = integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y + rect_height_2_, rect_width_, 1) -
360  integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, 1);
361  Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
362  double normal_length = normal_vector.squaredNorm ();
363  if (normal_length == 0.0f)
364  {
365  normal.getNormalVector3fMap ().setConstant (bad_point);
366  normal.curvature = bad_point;
367  return;
368  }
369 
370  normal_vector /= sqrt (normal_length);
371 
372  float nx = static_cast<float> (normal_vector [0]);
373  float ny = static_cast<float> (normal_vector [1]);
374  float nz = static_cast<float> (normal_vector [2]);
375 
376  flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
377 
378  normal.normal_x = nx;
379  normal.normal_y = ny;
380  normal.normal_z = nz;
381  normal.curvature = bad_point;
382  return;
383  }
384 
385  normal.getNormalVector3fMap ().setConstant (bad_point);
386  normal.curvature = bad_point;
387  return;
388 }
389 
390 //////////////////////////////////////////////////////////////////////////////////////////
391 template <typename T>
392 void
393 sumArea (int start_x, int start_y, int end_x, int end_y, const int width, const int height,
394  const boost::function<T(unsigned, unsigned, unsigned, unsigned)> &f,
395  T & result)
396 {
397  if (start_x < 0)
398  {
399  if (start_y < 0)
400  {
401  result += f (0, 0, end_x, end_y);
402  result += f (0, 0, -start_x, -start_y);
403  result += f (0, 0, -start_x, end_y);
404  result += f (0, 0, end_x, -start_y);
405  }
406  else if (end_y >= height)
407  {
408  result += f (0, start_y, end_x, height-1);
409  result += f (0, start_y, -start_x, height-1);
410  result += f (0, height-(end_y-(height-1)), end_x, height-1);
411  result += f (0, height-(end_y-(height-1)), -start_x, height-1);
412  }
413  else
414  {
415  result += f (0, start_y, end_x, end_y);
416  result += f (0, start_y, -start_x, end_y);
417  }
418  }
419  else if (start_y < 0)
420  {
421  if (end_x >= width)
422  {
423  result += f (start_x, 0, width-1, end_y);
424  result += f (start_x, 0, width-1, -start_y);
425  result += f (width-(end_x-(width-1)), 0, width-1, end_y);
426  result += f (width-(end_x-(width-1)), 0, width-1, -start_y);
427  }
428  else
429  {
430  result += f (start_x, 0, end_x, end_y);
431  result += f (start_x, 0, end_x, -start_y);
432  }
433  }
434  else if (end_x >= width)
435  {
436  if (end_y >= height)
437  {
438  result += f (start_x, start_y, width-1, height-1);
439  result += f (start_x, height-(end_y-(height-1)), width-1, height-1);
440  result += f (width-(end_x-(width-1)), start_y, width-1, height-1);
441  result += f (width-(end_x-(width-1)), height-(end_y-(height-1)), width-1, height-1);
442  }
443  else
444  {
445  result += f (start_x, start_y, width-1, end_y);
446  result += f (width-(end_x-(width-1)), start_y, width-1, end_y);
447  }
448  }
449  else if (end_y >= height)
450  {
451  result += f (start_x, start_y, end_x, height-1);
452  result += f (start_x, height-(end_y-(height-1)), end_x, height-1);
453  }
454  else
455  {
456  result += f (start_x, start_y, end_x, end_y);
457  }
458 }
459 
460 //////////////////////////////////////////////////////////////////////////////////////////
461 template <typename PointInT, typename PointOutT> void
463  const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
464 {
465  float bad_point = std::numeric_limits<float>::quiet_NaN ();
466 
467  const int width = input_->width;
468  const int height = input_->height;
469 
470  // ==============================================================
471  if (normal_estimation_method_ == COVARIANCE_MATRIX)
472  {
473  if (!init_covariance_matrix_)
474  initCovarianceMatrixMethod ();
475 
476  const int start_x = pos_x - rect_width_2_;
477  const int start_y = pos_y - rect_height_2_;
478  const int end_x = start_x + rect_width_;
479  const int end_y = start_y + rect_height_;
480 
481  unsigned count = 0;
482  sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_XYZ_, _1, _2, _3, _4), count);
483 
484  // no valid points within the rectangular reagion?
485  if (count == 0)
486  {
487  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
488  return;
489  }
490 
491  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
492  Eigen::Vector3f center;
493  typename IntegralImage2D<float, 3>::SecondOrderType so_elements;
494  typename IntegralImage2D<float, 3>::ElementType tmp_center;
495  typename IntegralImage2D<float, 3>::SecondOrderType tmp_so_elements;
496 
497  center[0] = 0;
498  center[1] = 0;
499  center[2] = 0;
500  tmp_center[0] = 0;
501  tmp_center[1] = 0;
502  tmp_center[2] = 0;
503  so_elements[0] = 0;
504  so_elements[1] = 0;
505  so_elements[2] = 0;
506  so_elements[3] = 0;
507  so_elements[4] = 0;
508  so_elements[5] = 0;
509 
510  sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_XYZ_, _1, _2, _3, _4), tmp_center);
511  sumArea<typename IntegralImage2D<float, 3>::SecondOrderType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getSecondOrderSumSE, &integral_image_XYZ_, _1, _2, _3, _4), so_elements);
512 
513  center[0] = float (tmp_center[0]);
514  center[1] = float (tmp_center[1]);
515  center[2] = float (tmp_center[2]);
516 
517  covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
518  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
519  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
520  covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
521  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
522  covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
523  covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
524  float eigen_value;
525  Eigen::Vector3f eigen_vector;
526  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
527  flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
528  normal.getNormalVector3fMap () = eigen_vector;
529 
530  // Compute the curvature surface change
531  if (eigen_value > 0.0)
532  normal.curvature = fabsf (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
533  else
534  normal.curvature = 0;
535 
536  return;
537  }
538  // =======================================================
539  else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
540  {
541  if (!init_average_3d_gradient_)
542  initAverage3DGradientMethod ();
543 
544  const int start_x = pos_x - rect_width_2_;
545  const int start_y = pos_y - rect_height_2_;
546  const int end_x = start_x + rect_width_;
547  const int end_y = start_y + rect_height_;
548 
549  unsigned count_x = 0;
550  unsigned count_y = 0;
551 
552  sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_DX_, _1, _2, _3, _4), count_x);
553  sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_DY_, _1, _2, _3, _4), count_y);
554 
555 
556  if (count_x == 0 || count_y == 0)
557  {
558  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
559  return;
560  }
561  Eigen::Vector3d gradient_x (0, 0, 0);
562  Eigen::Vector3d gradient_y (0, 0, 0);
563 
564  sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_DX_, _1, _2, _3, _4), gradient_x);
565  sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_DY_, _1, _2, _3, _4), gradient_y);
566 
567 
568  Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
569  double normal_length = normal_vector.squaredNorm ();
570  if (normal_length == 0.0f)
571  {
572  normal.getNormalVector3fMap ().setConstant (bad_point);
573  normal.curvature = bad_point;
574  return;
575  }
576 
577  normal_vector /= sqrt (normal_length);
578 
579  float nx = static_cast<float> (normal_vector [0]);
580  float ny = static_cast<float> (normal_vector [1]);
581  float nz = static_cast<float> (normal_vector [2]);
582 
583  flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
584 
585  normal.normal_x = nx;
586  normal.normal_y = ny;
587  normal.normal_z = nz;
588  normal.curvature = bad_point;
589  return;
590  }
591  // ======================================================
592  else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
593  {
594  if (!init_depth_change_)
595  initAverageDepthChangeMethod ();
596 
597  int point_index_L_x = pos_x - rect_width_4_ - 1;
598  int point_index_L_y = pos_y;
599  int point_index_R_x = pos_x + rect_width_4_ + 1;
600  int point_index_R_y = pos_y;
601  int point_index_U_x = pos_x - 1;
602  int point_index_U_y = pos_y - rect_height_4_;
603  int point_index_D_x = pos_x + 1;
604  int point_index_D_y = pos_y + rect_height_4_;
605 
606  if (point_index_L_x < 0)
607  point_index_L_x = -point_index_L_x;
608  if (point_index_U_x < 0)
609  point_index_U_x = -point_index_U_x;
610  if (point_index_U_y < 0)
611  point_index_U_y = -point_index_U_y;
612 
613  if (point_index_R_x >= width)
614  point_index_R_x = width-(point_index_R_x-(width-1));
615  if (point_index_D_x >= width)
616  point_index_D_x = width-(point_index_D_x-(width-1));
617  if (point_index_D_y >= height)
618  point_index_D_y = height-(point_index_D_y-(height-1));
619 
620  const int start_x_L = pos_x - rect_width_2_;
621  const int start_y_L = pos_y - rect_height_4_;
622  const int end_x_L = start_x_L + rect_width_2_;
623  const int end_y_L = start_y_L + rect_height_2_;
624 
625  const int start_x_R = pos_x + 1;
626  const int start_y_R = pos_y - rect_height_4_;
627  const int end_x_R = start_x_R + rect_width_2_;
628  const int end_y_R = start_y_R + rect_height_2_;
629 
630  const int start_x_U = pos_x - rect_width_4_;
631  const int start_y_U = pos_y - rect_height_2_;
632  const int end_x_U = start_x_U + rect_width_2_;
633  const int end_y_U = start_y_U + rect_height_2_;
634 
635  const int start_x_D = pos_x - rect_width_4_;
636  const int start_y_D = pos_y + 1;
637  const int end_x_D = start_x_D + rect_width_2_;
638  const int end_y_D = start_y_D + rect_height_2_;
639 
640  unsigned count_L_z = 0;
641  unsigned count_R_z = 0;
642  unsigned count_U_z = 0;
643  unsigned count_D_z = 0;
644 
645  sumArea<unsigned>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_L_z);
646  sumArea<unsigned>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_R_z);
647  sumArea<unsigned>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_U_z);
648  sumArea<unsigned>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_D_z);
649 
650  if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
651  {
652  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
653  return;
654  }
655 
656  float mean_L_z = 0;
657  float mean_R_z = 0;
658  float mean_U_z = 0;
659  float mean_D_z = 0;
660 
661  sumArea<float>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_L_z);
662  sumArea<float>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_R_z);
663  sumArea<float>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_U_z);
664  sumArea<float>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_D_z);
665 
666  mean_L_z /= float (count_L_z);
667  mean_R_z /= float (count_R_z);
668  mean_U_z /= float (count_U_z);
669  mean_D_z /= float (count_D_z);
670 
671 
672  PointInT pointL = input_->points[point_index_L_y*width + point_index_L_x];
673  PointInT pointR = input_->points[point_index_R_y*width + point_index_R_x];
674  PointInT pointU = input_->points[point_index_U_y*width + point_index_U_x];
675  PointInT pointD = input_->points[point_index_D_y*width + point_index_D_x];
676 
677  const float mean_x_z = mean_R_z - mean_L_z;
678  const float mean_y_z = mean_D_z - mean_U_z;
679 
680  const float mean_x_x = pointR.x - pointL.x;
681  const float mean_x_y = pointR.y - pointL.y;
682  const float mean_y_x = pointD.x - pointU.x;
683  const float mean_y_y = pointD.y - pointU.y;
684 
685  float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
686  float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
687  float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
688 
689  const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
690 
691  if (normal_length == 0.0f)
692  {
693  normal.getNormalVector3fMap ().setConstant (bad_point);
694  normal.curvature = bad_point;
695  return;
696  }
697 
698  flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
699 
700  const float scale = 1.0f / sqrtf (normal_length);
701 
702  normal.normal_x = normal_x * scale;
703  normal.normal_y = normal_y * scale;
704  normal.normal_z = normal_z * scale;
705  normal.curvature = bad_point;
706 
707  return;
708  }
709  // ========================================================
710  else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
711  {
712  PCL_THROW_EXCEPTION (PCLException, "BORDER_POLICY_MIRROR not supported for normal estimation method SIMPLE_3D_GRADIENT");
713  }
714 
715  normal.getNormalVector3fMap ().setConstant (bad_point);
716  normal.curvature = bad_point;
717  return;
718 }
719 
720 //////////////////////////////////////////////////////////////////////////////////////////
721 template <typename PointInT, typename PointOutT> void
723 {
724  output.sensor_origin_ = input_->sensor_origin_;
725  output.sensor_orientation_ = input_->sensor_orientation_;
726 
727  float bad_point = std::numeric_limits<float>::quiet_NaN ();
728 
729  // compute depth-change map
730  unsigned char * depthChangeMap = new unsigned char[input_->points.size ()];
731  memset (depthChangeMap, 255, input_->points.size ());
732 
733  unsigned index = 0;
734  for (unsigned int ri = 0; ri < input_->height-1; ++ri)
735  {
736  for (unsigned int ci = 0; ci < input_->width-1; ++ci, ++index)
737  {
738  index = ri * input_->width + ci;
739 
740  const float depth = input_->points [index].z;
741  const float depthR = input_->points [index + 1].z;
742  const float depthD = input_->points [index + input_->width].z;
743 
744  //const float depthDependendDepthChange = (max_depth_change_factor_ * (fabs(depth)+1.0f))/(500.0f*0.001f);
745  const float depthDependendDepthChange = (max_depth_change_factor_ * (fabsf (depth) + 1.0f) * 2.0f);
746 
747  if (fabs (depth - depthR) > depthDependendDepthChange
748  || !pcl_isfinite (depth) || !pcl_isfinite (depthR))
749  {
750  depthChangeMap[index] = 0;
751  depthChangeMap[index+1] = 0;
752  }
753  if (fabs (depth - depthD) > depthDependendDepthChange
754  || !pcl_isfinite (depth) || !pcl_isfinite (depthD))
755  {
756  depthChangeMap[index] = 0;
757  depthChangeMap[index + input_->width] = 0;
758  }
759  }
760  }
761 
762  // compute distance map
763  //float *distanceMap = new float[input_->points.size ()];
764  if (distance_map_ != NULL) delete[] distance_map_;
765  distance_map_ = new float[input_->points.size ()];
766  float *distanceMap = distance_map_;
767  for (size_t index = 0; index < input_->points.size (); ++index)
768  {
769  if (depthChangeMap[index] == 0)
770  distanceMap[index] = 0.0f;
771  else
772  distanceMap[index] = static_cast<float> (input_->width + input_->height);
773  }
774 
775  // first pass
776  float* previous_row = distanceMap;
777  float* current_row = previous_row + input_->width;
778  for (size_t ri = 1; ri < input_->height; ++ri)
779  {
780  for (size_t ci = 1; ci < input_->width; ++ci)
781  {
782  const float upLeft = previous_row [ci - 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci-1] + 1.4f;
783  const float up = previous_row [ci] + 1.0f; //distanceMap[(ri-1)*input_->width + ci] + 1.0f;
784  const float upRight = previous_row [ci + 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci+1] + 1.4f;
785  const float left = current_row [ci - 1] + 1.0f; //distanceMap[ri*input_->width + ci-1] + 1.0f;
786  const float center = current_row [ci]; //distanceMap[ri*input_->width + ci];
787 
788  const float minValue = std::min (std::min (upLeft, up), std::min (left, upRight));
789 
790  if (minValue < center)
791  current_row [ci] = minValue; //distanceMap[ri * input_->width + ci] = minValue;
792  }
793  previous_row = current_row;
794  current_row += input_->width;
795  }
796 
797  float* next_row = distanceMap + input_->width * (input_->height - 1);
798  current_row = next_row - input_->width;
799  // second pass
800  for (int ri = input_->height-2; ri >= 0; --ri)
801  {
802  for (int ci = input_->width-2; ci >= 0; --ci)
803  {
804  const float lowerLeft = next_row [ci - 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci-1] + 1.4f;
805  const float lower = next_row [ci] + 1.0f; //distanceMap[(ri+1)*input_->width + ci] + 1.0f;
806  const float lowerRight = next_row [ci + 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci+1] + 1.4f;
807  const float right = current_row [ci + 1] + 1.0f; //distanceMap[ri*input_->width + ci+1] + 1.0f;
808  const float center = current_row [ci]; //distanceMap[ri*input_->width + ci];
809 
810  const float minValue = std::min (std::min (lowerLeft, lower), std::min (right, lowerRight));
811 
812  if (minValue < center)
813  current_row [ci] = minValue; //distanceMap[ri*input_->width + ci] = minValue;
814  }
815  next_row = current_row;
816  current_row -= input_->width;
817  }
818 
819  if (border_policy_ == BORDER_POLICY_IGNORE)
820  {
821  // Set all normals that we do not touch to NaN
822  // top and bottom borders
823  // That sets the output density to false!
824  output.is_dense = false;
825  unsigned border = int(normal_smoothing_size_);
826  PointOutT* vec1 = &output [0];
827  PointOutT* vec2 = vec1 + input_->width * (input_->height - border);
828 
829  size_t count = border * input_->width;
830  for (size_t idx = 0; idx < count; ++idx)
831  {
832  vec1 [idx].getNormalVector3fMap ().setConstant (bad_point);
833  vec1 [idx].curvature = bad_point;
834  vec2 [idx].getNormalVector3fMap ().setConstant (bad_point);
835  vec2 [idx].curvature = bad_point;
836  }
837 
838  // left and right borders actually columns
839  vec1 = &output [border * input_->width];
840  vec2 = vec1 + input_->width - border;
841  for (size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width)
842  {
843  for (size_t ci = 0; ci < border; ++ci)
844  {
845  vec1 [ci].getNormalVector3fMap ().setConstant (bad_point);
846  vec1 [ci].curvature = bad_point;
847  vec2 [ci].getNormalVector3fMap ().setConstant (bad_point);
848  vec2 [ci].curvature = bad_point;
849  }
850  }
851 
852  if (use_depth_dependent_smoothing_)
853  {
854  index = border + input_->width * border;
855  unsigned skip = (border << 1);
856  for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
857  {
858  for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
859  {
860  index = ri * input_->width + ci;
861 
862  const float depth = input_->points[index].z;
863  if (!pcl_isfinite (depth))
864  {
865  output[index].getNormalVector3fMap ().setConstant (bad_point);
866  output[index].curvature = bad_point;
867  continue;
868  }
869 
870  float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
871 
872  if (smoothing > 2.0f)
873  {
874  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
875  computePointNormal (ci, ri, index, output [index]);
876  }
877  else
878  {
879  output[index].getNormalVector3fMap ().setConstant (bad_point);
880  output[index].curvature = bad_point;
881  }
882  }
883  }
884  }
885  else
886  {
887  float smoothing_constant = normal_smoothing_size_;
888 
889  index = border + input_->width * border;
890  unsigned skip = (border << 1);
891  for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
892  {
893  for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
894  {
895  index = ri * input_->width + ci;
896 
897  if (!pcl_isfinite (input_->points[index].z))
898  {
899  output [index].getNormalVector3fMap ().setConstant (bad_point);
900  output [index].curvature = bad_point;
901  continue;
902  }
903 
904  float smoothing = (std::min)(distanceMap[index], smoothing_constant);
905 
906  if (smoothing > 2.0f)
907  {
908  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
909  computePointNormal (ci, ri, index, output [index]);
910  }
911  else
912  {
913  output [index].getNormalVector3fMap ().setConstant (bad_point);
914  output [index].curvature = bad_point;
915  }
916  }
917  }
918  }
919  }
920  else if (border_policy_ == BORDER_POLICY_MIRROR)
921  {
922  output.is_dense = false;
923 
924  if (use_depth_dependent_smoothing_)
925  {
926  //index = 0;
927  //unsigned skip = 0;
928  //for (unsigned ri = 0; ri < input_->height; ++ri, index += skip)
929  for (unsigned ri = 0; ri < input_->height; ++ri)
930  {
931  //for (unsigned ci = 0; ci < input_->width; ++ci, ++index)
932  for (unsigned ci = 0; ci < input_->width; ++ci)
933  {
934  index = ri * input_->width + ci;
935 
936  const float depth = input_->points[index].z;
937  if (!pcl_isfinite (depth))
938  {
939  output[index].getNormalVector3fMap ().setConstant (bad_point);
940  output[index].curvature = bad_point;
941  continue;
942  }
943 
944  float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
945 
946  if (smoothing > 2.0f)
947  {
948  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
949  computePointNormalMirror (ci, ri, index, output [index]);
950  }
951  else
952  {
953  output[index].getNormalVector3fMap ().setConstant (bad_point);
954  output[index].curvature = bad_point;
955  }
956  }
957  }
958  }
959  else
960  {
961  float smoothing_constant = normal_smoothing_size_;
962 
963  //index = border + input_->width * border;
964  //unsigned skip = (border << 1);
965  //for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
966  for (unsigned ri = 0; ri < input_->height; ++ri)
967  {
968  //for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
969  for (unsigned ci = 0; ci < input_->width; ++ci)
970  {
971  index = ri * input_->width + ci;
972 
973  if (!pcl_isfinite (input_->points[index].z))
974  {
975  output [index].getNormalVector3fMap ().setConstant (bad_point);
976  output [index].curvature = bad_point;
977  continue;
978  }
979 
980  float smoothing = (std::min)(distanceMap[index], smoothing_constant);
981 
982  if (smoothing > 2.0f)
983  {
984  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
985  computePointNormalMirror (ci, ri, index, output [index]);
986  }
987  else
988  {
989  output [index].getNormalVector3fMap ().setConstant (bad_point);
990  output [index].curvature = bad_point;
991  }
992  }
993  }
994  }
995  }
996 
997  delete[] depthChangeMap;
998  //delete[] distanceMap;
999 }
1000 
1001 //////////////////////////////////////////////////////////////////////////////////////////
1002 template <typename PointInT, typename PointOutT> bool
1004 {
1005  if (!input_->isOrganized ())
1006  {
1007  PCL_ERROR ("[pcl::IntegralImageNormalEstimation::initCompute] Input dataset is not organized (height = 1).\n");
1008  return (false);
1009  }
1010  return (Feature<PointInT, PointOutT>::initCompute ());
1011 }
1012 
1013 #define PCL_INSTANTIATE_IntegralImageNormalEstimation(T,NT) template class PCL_EXPORTS pcl::IntegralImageNormalEstimation<T,NT>;
1014 
1015 #endif
1016 
ElementType getFirstOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the first order sum within a given rectangle.
SecondOrderType getSecondOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the second order sum within a given rectangle.
void computeFeature(PointCloudOut &output)
Computes the normal for the complete cloud.
Determines an integral image representation for a given organized data array.
void setRectSize(const int width, const int height)
Set the regions size which is considered for normal estimation.
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition: exceptions.h:195
void computePointNormal(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
struct pcl::_PointXYZHSV EIGEN_ALIGN16
void computePointNormalMirror(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position with mirroring for border handling. ...
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:421
Surface normal estimation on organized data using integral images.
void computePointNormal(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &plane_parameters, float &curvature)
Compute the Least-Squares plane fit for a given set of points, and return the estimated plane paramet...
Definition: normal_3d.h:60
virtual ~IntegralImageNormalEstimation()
Destructor.
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:65
void flipNormalTowardsViewpoint(const PointT &point, float vp_x, float vp_y, float vp_z, Eigen::Matrix< Scalar, 4, 1 > &normal)
Flip (in place) the estimated normal of a point towards a given viewpoint.
Definition: normal_3d.h:119
void initData()
Initialize the data structures, based on the normal estimation method chosen.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Definition: eigen.h:304
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:423