Point Cloud Library (PCL)  1.12.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>
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_)[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_)[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  delete[] diff_x_;
144  delete[] diff_y_;
145  std::size_t data_size = (input_->size () << 2);
146  diff_x_ = new float[data_size];
147  diff_y_ = new float[data_size];
148 
149  memset (diff_x_, 0, sizeof(float) * data_size);
150  memset (diff_y_, 0, sizeof(float) * data_size);
151 
152  // x u x
153  // l x r
154  // x d x
155  const PointInT* point_up = &(input_->points [1]);
156  const PointInT* point_dn = point_up + (input_->width << 1);//&(input_->points [1 + (input_->width << 1)]);
157  const PointInT* point_lf = &(input_->points [input_->width]);
158  const PointInT* point_rg = point_lf + 2; //&(input_->points [input_->width + 2]);
159  float* diff_x_ptr = diff_x_ + ((input_->width + 1) << 2);
160  float* diff_y_ptr = diff_y_ + ((input_->width + 1) << 2);
161  unsigned diff_skip = 8; // skip last element in row and the first in the next row
162 
163  for (std::size_t ri = 1; ri < input_->height - 1; ++ri
164  , point_up += input_->width
165  , point_dn += input_->width
166  , point_lf += input_->width
167  , point_rg += input_->width
168  , diff_x_ptr += diff_skip
169  , diff_y_ptr += diff_skip)
170  {
171  for (std::size_t ci = 0; ci < input_->width - 2; ++ci, diff_x_ptr += 4, diff_y_ptr += 4)
172  {
173  diff_x_ptr[0] = point_rg[ci].x - point_lf[ci].x;
174  diff_x_ptr[1] = point_rg[ci].y - point_lf[ci].y;
175  diff_x_ptr[2] = point_rg[ci].z - point_lf[ci].z;
176 
177  diff_y_ptr[0] = point_dn[ci].x - point_up[ci].x;
178  diff_y_ptr[1] = point_dn[ci].y - point_up[ci].y;
179  diff_y_ptr[2] = point_dn[ci].z - point_up[ci].z;
180  }
181  }
182 
183  // Compute integral images
184  integral_image_DX_.setInput (diff_x_, input_->width, input_->height, 4, input_->width << 2);
185  integral_image_DY_.setInput (diff_y_, input_->width, input_->height, 4, input_->width << 2);
186  init_covariance_matrix_ = init_depth_change_ = init_simple_3d_gradient_ = false;
187  init_average_3d_gradient_ = true;
188 }
189 
190 //////////////////////////////////////////////////////////////////////////////////////////
191 template <typename PointInT, typename PointOutT> void
192 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initAverageDepthChangeMethod ()
193 {
194  // number of DataType entries per element (equal or bigger than dimensions)
195  int element_stride = sizeof (PointInT) / sizeof (float);
196  // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
197  int row_stride = element_stride * input_->width;
198 
199  const float *data_ = reinterpret_cast<const float*> (&(*input_)[0]);
200 
201  // integral image over the z - value
202  integral_image_depth_.setInput (&(data_[2]), input_->width, input_->height, element_stride, row_stride);
203  init_depth_change_ = true;
204  init_covariance_matrix_ = init_average_3d_gradient_ = init_simple_3d_gradient_ = false;
205 }
206 
207 //////////////////////////////////////////////////////////////////////////////////////////
208 template <typename PointInT, typename PointOutT> void
209 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computePointNormal (
210  const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
211 {
212  float bad_point = std::numeric_limits<float>::quiet_NaN ();
213 
214  if (normal_estimation_method_ == COVARIANCE_MATRIX)
215  {
216  if (!init_covariance_matrix_)
217  initCovarianceMatrixMethod ();
218 
219  unsigned count = integral_image_XYZ_.getFiniteElementsCount (pos_x - (rect_width_2_), pos_y - (rect_height_2_), rect_width_, rect_height_);
220 
221  // no valid points within the rectangular region?
222  if (count == 0)
223  {
224  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
225  return;
226  }
227 
228  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
229  Eigen::Vector3f center;
230  typename IntegralImage2D<float, 3>::SecondOrderType so_elements;
231  center = integral_image_XYZ_.getFirstOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_).template cast<float> ();
232  so_elements = integral_image_XYZ_.getSecondOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
233 
234  covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
235  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
236  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
237  covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
238  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
239  covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
240  covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
241  float eigen_value;
242  Eigen::Vector3f eigen_vector;
243  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
244  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
245  normal.getNormalVector3fMap () = eigen_vector;
246 
247  // Compute the curvature surface change
248  if (eigen_value > 0.0)
249  normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
250  else
251  normal.curvature = 0;
252 
253  return;
254  }
255  if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
256  {
257  if (!init_average_3d_gradient_)
258  initAverage3DGradientMethod ();
259 
260  unsigned count_x = integral_image_DX_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
261  unsigned count_y = integral_image_DY_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
262  if (count_x == 0 || count_y == 0)
263  {
264  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
265  return;
266  }
267  Eigen::Vector3d gradient_x = integral_image_DX_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
268  Eigen::Vector3d gradient_y = integral_image_DY_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
269 
270  Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
271  double normal_length = normal_vector.squaredNorm ();
272  if (normal_length == 0.0f)
273  {
274  normal.getNormalVector3fMap ().setConstant (bad_point);
275  normal.curvature = bad_point;
276  return;
277  }
278 
279  normal_vector /= sqrt (normal_length);
280 
281  float nx = static_cast<float> (normal_vector [0]);
282  float ny = static_cast<float> (normal_vector [1]);
283  float nz = static_cast<float> (normal_vector [2]);
284 
285  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
286 
287  normal.normal_x = nx;
288  normal.normal_y = ny;
289  normal.normal_z = nz;
290  normal.curvature = bad_point;
291  return;
292  }
293  if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
294  {
295  if (!init_depth_change_)
296  initAverageDepthChangeMethod ();
297 
298  // width and height are at least 3 x 3
299  unsigned count_L_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
300  unsigned count_R_z = integral_image_depth_.getFiniteElementsCount (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
301  unsigned count_U_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_);
302  unsigned count_D_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_);
303 
304  if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
305  {
306  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
307  return;
308  }
309 
310  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);
311  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);
312  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);
313  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);
314 
315  PointInT pointL = (*input_)[point_index - rect_width_4_ - 1];
316  PointInT pointR = (*input_)[point_index + rect_width_4_ + 1];
317  PointInT pointU = (*input_)[point_index - rect_height_4_ * input_->width - 1];
318  PointInT pointD = (*input_)[point_index + rect_height_4_ * input_->width + 1];
319 
320  const float mean_x_z = mean_R_z - mean_L_z;
321  const float mean_y_z = mean_D_z - mean_U_z;
322 
323  const float mean_x_x = pointR.x - pointL.x;
324  const float mean_x_y = pointR.y - pointL.y;
325  const float mean_y_x = pointD.x - pointU.x;
326  const float mean_y_y = pointD.y - pointU.y;
327 
328  float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
329  float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
330  float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
331 
332  const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
333 
334  if (normal_length == 0.0f)
335  {
336  normal.getNormalVector3fMap ().setConstant (bad_point);
337  normal.curvature = bad_point;
338  return;
339  }
340 
341  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
342 
343  const float scale = 1.0f / std::sqrt (normal_length);
344 
345  normal.normal_x = normal_x * scale;
346  normal.normal_y = normal_y * scale;
347  normal.normal_z = normal_z * scale;
348  normal.curvature = bad_point;
349 
350  return;
351  }
352  if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
353  {
354  if (!init_simple_3d_gradient_)
355  initSimple3DGradientMethod ();
356 
357  // this method does not work if lots of NaNs are in the neighborhood of the point
358  Eigen::Vector3d gradient_x = integral_image_XYZ_.getFirstOrderSum (pos_x + rect_width_2_, pos_y - rect_height_2_, 1, rect_height_) -
359  integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, 1, rect_height_);
360 
361  Eigen::Vector3d gradient_y = integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y + rect_height_2_, rect_width_, 1) -
362  integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, 1);
363  Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
364  double normal_length = normal_vector.squaredNorm ();
365  if (normal_length == 0.0f)
366  {
367  normal.getNormalVector3fMap ().setConstant (bad_point);
368  normal.curvature = bad_point;
369  return;
370  }
371 
372  normal_vector /= sqrt (normal_length);
373 
374  float nx = static_cast<float> (normal_vector [0]);
375  float ny = static_cast<float> (normal_vector [1]);
376  float nz = static_cast<float> (normal_vector [2]);
377 
378  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
379 
380  normal.normal_x = nx;
381  normal.normal_y = ny;
382  normal.normal_z = nz;
383  normal.curvature = bad_point;
384  return;
385  }
386 
387  normal.getNormalVector3fMap ().setConstant (bad_point);
388  normal.curvature = bad_point;
389  return;
390 }
391 
392 //////////////////////////////////////////////////////////////////////////////////////////
393 template <typename T>
394 void
395 sumArea (int start_x, int start_y, int end_x, int end_y, const int width, const int height,
396  const std::function<T(unsigned, unsigned, unsigned, unsigned)> &f,
397  T & result)
398 {
399  if (start_x < 0)
400  {
401  if (start_y < 0)
402  {
403  result += f (0, 0, end_x, end_y);
404  result += f (0, 0, -start_x, -start_y);
405  result += f (0, 0, -start_x, end_y);
406  result += f (0, 0, end_x, -start_y);
407  }
408  else if (end_y >= height)
409  {
410  result += f (0, start_y, end_x, height-1);
411  result += f (0, start_y, -start_x, height-1);
412  result += f (0, height-(end_y-(height-1)), end_x, height-1);
413  result += f (0, height-(end_y-(height-1)), -start_x, height-1);
414  }
415  else
416  {
417  result += f (0, start_y, end_x, end_y);
418  result += f (0, start_y, -start_x, end_y);
419  }
420  }
421  else if (start_y < 0)
422  {
423  if (end_x >= width)
424  {
425  result += f (start_x, 0, width-1, end_y);
426  result += f (start_x, 0, width-1, -start_y);
427  result += f (width-(end_x-(width-1)), 0, width-1, end_y);
428  result += f (width-(end_x-(width-1)), 0, width-1, -start_y);
429  }
430  else
431  {
432  result += f (start_x, 0, end_x, end_y);
433  result += f (start_x, 0, end_x, -start_y);
434  }
435  }
436  else if (end_x >= width)
437  {
438  if (end_y >= height)
439  {
440  result += f (start_x, start_y, width-1, height-1);
441  result += f (start_x, height-(end_y-(height-1)), width-1, height-1);
442  result += f (width-(end_x-(width-1)), start_y, width-1, height-1);
443  result += f (width-(end_x-(width-1)), height-(end_y-(height-1)), width-1, height-1);
444  }
445  else
446  {
447  result += f (start_x, start_y, width-1, end_y);
448  result += f (width-(end_x-(width-1)), start_y, width-1, end_y);
449  }
450  }
451  else if (end_y >= height)
452  {
453  result += f (start_x, start_y, end_x, height-1);
454  result += f (start_x, height-(end_y-(height-1)), end_x, height-1);
455  }
456  else
457  {
458  result += f (start_x, start_y, end_x, end_y);
459  }
460 }
461 
462 //////////////////////////////////////////////////////////////////////////////////////////
463 template <typename PointInT, typename PointOutT> void
464 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computePointNormalMirror (
465  const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
466 {
467  float bad_point = std::numeric_limits<float>::quiet_NaN ();
468 
469  const int width = input_->width;
470  const int height = input_->height;
471 
472  // ==============================================================
473  if (normal_estimation_method_ == COVARIANCE_MATRIX)
474  {
475  if (!init_covariance_matrix_)
476  initCovarianceMatrixMethod ();
477 
478  const int start_x = pos_x - rect_width_2_;
479  const int start_y = pos_y - rect_height_2_;
480  const int end_x = start_x + rect_width_;
481  const int end_y = start_y + rect_height_;
482 
483  unsigned count = 0;
484  auto cb_xyz_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getFiniteElementsCountSE (p1, p2, p3, p4); };
485  sumArea<unsigned> (start_x, start_y, end_x, end_y, width, height, cb_xyz_fecse, count);
486 
487  // no valid points within the rectangular region?
488  if (count == 0)
489  {
490  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
491  return;
492  }
493 
494  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
495  Eigen::Vector3f center;
496  typename IntegralImage2D<float, 3>::SecondOrderType so_elements;
497  typename IntegralImage2D<float, 3>::ElementType tmp_center;
498  typename IntegralImage2D<float, 3>::SecondOrderType tmp_so_elements;
499 
500  center[0] = 0;
501  center[1] = 0;
502  center[2] = 0;
503  tmp_center[0] = 0;
504  tmp_center[1] = 0;
505  tmp_center[2] = 0;
506  so_elements[0] = 0;
507  so_elements[1] = 0;
508  so_elements[2] = 0;
509  so_elements[3] = 0;
510  so_elements[4] = 0;
511  so_elements[5] = 0;
512 
513  auto cb_xyz_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getFirstOrderSumSE (p1, p2, p3, p4); };
514  sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_fosse, tmp_center);
515  auto cb_xyz_sosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getSecondOrderSumSE (p1, p2, p3, p4); };
516  sumArea<typename IntegralImage2D<float, 3>::SecondOrderType>(start_x, start_y, end_x, end_y, width, height, cb_xyz_sosse, so_elements);
517 
518  center[0] = float (tmp_center[0]);
519  center[1] = float (tmp_center[1]);
520  center[2] = float (tmp_center[2]);
521 
522  covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
523  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
524  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
525  covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
526  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
527  covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
528  covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
529  float eigen_value;
530  Eigen::Vector3f eigen_vector;
531  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
532  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
533  normal.getNormalVector3fMap () = eigen_vector;
534 
535  // Compute the curvature surface change
536  if (eigen_value > 0.0)
537  normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
538  else
539  normal.curvature = 0;
540 
541  return;
542  }
543  // =======================================================
544  if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
545  {
546  if (!init_average_3d_gradient_)
547  initAverage3DGradientMethod ();
548 
549  const int start_x = pos_x - rect_width_2_;
550  const int start_y = pos_y - rect_height_2_;
551  const int end_x = start_x + rect_width_;
552  const int end_y = start_y + rect_height_;
553 
554  unsigned count_x = 0;
555  unsigned count_y = 0;
556 
557  auto cb_dx_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DX_.getFiniteElementsCountSE (p1, p2, p3, p4); };
558  sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, cb_dx_fecse, count_x);
559  auto cb_dy_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DY_.getFiniteElementsCountSE (p1, p2, p3, p4); };
560  sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, cb_dy_fecse, count_y);
561 
562 
563  if (count_x == 0 || count_y == 0)
564  {
565  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
566  return;
567  }
568  Eigen::Vector3d gradient_x (0, 0, 0);
569  Eigen::Vector3d gradient_y (0, 0, 0);
570 
571  auto cb_dx_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DX_.getFirstOrderSumSE (p1, p2, p3, p4); };
572  sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dx_fosse, gradient_x);
573  auto cb_dy_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_DY_.getFirstOrderSumSE (p1, p2, p3, p4); };
574  sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, cb_dy_fosse, gradient_y);
575 
576 
577  Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
578  double normal_length = normal_vector.squaredNorm ();
579  if (normal_length == 0.0f)
580  {
581  normal.getNormalVector3fMap ().setConstant (bad_point);
582  normal.curvature = bad_point;
583  return;
584  }
585 
586  normal_vector /= sqrt (normal_length);
587 
588  float nx = static_cast<float> (normal_vector [0]);
589  float ny = static_cast<float> (normal_vector [1]);
590  float nz = static_cast<float> (normal_vector [2]);
591 
592  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
593 
594  normal.normal_x = nx;
595  normal.normal_y = ny;
596  normal.normal_z = nz;
597  normal.curvature = bad_point;
598  return;
599  }
600  // ======================================================
601  if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
602  {
603  if (!init_depth_change_)
604  initAverageDepthChangeMethod ();
605 
606  int point_index_L_x = pos_x - rect_width_4_ - 1;
607  int point_index_L_y = pos_y;
608  int point_index_R_x = pos_x + rect_width_4_ + 1;
609  int point_index_R_y = pos_y;
610  int point_index_U_x = pos_x - 1;
611  int point_index_U_y = pos_y - rect_height_4_;
612  int point_index_D_x = pos_x + 1;
613  int point_index_D_y = pos_y + rect_height_4_;
614 
615  if (point_index_L_x < 0)
616  point_index_L_x = -point_index_L_x;
617  if (point_index_U_x < 0)
618  point_index_U_x = -point_index_U_x;
619  if (point_index_U_y < 0)
620  point_index_U_y = -point_index_U_y;
621 
622  if (point_index_R_x >= width)
623  point_index_R_x = width-(point_index_R_x-(width-1));
624  if (point_index_D_x >= width)
625  point_index_D_x = width-(point_index_D_x-(width-1));
626  if (point_index_D_y >= height)
627  point_index_D_y = height-(point_index_D_y-(height-1));
628 
629  const int start_x_L = pos_x - rect_width_2_;
630  const int start_y_L = pos_y - rect_height_4_;
631  const int end_x_L = start_x_L + rect_width_2_;
632  const int end_y_L = start_y_L + rect_height_2_;
633 
634  const int start_x_R = pos_x + 1;
635  const int start_y_R = pos_y - rect_height_4_;
636  const int end_x_R = start_x_R + rect_width_2_;
637  const int end_y_R = start_y_R + rect_height_2_;
638 
639  const int start_x_U = pos_x - rect_width_4_;
640  const int start_y_U = pos_y - rect_height_2_;
641  const int end_x_U = start_x_U + rect_width_2_;
642  const int end_y_U = start_y_U + rect_height_2_;
643 
644  const int start_x_D = pos_x - rect_width_4_;
645  const int start_y_D = pos_y + 1;
646  const int end_x_D = start_x_D + rect_width_2_;
647  const int end_y_D = start_y_D + rect_height_2_;
648 
649  unsigned count_L_z = 0;
650  unsigned count_R_z = 0;
651  unsigned count_U_z = 0;
652  unsigned count_D_z = 0;
653 
654  auto cb_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_depth_.getFiniteElementsCountSE (p1, p2, p3, p4); };
655  sumArea<unsigned>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fecse, count_L_z);
656  sumArea<unsigned>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fecse, count_R_z);
657  sumArea<unsigned>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fecse, count_U_z);
658  sumArea<unsigned>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fecse, count_D_z);
659 
660  if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
661  {
662  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
663  return;
664  }
665 
666  float mean_L_z = 0;
667  float mean_R_z = 0;
668  float mean_U_z = 0;
669  float mean_D_z = 0;
670 
671  auto cb_fosse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_depth_.getFirstOrderSumSE (p1, p2, p3, p4); };
672  sumArea<float>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, cb_fosse, mean_L_z);
673  sumArea<float>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, cb_fosse, mean_R_z);
674  sumArea<float>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, cb_fosse, mean_U_z);
675  sumArea<float>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, cb_fosse, mean_D_z);
676 
677  mean_L_z /= float (count_L_z);
678  mean_R_z /= float (count_R_z);
679  mean_U_z /= float (count_U_z);
680  mean_D_z /= float (count_D_z);
681 
682 
683  PointInT pointL = (*input_)[point_index_L_y*width + point_index_L_x];
684  PointInT pointR = (*input_)[point_index_R_y*width + point_index_R_x];
685  PointInT pointU = (*input_)[point_index_U_y*width + point_index_U_x];
686  PointInT pointD = (*input_)[point_index_D_y*width + point_index_D_x];
687 
688  const float mean_x_z = mean_R_z - mean_L_z;
689  const float mean_y_z = mean_D_z - mean_U_z;
690 
691  const float mean_x_x = pointR.x - pointL.x;
692  const float mean_x_y = pointR.y - pointL.y;
693  const float mean_y_x = pointD.x - pointU.x;
694  const float mean_y_y = pointD.y - pointU.y;
695 
696  float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
697  float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
698  float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
699 
700  const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
701 
702  if (normal_length == 0.0f)
703  {
704  normal.getNormalVector3fMap ().setConstant (bad_point);
705  normal.curvature = bad_point;
706  return;
707  }
708 
709  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
710 
711  const float scale = 1.0f / std::sqrt (normal_length);
712 
713  normal.normal_x = normal_x * scale;
714  normal.normal_y = normal_y * scale;
715  normal.normal_z = normal_z * scale;
716  normal.curvature = bad_point;
717 
718  return;
719  }
720  // ========================================================
721  if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
722  {
723  PCL_THROW_EXCEPTION (PCLException, "BORDER_POLICY_MIRROR not supported for normal estimation method SIMPLE_3D_GRADIENT");
724  }
725 
726  normal.getNormalVector3fMap ().setConstant (bad_point);
727  normal.curvature = bad_point;
728  return;
729 }
730 
731 //////////////////////////////////////////////////////////////////////////////////////////
732 template <typename PointInT, typename PointOutT> void
733 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
734 {
735  output.sensor_origin_ = input_->sensor_origin_;
736  output.sensor_orientation_ = input_->sensor_orientation_;
737 
738  float bad_point = std::numeric_limits<float>::quiet_NaN ();
739 
740  // compute depth-change map
741  unsigned char * depthChangeMap = new unsigned char[input_->size ()];
742  memset (depthChangeMap, 255, input_->size ());
743 
744  unsigned index = 0;
745  for (unsigned int ri = 0; ri < input_->height-1; ++ri)
746  {
747  for (unsigned int ci = 0; ci < input_->width-1; ++ci, ++index)
748  {
749  index = ri * input_->width + ci;
750 
751  const float depth = input_->points [index].z;
752  const float depthR = input_->points [index + 1].z;
753  const float depthD = input_->points [index + input_->width].z;
754 
755  //const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs(depth)+1.0f))/(500.0f*0.001f);
756  const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs (depth) + 1.0f) * 2.0f);
757 
758  if (std::fabs (depth - depthR) > depthDependendDepthChange
759  || !std::isfinite (depth) || !std::isfinite (depthR))
760  {
761  depthChangeMap[index] = 0;
762  depthChangeMap[index+1] = 0;
763  }
764  if (std::fabs (depth - depthD) > depthDependendDepthChange
765  || !std::isfinite (depth) || !std::isfinite (depthD))
766  {
767  depthChangeMap[index] = 0;
768  depthChangeMap[index + input_->width] = 0;
769  }
770  }
771  }
772 
773  // compute distance map
774  //float *distanceMap = new float[input_->size ()];
775  delete[] distance_map_;
776  distance_map_ = new float[input_->size ()];
777  float *distanceMap = distance_map_;
778  for (std::size_t index = 0; index < input_->size (); ++index)
779  {
780  if (depthChangeMap[index] == 0)
781  distanceMap[index] = 0.0f;
782  else
783  distanceMap[index] = static_cast<float> (input_->width + input_->height);
784  }
785 
786  // first pass
787  float* previous_row = distanceMap;
788  float* current_row = previous_row + input_->width;
789  for (std::size_t ri = 1; ri < input_->height; ++ri)
790  {
791  for (std::size_t ci = 1; ci < input_->width; ++ci)
792  {
793  const float upLeft = previous_row [ci - 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci-1] + 1.4f;
794  const float up = previous_row [ci] + 1.0f; //distanceMap[(ri-1)*input_->width + ci] + 1.0f;
795  const float upRight = previous_row [ci + 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci+1] + 1.4f;
796  const float left = current_row [ci - 1] + 1.0f; //distanceMap[ri*input_->width + ci-1] + 1.0f;
797  const float center = current_row [ci]; //distanceMap[ri*input_->width + ci];
798 
799  const float minValue = std::min (std::min (upLeft, up), std::min (left, upRight));
800 
801  if (minValue < center)
802  current_row [ci] = minValue; //distanceMap[ri * input_->width + ci] = minValue;
803  }
804  previous_row = current_row;
805  current_row += input_->width;
806  }
807 
808  float* next_row = distanceMap + input_->width * (input_->height - 1);
809  current_row = next_row - input_->width;
810  // second pass
811  for (int ri = input_->height-2; ri >= 0; --ri)
812  {
813  for (int ci = input_->width-2; ci >= 0; --ci)
814  {
815  const float lowerLeft = next_row [ci - 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci-1] + 1.4f;
816  const float lower = next_row [ci] + 1.0f; //distanceMap[(ri+1)*input_->width + ci] + 1.0f;
817  const float lowerRight = next_row [ci + 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci+1] + 1.4f;
818  const float right = current_row [ci + 1] + 1.0f; //distanceMap[ri*input_->width + ci+1] + 1.0f;
819  const float center = current_row [ci]; //distanceMap[ri*input_->width + ci];
820 
821  const float minValue = std::min (std::min (lowerLeft, lower), std::min (right, lowerRight));
822 
823  if (minValue < center)
824  current_row [ci] = minValue; //distanceMap[ri*input_->width + ci] = minValue;
825  }
826  next_row = current_row;
827  current_row -= input_->width;
828  }
829 
830  if (indices_->size () < input_->size ())
831  computeFeaturePart (distanceMap, bad_point, output);
832  else
833  computeFeatureFull (distanceMap, bad_point, output);
834 
835  delete[] depthChangeMap;
836 }
837 
838 //////////////////////////////////////////////////////////////////////////////////////////
839 template <typename PointInT, typename PointOutT> void
840 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computeFeatureFull (const float *distanceMap,
841  const float &bad_point,
842  PointCloudOut &output)
843 {
844  unsigned index = 0;
845 
846  if (border_policy_ == BORDER_POLICY_IGNORE)
847  {
848  // Set all normals that we do not touch to NaN
849  // top and bottom borders
850  // That sets the output density to false!
851  output.is_dense = false;
852  unsigned border = int(normal_smoothing_size_);
853  PointOutT* vec1 = &output [0];
854  PointOutT* vec2 = vec1 + input_->width * (input_->height - border);
855 
856  std::size_t count = border * input_->width;
857  for (std::size_t idx = 0; idx < count; ++idx)
858  {
859  vec1 [idx].getNormalVector3fMap ().setConstant (bad_point);
860  vec1 [idx].curvature = bad_point;
861  vec2 [idx].getNormalVector3fMap ().setConstant (bad_point);
862  vec2 [idx].curvature = bad_point;
863  }
864 
865  // left and right borders actually columns
866  vec1 = &output [border * input_->width];
867  vec2 = vec1 + input_->width - border;
868  for (std::size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width)
869  {
870  for (std::size_t ci = 0; ci < border; ++ci)
871  {
872  vec1 [ci].getNormalVector3fMap ().setConstant (bad_point);
873  vec1 [ci].curvature = bad_point;
874  vec2 [ci].getNormalVector3fMap ().setConstant (bad_point);
875  vec2 [ci].curvature = bad_point;
876  }
877  }
878 
879  if (use_depth_dependent_smoothing_)
880  {
881  index = border + input_->width * border;
882  unsigned skip = (border << 1);
883  for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
884  {
885  for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
886  {
887  index = ri * input_->width + ci;
888 
889  const float depth = (*input_)[index].z;
890  if (!std::isfinite (depth))
891  {
892  output[index].getNormalVector3fMap ().setConstant (bad_point);
893  output[index].curvature = bad_point;
894  continue;
895  }
896 
897  float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
898 
899  if (smoothing > 2.0f)
900  {
901  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
902  computePointNormal (ci, ri, index, output [index]);
903  }
904  else
905  {
906  output[index].getNormalVector3fMap ().setConstant (bad_point);
907  output[index].curvature = bad_point;
908  }
909  }
910  }
911  }
912  else
913  {
914  float smoothing_constant = normal_smoothing_size_;
915 
916  index = border + input_->width * border;
917  unsigned skip = (border << 1);
918  for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
919  {
920  for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
921  {
922  index = ri * input_->width + ci;
923 
924  if (!std::isfinite ((*input_)[index].z))
925  {
926  output [index].getNormalVector3fMap ().setConstant (bad_point);
927  output [index].curvature = bad_point;
928  continue;
929  }
930 
931  float smoothing = (std::min)(distanceMap[index], smoothing_constant);
932 
933  if (smoothing > 2.0f)
934  {
935  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
936  computePointNormal (ci, ri, index, output [index]);
937  }
938  else
939  {
940  output [index].getNormalVector3fMap ().setConstant (bad_point);
941  output [index].curvature = bad_point;
942  }
943  }
944  }
945  }
946  }
947  else if (border_policy_ == BORDER_POLICY_MIRROR)
948  {
949  output.is_dense = false;
950 
951  if (use_depth_dependent_smoothing_)
952  {
953  //index = 0;
954  //unsigned skip = 0;
955  //for (unsigned ri = 0; ri < input_->height; ++ri, index += skip)
956  for (unsigned ri = 0; ri < input_->height; ++ri)
957  {
958  //for (unsigned ci = 0; ci < input_->width; ++ci, ++index)
959  for (unsigned ci = 0; ci < input_->width; ++ci)
960  {
961  index = ri * input_->width + ci;
962 
963  const float depth = (*input_)[index].z;
964  if (!std::isfinite (depth))
965  {
966  output[index].getNormalVector3fMap ().setConstant (bad_point);
967  output[index].curvature = bad_point;
968  continue;
969  }
970 
971  float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
972 
973  if (smoothing > 2.0f)
974  {
975  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
976  computePointNormalMirror (ci, ri, index, output [index]);
977  }
978  else
979  {
980  output[index].getNormalVector3fMap ().setConstant (bad_point);
981  output[index].curvature = bad_point;
982  }
983  }
984  }
985  }
986  else
987  {
988  float smoothing_constant = normal_smoothing_size_;
989 
990  //index = border + input_->width * border;
991  //unsigned skip = (border << 1);
992  //for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
993  for (unsigned ri = 0; ri < input_->height; ++ri)
994  {
995  //for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
996  for (unsigned ci = 0; ci < input_->width; ++ci)
997  {
998  index = ri * input_->width + ci;
999 
1000  if (!std::isfinite ((*input_)[index].z))
1001  {
1002  output [index].getNormalVector3fMap ().setConstant (bad_point);
1003  output [index].curvature = bad_point;
1004  continue;
1005  }
1006 
1007  float smoothing = (std::min)(distanceMap[index], smoothing_constant);
1008 
1009  if (smoothing > 2.0f)
1010  {
1011  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1012  computePointNormalMirror (ci, ri, index, output [index]);
1013  }
1014  else
1015  {
1016  output [index].getNormalVector3fMap ().setConstant (bad_point);
1017  output [index].curvature = bad_point;
1018  }
1019  }
1020  }
1021  }
1022  }
1023 }
1024 
1025 ///////////////////////////////////////////////////////////////////////////////////////////
1026 template <typename PointInT, typename PointOutT> void
1027 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computeFeaturePart (const float *distanceMap,
1028  const float &bad_point,
1029  PointCloudOut &output)
1030 {
1031  if (border_policy_ == BORDER_POLICY_IGNORE)
1032  {
1033  output.is_dense = false;
1034  unsigned border = int(normal_smoothing_size_);
1035  unsigned bottom = input_->height > border ? input_->height - border : 0;
1036  unsigned right = input_->width > border ? input_->width - border : 0;
1037  if (use_depth_dependent_smoothing_)
1038  {
1039  // Iterating over the entire index vector
1040  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1041  {
1042  unsigned pt_index = (*indices_)[idx];
1043  unsigned u = pt_index % input_->width;
1044  unsigned v = pt_index / input_->width;
1045  if (v < border || v > bottom)
1046  {
1047  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1048  output[idx].curvature = bad_point;
1049  continue;
1050  }
1051 
1052  if (u < border || u > right)
1053  {
1054  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1055  output[idx].curvature = bad_point;
1056  continue;
1057  }
1058 
1059  const float depth = (*input_)[pt_index].z;
1060  if (!std::isfinite (depth))
1061  {
1062  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1063  output[idx].curvature = bad_point;
1064  continue;
1065  }
1066 
1067  float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
1068  if (smoothing > 2.0f)
1069  {
1070  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1071  computePointNormal (u, v, pt_index, output [idx]);
1072  }
1073  else
1074  {
1075  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1076  output[idx].curvature = bad_point;
1077  }
1078  }
1079  }
1080  else
1081  {
1082  float smoothing_constant = normal_smoothing_size_;
1083  // Iterating over the entire index vector
1084  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1085  {
1086  unsigned pt_index = (*indices_)[idx];
1087  unsigned u = pt_index % input_->width;
1088  unsigned v = pt_index / input_->width;
1089  if (v < border || v > bottom)
1090  {
1091  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1092  output[idx].curvature = bad_point;
1093  continue;
1094  }
1095 
1096  if (u < border || u > right)
1097  {
1098  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1099  output[idx].curvature = bad_point;
1100  continue;
1101  }
1102 
1103  if (!std::isfinite ((*input_)[pt_index].z))
1104  {
1105  output [idx].getNormalVector3fMap ().setConstant (bad_point);
1106  output [idx].curvature = bad_point;
1107  continue;
1108  }
1109 
1110  float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1111 
1112  if (smoothing > 2.0f)
1113  {
1114  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1115  computePointNormal (u, v, pt_index, output [idx]);
1116  }
1117  else
1118  {
1119  output [idx].getNormalVector3fMap ().setConstant (bad_point);
1120  output [idx].curvature = bad_point;
1121  }
1122  }
1123  }
1124  }// border_policy_ == BORDER_POLICY_IGNORE
1125  else if (border_policy_ == BORDER_POLICY_MIRROR)
1126  {
1127  output.is_dense = false;
1128 
1129  if (use_depth_dependent_smoothing_)
1130  {
1131  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1132  {
1133  unsigned pt_index = (*indices_)[idx];
1134  unsigned u = pt_index % input_->width;
1135  unsigned v = pt_index / input_->width;
1136 
1137  const float depth = (*input_)[pt_index].z;
1138  if (!std::isfinite (depth))
1139  {
1140  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1141  output[idx].curvature = bad_point;
1142  continue;
1143  }
1144 
1145  float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
1146 
1147  if (smoothing > 2.0f)
1148  {
1149  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1150  computePointNormalMirror (u, v, pt_index, output [idx]);
1151  }
1152  else
1153  {
1154  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1155  output[idx].curvature = bad_point;
1156  }
1157  }
1158  }
1159  else
1160  {
1161  float smoothing_constant = normal_smoothing_size_;
1162  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1163  {
1164  unsigned pt_index = (*indices_)[idx];
1165  unsigned u = pt_index % input_->width;
1166  unsigned v = pt_index / input_->width;
1167 
1168  if (!std::isfinite ((*input_)[pt_index].z))
1169  {
1170  output [idx].getNormalVector3fMap ().setConstant (bad_point);
1171  output [idx].curvature = bad_point;
1172  continue;
1173  }
1174 
1175  float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1176 
1177  if (smoothing > 2.0f)
1178  {
1179  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1180  computePointNormalMirror (u, v, pt_index, output [idx]);
1181  }
1182  else
1183  {
1184  output [idx].getNormalVector3fMap ().setConstant (bad_point);
1185  output [idx].curvature = bad_point;
1186  }
1187  }
1188  }
1189  } // border_policy_ == BORDER_POLICY_MIRROR
1190 }
1191 
1192 //////////////////////////////////////////////////////////////////////////////////////////
1193 template <typename PointInT, typename PointOutT> bool
1194 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initCompute ()
1195 {
1196  if (!input_->isOrganized ())
1197  {
1198  PCL_ERROR ("[pcl::IntegralImageNormalEstimation::initCompute] Input dataset is not organized (height = 1).\n");
1199  return (false);
1200  }
1201  return (Feature<PointInT, PointOutT>::initCompute ());
1202 }
1203 
1204 #define PCL_INSTANTIATE_IntegralImageNormalEstimation(T,NT) template class PCL_EXPORTS pcl::IntegralImageNormalEstimation<T,NT>;
1205 
1206 #endif
1207 
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::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:840
pcl::IntegralImageNormalEstimation
Surface normal estimation on organized data using integral images.
Definition: integral_image_normal.h:65
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:114
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:733
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:1027
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:398
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:72
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:406
pcl::PointCloud::sensor_orientation_
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:408
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:403
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:464
pcl::PCLException
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:63
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:209
pcl::InitFailedException
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition: exceptions.h:193
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:86
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