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