Point Cloud Library (PCL)
1.7.0
|
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 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 */ 00037 00038 #include <pcl/pcl_config.h> 00039 00040 #ifndef PCL_IO_PCD_GRABBER_H_ 00041 #define PCL_IO_PCD_GRABBER_H_ 00042 00043 #include <pcl/common/io.h> 00044 #include <pcl/io/grabber.h> 00045 #include <pcl/io/file_grabber.h> 00046 #include <pcl/common/time_trigger.h> 00047 #include <string> 00048 #include <vector> 00049 #include <pcl/conversions.h> 00050 00051 #ifdef HAVE_OPENNI 00052 #include <pcl/io/openni_camera/openni_image.h> 00053 #include <pcl/io/openni_camera/openni_image_rgb24.h> 00054 #include <pcl/io/openni_camera/openni_depth_image.h> 00055 #endif 00056 00057 namespace pcl 00058 { 00059 /** \brief Base class for PCD file grabber. 00060 * \ingroup io 00061 */ 00062 class PCL_EXPORTS PCDGrabberBase : public Grabber 00063 { 00064 public: 00065 /** \brief Constructor taking just one PCD file or one TAR file containing multiple PCD files. 00066 * \param[in] pcd_file path to the PCD file 00067 * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list. 00068 * \param[in] repeat whether to play PCD file in an endless loop or not. 00069 */ 00070 PCDGrabberBase (const std::string& pcd_file, float frames_per_second, bool repeat); 00071 00072 /** \brief Constructor taking a list of paths to PCD files, that are played in the order they appear in the list. 00073 * \param[in] pcd_files vector of paths to PCD files. 00074 * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list. 00075 * \param[in] repeat whether to play PCD file in an endless loop or not. 00076 */ 00077 PCDGrabberBase (const std::vector<std::string>& pcd_files, float frames_per_second, bool repeat); 00078 00079 /** \brief Copy constructor. 00080 * \param[in] src the PCD Grabber base object to copy into this 00081 */ 00082 PCDGrabberBase (const PCDGrabberBase &src) : Grabber (), impl_ () 00083 { 00084 *this = src; 00085 } 00086 00087 /** \brief Copy operator. 00088 * \param[in] src the PCD Grabber base object to copy into this 00089 */ 00090 PCDGrabberBase& 00091 operator = (const PCDGrabberBase &src) 00092 { 00093 impl_ = src.impl_; 00094 return (*this); 00095 } 00096 00097 /** \brief Virtual destructor. */ 00098 virtual ~PCDGrabberBase () throw (); 00099 00100 /** \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. */ 00101 virtual void 00102 start (); 00103 00104 /** \brief Stops playing the list of PCD files if frames_per_second is > 0. Otherwise the method has no effect. */ 00105 virtual void 00106 stop (); 00107 00108 /** \brief Triggers a callback with new data */ 00109 virtual void 00110 trigger (); 00111 00112 /** \brief Indicates whether the grabber is streaming or not. 00113 * \return true if grabber is started and hasn't run out of PCD files. 00114 */ 00115 virtual bool 00116 isRunning () const; 00117 00118 /** \return The name of the grabber */ 00119 virtual std::string 00120 getName () const; 00121 00122 /** \brief Rewinds to the first PCD file in the list.*/ 00123 virtual void 00124 rewind (); 00125 00126 /** \brief Returns the frames_per_second. 0 if grabber is trigger-based */ 00127 virtual float 00128 getFramesPerSecond () const; 00129 00130 /** \brief Returns whether the repeat flag is on */ 00131 bool 00132 isRepeatOn () const; 00133 00134 /** \brief Get cloud (in ROS form) at a particular location */ 00135 bool 00136 getCloudAt (size_t idx, 00137 pcl::PCLPointCloud2 &blob, 00138 Eigen::Vector4f &origin, 00139 Eigen::Quaternionf &orientation) const; 00140 00141 /** \brief Returns the size */ 00142 size_t 00143 numFrames () const; 00144 00145 private: 00146 virtual void 00147 publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const = 0; 00148 00149 // to separate and hide the implementation from interface: PIMPL 00150 struct PCDGrabberImpl; 00151 PCDGrabberImpl* impl_; 00152 }; 00153 00154 //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00155 template <typename T> class PointCloud; 00156 template <typename PointT> class PCDGrabber : public PCDGrabberBase, public FileGrabber<PointT> 00157 { 00158 public: 00159 PCDGrabber (const std::string& pcd_path, float frames_per_second = 0, bool repeat = false); 00160 PCDGrabber (const std::vector<std::string>& pcd_files, float frames_per_second = 0, bool repeat = false); 00161 00162 /** \brief Virtual destructor. */ 00163 virtual ~PCDGrabber () throw () {} 00164 00165 // Inherited from FileGrabber 00166 const boost::shared_ptr< const pcl::PointCloud<PointT> > 00167 operator[] (size_t idx) const; 00168 00169 // Inherited from FileGrabber 00170 size_t 00171 size () const; 00172 protected: 00173 00174 virtual void 00175 publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const; 00176 00177 boost::signals2::signal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>* signal_; 00178 00179 #ifdef HAVE_OPENNI 00180 boost::signals2::signal<void (const boost::shared_ptr<openni_wrapper::DepthImage>&)>* depth_image_signal_; 00181 boost::signals2::signal<void (const boost::shared_ptr<openni_wrapper::Image>&)>* image_signal_; 00182 boost::signals2::signal<void (const boost::shared_ptr<openni_wrapper::Image>&, const boost::shared_ptr<openni_wrapper::DepthImage>&, float constant)>* image_depth_image_signal_; 00183 #endif 00184 }; 00185 00186 //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00187 template<typename PointT> 00188 PCDGrabber<PointT>::PCDGrabber (const std::string& pcd_path, float frames_per_second, bool repeat) 00189 : PCDGrabberBase (pcd_path, frames_per_second, repeat) 00190 { 00191 signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>(); 00192 #ifdef HAVE_OPENNI 00193 depth_image_signal_ = createSignal <void (const boost::shared_ptr<openni_wrapper::DepthImage>&)> (); 00194 image_signal_ = createSignal <void (const boost::shared_ptr<openni_wrapper::Image>&)> (); 00195 image_depth_image_signal_ = createSignal <void (const boost::shared_ptr<openni_wrapper::Image>&, const boost::shared_ptr<openni_wrapper::DepthImage>&, float constant)> (); 00196 #endif 00197 } 00198 00199 //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00200 template<typename PointT> 00201 PCDGrabber<PointT>::PCDGrabber (const std::vector<std::string>& pcd_files, float frames_per_second, bool repeat) 00202 : PCDGrabberBase (pcd_files, frames_per_second, repeat), signal_ () 00203 { 00204 signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>(); 00205 #ifdef HAVE_OPENNI 00206 depth_image_signal_ = createSignal <void (const boost::shared_ptr<openni_wrapper::DepthImage>&)> (); 00207 image_signal_ = createSignal <void (const boost::shared_ptr<openni_wrapper::Image>&)> (); 00208 image_depth_image_signal_ = createSignal <void (const boost::shared_ptr<openni_wrapper::Image>&, const boost::shared_ptr<openni_wrapper::DepthImage>&, float constant)> (); 00209 #endif 00210 } 00211 00212 //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00213 template<typename PointT> const boost::shared_ptr< const pcl::PointCloud<PointT> > 00214 PCDGrabber<PointT>::operator[] (size_t idx) const 00215 { 00216 pcl::PCLPointCloud2 blob; 00217 Eigen::Vector4f origin; 00218 Eigen::Quaternionf orientation; 00219 getCloudAt (idx, blob, origin, orientation); 00220 typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ()); 00221 pcl::fromPCLPointCloud2 (blob, *cloud); 00222 cloud->sensor_origin_ = origin; 00223 cloud->sensor_orientation_ = orientation; 00224 return (cloud); 00225 } 00226 00227 /////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00228 template <typename PointT> size_t 00229 PCDGrabber<PointT>::size () const 00230 { 00231 return (numFrames ()); 00232 } 00233 00234 //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00235 template<typename PointT> void 00236 PCDGrabber<PointT>::publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const 00237 { 00238 typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ()); 00239 pcl::fromPCLPointCloud2 (blob, *cloud); 00240 cloud->sensor_origin_ = origin; 00241 cloud->sensor_orientation_ = orientation; 00242 00243 signal_->operator () (cloud); 00244 00245 #ifdef HAVE_OPENNI 00246 // If dataset is not organized, return 00247 if (!cloud->isOrganized ()) 00248 return; 00249 00250 boost::shared_ptr<xn::DepthMetaData> depth_meta_data (new xn::DepthMetaData); 00251 depth_meta_data->AllocateData (cloud->width, cloud->height); 00252 XnDepthPixel* depth_map = depth_meta_data->WritableData (); 00253 uint32_t k = 0; 00254 for (uint32_t i = 0; i < cloud->height; ++i) 00255 for (uint32_t j = 0; j < cloud->width; ++j) 00256 { 00257 depth_map[k] = static_cast<XnDepthPixel> ((*cloud)[k].z * 1000); 00258 ++k; 00259 } 00260 00261 boost::shared_ptr<openni_wrapper::DepthImage> depth_image (new openni_wrapper::DepthImage (depth_meta_data, 0.075f, 525, 0, 0)); 00262 if (depth_image_signal_->num_slots() > 0) 00263 depth_image_signal_->operator()(depth_image); 00264 00265 // ---[ RGB special case 00266 std::vector<pcl::PCLPointField> fields; 00267 int rgba_index = pcl::getFieldIndex (*cloud, "rgb", fields); 00268 if (rgba_index == -1) 00269 rgba_index = pcl::getFieldIndex (*cloud, "rgba", fields); 00270 if (rgba_index >= 0) 00271 { 00272 rgba_index = fields[rgba_index].offset; 00273 00274 boost::shared_ptr<xn::ImageMetaData> image_meta_data (new xn::ImageMetaData); 00275 image_meta_data->AllocateData (cloud->width, cloud->height, XN_PIXEL_FORMAT_RGB24); 00276 XnRGB24Pixel* image_map = image_meta_data->WritableRGB24Data (); 00277 k = 0; 00278 for (uint32_t i = 0; i < cloud->height; ++i) 00279 { 00280 for (uint32_t j = 0; j < cloud->width; ++j) 00281 { 00282 // Fill r/g/b data, assuming that the order is BGRA 00283 pcl::RGB rgb; 00284 memcpy (&rgb, reinterpret_cast<const char*> (&cloud->points[k]) + rgba_index, sizeof (RGB)); 00285 image_map[k].nRed = static_cast<XnUInt8> (rgb.r); 00286 image_map[k].nGreen = static_cast<XnUInt8> (rgb.g); 00287 image_map[k].nBlue = static_cast<XnUInt8> (rgb.b); 00288 ++k; 00289 } 00290 } 00291 00292 boost::shared_ptr<openni_wrapper::Image> image (new openni_wrapper::ImageRGB24 (image_meta_data)); 00293 if (image_signal_->num_slots() > 0) 00294 image_signal_->operator()(image); 00295 00296 if (image_depth_image_signal_->num_slots() > 0) 00297 image_depth_image_signal_->operator()(image, depth_image, 1.0f / 525.0f); 00298 } 00299 #endif 00300 } 00301 } 00302 #endif