range_image.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, 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 Willow Garage, Inc. 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_H_
#define PCL_RANGE_IMAGE_H_

#include <pcl/point_cloud.h>
#include "pcl/pcl_macros.h"
#include "pcl/point_types.h"
#include "pcl/common/common_headers.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <pcl/common/vector_average.h>
#include <typeinfo>


namespace pcl
{

  class RangeImage : public pcl::PointCloud<PointWithRange>
  {
    public:
      // =====TYPEDEFS=====
      typedef pcl::PointCloud<PointWithRange> BaseClass;
      typedef std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > VectorOfEigenVector3f;
      
      enum CoordinateFrame
      {
        CAMERA_FRAME = 0,
        LASER_FRAME  = 1
      };

      // =====CONSTRUCTOR & DESTRUCTOR=====
      RangeImage ();
      ~RangeImage ();
      
      // =====STATIC METHODS=====
      static inline float
      getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center, 
                       float radius);
    
      static inline Eigen::Vector3f
      getEigenVector3f (const PointWithRange& point);
      
      static void
        getCoordinateFrameTransformation (RangeImage::CoordinateFrame coordinate_frame,
                                         Eigen::Affine3f& transformation);
      
      template <typename PointCloudTypeWithViewpoints>
      static Eigen::Vector3f
        getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud);
      
      static void
        extractFarRanges (const sensor_msgs::PointCloud2& point_cloud_data, PointCloud<PointWithViewpoint>& far_ranges);
      
      // =====METHODS=====
      void reset ();
      
      template <typename PointCloudType>
      void
        createFromPointCloud (const PointCloudType& point_cloud, float angular_resolution, float max_angle_width,
                             float max_angle_height, const Eigen::Affine3f& sensor_pose,
                             CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
                             float min_range=0.0f, int border_size=0);
  
      template <typename PointCloudType>
      void
        createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, float angular_resolution,
                                          const Eigen::Vector3f& point_cloud_center, float point_cloud_radius,
                                          const Eigen::Affine3f& sensor_pose,
                                          CoordinateFrame coordinate_frame=CAMERA_FRAME,
                                          float noise_level=0.0f, float min_range=0.0f, int border_size=0);
      
      template <typename PointCloudTypeWithViewpoints>
      void
        createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud, float angular_resolution,
                                           float max_angle_width, float max_angle_height,
                                           CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
                                           float min_range=0.0f, int border_size=0);
      
      void
        integrateFarRanges (const PointCloud<PointWithViewpoint>& far_ranges);
      
      void
        cropImage (int border_size=0, int top=-1, int right=-1, int bottom=-1, int left=-1);
      
      float*
        getRangesArray () const;
      
      inline const Eigen::Affine3f&
        getTransformationToRangeImageSystem () const { return to_range_image_system_;}
      
      inline const Eigen::Affine3f&
        getTransformationToWorldSystem () const { return to_world_system_;}
      
      inline float
        getAngularResolution () const { return angular_resolution_;}
      
      inline void
        setAngularResolution (float angular_resolution);
      
      inline const PointWithRange&
        getPoint (int image_x, int image_y) const;

      inline PointWithRange&
        getPoint (int image_x, int image_y);
      
      inline const PointWithRange&
        getPoint (float image_x, float image_y) const;
      inline PointWithRange&
        getPoint (float image_x, float image_y);
      
      inline const PointWithRange&
        getPointNoCheck (int image_x, int image_y) const;

      inline PointWithRange&
        getPointNoCheck (int image_x, int image_y);

      inline void
      getPoint (int image_x, int image_y, Eigen::Vector3f& point) const;
      
      inline void
      getPoint (int index, Eigen::Vector3f& point) const;
      
      inline const Eigen::Map<const Eigen::Vector3f>
      getEigenVector3f (int x, int y) const;

      inline const Eigen::Map<const Eigen::Vector3f>
      getEigenVector3f (int index) const;
      
      inline const PointWithRange&
      getPoint (int index) const;

      inline const PointWithRange&
      getPointConsideringWrapAround (int image_x, int image_y) const;
      
      inline void
      calculate3DPoint (float image_x, float image_y, float range, PointWithRange& point) const;
      inline void
      calculate3DPoint (float image_x, float image_y, PointWithRange& point) const;

      virtual inline void
      calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const;
      inline void
      calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const;
      
      void
      recalculate3DPointPositions ();
      
      inline virtual void
      getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const;

      inline void
        getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y, float& range) const;
      
      inline void
        getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y) const;
      
      inline void
        getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y) const;
      
      inline void
        getImagePoint (float x, float y, float z, float& image_x, float& image_y, float& range) const;
      
      inline void
        getImagePoint (float x, float y, float z, float& image_x, float& image_y) const;
      
      inline void
        getImagePoint (float x, float y, float z, int& image_x, int& image_y) const;
      
      inline float
        checkPoint (const Eigen::Vector3f& point, PointWithRange& point_in_image) const;

      inline float
        getRangeDifference (const Eigen::Vector3f& point) const;
      
      inline void
        getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const;
      
      inline void
        getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const;
      
      inline void
        real2DToInt2D (float x, float y, int& xInt, int& yInt) const;
      
      inline bool
        isInImage (int x, int y) const;
      
      inline bool
        isValid (int x, int y) const;
      
      inline bool
        isValid (int index) const;
      
      inline bool
        isObserved (int x, int y) const;

      inline bool
        isMaxRange (int x, int y) const;
      
      inline bool
        getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size=1) const;
      
      inline bool
        getNormalForClosestNeighbors (int x, int y, int radius, const PointWithRange& point,
                                        int no_of_nearest_neighbors, Eigen::Vector3f& normal, int step_size=1) const;
      
      inline bool
        getNormalForClosestNeighbors (int x, int y, int radius, const Eigen::Vector3f& point,
                                               int no_of_nearest_neighbors, Eigen::Vector3f& normal,
                                               Eigen::Vector3f* point_on_plane=NULL, int step_size=1) const;
      
      inline bool
        getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius=2) const;
      
      inline bool
        getSurfaceInformation (int x, int y, int radius, const Eigen::Vector3f& point,
                              int no_of_closest_neighbors, int step_size,
                              float& max_closest_neighbor_distance_squared,
                              Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
                              Eigen::Vector3f* normal_all_neighbors=NULL,
                              Eigen::Vector3f* mean_all_neighbors=NULL,
                              Eigen::Vector3f* eigen_values_all_neighbors=NULL) const;
      
      // Return the squared distance to the n-th neighbors of the point at x,y
      inline float
        getSquaredDistanceOfNthNeighbor (int x, int y, int radius, int n, int step_size) const;
      
      inline float
        getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const;
      inline float
        getImpactAngle (int x1, int y1, int x2, int y2) const;
      
      inline float
        getImpactAngleBasedOnLocalNormal (int x, int y, int radius) const;
      float*
        getImpactAngleImageBasedOnLocalNormals (int radius) const;

      inline float
        getNormalBasedAcutenessValue (int x, int y, int radius) const;
      
      inline float
        getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const;
      inline float
        getAcutenessValue (int x1, int y1, int x2, int y2) const;
      
      void
        getAcutenessValueImages (int pixel_distance, float*& acuteness_value_image_x,
                                float*& acuteness_value_image_y) const;
      
      //inline float
      //  getSurfaceChange(const PointWithRange& point, const PointWithRange& neighbor1,
      //                   const PointWithRange& neighbor2) const;
      
      float
        getSurfaceChange (int x, int y, int radius) const;
      
      float*
        getSurfaceChangeImage (int radius) const;
      
      inline void
        getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const;
      
      void
        getSurfaceAngleChangeImages (int radius, float*& angle_change_image_x, float*& angle_change_image_y) const;
      
      inline float
        getCurvature (int x, int y, int radius, int step_size) const;
      
      inline const Eigen::Vector3f
        getSensorPos () const;
      
      void
        setUnseenToMaxRange ();
      
      int
        getImageOffsetX () const { return image_offset_x_;}
      int
        getImageOffsetY () const { return image_offset_y_;}
      
      virtual void
        getSubImage (int sub_image_image_offset_x, int sub_image_image_offset_y, int sub_image_width,
                     int sub_image_height, int combine_pixels, RangeImage& sub_image) const;
      
      virtual void 
        getHalfImage(RangeImage& half_image) const;
      
      void
        getMinMaxRanges (float& min_range, float& max_range) const;
      
      void
        change3dPointsToLocalCoordinateFrame ();
      
      float*
        getInterpolatedSurfaceProjection (const Eigen::Affine3f& pose, int pixel_size, float world_size) const;
      
      float*
        getInterpolatedSurfaceProjection (const Eigen::Vector3f& point, int pixel_size, float world_size) const;
      
      inline Eigen::Affine3f
        getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const;
      inline void
        getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point,
                                                  Eigen::Affine3f& transformation) const;
      inline void
        getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const;

      bool
        getNormalBasedUprightTransformation (const Eigen::Vector3f& point,
                                            float max_dist, Eigen::Affine3f& transformation) const;
      
      void
        getIntegralImage (float*& integral_image, int*& valid_points_num_image) const;
      
      void
        getBlurredImageUsingIntegralImage (int blur_radius, float* integral_image, int* valid_points_num_image,
                                          RangeImage& range_image) const;
      
      void
        getBlurredImage (int blur_radius, RangeImage& range_image) const;
      
      inline float
        getEuclideanDistanceSquared (int x1, int y1, int x2, int y2) const;
      inline float
        getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const;
      
      void
        getRangeImageWithSmoothedSurface (int radius, RangeImage& smoothed_range_image) const;
      //void getLocalNormals(int radius) const;
      
      inline void
        get1dPointAverage (int x, int y, int delta_x, int delta_y, int no_of_points,
                           PointWithRange& average_point) const;
      
      Eigen::Affine3f
        doIcp (const VectorOfEigenVector3f& points,
               const Eigen::Affine3f& initial_guess, int search_radius,
               float max_distance_start, float max_distance_end, int num_iterations) const;
      
      inline bool
        getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const;
      
      inline void
        getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const;
      
      virtual RangeImage* getNew() const { return new RangeImage; }

      
      // =====MEMBER VARIABLES=====
      // BaseClass:
      //   roslib::Header header;
      //   std::vector<PointT> points;
      //   uint32_t width;
      //   uint32_t height;
      //   bool is_dense;

      static bool debug; 
    protected:
      // =====PROTECTED MEMBER VARIABLES=====
      Eigen::Affine3f to_range_image_system_; 
      Eigen::Affine3f to_world_system_;       
      float angular_resolution_;               
      float angular_resolution_reciprocal_;    
      int image_offset_x_, image_offset_y_;    
      PointWithRange unobserved_point;         
      // =====PROTECTED METHODS=====
      template <typename PointCloudType>
      void
        doZBuffer (const PointCloudType& point_cloud, float noise_level,
                  float min_range, int& top, int& right, int& bottom, int& left);

    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };

  inline std::ostream&
    operator<< (std::ostream& os, const RangeImage& r)
  {
    os << "range image of size "<<r.width<<"x"<<r.height<<" with angular resolution "
       << RAD2DEG (r.getAngularResolution ()) << "deg/pixel and "<<r.points.size ()<<" points";
    return os;
  }

}  // namespace end


#include "pcl/range_image/range_image.hpp"  // Definitions of templated and inline functions

#endif  //#ifndef PCL_RANGE_IMAGE_H_

---

pcl
Author(s): See http://pcl.ros.org/authors for the complete list of authors.
autogenerated on Fri Jan 11 09:55:23 2013