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  */
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 (std::size_t idx) const;
149 
150  /** \brief Query only the timestamp of an index, if it exists */
151  bool
152  getTimestampAtIndex (std::size_t idx, std::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  std::size_t
198  numFrames () const;
199 
200  /** \brief Gets the cloud in ROS form at location idx */
201  bool
202  getCloudAt (std::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  using ConstPtr = boost::shared_ptr<const ImageGrabber>;
222 
223  ImageGrabber (const std::string& dir,
224  float frames_per_second = 0,
225  bool repeat = false,
226  bool pclzf_mode = false);
227 
228  ImageGrabber (const std::string& depth_dir,
229  const std::string& rgb_dir,
230  float frames_per_second = 0,
231  bool repeat = false);
232 
233  ImageGrabber (const std::vector<std::string>& depth_image_files,
234  float frames_per_second = 0,
235  bool repeat = false);
236 
237  /** \brief Empty destructor */
238  ~ImageGrabber () throw () {}
239 
240  // Inherited from FileGrabber
241  const typename pcl::PointCloud<PointT>::ConstPtr
242  operator[] (std::size_t idx) const override;
243 
244  // Inherited from FileGrabber
245  std::size_t
246  size () const override;
247 
248  protected:
249  void
250  publish (const pcl::PCLPointCloud2& blob,
251  const Eigen::Vector4f& origin,
252  const Eigen::Quaternionf& orientation) const override;
253  boost::signals2::signal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>* signal_;
254  };
255 
256  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
257  template<typename PointT>
258  ImageGrabber<PointT>::ImageGrabber (const std::string& dir,
259  float frames_per_second,
260  bool repeat,
261  bool pclzf_mode)
262  : ImageGrabberBase (dir, frames_per_second, repeat, pclzf_mode)
263  {
264  signal_ = createSignal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>();
265  }
266 
267  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
268  template<typename PointT>
269  ImageGrabber<PointT>::ImageGrabber (const std::string& depth_dir,
270  const std::string& rgb_dir,
271  float frames_per_second,
272  bool repeat)
273  : ImageGrabberBase (depth_dir, rgb_dir, frames_per_second, repeat)
274  {
275  signal_ = createSignal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>();
276  }
277 
278  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
279  template<typename PointT>
280  ImageGrabber<PointT>::ImageGrabber (const std::vector<std::string>& depth_image_files,
281  float frames_per_second,
282  bool repeat)
283  : ImageGrabberBase (depth_image_files, frames_per_second, repeat), signal_ ()
284  {
285  signal_ = createSignal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>();
286  }
287 
288  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
289  template<typename PointT> const typename pcl::PointCloud<PointT>::ConstPtr
290  ImageGrabber<PointT>::operator[] (std::size_t idx) const
291  {
292  pcl::PCLPointCloud2 blob;
293  Eigen::Vector4f origin;
294  Eigen::Quaternionf orientation;
295  getCloudAt (idx, blob, origin, orientation);
297  pcl::fromPCLPointCloud2 (blob, *cloud);
298  cloud->sensor_origin_ = origin;
299  cloud->sensor_orientation_ = orientation;
300  return (cloud);
301  }
302 
303  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
304  template <typename PointT> std::size_t
306  {
307  return (numFrames ());
308  }
309 
310  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
311  template<typename PointT> void
312  ImageGrabber<PointT>::publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const
313  {
315  pcl::fromPCLPointCloud2 (blob, *cloud);
316  cloud->sensor_origin_ = origin;
317  cloud->sensor_orientation_ = orientation;
318 
319  signal_->operator () (cloud);
320  }
321 }
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
std::size_t size() const override
size Returns the number of clouds currently loaded by the grabber
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.
boost::shared_ptr< const ImageGrabber > ConstPtr
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:405
bool getCloudAt(std::size_t idx, pcl::PCLPointCloud2 &blob, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) const
Gets the cloud in ROS form at location idx.
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:407
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:412
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:413
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:81
#define PCL_EXPORTS
Definition: pcl_macros.h:241
const pcl::PointCloud< PointT >::ConstPtr operator[](std::size_t idx) const override
operator[] Returns the idx-th cloud in the dataset, without bounds checking.
boost::signals2::signal< void(const typename pcl::PointCloud< PointT >::ConstPtr &)> * signal_
std::size_t numFrames() const
Convenience function to see how many frames this consists of.