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