pcl: range_image.h Source File

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, 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_H_
00039 #define PCL_RANGE_IMAGE_H_
00040 
00041 #include <pcl/point_cloud.h>
00042 #include <pcl/pcl_macros.h>
00043 #include <pcl/point_types.h>
00044 #include <pcl/common/common_headers.h>
00045 #include <pcl/common/vector_average.h>
00046 #include <typeinfo>
00047 
00048 namespace pcl
00049 {
00055   class RangeImage : public pcl::PointCloud<PointWithRange>
00056   {
00057     public:
00058       // =====TYPEDEFS=====
00059       typedef pcl::PointCloud<PointWithRange> BaseClass;
00060       typedef std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > VectorOfEigenVector3f;
00061       typedef boost::shared_ptr<RangeImage> Ptr;
00062       typedef boost::shared_ptr<const RangeImage> ConstPtr;
00063       
00064       enum CoordinateFrame
00065       {
00066         CAMERA_FRAME = 0,
00067         LASER_FRAME  = 1
00068       };
00069 
00070       
00071       // =====CONSTRUCTOR & DESTRUCTOR=====
00073       PCL_EXPORTS RangeImage ();
00075       PCL_EXPORTS virtual ~RangeImage ();
00076       
00077       // =====STATIC VARIABLES=====
00079       static int max_no_of_threads;
00080       
00081       // =====STATIC METHODS=====
00088       static inline float
00089       getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center, 
00090                        float radius);
00091       
00096       static inline Eigen::Vector3f
00097       getEigenVector3f (const PointWithRange& point);
00098       
00103       PCL_EXPORTS static void
00104       getCoordinateFrameTransformation (RangeImage::CoordinateFrame coordinate_frame,
00105                                         Eigen::Affine3f& transformation);
00106       
00112       template <typename PointCloudTypeWithViewpoints> static Eigen::Vector3f
00113       getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud);
00114       
00119       PCL_EXPORTS static void
00120       extractFarRanges (const pcl::PCLPointCloud2& point_cloud_data, PointCloud<PointWithViewpoint>& far_ranges);
00121       
00122       // =====METHODS=====
00124       inline Ptr 
00125       makeShared () { return Ptr (new RangeImage (*this)); } 
00126       
00128       PCL_EXPORTS void 
00129       reset ();
00130       
00145       template <typename PointCloudType> void
00146       createFromPointCloud (const PointCloudType& point_cloud, float angular_resolution=pcl::deg2rad (0.5f),
00147           float max_angle_width=pcl::deg2rad (360.0f), float max_angle_height=pcl::deg2rad (180.0f),
00148           const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
00149           CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
00150           float min_range=0.0f, int border_size=0);
00151       
00169       template <typename PointCloudType> void
00170       createFromPointCloud (const PointCloudType& point_cloud,
00171           float angular_resolution_x=pcl::deg2rad (0.5f), float angular_resolution_y=pcl::deg2rad (0.5f),
00172           float max_angle_width=pcl::deg2rad (360.0f), float max_angle_height=pcl::deg2rad (180.0f),
00173           const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
00174           CoordinateFrame coordinate_frame=CAMERA_FRAME,
00175           float noise_level=0.0f, float min_range=0.0f, int border_size=0);
00176       
00192       template <typename PointCloudType> void
00193       createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, float angular_resolution,
00194                                          const Eigen::Vector3f& point_cloud_center, float point_cloud_radius,
00195                                          const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
00196                                          CoordinateFrame coordinate_frame=CAMERA_FRAME,
00197                                          float noise_level=0.0f, float min_range=0.0f, int border_size=0);
00198       
00218       template <typename PointCloudType> void
00219       createFromPointCloudWithKnownSize (const PointCloudType& point_cloud,
00220                                          float angular_resolution_x, float angular_resolution_y,
00221                                          const Eigen::Vector3f& point_cloud_center, float point_cloud_radius,
00222                                          const Eigen::Affine3f& sensor_pose = Eigen::Affine3f::Identity (),
00223                                          CoordinateFrame coordinate_frame=CAMERA_FRAME,
00224                                          float noise_level=0.0f, float min_range=0.0f, int border_size=0);
00225       
00241       template <typename PointCloudTypeWithViewpoints> void
00242       createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud, float angular_resolution,
00243                                           float max_angle_width, float max_angle_height,
00244                                           CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
00245                                           float min_range=0.0f, int border_size=0);
00246       
00265       template <typename PointCloudTypeWithViewpoints> void
00266       createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud,
00267                                           float angular_resolution_x, float angular_resolution_y,
00268                                           float max_angle_width, float max_angle_height,
00269                                           CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
00270                                           float min_range=0.0f, int border_size=0);
00271       
00279       void
00280       createEmpty (float angular_resolution, const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (),
00281                    RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad (360.0f),
00282                    float angle_height=pcl::deg2rad (180.0f));     
00283       
00294       void
00295       createEmpty (float angular_resolution_x, float angular_resolution_y,
00296                    const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity (),
00297                    RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad (360.0f),
00298                    float angle_height=pcl::deg2rad (180.0f));
00299       
00312       template <typename PointCloudType> void
00313       doZBuffer (const PointCloudType& point_cloud, float noise_level,
00314                  float min_range, int& top, int& right, int& bottom, int& left);
00315       
00317       template <typename PointCloudType> void
00318       integrateFarRanges (const PointCloudType& far_ranges);
00319       
00327       PCL_EXPORTS void
00328       cropImage (int border_size=0, int top=-1, int right=-1, int bottom=-1, int left=-1);
00329       
00334       PCL_EXPORTS float*
00335       getRangesArray () const;
00336       
00339       inline const Eigen::Affine3f&
00340       getTransformationToRangeImageSystem () const { return (to_range_image_system_); }
00341       
00344       inline void 
00345       setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system);
00346       
00349       inline const Eigen::Affine3f&
00350       getTransformationToWorldSystem () const { return to_world_system_;}
00351       
00354       inline float
00355       getAngularResolution () const { return angular_resolution_x_;}
00356       
00358       inline float
00359       getAngularResolutionX () const { return angular_resolution_x_;}
00360       
00362       inline float
00363       getAngularResolutionY () const { return angular_resolution_y_;}
00364       
00366       inline void
00367       getAngularResolution (float& angular_resolution_x, float& angular_resolution_y) const;
00368       
00372       inline void
00373       setAngularResolution (float angular_resolution);
00374       
00379       inline void
00380       setAngularResolution (float angular_resolution_x, float angular_resolution_y);
00381 
00382       
00388       inline const PointWithRange&
00389       getPoint (int image_x, int image_y) const;
00390 
00392       inline PointWithRange&
00393       getPoint (int image_x, int image_y);
00394       
00396       inline const PointWithRange&
00397       getPoint (float image_x, float image_y) const;
00398       
00400       inline PointWithRange&
00401       getPoint (float image_x, float image_y);
00402       
00409       inline const PointWithRange&
00410       getPointNoCheck (int image_x, int image_y) const;
00411 
00413       inline PointWithRange&
00414       getPointNoCheck (int image_x, int image_y);
00415 
00417       inline void
00418       getPoint (int image_x, int image_y, Eigen::Vector3f& point) const;
00419       
00421       inline void
00422       getPoint (int index, Eigen::Vector3f& point) const;
00423       
00425       inline const Eigen::Map<const Eigen::Vector3f>
00426       getEigenVector3f (int x, int y) const;
00427 
00429       inline const Eigen::Map<const Eigen::Vector3f>
00430       getEigenVector3f (int index) const;
00431       
00433       inline const PointWithRange&
00434       getPoint (int index) const;
00435 
00437       inline void
00438       calculate3DPoint (float image_x, float image_y, float range, PointWithRange& point) const;
00440       inline void
00441       calculate3DPoint (float image_x, float image_y, PointWithRange& point) const;
00442 
00444       virtual inline void
00445       calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const;
00447       inline void
00448       calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const;
00449       
00451       PCL_EXPORTS void
00452       recalculate3DPointPositions ();
00453       
00455       inline virtual void
00456       getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const;
00457 
00459       inline void
00460       getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y, float& range) const;
00461       
00463       inline void
00464       getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y) const;
00465       
00467       inline void
00468       getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y) const;
00469       
00471       inline void
00472       getImagePoint (float x, float y, float z, float& image_x, float& image_y, float& range) const;
00473       
00475       inline void
00476       getImagePoint (float x, float y, float z, float& image_x, float& image_y) const;
00477       
00479       inline void
00480       getImagePoint (float x, float y, float z, int& image_x, int& image_y) const;
00481       
00484       inline float
00485       checkPoint (const Eigen::Vector3f& point, PointWithRange& point_in_image) const;
00486 
00490       inline float
00491       getRangeDifference (const Eigen::Vector3f& point) const;
00492       
00494       inline void
00495       getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const;
00496       
00498       inline void
00499       getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const;
00500       
00502       inline void
00503       real2DToInt2D (float x, float y, int& xInt, int& yInt) const;
00504       
00506       inline bool
00507       isInImage (int x, int y) const;
00508       
00510       inline bool
00511       isValid (int x, int y) const;
00512       
00514       inline bool
00515       isValid (int index) const;
00516       
00518       inline bool
00519       isObserved (int x, int y) const;
00520 
00522       inline bool
00523       isMaxRange (int x, int y) const;
00524       
00528       inline bool
00529       getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size=1) const;
00530       
00532       inline bool
00533       getNormalForClosestNeighbors (int x, int y, int radius, const PointWithRange& point,
00534                                     int no_of_nearest_neighbors, Eigen::Vector3f& normal, int step_size=1) const;
00535       
00537       inline bool
00538       getNormalForClosestNeighbors (int x, int y, int radius, const Eigen::Vector3f& point,
00539                                     int no_of_nearest_neighbors, Eigen::Vector3f& normal,
00540                                     Eigen::Vector3f* point_on_plane=NULL, int step_size=1) const;
00541       
00543       inline bool
00544       getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius=2) const;
00545       
00548       inline bool
00549       getSurfaceInformation (int x, int y, int radius, const Eigen::Vector3f& point,
00550                              int no_of_closest_neighbors, int step_size,
00551                              float& max_closest_neighbor_distance_squared,
00552                              Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
00553                              Eigen::Vector3f* normal_all_neighbors=NULL,
00554                              Eigen::Vector3f* mean_all_neighbors=NULL,
00555                              Eigen::Vector3f* eigen_values_all_neighbors=NULL) const;
00556       
00557       // Return the squared distance to the n-th neighbors of the point at x,y
00558       inline float
00559       getSquaredDistanceOfNthNeighbor (int x, int y, int radius, int n, int step_size) const;
00560       
00563       inline float
00564       getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const;
00566       inline float
00567       getImpactAngle (int x1, int y1, int x2, int y2) const;
00568       
00571       inline float
00572       getImpactAngleBasedOnLocalNormal (int x, int y, int radius) const;
00574       PCL_EXPORTS float*
00575       getImpactAngleImageBasedOnLocalNormals (int radius) const;
00576 
00580       inline float
00581       getNormalBasedAcutenessValue (int x, int y, int radius) const;
00582       
00585       inline float
00586       getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const;
00588       inline float
00589       getAcutenessValue (int x1, int y1, int x2, int y2) const;
00590       
00592       PCL_EXPORTS void
00593       getAcutenessValueImages (int pixel_distance, float*& acuteness_value_image_x,
00594                                float*& acuteness_value_image_y) const;
00595       
00598       //inline float
00599       //  getSurfaceChange (const PointWithRange& point, const PointWithRange& neighbor1,
00600       //                   const PointWithRange& neighbor2) const;
00601       
00603       PCL_EXPORTS float
00604       getSurfaceChange (int x, int y, int radius) const;
00605       
00607       PCL_EXPORTS float*
00608       getSurfaceChangeImage (int radius) const;
00609       
00612       inline void
00613       getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const;
00614       
00616       PCL_EXPORTS void
00617       getSurfaceAngleChangeImages (int radius, float*& angle_change_image_x, float*& angle_change_image_y) const;
00618       
00620       inline float
00621       getCurvature (int x, int y, int radius, int step_size) const;
00622       
00624       inline const Eigen::Vector3f
00625       getSensorPos () const;
00626       
00628       PCL_EXPORTS void
00629       setUnseenToMaxRange ();
00630       
00632       inline int
00633       getImageOffsetX () const { return image_offset_x_;}
00635       inline int
00636       getImageOffsetY () const { return image_offset_y_;}
00637       
00639       inline void
00640       setImageOffsets (int offset_x, int offset_y) { image_offset_x_=offset_x; image_offset_y_=offset_y;}
00641  
00642 
00643       
00657       virtual void
00658       getSubImage (int sub_image_image_offset_x, int sub_image_image_offset_y, int sub_image_width,
00659                    int sub_image_height, int combine_pixels, RangeImage& sub_image) const;
00660       
00662       virtual void 
00663       getHalfImage (RangeImage& half_image) const;
00664       
00666       PCL_EXPORTS void
00667       getMinMaxRanges (float& min_range, float& max_range) const;
00668       
00670       PCL_EXPORTS void
00671       change3dPointsToLocalCoordinateFrame ();
00672       
00677       PCL_EXPORTS float*
00678       getInterpolatedSurfaceProjection (const Eigen::Affine3f& pose, int pixel_size, float world_size) const;
00679       
00681       PCL_EXPORTS float*
00682       getInterpolatedSurfaceProjection (const Eigen::Vector3f& point, int pixel_size, float world_size) const;
00683       
00685       inline Eigen::Affine3f
00686       getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const;
00688       inline void
00689       getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point,
00690                                                 Eigen::Affine3f& transformation) const;
00692       inline void
00693       getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const;
00694 
00696       PCL_EXPORTS bool
00697       getNormalBasedUprightTransformation (const Eigen::Vector3f& point,
00698                                            float max_dist, Eigen::Affine3f& transformation) const;
00699       
00702       PCL_EXPORTS void
00703       getIntegralImage (float*& integral_image, int*& valid_points_num_image) const;
00704       
00706       PCL_EXPORTS void     // Template necessary so that this function also works in derived classes
00707       getBlurredImageUsingIntegralImage (int blur_radius, float* integral_image, int* valid_points_num_image,
00708                                          RangeImage& range_image) const;
00709       
00711       PCL_EXPORTS virtual void     // Template necessary so that this function also works in derived classes
00712       getBlurredImage (int blur_radius, RangeImage& range_image) const;
00713       
00716       inline float
00717       getEuclideanDistanceSquared (int x1, int y1, int x2, int y2) const;
00719       inline float
00720       getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const;
00721       
00723       PCL_EXPORTS void
00724       getRangeImageWithSmoothedSurface (int radius, RangeImage& smoothed_range_image) const;
00725       //void getLocalNormals (int radius) const;
00726       
00731       inline void
00732       get1dPointAverage (int x, int y, int delta_x, int delta_y, int no_of_points,
00733                          PointWithRange& average_point) const;
00734       
00737       PCL_EXPORTS float
00738       getOverlap (const RangeImage& other_range_image, const Eigen::Affine3f& relative_transformation,
00739                   int search_radius, float max_distance, int pixel_step=1) const;
00740       
00742       inline bool
00743       getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const;
00744       
00746       inline void
00747       getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const;
00748       
00751       PCL_EXPORTS virtual RangeImage* 
00752       getNew () const { return new RangeImage; }
00753 
00755       PCL_EXPORTS virtual void
00756       copyTo (RangeImage& other) const;
00757 
00758       
00759       // =====MEMBER VARIABLES=====
00760       // BaseClass:
00761       //   roslib::Header header;
00762       //   std::vector<PointT> points;
00763       //   uint32_t width;
00764       //   uint32_t height;
00765       //   bool is_dense;
00766 
00767       static bool debug; 
00769     protected:
00770       // =====PROTECTED MEMBER VARIABLES=====
00771       Eigen::Affine3f to_range_image_system_;  
00772       Eigen::Affine3f to_world_system_;        
00773       float angular_resolution_x_;             
00774       float angular_resolution_y_;             
00775       float angular_resolution_x_reciprocal_;  
00777       float angular_resolution_y_reciprocal_;  
00779       int image_offset_x_, image_offset_y_;    
00781       PointWithRange unobserved_point;         
00784       // =====PROTECTED METHODS=====
00785 
00786 
00787       // =====STATIC PROTECTED=====
00788       static const int lookup_table_size;
00789       static std::vector<float> asin_lookup_table;
00790       static std::vector<float> atan_lookup_table;
00791       static std::vector<float> cos_lookup_table;
00793       static void
00794       createLookupTables ();
00795 
00797       static inline float
00798       asinLookUp (float value);
00799       
00801       static inline float
00802       atan2LookUp (float y, float x);
00803      
00805       static inline float
00806       cosLookUp (float value);
00807 
00808 
00809     public:
00810       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00811   };
00812 
00816   inline std::ostream&
00817   operator<< (std::ostream& os, const RangeImage& r)
00818   {
00819     os << "header: " << std::endl;
00820     os << r.header;
00821     os << "points[]: " << r.points.size () << std::endl;
00822     os << "width: " << r.width << std::endl;
00823     os << "height: " << r.height << std::endl;
00824     os << "sensor_origin_: "
00825        << r.sensor_origin_[0] << ' '
00826        << r.sensor_origin_[1] << ' '
00827        << r.sensor_origin_[2] << std::endl;
00828     os << "sensor_orientation_: "
00829        << r.sensor_orientation_.x () << ' '
00830        << r.sensor_orientation_.y () << ' '
00831        << r.sensor_orientation_.z () << ' '
00832        << r.sensor_orientation_.w () << std::endl;
00833     os << "is_dense: " << r.is_dense << std::endl;
00834     os << "angular resolution: "
00835        << RAD2DEG (r.getAngularResolutionX ()) << "deg/pixel in x and "
00836        << RAD2DEG (r.getAngularResolutionY ()) << "deg/pixel in y.\n" << std::endl;
00837     return (os);
00838   }
00839 
00840 }  // namespace end
00841 
00842 
00843 #include <pcl/range_image/impl/range_image.hpp>  // Definitions of templated and inline functions
00844 
00845 #endif  //#ifndef PCL_RANGE_IMAGE_H_