Point Cloud Library (PCL)  1.11.1
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/memory.h>
42 #include <pcl/pcl_macros.h>
43 #include <pcl/point_cloud.h>
44 #include <pcl/point_types.h>
45 #include <pcl/features/feature.h>
46 #include <pcl/features/integral_image2D.h>
47 
48 namespace pcl
49 {
50  /** \brief Surface normal estimation on organized data using integral images.
51  *
52  * For detailed information about this method see:
53  *
54  * S. Holzer and R. B. Rusu and M. Dixon and S. Gedikli and N. Navab,
55  * Adaptive Neighborhood Selection for Real-Time Surface Normal Estimation
56  * from Organized Point Cloud Data Using Integral Images, IROS 2012.
57  *
58  * D. Holz, S. Holzer, R. B. Rusu, and S. Behnke (2011, July).
59  * Real-Time Plane Segmentation using RGB-D Cameras. In Proceedings of
60  * the 15th RoboCup International Symposium, Istanbul, Turkey.
61  * http://www.ais.uni-bonn.de/~holz/papers/holz_2011_robocup.pdf
62  *
63  * \author Stefan Holzer
64  */
65  template <typename PointInT, typename PointOutT>
66  class IntegralImageNormalEstimation: public Feature<PointInT, PointOutT>
67  {
73 
74  public:
75  using Ptr = shared_ptr<IntegralImageNormalEstimation<PointInT, PointOutT> >;
76  using ConstPtr = shared_ptr<const IntegralImageNormalEstimation<PointInT, PointOutT> >;
77 
78  /** \brief Different types of border handling. */
80  {
83  };
84 
85  /** \brief Different normal estimation methods.
86  * <ul>
87  * <li><b>COVARIANCE_MATRIX</b> - creates 9 integral images to compute the normal for a specific point
88  * from the covariance matrix of its local neighborhood.</li>
89  * <li><b>AVERAGE_3D_GRADIENT</b> - creates 6 integral images to compute smoothed versions of
90  * horizontal and vertical 3D gradients and computes the normals using the cross-product between these
91  * two gradients.
92  * <li><b>AVERAGE_DEPTH_CHANGE</b> - creates only a single integral image and computes the normals
93  * from the average depth changes.
94  * </ul>
95  */
97  {
102  };
103 
106 
107  /** \brief Constructor */
109  : normal_estimation_method_(AVERAGE_3D_GRADIENT)
110  , border_policy_ (BORDER_POLICY_IGNORE)
111  , rect_width_ (0), rect_width_2_ (0), rect_width_4_ (0)
112  , rect_height_ (0), rect_height_2_ (0), rect_height_4_ (0)
113  , distance_threshold_ (0)
114  , integral_image_DX_ (false)
115  , integral_image_DY_ (false)
116  , integral_image_depth_ (false)
117  , integral_image_XYZ_ (true)
118  , diff_x_ (nullptr)
119  , diff_y_ (nullptr)
120  , depth_data_ (nullptr)
121  , distance_map_ (nullptr)
122  , use_depth_dependent_smoothing_ (false)
123  , max_depth_change_factor_ (20.0f*0.001f)
124  , normal_smoothing_size_ (10.0f)
125  , init_covariance_matrix_ (false)
126  , init_average_3d_gradient_ (false)
127  , init_simple_3d_gradient_ (false)
128  , init_depth_change_ (false)
129  , vpx_ (0.0f)
130  , vpy_ (0.0f)
131  , vpz_ (0.0f)
132  , use_sensor_origin_ (true)
133  {
134  feature_name_ = "IntegralImagesNormalEstimation";
135  tree_.reset ();
136  k_ = 1;
137  }
138 
139  /** \brief Destructor **/
141 
142  /** \brief Set the regions size which is considered for normal estimation.
143  * \param[in] width the width of the search rectangle
144  * \param[in] height the height of the search rectangle
145  */
146  void
147  setRectSize (const int width, const int height);
148 
149  /** \brief Sets the policy for handling borders.
150  * \param[in] border_policy the border policy.
151  */
152  void
153  setBorderPolicy (const BorderPolicy border_policy)
154  {
155  border_policy_ = border_policy;
156  }
157 
158  /** \brief Computes the normal at the specified position.
159  * \param[in] pos_x x position (pixel)
160  * \param[in] pos_y y position (pixel)
161  * \param[in] point_index the position index of the point
162  * \param[out] normal the output estimated normal
163  */
164  void
165  computePointNormal (const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal);
166 
167  /** \brief Computes the normal at the specified position with mirroring for border handling.
168  * \param[in] pos_x x position (pixel)
169  * \param[in] pos_y y position (pixel)
170  * \param[in] point_index the position index of the point
171  * \param[out] normal the output estimated normal
172  */
173  void
174  computePointNormalMirror (const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal);
175 
176  /** \brief The depth change threshold for computing object borders
177  * \param[in] max_depth_change_factor the depth change threshold for computing object borders based on
178  * depth changes
179  */
180  void
181  setMaxDepthChangeFactor (float max_depth_change_factor)
182  {
183  max_depth_change_factor_ = max_depth_change_factor;
184  }
185 
186  /** \brief Set the normal smoothing size
187  * \param[in] normal_smoothing_size factor which influences the size of the area used to smooth normals
188  * (depth dependent if useDepthDependentSmoothing is true)
189  */
190  void
191  setNormalSmoothingSize (float normal_smoothing_size)
192  {
193  if (normal_smoothing_size <= 0)
194  {
195  PCL_ERROR ("[pcl::%s::setNormalSmoothingSize] Invalid normal smoothing size given! (%f). Allowed ranges are: 0 < N. Defaulting to %f.\n",
196  feature_name_.c_str (), normal_smoothing_size, normal_smoothing_size_);
197  return;
198  }
199  normal_smoothing_size_ = normal_smoothing_size;
200  }
201 
202  /** \brief Set the normal estimation method. The current implemented algorithms are:
203  * <ul>
204  * <li><b>COVARIANCE_MATRIX</b> - creates 9 integral images to compute the normal for a specific point
205  * from the covariance matrix of its local neighborhood.</li>
206  * <li><b>AVERAGE_3D_GRADIENT</b> - creates 6 integral images to compute smoothed versions of
207  * horizontal and vertical 3D gradients and computes the normals using the cross-product between these
208  * two gradients.
209  * <li><b>AVERAGE_DEPTH_CHANGE</b> - creates only a single integral image and computes the normals
210  * from the average depth changes.
211  * </ul>
212  * \param[in] normal_estimation_method the method used for normal estimation
213  */
214  void
216  {
217  normal_estimation_method_ = normal_estimation_method;
218  }
219 
220  /** \brief Set whether to use depth depending smoothing or not
221  * \param[in] use_depth_dependent_smoothing decides whether the smoothing is depth dependent
222  */
223  void
224  setDepthDependentSmoothing (bool use_depth_dependent_smoothing)
225  {
226  use_depth_dependent_smoothing_ = use_depth_dependent_smoothing;
227  }
228 
229  /** \brief Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
230  * \param[in] cloud the const boost shared pointer to a PointCloud message
231  */
232  inline void
233  setInputCloud (const typename PointCloudIn::ConstPtr &cloud) override
234  {
235  input_ = cloud;
236  if (!cloud->isOrganized ())
237  {
238  PCL_ERROR ("[pcl::IntegralImageNormalEstimation::setInputCloud] Input dataset is not organized (height = 1).\n");
239  return;
240  }
241 
242  init_covariance_matrix_ = init_average_3d_gradient_ = init_depth_change_ = false;
243 
244  if (use_sensor_origin_)
245  {
246  vpx_ = input_->sensor_origin_.coeff (0);
247  vpy_ = input_->sensor_origin_.coeff (1);
248  vpz_ = input_->sensor_origin_.coeff (2);
249  }
250 
251  // Initialize the correct data structure based on the normal estimation method chosen
252  initData ();
253  }
254 
255  /** \brief Returns a pointer to the distance map which was computed internally
256  */
257  inline float*
259  {
260  return (distance_map_);
261  }
262 
263  /** \brief Set the viewpoint.
264  * \param vpx the X coordinate of the viewpoint
265  * \param vpy the Y coordinate of the viewpoint
266  * \param vpz the Z coordinate of the viewpoint
267  */
268  inline void
269  setViewPoint (float vpx, float vpy, float vpz)
270  {
271  vpx_ = vpx;
272  vpy_ = vpy;
273  vpz_ = vpz;
274  use_sensor_origin_ = false;
275  }
276 
277  /** \brief Get the viewpoint.
278  * \param [out] vpx x-coordinate of the view point
279  * \param [out] vpy y-coordinate of the view point
280  * \param [out] vpz z-coordinate of the view point
281  * \note this method returns the currently used viewpoint for normal flipping.
282  * If the viewpoint is set manually using the setViewPoint method, this method will return the set view point coordinates.
283  * If an input cloud is set, it will return the sensor origin otherwise it will return the origin (0, 0, 0)
284  */
285  inline void
286  getViewPoint (float &vpx, float &vpy, float &vpz)
287  {
288  vpx = vpx_;
289  vpy = vpy_;
290  vpz = vpz_;
291  }
292 
293  /** \brief sets whether the sensor origin or a user given viewpoint should be used. After this method, the
294  * normal estimation method uses the sensor origin of the input cloud.
295  * to use a user defined view point, use the method setViewPoint
296  */
297  inline void
299  {
300  use_sensor_origin_ = true;
301  if (input_)
302  {
303  vpx_ = input_->sensor_origin_.coeff (0);
304  vpy_ = input_->sensor_origin_.coeff (1);
305  vpz_ = input_->sensor_origin_.coeff (2);
306  }
307  else
308  {
309  vpx_ = 0;
310  vpy_ = 0;
311  vpz_ = 0;
312  }
313  }
314 
315  protected:
316 
317  /** \brief Computes the normal for the complete cloud or only \a indices_ if provided.
318  * \param[out] output the resultant normals
319  */
320  void
321  computeFeature (PointCloudOut &output) override;
322 
323  /** \brief Computes the normal for the complete cloud.
324  * \param[in] distance_map distance map
325  * \param[in] bad_point constant given to invalid normal components
326  * \param[out] output the resultant normals
327  */
328  void
329  computeFeatureFull (const float* distance_map, const float& bad_point, PointCloudOut& output);
330 
331  /** \brief Computes the normal for part of the cloud specified by \a indices_
332  * \param[in] distance_map distance map
333  * \param[in] bad_point constant given to invalid normal components
334  * \param[out] output the resultant normals
335  */
336  void
337  computeFeaturePart (const float* distance_map, const float& bad_point, PointCloudOut& output);
338 
339  /** \brief Initialize the data structures, based on the normal estimation method chosen. */
340  void
341  initData ();
342 
343  private:
344 
345  /** \brief Flip (in place) the estimated normal of a point towards a given viewpoint
346  * \param point a given point
347  * \param vp_x the X coordinate of the viewpoint
348  * \param vp_y the X coordinate of the viewpoint
349  * \param vp_z the X coordinate of the viewpoint
350  * \param nx the resultant X component of the plane normal
351  * \param ny the resultant Y component of the plane normal
352  * \param nz the resultant Z component of the plane normal
353  * \ingroup features
354  */
355  inline void
356  flipNormalTowardsViewpoint (const PointInT &point,
357  float vp_x, float vp_y, float vp_z,
358  float &nx, float &ny, float &nz)
359  {
360  // See if we need to flip any plane normals
361  vp_x -= point.x;
362  vp_y -= point.y;
363  vp_z -= point.z;
364 
365  // Dot product between the (viewpoint - point) and the plane normal
366  float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz);
367 
368  // Flip the plane normal
369  if (cos_theta < 0)
370  {
371  nx *= -1;
372  ny *= -1;
373  nz *= -1;
374  }
375  }
376 
377  /** \brief The normal estimation method to use. Currently, 3 implementations are provided:
378  *
379  * - COVARIANCE_MATRIX
380  * - AVERAGE_3D_GRADIENT
381  * - AVERAGE_DEPTH_CHANGE
382  */
383  NormalEstimationMethod normal_estimation_method_;
384 
385  /** \brief The policy for handling borders. */
386  BorderPolicy border_policy_;
387 
388  /** The width of the neighborhood region used for computing the normal. */
389  int rect_width_;
390  int rect_width_2_;
391  int rect_width_4_;
392  /** The height of the neighborhood region used for computing the normal. */
393  int rect_height_;
394  int rect_height_2_;
395  int rect_height_4_;
396 
397  /** the threshold used to detect depth discontinuities */
398  float distance_threshold_;
399 
400  /** integral image in x-direction */
401  IntegralImage2D<float, 3> integral_image_DX_;
402  /** integral image in y-direction */
403  IntegralImage2D<float, 3> integral_image_DY_;
404  /** integral image */
405  IntegralImage2D<float, 1> integral_image_depth_;
406  /** integral image xyz */
407  IntegralImage2D<float, 3> integral_image_XYZ_;
408 
409  /** derivatives in x-direction */
410  float *diff_x_;
411  /** derivatives in y-direction */
412  float *diff_y_;
413 
414  /** depth data */
415  float *depth_data_;
416 
417  /** distance map */
418  float *distance_map_;
419 
420  /** \brief Smooth data based on depth (true/false). */
421  bool use_depth_dependent_smoothing_;
422 
423  /** \brief Threshold for detecting depth discontinuities */
424  float max_depth_change_factor_;
425 
426  /** \brief */
427  float normal_smoothing_size_;
428 
429  /** \brief True when a dataset has been received and the covariance_matrix data has been initialized. */
430  bool init_covariance_matrix_;
431 
432  /** \brief True when a dataset has been received and the average 3d gradient data has been initialized. */
433  bool init_average_3d_gradient_;
434 
435  /** \brief True when a dataset has been received and the simple 3d gradient data has been initialized. */
436  bool init_simple_3d_gradient_;
437 
438  /** \brief True when a dataset has been received and the depth change data has been initialized. */
439  bool init_depth_change_;
440 
441  /** \brief Values describing the viewpoint ("pinhole" camera model assumed). For per point viewpoints, inherit
442  * from NormalEstimation and provide your own computeFeature (). By default, the viewpoint is set to 0,0,0. */
443  float vpx_, vpy_, vpz_;
444 
445  /** whether the sensor origin of the input cloud or a user given viewpoint should be used.*/
446  bool use_sensor_origin_;
447 
448  /** \brief This method should get called before starting the actual computation. */
449  bool
450  initCompute () override;
451 
452  /** \brief Internal initialization method for COVARIANCE_MATRIX estimation. */
453  void
454  initCovarianceMatrixMethod ();
455 
456  /** \brief Internal initialization method for AVERAGE_3D_GRADIENT estimation. */
457  void
458  initAverage3DGradientMethod ();
459 
460  /** \brief Internal initialization method for AVERAGE_DEPTH_CHANGE estimation. */
461  void
462  initAverageDepthChangeMethod ();
463 
464  /** \brief Internal initialization method for SIMPLE_3D_GRADIENT estimation. */
465  void
466  initSimple3DGradientMethod ();
467 
468  public:
470  };
471 }
472 
473 #ifdef PCL_NO_PRECOMPILE
474 #include <pcl/features/impl/integral_image_normal.hpp>
475 #endif
Defines functions, macros and traits for allocating and using memory.
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:223
int k_
The number of K nearest neighbors to use for each point.
Definition: feature.h:243
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: memory.h:63
KdTreePtr tree_
A pointer to the spatial search object.
Definition: feature.h:234
void setBorderPolicy(const BorderPolicy border_policy)
Sets the policy for handling borders.
void getViewPoint(float &vpx, float &vpy, float &vpz)
Get the viewpoint.
shared_ptr< const Feature< pcl::PointXYZRGBA, pcl::Normal > > ConstPtr
Definition: feature.h:115
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.
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
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.
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_.
shared_ptr< const PointCloud< pcl::PointXYZRGBA > > ConstPtr
Definition: point_cloud.h:430
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:150
Feature represents the base feature class.
Definition: feature.h:106
NormalEstimationMethod
Different normal estimation methods.
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
Defines all the PCL and non-PCL macros used.
shared_ptr< Feature< pcl::PointXYZRGBA, pcl::Normal > > Ptr
Definition: feature.h:114
void setDepthDependentSmoothing(bool use_depth_dependent_smoothing)
Set whether to use depth depending smoothing or not.