Loading...
Searching...
No Matches
pcd_grabber.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2012, Willow Garage, 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/common/io.h>
#include <pcl/io/grabber.h>
#include <pcl/io/file_grabber.h>
#include <pcl/common/time_trigger.h>
#include <pcl/conversions.h>
#include <pcl/memory.h>
#ifdef HAVE_OPENNI
#include <pcl/io/openni_camera/openni_image.h>
#include <pcl/io/openni_camera/openni_image_rgb24.h>
#include <pcl/io/openni_camera/openni_depth_image.h>
#endif
#include <string>
#include <vector>
namespace pcl
{
/** \brief Base class for PCD file grabber.
* \ingroup io
*/
class PCL_EXPORTS PCDGrabberBase : public Grabber
{
public:
/** \brief Constructor taking just one PCD file or one TAR file containing multiple PCD files.
* \param[in] pcd_file path to the PCD file
* \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.
*/
PCDGrabberBase (const std::string& pcd_file, 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] pcd_files vector of paths to PCD 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.
*/
PCDGrabberBase (const std::vector<std::string>& pcd_files, float frames_per_second, bool repeat);
/** \brief Virtual destructor. */
~PCDGrabberBase () noexcept override;
/** \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 Indicates whether the grabber is streaming or not.
* \return true if grabber is started and hasn't run out of PCD files.
*/
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 Get cloud (in ROS form) at a particular location */
bool
getCloudAt (std::size_t idx,
pcl::PCLPointCloud2 &blob,
Eigen::Vector4f &origin,
Eigen::Quaternionf &orientation) const;
/** \brief Returns the size */
std::size_t
numFrames () const;
private:
virtual void
publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation, const std::string& file_name) const = 0;
// to separate and hide the implementation from interface: PIMPL
struct PCDGrabberImpl;
PCDGrabberImpl* impl_;
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename T> class PointCloud;
template <typename PointT> class PCDGrabber : public PCDGrabberBase, public FileGrabber<PointT>
{
public:
using Ptr = shared_ptr<PCDGrabber>;
using ConstPtr = shared_ptr<const PCDGrabber>;
PCDGrabber (const std::string& pcd_path, float frames_per_second = 0, bool repeat = false);
PCDGrabber (const std::vector<std::string>& pcd_files, float frames_per_second = 0, bool repeat = false);
/** \brief Virtual destructor. */
~PCDGrabber () noexcept override
{
stop ();
}
// 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 std::string& file_name) const override;
boost::signals2::signal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>* signal_;
boost::signals2::signal<void (const std::string&)>* file_name_signal_;
#ifdef HAVE_OPENNI
boost::signals2::signal<void (const openni_wrapper::DepthImage::Ptr&)>* depth_image_signal_;
boost::signals2::signal<void (const openni_wrapper::Image::Ptr&)>* image_signal_;
boost::signals2::signal<void (const openni_wrapper::Image::Ptr&, const openni_wrapper::DepthImage::Ptr&, float constant)>* image_depth_image_signal_;
#endif
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
PCDGrabber<PointT>::PCDGrabber (const std::string& pcd_path, float frames_per_second, bool repeat)
: PCDGrabberBase (pcd_path, frames_per_second, repeat)
{
signal_ = createSignal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>();
file_name_signal_ = createSignal<void (const std::string&)>();
#ifdef HAVE_OPENNI
depth_image_signal_ = createSignal <void (const openni_wrapper::DepthImage::Ptr&)> ();
image_signal_ = createSignal <void (const openni_wrapper::Image::Ptr&)> ();
image_depth_image_signal_ = createSignal <void (const openni_wrapper::Image::Ptr&, const openni_wrapper::DepthImage::Ptr&, float constant)> ();
#endif
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT>
PCDGrabber<PointT>::PCDGrabber (const std::vector<std::string>& pcd_files, float frames_per_second, bool repeat)
: PCDGrabberBase (pcd_files, frames_per_second, repeat), signal_ ()
{
signal_ = createSignal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>();
file_name_signal_ = createSignal<void (const std::string&)>();
#ifdef HAVE_OPENNI
depth_image_signal_ = createSignal <void (const openni_wrapper::DepthImage::Ptr&)> ();
image_signal_ = createSignal <void (const openni_wrapper::Image::Ptr&)> ();
image_depth_image_signal_ = createSignal <void (const openni_wrapper::Image::Ptr&, const openni_wrapper::DepthImage::Ptr&, float constant)> ();
#endif
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> const typename pcl::PointCloud<PointT>::ConstPtr
PCDGrabber<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
PCDGrabber<PointT>::size () const
{
return (numFrames ());
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
PCDGrabber<PointT>::publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation, const std::string& file_name) 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);
if (file_name_signal_->num_slots() > 0)
file_name_signal_->operator()(file_name);
#ifdef HAVE_OPENNI
// If dataset is not organized, return
if (!cloud->isOrganized ())
return;
shared_ptr<xn::DepthMetaData> depth_meta_data (new xn::DepthMetaData);
depth_meta_data->AllocateData (cloud->width, cloud->height);
XnDepthPixel* depth_map = depth_meta_data->WritableData ();
std::uint32_t k = 0;
for (std::uint32_t i = 0; i < cloud->height; ++i)
for (std::uint32_t j = 0; j < cloud->width; ++j)
{
depth_map[k] = static_cast<XnDepthPixel> ((*cloud)[k].z * 1000);
++k;
}
openni_wrapper::DepthImage::Ptr depth_image (new openni_wrapper::DepthImage (depth_meta_data, 0.075f, 525, 0, 0));
if (depth_image_signal_->num_slots() > 0)
depth_image_signal_->operator()(depth_image);
// ---[ RGB special case
std::vector<pcl::PCLPointField> fields;
int rgba_index = pcl::getFieldIndex<PointT> ("rgb", fields);
if (rgba_index == -1)
rgba_index = pcl::getFieldIndex<PointT> ("rgba", fields);
if (rgba_index >= 0)
{
rgba_index = fields[rgba_index].offset;
shared_ptr<xn::ImageMetaData> image_meta_data (new xn::ImageMetaData);
image_meta_data->AllocateData (cloud->width, cloud->height, XN_PIXEL_FORMAT_RGB24);
XnRGB24Pixel* image_map = image_meta_data->WritableRGB24Data ();
k = 0;
for (std::uint32_t i = 0; i < cloud->height; ++i)
{
for (std::uint32_t j = 0; j < cloud->width; ++j)
{
// Fill r/g/b data, assuming that the order is BGRA
pcl::RGB rgb;
memcpy (&rgb, reinterpret_cast<const char*> (&(*cloud)[k]) + rgba_index, sizeof (RGB));
image_map[k].nRed = static_cast<XnUInt8> (rgb.r);
image_map[k].nGreen = static_cast<XnUInt8> (rgb.g);
image_map[k].nBlue = static_cast<XnUInt8> (rgb.b);
++k;
}
}
openni_wrapper::Image::Ptr image (new openni_wrapper::ImageRGB24 (image_meta_data));
if (image_signal_->num_slots() > 0)
image_signal_->operator()(image);
if (image_depth_image_signal_->num_slots() > 0)
image_depth_image_signal_->operator()(image, depth_image, 1.0f / 525.0f);
}
#endif
}
}
openni_wrapper::DepthImage
This class provides methods to fill a depth or disparity image.
Definition openni_depth_image.h:57
openni_wrapper::DepthImage::Ptr
pcl::shared_ptr< DepthImage > Ptr
Definition openni_depth_image.h:59
openni_wrapper::Image::Ptr
pcl::shared_ptr< Image > Ptr
Definition openni_image.h:61
openni_wrapper::ImageRGB24
This class provides methods to fill a RGB or Grayscale image buffer from underlying RGB24 image.
Definition openni_image_rgb24.h:56
pcl::FileGrabber
FileGrabber provides a container-style interface for grabbers which operate on fixed-size input.
Definition file_grabber.h:54
pcl::Grabber
Grabber interface for PCL 1.x device drivers.
Definition grabber.h:60
pcl::Grabber::createSignal
boost::signals2::signal< T > * createSignal()
Definition grabber.h:252
pcl::PCDGrabberBase
Base class for PCD file grabber.
Definition pcd_grabber.h:64
pcl::PCDGrabberBase::PCDGrabberBase
PCDGrabberBase(const std::string &pcd_file, float frames_per_second, bool repeat)
Constructor taking just one PCD file or one TAR file containing multiple PCD files.
pcl::PCDGrabberBase::~PCDGrabberBase
~PCDGrabberBase() noexcept override
Virtual destructor.
pcl::PCDGrabberBase::PCDGrabberBase
PCDGrabberBase(const std::vector< std::string > &pcd_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...
pcl::PCDGrabber
Definition pcd_grabber.h:140
pcl::PCDGrabber::Ptr
shared_ptr< PCDGrabber > Ptr
Definition pcd_grabber.h:142
pcl::PCDGrabber::file_name_signal_
boost::signals2::signal< void(const std::string &)> * file_name_signal_
Definition pcd_grabber.h:167
pcl::PCDGrabber::image_depth_image_signal_
boost::signals2::signal< void(const openni_wrapper::Image::Ptr &, const openni_wrapper::DepthImage::Ptr &, float constant)> * image_depth_image_signal_
Definition pcd_grabber.h:172
pcl::PCDGrabber::image_signal_
boost::signals2::signal< void(const openni_wrapper::Image::Ptr &)> * image_signal_
Definition pcd_grabber.h:171
pcl::PCDGrabber::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 pcd_grabber.h:206
pcl::PCDGrabber::depth_image_signal_
boost::signals2::signal< void(const openni_wrapper::DepthImage::Ptr &)> * depth_image_signal_
Definition pcd_grabber.h:170
pcl::PCDGrabber::PCDGrabber
PCDGrabber(const std::string &pcd_path, float frames_per_second=0, bool repeat=false)
Definition pcd_grabber.h:178
pcl::PCDGrabber::publish
void publish(const pcl::PCLPointCloud2 &blob, const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation, const std::string &file_name) const override
Definition pcd_grabber.h:228
pcl::PCDGrabber::signal_
boost::signals2::signal< void(const typename pcl::PointCloud< PointT >::ConstPtr &)> * signal_
Definition pcd_grabber.h:166
pcl::PCDGrabber::~PCDGrabber
~PCDGrabber() noexcept override
Virtual destructor.
Definition pcd_grabber.h:149
pcl::PCDGrabber::size
std::size_t size() const override
size Returns the number of clouds currently loaded by the grabber
Definition pcd_grabber.h:221
pcl::PCDGrabber::ConstPtr
shared_ptr< const PCDGrabber > ConstPtr
Definition pcd_grabber.h:143
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition point_cloud.h:173
pcl::PointCloud::sensor_orientation_
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition point_cloud.h:408
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition point_cloud.h:398
pcl::PointCloud::isOrganized
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
Definition point_cloud.h:310
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition point_cloud.h:400
pcl::PointCloud::sensor_origin_
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition point_cloud.h:406
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition point_cloud.h:413
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition point_cloud.h:414
memory.h
Defines functions, macros and traits for allocating and using memory.
Eigen
Definition bfgs.h:10
pcl
Definition convolution.h:46
pcl::fromPCLPointCloud2
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:163
pcl::PCLPointCloud2
Definition PCLPointCloud2.h:17
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition point_types.hpp:623
pcl::RGB
A structure representing RGB color information.
Definition point_types.hpp:392