Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/features/include/pcl/features/range_image_border_extractor.h
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_