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