range_image_border_extractor.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  Copyright (c) 2012-, Open Perception, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder(s) nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef PCL_RANGE_IMAGE_BORDER_EXTRACTOR_H_
#define PCL_RANGE_IMAGE_BORDER_EXTRACTOR_H_

#include <pcl/point_types.h>
#include <pcl/features/feature.h>

#if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
#pragma GCC diagnostic ignored "-Weffc++"
#endif
namespace pcl
{
  // FORWARD DECLARATIONS:
  class RangeImage;
  template <typename PointType>
  class PointCloud;

  class PCL_EXPORTS RangeImageBorderExtractor : public Feature<PointWithRange,BorderDescription>
  {
    public:
      typedef boost::shared_ptr<RangeImageBorderExtractor> Ptr;
      typedef boost::shared_ptr<const RangeImageBorderExtractor> ConstPtr;
      // =====TYPEDEFS=====
      typedef Feature<PointWithRange,BorderDescription> BaseClass;
      
      // =====PUBLIC STRUCTS=====
      struct LocalSurface
      {
        LocalSurface () : 
          normal (), neighborhood_mean (), eigen_values (), normal_no_jumps (), 
          neighborhood_mean_no_jumps (), eigen_values_no_jumps (), max_neighbor_distance_squared () {}

        Eigen::Vector3f normal;
        Eigen::Vector3f neighborhood_mean;
        Eigen::Vector3f eigen_values;
        Eigen::Vector3f normal_no_jumps;
        Eigen::Vector3f neighborhood_mean_no_jumps;
        Eigen::Vector3f eigen_values_no_jumps;
        float max_neighbor_distance_squared;
      };
      
      struct ShadowBorderIndices 
      {
        ShadowBorderIndices () : left (-1), right (-1), top (-1), bottom (-1) {}
        int left, right, top, bottom;
      };

      struct Parameters
      {
        Parameters () : max_no_of_threads(1), pixel_radius_borders (3), pixel_radius_plane_extraction (2), pixel_radius_border_direction (2), 
                       minimum_border_probability (0.8f), pixel_radius_principal_curvature (2) {}
        int max_no_of_threads;
        int pixel_radius_borders;
        int pixel_radius_plane_extraction;
        int pixel_radius_border_direction;
        float minimum_border_probability;
        int pixel_radius_principal_curvature;
      };
      
      // =====STATIC METHODS=====
      static inline float
      getObstacleBorderAngle (const BorderTraits& border_traits);
      
      // =====CONSTRUCTOR & DESTRUCTOR=====
      RangeImageBorderExtractor (const RangeImage* range_image=NULL);
      virtual ~RangeImageBorderExtractor ();
      
      // =====METHODS=====
      void
      setRangeImage (const RangeImage* range_image);
      
      void
      clearData ();
       
      float*
      getAnglesImageForBorderDirections ();

      float*
      getAnglesImageForSurfaceChangeDirections ();
      
      void
      compute (PointCloudOut& output);
      
      // =====GETTER=====
      Parameters&
      getParameters () { return (parameters_); }

      bool
      hasRangeImage () const { return range_image_ != NULL; }

      const RangeImage&
      getRangeImage () const { return *range_image_; }

      float*
      getBorderScoresLeft ()   { extractBorderScoreImages (); return border_scores_left_; }

      float*
      getBorderScoresRight ()  { extractBorderScoreImages (); return border_scores_right_; }

      float*
      getBorderScoresTop ()    { extractBorderScoreImages (); return border_scores_top_; }

      float*
      getBorderScoresBottom () { extractBorderScoreImages (); return border_scores_bottom_; }

      LocalSurface**
      getSurfaceStructure () { extractLocalSurfaceStructure (); return surface_structure_; }

      PointCloudOut&
      getBorderDescriptions () { classifyBorders (); return *border_descriptions_; }

      ShadowBorderIndices**
      getShadowBorderInformations () { findAndEvaluateShadowBorders (); return shadow_border_informations_; }

      Eigen::Vector3f**
      getBorderDirections () { calculateBorderDirections (); return border_directions_; }

      float*
      getSurfaceChangeScores () { calculateSurfaceChanges (); return surface_change_scores_; }

      Eigen::Vector3f*
      getSurfaceChangeDirections () { calculateSurfaceChanges (); return surface_change_directions_; }
      
      
    protected:
      // =====PROTECTED MEMBER VARIABLES=====
      Parameters parameters_;
      const RangeImage* range_image_;
      int range_image_size_during_extraction_;
      float* border_scores_left_, * border_scores_right_, * border_scores_top_, * border_scores_bottom_;
      LocalSurface** surface_structure_;
      PointCloudOut* border_descriptions_;
      ShadowBorderIndices** shadow_border_informations_;
      Eigen::Vector3f** border_directions_;
      
      float* surface_change_scores_;
      Eigen::Vector3f* surface_change_directions_;
      
      
      // =====PROTECTED METHODS=====
      inline float
      getNeighborDistanceChangeScore (const LocalSurface& local_surface, int x, int y, 
                                      int offset_x, int offset_y, int pixel_radius=1) const;
      
      inline float
      getNormalBasedBorderScore (const LocalSurface& local_surface, int x, int y, 
                                 int offset_x, int offset_y) const;
      
      inline bool
      changeScoreAccordingToShadowBorderValue (int x, int y, int offset_x, int offset_y, float* border_scores,
                                               float* border_scores_other_direction, int& shadow_border_idx) const;
      
      inline float
      updatedScoreAccordingToNeighborValues (int x, int y, const float* border_scores) const;

      float*
      updatedScoresAccordingToNeighborValues (const float* border_scores) const;

      void
      updateScoresAccordingToNeighborValues ();
      
      inline bool
      checkPotentialBorder (int x, int y, int offset_x, int offset_y, float* border_scores_left,
                            float* border_scores_right, int& shadow_border_idx) const;

      inline bool
      checkIfMaximum (int x, int y, int offset_x, int offset_y, float* border_scores, int shadow_border_idx) const;
      
      void
      findAndEvaluateShadowBorders ();
      
      void
      extractLocalSurfaceStructure ();
      
      void
      extractBorderScoreImages ();
      
      void
      classifyBorders ();
      
      inline void
      calculateBorderDirection (int x, int y);
      
      void
      calculateBorderDirections ();
      
      inline bool
      get3dDirection (const BorderDescription& border_description, Eigen::Vector3f& direction,
                      const LocalSurface* local_surface=NULL);
      
      inline bool
      calculateMainPrincipalCurvature (int x, int y, int radius, float& magnitude,
                                       Eigen::Vector3f& main_direction) const;
      
      void
      calculateSurfaceChanges ();

      void
      blurSurfaceChanges ();
      
      virtual void
      computeFeature (PointCloudOut &output);
  };
}  // namespace end
#if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
#pragma GCC diagnostic warning "-Weffc++"
#endif

#include <pcl/features/impl/range_image_border_extractor.hpp>  // Definitions of templated and inline functions

#endif  //#ifndef PCL_RANGE_IMAGE_BORDER_EXTRACTOR_H_