Point Cloud Library (PCL)  1.9.1-dev
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 
43 #include "pcl/pcl_config.h"
44 #include <pcl/io/grabber.h>
45 #include <pcl/io/file_grabber.h>
46 #include <pcl/common/time_trigger.h>
47 #include <string>
48 #include <vector>
49 #include <pcl/conversions.h>
50 
51 namespace pcl
52 {
53  /** \brief Base class for Image file grabber.
54  * \ingroup io
55  */
56  class PCL_EXPORTS ImageGrabberBase : public Grabber
57  {
58  public:
59  /** \brief Constructor taking a folder of depth+[rgb] images.
60  * \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]
61  * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list.
62  * \param[in] repeat whether to play PCD file in an endless loop or not.
63  * \param pclzf_mode
64  */
65  ImageGrabberBase (const std::string& directory, float frames_per_second, bool repeat, bool pclzf_mode);
66 
67  ImageGrabberBase (const std::string& depth_directory, const std::string& rgb_directory, float frames_per_second, bool repeat);
68  /** \brief Constructor taking a list of paths to PCD files, that are played in the order they appear in the list.
69  * \param[in] depth_image_files Path to the depth image files files.
70  * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list.
71  * \param[in] repeat whether to play PCD file in an endless loop or not.
72  */
73  ImageGrabberBase (const std::vector<std::string>& depth_image_files, float frames_per_second, bool repeat);
74 
75  /** \brief Copy constructor.
76  * \param[in] src the Image Grabber base object to copy into this
77  */
78  ImageGrabberBase (const ImageGrabberBase &src) : impl_ ()
79  {
80  *this = src;
81  }
82 
83  /** \brief Copy operator.
84  * \param[in] src the Image Grabber base object to copy into this
85  */
87  operator = (const ImageGrabberBase &src)
88  {
89  impl_ = src.impl_;
90  return (*this);
91  }
92 
93  /** \brief Virtual destructor. */
94  ~ImageGrabberBase () throw ();
95 
96  /** \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. */
97  void
98  start () override;
99 
100  /** \brief Stops playing the list of PCD files if frames_per_second is > 0. Otherwise the method has no effect. */
101  void
102  stop () override;
103 
104  /** \brief Triggers a callback with new data */
105  virtual void
106  trigger ();
107 
108  /** \brief whether the grabber is started (publishing) or not.
109  * \return true only if publishing.
110  */
111  bool
112  isRunning () const override;
113 
114  /** \return The name of the grabber */
115  std::string
116  getName () const override;
117 
118  /** \brief Rewinds to the first PCD file in the list.*/
119  virtual void
120  rewind ();
121 
122  /** \brief Returns the frames_per_second. 0 if grabber is trigger-based */
123  float
124  getFramesPerSecond () const override;
125 
126  /** \brief Returns whether the repeat flag is on */
127  bool
128  isRepeatOn () const;
129 
130  /** \brief Returns if the last frame is reached */
131  bool
132  atLastFrame () const;
133 
134  /** \brief Returns the filename of the current indexed file */
135  std::string
136  getCurrentDepthFileName () const;
137 
138  /** \brief Returns the filename of the previous indexed file
139  * SDM: adding this back in, but is this useful, or confusing? */
140  std::string
141  getPrevDepthFileName () const;
142 
143  /** \brief Get the depth filename at a particular index */
144  std::string
145  getDepthFileNameAtIndex (size_t idx) const;
146 
147  /** \brief Query only the timestamp of an index, if it exists */
148  bool
149  getTimestampAtIndex (size_t idx, pcl::uint64_t &timestamp) const;
150 
151  /** \brief Manually set RGB image files.
152  * \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
153  */
154  void
155  setRGBImageFiles (const std::vector<std::string>& rgb_image_files);
156 
157  /** \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.
158  * \param[in] focal_length_x Horizontal focal length (fx)
159  * \param[in] focal_length_y Vertical focal length (fy)
160  * \param[in] principal_point_x Horizontal coordinates of the principal point (cx)
161  * \param[in] principal_point_y Vertical coordinates of the principal point (cy)
162  */
163  virtual void
164  setCameraIntrinsics (const double focal_length_x,
165  const double focal_length_y,
166  const double principal_point_x,
167  const double principal_point_y);
168 
169  /** \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.
170  * \param[out] focal_length_x Horizontal focal length (fx)
171  * \param[out] focal_length_y Vertical focal length (fy)
172  * \param[out] principal_point_x Horizontal coordinates of the principal point (cx)
173  * \param[out] principal_point_y Vertical coordinates of the principal point (cy)
174  */
175  virtual void
176  getCameraIntrinsics (double &focal_length_x,
177  double &focal_length_y,
178  double &principal_point_x,
179  double &principal_point_y) const;
180 
181  /** \brief Define the units the depth data is stored in.
182  * Defaults to mm (0.001), meaning a brightness of 1000 corresponds to 1 m*/
183  void
184  setDepthImageUnits (float units);
185 
186  /** \brief Set the number of threads, if we wish to use OpenMP for quicker cloud population.
187  * Note that for a standard (< 4 core) machine this is unlikely to yield a drastic speedup.*/
188  void
189  setNumberOfThreads (unsigned int nr_threads = 0);
190 
191  protected:
192  /** \brief Convenience function to see how many frames this consists of
193  */
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  ~ImageGrabber () throw () {}
233 
234  // Inherited from FileGrabber
235  const boost::shared_ptr< const pcl::PointCloud<PointT> >
236  operator[] (size_t idx) const override;
237 
238  // Inherited from FileGrabber
239  size_t
240  size () const override;
241 
242  protected:
243  void
244  publish (const pcl::PCLPointCloud2& blob,
245  const Eigen::Vector4f& origin,
246  const Eigen::Quaternionf& orientation) const override;
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 }
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:168
Base class for Image file grabber.
Definition: image_grabber.h:56
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:44
~ImageGrabber()
Empty destructor.
bool getCloudAt(size_t idx, pcl::PCLPointCloud2 &blob, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) const
Gets the cloud in ROS form at location idx.
Grabber interface for PCL 1.x device drivers.
Definition: grabber.h:57
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:427
const boost::shared_ptr< const pcl::PointCloud< PointT > > operator[](size_t idx) const override
operator[] Returns the idx-th cloud in the dataset, without bounds checking.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:420
size_t numFrames() const
Convenience function to see how many frames this consists of.
FileGrabber provides a container-style interface for grabbers which operate on fixed-size input...
Definition: file_grabber.h:53
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:422
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void publish(const pcl::PCLPointCloud2 &blob, const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation) const override
boost::signals2::signal< void(const boost::shared_ptr< const pcl::PointCloud< PointT > > &)> * signal_
ImageGrabber(const std::string &dir, float frames_per_second=0, bool repeat=false, bool pclzf_mode=false)
size_t size() const override
size Returns the number of clouds currently loaded by the grabber
ImageGrabberBase(const ImageGrabberBase &src)
Copy constructor.
Definition: image_grabber.h:78