Point Cloud Library (PCL)
1.7.0
|
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 ×tamp) 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