Point Cloud Library (PCL)  1.12.1-dev
integral_image_normal.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 #pragma once
39 #include <pcl/features/integral_image_normal.h>
40 
41 #include <algorithm>
42 
43 //////////////////////////////////////////////////////////////////////////////////////////
44 template <typename PointInT, typename PointOutT>
45 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::~IntegralImageNormalEstimation ()
46 {
47  delete[] diff_x_;
48  delete[] diff_y_;
49  delete[] depth_data_;
50  delete[] distance_map_;
51 }
52 
53 //////////////////////////////////////////////////////////////////////////////////////////
54 template <typename PointInT, typename PointOutT> void
55 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initData ()
56 {
57  if (border_policy_ != BORDER_POLICY_IGNORE &&
58  border_policy_ != BORDER_POLICY_MIRROR)
59  PCL_THROW_EXCEPTION (InitFailedException,
60  "[pcl::IntegralImageNormalEstimation::initData] unknown border policy.");
61 
62  if (normal_estimation_method_ != COVARIANCE_MATRIX &&
63  normal_estimation_method_ != AVERAGE_3D_GRADIENT &&
64  normal_estimation_method_ != AVERAGE_DEPTH_CHANGE &&
65  normal_estimation_method_ != SIMPLE_3D_GRADIENT)
66  PCL_THROW_EXCEPTION (InitFailedException,
67  "[pcl::IntegralImageNormalEstimation::initData] unknown normal estimation method.");
68 
69  // compute derivatives
70  delete[] diff_x_;
71  delete[] diff_y_;
72  delete[] depth_data_;
73  delete[] distance_map_;
74  diff_x_ = nullptr;
75  diff_y_ = nullptr;
76  depth_data_ = nullptr;
77  distance_map_ = nullptr;
78 
79  if (normal_estimation_method_ == COVARIANCE_MATRIX)
80  initCovarianceMatrixMethod ();
81  else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
82  initAverage3DGradientMethod ();
83  else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
84  initAverageDepthChangeMethod ();
85  else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
86  initSimple3DGradientMethod ();
87 }
88 
89 
90 //////////////////////////////////////////////////////////////////////////////////////////
91 template <typename PointInT, typename PointOutT> void
92 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::setRectSize (const int width, const int height)
93 {
94  rect_width_ = width;
95  rect_width_2_ = width/2;
96  rect_width_4_ = width/4;
97  rect_height_ = height;
98  rect_height_2_ = height/2;
99  rect_height_4_ = height/4;
100 }
101 
102 //////////////////////////////////////////////////////////////////////////////////////////
103 template <typename PointInT, typename PointOutT> void
104 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initSimple3DGradientMethod ()
105 {
106  // number of DataType entries per element (equal or bigger than dimensions)
107  int element_stride = sizeof (PointInT) / sizeof (float);
108  // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
109  int row_stride = element_stride * input_->width;
110 
111  const float *data_ = reinterpret_cast<const float*> (&(*input_)[0]);
112 
113  integral_image_XYZ_.setSecondOrderComputation (false);
114  integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride);
115 
116  init_simple_3d_gradient_ = true;
117  init_covariance_matrix_ = init_average_3d_gradient_ = init_depth_change_ = false;
118 }
119 
120 //////////////////////////////////////////////////////////////////////////////////////////
121 template <typename PointInT, typename PointOutT> void
122 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initCovarianceMatrixMethod ()
123 {
124  // number of DataType entries per element (equal or bigger than dimensions)
125  int element_stride = sizeof (PointInT) / sizeof (float);
126  // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
127  int row_stride = element_stride * input_->width;
128 
129  const float *data_ = reinterpret_cast<const float*> (&(*input_)[0]);
130 
131  integral_image_XYZ_.setSecondOrderComputation (true);
132  integral_image_XYZ_.setInput (data_, input_->width, input_->height, element_stride, row_stride);
133 
134  init_covariance_matrix_ = true;
135  init_average_3d_gradient_ = init_depth_change_ = init_simple_3d_gradient_ = false;
136 }
137 
138 //////////////////////////////////////////////////////////////////////////////////////////
139 template <typename PointInT, typename PointOutT> void
140 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initAverage3DGradientMethod ()
141 {
142  delete[] diff_x_;
143  delete[] diff_y_;
144  std::size_t data_size = (input_->size () << 2);
145  diff_x_ = new float[data_size]{};
146  diff_y_ = new float[data_size]{};
147 
148  // x u x
149  // l x r
150  // x d x
151  const PointInT* point_up = &(input_->points [1]);
152  const PointInT* point_dn = point_up + (input_->width << 1);//&(input_->points [1 + (input_->width << 1)]);
153  const PointInT* point_lf = &(input_->points [input_->width]);
154  const PointInT* point_rg = point_lf + 2; //&(input_->points [input_->width + 2]);
155  float* diff_x_ptr = diff_x_ + ((input_->width + 1) << 2);
156  float* diff_y_ptr = diff_y_ + ((input_->width + 1) << 2);
157  unsigned diff_skip = 8; // skip last element in row and the first in the next row
158 
159  for (std::size_t ri = 1; ri < input_->height - 1; ++ri
160  , point_up += input_->width
161  , point_dn += input_->width
162  , point_lf += input_->width
163  , point_rg += input_->width
164  , diff_x_ptr += diff_skip
165  , diff_y_ptr += diff_skip)
166  {
167  for (std::size_t ci = 0; ci < input_->width - 2; ++ci, diff_x_ptr += 4, diff_y_ptr += 4)
168  {
169  diff_x_ptr[0] = point_rg[ci].x - point_lf[ci].x;
170  diff_x_ptr[1] = point_rg[ci].y - point_lf[ci].y;
171  diff_x_ptr[2] = point_rg[ci].z - point_lf[ci].z;
172 
173  diff_y_ptr[0] = point_dn[ci].x - point_up[ci].x;
174  diff_y_ptr[1] = point_dn[ci].y - point_up[ci].y;
175  diff_y_ptr[2] = point_dn[ci].z - point_up[ci].z;
176  }
177  }
178 
179  // Compute integral images
180  integral_image_DX_.setInput (diff_x_, input_->width, input_->height, 4, input_->width << 2);
181  integral_image_DY_.setInput (diff_y_, input_->width, input_->height, 4, input_->width << 2);
182  init_covariance_matrix_ = init_depth_change_ = init_simple_3d_gradient_ = false;
183  init_average_3d_gradient_ = true;
184 }
185 
186 //////////////////////////////////////////////////////////////////////////////////////////
187 template <typename PointInT, typename PointOutT> void
188 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::initAverageDepthChangeMethod ()
189 {
190  // number of DataType entries per element (equal or bigger than dimensions)
191  int element_stride = sizeof (PointInT) / sizeof (float);
192  // number of DataType entries per row (equal or bigger than element_stride number of elements per row)
193  int row_stride = element_stride * input_->width;
194 
195  const float *data_ = reinterpret_cast<const float*> (&(*input_)[0]);
196 
197  // integral image over the z - value
198  integral_image_depth_.setInput (&(data_[2]), input_->width, input_->height, element_stride, row_stride);
199  init_depth_change_ = true;
200  init_covariance_matrix_ = init_average_3d_gradient_ = init_simple_3d_gradient_ = false;
201 }
202 
203 //////////////////////////////////////////////////////////////////////////////////////////
204 template <typename PointInT, typename PointOutT> void
205 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computePointNormal (
206  const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
207 {
208  float bad_point = std::numeric_limits<float>::quiet_NaN ();
209 
210  if (normal_estimation_method_ == COVARIANCE_MATRIX)
211  {
212  if (!init_covariance_matrix_)
213  initCovarianceMatrixMethod ();
214 
215  unsigned count = integral_image_XYZ_.getFiniteElementsCount (pos_x - (rect_width_2_), pos_y - (rect_height_2_), rect_width_, rect_height_);
216 
217  // no valid points within the rectangular region?
218  if (count == 0)
219  {
220  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
221  return;
222  }
223 
224  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
225  Eigen::Vector3f center;
226  typename IntegralImage2D<float, 3>::SecondOrderType so_elements;
227  center = integral_image_XYZ_.getFirstOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_).template cast<float> ();
228  so_elements = integral_image_XYZ_.getSecondOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
229 
230  covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
231  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
232  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
233  covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
234  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
235  covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
236  covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
237  float eigen_value;
238  Eigen::Vector3f eigen_vector;
239  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
240  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
241  normal.getNormalVector3fMap () = eigen_vector;
242 
243  // Compute the curvature surface change
244  if (eigen_value > 0.0)
245  normal.curvature = std::abs (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
246  else
247  normal.curvature = 0;
248 
249  return;
250  }
251  if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
252  {
253  if (!init_average_3d_gradient_)
254  initAverage3DGradientMethod ();
255 
256  unsigned count_x = integral_image_DX_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
257  unsigned count_y = integral_image_DY_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
258  if (count_x == 0 || count_y == 0)
259  {
260  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
261  return;
262  }
263  Eigen::Vector3d gradient_x = integral_image_DX_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
264  Eigen::Vector3d gradient_y = integral_image_DY_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
265 
266  Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
267  double normal_length = normal_vector.squaredNorm ();
268  if (normal_length == 0.0f)
269  {
270  normal.getNormalVector3fMap ().setConstant (bad_point);
271  normal.curvature = bad_point;
272  return;
273  }
274 
275  normal_vector /= sqrt (normal_length);
276 
277  float nx = static_cast<float> (normal_vector [0]);
278  float ny = static_cast<float> (normal_vector [1]);
279  float nz = static_cast<float> (normal_vector [2]);
280 
281  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
282 
283  normal.normal_x = nx;
284  normal.normal_y = ny;
285  normal.normal_z = nz;
286  normal.curvature = bad_point;
287  return;
288  }
289  if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
290  {
291  if (!init_depth_change_)
292  initAverageDepthChangeMethod ();
293 
294  // width and height are at least 3 x 3
295  unsigned count_L_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
296  unsigned count_R_z = integral_image_depth_.getFiniteElementsCount (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
297  unsigned count_U_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_);
298  unsigned count_D_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_);
299 
300  if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
301  {
302  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
303  return;
304  }
305 
306  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);
307  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);
308  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);
309  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);
310 
311  PointInT pointL = (*input_)[point_index - rect_width_4_ - 1];
312  PointInT pointR = (*input_)[point_index + rect_width_4_ + 1];
313  PointInT pointU = (*input_)[point_index - rect_height_4_ * input_->width - 1];
314  PointInT pointD = (*input_)[point_index + rect_height_4_ * input_->width + 1];
315 
316  const float mean_x_z = mean_R_z - mean_L_z;
317  const float mean_y_z = mean_D_z - mean_U_z;
318 
319  const float mean_x_x = pointR.x - pointL.x;
320  const float mean_x_y = pointR.y - pointL.y;
321  const float mean_y_x = pointD.x - pointU.x;
322  const float mean_y_y = pointD.y - pointU.y;
323 
324  float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
325  float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
326  float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
327 
328  const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
329 
330  if (normal_length == 0.0f)
331  {
332  normal.getNormalVector3fMap ().setConstant (bad_point);
333  normal.curvature = bad_point;
334  return;
335  }
336 
337  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
338 
339  const float scale = 1.0f / std::sqrt (normal_length);
340 
341  normal.normal_x = normal_x * scale;
342  normal.normal_y = normal_y * scale;
343  normal.normal_z = normal_z * scale;
344  normal.curvature = bad_point;
345 
346  return;
347  }
348  if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
349  {
350  if (!init_simple_3d_gradient_)
351  initSimple3DGradientMethod ();
352 
353  // this method does not work if lots of NaNs are in the neighborhood of the point
354  Eigen::Vector3d gradient_x = integral_image_XYZ_.getFirstOrderSum (pos_x + rect_width_2_, pos_y - rect_height_2_, 1, rect_height_) -
355  integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, 1, rect_height_);
356 
357  Eigen::Vector3d gradient_y = integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y + rect_height_2_, rect_width_, 1) -
358  integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, 1);
359  Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
360  double normal_length = normal_vector.squaredNorm ();
361  if (normal_length == 0.0f)
362  {
363  normal.getNormalVector3fMap ().setConstant (bad_point);
364  normal.curvature = bad_point;
365  return;
366  }
367 
368  normal_vector /= sqrt (normal_length);
369 
370  float nx = static_cast<float> (normal_vector [0]);
371  float ny = static_cast<float> (normal_vector [1]);
372  float nz = static_cast<float> (normal_vector [2]);
373 
374  flipNormalTowardsViewpoint ((*input_)[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
375 
376  normal.normal_x = nx;
377  normal.normal_y = ny;
378  normal.normal_z = nz;
379  normal.curvature = bad_point;
380  return;
381  }
382 
383  normal.getNormalVector3fMap ().setConstant (bad_point);
384  normal.curvature = bad_point;
385  return;
386 }
387 
388 //////////////////////////////////////////////////////////////////////////////////////////
389 template <typename T>
390 void
391 sumArea (int start_x, int start_y, int end_x, int end_y, const int width, const int height,
392  const std::function<T(unsigned, unsigned, unsigned, unsigned)> &f,
393  T & result)
394 {
395  if (start_x < 0)
396  {
397  if (start_y < 0)
398  {
399  result += f (0, 0, end_x, end_y);
400  result += f (0, 0, -start_x, -start_y);
401  result += f (0, 0, -start_x, end_y);
402  result += f (0, 0, end_x, -start_y);
403  }
404  else if (end_y >= height)
405  {
406  result += f (0, start_y, end_x, height-1);
407  result += f (0, start_y, -start_x, height-1);
408  result += f (0, height-(end_y-(height-1)), end_x, height-1);
409  result += f (0, height-(end_y-(height-1)), -start_x, height-1);
410  }
411  else
412  {
413  result += f (0, start_y, end_x, end_y);
414  result += f (0, start_y, -start_x, end_y);
415  }
416  }
417  else if (start_y < 0)
418  {
419  if (end_x >= width)
420  {
421  result += f (start_x, 0, width-1, end_y);
422  result += f (start_x, 0, width-1, -start_y);
423  result += f (width-(end_x-(width-1)), 0, width-1, end_y);
424  result += f (width-(end_x-(width-1)), 0, width-1, -start_y);
425  }
426  else
427  {
428  result += f (start_x, 0, end_x, end_y);
429  result += f (start_x, 0, end_x, -start_y);
430  }
431  }
432  else if (end_x >= width)
433  {
434  if (end_y >= height)
435  {
436  result += f (start_x, start_y, width-1, height-1);
437  result += f (start_x, height-(end_y-(height-1)), width-1, height-1);
438  result += f (width-(end_x-(width-1)), start_y, width-1, height-1);
439  result += f (width-(end_x-(width-1)), height-(end_y-(height-1)), width-1, height-1);
440  }
441  else
442  {
443  result += f (start_x, start_y, width-1, end_y);
444  result += f (width-(end_x-(width-1)), start_y, width-1, end_y);
445  }
446  }
447  else if (end_y >= height)
448  {
449  result += f (start_x, start_y, end_x, height-1);
450  result += f (start_x, height-(end_y-(height-1)), end_x, height-1);
451  }
452  else
453  {
454  result += f (start_x, start_y, end_x, end_y);
455  }
456 }
457 
458 //////////////////////////////////////////////////////////////////////////////////////////
459 template <typename PointInT, typename PointOutT> void
460 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computePointNormalMirror (
461  const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
462 {
463  float bad_point = std::numeric_limits<float>::quiet_NaN ();
464 
465  const int width = input_->width;
466  const int height = input_->height;
467 
468  // ==============================================================
469  if (normal_estimation_method_ == COVARIANCE_MATRIX)
470  {
471  if (!init_covariance_matrix_)
472  initCovarianceMatrixMethod ();
473 
474  const int start_x = pos_x - rect_width_2_;
475  const int start_y = pos_y - rect_height_2_;
476  const int end_x = start_x + rect_width_;
477  const int end_y = start_y + rect_height_;
478 
479  unsigned count = 0;
480  auto cb_xyz_fecse = [this] (unsigned p1, unsigned p2, unsigned p3, unsigned p4) { return integral_image_XYZ_.getFiniteElementsCountSE (p1, p2, p3, p4); };
481  sumArea<unsigned> (start_x, start_y, end_x, end_y, width, height, cb_xyz_fecse, count);
482 
483  // no valid points within the rectangular region?
484  if (count == 0)
485  {
486  normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
487  return;
488  }
489 
490  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
491  Eigen::Vector3f center;
492  typename IntegralImage2D<float, 3>::SecondOrderType so_elements;
493  typename IntegralImage2D<float, 3>::ElementType tmp_center;
494  typename IntegralImage2D<float, 3>::SecondOrderType tmp_so_elements;
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] = float (tmp_center[0]);
515  center[1] = float (tmp_center[1]);
516  center[2] = 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 /= float (count_L_z);
674  mean_R_z /= float (count_R_z);
675  mean_U_z /= float (count_U_z);
676  mean_D_z /= 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  unsigned border = int(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  computePointNormal (ci, ri, index, output [index]);
899  }
900  else
901  {
902  output[index].getNormalVector3fMap ().setConstant (bad_point);
903  output[index].curvature = bad_point;
904  }
905  }
906  }
907  }
908  else
909  {
910  float smoothing_constant = normal_smoothing_size_;
911 
912  index = border + input_->width * border;
913  unsigned skip = (border << 1);
914  for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
915  {
916  for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
917  {
918  index = ri * input_->width + ci;
919 
920  if (!std::isfinite ((*input_)[index].z))
921  {
922  output [index].getNormalVector3fMap ().setConstant (bad_point);
923  output [index].curvature = bad_point;
924  continue;
925  }
926 
927  float smoothing = (std::min)(distanceMap[index], smoothing_constant);
928 
929  if (smoothing > 2.0f)
930  {
931  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
932  computePointNormal (ci, ri, index, output [index]);
933  }
934  else
935  {
936  output [index].getNormalVector3fMap ().setConstant (bad_point);
937  output [index].curvature = bad_point;
938  }
939  }
940  }
941  }
942  }
943  else if (border_policy_ == BORDER_POLICY_MIRROR)
944  {
945  output.is_dense = false;
946 
947  if (use_depth_dependent_smoothing_)
948  {
949  //index = 0;
950  //unsigned skip = 0;
951  //for (unsigned ri = 0; ri < input_->height; ++ri, index += skip)
952  for (unsigned ri = 0; ri < input_->height; ++ri)
953  {
954  //for (unsigned ci = 0; ci < input_->width; ++ci, ++index)
955  for (unsigned ci = 0; ci < input_->width; ++ci)
956  {
957  index = ri * input_->width + ci;
958 
959  const float depth = (*input_)[index].z;
960  if (!std::isfinite (depth))
961  {
962  output[index].getNormalVector3fMap ().setConstant (bad_point);
963  output[index].curvature = bad_point;
964  continue;
965  }
966 
967  float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
968 
969  if (smoothing > 2.0f)
970  {
971  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
972  computePointNormalMirror (ci, ri, index, output [index]);
973  }
974  else
975  {
976  output[index].getNormalVector3fMap ().setConstant (bad_point);
977  output[index].curvature = bad_point;
978  }
979  }
980  }
981  }
982  else
983  {
984  float smoothing_constant = normal_smoothing_size_;
985 
986  //index = border + input_->width * border;
987  //unsigned skip = (border << 1);
988  //for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
989  for (unsigned ri = 0; ri < input_->height; ++ri)
990  {
991  //for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
992  for (unsigned ci = 0; ci < input_->width; ++ci)
993  {
994  index = ri * input_->width + ci;
995 
996  if (!std::isfinite ((*input_)[index].z))
997  {
998  output [index].getNormalVector3fMap ().setConstant (bad_point);
999  output [index].curvature = bad_point;
1000  continue;
1001  }
1002 
1003  float smoothing = (std::min)(distanceMap[index], smoothing_constant);
1004 
1005  if (smoothing > 2.0f)
1006  {
1007  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1008  computePointNormalMirror (ci, ri, index, output [index]);
1009  }
1010  else
1011  {
1012  output [index].getNormalVector3fMap ().setConstant (bad_point);
1013  output [index].curvature = bad_point;
1014  }
1015  }
1016  }
1017  }
1018  }
1019 }
1020 
1021 ///////////////////////////////////////////////////////////////////////////////////////////
1022 template <typename PointInT, typename PointOutT> void
1023 pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computeFeaturePart (const float *distanceMap,
1024  const float &bad_point,
1025  PointCloudOut &output)
1026 {
1027  if (border_policy_ == BORDER_POLICY_IGNORE)
1028  {
1029  output.is_dense = false;
1030  unsigned border = int(normal_smoothing_size_);
1031  unsigned bottom = input_->height > border ? input_->height - border : 0;
1032  unsigned right = input_->width > border ? input_->width - border : 0;
1033  if (use_depth_dependent_smoothing_)
1034  {
1035  // Iterating over the entire index vector
1036  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1037  {
1038  unsigned pt_index = (*indices_)[idx];
1039  unsigned u = pt_index % input_->width;
1040  unsigned v = pt_index / input_->width;
1041  if (v < border || v > bottom)
1042  {
1043  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1044  output[idx].curvature = bad_point;
1045  continue;
1046  }
1047 
1048  if (u < border || u > right)
1049  {
1050  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1051  output[idx].curvature = bad_point;
1052  continue;
1053  }
1054 
1055  const float depth = (*input_)[pt_index].z;
1056  if (!std::isfinite (depth))
1057  {
1058  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1059  output[idx].curvature = bad_point;
1060  continue;
1061  }
1062 
1063  float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
1064  if (smoothing > 2.0f)
1065  {
1066  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1067  computePointNormal (u, v, pt_index, output [idx]);
1068  }
1069  else
1070  {
1071  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1072  output[idx].curvature = bad_point;
1073  }
1074  }
1075  }
1076  else
1077  {
1078  float smoothing_constant = normal_smoothing_size_;
1079  // Iterating over the entire index vector
1080  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1081  {
1082  unsigned pt_index = (*indices_)[idx];
1083  unsigned u = pt_index % input_->width;
1084  unsigned v = pt_index / input_->width;
1085  if (v < border || v > bottom)
1086  {
1087  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1088  output[idx].curvature = bad_point;
1089  continue;
1090  }
1091 
1092  if (u < border || u > right)
1093  {
1094  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1095  output[idx].curvature = bad_point;
1096  continue;
1097  }
1098 
1099  if (!std::isfinite ((*input_)[pt_index].z))
1100  {
1101  output [idx].getNormalVector3fMap ().setConstant (bad_point);
1102  output [idx].curvature = bad_point;
1103  continue;
1104  }
1105 
1106  float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
1107 
1108  if (smoothing > 2.0f)
1109  {
1110  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1111  computePointNormal (u, v, pt_index, output [idx]);
1112  }
1113  else
1114  {
1115  output [idx].getNormalVector3fMap ().setConstant (bad_point);
1116  output [idx].curvature = bad_point;
1117  }
1118  }
1119  }
1120  }// border_policy_ == BORDER_POLICY_IGNORE
1121  else if (border_policy_ == BORDER_POLICY_MIRROR)
1122  {
1123  output.is_dense = false;
1124 
1125  if (use_depth_dependent_smoothing_)
1126  {
1127  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
1128  {
1129  unsigned pt_index = (*indices_)[idx];
1130  unsigned u = pt_index % input_->width;
1131  unsigned v = pt_index / input_->width;
1132 
1133  const float depth = (*input_)[pt_index].z;
1134  if (!std::isfinite (depth))
1135  {
1136  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1137  output[idx].curvature = bad_point;
1138  continue;
1139  }
1140 
1141  float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
1142 
1143  if (smoothing > 2.0f)
1144  {
1145  setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
1146  computePointNormalMirror (u, v, pt_index, output [idx]);
1147  }
1148  else
1149  {
1150  output[idx].getNormalVector3fMap ().setConstant (bad_point);
1151  output[idx].curvature = bad_point;
1152  }
1153  }
1154  }
1155  else
1156  {
1157  float smoothing_constant = normal_smoothing_size_;
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], smoothing_constant);
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:194
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:114
pcl::IntegralImage2D::getFirstOrderSum
ElementType getFirstOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the first order sum within a given rectangle.
Definition: integral_image2D.hpp:72
pcl::IntegralImage2D::getSecondOrderSum
SecondOrderType getSecondOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the second order sum within a given rectangle.
Definition: integral_image2D.hpp:86
pcl::IntegralImageNormalEstimation
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:55
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:92
pcl::IntegralImageNormalEstimation::~IntegralImageNormalEstimation
~IntegralImageNormalEstimation() override
Destructor.
Definition: integral_image_normal.hpp:45
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:205
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:1023
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:460
pcl::PCLException
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:64
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:296
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