Point Cloud Library (PCL)  1.7.2
range_image_border_extractor.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010, 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 #ifndef PCL_RANGE_IMAGE_BORDER_EXTRACTOR_H_
39 #define PCL_RANGE_IMAGE_BORDER_EXTRACTOR_H_
40 
41 #include <pcl/point_types.h>
42 #include <pcl/features/feature.h>
43 
44 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
45 #pragma GCC diagnostic ignored "-Weffc++"
46 #endif
47 namespace pcl
48 {
49  // FORWARD DECLARATIONS:
50  class RangeImage;
51  template <typename PointType>
52  class PointCloud;
53 
54  /** \brief @b Extract obstacle borders from range images, meaning positions where there is a transition from foreground
55  * to background.
56  * \author Bastian Steder
57  * \ingroup features
58  */
59  class PCL_EXPORTS RangeImageBorderExtractor : public Feature<PointWithRange,BorderDescription>
60  {
61  public:
62  typedef boost::shared_ptr<RangeImageBorderExtractor> Ptr;
63  typedef boost::shared_ptr<const RangeImageBorderExtractor> ConstPtr;
64  // =====TYPEDEFS=====
66 
67  // =====PUBLIC STRUCTS=====
68  //! Stores some information extracted from the neighborhood of a point
69  struct LocalSurface
70  {
72  normal (), neighborhood_mean (), eigen_values (), normal_no_jumps (),
73  neighborhood_mean_no_jumps (), eigen_values_no_jumps (), max_neighbor_distance_squared () {}
74 
75  Eigen::Vector3f normal;
76  Eigen::Vector3f neighborhood_mean;
77  Eigen::Vector3f eigen_values;
78  Eigen::Vector3f normal_no_jumps;
79  Eigen::Vector3f neighborhood_mean_no_jumps;
80  Eigen::Vector3f eigen_values_no_jumps;
82  };
83 
84  //! Stores the indices of the shadow border corresponding to obstacle borders
86  {
87  ShadowBorderIndices () : left (-1), right (-1), top (-1), bottom (-1) {}
88  int left, right, top, bottom;
89  };
90 
91  //! Parameters used in this class
92  struct Parameters
93  {
94  Parameters () : max_no_of_threads(1), pixel_radius_borders (3), pixel_radius_plane_extraction (2), pixel_radius_border_direction (2),
95  minimum_border_probability (0.8f), pixel_radius_principal_curvature (2) {}
102  };
103 
104  // =====STATIC METHODS=====
105  /** \brief Take the information from BorderTraits to calculate the local direction of the border
106  * \param border_traits contains the information needed to calculate the border angle
107  */
108  static inline float
109  getObstacleBorderAngle (const BorderTraits& border_traits);
110 
111  // =====CONSTRUCTOR & DESTRUCTOR=====
112  /** Constructor */
113  RangeImageBorderExtractor (const RangeImage* range_image=NULL);
114  /** Destructor */
115  virtual ~RangeImageBorderExtractor ();
116 
117  // =====METHODS=====
118  /** \brief Provide a pointer to the range image
119  * \param range_image a pointer to the range_image
120  */
121  void
122  setRangeImage (const RangeImage* range_image);
123 
124  /** \brief Erase all data calculated for the current range image */
125  void
126  clearData ();
127 
128  /** \brief Get the 2D directions in the range image from the border directions - probably mainly useful for
129  * visualization
130  */
131  float*
132  getAnglesImageForBorderDirections ();
133 
134  /** \brief Get the 2D directions in the range image from the surface change directions - probably mainly useful for
135  * visualization
136  */
137  float*
138  getAnglesImageForSurfaceChangeDirections ();
139 
140  /** Overwrite the compute function of the base class */
141  void
142  compute (PointCloudOut& output);
143 
144  // =====GETTER=====
145  Parameters&
146  getParameters () { return (parameters_); }
147 
148  bool
149  hasRangeImage () const { return range_image_ != NULL; }
150 
151  const RangeImage&
152  getRangeImage () const { return *range_image_; }
153 
154  float*
155  getBorderScoresLeft () { extractBorderScoreImages (); return border_scores_left_; }
156 
157  float*
158  getBorderScoresRight () { extractBorderScoreImages (); return border_scores_right_; }
159 
160  float*
161  getBorderScoresTop () { extractBorderScoreImages (); return border_scores_top_; }
162 
163  float*
164  getBorderScoresBottom () { extractBorderScoreImages (); return border_scores_bottom_; }
165 
166  LocalSurface**
167  getSurfaceStructure () { extractLocalSurfaceStructure (); return surface_structure_; }
168 
169  PointCloudOut&
170  getBorderDescriptions () { classifyBorders (); return *border_descriptions_; }
171 
172  ShadowBorderIndices**
173  getShadowBorderInformations () { findAndEvaluateShadowBorders (); return shadow_border_informations_; }
174 
175  Eigen::Vector3f**
176  getBorderDirections () { calculateBorderDirections (); return border_directions_; }
177 
178  float*
179  getSurfaceChangeScores () { calculateSurfaceChanges (); return surface_change_scores_; }
180 
181  Eigen::Vector3f*
182  getSurfaceChangeDirections () { calculateSurfaceChanges (); return surface_change_directions_; }
183 
184 
185  protected:
186  // =====PROTECTED MEMBER VARIABLES=====
190  float* border_scores_left_, * border_scores_right_, * border_scores_top_, * border_scores_bottom_;
192  PointCloudOut* border_descriptions_;
194  Eigen::Vector3f** border_directions_;
195 
197  Eigen::Vector3f* surface_change_directions_;
198 
199 
200  // =====PROTECTED METHODS=====
201  /** \brief Calculate a border score based on how distant the neighbor is, compared to the closest neighbors
202  * /param local_surface
203  * /param x
204  * /param y
205  * /param offset_x
206  * /param offset_y
207  * /param pixel_radius (defaults to 1)
208  * /return the resulting border score
209  */
210  inline float
211  getNeighborDistanceChangeScore (const LocalSurface& local_surface, int x, int y,
212  int offset_x, int offset_y, int pixel_radius=1) const;
213 
214  /** \brief Calculate a border score based on how much the neighbor is away from the local surface plane
215  * \param local_surface
216  * \param x
217  * \param y
218  * \param offset_x
219  * \param offset_y
220  * \return the resulting border score
221  */
222  inline float
223  getNormalBasedBorderScore (const LocalSurface& local_surface, int x, int y,
224  int offset_x, int offset_y) const;
225 
226  /** \brief Find the best corresponding shadow border and lower score according to the shadow borders value
227  * \param x
228  * \param y
229  * \param offset_x
230  * \param offset_y
231  * \param border_scores
232  * \param border_scores_other_direction
233  * \param shadow_border_idx
234  * \return
235  */
236  inline bool
237  changeScoreAccordingToShadowBorderValue (int x, int y, int offset_x, int offset_y, float* border_scores,
238  float* border_scores_other_direction, int& shadow_border_idx) const;
239 
240  /** \brief Returns a new score for the given pixel that is >= the original value, based on the neighbors values
241  * \param x the x-coordinate of the input pixel
242  * \param y the y-coordinate of the input pixel
243  * \param border_scores the input border scores
244  * \return the resulting updated border score
245  */
246  inline float
247  updatedScoreAccordingToNeighborValues (int x, int y, const float* border_scores) const;
248 
249  /** \brief For all pixels, returns a new score that is >= the original value, based on the neighbors values
250  * \param border_scores the input border scores
251  * \return a pointer to the resulting array of updated scores
252  */
253  float*
254  updatedScoresAccordingToNeighborValues (const float* border_scores) const;
255 
256  /** \brief Replace all border score values with updates according to \a updatedScoreAccordingToNeighborValues */
257  void
258  updateScoresAccordingToNeighborValues ();
259 
260  /** \brief Check if a potential border point has a corresponding shadow border
261  * \param x the x-coordinate of the input point
262  * \param y the y-coordinate of the input point
263  * \param offset_x
264  * \param offset_y
265  * \param border_scores_left
266  * \param border_scores_right
267  * \param shadow_border_idx
268  * \return a boolean value indicating whether or not the point has a corresponding shadow border
269  */
270  inline bool
271  checkPotentialBorder (int x, int y, int offset_x, int offset_y, float* border_scores_left,
272  float* border_scores_right, int& shadow_border_idx) const;
273 
274  /** \brief Check if a potential border point is a maximum regarding the border score
275  * \param x the x-coordinate of the input point
276  * \param y the y-coordinate of the input point
277  * \param offset_x
278  * \param offset_y
279  * \param border_scores
280  * \param shadow_border_idx
281  * \result a boolean value indicating whether or not the point is a maximum
282  */
283  inline bool
284  checkIfMaximum (int x, int y, int offset_x, int offset_y, float* border_scores, int shadow_border_idx) const;
285 
286  /** \brief Find the best corresponding shadow border and lower score according to the shadow borders value */
287  void
288  findAndEvaluateShadowBorders ();
289 
290  /** \brief Extract local plane information in every point (see getSurfaceStructure ()) */
291  void
292  extractLocalSurfaceStructure ();
293 
294  /** \brief Get images representing the probability that the corresponding pixels are borders in that direction
295  * (see getBorderScores... ())
296  */
297  void
298  extractBorderScoreImages ();
299 
300  /** \brief Classify the pixels in the range image according to the different classes defined below in
301  * enum BorderClass. minImpactAngle (in radians) defines how flat the angle at which a surface was seen can be.
302  */
303  void
304  classifyBorders ();
305 
306  /** \brief Calculate the 3D direction of the border just using the border traits at this position (facing away from
307  * the obstacle)
308  * \param x the x-coordinate of the input position
309  * \param y the y-coordinate of the input position
310  */
311  inline void
312  calculateBorderDirection (int x, int y);
313 
314  /** \brief Call \a calculateBorderDirection for every point and average the result over
315  * parameters_.pixel_radius_border_direction
316  */
317  void
318  calculateBorderDirections ();
319 
320  /** \brief Calculate a 3d direction from a border point by projecting the direction in the range image - returns
321  * false if direction could not be calculated
322  * \param border_description
323  * \param direction
324  * \param local_surface
325  * \return a boolean value indicating whether or not a direction could be calculated
326  */
327  inline bool
328  get3dDirection (const BorderDescription& border_description, Eigen::Vector3f& direction,
329  const LocalSurface* local_surface=NULL);
330 
331  /** \brief Calculate the main principal curvature (the largest eigenvalue and corresponding eigenvector for the
332  * normals in the area) in the given point
333  * \param x the x-coordinate of the input point
334  * \param y the y-coordinate of the input point
335  * \param radius the pixel radius that is used to find neighboring points
336  * \param magnitude the resulting magnitude
337  * \param main_direction the resulting direction
338  */
339  inline bool
340  calculateMainPrincipalCurvature (int x, int y, int radius, float& magnitude,
341  Eigen::Vector3f& main_direction) const;
342 
343  /** \brief Uses either the border or principal curvature to define a score how much the surface changes in a point
344  (1 for a border) and what the main direction of that change is */
345  void
346  calculateSurfaceChanges ();
347 
348  /** \brief Apply a blur to the surface change images */
349  void
350  blurSurfaceChanges ();
351 
352  /** \brief Implementation of abstract derived function */
353  virtual void
354  computeFeature (PointCloudOut &output);
355  };
356 } // namespace end
357 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
358 #pragma GCC diagnostic warning "-Weffc++"
359 #endif
360 
361 #include <pcl/features/impl/range_image_border_extractor.hpp> // Definitions of templated and inline functions
362 
363 #endif //#ifndef PCL_RANGE_IMAGE_BORDER_EXTRACTOR_H_
RangeImage is derived from pcl/PointCloud and provides functionalities with focus on situations where...
Definition: range_image.h:55
Extract obstacle borders from range images, meaning positions where there is a transition from foregr...
const RangeImage & getRangeImage() const
boost::shared_ptr< RangeImageBorderExtractor > Ptr
ShadowBorderIndices ** shadow_border_informations_
boost::shared_ptr< const RangeImageBorderExtractor > ConstPtr
Stores some information extracted from the neighborhood of a point.
A structure to store if a point in a range image lies on a border between an obstacle and the backgro...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Stores the indices of the shadow border corresponding to obstacle borders.
Feature represents the base feature class.
Definition: feature.h:105
Feature< PointWithRange, BorderDescription > BaseClass
std::bitset< 32 > BorderTraits
Data type to store extended information about a transition from foreground to backgroundSpecification...
Definition: point_types.h:271
ShadowBorderIndices ** getShadowBorderInformations()