Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/io/include/pcl/io/image_grabber.h
00001 
00002 /*
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Point Cloud Library (PCL) - www.pointclouds.org
00006  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00007  *  Copyright (c) 2012-, Open Perception, Inc.
00008  *
00009  *  All rights reserved.
00010  *
00011  *  Redistribution and use in source and binary forms, with or without
00012  *  modification, are permitted provided that the following conditions
00013  *  are met:
00014  *
00015  *   * Redistributions of source code must retain the above copyright
00016  *     notice, this list of conditions and the following disclaimer.
00017  *   * Redistributions in binary form must reproduce the above
00018  *     copyright notice, this list of conditions and the following
00019  *     disclaimer in the documentation and/or other materials provided
00020  *     with the distribution.
00021  *   * Neither the name of the copyright holder(s) nor the names of its
00022  *     contributors may be used to endorse or promote products derived
00023  *     from this software without specific prior written permission.
00024  *
00025  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00026  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00027  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00028  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00029  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00030  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00031  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00032  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00033  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00034  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00035  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00036  *  POSSIBILITY OF SUCH DAMAGE.
00037  *
00038  *
00039  */
00040 
00041 #pragma once
00042 #ifndef __PCL_IO_IMAGE_GRABBER__
00043 #define __PCL_IO_IMAGE_GRABBER__
00044 
00045 #include "pcl/pcl_config.h"
00046 #include <pcl/io/grabber.h>
00047 #include <pcl/io/file_grabber.h>
00048 #include <pcl/common/time_trigger.h>
00049 #include <string>
00050 #include <vector>
00051 #include <pcl/conversions.h>
00052 
00053 namespace pcl
00054 {
00055   /** \brief Base class for Image file grabber.
00056    * \ingroup io
00057    */
00058   class PCL_EXPORTS ImageGrabberBase : public Grabber
00059   {
00060     public:
00061     /** \brief Constructor taking a folder of depth+[rgb] images.
00062      * \param[in] directory Directory which contains an ordered set of images corresponding to an [RGB]D video, stored as TIFF, PNG, JPG, or PPM files. The naming convention is: frame_[timestamp]_["depth"/"rgb"].[extension]
00063      * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list.
00064      * \param[in] repeat whether to play PCD file in an endless loop or not.
00065      */
00066     ImageGrabberBase (const std::string& directory, float frames_per_second, bool repeat, bool pclzf_mode);
00067 
00068     ImageGrabberBase (const std::string& depth_directory, const std::string& rgb_directory, float frames_per_second, bool repeat);
00069     /** \brief Constructor taking a list of paths to PCD files, that are played in the order they appear in the list.
00070      * \param[in] depth_image_files Path to the depth image files files.
00071      * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list.
00072      * \param[in] repeat whether to play PCD file in an endless loop or not.
00073      */
00074     ImageGrabberBase (const std::vector<std::string>& depth_image_files, float frames_per_second, bool repeat);
00075 
00076     /** \brief Copy constructor.
00077      * \param[in] src the Image Grabber base object to copy into this
00078      */
00079     ImageGrabberBase (const ImageGrabberBase &src) : Grabber (), impl_ ()
00080     {
00081       *this = src;
00082     }
00083 
00084     /** \brief Copy operator.
00085      * \param[in] src the Image Grabber base object to copy into this
00086      */
00087     ImageGrabberBase&
00088     operator = (const ImageGrabberBase &src)
00089     {
00090       impl_ = src.impl_;
00091       return (*this);
00092     }
00093 
00094     /** \brief Virtual destructor. */
00095     virtual ~ImageGrabberBase () throw ();
00096 
00097     /** \brief Starts playing the list of PCD files if frames_per_second is > 0. Otherwise it works as a trigger: publishes only the next PCD file in the list. */
00098     virtual void 
00099     start ();
00100       
00101     /** \brief Stops playing the list of PCD files if frames_per_second is > 0. Otherwise the method has no effect. */
00102     virtual void 
00103     stop ();
00104       
00105     /** \brief Triggers a callback with new data */
00106     virtual void 
00107     trigger ();
00108 
00109     /** \brief whether the grabber is started (publishing) or not.
00110      * \return true only if publishing.
00111      */
00112     virtual bool 
00113     isRunning () const;
00114       
00115     /** \return The name of the grabber */
00116     virtual std::string 
00117     getName () const;
00118       
00119     /** \brief Rewinds to the first PCD file in the list.*/
00120     virtual void 
00121     rewind ();
00122 
00123     /** \brief Returns the frames_per_second. 0 if grabber is trigger-based */
00124     virtual float 
00125     getFramesPerSecond () const;
00126 
00127     /** \brief Returns whether the repeat flag is on */
00128     bool 
00129     isRepeatOn () const;
00130 
00131     /** \brief Returns if the last frame is reached */
00132     bool
00133     atLastFrame () const;
00134 
00135     /** \brief Returns the filename of the current indexed file */
00136     std::string
00137     getCurrentDepthFileName () const;
00138 
00139     /** \brief Returns the filename of the previous indexed file 
00140      *  SDM: adding this back in, but is this useful, or confusing? */
00141     std::string
00142     getPrevDepthFileName () const;
00143 
00144     /** \brief Get the depth filename at a particular index */
00145     std::string
00146     getDepthFileNameAtIndex (size_t idx) const;
00147 
00148     /** \brief Query only the timestamp of an index, if it exists */
00149     bool
00150     getTimestampAtIndex (size_t idx, uint64_t &timestamp) const;
00151 
00152     /** \brief Manually set RGB image files.
00153      * \param[in] rgb_image_files A vector of [tiff/png/jpg/ppm] files to use as input. There must be a 1-to-1 correspondence between these and the depth images you set
00154      */
00155     void
00156     setRGBImageFiles (const std::vector<std::string>& rgb_image_files);
00157 
00158     /** \brief Define custom focal length and center pixel. This will override ANY other setting of parameters for the duration of the grabber's life, whether by factory defaults or explicitly read from a frame_[timestamp].xml file. 
00159      *  \param[in] focal_length_x Horizontal focal length (fx)
00160      *  \param[in] focal_length_y Vertical focal length (fy)
00161      *  \param[in] principal_point_x Horizontal coordinates of the principal point (cx)
00162      *  \param[in] principal_point_y Vertical coordinates of the principal point (cy)
00163      */
00164     virtual void
00165     setCameraIntrinsics (const double focal_length_x, 
00166                          const double focal_length_y, 
00167                          const double principal_point_x, 
00168                          const double principal_point_y);
00169     
00170     /** \brief Get the current focal length and center pixel. If the intrinsics have been manually set with @setCameraIntrinsics@, this will return those values. Else, if start () has been called and the grabber has found a frame_[timestamp].xml file, this will return the most recent values read. Else, returns factory defaults.
00171      *  \param[out] focal_length_x Horizontal focal length (fx)
00172      *  \param[out] focal_length_y Vertical focal length (fy)
00173      *  \param[out] principal_point_x Horizontal coordinates of the principal point (cx)
00174      *  \param[out] principal_point_y Vertical coordinates of the principal point (cy)
00175      */
00176     virtual void
00177     getCameraIntrinsics (double &focal_length_x, 
00178                          double &focal_length_y, 
00179                          double &principal_point_x, 
00180                          double &principal_point_y) const;
00181 
00182     /** \brief Define the units the depth data is stored in.
00183      *  Defaults to mm (0.001), meaning a brightness of 1000 corresponds to 1 m*/
00184     void
00185     setDepthImageUnits (float units);
00186     
00187     /** \brief Set the number of threads, if we wish to use OpenMP for quicker cloud population.
00188      *  Note that for a standard (< 4 core) machine this is unlikely to yield a drastic speedup.*/
00189     void
00190     setNumberOfThreads (unsigned int nr_threads = 0);
00191 
00192     protected:
00193     /** \brief Convenience function to see how many frames this consists of */
00194     size_t
00195     numFrames () const;
00196     
00197     /** \brief Gets the cloud in ROS form at location idx */
00198     bool
00199     getCloudAt (size_t idx, pcl::PCLPointCloud2 &blob, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) const;
00200 
00201 
00202     private:
00203     virtual void 
00204     publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const = 0;
00205 
00206 
00207     // to separate and hide the implementation from interface: PIMPL
00208     struct ImageGrabberImpl;
00209     ImageGrabberImpl* impl_;
00210   };
00211 
00212   ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00213   template <typename T> class PointCloud;
00214   template <typename PointT> class ImageGrabber : public ImageGrabberBase, public FileGrabber<PointT>
00215   {
00216     public:
00217     ImageGrabber (const std::string& dir, 
00218                   float frames_per_second = 0, 
00219                   bool repeat = false, 
00220                   bool pclzf_mode = false);
00221 
00222     ImageGrabber (const std::string& depth_dir, 
00223                   const std::string& rgb_dir, 
00224                   float frames_per_second = 0, 
00225                   bool repeat = false);
00226 
00227     ImageGrabber (const std::vector<std::string>& depth_image_files, 
00228                   float frames_per_second = 0, 
00229                   bool repeat = false);
00230       
00231     /** \brief Empty destructor */
00232     virtual ~ImageGrabber () throw () {}
00233     
00234     // Inherited from FileGrabber
00235     const boost::shared_ptr< const pcl::PointCloud<PointT> >
00236     operator[] (size_t idx) const;
00237 
00238     // Inherited from FileGrabber
00239     size_t
00240     size () const;
00241 
00242     protected:
00243     virtual void 
00244     publish (const pcl::PCLPointCloud2& blob,
00245              const Eigen::Vector4f& origin, 
00246              const Eigen::Quaternionf& orientation) const;
00247     boost::signals2::signal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>* signal_;
00248   };
00249 
00250   ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00251   template<typename PointT>
00252   ImageGrabber<PointT>::ImageGrabber (const std::string& dir, 
00253                                       float frames_per_second, 
00254                                       bool repeat, 
00255                                       bool pclzf_mode)
00256     : ImageGrabberBase (dir, frames_per_second, repeat, pclzf_mode)
00257   {
00258     signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
00259   }
00260   
00261   ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00262   template<typename PointT>
00263   ImageGrabber<PointT>::ImageGrabber (const std::string& depth_dir, 
00264                                       const std::string& rgb_dir, 
00265                                       float frames_per_second, 
00266                                       bool repeat)
00267     : ImageGrabberBase (depth_dir, rgb_dir, frames_per_second, repeat)
00268   {
00269     signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
00270   }
00271 
00272   ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00273   template<typename PointT>
00274   ImageGrabber<PointT>::ImageGrabber (const std::vector<std::string>& depth_image_files, 
00275                                       float frames_per_second, 
00276                                       bool repeat)
00277     : ImageGrabberBase (depth_image_files, frames_per_second, repeat), signal_ ()
00278   {
00279     signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
00280   }
00281 
00282   ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00283   template<typename PointT> const boost::shared_ptr< const pcl::PointCloud<PointT> >
00284   ImageGrabber<PointT>::operator[] (size_t idx) const
00285   {
00286     pcl::PCLPointCloud2 blob;
00287     Eigen::Vector4f origin;
00288     Eigen::Quaternionf orientation;
00289     getCloudAt (idx, blob, origin, orientation);
00290     typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
00291     pcl::fromPCLPointCloud2 (blob, *cloud);
00292     cloud->sensor_origin_ = origin;
00293     cloud->sensor_orientation_ = orientation;
00294     return (cloud);
00295   }
00296 
00297   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
00298   template <typename PointT> size_t
00299   ImageGrabber<PointT>::size () const
00300   {
00301     return (numFrames ());
00302   }
00303 
00304   ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00305   template<typename PointT> void
00306   ImageGrabber<PointT>::publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const
00307   {
00308     typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
00309     pcl::fromPCLPointCloud2 (blob, *cloud);
00310     cloud->sensor_origin_ = origin;
00311     cloud->sensor_orientation_ = orientation;
00312 
00313     signal_->operator () (cloud);
00314   }
00315 }
00316 #endif