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