Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 */ 00037 00038 #ifndef PCL_RANGE_IMAGE_BORDER_EXTRACTOR_H_ 00039 #define PCL_RANGE_IMAGE_BORDER_EXTRACTOR_H_ 00040 00041 #include <pcl/point_types.h> 00042 #include <pcl/features/feature.h> 00043 00044 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 00045 #pragma GCC diagnostic ignored "-Weffc++" 00046 #endif 00047 namespace pcl 00048 { 00049 // FORWARD DECLARATIONS: 00050 class RangeImage; 00051 template <typename PointType> 00052 class PointCloud; 00053 00054 /** \brief @b Extract obstacle borders from range images, meaning positions where there is a transition from foreground 00055 * to background. 00056 * \author Bastian Steder 00057 * \ingroup features 00058 */ 00059 class PCL_EXPORTS RangeImageBorderExtractor : public Feature<PointWithRange,BorderDescription> 00060 { 00061 public: 00062 typedef boost::shared_ptr<RangeImageBorderExtractor> Ptr; 00063 typedef boost::shared_ptr<const RangeImageBorderExtractor> ConstPtr; 00064 // =====TYPEDEFS===== 00065 typedef Feature<PointWithRange,BorderDescription> BaseClass; 00066 00067 // =====PUBLIC STRUCTS===== 00068 //! Stores some information extracted from the neighborhood of a point 00069 struct LocalSurface 00070 { 00071 LocalSurface () : 00072 normal (), neighborhood_mean (), eigen_values (), normal_no_jumps (), 00073 neighborhood_mean_no_jumps (), eigen_values_no_jumps (), max_neighbor_distance_squared () {} 00074 00075 Eigen::Vector3f normal; 00076 Eigen::Vector3f neighborhood_mean; 00077 Eigen::Vector3f eigen_values; 00078 Eigen::Vector3f normal_no_jumps; 00079 Eigen::Vector3f neighborhood_mean_no_jumps; 00080 Eigen::Vector3f eigen_values_no_jumps; 00081 float max_neighbor_distance_squared; 00082 }; 00083 00084 //! Stores the indices of the shadow border corresponding to obstacle borders 00085 struct ShadowBorderIndices 00086 { 00087 ShadowBorderIndices () : left (-1), right (-1), top (-1), bottom (-1) {} 00088 int left, right, top, bottom; 00089 }; 00090 00091 //! Parameters used in this class 00092 struct Parameters 00093 { 00094 Parameters () : max_no_of_threads(1), pixel_radius_borders (3), pixel_radius_plane_extraction (2), pixel_radius_border_direction (2), 00095 minimum_border_probability (0.8f), pixel_radius_principal_curvature (2) {} 00096 int max_no_of_threads; 00097 int pixel_radius_borders; 00098 int pixel_radius_plane_extraction; 00099 int pixel_radius_border_direction; 00100 float minimum_border_probability; 00101 int pixel_radius_principal_curvature; 00102 }; 00103 00104 // =====STATIC METHODS===== 00105 /** \brief Take the information from BorderTraits to calculate the local direction of the border 00106 * \param border_traits contains the information needed to calculate the border angle 00107 */ 00108 static inline float 00109 getObstacleBorderAngle (const BorderTraits& border_traits); 00110 00111 // =====CONSTRUCTOR & DESTRUCTOR===== 00112 /** Constructor */ 00113 RangeImageBorderExtractor (const RangeImage* range_image=NULL); 00114 /** Destructor */ 00115 virtual ~RangeImageBorderExtractor (); 00116 00117 // =====METHODS===== 00118 /** \brief Provide a pointer to the range image 00119 * \param range_image a pointer to the range_image 00120 */ 00121 void 00122 setRangeImage (const RangeImage* range_image); 00123 00124 /** \brief Erase all data calculated for the current range image */ 00125 void 00126 clearData (); 00127 00128 /** \brief Get the 2D directions in the range image from the border directions - probably mainly useful for 00129 * visualization 00130 */ 00131 float* 00132 getAnglesImageForBorderDirections (); 00133 00134 /** \brief Get the 2D directions in the range image from the surface change directions - probably mainly useful for 00135 * visualization 00136 */ 00137 float* 00138 getAnglesImageForSurfaceChangeDirections (); 00139 00140 /** Overwrite the compute function of the base class */ 00141 void 00142 compute (PointCloudOut& output); 00143 00144 // =====GETTER===== 00145 Parameters& 00146 getParameters () { return (parameters_); } 00147 00148 bool 00149 hasRangeImage () const { return range_image_ != NULL; } 00150 00151 const RangeImage& 00152 getRangeImage () const { return *range_image_; } 00153 00154 float* 00155 getBorderScoresLeft () { extractBorderScoreImages (); return border_scores_left_; } 00156 00157 float* 00158 getBorderScoresRight () { extractBorderScoreImages (); return border_scores_right_; } 00159 00160 float* 00161 getBorderScoresTop () { extractBorderScoreImages (); return border_scores_top_; } 00162 00163 float* 00164 getBorderScoresBottom () { extractBorderScoreImages (); return border_scores_bottom_; } 00165 00166 LocalSurface** 00167 getSurfaceStructure () { extractLocalSurfaceStructure (); return surface_structure_; } 00168 00169 PointCloudOut& 00170 getBorderDescriptions () { classifyBorders (); return *border_descriptions_; } 00171 00172 ShadowBorderIndices** 00173 getShadowBorderInformations () { findAndEvaluateShadowBorders (); return shadow_border_informations_; } 00174 00175 Eigen::Vector3f** 00176 getBorderDirections () { calculateBorderDirections (); return border_directions_; } 00177 00178 float* 00179 getSurfaceChangeScores () { calculateSurfaceChanges (); return surface_change_scores_; } 00180 00181 Eigen::Vector3f* 00182 getSurfaceChangeDirections () { calculateSurfaceChanges (); return surface_change_directions_; } 00183 00184 00185 protected: 00186 // =====PROTECTED MEMBER VARIABLES===== 00187 Parameters parameters_; 00188 const RangeImage* range_image_; 00189 int range_image_size_during_extraction_; 00190 float* border_scores_left_, * border_scores_right_, * border_scores_top_, * border_scores_bottom_; 00191 LocalSurface** surface_structure_; 00192 PointCloudOut* border_descriptions_; 00193 ShadowBorderIndices** shadow_border_informations_; 00194 Eigen::Vector3f** border_directions_; 00195 00196 float* surface_change_scores_; 00197 Eigen::Vector3f* surface_change_directions_; 00198 00199 00200 // =====PROTECTED METHODS===== 00201 /** \brief Calculate a border score based on how distant the neighbor is, compared to the closest neighbors 00202 * /param local_surface 00203 * /param x 00204 * /param y 00205 * /param offset_x 00206 * /param offset_y 00207 * /param pixel_radius (defaults to 1) 00208 * /return the resulting border score 00209 */ 00210 inline float 00211 getNeighborDistanceChangeScore (const LocalSurface& local_surface, int x, int y, 00212 int offset_x, int offset_y, int pixel_radius=1) const; 00213 00214 /** \brief Calculate a border score based on how much the neighbor is away from the local surface plane 00215 * \param local_surface 00216 * \param x 00217 * \param y 00218 * \param offset_x 00219 * \param offset_y 00220 * \return the resulting border score 00221 */ 00222 inline float 00223 getNormalBasedBorderScore (const LocalSurface& local_surface, int x, int y, 00224 int offset_x, int offset_y) const; 00225 00226 /** \brief Find the best corresponding shadow border and lower score according to the shadow borders value 00227 * \param x 00228 * \param y 00229 * \param offset_x 00230 * \param offset_y 00231 * \param border_scores 00232 * \param border_scores_other_direction 00233 * \param shadow_border_idx 00234 * \return 00235 */ 00236 inline bool 00237 changeScoreAccordingToShadowBorderValue (int x, int y, int offset_x, int offset_y, float* border_scores, 00238 float* border_scores_other_direction, int& shadow_border_idx) const; 00239 00240 /** \brief Returns a new score for the given pixel that is >= the original value, based on the neighbors values 00241 * \param x the x-coordinate of the input pixel 00242 * \param y the y-coordinate of the input pixel 00243 * \param border_scores the input border scores 00244 * \return the resulting updated border score 00245 */ 00246 inline float 00247 updatedScoreAccordingToNeighborValues (int x, int y, const float* border_scores) const; 00248 00249 /** \brief For all pixels, returns a new score that is >= the original value, based on the neighbors values 00250 * \param border_scores the input border scores 00251 * \return a pointer to the resulting array of updated scores 00252 */ 00253 float* 00254 updatedScoresAccordingToNeighborValues (const float* border_scores) const; 00255 00256 /** \brief Replace all border score values with updates according to \a updatedScoreAccordingToNeighborValues */ 00257 void 00258 updateScoresAccordingToNeighborValues (); 00259 00260 /** \brief Check if a potential border point has a corresponding shadow border 00261 * \param x the x-coordinate of the input point 00262 * \param y the y-coordinate of the input point 00263 * \param offset_x 00264 * \param offset_y 00265 * \param border_scores_left 00266 * \param border_scores_right 00267 * \param shadow_border_idx 00268 * \return a boolean value indicating whether or not the point has a corresponding shadow border 00269 */ 00270 inline bool 00271 checkPotentialBorder (int x, int y, int offset_x, int offset_y, float* border_scores_left, 00272 float* border_scores_right, int& shadow_border_idx) const; 00273 00274 /** \brief Check if a potential border point is a maximum regarding the border score 00275 * \param x the x-coordinate of the input point 00276 * \param y the y-coordinate of the input point 00277 * \param offset_x 00278 * \param offset_y 00279 * \param border_scores 00280 * \param shadow_border_idx 00281 * \result a boolean value indicating whether or not the point is a maximum 00282 */ 00283 inline bool 00284 checkIfMaximum (int x, int y, int offset_x, int offset_y, float* border_scores, int shadow_border_idx) const; 00285 00286 /** \brief Find the best corresponding shadow border and lower score according to the shadow borders value */ 00287 void 00288 findAndEvaluateShadowBorders (); 00289 00290 /** \brief Extract local plane information in every point (see getSurfaceStructure ()) */ 00291 void 00292 extractLocalSurfaceStructure (); 00293 00294 /** \brief Get images representing the probability that the corresponding pixels are borders in that direction 00295 * (see getBorderScores... ()) 00296 */ 00297 void 00298 extractBorderScoreImages (); 00299 00300 /** \brief Classify the pixels in the range image according to the different classes defined below in 00301 * enum BorderClass. minImpactAngle (in radians) defines how flat the angle at which a surface was seen can be. 00302 */ 00303 void 00304 classifyBorders (); 00305 00306 /** \brief Calculate the 3D direction of the border just using the border traits at this position (facing away from 00307 * the obstacle) 00308 * \param x the x-coordinate of the input position 00309 * \param y the y-coordinate of the input position 00310 */ 00311 inline void 00312 calculateBorderDirection (int x, int y); 00313 00314 /** \brief Call \a calculateBorderDirection for every point and average the result over 00315 * parameters_.pixel_radius_border_direction 00316 */ 00317 void 00318 calculateBorderDirections (); 00319 00320 /** \brief Calculate a 3d direction from a border point by projecting the direction in the range image - returns 00321 * false if direction could not be calculated 00322 * \param border_description 00323 * \param direction 00324 * \param local_surface 00325 * \return a boolean value indicating whether or not a direction could be calculated 00326 */ 00327 inline bool 00328 get3dDirection (const BorderDescription& border_description, Eigen::Vector3f& direction, 00329 const LocalSurface* local_surface=NULL); 00330 00331 /** \brief Calculate the main principal curvature (the largest eigenvalue and corresponding eigenvector for the 00332 * normals in the area) in the given point 00333 * \param x the x-coordinate of the input point 00334 * \param y the y-coordinate of the input point 00335 * \param radius the pixel radius that is used to find neighboring points 00336 * \param magnitude the resulting magnitude 00337 * \param main_direction the resulting direction 00338 */ 00339 inline bool 00340 calculateMainPrincipalCurvature (int x, int y, int radius, float& magnitude, 00341 Eigen::Vector3f& main_direction) const; 00342 00343 /** \brief Uses either the border or principal curvature to define a score how much the surface changes in a point 00344 (1 for a border) and what the main direction of that change is */ 00345 void 00346 calculateSurfaceChanges (); 00347 00348 /** \brief Apply a blur to the surface change images */ 00349 void 00350 blurSurfaceChanges (); 00351 00352 /** \brief Implementation of abstract derived function */ 00353 virtual void 00354 computeFeature (PointCloudOut &output); 00355 }; 00356 } // namespace end 00357 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 00358 #pragma GCC diagnostic warning "-Weffc++" 00359 #endif 00360 00361 #include <pcl/features/impl/range_image_border_extractor.hpp> // Definitions of templated and inline functions 00362 00363 #endif //#ifndef PCL_RANGE_IMAGE_BORDER_EXTRACTOR_H_