image_grabber.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, Inc.
* Copyright (c) 2012-, Open Perception, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*
*/
#pragma once
#include <pcl/pcl_config.h>
#include <pcl/io/grabber.h>
#include <pcl/io/file_grabber.h>
#include <pcl/common/time_trigger.h>
#include <pcl/conversions.h>
#include <boost/shared_ptr.hpp>
#include <string>
#include <vector>
namespace pcl
{
/** \brief Base class for Image file grabber.
* \ingroup io
*/
class PCL_EXPORTS ImageGrabberBase : public Grabber
{
public:
/** \brief Constructor taking a folder of depth+[rgb] images.
* \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]
* \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list.
* \param[in] repeat whether to play PCD file in an endless loop or not.
* \param pclzf_mode
*/
ImageGrabberBase (const std::string& directory, float frames_per_second, bool repeat, bool pclzf_mode);
ImageGrabberBase (const std::string& depth_directory, const std::string& rgb_directory, float frames_per_second, bool repeat);
/** \brief Constructor taking a list of paths to PCD files, that are played in the order they appear in the list.
* \param[in] depth_image_files Path to the depth image files files.
* \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list.
* \param[in] repeat whether to play PCD file in an endless loop or not.
*/
ImageGrabberBase (const std::vector<std::string>& depth_image_files, float frames_per_second, bool repeat);
/** \brief Copy constructor.
* \param[in] src the Image Grabber base object to copy into this
*/
ImageGrabberBase (const ImageGrabberBase &src) : impl_ ()
{
*this = src;
}
/** \brief Copy operator.
* \param[in] src the Image Grabber base object to copy into this
*/
ImageGrabberBase&
operator = (const ImageGrabberBase &src)
{
impl_ = src.impl_;
return (*this);
}
/** \brief Virtual destructor. */
~ImageGrabberBase () throw ();
/** \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. */
void
start () override;
/** \brief Stops playing the list of PCD files if frames_per_second is > 0. Otherwise the method has no effect. */
void
stop () override;
/** \brief Triggers a callback with new data */
virtual void
trigger ();
/** \brief whether the grabber is started (publishing) or not.
* \return true only if publishing.
*/
bool
isRunning () const override;
/** \return The name of the grabber */
std::string
getName () const override;
/** \brief Rewinds to the first PCD file in the list.*/
virtual void
rewind ();
/** \brief Returns the frames_per_second. 0 if grabber is trigger-based */
float
getFramesPerSecond () const override;
/** \brief Returns whether the repeat flag is on */
bool
isRepeatOn () const;
/** \brief Returns if the last frame is reached */
bool
atLastFrame () const;
/** \brief Returns the filename of the current indexed file */
std::string
getCurrentDepthFileName () const;
/** \brief Returns the filename of the previous indexed file
* SDM: adding this back in, but is this useful, or confusing? */
std::string
getPrevDepthFileName () const;
/** \brief Get the depth filename at a particular index */
std::string
getDepthFileNameAtIndex (std::size_t idx) const;
/** \brief Query only the timestamp of an index, if it exists */
bool
getTimestampAtIndex (std::size_t idx, std::uint64_t &timestamp) const;
/** \brief Manually set RGB image files.
* \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
*/
void
setRGBImageFiles (const std::vector<std::string>& rgb_image_files);
/** \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.
* \param[in] focal_length_x Horizontal focal length (fx)
* \param[in] focal_length_y Vertical focal length (fy)
* \param[in] principal_point_x Horizontal coordinates of the principal point (cx)
* \param[in] principal_point_y Vertical coordinates of the principal point (cy)
*/
virtual void
setCameraIntrinsics (const double focal_length_x,
const double focal_length_y,
const double principal_point_x,
const double principal_point_y);
/** \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.
* \param[out] focal_length_x Horizontal focal length (fx)
* \param[out] focal_length_y Vertical focal length (fy)
* \param[out] principal_point_x Horizontal coordinates of the principal point (cx)
* \param[out] principal_point_y Vertical coordinates of the principal point (cy)
*/
virtual void
getCameraIntrinsics (double &focal_length_x,
double &focal_length_y,
double &principal_point_x,
double &principal_point_y) const;
/** \brief Define the units the depth data is stored in.
* Defaults to mm (0.001), meaning a brightness of 1000 corresponds to 1 m*/
void
setDepthImageUnits (float units);
/** \brief Set the number of threads, if we wish to use OpenMP for quicker cloud population.
* Note that for a standard (< 4 core) machine this is unlikely to yield a drastic speedup.*/
void
setNumberOfThreads (unsigned int nr_threads = 0);
protected:
/** \brief Convenience function to see how many frames this consists of
*/
std::size_t
numFrames () const;
/** \brief Gets the cloud in ROS form at location idx */
bool
getCloudAt (std::size_t idx, pcl::PCLPointCloud2 &blob, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) const;
private:
virtual void
publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const = 0;
// to separate and hide the implementation from interface: PIMPL
struct ImageGrabberImpl;
ImageGrabberImpl* impl_;
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename T> class PointCloud;
template <typename PointT> class ImageGrabber : public ImageGrabberBase, public FileGrabber<PointT>
{
public:
using Ptr = shared_ptr<ImageGrabber>;
using ConstPtr = shared_ptr<const ImageGrabber>;
ImageGrabber (const std::string& dir,
float frames_per_second = 0,
bool repeat = false,
bool pclzf_mode = false);
ImageGrabber (const std::string& depth_dir,
const std::string& rgb_dir,
float frames_per_second = 0,
bool repeat = false);
ImageGrabber (const std::vector<std::string>& depth_image_files,
float frames_per_second = 0,
bool repeat = false);
/** \brief Empty destructor */
~ImageGrabber () throw () {}
// Inherited from FileGrabber
const typename pcl::PointCloud<PointT>::ConstPtr
operator[] (std::size_t idx) const override;
// Inherited from FileGrabber
std::size_t
size () const override;
protected:
void
publish (const pcl::PCLPointCloud2& blob,
const Eigen::Vector4f& origin,
const Eigen::Quaternionf& orientation) const override;
boost::signals2::signal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>* signal_;
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
ImageGrabber<PointT>::ImageGrabber (const std::string& dir,
float frames_per_second,
bool repeat,
bool pclzf_mode)
: ImageGrabberBase (dir, frames_per_second, repeat, pclzf_mode)
{
signal_ = createSignal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>();
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
ImageGrabber<PointT>::ImageGrabber (const std::string& depth_dir,
const std::string& rgb_dir,
float frames_per_second,
bool repeat)
: ImageGrabberBase (depth_dir, rgb_dir, frames_per_second, repeat)
{
signal_ = createSignal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>();
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
ImageGrabber<PointT>::ImageGrabber (const std::vector<std::string>& depth_image_files,
float frames_per_second,
bool repeat)
: ImageGrabberBase (depth_image_files, frames_per_second, repeat), signal_ ()
{
signal_ = createSignal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>();
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> const typename pcl::PointCloud<PointT>::ConstPtr
ImageGrabber<PointT>::operator[] (std::size_t idx) const
{
pcl::PCLPointCloud2 blob;
Eigen::Vector4f origin;
Eigen::Quaternionf orientation;
getCloudAt (idx, blob, origin, orientation);
typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
pcl::fromPCLPointCloud2 (blob, *cloud);
cloud->sensor_origin_ = origin;
cloud->sensor_orientation_ = orientation;
return (cloud);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> std::size_t
ImageGrabber<PointT>::size () const
{
return (numFrames ());
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
ImageGrabber<PointT>::publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const
{
typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
pcl::fromPCLPointCloud2 (blob, *cloud);
cloud->sensor_origin_ = origin;
cloud->sensor_orientation_ = orientation;
signal_->operator () (cloud);
}
}
pcl::ImageGrabberBase
Base class for Image file grabber.
Definition: image_grabber.h:59
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
Eigen
Definition: bfgs.h:9
pcl::ImageGrabber
Definition: image_grabber.h:217
pcl::ImageGrabberBase::ImageGrabberBase
ImageGrabberBase(const ImageGrabberBase &src)
Copy constructor.
Definition: image_grabber.h:81
pcl::ImageGrabber::~ImageGrabber
~ImageGrabber()
Empty destructor.
Definition: image_grabber.h:238
pcl::ImageGrabber::ConstPtr
shared_ptr< const ImageGrabber > ConstPtr
Definition: image_grabber.h:221
pcl::ImageGrabber::signal_
boost::signals2::signal< void(const typename pcl::PointCloud< PointT >::ConstPtr &)> * signal_
Definition: image_grabber.h:253
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:52
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:623
pcl::Grabber
Grabber interface for PCL 1.x device drivers.
Definition: grabber.h:57
pcl::ImageGrabber::publish
void publish(const pcl::PCLPointCloud2 &blob, const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation) const override
Definition: image_grabber.h:312
pcl::FileGrabber
FileGrabber provides a container-style interface for grabbers which operate on fixed-size input.
Definition: file_grabber.h:53
pcl::Grabber::createSignal
boost::signals2::signal< T > * createSignal()
Definition: grabber.h:232
pcl::PointCloud::sensor_origin_
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:408
pcl::PointCloud::sensor_orientation_
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:410
pcl::PCLPointCloud2
Definition: PCLPointCloud2.h:19
pcl::ImageGrabber::ImageGrabber
ImageGrabber(const std::string &dir, float frames_per_second=0, bool repeat=false, bool pclzf_mode=false)
Definition: image_grabber.h:258
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:415
pcl::ImageGrabber::size
std::size_t size() const override
size Returns the number of clouds currently loaded by the grabber
Definition: image_grabber.h:305
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:416
pcl::ImageGrabber::Ptr
shared_ptr< ImageGrabber > Ptr
Definition: image_grabber.h:220
PCL_EXPORTS
#define PCL_EXPORTS
Definition: pcl_macros.h:253
pcl::fromPCLPointCloud2
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
pcl::shared_ptr
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90
pcl::ImageGrabber::operator[]
const pcl::PointCloud< PointT >::ConstPtr operator[](std::size_t idx) const override
operator[] Returns the idx-th cloud in the dataset, without bounds checking.
Definition: image_grabber.h:290
pcl::uint64_t
std::uint64_t uint64_t
Definition: pcl_macros.h:98