Point Cloud Library (PCL)  1.8.1
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  * \param pclzf_mode
66  */
67  ImageGrabberBase (const std::string& directory, float frames_per_second, bool repeat, bool pclzf_mode);
68 
69  ImageGrabberBase (const std::string& depth_directory, const std::string& rgb_directory, float frames_per_second, bool repeat);
70  /** \brief Constructor taking a list of paths to PCD files, that are played in the order they appear in the list.
71  * \param[in] depth_image_files Path to the depth image files files.
72  * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list.
73  * \param[in] repeat whether to play PCD file in an endless loop or not.
74  */
75  ImageGrabberBase (const std::vector<std::string>& depth_image_files, float frames_per_second, bool repeat);
76 
77  /** \brief Copy constructor.
78  * \param[in] src the Image Grabber base object to copy into this
79  */
80  ImageGrabberBase (const ImageGrabberBase &src) : Grabber (), impl_ ()
81  {
82  *this = src;
83  }
84 
85  /** \brief Copy operator.
86  * \param[in] src the Image Grabber base object to copy into this
87  */
89  operator = (const ImageGrabberBase &src)
90  {
91  impl_ = src.impl_;
92  return (*this);
93  }
94 
95  /** \brief Virtual destructor. */
96  virtual ~ImageGrabberBase () throw ();
97 
98  /** \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. */
99  virtual void
100  start ();
101 
102  /** \brief Stops playing the list of PCD files if frames_per_second is > 0. Otherwise the method has no effect. */
103  virtual void
104  stop ();
105 
106  /** \brief Triggers a callback with new data */
107  virtual void
108  trigger ();
109 
110  /** \brief whether the grabber is started (publishing) or not.
111  * \return true only if publishing.
112  */
113  virtual bool
114  isRunning () const;
115 
116  /** \return The name of the grabber */
117  virtual std::string
118  getName () const;
119 
120  /** \brief Rewinds to the first PCD file in the list.*/
121  virtual void
122  rewind ();
123 
124  /** \brief Returns the frames_per_second. 0 if grabber is trigger-based */
125  virtual float
126  getFramesPerSecond () const;
127 
128  /** \brief Returns whether the repeat flag is on */
129  bool
130  isRepeatOn () const;
131 
132  /** \brief Returns if the last frame is reached */
133  bool
134  atLastFrame () const;
135 
136  /** \brief Returns the filename of the current indexed file */
137  std::string
138  getCurrentDepthFileName () const;
139 
140  /** \brief Returns the filename of the previous indexed file
141  * SDM: adding this back in, but is this useful, or confusing? */
142  std::string
143  getPrevDepthFileName () const;
144 
145  /** \brief Get the depth filename at a particular index */
146  std::string
147  getDepthFileNameAtIndex (size_t idx) const;
148 
149  /** \brief Query only the timestamp of an index, if it exists */
150  bool
151  getTimestampAtIndex (size_t idx, pcl::uint64_t &timestamp) const;
152 
153  /** \brief Manually set RGB image files.
154  * \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
155  */
156  void
157  setRGBImageFiles (const std::vector<std::string>& rgb_image_files);
158 
159  /** \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.
160  * \param[in] focal_length_x Horizontal focal length (fx)
161  * \param[in] focal_length_y Vertical focal length (fy)
162  * \param[in] principal_point_x Horizontal coordinates of the principal point (cx)
163  * \param[in] principal_point_y Vertical coordinates of the principal point (cy)
164  */
165  virtual void
166  setCameraIntrinsics (const double focal_length_x,
167  const double focal_length_y,
168  const double principal_point_x,
169  const double principal_point_y);
170 
171  /** \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.
172  * \param[out] focal_length_x Horizontal focal length (fx)
173  * \param[out] focal_length_y Vertical focal length (fy)
174  * \param[out] principal_point_x Horizontal coordinates of the principal point (cx)
175  * \param[out] principal_point_y Vertical coordinates of the principal point (cy)
176  */
177  virtual void
178  getCameraIntrinsics (double &focal_length_x,
179  double &focal_length_y,
180  double &principal_point_x,
181  double &principal_point_y) const;
182 
183  /** \brief Define the units the depth data is stored in.
184  * Defaults to mm (0.001), meaning a brightness of 1000 corresponds to 1 m*/
185  void
186  setDepthImageUnits (float units);
187 
188  /** \brief Set the number of threads, if we wish to use OpenMP for quicker cloud population.
189  * Note that for a standard (< 4 core) machine this is unlikely to yield a drastic speedup.*/
190  void
191  setNumberOfThreads (unsigned int nr_threads = 0);
192 
193  protected:
194  /** \brief Convenience function to see how many frames this consists of
195  */
196  size_t
197  numFrames () const;
198 
199  /** \brief Gets the cloud in ROS form at location idx */
200  bool
201  getCloudAt (size_t idx, pcl::PCLPointCloud2 &blob, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) const;
202 
203 
204  private:
205  virtual void
206  publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const = 0;
207 
208 
209  // to separate and hide the implementation from interface: PIMPL
210  struct ImageGrabberImpl;
211  ImageGrabberImpl* impl_;
212  };
213 
214  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
215  template <typename T> class PointCloud;
216  template <typename PointT> class ImageGrabber : public ImageGrabberBase, public FileGrabber<PointT>
217  {
218  public:
219  ImageGrabber (const std::string& dir,
220  float frames_per_second = 0,
221  bool repeat = false,
222  bool pclzf_mode = false);
223 
224  ImageGrabber (const std::string& depth_dir,
225  const std::string& rgb_dir,
226  float frames_per_second = 0,
227  bool repeat = false);
228 
229  ImageGrabber (const std::vector<std::string>& depth_image_files,
230  float frames_per_second = 0,
231  bool repeat = false);
232 
233  /** \brief Empty destructor */
234  virtual ~ImageGrabber () throw () {}
235 
236  // Inherited from FileGrabber
237  const boost::shared_ptr< const pcl::PointCloud<PointT> >
238  operator[] (size_t idx) const;
239 
240  // Inherited from FileGrabber
241  size_t
242  size () const;
243 
244  protected:
245  virtual void
246  publish (const pcl::PCLPointCloud2& blob,
247  const Eigen::Vector4f& origin,
248  const Eigen::Quaternionf& orientation) const;
249  boost::signals2::signal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>* signal_;
250  };
251 
252  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
253  template<typename PointT>
254  ImageGrabber<PointT>::ImageGrabber (const std::string& dir,
255  float frames_per_second,
256  bool repeat,
257  bool pclzf_mode)
258  : ImageGrabberBase (dir, frames_per_second, repeat, pclzf_mode)
259  {
260  signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
261  }
262 
263  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
264  template<typename PointT>
265  ImageGrabber<PointT>::ImageGrabber (const std::string& depth_dir,
266  const std::string& rgb_dir,
267  float frames_per_second,
268  bool repeat)
269  : ImageGrabberBase (depth_dir, rgb_dir, frames_per_second, repeat)
270  {
271  signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
272  }
273 
274  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
275  template<typename PointT>
276  ImageGrabber<PointT>::ImageGrabber (const std::vector<std::string>& depth_image_files,
277  float frames_per_second,
278  bool repeat)
279  : ImageGrabberBase (depth_image_files, frames_per_second, repeat), signal_ ()
280  {
281  signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
282  }
283 
284  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
285  template<typename PointT> const boost::shared_ptr< const pcl::PointCloud<PointT> >
287  {
288  pcl::PCLPointCloud2 blob;
289  Eigen::Vector4f origin;
290  Eigen::Quaternionf orientation;
291  getCloudAt (idx, blob, origin, orientation);
293  pcl::fromPCLPointCloud2 (blob, *cloud);
294  cloud->sensor_origin_ = origin;
295  cloud->sensor_orientation_ = orientation;
296  return (cloud);
297  }
298 
299  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
300  template <typename PointT> size_t
302  {
303  return (numFrames ());
304  }
305 
306  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
307  template<typename PointT> void
308  ImageGrabber<PointT>::publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const
309  {
311  pcl::fromPCLPointCloud2 (blob, *cloud);
312  cloud->sensor_origin_ = origin;
313  cloud->sensor_orientation_ = orientation;
314 
315  signal_->operator () (cloud);
316  }
317 }
318 #endif
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
Definition: conversions.h:169
size_t size() const
size Returns the number of clouds currently loaded by the grabber
Base class for Image file grabber.
Definition: image_grabber.h:58
Grabber interface for PCL 1.x device drivers.
Definition: grabber.h:58
const boost::shared_ptr< const pcl::PointCloud< PointT > > operator[](size_t idx) const
operator[] Returns the idx-th cloud in the dataset, without bounds checking.
Definition: bfgs.h:10
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:421
virtual ~ImageGrabber()
Empty destructor.
boost::signals2::signal< T > * createSignal()
Definition: grabber.h:221
FileGrabber provides a container-style interface for grabbers which operate on fixed-size input.
Definition: file_grabber.h:55
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:423
virtual void publish(const pcl::PCLPointCloud2 &blob, const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation) const
PointCloud represents the base class in PCL for storing collections of 3D points.
boost::signals2::signal< void(const boost::shared_ptr< const pcl::PointCloud< PointT > > &)> * signal_
A point structure representing Euclidean xyz coordinates, and the RGB color.
ImageGrabber(const std::string &dir, float frames_per_second=0, bool repeat=false, bool pclzf_mode=false)
ImageGrabberBase(const ImageGrabberBase &src)
Copy constructor.
Definition: image_grabber.h:80