Point Cloud Library (PCL)  1.9.1-dev
integral_image_normal.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
39 #pragma once
40 
41 #include <pcl/pcl_macros.h>
42 #include <pcl/point_cloud.h>
43 #include <pcl/point_types.h>
44 #include <pcl/features/feature.h>
45 #include <pcl/features/integral_image2D.h>
46 
47 namespace pcl
48 {
49  /** \brief Surface normal estimation on organized data using integral images.
50  *
51  * For detailed information about this method see:
52  *
53  * S. Holzer and R. B. Rusu and M. Dixon and S. Gedikli and N. Navab,
54  * Adaptive Neighborhood Selection for Real-Time Surface Normal Estimation
55  * from Organized Point Cloud Data Using Integral Images, IROS 2012.
56  *
57  * D. Holz, S. Holzer, R. B. Rusu, and S. Behnke (2011, July).
58  * Real-Time Plane Segmentation using RGB-D Cameras. In Proceedings of
59  * the 15th RoboCup International Symposium, Istanbul, Turkey.
60  * http://www.ais.uni-bonn.de/~holz/papers/holz_2011_robocup.pdf
61  *
62  * \author Stefan Holzer
63  */
64  template <typename PointInT, typename PointOutT>
65  class IntegralImageNormalEstimation: public Feature<PointInT, PointOutT>
66  {
72 
73  public:
74  using Ptr = boost::shared_ptr<IntegralImageNormalEstimation<PointInT, PointOutT> >;
75  using ConstPtr = boost::shared_ptr<const IntegralImageNormalEstimation<PointInT, PointOutT> >;
76 
77  /** \brief Different types of border handling. */
79  {
82  };
83 
84  /** \brief Different normal estimation methods.
85  * <ul>
86  * <li><b>COVARIANCE_MATRIX</b> - creates 9 integral images to compute the normal for a specific point
87  * from the covariance matrix of its local neighborhood.</li>
88  * <li><b>AVERAGE_3D_GRADIENT</b> - creates 6 integral images to compute smoothed versions of
89  * horizontal and vertical 3D gradients and computes the normals using the cross-product between these
90  * two gradients.
91  * <li><b>AVERAGE_DEPTH_CHANGE</b> - creates only a single integral image and computes the normals
92  * from the average depth changes.
93  * </ul>
94  */
96  {
101  };
102 
105 
106  /** \brief Constructor */
108  : normal_estimation_method_(AVERAGE_3D_GRADIENT)
109  , border_policy_ (BORDER_POLICY_IGNORE)
110  , rect_width_ (0), rect_width_2_ (0), rect_width_4_ (0)
111  , rect_height_ (0), rect_height_2_ (0), rect_height_4_ (0)
112  , distance_threshold_ (0)
113  , integral_image_DX_ (false)
114  , integral_image_DY_ (false)
115  , integral_image_depth_ (false)
116  , integral_image_XYZ_ (true)
117  , diff_x_ (nullptr)
118  , diff_y_ (nullptr)
119  , depth_data_ (nullptr)
120  , distance_map_ (nullptr)
121  , use_depth_dependent_smoothing_ (false)
122  , max_depth_change_factor_ (20.0f*0.001f)
123  , normal_smoothing_size_ (10.0f)
124  , init_covariance_matrix_ (false)
125  , init_average_3d_gradient_ (false)
126  , init_simple_3d_gradient_ (false)
127  , init_depth_change_ (false)
128  , vpx_ (0.0f)
129  , vpy_ (0.0f)
130  , vpz_ (0.0f)
131  , use_sensor_origin_ (true)
132  {
133  feature_name_ = "IntegralImagesNormalEstimation";
134  tree_.reset ();
135  k_ = 1;
136  }
137 
138  /** \brief Destructor **/
140 
141  /** \brief Set the regions size which is considered for normal estimation.
142  * \param[in] width the width of the search rectangle
143  * \param[in] height the height of the search rectangle
144  */
145  void
146  setRectSize (const int width, const int height);
147 
148  /** \brief Sets the policy for handling borders.
149  * \param[in] border_policy the border policy.
150  */
151  void
152  setBorderPolicy (const BorderPolicy border_policy)
153  {
154  border_policy_ = border_policy;
155  }
156 
157  /** \brief Computes the normal at the specified position.
158  * \param[in] pos_x x position (pixel)
159  * \param[in] pos_y y position (pixel)
160  * \param[in] point_index the position index of the point
161  * \param[out] normal the output estimated normal
162  */
163  void
164  computePointNormal (const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal);
165 
166  /** \brief Computes the normal at the specified position with mirroring for border handling.
167  * \param[in] pos_x x position (pixel)
168  * \param[in] pos_y y position (pixel)
169  * \param[in] point_index the position index of the point
170  * \param[out] normal the output estimated normal
171  */
172  void
173  computePointNormalMirror (const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal);
174 
175  /** \brief The depth change threshold for computing object borders
176  * \param[in] max_depth_change_factor the depth change threshold for computing object borders based on
177  * depth changes
178  */
179  void
180  setMaxDepthChangeFactor (float max_depth_change_factor)
181  {
182  max_depth_change_factor_ = max_depth_change_factor;
183  }
184 
185  /** \brief Set the normal smoothing size
186  * \param[in] normal_smoothing_size factor which influences the size of the area used to smooth normals
187  * (depth dependent if useDepthDependentSmoothing is true)
188  */
189  void
190  setNormalSmoothingSize (float normal_smoothing_size)
191  {
192  if (normal_smoothing_size <= 0)
193  {
194  PCL_ERROR ("[pcl::%s::setNormalSmoothingSize] Invalid normal smoothing size given! (%f). Allowed ranges are: 0 < N. Defaulting to %f.\n",
195  feature_name_.c_str (), normal_smoothing_size, normal_smoothing_size_);
196  return;
197  }
198  normal_smoothing_size_ = normal_smoothing_size;
199  }
200 
201  /** \brief Set the normal estimation method. The current implemented algorithms are:
202  * <ul>
203  * <li><b>COVARIANCE_MATRIX</b> - creates 9 integral images to compute the normal for a specific point
204  * from the covariance matrix of its local neighborhood.</li>
205  * <li><b>AVERAGE_3D_GRADIENT</b> - creates 6 integral images to compute smoothed versions of
206  * horizontal and vertical 3D gradients and computes the normals using the cross-product between these
207  * two gradients.
208  * <li><b>AVERAGE_DEPTH_CHANGE</b> - creates only a single integral image and computes the normals
209  * from the average depth changes.
210  * </ul>
211  * \param[in] normal_estimation_method the method used for normal estimation
212  */
213  void
215  {
216  normal_estimation_method_ = normal_estimation_method;
217  }
218 
219  /** \brief Set whether to use depth depending smoothing or not
220  * \param[in] use_depth_dependent_smoothing decides whether the smoothing is depth dependent
221  */
222  void
223  setDepthDependentSmoothing (bool use_depth_dependent_smoothing)
224  {
225  use_depth_dependent_smoothing_ = use_depth_dependent_smoothing;
226  }
227 
228  /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
229  * \param[in] cloud the const boost shared pointer to a PointCloud message
230  */
231  inline void
232  setInputCloud (const typename PointCloudIn::ConstPtr &cloud) override
233  {
234  input_ = cloud;
235  if (!cloud->isOrganized ())
236  {
237  PCL_ERROR ("[pcl::IntegralImageNormalEstimation::setInputCloud] Input dataset is not organized (height = 1).\n");
238  return;
239  }
240 
241  init_covariance_matrix_ = init_average_3d_gradient_ = init_depth_change_ = false;
242 
243  if (use_sensor_origin_)
244  {
245  vpx_ = input_->sensor_origin_.coeff (0);
246  vpy_ = input_->sensor_origin_.coeff (1);
247  vpz_ = input_->sensor_origin_.coeff (2);
248  }
249 
250  // Initialize the correct data structure based on the normal estimation method chosen
251  initData ();
252  }
253 
254  /** \brief Returns a pointer to the distance map which was computed internally
255  */
256  inline float*
258  {
259  return (distance_map_);
260  }
261 
262  /** \brief Set the viewpoint.
263  * \param vpx the X coordinate of the viewpoint
264  * \param vpy the Y coordinate of the viewpoint
265  * \param vpz the Z coordinate of the viewpoint
266  */
267  inline void
268  setViewPoint (float vpx, float vpy, float vpz)
269  {
270  vpx_ = vpx;
271  vpy_ = vpy;
272  vpz_ = vpz;
273  use_sensor_origin_ = false;
274  }
275 
276  /** \brief Get the viewpoint.
277  * \param [out] vpx x-coordinate of the view point
278  * \param [out] vpy y-coordinate of the view point
279  * \param [out] vpz z-coordinate of the view point
280  * \note this method returns the currently used viewpoint for normal flipping.
281  * If the viewpoint is set manually using the setViewPoint method, this method will return the set view point coordinates.
282  * If an input cloud is set, it will return the sensor origin otherwise it will return the origin (0, 0, 0)
283  */
284  inline void
285  getViewPoint (float &vpx, float &vpy, float &vpz)
286  {
287  vpx = vpx_;
288  vpy = vpy_;
289  vpz = vpz_;
290  }
291 
292  /** \brief sets whether the sensor origin or a user given viewpoint should be used. After this method, the
293  * normal estimation method uses the sensor origin of the input cloud.
294  * to use a user defined view point, use the method setViewPoint
295  */
296  inline void
298  {
299  use_sensor_origin_ = true;
300  if (input_)
301  {
302  vpx_ = input_->sensor_origin_.coeff (0);
303  vpy_ = input_->sensor_origin_.coeff (1);
304  vpz_ = input_->sensor_origin_.coeff (2);
305  }
306  else
307  {
308  vpx_ = 0;
309  vpy_ = 0;
310  vpz_ = 0;
311  }
312  }
313 
314  protected:
315 
316  /** \brief Computes the normal for the complete cloud or only \a indices_ if provided.
317  * \param[out] output the resultant normals
318  */
319  void
320  computeFeature (PointCloudOut &output) override;
321 
322  /** \brief Computes the normal for the complete cloud.
323  * \param[in] distance_map distance map
324  * \param[in] bad_point constant given to invalid normal components
325  * \param[out] output the resultant normals
326  */
327  void
328  computeFeatureFull (const float* distance_map, const float& bad_point, PointCloudOut& output);
329 
330  /** \brief Computes the normal for part of the cloud specified by \a indices_
331  * \param[in] distance_map distance map
332  * \param[in] bad_point constant given to invalid normal components
333  * \param[out] output the resultant normals
334  */
335  void
336  computeFeaturePart (const float* distance_map, const float& bad_point, PointCloudOut& output);
337 
338  /** \brief Initialize the data structures, based on the normal estimation method chosen. */
339  void
340  initData ();
341 
342  private:
343 
344  /** \brief Flip (in place) the estimated normal of a point towards a given viewpoint
345  * \param point a given point
346  * \param vp_x the X coordinate of the viewpoint
347  * \param vp_y the X coordinate of the viewpoint
348  * \param vp_z the X coordinate of the viewpoint
349  * \param nx the resultant X component of the plane normal
350  * \param ny the resultant Y component of the plane normal
351  * \param nz the resultant Z component of the plane normal
352  * \ingroup features
353  */
354  inline void
355  flipNormalTowardsViewpoint (const PointInT &point,
356  float vp_x, float vp_y, float vp_z,
357  float &nx, float &ny, float &nz)
358  {
359  // See if we need to flip any plane normals
360  vp_x -= point.x;
361  vp_y -= point.y;
362  vp_z -= point.z;
363 
364  // Dot product between the (viewpoint - point) and the plane normal
365  float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz);
366 
367  // Flip the plane normal
368  if (cos_theta < 0)
369  {
370  nx *= -1;
371  ny *= -1;
372  nz *= -1;
373  }
374  }
375 
376  /** \brief The normal estimation method to use. Currently, 3 implementations are provided:
377  *
378  * - COVARIANCE_MATRIX
379  * - AVERAGE_3D_GRADIENT
380  * - AVERAGE_DEPTH_CHANGE
381  */
382  NormalEstimationMethod normal_estimation_method_;
383 
384  /** \brief The policy for handling borders. */
385  BorderPolicy border_policy_;
386 
387  /** The width of the neighborhood region used for computing the normal. */
388  int rect_width_;
389  int rect_width_2_;
390  int rect_width_4_;
391  /** The height of the neighborhood region used for computing the normal. */
392  int rect_height_;
393  int rect_height_2_;
394  int rect_height_4_;
395 
396  /** the threshold used to detect depth discontinuities */
397  float distance_threshold_;
398 
399  /** integral image in x-direction */
400  IntegralImage2D<float, 3> integral_image_DX_;
401  /** integral image in y-direction */
402  IntegralImage2D<float, 3> integral_image_DY_;
403  /** integral image */
404  IntegralImage2D<float, 1> integral_image_depth_;
405  /** integral image xyz */
406  IntegralImage2D<float, 3> integral_image_XYZ_;
407 
408  /** derivatives in x-direction */
409  float *diff_x_;
410  /** derivatives in y-direction */
411  float *diff_y_;
412 
413  /** depth data */
414  float *depth_data_;
415 
416  /** distance map */
417  float *distance_map_;
418 
419  /** \brief Smooth data based on depth (true/false). */
420  bool use_depth_dependent_smoothing_;
421 
422  /** \brief Threshold for detecting depth discontinuities */
423  float max_depth_change_factor_;
424 
425  /** \brief */
426  float normal_smoothing_size_;
427 
428  /** \brief True when a dataset has been received and the covariance_matrix data has been initialized. */
429  bool init_covariance_matrix_;
430 
431  /** \brief True when a dataset has been received and the average 3d gradient data has been initialized. */
432  bool init_average_3d_gradient_;
433 
434  /** \brief True when a dataset has been received and the simple 3d gradient data has been initialized. */
435  bool init_simple_3d_gradient_;
436 
437  /** \brief True when a dataset has been received and the depth change data has been initialized. */
438  bool init_depth_change_;
439 
440  /** \brief Values describing the viewpoint ("pinhole" camera model assumed). For per point viewpoints, inherit
441  * from NormalEstimation and provide your own computeFeature (). By default, the viewpoint is set to 0,0,0. */
442  float vpx_, vpy_, vpz_;
443 
444  /** whether the sensor origin of the input cloud or a user given viewpoint should be used.*/
445  bool use_sensor_origin_;
446 
447  /** \brief This method should get called before starting the actual computation. */
448  bool
449  initCompute () override;
450 
451  /** \brief Internal initialization method for COVARIANCE_MATRIX estimation. */
452  void
453  initCovarianceMatrixMethod ();
454 
455  /** \brief Internal initialization method for AVERAGE_3D_GRADIENT estimation. */
456  void
457  initAverage3DGradientMethod ();
458 
459  /** \brief Internal initialization method for AVERAGE_DEPTH_CHANGE estimation. */
460  void
461  initAverageDepthChangeMethod ();
462 
463  /** \brief Internal initialization method for SIMPLE_3D_GRADIENT estimation. */
464  void
465  initSimple3DGradientMethod ();
466 
467  public:
469  };
470 }
471 
472 #ifdef PCL_NO_PRECOMPILE
473 #include <pcl/features/impl/integral_image_normal.hpp>
474 #endif
boost::shared_ptr< const Feature< pcl::PointXYZRGBA, pcl::Normal > > ConstPtr
Definition: feature.h:114
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. ...
std::string feature_name_
The feature name.
Definition: feature.h:222
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
int k_
The number of K nearest neighbors to use for each point.
Definition: feature.h:242
void computePointNormal(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:359
KdTreePtr tree_
A pointer to the spatial search object.
Definition: feature.h:233
void setBorderPolicy(const BorderPolicy border_policy)
Sets the policy for handling borders.
void getViewPoint(float &vpx, float &vpy, float &vpz)
Get the viewpoint.
void setRectSize(const int width, const int height)
Set the regions size which is considered for normal estimation.
void initData()
Initialize the data structures, based on the normal estimation method chosen.
Defines all the PCL implemented PointT point type structures.
Surface normal estimation on organized data using integral images.
void setMaxDepthChangeFactor(float max_depth_change_factor)
The depth change threshold for computing object borders.
void useSensorOriginAsViewPoint()
sets whether the sensor origin or a user given viewpoint should be used.
void computeFeatureFull(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for the complete cloud.
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
BorderPolicy
Different types of border handling.
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
void computeFeature(PointCloudOut &output) override
Computes the normal for the complete cloud or only indices_ if provided.
void setViewPoint(float vpx, float vpy, float vpz)
Set the viewpoint.
boost::shared_ptr< const PointCloud< pcl::PointXYZRGBA > > ConstPtr
Definition: point_cloud.h:413
float * getDistanceMap()
Returns a pointer to the distance map which was computed internally.
void computeFeaturePart(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for part of the cloud specified by indices_.
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:151
Feature represents the base feature class.
Definition: feature.h:105
NormalEstimationMethod
Different normal estimation methods.
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
boost::shared_ptr< Feature< pcl::PointXYZRGBA, pcl::Normal > > Ptr
Definition: feature.h:113
Defines all the PCL and non-PCL macros used.
void setDepthDependentSmoothing(bool use_depth_dependent_smoothing)
Set whether to use depth depending smoothing or not.