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