Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/common/include/pcl/range_image/range_image.h
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 {
00050   /** \brief RangeImage is derived from pcl/PointCloud and provides functionalities with focus on situations where
00051     *  a 3D scene was captured from a specific view point. 
00052     * \author Bastian Steder
00053     * \ingroup range_image
00054     */
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=====
00072       /** Constructor */
00073       PCL_EXPORTS RangeImage ();
00074       /** Destructor */
00075       PCL_EXPORTS virtual ~RangeImage ();
00076       
00077       // =====STATIC VARIABLES=====
00078       /** The maximum number of openmp threads that can be used in this class */
00079       static int max_no_of_threads;
00080       
00081       // =====STATIC METHODS=====
00082       /** \brief Get the size of a certain area when seen from the given pose
00083         * \param viewer_pose an affine matrix defining the pose of the viewer
00084         * \param center the center of the area
00085         * \param radius the radius of the area
00086         * \return the size of the area as viewed according to \a viewer_pose
00087         */
00088       static inline float
00089       getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center, 
00090                        float radius);
00091       
00092       /** \brief Get Eigen::Vector3f from PointWithRange
00093         * \param point the input point
00094         * \return an Eigen::Vector3f representation of the input point
00095         */
00096       static inline Eigen::Vector3f
00097       getEigenVector3f (const PointWithRange& point);
00098       
00099       /** \brief Get the transformation that transforms the given coordinate frame into CAMERA_FRAME
00100         * \param coordinate_frame the input coordinate frame
00101         * \param transformation the resulting transformation that warps \a coordinate_frame into CAMERA_FRAME
00102         */
00103       PCL_EXPORTS static void
00104       getCoordinateFrameTransformation (RangeImage::CoordinateFrame coordinate_frame,
00105                                         Eigen::Affine3f& transformation);
00106       
00107       /** \brief Get the average viewpoint of a point cloud where each point carries viewpoint information as 
00108         * vp_x, vp_y, vp_z
00109         * \param point_cloud the input point cloud
00110         * \return the average viewpoint (as an Eigen::Vector3f)
00111         */
00112       template <typename PointCloudTypeWithViewpoints> static Eigen::Vector3f
00113       getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud);
00114       
00115       /** \brief Check if the provided data includes far ranges and add them to far_ranges
00116         * \param point_cloud_data a PCLPointCloud2 message containing the input cloud
00117         * \param far_ranges the resulting cloud containing those points with far ranges
00118         */
00119       PCL_EXPORTS static void
00120       extractFarRanges (const pcl::PCLPointCloud2& point_cloud_data, PointCloud<PointWithViewpoint>& far_ranges);
00121       
00122       // =====METHODS=====
00123       /** \brief Get a boost shared pointer of a copy of this */
00124       inline Ptr 
00125       makeShared () { return Ptr (new RangeImage (*this)); } 
00126       
00127       /** \brief Reset all values to an empty range image */
00128       PCL_EXPORTS void 
00129       reset ();
00130       
00131       /** \brief Create the depth image from a point cloud
00132         * \param point_cloud the input point cloud
00133         * \param angular_resolution the angular difference (in radians) between the individual pixels in the image
00134         * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor
00135         * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor
00136         * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to
00137         *                    Eigen::Affine3f::Identity () )
00138         * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
00139         * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
00140         *                      but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
00141         *                      will always take the minimum per cell.
00142         * \param min_range the minimum visible range (defaults to 0)
00143         * \param border_size the border size (defaults to 0)
00144         */
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       
00152       /** \brief Create the depth image from a point cloud
00153         * \param point_cloud the input point cloud
00154         * \param angular_resolution_x the angular difference (in radians) between the
00155         *                             individual pixels in the image in the x-direction
00156         * \param angular_resolution_y the angular difference (in radians) between the
00157         *                             individual pixels in the image in the y-direction
00158         * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor
00159         * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor
00160         * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to
00161         *                    Eigen::Affine3f::Identity () )
00162         * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
00163         * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
00164         *                      but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
00165         *                      will always take the minimum per cell.
00166         * \param min_range the minimum visible range (defaults to 0)
00167         * \param border_size the border size (defaults to 0)
00168         */
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       
00177       /** \brief Create the depth image from a point cloud, getting a hint about the size of the scene for 
00178         * faster calculation.
00179         * \param point_cloud the input point cloud
00180         * \param angular_resolution the angle (in radians) between each sample in the depth image
00181         * \param point_cloud_center the center of bounding sphere
00182         * \param point_cloud_radius the radius of the bounding sphere
00183         * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to
00184         *                     Eigen::Affine3f::Identity () )
00185         * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
00186         * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
00187         *                      but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
00188         *                      will always take the minimum per cell.
00189         * \param min_range the minimum visible range (defaults to 0)
00190         * \param border_size the border size (defaults to 0)
00191         */
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       
00199       /** \brief Create the depth image from a point cloud, getting a hint about the size of the scene for 
00200         * faster calculation.
00201         * \param point_cloud the input point cloud
00202         * \param angular_resolution_x the angular difference (in radians) between the
00203         *                             individual pixels in the image in the x-direction
00204         * \param angular_resolution_y the angular difference (in radians) between the
00205         *                             individual pixels in the image in the y-direction
00206         * \param angular_resolution the angle (in radians) between each sample in the depth image
00207         * \param point_cloud_center the center of bounding sphere
00208         * \param point_cloud_radius the radius of the bounding sphere
00209         * \param sensor_pose an affine matrix defining the pose of the sensor (defaults to
00210         *                     Eigen::Affine3f::Identity () )
00211         * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
00212         * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
00213         *                      but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
00214         *                      will always take the minimum per cell.
00215         * \param min_range the minimum visible range (defaults to 0)
00216         * \param border_size the border size (defaults to 0)
00217         */
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       
00226       /** \brief Create the depth image from a point cloud, using the average viewpoint of the points 
00227         * (vp_x,vp_y,vp_z in the point type) in the point cloud as sensor pose (assuming a rotation of (0,0,0)).
00228         * \param point_cloud the input point cloud
00229         * \param angular_resolution the angle (in radians) between each sample in the depth image
00230         * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor
00231         * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor
00232         * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
00233         * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
00234         *                      but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
00235         *                      will always take the minimum per cell.
00236         * \param min_range the minimum visible range (defaults to 0)
00237         * \param border_size the border size (defaults to 0)
00238         * \note If wrong_coordinate_system is true, the sensor pose will be rotated to change from a coordinate frame
00239         * with x to the front, y to the left and z to the top to the coordinate frame we use here (x to the right, y 
00240         * to the bottom and z to the front) */
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       
00247       /** \brief Create the depth image from a point cloud, using the average viewpoint of the points 
00248         * (vp_x,vp_y,vp_z in the point type) in the point cloud as sensor pose (assuming a rotation of (0,0,0)).
00249         * \param point_cloud the input point cloud
00250         * \param angular_resolution_x the angular difference (in radians) between the
00251         *                             individual pixels in the image in the x-direction
00252         * \param angular_resolution_y the angular difference (in radians) between the
00253         *                             individual pixels in the image in the y-direction
00254         * \param max_angle_width an angle (in radians) defining the horizontal bounds of the sensor
00255         * \param max_angle_height an angle (in radians) defining the vertical bounds of the sensor
00256         * \param coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
00257         * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
00258         *                      but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
00259         *                      will always take the minimum per cell.
00260         * \param min_range the minimum visible range (defaults to 0)
00261         * \param border_size the border size (defaults to 0)
00262         * \note If wrong_coordinate_system is true, the sensor pose will be rotated to change from a coordinate frame
00263         * with x to the front, y to the left and z to the top to the coordinate frame we use here (x to the right, y 
00264         * to the bottom and z to the front) */
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       
00272       /** \brief Create an empty depth image (filled with unobserved points)
00273         * \param[in] angular_resolution the angle (in radians) between each sample in the depth image
00274         * \param[in] sensor_pose an affine matrix defining the pose of the sensor (defaults to  Eigen::Affine3f::Identity () )
00275         * \param[in] coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
00276         * \param[in] angle_width an angle (in radians) defining the horizontal bounds of the sensor (defaults to 2*pi (360deg))
00277         * \param[in] angle_height an angle (in radians) defining the vertical bounds of the sensor (defaults to pi (180deg))
00278         */
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       
00284       /** \brief Create an empty depth image (filled with unobserved points)
00285         * \param angular_resolution_x the angular difference (in radians) between the
00286         *                             individual pixels in the image in the x-direction
00287         * \param angular_resolution_y the angular difference (in radians) between the
00288         *                             individual pixels in the image in the y-direction
00289         * \param[in] sensor_pose an affine matrix defining the pose of the sensor (defaults to  Eigen::Affine3f::Identity () )
00290         * \param[in] coordinate_frame the coordinate frame (defaults to CAMERA_FRAME)
00291         * \param[in] angle_width an angle (in radians) defining the horizontal bounds of the sensor (defaults to 2*pi (360deg))
00292         * \param[in] angle_height an angle (in radians) defining the vertical bounds of the sensor (defaults to pi (180deg))
00293         */
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       
00300       /** \brief Integrate the given point cloud into the current range image using a z-buffer
00301         * \param point_cloud the input point cloud
00302         * \param noise_level - The distance in meters inside of which the z-buffer will not use the minimum,
00303         *                      but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and
00304         *                      will always take the minimum per cell.
00305         * \param min_range the minimum visible range
00306         * \param top    returns the minimum y pixel position in the image where a point was added
00307         * \param right  returns the maximum x pixel position in the image where a point was added
00308         * \param bottom returns the maximum y pixel position in the image where a point was added
00309         * \param top returns the minimum y position in the image where a point was added
00310         * \param left   returns the minimum x pixel position in the image where a point was added
00311         */
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       
00316       /** \brief Integrates the given far range measurements into the range image */
00317       template <typename PointCloudType> void
00318       integrateFarRanges (const PointCloudType& far_ranges);
00319       
00320       /** \brief Cut the range image to the minimal size so that it still contains all actual range readings.
00321         * \param border_size allows increase from the minimal size by the specified number of pixels (defaults to 0)
00322         * \param top if positive, this value overrides the position of the top edge (defaults to -1)
00323         * \param right if positive, this value overrides the position of the right edge (defaults to -1)
00324         * \param bottom if positive, this value overrides the position of the bottom edge (defaults to -1)
00325         * \param left if positive, this value overrides the position of the left edge (defaults to -1)
00326         */
00327       PCL_EXPORTS void
00328       cropImage (int border_size=0, int top=-1, int right=-1, int bottom=-1, int left=-1);
00329       
00330       /** \brief Get all the range values in one float array of size width*height  
00331         * \return a pointer to a new float array containing the range values
00332         * \note This method allocates a new float array; the caller is responsible for freeing this memory.
00333         */
00334       PCL_EXPORTS float*
00335       getRangesArray () const;
00336       
00337       /** Getter for the transformation from the world system into the range image system
00338        *  (the sensor coordinate frame) */
00339       inline const Eigen::Affine3f&
00340       getTransformationToRangeImageSystem () const { return (to_range_image_system_); }
00341       
00342       /** Setter for the transformation from the range image system
00343        *  (the sensor coordinate frame) into the world system */
00344       inline void 
00345       setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system);
00346       
00347       /** Getter for the transformation from the range image system
00348        *  (the sensor coordinate frame) into the world system */
00349       inline const Eigen::Affine3f&
00350       getTransformationToWorldSystem () const { return to_world_system_;}
00351       
00352       /** Getter for the angular resolution of the range image in x direction in radians per pixel.
00353        *  Provided for downwards compatability */
00354       inline float
00355       getAngularResolution () const { return angular_resolution_x_;}
00356       
00357       /** Getter for the angular resolution of the range image in x direction in radians per pixel. */
00358       inline float
00359       getAngularResolutionX () const { return angular_resolution_x_;}
00360       
00361       /** Getter for the angular resolution of the range image in y direction in radians per pixel. */
00362       inline float
00363       getAngularResolutionY () const { return angular_resolution_y_;}
00364       
00365       /** Getter for the angular resolution of the range image in x and y direction (in radians). */
00366       inline void
00367       getAngularResolution (float& angular_resolution_x, float& angular_resolution_y) const;
00368       
00369       /** \brief Set the angular resolution of the range image
00370         * \param angular_resolution the new angular resolution in x and y direction (in radians per pixel)
00371         */
00372       inline void
00373       setAngularResolution (float angular_resolution);
00374       
00375       /** \brief Set the angular resolution of the range image
00376         * \param angular_resolution_x the new angular resolution in x direction (in radians per pixel)
00377         * \param angular_resolution_y the new angular resolution in y direction (in radians per pixel)
00378         */
00379       inline void
00380       setAngularResolution (float angular_resolution_x, float angular_resolution_y);
00381 
00382       
00383       /** \brief Return the 3D point with range at the given image position
00384         * \param image_x the x coordinate
00385         * \param image_y the y coordinate
00386         * \return the point at the specified location (returns unobserved_point if outside of the image bounds)
00387         */
00388       inline const PointWithRange&
00389       getPoint (int image_x, int image_y) const;
00390 
00391       /** \brief Non-const-version of getPoint */
00392       inline PointWithRange&
00393       getPoint (int image_x, int image_y);
00394       
00395       /** Return the 3d point with range at the given image position */
00396       inline const PointWithRange&
00397       getPoint (float image_x, float image_y) const;
00398       
00399       /** Non-const-version of the above */
00400       inline PointWithRange&
00401       getPoint (float image_x, float image_y);
00402       
00403       /** \brief Return the 3D point with range at the given image position.  This methd performs no error checking
00404         * to make sure the specified image position is inside of the image!
00405         * \param image_x the x coordinate
00406         * \param image_y the y coordinate
00407         * \return the point at the specified location (program may fail if the location is outside of the image bounds)
00408         */
00409       inline const PointWithRange&
00410       getPointNoCheck (int image_x, int image_y) const;
00411 
00412       /** Non-const-version of getPointNoCheck */
00413       inline PointWithRange&
00414       getPointNoCheck (int image_x, int image_y);
00415 
00416       /** Same as above */
00417       inline void
00418       getPoint (int image_x, int image_y, Eigen::Vector3f& point) const;
00419       
00420       /** Same as above */
00421       inline void
00422       getPoint (int index, Eigen::Vector3f& point) const;
00423       
00424       /** Same as above */
00425       inline const Eigen::Map<const Eigen::Vector3f>
00426       getEigenVector3f (int x, int y) const;
00427 
00428       /** Same as above */
00429       inline const Eigen::Map<const Eigen::Vector3f>
00430       getEigenVector3f (int index) const;
00431       
00432       /** Return the 3d point with range at the given index (whereas index=y*width+x) */
00433       inline const PointWithRange&
00434       getPoint (int index) const;
00435 
00436       /** Calculate the 3D point according to the given image point and range */
00437       inline void
00438       calculate3DPoint (float image_x, float image_y, float range, PointWithRange& point) const;
00439       /** Calculate the 3D point according to the given image point and the range value at the closest pixel */
00440       inline void
00441       calculate3DPoint (float image_x, float image_y, PointWithRange& point) const;
00442 
00443       /** Calculate the 3D point according to the given image point and range */
00444       virtual inline void
00445       calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const;
00446       /** Calculate the 3D point according to the given image point and the range value at the closest pixel */
00447       inline void
00448       calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const;
00449       
00450       /** Recalculate all 3D point positions according to their pixel position and range */
00451       PCL_EXPORTS void
00452       recalculate3DPointPositions ();
00453       
00454       /** Get imagePoint from 3D point in world coordinates */
00455       inline virtual void
00456       getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const;
00457 
00458       /** Same as above */
00459       inline void
00460       getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y, float& range) const;
00461       
00462       /** Same as above */
00463       inline void
00464       getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y) const;
00465       
00466       /** Same as above */
00467       inline void
00468       getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y) const;
00469       
00470       /** Same as above */
00471       inline void
00472       getImagePoint (float x, float y, float z, float& image_x, float& image_y, float& range) const;
00473       
00474       /** Same as above */
00475       inline void
00476       getImagePoint (float x, float y, float z, float& image_x, float& image_y) const;
00477       
00478       /** Same as above */
00479       inline void
00480       getImagePoint (float x, float y, float z, int& image_x, int& image_y) const;
00481       
00482       /** point_in_image will be the point in the image at the position the given point would be. Returns
00483        * the range of the given point. */
00484       inline float
00485       checkPoint (const Eigen::Vector3f& point, PointWithRange& point_in_image) const;
00486 
00487       /** Returns the difference in range between the given point and the range of the point in the image
00488        * at the position the given point would be.
00489        *  (Return value is point_in_image.range-given_point.range) */
00490       inline float
00491       getRangeDifference (const Eigen::Vector3f& point) const;
00492       
00493       /** Get the image point corresponding to the given angles */
00494       inline void
00495       getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const;
00496       
00497       /** Get the angles corresponding to the given image point */
00498       inline void
00499       getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const;
00500       
00501       /** Transforms an image point in float values to an image point in int values */
00502       inline void
00503       real2DToInt2D (float x, float y, int& xInt, int& yInt) const;
00504       
00505       /** Check if a point is inside of the image */
00506       inline bool
00507       isInImage (int x, int y) const;
00508       
00509       /** Check if a point is inside of the image and has a finite range */
00510       inline bool
00511       isValid (int x, int y) const;
00512       
00513       /** Check if a point has a finite range */
00514       inline bool
00515       isValid (int index) const;
00516       
00517       /** Check if a point is inside of the image and has either a finite range or a max reading (range=INFINITY) */
00518       inline bool
00519       isObserved (int x, int y) const;
00520 
00521       /** Check if a point is a max range (range=INFINITY) - please check isInImage or isObserved first! */
00522       inline bool
00523       isMaxRange (int x, int y) const;
00524       
00525       /** Calculate the normal of an image point using the neighbors with a maximum pixel distance of radius.
00526        *  step_size determines how many pixels are used. 1 means all, 2 only every second, etc..
00527        *  Returns false if it was unable to calculate a normal.*/
00528       inline bool
00529       getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size=1) const;
00530       
00531       /** Same as above, but only the no_of_nearest_neighbors points closest to the given point are considered. */
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       
00536       /** Same as above */
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       
00542       /** Same as above, using default values */
00543       inline bool
00544       getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius=2) const;
00545       
00546       /** Same as above but extracts some more data and can also return the extracted
00547        * information for all neighbors in radius if normal_all_neighbors is not NULL */
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       
00561       /** Calculate the impact angle based on the sensor position and the two given points - will return
00562        * -INFINITY if one of the points is unobserved */
00563       inline float
00564       getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const;
00565       //! Same as above
00566       inline float
00567       getImpactAngle (int x1, int y1, int x2, int y2) const;
00568       
00569       /** Extract a local normal (with a heuristic not to include background points) and calculate the impact
00570        *  angle based on this */
00571       inline float
00572       getImpactAngleBasedOnLocalNormal (int x, int y, int radius) const;
00573       /** Uses the above function for every point in the image */
00574       PCL_EXPORTS float*
00575       getImpactAngleImageBasedOnLocalNormals (int radius) const;
00576 
00577       /** Calculate a score [0,1] that tells how acute the impact angle is (1.0f - getImpactAngle/90deg)
00578        *  This uses getImpactAngleBasedOnLocalNormal
00579        *  Will return -INFINITY if no normal could be calculated */
00580       inline float
00581       getNormalBasedAcutenessValue (int x, int y, int radius) const;
00582       
00583       /** Calculate a score [0,1] that tells how acute the impact angle is (1.0f - getImpactAngle/90deg)
00584        *  will return -INFINITY if one of the points is unobserved */
00585       inline float
00586       getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const;
00587       //! Same as above
00588       inline float
00589       getAcutenessValue (int x1, int y1, int x2, int y2) const;
00590       
00591       /** Calculate getAcutenessValue for every point */
00592       PCL_EXPORTS void
00593       getAcutenessValueImages (int pixel_distance, float*& acuteness_value_image_x,
00594                                float*& acuteness_value_image_y) const;
00595       
00596       /** Calculates, how much the surface changes at a point. Pi meaning a flat suface and 0.0f
00597        *  would be a needle point */
00598       //inline float
00599       //  getSurfaceChange (const PointWithRange& point, const PointWithRange& neighbor1,
00600       //                   const PointWithRange& neighbor2) const;
00601       
00602       /** Calculates, how much the surface changes at a point. 1 meaning a 90deg angle and 0 a flat suface */
00603       PCL_EXPORTS float
00604       getSurfaceChange (int x, int y, int radius) const;
00605       
00606       /** Uses the above function for every point in the image */
00607       PCL_EXPORTS float*
00608       getSurfaceChangeImage (int radius) const;
00609       
00610       /** Calculates, how much the surface changes at a point. Returns an angle [0.0f, PI] for x and y direction.
00611        *  A return value of -INFINITY means that a point was unobserved. */
00612       inline void
00613       getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const;
00614       
00615       /** Uses the above function for every point in the image */
00616       PCL_EXPORTS void
00617       getSurfaceAngleChangeImages (int radius, float*& angle_change_image_x, float*& angle_change_image_y) const;
00618       
00619       /** Calculates the curvature in a point using pca */
00620       inline float
00621       getCurvature (int x, int y, int radius, int step_size) const;
00622       
00623       //! Get the sensor position
00624       inline const Eigen::Vector3f
00625       getSensorPos () const;
00626       
00627       /** Sets all -INFINITY values to INFINITY */
00628       PCL_EXPORTS void
00629       setUnseenToMaxRange ();
00630       
00631       //! Getter for image_offset_x_
00632       inline int
00633       getImageOffsetX () const { return image_offset_x_;}
00634       //! Getter for image_offset_y_
00635       inline int
00636       getImageOffsetY () const { return image_offset_y_;}
00637       
00638       //! Setter for image offsets
00639       inline void
00640       setImageOffsets (int offset_x, int offset_y) { image_offset_x_=offset_x; image_offset_y_=offset_y;}
00641  
00642 
00643       
00644       /** Get a sub part of the complete image as a new range image.
00645         * \param sub_image_image_offset_x - The x coordinate of the top left pixel of the sub image.
00646         *                         This is always according to absolute 0,0 meaning -180°,-90°
00647         *                         and it is already in the system of the new image, so the
00648         *                         actual pixel used in the original image is
00649         *                         combine_pixels* (image_offset_x-image_offset_x_)
00650         * \param sub_image_image_offset_y - Same as image_offset_x for the y coordinate
00651         * \param sub_image_width - width of the new image
00652         * \param sub_image_height - height of the new image
00653         * \param combine_pixels - shrinking factor, meaning the new angular resolution
00654         *                         is combine_pixels times the old one
00655         * \param sub_image - the output image
00656         */
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       
00661       //! Get a range image with half the resolution
00662       virtual void 
00663       getHalfImage (RangeImage& half_image) const;
00664       
00665       //! Find the minimum and maximum range in the image
00666       PCL_EXPORTS void
00667       getMinMaxRanges (float& min_range, float& max_range) const;
00668       
00669       //! This function sets the sensor pose to 0 and transforms all point positions to this local coordinate frame
00670       PCL_EXPORTS void
00671       change3dPointsToLocalCoordinateFrame ();
00672       
00673       /** Calculate a range patch as the z values of the coordinate frame given by pose.
00674        *  The patch will have size pixel_size x pixel_size and each pixel
00675        *  covers world_size/pixel_size meters in the world
00676        *  You are responsible for deleting the structure afterwards! */
00677       PCL_EXPORTS float*
00678       getInterpolatedSurfaceProjection (const Eigen::Affine3f& pose, int pixel_size, float world_size) const;
00679       
00680       //! Same as above, but using the local coordinate frame defined by point and the viewing direction
00681       PCL_EXPORTS float*
00682       getInterpolatedSurfaceProjection (const Eigen::Vector3f& point, int pixel_size, float world_size) const;
00683       
00684       //! Get the local coordinate frame with 0,0,0 in point, upright and Z as the viewing direction
00685       inline Eigen::Affine3f
00686       getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const;
00687       //! Same as above, using a reference for the retrurn value
00688       inline void
00689       getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point,
00690                                                 Eigen::Affine3f& transformation) const;
00691       //! Same as above, but only returning the rotation
00692       inline void
00693       getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const;
00694 
00695       /** Get a local coordinate frame at the given point based on the normal. */
00696       PCL_EXPORTS bool
00697       getNormalBasedUprightTransformation (const Eigen::Vector3f& point,
00698                                            float max_dist, Eigen::Affine3f& transformation) const;
00699       
00700       /** Get the integral image of the range values (used for fast blur operations).
00701        *  You are responsible for deleting it after usage! */
00702       PCL_EXPORTS void
00703       getIntegralImage (float*& integral_image, int*& valid_points_num_image) const;
00704       
00705       /** Get a blurred version of the range image using box filters on the provided integral image*/
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       
00710       /** Get a blurred version of the range image using box filters */
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       
00714       /** Get the squared euclidean distance between the two image points.
00715        *  Returns -INFINITY if one of the points was not observed */
00716       inline float
00717       getEuclideanDistanceSquared (int x1, int y1, int x2, int y2) const;
00718       //! Doing the above for some steps in the given direction and averaging
00719       inline float
00720       getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const;
00721       
00722       //! Project all points on the local plane approximation, thereby smoothing the surface of the scan
00723       PCL_EXPORTS void
00724       getRangeImageWithSmoothedSurface (int radius, RangeImage& smoothed_range_image) const;
00725       //void getLocalNormals (int radius) const;
00726       
00727       /** Calculates the average 3D position of the no_of_points points described by the start
00728        *  point x,y in the direction delta.
00729        *  Returns a max range point (range=INFINITY) if the first point is max range and an
00730        *  unobserved point (range=-INFINITY) if non of the points is observed. */
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       
00735       /** Calculates the overlap of two range images given the relative transformation
00736        *  (from the given image to *this) */
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       
00741       /** Get the viewing direction for the given point */
00742       inline bool
00743       getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const;
00744       
00745       /** Get the viewing direction for the given point */
00746       inline void
00747       getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const;
00748       
00749       /** Return a newly created Range image.
00750        *  Can be reimplmented in derived classes like RangeImagePlanar to return an image of the same type. */
00751       PCL_EXPORTS virtual RangeImage* 
00752       getNew () const { return new RangeImage; }
00753 
00754       /** Copy other to *this. Necessary for use in virtual functions that need to copy derived RangeImage classes (like RangeImagePlanar) */
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; /**< Just for... well... debugging purposes. :-) */
00768       
00769     protected:
00770       // =====PROTECTED MEMBER VARIABLES=====
00771       Eigen::Affine3f to_range_image_system_;  /**< Inverse of to_world_system_ */
00772       Eigen::Affine3f to_world_system_;        /**< Inverse of to_range_image_system_ */
00773       float angular_resolution_x_;             /**< Angular resolution of the range image in x direction in radians per pixel */
00774       float angular_resolution_y_;             /**< Angular resolution of the range image in y direction in radians per pixel */
00775       float angular_resolution_x_reciprocal_;  /**< 1.0/angular_resolution_x_ - provided for better performace of
00776                                                 *   multiplication compared to division */
00777       float angular_resolution_y_reciprocal_;  /**< 1.0/angular_resolution_y_ - provided for better performace of
00778                                                 *   multiplication compared to division */
00779       int image_offset_x_, image_offset_y_;    /**< Position of the top left corner of the range image compared to
00780                                                 *   an image of full size (360x180 degrees) */
00781       PointWithRange unobserved_point;         /**< This point is used to be able to return
00782                                                 *   a reference to a non-existing point */
00783       
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;
00792       /** Create lookup tables for trigonometric functions */
00793       static void
00794       createLookupTables ();
00795 
00796       /** Query the asin lookup table */
00797       static inline float
00798       asinLookUp (float value);
00799       
00800       /** Query the atan2 lookup table */
00801       static inline float
00802       atan2LookUp (float y, float x);
00803      
00804       /** Query the cos lookup table */
00805       static inline float
00806       cosLookUp (float value);
00807 
00808 
00809     public:
00810       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00811   };
00812 
00813   /**
00814     * /ingroup range_image
00815     */
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_