Point Cloud Library (PCL)  1.11.0-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>
46 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::~IntegralImageNormalEstimation ()
47 {
48  delete[] diff_x_;
49  delete[] diff_y_;
50  delete[] depth_data_;
51  delete[] distance_map_;
52 }
53 
54 //////////////////////////////////////////////////////////////////////////////////////////
55 template <typename PointInT, typename PointOutT> void
56 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initData ()
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  delete[] diff_x_;
72  delete[] diff_y_;
73  delete[] depth_data_;
74  delete[] distance_map_;
75  diff_x_ = nullptr;
76  diff_y_ = nullptr;
77  depth_data_ = nullptr;
78  distance_map_ = nullptr;
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
93 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::setRectSize (const int width, const int height)
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
105 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initSimple3DGradientMethod ()
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
123 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initCovarianceMatrixMethod ()
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
141 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initAverage3DGradientMethod ()
142 {
143  std::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 (std::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 (std::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
190 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initAverageDepthChangeMethod ()
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
207 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computePointNormal (
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 = std::abs (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  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  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  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 std::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
462 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computePointNormalMirror (
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  auto cb_xyz_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getFiniteElementsCountSE (p1, p2, p3, p4); };
483  sumArea<unsigned> (start_x, start_y, end_x, end_y, width, height, cb_xyz_fecse, count);
484 
485  // no valid points within the rectangular region?
486  if (count == 0)
487  {
488  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
489  return;
490  }
491 
492  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
493  Eigen::Vector3f center;
494  typename IntegralImage2D<float, 3>::SecondOrderType so_elements;
495  typename IntegralImage2D<float, 3>::ElementType tmp_center;
496  typename IntegralImage2D<float, 3>::SecondOrderType tmp_so_elements;
497 
498  center[0] = 0;
499  center[1] = 0;
500  center[2] = 0;
501  tmp_center[0] = 0;
502  tmp_center[1] = 0;
503  tmp_center[2] = 0;
504  so_elements[0] = 0;
505  so_elements[1] = 0;
506  so_elements[2] = 0;
507  so_elements[3] = 0;
508  so_elements[4] = 0;
509  so_elements[5] = 0;
510 
511  auto cb_xyz_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getFirstOrderSumSE (p1, p2, p3, p4); };
512  sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_fosse, tmp_center);
513  auto cb_xyz_sosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getSecondOrderSumSE (p1, p2, p3, p4); };
514  sumArea<typename IntegralImage2D<float, 3>::SecondOrderType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_sosse, so_elements);
515 
516  center[0] = float (tmp_center[0]);
517  center[1] = float (tmp_center[1]);
518  center[2] = float (tmp_center[2]);
519 
520  covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
521  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
522  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
523  covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
524  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
525  covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
526  covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
527  float eigen_value;
528  Eigen::Vector3f eigen_vector;
529  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
530  flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
531  normal.getNormalVector3fMap () = eigen_vector;
532 
533  // Compute the curvature surface change
534  if (eigen_value > 0.0)
535  normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
536  else
537  normal.curvature = 0;
538 
539  return;
540  }
541  // =======================================================
542  if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
543  {
544  if (!init_average_3d_gradient_)
545  initAverage3DGradientMethod ();
546 
547  const int start_x = pos_x - rect_width_2_;
548  const int start_y = pos_y - rect_height_2_;
549  const int end_x = start_x + rect_width_;
550  const int end_y = start_y + rect_height_;
551 
552  unsigned count_x = 0;
553  unsigned count_y = 0;
554 
555  auto cb_dx_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DX_.getFiniteElementsCountSE (p1, p2, p3, p4); };
556  sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, cb_dx_fecse, count_x);
557  auto cb_dy_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DY_.getFiniteElementsCountSE (p1, p2, p3, p4); };
558  sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, cb_dy_fecse, count_y);
559 
560 
561  if (count_x == 0 || count_y == 0)
562  {
563  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
564  return;
565  }
566  Eigen::Vector3d gradient_x (0, 0, 0);
567  Eigen::Vector3d gradient_y (0, 0, 0);
568 
569  auto cb_dx_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DX_.getFirstOrderSumSE (p1, p2, p3, p4); };
570  sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dx_fosse, gradient_x);
571  auto cb_dy_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DY_.getFirstOrderSumSE (p1, p2, p3, p4); };
572  sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dy_fosse, gradient_y);
573 
574 
575  Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
576  double normal_length = normal_vector.squaredNorm ();
577  if (normal_length == 0.0f)
578  {
579  normal.getNormalVector3fMap ().setConstant (bad_point);
580  normal.curvature = bad_point;
581  return;
582  }
583 
584  normal_vector /= sqrt (normal_length);
585 
586  float nx = static_cast<float> (normal_vector [0]);
587  float ny = static_cast<float> (normal_vector [1]);
588  float nz = static_cast<float> (normal_vector [2]);
589 
590  flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
591 
592  normal.normal_x = nx;
593  normal.normal_y = ny;
594  normal.normal_z = nz;
595  normal.curvature = bad_point;
596  return;
597  }
598  // ======================================================
599  if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
600  {
601  if (!init_depth_change_)
602  initAverageDepthChangeMethod ();
603 
604  int point_index_L_x = pos_x - rect_width_4_ - 1;
605  int point_index_L_y = pos_y;
606  int point_index_R_x = pos_x + rect_width_4_ + 1;
607  int point_index_R_y = pos_y;
608  int point_index_U_x = pos_x - 1;
609  int point_index_U_y = pos_y - rect_height_4_;
610  int point_index_D_x = pos_x + 1;
611  int point_index_D_y = pos_y + rect_height_4_;
612 
613  if (point_index_L_x < 0)
614  point_index_L_x = -point_index_L_x;
615  if (point_index_U_x < 0)
616  point_index_U_x = -point_index_U_x;
617  if (point_index_U_y < 0)
618  point_index_U_y = -point_index_U_y;
619 
620  if (point_index_R_x >= width)
621  point_index_R_x = width-(point_index_R_x-(width-1));
622  if (point_index_D_x >= width)
623  point_index_D_x = width-(point_index_D_x-(width-1));
624  if (point_index_D_y >= height)
625  point_index_D_y = height-(point_index_D_y-(height-1));
626 
627  const int start_x_L = pos_x - rect_width_2_;
628  const int start_y_L = pos_y - rect_height_4_;
629  const int end_x_L = start_x_L + rect_width_2_;
630  const int end_y_L = start_y_L + rect_height_2_;
631 
632  const int start_x_R = pos_x + 1;
633  const int start_y_R = pos_y - rect_height_4_;
634  const int end_x_R = start_x_R + rect_width_2_;
635  const int end_y_R = start_y_R + rect_height_2_;
636 
637  const int start_x_U = pos_x - rect_width_4_;
638  const int start_y_U = pos_y - rect_height_2_;
639  const int end_x_U = start_x_U + rect_width_2_;
640  const int end_y_U = start_y_U + rect_height_2_;
641 
642  const int start_x_D = pos_x - rect_width_4_;
643  const int start_y_D = pos_y + 1;
644  const int end_x_D = start_x_D + rect_width_2_;
645  const int end_y_D = start_y_D + rect_height_2_;
646 
647  unsigned count_L_z = 0;
648  unsigned count_R_z = 0;
649  unsigned count_U_z = 0;
650  unsigned count_D_z = 0;
651 
652  auto cb_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_depth_.getFiniteElementsCountSE (p1, p2, p3, p4); };
653  sumArea<unsigned>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fecse, count_L_z);
654  sumArea<unsigned>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fecse, count_R_z);
655  sumArea<unsigned>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fecse, count_U_z);
656  sumArea<unsigned>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fecse, count_D_z);
657 
658  if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
659  {
660  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
661  return;
662  }
663 
664  float mean_L_z = 0;
665  float mean_R_z = 0;
666  float mean_U_z = 0;
667  float mean_D_z = 0;
668 
669  auto cb_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_depth_.getFirstOrderSumSE (p1, p2, p3, p4); };
670  sumArea<float>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fosse, mean_L_z);
671  sumArea<float>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fosse, mean_R_z);
672  sumArea<float>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fosse, mean_U_z);
673  sumArea<float>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fosse, mean_D_z);
674 
675  mean_L_z /= float (count_L_z);
676  mean_R_z /= float (count_R_z);
677  mean_U_z /= float (count_U_z);
678  mean_D_z /= float (count_D_z);
679 
680 
681  PointInT pointL = input_->points[point_index_L_y*width + point_index_L_x];
682  PointInT pointR = input_->points[point_index_R_y*width + point_index_R_x];
683  PointInT pointU = input_->points[point_index_U_y*width + point_index_U_x];
684  PointInT pointD = input_->points[point_index_D_y*width + point_index_D_x];
685 
686  const float mean_x_z = mean_R_z - mean_L_z;
687  const float mean_y_z = mean_D_z - mean_U_z;
688 
689  const float mean_x_x = pointR.x - pointL.x;
690  const float mean_x_y = pointR.y - pointL.y;
691  const float mean_y_x = pointD.x - pointU.x;
692  const float mean_y_y = pointD.y - pointU.y;
693 
694  float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
695  float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
696  float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
697 
698  const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
699 
700  if (normal_length == 0.0f)
701  {
702  normal.getNormalVector3fMap ().setConstant (bad_point);
703  normal.curvature = bad_point;
704  return;
705  }
706 
707  flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
708 
709  const float scale = 1.0f / std::sqrt (normal_length);
710 
711  normal.normal_x = normal_x * scale;
712  normal.normal_y = normal_y * scale;
713  normal.normal_z = normal_z * scale;
714  normal.curvature = bad_point;
715 
716  return;
717  }
718  // ========================================================
719  if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
720  {
721  PCL_THROW_EXCEPTION (PCLException, "BORDER_POLICY_MIRROR not supported for normal estimation method SIMPLE_3D_GRADIENT");
722  }
723 
724  normal.getNormalVector3fMap ().setConstant (bad_point);
725  normal.curvature = bad_point;
726  return;
727 }
728 
729 //////////////////////////////////////////////////////////////////////////////////////////
730 template <typename PointInT, typename PointOutT> void
731 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
732 {
733  output.sensor_origin_ = input_->sensor_origin_;
734  output.sensor_orientation_ = input_->sensor_orientation_;
735 
736  float bad_point = std::numeric_limits<float>::quiet_NaN ();
737 
738  // compute depth-change map
739  unsigned char * depthChangeMap = new unsigned char[input_->points.size ()];
740  memset (depthChangeMap, 255, input_->points.size ());
741 
742  unsigned index = 0;
743  for (unsigned int ri = 0; ri < input_->height-1; ++ri)
744  {
745  for (unsigned int ci = 0; ci < input_->width-1; ++ci, ++index)
746  {
747  index = ri * input_->width + ci;
748 
749  const float depth = input_->points [index].z;
750  const float depthR = input_->points [index + 1].z;
751  const float depthD = input_->points [index + input_->width].z;
752 
753  //const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs(depth)+1.0f))/(500.0f*0.001f);
754  const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs (depth) + 1.0f) * 2.0f);
755 
756  if (std::fabs (depth - depthR) > depthDependendDepthChange
757  || !std::isfinite (depth) || !std::isfinite (depthR))
758  {
759  depthChangeMap[index] = 0;
760  depthChangeMap[index+1] = 0;
761  }
762  if (std::fabs (depth - depthD) > depthDependendDepthChange
763  || !std::isfinite (depth) || !std::isfinite (depthD))
764  {
765  depthChangeMap[index] = 0;
766  depthChangeMap[index + input_->width] = 0;
767  }
768  }
769  }
770 
771  // compute distance map
772  //float *distanceMap = new float[input_->points.size ()];
773  delete[] distance_map_;
774  distance_map_ = new float[input_->points.size ()];
775  float *distanceMap = distance_map_;
776  for (std::size_t index = 0; index < input_->points.size (); ++index)
777  {
778  if (depthChangeMap[index] == 0)
779  distanceMap[index] = 0.0f;
780  else
781  distanceMap[index] = static_cast<float> (input_->width + input_->height);
782  }
783 
784  // first pass
785  float* previous_row = distanceMap;
786  float* current_row = previous_row + input_->width;
787  for (std::size_t ri = 1; ri < input_->height; ++ri)
788  {
789  for (std::size_t ci = 1; ci < input_->width; ++ci)
790  {
791  const float upLeft = previous_row [ci - 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci-1] + 1.4f;
792  const float up = previous_row [ci] + 1.0f; //distanceMap[(ri-1)*input_->width + ci] + 1.0f;
793  const float upRight = previous_row [ci + 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci+1] + 1.4f;
794  const float left = current_row [ci - 1] + 1.0f; //distanceMap[ri*input_->width + ci-1] + 1.0f;
795  const float center = current_row [ci]; //distanceMap[ri*input_->width + ci];
796 
797  const float minValue = std::min (std::min (upLeft, up), std::min (left, upRight));
798 
799  if (minValue < center)
800  current_row [ci] = minValue; //distanceMap[ri * input_->width + ci] = minValue;
801  }
802  previous_row = current_row;
803  current_row += input_->width;
804  }
805 
806  float* next_row = distanceMap + input_->width * (input_->height - 1);
807  current_row = next_row - input_->width;
808  // second pass
809  for (int ri = input_->height-2; ri >= 0; --ri)
810  {
811  for (int ci = input_->width-2; ci >= 0; --ci)
812  {
813  const float lowerLeft = next_row [ci - 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci-1] + 1.4f;
814  const float lower = next_row [ci] + 1.0f; //distanceMap[(ri+1)*input_->width + ci] + 1.0f;
815  const float lowerRight = next_row [ci + 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci+1] + 1.4f;
816  const float right = current_row [ci + 1] + 1.0f; //distanceMap[ri*input_->width + ci+1] + 1.0f;
817  const float center = current_row [ci]; //distanceMap[ri*input_->width + ci];
818 
819  const float minValue = std::min (std::min (lowerLeft, lower), std::min (right, lowerRight));
820 
821  if (minValue < center)
822  current_row [ci] = minValue; //distanceMap[ri*input_->width + ci] = minValue;
823  }
824  next_row = current_row;
825  current_row -= input_->width;
826  }
827 
828  if (indices_->size () < input_->size ())
829  computeFeaturePart (distanceMap, bad_point, output);
830  else
831  computeFeatureFull (distanceMap, bad_point, output);
832 
833  delete[] depthChangeMap;
834 }
835 
836 //////////////////////////////////////////////////////////////////////////////////////////
837 template <typename PointInT, typename PointOutT> void
838 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computeFeatureFull (const float *distanceMap,
839  const float &bad_point,
840  PointCloudOut &output)
841 {
842  unsigned index = 0;
843 
844  if (border_policy_ == BORDER_POLICY_IGNORE)
845  {
846  // Set all normals that we do not touch to NaN
847  // top and bottom borders
848  // That sets the output density to false!
849  output.is_dense = false;
850  unsigned border = int(normal_smoothing_size_);
851  PointOutT* vec1 = &output [0];
852  PointOutT* vec2 = vec1 + input_->width * (input_->height - border);
853 
854  std::size_t count = border * input_->width;
855  for (std::size_t idx = 0; idx < count; ++idx)
856  {
857  vec1 [idx].getNormalVector3fMap ().setConstant (bad_point);
858  vec1 [idx].curvature = bad_point;
859  vec2 [idx].getNormalVector3fMap ().setConstant (bad_point);
860  vec2 [idx].curvature = bad_point;
861  }
862 
863  // left and right borders actually columns
864  vec1 = &output [border * input_->width];
865  vec2 = vec1 + input_->width - border;
866  for (std::size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width)
867  {
868  for (std::size_t ci = 0; ci < border; ++ci)
869  {
870  vec1 [ci].getNormalVector3fMap ().setConstant (bad_point);
871  vec1 [ci].curvature = bad_point;
872  vec2 [ci].getNormalVector3fMap ().setConstant (bad_point);
873  vec2 [ci].curvature = bad_point;
874  }
875  }
876 
877  if (use_depth_dependent_smoothing_)
878  {
879  index = border + input_->width * border;
880  unsigned skip = (border << 1);
881  for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
882  {
883  for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
884  {
885  index = ri * input_->width + ci;
886 
887  const float depth = input_->points[index].z;
888  if (!std::isfinite (depth))
889  {
890  output[index].getNormalVector3fMap ().setConstant (bad_point);
891  output[index].curvature = bad_point;
892  continue;
893  }
894 
895  float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
896 
897  if (smoothing > 2.0f)
898  {
899  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
900  computePointNormal (ci, ri, index, output [index]);
901  }
902  else
903  {
904  output[index].getNormalVector3fMap ().setConstant (bad_point);
905  output[index].curvature = bad_point;
906  }
907  }
908  }
909  }
910  else
911  {
912  float smoothing_constant = normal_smoothing_size_;
913 
914  index = border + input_->width * border;
915  unsigned skip = (border << 1);
916  for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
917  {
918  for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
919  {
920  index = ri * input_->width + ci;
921 
922  if (!std::isfinite (input_->points[index].z))
923  {
924  output [index].getNormalVector3fMap ().setConstant (bad_point);
925  output [index].curvature = bad_point;
926  continue;
927  }
928 
929  float smoothing = (std::min)(distanceMap[index], smoothing_constant);
930 
931  if (smoothing > 2.0f)
932  {
933  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
934  computePointNormal (ci, ri, index, output [index]);
935  }
936  else
937  {
938  output [index].getNormalVector3fMap ().setConstant (bad_point);
939  output [index].curvature = bad_point;
940  }
941  }
942  }
943  }
944  }
945  else if (border_policy_ == BORDER_POLICY_MIRROR)
946  {
947  output.is_dense = false;
948 
949  if (use_depth_dependent_smoothing_)
950  {
951  //index = 0;
952  //unsigned skip = 0;
953  //for (unsigned ri = 0; ri < input_->height; ++ri, index += skip)
954  for (unsigned ri = 0; ri < input_->height; ++ri)
955  {
956  //for (unsigned ci = 0; ci < input_->width; ++ci, ++index)
957  for (unsigned ci = 0; ci < input_->width; ++ci)
958  {
959  index = ri * input_->width + ci;
960 
961  const float depth = input_->points[index].z;
962  if (!std::isfinite (depth))
963  {
964  output[index].getNormalVector3fMap ().setConstant (bad_point);
965  output[index].curvature = bad_point;
966  continue;
967  }
968 
969  float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
970 
971  if (smoothing > 2.0f)
972  {
973  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
974  computePointNormalMirror (ci, ri, index, output [index]);
975  }
976  else
977  {
978  output[index].getNormalVector3fMap ().setConstant (bad_point);
979  output[index].curvature = bad_point;
980  }
981  }
982  }
983  }
984  else
985  {
986  float smoothing_constant = normal_smoothing_size_;
987 
988  //index = border + input_->width * border;
989  //unsigned skip = (border << 1);
990  //for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
991  for (unsigned ri = 0; ri < input_->height; ++ri)
992  {
993  //for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
994  for (unsigned ci = 0; ci < input_->width; ++ci)
995  {
996  index = ri * input_->width + ci;
997 
998  if (!std::isfinite (input_->points[index].z))
999  {
1000  output [index].getNormalVector3fMap ().setConstant (bad_point);
1001  output [index].curvature = bad_point;
1002  continue;
1003  }
1004 
1005  float smoothing = (std::min)(distanceMap[index], smoothing_constant);
1006 
1007  if (smoothing > 2.0f)
1008  {
1009  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1010  computePointNormalMirror (ci, ri, index, output [index]);
1011  }
1012  else
1013  {
1014  output [index].getNormalVector3fMap ().setConstant (bad_point);
1015  output [index].curvature = bad_point;
1016  }
1017  }
1018  }
1019  }
1020  }
1021 }
1022 
1023 ///////////////////////////////////////////////////////////////////////////////////////////
1024 template <typename PointInT, typename PointOutT> void
1025 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computeFeaturePart (const float *distanceMap,
1026  const float &bad_point,
1027  PointCloudOut &output)
1028 {
1029  if (border_policy_ == BORDER_POLICY_IGNORE)
1030  {
1031  output.is_dense = false;
1032  unsigned border = int(normal_smoothing_size_);
1033  unsigned bottom = input_->height > border ? input_->height - border : 0;
1034  unsigned right = input_->width > border ? input_->width - border : 0;
1035  if (use_depth_dependent_smoothing_)
1036  {
1037  // Iterating over the entire index vector
1038  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1039  {
1040  unsigned pt_index = (*indices_)[idx];
1041  unsigned u = pt_index % input_->width;
1042  unsigned v = pt_index / input_->width;
1043  if (v < border || v > bottom)
1044  {
1045  output.points[idx].getNormalVector3fMap ().setConstant (bad_point);
1046  output.points[idx].curvature = bad_point;
1047  continue;
1048  }
1049 
1050  if (u < border || u > right)
1051  {
1052  output.points[idx].getNormalVector3fMap ().setConstant (bad_point);
1053  output.points[idx].curvature = bad_point;
1054  continue;
1055  }
1056 
1057  const float depth = input_->points[pt_index].z;
1058  if (!std::isfinite (depth))
1059  {
1060  output.points[idx].getNormalVector3fMap ().setConstant (bad_point);
1061  output.points[idx].curvature = bad_point;
1062  continue;
1063  }
1064 
1065  float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
1066  if (smoothing > 2.0f)
1067  {
1068  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1069  computePointNormal (u, v, pt_index, output [idx]);
1070  }
1071  else
1072  {
1073  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1074  output[idx].curvature = bad_point;
1075  }
1076  }
1077  }
1078  else
1079  {
1080  float smoothing_constant = normal_smoothing_size_;
1081  // Iterating over the entire index vector
1082  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1083  {
1084  unsigned pt_index = (*indices_)[idx];
1085  unsigned u = pt_index % input_->width;
1086  unsigned v = pt_index / input_->width;
1087  if (v < border || v > bottom)
1088  {
1089  output.points[idx].getNormalVector3fMap ().setConstant (bad_point);
1090  output.points[idx].curvature = bad_point;
1091  continue;
1092  }
1093 
1094  if (u < border || u > right)
1095  {
1096  output.points[idx].getNormalVector3fMap ().setConstant (bad_point);
1097  output.points[idx].curvature = bad_point;
1098  continue;
1099  }
1100 
1101  if (!std::isfinite (input_->points[pt_index].z))
1102  {
1103  output [idx].getNormalVector3fMap ().setConstant (bad_point);
1104  output [idx].curvature = bad_point;
1105  continue;
1106  }
1107 
1108  float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1109 
1110  if (smoothing > 2.0f)
1111  {
1112  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1113  computePointNormal (u, v, pt_index, output [idx]);
1114  }
1115  else
1116  {
1117  output [idx].getNormalVector3fMap ().setConstant (bad_point);
1118  output [idx].curvature = bad_point;
1119  }
1120  }
1121  }
1122  }// border_policy_ == BORDER_POLICY_IGNORE
1123  else if (border_policy_ == BORDER_POLICY_MIRROR)
1124  {
1125  output.is_dense = false;
1126 
1127  if (use_depth_dependent_smoothing_)
1128  {
1129  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1130  {
1131  unsigned pt_index = (*indices_)[idx];
1132  unsigned u = pt_index % input_->width;
1133  unsigned v = pt_index / input_->width;
1134 
1135  const float depth = input_->points[pt_index].z;
1136  if (!std::isfinite (depth))
1137  {
1138  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1139  output[idx].curvature = bad_point;
1140  continue;
1141  }
1142 
1143  float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
1144 
1145  if (smoothing > 2.0f)
1146  {
1147  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1148  computePointNormalMirror (u, v, pt_index, output [idx]);
1149  }
1150  else
1151  {
1152  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1153  output[idx].curvature = bad_point;
1154  }
1155  }
1156  }
1157  else
1158  {
1159  float smoothing_constant = normal_smoothing_size_;
1160  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1161  {
1162  unsigned pt_index = (*indices_)[idx];
1163  unsigned u = pt_index % input_->width;
1164  unsigned v = pt_index / input_->width;
1165 
1166  if (!std::isfinite (input_->points[pt_index].z))
1167  {
1168  output [idx].getNormalVector3fMap ().setConstant (bad_point);
1169  output [idx].curvature = bad_point;
1170  continue;
1171  }
1172 
1173  float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1174 
1175  if (smoothing > 2.0f)
1176  {
1177  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1178  computePointNormalMirror (u, v, pt_index, output [idx]);
1179  }
1180  else
1181  {
1182  output [idx].getNormalVector3fMap ().setConstant (bad_point);
1183  output [idx].curvature = bad_point;
1184  }
1185  }
1186  }
1187  } // border_policy_ == BORDER_POLICY_MIRROR
1188 }
1189 
1190 //////////////////////////////////////////////////////////////////////////////////////////
1191 template <typename PointInT, typename PointOutT> bool
1192 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initCompute ()
1193 {
1194  if (!input_->isOrganized ())
1195  {
1196  PCL_ERROR ("[pcl::IntegralImageNormalEstimation::initCompute] Input dataset is not organized (height = 1).\n");
1197  return (false);
1198  }
1199  return (Feature<PointInT, PointOutT>::initCompute ());
1200 }
1201 
1202 #define PCL_INSTANTIATE_IntegralImageNormalEstimation(T,NT) template class PCL_EXPORTS pcl::IntegralImageNormalEstimation<T,NT>;
1203 
1204 #endif
1205 
pcl::IntegralImage2D
Determines an integral image representation for a given organized data array.
Definition: integral_image2D.h:109
pcl::IntegralImageNormalEstimation::initData
void initData()
Initialize the data structures, based on the normal estimation method chosen.
Definition: integral_image_normal.hpp:56
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
pcl::IntegralImageNormalEstimation::computeFeatureFull
void computeFeatureFull(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for the complete cloud.
Definition: integral_image_normal.hpp:838
pcl::IntegralImageNormalEstimation
Surface normal estimation on organized data using integral images.
Definition: integral_image_normal.h:66
pcl::IntegralImage2D::getFirstOrderSumSE
ElementType getFirstOrderSumSE(unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
Compute the first order sum within a given rectangle.
Definition: integral_image2D.hpp:116
pcl::IntegralImageNormalEstimation::computeFeature
void computeFeature(PointCloudOut &output) override
Computes the normal for the complete cloud or only indices_ if provided.
Definition: integral_image_normal.hpp:731
pcl::IntegralImageNormalEstimation::computeFeaturePart
void computeFeaturePart(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for part of the cloud specified by indices_.
Definition: integral_image_normal.hpp:1025
pcl::PointCloud< pcl::Normal >
pcl::eigen33
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:296
pcl::computePointNormal
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:61
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
pcl::flipNormalTowardsViewpoint
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:122
pcl::IntegralImage2D::getFirstOrderSum
ElementType getFirstOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the first order sum within a given rectangle.
Definition: integral_image2D.hpp:74
pcl::IntegralImageNormalEstimation::~IntegralImageNormalEstimation
~IntegralImageNormalEstimation()
Destructor.
Definition: integral_image_normal.hpp:46
pcl::PointCloud::sensor_origin_
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:422
pcl::PointCloud::sensor_orientation_
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:424
pcl::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:419
pcl::IntegralImageNormalEstimation::computePointNormalMirror
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.
Definition: integral_image_normal.hpp:462
pcl::PCLException
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:64
pcl::IntegralImageNormalEstimation::computePointNormal
void computePointNormal(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position.
Definition: integral_image_normal.hpp:207
pcl::InitFailedException
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition: exceptions.h:194
pcl::IntegralImage2D::getSecondOrderSum
SecondOrderType getSecondOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the second order sum within a given rectangle.
Definition: integral_image2D.hpp:88
pcl::IntegralImageNormalEstimation::setRectSize
void setRectSize(const int width, const int height)
Set the regions size which is considered for normal estimation.
Definition: integral_image_normal.hpp:93