Point Cloud Library (PCL)  1.7.0
image_grabber.h
1 
2 /*
3  * Software License Agreement (BSD License)
4  *
5  * Point Cloud Library (PCL) - www.pointclouds.org
6  * Copyright (c) 2010-2011, Willow Garage, Inc.
7  * Copyright (c) 2012-, Open Perception, Inc.
8  *
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * * Redistributions of source code must retain the above copyright
16  * notice, this list of conditions and the following disclaimer.
17  * * Redistributions in binary form must reproduce the above
18  * copyright notice, this list of conditions and the following
19  * disclaimer in the documentation and/or other materials provided
20  * with the distribution.
21  * * Neither the name of the copyright holder(s) nor the names of its
22  * contributors may be used to endorse or promote products derived
23  * from this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  *
38  *
39  */
40 
41 #pragma once
42 #ifndef __PCL_IO_IMAGE_GRABBER__
43 #define __PCL_IO_IMAGE_GRABBER__
44 
45 #include "pcl/pcl_config.h"
46 #include <pcl/io/grabber.h>
47 #include <pcl/io/file_grabber.h>
48 #include <pcl/common/time_trigger.h>
49 #include <string>
50 #include <vector>
51 #include <pcl/conversions.h>
52 
53 namespace pcl
54 {
55  /** \brief Base class for Image file grabber.
56  * \ingroup io
57  */
58  class PCL_EXPORTS ImageGrabberBase : public Grabber
59  {
60  public:
61  /** \brief Constructor taking a folder of depth+[rgb] images.
62  * \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]
63  * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list.
64  * \param[in] repeat whether to play PCD file in an endless loop or not.
65  */
66  ImageGrabberBase (const std::string& directory, float frames_per_second, bool repeat, bool pclzf_mode);
67 
68  ImageGrabberBase (const std::string& depth_directory, const std::string& rgb_directory, float frames_per_second, bool repeat);
69  /** \brief Constructor taking a list of paths to PCD files, that are played in the order they appear in the list.
70  * \param[in] depth_image_files Path to the depth image files files.
71  * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list.
72  * \param[in] repeat whether to play PCD file in an endless loop or not.
73  */
74  ImageGrabberBase (const std::vector<std::string>& depth_image_files, float frames_per_second, bool repeat);
75 
76  /** \brief Copy constructor.
77  * \param[in] src the Image Grabber base object to copy into this
78  */
79  ImageGrabberBase (const ImageGrabberBase &src) : Grabber (), impl_ ()
80  {
81  *this = src;
82  }
83 
84  /** \brief Copy operator.
85  * \param[in] src the Image Grabber base object to copy into this
86  */
88  operator = (const ImageGrabberBase &src)
89  {
90  impl_ = src.impl_;
91  return (*this);
92  }
93 
94  /** \brief Virtual destructor. */
95  virtual ~ImageGrabberBase () throw ();
96 
97  /** \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. */
98  virtual void
99  start ();
100 
101  /** \brief Stops playing the list of PCD files if frames_per_second is > 0. Otherwise the method has no effect. */
102  virtual void
103  stop ();
104 
105  /** \brief Triggers a callback with new data */
106  virtual void
107  trigger ();
108 
109  /** \brief whether the grabber is started (publishing) or not.
110  * \return true only if publishing.
111  */
112  virtual bool
113  isRunning () const;
114 
115  /** \return The name of the grabber */
116  virtual std::string
117  getName () const;
118 
119  /** \brief Rewinds to the first PCD file in the list.*/
120  virtual void
121  rewind ();
122 
123  /** \brief Returns the frames_per_second. 0 if grabber is trigger-based */
124  virtual float
125  getFramesPerSecond () const;
126 
127  /** \brief Returns whether the repeat flag is on */
128  bool
129  isRepeatOn () const;
130 
131  /** \brief Returns if the last frame is reached */
132  bool
133  atLastFrame () const;
134 
135  /** \brief Returns the filename of the current indexed file */
136  std::string
137  getCurrentDepthFileName () const;
138 
139  /** \brief Returns the filename of the previous indexed file
140  * SDM: adding this back in, but is this useful, or confusing? */
141  std::string
142  getPrevDepthFileName () const;
143 
144  /** \brief Get the depth filename at a particular index */
145  std::string
146  getDepthFileNameAtIndex (size_t idx) const;
147 
148  /** \brief Query only the timestamp of an index, if it exists */
149  bool
150  getTimestampAtIndex (size_t idx, uint64_t &timestamp) const;
151 
152  /** \brief Manually set RGB image files.
153  * \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
154  */
155  void
156  setRGBImageFiles (const std::vector<std::string>& rgb_image_files);
157 
158  /** \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.
159  * \param[in] focal_length_x Horizontal focal length (fx)
160  * \param[in] focal_length_y Vertical focal length (fy)
161  * \param[in] principal_point_x Horizontal coordinates of the principal point (cx)
162  * \param[in] principal_point_y Vertical coordinates of the principal point (cy)
163  */
164  virtual void
165  setCameraIntrinsics (const double focal_length_x,
166  const double focal_length_y,
167  const double principal_point_x,
168  const double principal_point_y);
169 
170  /** \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.
171  * \param[out] focal_length_x Horizontal focal length (fx)
172  * \param[out] focal_length_y Vertical focal length (fy)
173  * \param[out] principal_point_x Horizontal coordinates of the principal point (cx)
174  * \param[out] principal_point_y Vertical coordinates of the principal point (cy)
175  */
176  virtual void
177  getCameraIntrinsics (double &focal_length_x,
178  double &focal_length_y,
179  double &principal_point_x,
180  double &principal_point_y) const;
181 
182  /** \brief Define the units the depth data is stored in.
183  * Defaults to mm (0.001), meaning a brightness of 1000 corresponds to 1 m*/
184  void
185  setDepthImageUnits (float units);
186 
187  /** \brief Set the number of threads, if we wish to use OpenMP for quicker cloud population.
188  * Note that for a standard (< 4 core) machine this is unlikely to yield a drastic speedup.*/
189  void
190  setNumberOfThreads (unsigned int nr_threads = 0);
191 
192  protected:
193  /** \brief Convenience function to see how many frames this consists of */
194  size_t
195  numFrames () const;
196 
197  /** \brief Gets the cloud in ROS form at location idx */
198  bool
199  getCloudAt (size_t idx, pcl::PCLPointCloud2 &blob, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) const;
200 
201 
202  private:
203  virtual void
204  publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const = 0;
205 
206 
207  // to separate and hide the implementation from interface: PIMPL
208  struct ImageGrabberImpl;
209  ImageGrabberImpl* impl_;
210  };
211 
212  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
213  template <typename T> class PointCloud;
214  template <typename PointT> class ImageGrabber : public ImageGrabberBase, public FileGrabber<PointT>
215  {
216  public:
217  ImageGrabber (const std::string& dir,
218  float frames_per_second = 0,
219  bool repeat = false,
220  bool pclzf_mode = false);
221 
222  ImageGrabber (const std::string& depth_dir,
223  const std::string& rgb_dir,
224  float frames_per_second = 0,
225  bool repeat = false);
226 
227  ImageGrabber (const std::vector<std::string>& depth_image_files,
228  float frames_per_second = 0,
229  bool repeat = false);
230 
231  /** \brief Empty destructor */
232  virtual ~ImageGrabber () throw () {}
233 
234  // Inherited from FileGrabber
235  const boost::shared_ptr< const pcl::PointCloud<PointT> >
236  operator[] (size_t idx) const;
237 
238  // Inherited from FileGrabber
239  size_t
240  size () const;
241 
242  protected:
243  virtual void
244  publish (const pcl::PCLPointCloud2& blob,
245  const Eigen::Vector4f& origin,
246  const Eigen::Quaternionf& orientation) const;
247  boost::signals2::signal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>* signal_;
248  };
249 
250  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
251  template<typename PointT>
252  ImageGrabber<PointT>::ImageGrabber (const std::string& dir,
253  float frames_per_second,
254  bool repeat,
255  bool pclzf_mode)
256  : ImageGrabberBase (dir, frames_per_second, repeat, pclzf_mode)
257  {
258  signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
259  }
260 
261  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
262  template<typename PointT>
263  ImageGrabber<PointT>::ImageGrabber (const std::string& depth_dir,
264  const std::string& rgb_dir,
265  float frames_per_second,
266  bool repeat)
267  : ImageGrabberBase (depth_dir, rgb_dir, frames_per_second, repeat)
268  {
269  signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
270  }
271 
272  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
273  template<typename PointT>
274  ImageGrabber<PointT>::ImageGrabber (const std::vector<std::string>& depth_image_files,
275  float frames_per_second,
276  bool repeat)
277  : ImageGrabberBase (depth_image_files, frames_per_second, repeat), signal_ ()
278  {
279  signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
280  }
281 
282  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
283  template<typename PointT> const boost::shared_ptr< const pcl::PointCloud<PointT> >
285  {
286  pcl::PCLPointCloud2 blob;
287  Eigen::Vector4f origin;
288  Eigen::Quaternionf orientation;
289  getCloudAt (idx, blob, origin, orientation);
291  pcl::fromPCLPointCloud2 (blob, *cloud);
292  cloud->sensor_origin_ = origin;
293  cloud->sensor_orientation_ = orientation;
294  return (cloud);
295  }
296 
297  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
298  template <typename PointT> size_t
300  {
301  return (numFrames ());
302  }
303 
304  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
305  template<typename PointT> void
306  ImageGrabber<PointT>::publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const
307  {
309  pcl::fromPCLPointCloud2 (blob, *cloud);
310  cloud->sensor_origin_ = origin;
311  cloud->sensor_orientation_ = orientation;
312 
313  signal_->operator () (cloud);
314  }
315 }
316 #endif