Point Cloud Library (PCL)  1.8.1-dev
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 region?
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 / std::sqrt (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 region?
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 / std::sqrt (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 (indices_->size () < input_->size ())
820  computeFeaturePart (distanceMap, bad_point, output);
821  else
822  computeFeatureFull (distanceMap, bad_point, output);
823 
824  delete[] depthChangeMap;
825 }
826 
827 //////////////////////////////////////////////////////////////////////////////////////////
828 template <typename PointInT, typename PointOutT> void
830  const float &bad_point,
831  PointCloudOut &output)
832 {
833  unsigned index = 0;
834 
835  if (border_policy_ == BORDER_POLICY_IGNORE)
836  {
837  // Set all normals that we do not touch to NaN
838  // top and bottom borders
839  // That sets the output density to false!
840  output.is_dense = false;
841  unsigned border = int(normal_smoothing_size_);
842  PointOutT* vec1 = &output [0];
843  PointOutT* vec2 = vec1 + input_->width * (input_->height - border);
844 
845  size_t count = border * input_->width;
846  for (size_t idx = 0; idx < count; ++idx)
847  {
848  vec1 [idx].getNormalVector3fMap ().setConstant (bad_point);
849  vec1 [idx].curvature = bad_point;
850  vec2 [idx].getNormalVector3fMap ().setConstant (bad_point);
851  vec2 [idx].curvature = bad_point;
852  }
853 
854  // left and right borders actually columns
855  vec1 = &output [border * input_->width];
856  vec2 = vec1 + input_->width - border;
857  for (size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width)
858  {
859  for (size_t ci = 0; ci < border; ++ci)
860  {
861  vec1 [ci].getNormalVector3fMap ().setConstant (bad_point);
862  vec1 [ci].curvature = bad_point;
863  vec2 [ci].getNormalVector3fMap ().setConstant (bad_point);
864  vec2 [ci].curvature = bad_point;
865  }
866  }
867 
868  if (use_depth_dependent_smoothing_)
869  {
870  index = border + input_->width * border;
871  unsigned skip = (border << 1);
872  for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
873  {
874  for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
875  {
876  index = ri * input_->width + ci;
877 
878  const float depth = input_->points[index].z;
879  if (!pcl_isfinite (depth))
880  {
881  output[index].getNormalVector3fMap ().setConstant (bad_point);
882  output[index].curvature = bad_point;
883  continue;
884  }
885 
886  float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
887 
888  if (smoothing > 2.0f)
889  {
890  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
891  computePointNormal (ci, ri, index, output [index]);
892  }
893  else
894  {
895  output[index].getNormalVector3fMap ().setConstant (bad_point);
896  output[index].curvature = bad_point;
897  }
898  }
899  }
900  }
901  else
902  {
903  float smoothing_constant = normal_smoothing_size_;
904 
905  index = border + input_->width * border;
906  unsigned skip = (border << 1);
907  for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
908  {
909  for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
910  {
911  index = ri * input_->width + ci;
912 
913  if (!pcl_isfinite (input_->points[index].z))
914  {
915  output [index].getNormalVector3fMap ().setConstant (bad_point);
916  output [index].curvature = bad_point;
917  continue;
918  }
919 
920  float smoothing = (std::min)(distanceMap[index], smoothing_constant);
921 
922  if (smoothing > 2.0f)
923  {
924  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
925  computePointNormal (ci, ri, index, output [index]);
926  }
927  else
928  {
929  output [index].getNormalVector3fMap ().setConstant (bad_point);
930  output [index].curvature = bad_point;
931  }
932  }
933  }
934  }
935  }
936  else if (border_policy_ == BORDER_POLICY_MIRROR)
937  {
938  output.is_dense = false;
939 
940  if (use_depth_dependent_smoothing_)
941  {
942  //index = 0;
943  //unsigned skip = 0;
944  //for (unsigned ri = 0; ri < input_->height; ++ri, index += skip)
945  for (unsigned ri = 0; ri < input_->height; ++ri)
946  {
947  //for (unsigned ci = 0; ci < input_->width; ++ci, ++index)
948  for (unsigned ci = 0; ci < input_->width; ++ci)
949  {
950  index = ri * input_->width + ci;
951 
952  const float depth = input_->points[index].z;
953  if (!pcl_isfinite (depth))
954  {
955  output[index].getNormalVector3fMap ().setConstant (bad_point);
956  output[index].curvature = bad_point;
957  continue;
958  }
959 
960  float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
961 
962  if (smoothing > 2.0f)
963  {
964  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
965  computePointNormalMirror (ci, ri, index, output [index]);
966  }
967  else
968  {
969  output[index].getNormalVector3fMap ().setConstant (bad_point);
970  output[index].curvature = bad_point;
971  }
972  }
973  }
974  }
975  else
976  {
977  float smoothing_constant = normal_smoothing_size_;
978 
979  //index = border + input_->width * border;
980  //unsigned skip = (border << 1);
981  //for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
982  for (unsigned ri = 0; ri < input_->height; ++ri)
983  {
984  //for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
985  for (unsigned ci = 0; ci < input_->width; ++ci)
986  {
987  index = ri * input_->width + ci;
988 
989  if (!pcl_isfinite (input_->points[index].z))
990  {
991  output [index].getNormalVector3fMap ().setConstant (bad_point);
992  output [index].curvature = bad_point;
993  continue;
994  }
995 
996  float smoothing = (std::min)(distanceMap[index], smoothing_constant);
997 
998  if (smoothing > 2.0f)
999  {
1000  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1001  computePointNormalMirror (ci, ri, index, output [index]);
1002  }
1003  else
1004  {
1005  output [index].getNormalVector3fMap ().setConstant (bad_point);
1006  output [index].curvature = bad_point;
1007  }
1008  }
1009  }
1010  }
1011  }
1012 }
1013 
1014 ///////////////////////////////////////////////////////////////////////////////////////////
1015 template <typename PointInT, typename PointOutT> void
1017  const float &bad_point,
1018  PointCloudOut &output)
1019 {
1020  if (border_policy_ == BORDER_POLICY_IGNORE)
1021  {
1022  output.is_dense = false;
1023  unsigned border = int(normal_smoothing_size_);
1024  unsigned bottom = input_->height > border ? input_->height - border : 0;
1025  unsigned right = input_->width > border ? input_->width - border : 0;
1026  if (use_depth_dependent_smoothing_)
1027  {
1028  // Iterating over the entire index vector
1029  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1030  {
1031  unsigned pt_index = (*indices_)[idx];
1032  unsigned u = pt_index % input_->width;
1033  unsigned v = pt_index / input_->width;
1034  if (v < border || v > bottom)
1035  {
1036  output.points[idx].getNormalVector3fMap ().setConstant (bad_point);
1037  output.points[idx].curvature = bad_point;
1038  continue;
1039  }
1040 
1041  if (u < border || v > right)
1042  {
1043  output.points[idx].getNormalVector3fMap ().setConstant (bad_point);
1044  output.points[idx].curvature = bad_point;
1045  continue;
1046  }
1047 
1048  const float depth = input_->points[pt_index].z;
1049  if (!pcl_isfinite (depth))
1050  {
1051  output.points[idx].getNormalVector3fMap ().setConstant (bad_point);
1052  output.points[idx].curvature = bad_point;
1053  continue;
1054  }
1055 
1056  float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
1057  if (smoothing > 2.0f)
1058  {
1059  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1060  computePointNormal (u, v, pt_index, output [idx]);
1061  }
1062  else
1063  {
1064  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1065  output[idx].curvature = bad_point;
1066  }
1067  }
1068  }
1069  else
1070  {
1071  float smoothing_constant = normal_smoothing_size_;
1072  // Iterating over the entire index vector
1073  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1074  {
1075  unsigned pt_index = (*indices_)[idx];
1076  unsigned u = pt_index % input_->width;
1077  unsigned v = pt_index / input_->width;
1078  if (v < border || v > bottom)
1079  {
1080  output.points[idx].getNormalVector3fMap ().setConstant (bad_point);
1081  output.points[idx].curvature = bad_point;
1082  continue;
1083  }
1084 
1085  if (u < border || v > right)
1086  {
1087  output.points[idx].getNormalVector3fMap ().setConstant (bad_point);
1088  output.points[idx].curvature = bad_point;
1089  continue;
1090  }
1091 
1092  if (!pcl_isfinite (input_->points[pt_index].z))
1093  {
1094  output [idx].getNormalVector3fMap ().setConstant (bad_point);
1095  output [idx].curvature = bad_point;
1096  continue;
1097  }
1098 
1099  float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1100 
1101  if (smoothing > 2.0f)
1102  {
1103  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1104  computePointNormal (u, v, pt_index, output [idx]);
1105  }
1106  else
1107  {
1108  output [pt_index].getNormalVector3fMap ().setConstant (bad_point);
1109  output [pt_index].curvature = bad_point;
1110  }
1111  }
1112  }
1113  }// border_policy_ == BORDER_POLICY_IGNORE
1114  else if (border_policy_ == BORDER_POLICY_MIRROR)
1115  {
1116  output.is_dense = false;
1117 
1118  if (use_depth_dependent_smoothing_)
1119  {
1120  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1121  {
1122  unsigned pt_index = (*indices_)[idx];
1123  unsigned u = pt_index % input_->width;
1124  unsigned v = pt_index / input_->width;
1125 
1126  const float depth = input_->points[pt_index].z;
1127  if (!pcl_isfinite (depth))
1128  {
1129  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1130  output[idx].curvature = bad_point;
1131  continue;
1132  }
1133 
1134  float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
1135 
1136  if (smoothing > 2.0f)
1137  {
1138  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1139  computePointNormalMirror (u, v, pt_index, output [idx]);
1140  }
1141  else
1142  {
1143  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1144  output[idx].curvature = bad_point;
1145  }
1146  }
1147  }
1148  else
1149  {
1150  float smoothing_constant = normal_smoothing_size_;
1151  for (size_t idx = 0; idx < indices_->size (); ++idx)
1152  {
1153  unsigned pt_index = (*indices_)[idx];
1154  unsigned u = pt_index % input_->width;
1155  unsigned v = pt_index / input_->width;
1156 
1157  if (!pcl_isfinite (input_->points[pt_index].z))
1158  {
1159  output [idx].getNormalVector3fMap ().setConstant (bad_point);
1160  output [idx].curvature = bad_point;
1161  continue;
1162  }
1163 
1164  float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1165 
1166  if (smoothing > 2.0f)
1167  {
1168  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1169  computePointNormalMirror (u, v, pt_index, output [idx]);
1170  }
1171  else
1172  {
1173  output [idx].getNormalVector3fMap ().setConstant (bad_point);
1174  output [idx].curvature = bad_point;
1175  }
1176  }
1177  }
1178  } // border_policy_ == BORDER_POLICY_MIRROR
1179 }
1180 
1181 //////////////////////////////////////////////////////////////////////////////////////////
1182 template <typename PointInT, typename PointOutT> bool
1184 {
1185  if (!input_->isOrganized ())
1186  {
1187  PCL_ERROR ("[pcl::IntegralImageNormalEstimation::initCompute] Input dataset is not organized (height = 1).\n");
1188  return (false);
1189  }
1190  return (Feature<PointInT, PointOutT>::initCompute ());
1191 }
1192 
1193 #define PCL_INSTANTIATE_IntegralImageNormalEstimation(T,NT) template class PCL_EXPORTS pcl::IntegralImageNormalEstimation<T,NT>;
1194 
1195 #endif
1196 
SecondOrderType getSecondOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the second order sum within a given rectangle.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:421
virtual ~IntegralImageNormalEstimation()
Destructor.
struct pcl::PointXYZIEdge EIGEN_ALIGN16
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:64
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. ...
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 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
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:423
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
void setRectSize(const int width, const int height)
Set the regions size which is considered for normal estimation.
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:121
void computeFeature(PointCloudOut &output)
Computes the normal for the complete cloud or only indices_ if provided.
void initData()
Initialize the data structures, based on the normal estimation method chosen.
Surface normal estimation on organized data using integral images.
void computeFeatureFull(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for the complete cloud.
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition: exceptions.h:194
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Determines an integral image representation for a given organized data array.
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.hpp:251
void computeFeaturePart(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for part of the cloud specified by indices_.
ElementType getFirstOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the first order sum within a given rectangle.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418