Point Cloud Library (PCL)  1.12.0
integral_image_normal.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
39 #ifndef PCL_FEATURES_INTEGRALIMAGE_BASED_IMPL_NORMAL_ESTIMATOR_H_
40 #define PCL_FEATURES_INTEGRALIMAGE_BASED_IMPL_NORMAL_ESTIMATOR_H_
41 
42 #include <pcl/features/integral_image_normal.h>
43 
44 //////////////////////////////////////////////////////////////////////////////////////////
45 template <typename PointInT, typename PointOutT>
47 {
48  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
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
94 {
95  rect_width_ = width;
96  rect_width_2_ = width/2;
97  rect_width_4_ = width/4;
98  rect_height_ = height;
99  rect_height_2_ = height/2;
100  rect_height_4_ = height/4;
101 }
102 
103 //////////////////////////////////////////////////////////////////////////////////////////
104 template <typename PointInT, typename PointOutT> void
106 {
107  // number of DataType entries per element (equal or bigger than dimensions)
108  int element_stride = sizeof (PointInT) / sizeof (float);
109  // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
110  int row_stride = element_stride * input_->width;
111 
112  const float *data_ = reinterpret_cast<const float*> (&(*input_)[0]);
113 
114  integral_image_XYZ_.setSecondOrderComputation (false);
115  integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride);
116 
117  init_simple_3d_gradient_ = true;
118  init_covariance_matrix_ = init_average_3d_gradient_ = init_depth_change_ = false;
119 }
120 
121 //////////////////////////////////////////////////////////////////////////////////////////
122 template <typename PointInT, typename PointOutT> void
124 {
125  // number of DataType entries per element (equal or bigger than dimensions)
126  int element_stride = sizeof (PointInT) / sizeof (float);
127  // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
128  int row_stride = element_stride * input_->width;
129 
130  const float *data_ = reinterpret_cast<const float*> (&(*input_)[0]);
131 
132  integral_image_XYZ_.setSecondOrderComputation (true);
133  integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride);
134 
135  init_covariance_matrix_ = true;
136  init_average_3d_gradient_ = init_depth_change_ = init_simple_3d_gradient_ = false;
137 }
138 
139 //////////////////////////////////////////////////////////////////////////////////////////
140 template <typename PointInT, typename PointOutT> void
142 {
143  std::size_t data_size = (input_->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
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_)[0]);
198 
199  // integral image over the z - value
200  integral_image_depth_.setInput (&(data_[2]), input_->width, input_->height, element_stride, row_stride);
201  init_depth_change_ = true;
202  init_covariance_matrix_ = init_average_3d_gradient_ = init_simple_3d_gradient_ = false;
203 }
204 
205 //////////////////////////////////////////////////////////////////////////////////////////
206 template <typename PointInT, typename PointOutT> void
208  const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
209 {
210  float bad_point = std::numeric_limits<float>::quiet_NaN ();
211 
212  if (normal_estimation_method_ == COVARIANCE_MATRIX)
213  {
214  if (!init_covariance_matrix_)
215  initCovarianceMatrixMethod ();
216 
217  unsigned count = integral_image_XYZ_.getFiniteElementsCount (pos_x - (rect_width_2_), pos_y - (rect_height_2_), rect_width_, rect_height_);
218 
219  // no valid points within the rectangular region?
220  if (count == 0)
221  {
222  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
223  return;
224  }
225 
226  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
227  Eigen::Vector3f center;
228  typename IntegralImage2D<float, 3>::SecondOrderType so_elements;
229  center = integral_image_XYZ_.getFirstOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_).template cast<float> ();
230  so_elements = integral_image_XYZ_.getSecondOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
231 
232  covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
233  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
234  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
235  covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
236  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
237  covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
238  covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
239  float eigen_value;
240  Eigen::Vector3f eigen_vector;
241  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
242  flipNormalTowardsViewpoint ((*input_)[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_)[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_)[point_index - rect_width_4_ - 1];
314  PointInT pointR = (*input_)[point_index + rect_width_4_ + 1];
315  PointInT pointU = (*input_)[point_index - rect_height_4_ * input_->width - 1];
316  PointInT pointD = (*input_)[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_)[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_)[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
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_)[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_)[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_)[point_index_L_y*width + point_index_L_x];
682  PointInT pointR = (*input_)[point_index_R_y*width + point_index_R_x];
683  PointInT pointU = (*input_)[point_index_U_y*width + point_index_U_x];
684  PointInT pointD = (*input_)[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_)[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
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_->size ()];
740  memset (depthChangeMap, 255, input_->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_->size ()];
773  delete[] distance_map_;
774  distance_map_ = new float[input_->size ()];
775  float *distanceMap = distance_map_;
776  for (std::size_t index = 0; index < input_->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
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_)[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_)[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_)[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_)[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
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[idx].getNormalVector3fMap ().setConstant (bad_point);
1046  output[idx].curvature = bad_point;
1047  continue;
1048  }
1049 
1050  if (u < border || u > right)
1051  {
1052  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1053  output[idx].curvature = bad_point;
1054  continue;
1055  }
1056 
1057  const float depth = (*input_)[pt_index].z;
1058  if (!std::isfinite (depth))
1059  {
1060  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1061  output[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[idx].getNormalVector3fMap ().setConstant (bad_point);
1090  output[idx].curvature = bad_point;
1091  continue;
1092  }
1093 
1094  if (u < border || u > right)
1095  {
1096  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1097  output[idx].curvature = bad_point;
1098  continue;
1099  }
1100 
1101  if (!std::isfinite ((*input_)[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_)[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_)[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
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 
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition: exceptions.h:194
Determines an integral image representation for a given organized data array.
ElementType getFirstOrderSumSE(unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
Compute the first order sum within a given rectangle.
ElementType getFirstOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the first order sum within a given rectangle.
SecondOrderType getSecondOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the second order sum within a given rectangle.
Surface normal estimation on organized data using integral images.
void initData()
Initialize the data structures, based on the normal estimation method chosen.
void setRectSize(const int width, const int height)
Set the regions size which is considered for normal estimation.
void computeFeatureFull(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for the complete cloud.
void computePointNormal(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position.
void computeFeature(PointCloudOut &output) override
Computes the normal for the complete cloud or only indices_ if provided.
void computeFeaturePart(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for part of the cloud specified by indices_.
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.
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:64
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
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:408
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:406
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
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
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