Point Cloud Library (PCL) 1.12.0
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
54namespace 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;
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 {}
221
222 // Inherited from FileGrabber
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
273 {
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:256
Base class for Image file grabber.
Definition: image_grabber.h:60
ImageGrabberBase(const std::string &depth_directory, const std::string &rgb_directory, float frames_per_second, bool repeat)
~ImageGrabberBase() noexcept
Virtual destructor.
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.
~ImageGrabber() noexcept
Empty destructor.
boost::signals2::signal< void(const typename pcl::PointCloud< PointT >::ConstPtr &)> * signal_
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)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
Definition: conversions.h:166
#define PCL_EXPORTS
Definition: pcl_macros.h:323
A point structure representing Euclidean xyz coordinates, and the RGB color.