Point Cloud Library (PCL)  1.3.1
range_image.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #ifndef PCL_RANGE_IMAGE_H_
00036 #define PCL_RANGE_IMAGE_H_
00037 
00038 #include <pcl/point_cloud.h>
00039 #include "pcl/pcl_macros.h"
00040 #include "pcl/point_types.h"
00041 #include "pcl/common/common_headers.h"
00042 #include <Eigen/Core>
00043 #include <Eigen/Geometry>
00044 #include <pcl/common/vector_average.h>
00045 #include <typeinfo>
00046 
00047 
00048 namespace pcl
00049 {
00050 
00056   class RangeImage : public pcl::PointCloud<PointWithRange>
00057   {
00058     public:
00059       // =====TYPEDEFS=====
00060       typedef pcl::PointCloud<PointWithRange> BaseClass;
00061       typedef std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > VectorOfEigenVector3f;
00062       typedef boost::shared_ptr<RangeImage> Ptr;
00063       typedef boost::shared_ptr<const RangeImage> ConstPtr;
00064       
00065       enum CoordinateFrame
00066       {
00067         CAMERA_FRAME = 0,
00068         LASER_FRAME  = 1
00069       };
00070 
00071       
00072       // =====CONSTRUCTOR & DESTRUCTOR=====
00074       PCL_EXPORTS RangeImage ();
00076       PCL_EXPORTS ~RangeImage ();
00077       
00078       // =====STATIC VARIABLES=====
00080       static int max_no_of_threads;
00081       
00082       // =====STATIC METHODS=====
00089       static inline float
00090       getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center, 
00091                        float radius);
00092       
00097       static inline Eigen::Vector3f
00098       getEigenVector3f (const PointWithRange& point);
00099       
00104       PCL_EXPORTS static void
00105       getCoordinateFrameTransformation (RangeImage::CoordinateFrame coordinate_frame,
00106                                         Eigen::Affine3f& transformation);
00107       
00113       template <typename PointCloudTypeWithViewpoints> static Eigen::Vector3f
00114       getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud);
00115       
00120       PCL_EXPORTS static void
00121       extractFarRanges (const sensor_msgs::PointCloud2& point_cloud_data, PointCloud<PointWithViewpoint>& far_ranges);
00122       
00123       // =====METHODS=====
00125       inline Ptr 
00126       makeShared () { return Ptr (new RangeImage (*this)); } 
00127       
00129       PCL_EXPORTS void 
00130       reset ();
00131       
00145       template <typename PointCloudType> void
00146       createFromPointCloud (const PointCloudType& point_cloud, float angular_resolution, float max_angle_width,
00147                             float max_angle_height, const Eigen::Affine3f& sensor_pose,
00148                             CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
00149                             float min_range=0.0f, int border_size=0);
00150   
00165       template <typename PointCloudType> void
00166       createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, float angular_resolution,
00167                                          const Eigen::Vector3f& point_cloud_center, float point_cloud_radius,
00168                                          const Eigen::Affine3f& sensor_pose,
00169                                          CoordinateFrame coordinate_frame=CAMERA_FRAME,
00170                                          float noise_level=0.0f, float min_range=0.0f, int border_size=0);
00171       
00187       template <typename PointCloudTypeWithViewpoints> void
00188       createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud, float angular_resolution,
00189                                           float max_angle_width, float max_angle_height,
00190                                           CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
00191                                           float min_range=0.0f, int border_size=0);
00192 
00200       void
00201       createEmpty(float angular_resolution, const Eigen::Affine3f& sensor_pose=Eigen::Affine3f::Identity(),
00202                   RangeImage::CoordinateFrame coordinate_frame=CAMERA_FRAME, float angle_width=pcl::deg2rad(360.0f), float angle_height=pcl::deg2rad(180.0f));
00203       
00205       PCL_EXPORTS void
00206       integrateFarRanges (const PointCloud<PointWithViewpoint>& far_ranges);
00207       
00215       PCL_EXPORTS void
00216       cropImage (int border_size=0, int top=-1, int right=-1, int bottom=-1, int left=-1);
00217       
00222       PCL_EXPORTS float*
00223       getRangesArray () const;
00224       
00227       inline const Eigen::Affine3f&
00228       getTransformationToRangeImageSystem () const { return (to_range_image_system_); }
00229       
00232       inline void 
00233       setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system);
00234       
00237       inline const Eigen::Affine3f&
00238       getTransformationToWorldSystem () const { return to_world_system_;}
00239       
00241       inline float
00242       getAngularResolution () const { return angular_resolution_;}
00243       
00247       inline void
00248       setAngularResolution (float angular_resolution);
00249       
00255       inline const PointWithRange&
00256       getPoint (int image_x, int image_y) const;
00257 
00259       inline PointWithRange&
00260       getPoint (int image_x, int image_y);
00261       
00263       inline const PointWithRange&
00264       getPoint (float image_x, float image_y) const;
00265       
00267       inline PointWithRange&
00268       getPoint (float image_x, float image_y);
00269       
00276       inline const PointWithRange&
00277       getPointNoCheck (int image_x, int image_y) const;
00278 
00280       inline PointWithRange&
00281       getPointNoCheck (int image_x, int image_y);
00282 
00284       inline void
00285       getPoint (int image_x, int image_y, Eigen::Vector3f& point) const;
00286       
00288       inline void
00289       getPoint (int index, Eigen::Vector3f& point) const;
00290       
00292       inline const Eigen::Map<const Eigen::Vector3f>
00293       getEigenVector3f (int x, int y) const;
00294 
00296       inline const Eigen::Map<const Eigen::Vector3f>
00297       getEigenVector3f (int index) const;
00298       
00300       inline const PointWithRange&
00301       getPoint (int index) const;
00302 
00309       inline const PointWithRange&
00310       getPointConsideringWrapAround (int image_x, int image_y) const;
00311       
00313       inline void
00314       calculate3DPoint (float image_x, float image_y, float range, PointWithRange& point) const;
00316       inline void
00317       calculate3DPoint (float image_x, float image_y, PointWithRange& point) const;
00318 
00320       virtual inline void
00321       calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const;
00323       inline void
00324       calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const;
00325       
00327       PCL_EXPORTS void
00328       recalculate3DPointPositions ();
00329       
00331       inline virtual void
00332       getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const;
00333 
00335       inline void
00336       getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y, float& range) const;
00337       
00339       inline void
00340       getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y) const;
00341       
00343       inline void
00344       getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y) const;
00345       
00347       inline void
00348       getImagePoint (float x, float y, float z, float& image_x, float& image_y, float& range) const;
00349       
00351       inline void
00352       getImagePoint (float x, float y, float z, float& image_x, float& image_y) const;
00353       
00355       inline void
00356       getImagePoint (float x, float y, float z, int& image_x, int& image_y) const;
00357       
00360       inline float
00361       checkPoint (const Eigen::Vector3f& point, PointWithRange& point_in_image) const;
00362 
00366       inline float
00367       getRangeDifference (const Eigen::Vector3f& point) const;
00368       
00370       inline void
00371       getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const;
00372       
00374       inline void
00375       getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const;
00376       
00378       inline void
00379       real2DToInt2D (float x, float y, int& xInt, int& yInt) const;
00380       
00382       inline bool
00383       isInImage (int x, int y) const;
00384       
00386       inline bool
00387       isValid (int x, int y) const;
00388       
00390       inline bool
00391       isValid (int index) const;
00392       
00394       inline bool
00395       isObserved (int x, int y) const;
00396 
00398       inline bool
00399       isMaxRange (int x, int y) const;
00400       
00404       inline bool
00405       getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size=1) const;
00406       
00408       inline bool
00409       getNormalForClosestNeighbors (int x, int y, int radius, const PointWithRange& point,
00410                                     int no_of_nearest_neighbors, Eigen::Vector3f& normal, int step_size=1) const;
00411       
00413       inline bool
00414       getNormalForClosestNeighbors (int x, int y, int radius, const Eigen::Vector3f& point,
00415                                     int no_of_nearest_neighbors, Eigen::Vector3f& normal,
00416                                     Eigen::Vector3f* point_on_plane=NULL, int step_size=1) const;
00417       
00419       inline bool
00420       getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius=2) const;
00421       
00424       inline bool
00425       getSurfaceInformation (int x, int y, int radius, const Eigen::Vector3f& point,
00426                              int no_of_closest_neighbors, int step_size,
00427                              float& max_closest_neighbor_distance_squared,
00428                              Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values,
00429                              Eigen::Vector3f* normal_all_neighbors=NULL,
00430                              Eigen::Vector3f* mean_all_neighbors=NULL,
00431                              Eigen::Vector3f* eigen_values_all_neighbors=NULL) const;
00432       
00433       // Return the squared distance to the n-th neighbors of the point at x,y
00434       inline float
00435       getSquaredDistanceOfNthNeighbor (int x, int y, int radius, int n, int step_size) const;
00436       
00439       inline float
00440       getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const;
00442       inline float
00443       getImpactAngle (int x1, int y1, int x2, int y2) const;
00444       
00447       inline float
00448       getImpactAngleBasedOnLocalNormal (int x, int y, int radius) const;
00450       PCL_EXPORTS float*
00451       getImpactAngleImageBasedOnLocalNormals (int radius) const;
00452 
00456       inline float
00457       getNormalBasedAcutenessValue (int x, int y, int radius) const;
00458       
00461       inline float
00462       getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const;
00464       inline float
00465       getAcutenessValue (int x1, int y1, int x2, int y2) const;
00466       
00468       PCL_EXPORTS void
00469       getAcutenessValueImages (int pixel_distance, float*& acuteness_value_image_x,
00470                                float*& acuteness_value_image_y) const;
00471       
00474       //inline float
00475       //  getSurfaceChange(const PointWithRange& point, const PointWithRange& neighbor1,
00476       //                   const PointWithRange& neighbor2) const;
00477       
00479       PCL_EXPORTS float
00480       getSurfaceChange (int x, int y, int radius) const;
00481       
00483       PCL_EXPORTS float*
00484       getSurfaceChangeImage (int radius) const;
00485       
00488       inline void
00489       getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const;
00490       
00492       PCL_EXPORTS void
00493       getSurfaceAngleChangeImages (int radius, float*& angle_change_image_x, float*& angle_change_image_y) const;
00494       
00496       inline float
00497       getCurvature (int x, int y, int radius, int step_size) const;
00498       
00500       inline const Eigen::Vector3f
00501       getSensorPos () const;
00502       
00504       PCL_EXPORTS void
00505       setUnseenToMaxRange ();
00506       
00508       inline int
00509       getImageOffsetX () const { return image_offset_x_;}
00511       inline int
00512       getImageOffsetY () const { return image_offset_y_;}
00513       
00515       inline void
00516       setImageOffsets (int offset_x, int offset_y) { image_offset_x_=offset_x; image_offset_y_=offset_y;}
00517  
00518 
00519       
00533       virtual void
00534       getSubImage (int sub_image_image_offset_x, int sub_image_image_offset_y, int sub_image_width,
00535                    int sub_image_height, int combine_pixels, RangeImage& sub_image) const;
00536       
00538       virtual void 
00539       getHalfImage(RangeImage& half_image) const;
00540       
00542       PCL_EXPORTS void
00543       getMinMaxRanges (float& min_range, float& max_range) const;
00544       
00546       PCL_EXPORTS void
00547       change3dPointsToLocalCoordinateFrame ();
00548       
00553       PCL_EXPORTS float*
00554       getInterpolatedSurfaceProjection (const Eigen::Affine3f& pose, int pixel_size, float world_size) const;
00555       
00557       PCL_EXPORTS float*
00558       getInterpolatedSurfaceProjection (const Eigen::Vector3f& point, int pixel_size, float world_size) const;
00559       
00561       inline Eigen::Affine3f
00562       getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const;
00564       inline void
00565       getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point,
00566                                                 Eigen::Affine3f& transformation) const;
00568       inline void
00569       getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const;
00570 
00572       PCL_EXPORTS bool
00573       getNormalBasedUprightTransformation (const Eigen::Vector3f& point,
00574                                            float max_dist, Eigen::Affine3f& transformation) const;
00575       
00578       PCL_EXPORTS void
00579       getIntegralImage (float*& integral_image, int*& valid_points_num_image) const;
00580       
00582       PCL_EXPORTS void
00583       getBlurredImageUsingIntegralImage (int blur_radius, float* integral_image, int* valid_points_num_image,
00584                                          RangeImage& range_image) const;
00585       
00587       PCL_EXPORTS void
00588       getBlurredImage (int blur_radius, RangeImage& range_image) const;
00589       
00592       inline float
00593       getEuclideanDistanceSquared (int x1, int y1, int x2, int y2) const;
00595       inline float
00596       getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const;
00597       
00599       PCL_EXPORTS void
00600       getRangeImageWithSmoothedSurface (int radius, RangeImage& smoothed_range_image) const;
00601       //void getLocalNormals(int radius) const;
00602       
00607       inline void
00608       get1dPointAverage (int x, int y, int delta_x, int delta_y, int no_of_points,
00609                          PointWithRange& average_point) const;
00610       
00612       PCL_EXPORTS Eigen::Affine3f
00613       doIcp (const VectorOfEigenVector3f& points,
00614              const Eigen::Affine3f& initial_guess, int search_radius,
00615              float max_distance_start, float max_distance_end, int num_iterations) const;
00616       
00620       PCL_EXPORTS Eigen::Affine3f
00621       doIcp (const RangeImage& other_range_image,
00622              const Eigen::Affine3f& initial_guess, int search_radius,
00623              float max_distance_start, float max_distance_end,
00624              int num_iterations, int pixel_step_start=1, int pixel_step_end=1) const;
00625       
00626 
00628       struct ExtractedPlane {
00629         Eigen::Vector3f normal;  
00630         float d;               
00631         Eigen::Vector3f maximum_extensions; 
00632         Eigen::Vector3f mean;            
00633         Eigen::Vector3f eigen_values;    
00634         Eigen::Vector3f eigen_vector1,   
00635                         eigen_vector2,   
00636                         eigen_vector3;   
00637         std::vector<int> point_indices;  
00638       };
00639 
00646       PCL_EXPORTS void
00647       extractPlanes (float initial_max_plane_error, std::vector<ExtractedPlane>& planes) const;
00648       
00651       static inline bool
00652       isLargerPlane (const ExtractedPlane& p1, const ExtractedPlane& p2)
00653                     { return p1.maximum_extensions[2] > p2.maximum_extensions[2]; }
00654       
00657       PCL_EXPORTS float
00658       getOverlap (const RangeImage& other_range_image, const Eigen::Affine3f& relative_transformation,
00659                   int search_radius, float max_distance, int pixel_step=1) const;
00660       
00662       inline bool
00663       getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const;
00664       
00666       inline void
00667       getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const;
00668       
00671       virtual RangeImage* 
00672       getNew () const { return new RangeImage; }
00673 
00674       
00675       // =====MEMBER VARIABLES=====
00676       // BaseClass:
00677       //   roslib::Header header;
00678       //   std::vector<PointT> points;
00679       //   uint32_t width;
00680       //   uint32_t height;
00681       //   bool is_dense;
00682 
00683       static bool debug; 
00685     protected:
00686       // =====PROTECTED MEMBER VARIABLES=====
00687       Eigen::Affine3f to_range_image_system_; 
00688       Eigen::Affine3f to_world_system_;       
00689       float angular_resolution_;               
00690       float angular_resolution_reciprocal_;    
00692       int image_offset_x_, image_offset_y_;    
00694       PointWithRange unobserved_point;         
00697       // =====PROTECTED METHODS=====
00698       template <typename PointCloudType> void
00699       doZBuffer (const PointCloudType& point_cloud, float noise_level,
00700                  float min_range, int& top, int& right, int& bottom, int& left);
00701 
00702       // =====STATIC PROTECTED=====
00703       static const int lookup_table_size;
00704       static std::vector<float> asin_lookup_table;
00705       static std::vector<float> atan_lookup_table;
00706       static std::vector<float> cos_lookup_table;
00708       static void
00709       createLookupTables ();
00710 
00712       static inline float
00713       asinLookUp (float value);
00714       
00716       static inline float
00717       atan2LookUp (float y, float x);
00718      
00720       static inline float
00721       cosLookUp (float value);
00722 
00723 
00724     public:
00725       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00726   };
00727 
00731   inline std::ostream&
00732   operator<< (std::ostream& os, const RangeImage& r)
00733   {
00734     os << "header: " << std::endl;
00735     os << r.header;
00736     os << "points[]: " << r.points.size () << std::endl;
00737     os << "width: " << r.width << std::endl;
00738     os << "height: " << r.height << std::endl;
00739     os << "sensor_origin_: "
00740        << r.sensor_origin_[0] << ' '
00741        << r.sensor_origin_[1] << ' '
00742        << r.sensor_origin_[2] << std::endl;
00743     os << "sensor_orientation_: "
00744        << r.sensor_orientation_.x () << ' '
00745        << r.sensor_orientation_.y () << ' '
00746        << r.sensor_orientation_.z () << ' '
00747        << r.sensor_orientation_.w () << std::endl;
00748     os << "is_dense: " << r.is_dense << std::endl;
00749     os << "angular resolution: "
00750        << RAD2DEG (r.getAngularResolution ()) << "deg/pixel" << std::endl;
00751     return (os);
00752   }
00753 
00754 }  // namespace end
00755 
00756 
00757 #include "pcl/range_image/impl/range_image.hpp"  // Definitions of templated and inline functions
00758 
00759 #endif  //#ifndef PCL_RANGE_IMAGE_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines