Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/common/include/pcl/range_image/range_image_planar.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010, 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_PLANAR_H_
00039 #define PCL_RANGE_IMAGE_PLANAR_H_
00040 
00041 #include <pcl/range_image/range_image.h>
00042 
00043 namespace pcl
00044 {
00045   /** \brief @b RangeImagePlanar is derived from the original range image and differs from it because it's not a 
00046     * spherical projection, but using a projection plane (as normal cameras do), therefore being better applicable 
00047     * for range sensors that already provide a range image by themselves (stereo cameras, ToF-cameras), so that
00048     * a conversion to point cloud and then to a spherical range image becomes unnecessary.
00049     * \author Bastian Steder 
00050     * \ingroup range_image
00051     */
00052   class RangeImagePlanar : public RangeImage
00053   {
00054     public:
00055       // =====TYPEDEFS=====
00056       typedef RangeImage BaseClass;
00057       typedef boost::shared_ptr<RangeImagePlanar> Ptr;
00058       typedef boost::shared_ptr<const RangeImagePlanar> ConstPtr;
00059       
00060       // =====CONSTRUCTOR & DESTRUCTOR=====
00061       /** Constructor */
00062       PCL_EXPORTS RangeImagePlanar ();
00063       /** Destructor */
00064       PCL_EXPORTS virtual ~RangeImagePlanar ();
00065 
00066       /** Return a newly created RangeImagePlanar.
00067        *  Reimplmentation to return an image of the same type. */
00068       virtual RangeImage* 
00069       getNew () const { return new RangeImagePlanar; }
00070 
00071       /** Copy *this to other. Derived version - also copying additonal RangeImagePlanar members */
00072       PCL_EXPORTS virtual void
00073       copyTo (RangeImage& other) const;
00074       
00075       // =====PUBLIC METHODS=====
00076       /** \brief Get a boost shared pointer of a copy of this */
00077       inline Ptr 
00078       makeShared () { return Ptr (new RangeImagePlanar (*this)); } 
00079       
00080       /** \brief Create the image from an existing disparity image.
00081         * \param disparity_image the input disparity image data
00082         * \param di_width the disparity image width
00083         * \param di_height the disparity image height
00084         * \param focal_length the focal length of the primary camera that generated the disparity image
00085         * \param base_line the baseline of the stereo pair that generated the disparity image
00086         * \param desired_angular_resolution If this is set, the system will skip as many pixels as necessary to get as
00087         *         close to this angular resolution as possible while not going over this value (the density will not be
00088         *         lower than this value). The value is in radians per pixel. 
00089         */
00090       PCL_EXPORTS void
00091       setDisparityImage (const float* disparity_image, int di_width, int di_height,
00092                          float focal_length, float base_line, float desired_angular_resolution=-1);
00093       
00094       /** Create the image from an existing depth image.
00095         * \param depth_image the input depth image data as float values
00096         * \param di_width the disparity image width 
00097         * \param di_height the disparity image height
00098         * \param di_center_x the x-coordinate of the camera's center of projection
00099         * \param di_center_y the y-coordinate of the camera's center of projection
00100         * \param di_focal_length_x the camera's focal length in the horizontal direction
00101         * \param di_focal_length_y the camera's focal length in the vertical direction
00102         * \param desired_angular_resolution If this is set, the system will skip as many pixels as necessary to get as
00103         *         close to this angular resolution as possible while not going over this value (the density will not be
00104         *         lower than this value). The value is in radians per pixel.
00105         */
00106       PCL_EXPORTS void
00107       setDepthImage (const float* depth_image, int di_width, int di_height, float di_center_x, float di_center_y,
00108                      float di_focal_length_x, float di_focal_length_y, float desired_angular_resolution=-1);
00109       
00110       /** Create the image from an existing depth image.
00111         * \param depth_image the input disparity image data as short values describing millimeters
00112         * \param di_width the disparity image width 
00113         * \param di_height the disparity image height
00114         * \param di_center_x the x-coordinate of the camera's center of projection
00115         * \param di_center_y the y-coordinate of the camera's center of projection
00116         * \param di_focal_length_x the camera's focal length in the horizontal direction
00117         * \param di_focal_length_y the camera's focal length in the vertical direction
00118         * \param desired_angular_resolution If this is set, the system will skip as many pixels as necessary to get as
00119         *         close to this angular resolution as possible while not going over this value (the density will not be
00120         *         lower than this value). The value is in radians per pixel.
00121         */
00122       PCL_EXPORTS void
00123       setDepthImage (const unsigned short* depth_image, int di_width, int di_height, float di_center_x, float di_center_y,
00124                      float di_focal_length_x, float di_focal_length_y, float desired_angular_resolution=-1);
00125       
00126       /** Create the image from an existing point cloud.
00127         * \param point_cloud the source point cloud
00128         * \param di_width the disparity image width 
00129         * \param di_height the disparity image height
00130         * \param di_center_x the x-coordinate of the camera's center of projection
00131         * \param di_center_y the y-coordinate of the camera's center of projection
00132         * \param di_focal_length_x the camera's focal length in the horizontal direction
00133         * \param di_focal_length_y the camera's focal length in the vertical direction
00134         * \param sensor_pose the pose of the virtual depth camera
00135         * \param coordinate_frame the used coordinate frame of the point cloud
00136         * \param noise_level what is the typical noise of the sensor - is used for averaging in the z-buffer
00137         * \param min_range minimum range to consifder points
00138         */
00139       template <typename PointCloudType> void
00140       createFromPointCloudWithFixedSize (const PointCloudType& point_cloud,
00141                                          int di_width, int di_height, float di_center_x, float di_center_y,
00142                                          float di_focal_length_x, float di_focal_length_y,
00143                                          const Eigen::Affine3f& sensor_pose,
00144                                          CoordinateFrame coordinate_frame=CAMERA_FRAME, float noise_level=0.0f,
00145                                          float min_range=0.0f);
00146       
00147       // Since we reimplement some of these overloaded functions, we have to do the following:
00148       using RangeImage::calculate3DPoint;
00149       using RangeImage::getImagePoint;
00150       
00151       /** \brief Calculate the 3D point according to the given image point and range
00152         * \param image_x the x image position
00153         * \param image_y the y image position
00154         * \param range the range
00155         * \param point the resulting 3D point
00156         * \note Implementation according to planar range images (compared to spherical as in the original)
00157         */
00158       virtual inline void
00159       calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const;
00160       
00161       /** \brief Calculate the image point and range from the given 3D point
00162         * \param point the resulting 3D point
00163         * \param image_x the resulting x image position
00164         * \param image_y the resulting y image position
00165         * \param range the resulting range
00166         * \note Implementation according to planar range images (compared to spherical as in the original)
00167         */
00168       virtual inline void 
00169       getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const;
00170       
00171       /** Get a sub part of the complete image as a new range image.
00172         * \param sub_image_image_offset_x - The x coordinate of the top left pixel of the sub image.
00173         *                         This is always according to absolute 0,0 meaning -180°,-90°
00174         *                         and it is already in the system of the new image, so the
00175         *                         actual pixel used in the original image is
00176         *                         combine_pixels* (image_offset_x-image_offset_x_)
00177         * \param sub_image_image_offset_y - Same as image_offset_x for the y coordinate
00178         * \param sub_image_width - width of the new image
00179         * \param sub_image_height - height of the new image
00180         * \param combine_pixels - shrinking factor, meaning the new angular resolution
00181         *                         is combine_pixels times the old one
00182         * \param sub_image - the output image
00183         */
00184       PCL_EXPORTS virtual void
00185       getSubImage (int sub_image_image_offset_x, int sub_image_image_offset_y, int sub_image_width,
00186                    int sub_image_height, int combine_pixels, RangeImage& sub_image) const;
00187       
00188       //! Get a range image with half the resolution
00189       PCL_EXPORTS virtual void 
00190       getHalfImage (RangeImage& half_image) const;
00191       
00192       //! Getter for the focal length in X
00193       inline float
00194       getFocalLengthX () const { return focal_length_x_; }
00195       
00196       //! Getter for the focal length in Y
00197       inline float
00198       getFocalLengthY () const { return focal_length_y_; }
00199       
00200       //! Getter for the principal point in X
00201       inline float
00202       getCenterX () const { return center_x_; }
00203       
00204       //! Getter for the principal point in Y
00205       inline float
00206       getCenterY () const { return center_y_; }
00207 
00208 
00209     protected:
00210       float focal_length_x_, focal_length_y_; //!< The focal length of the image in pixels
00211       float focal_length_x_reciprocal_, focal_length_y_reciprocal_;  //!< 1/focal_length -> for internal use
00212       float center_x_, center_y_;      //!< The principle point of the image
00213   };
00214 }  // namespace end
00215 
00216 
00217 #include <pcl/range_image/impl/range_image_planar.hpp>  // Definitions of templated and inline functions
00218 
00219 #endif  //#ifndef PCL_RANGE_IMAGE_H_