Point Cloud Library (PCL) 1.12.0
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/features/feature.h>
45#include <pcl/features/integral_image2D.h>
46
47namespace 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 = shared_ptr<IntegralImageNormalEstimation<PointInT, PointOutT> >;
75 using ConstPtr = 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
Feature represents the base feature class.
Definition: feature.h:107
int k_
The number of K nearest neighbors to use for each point.
Definition: feature.h:243
shared_ptr< Feature< PointInT, PointOutT > > Ptr
Definition: feature.h:114
std::string feature_name_
The feature name.
Definition: feature.h:223
shared_ptr< const Feature< PointInT, PointOutT > > ConstPtr
Definition: feature.h:115
KdTreePtr tree_
A pointer to the spatial search object.
Definition: feature.h:234
Surface normal estimation on organized data using integral images.
float * getDistanceMap()
Returns a pointer to the distance map which was computed internally.
BorderPolicy
Different types of border handling.
NormalEstimationMethod
Different normal estimation methods.
void setDepthDependentSmoothing(bool use_depth_dependent_smoothing)
Set whether to use depth depending smoothing or not.
void setBorderPolicy(const BorderPolicy border_policy)
Sets the policy for handling borders.
void initData()
Initialize the data structures, based on the normal estimation method chosen.
void useSensorOriginAsViewPoint()
sets whether the sensor origin or a user given viewpoint should be used.
void setRectSize(const int width, const int height)
Set the regions size which is considered for normal estimation.
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
void getViewPoint(float &vpx, float &vpy, float &vpz)
Get the viewpoint.
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
void computeFeatureFull(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for the complete cloud.
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
void computePointNormal(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position.
void computeFeature(PointCloudOut &output) override
Computes the normal for the complete cloud or only indices_ if provided.
void computeFeaturePart(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for part of the cloud specified by indices_.
void setViewPoint(float vpx, float vpy, float vpz)
Set the viewpoint.
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
void setMaxDepthChangeFactor(float max_depth_change_factor)
The depth change threshold for computing object borders.
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.
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:147
shared_ptr< const PointCloud< PointInT > > ConstPtr
Definition: point_cloud.h:414
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
Defines functions, macros and traits for allocating and using memory.
Defines all the PCL and non-PCL macros used.