40 #include <pcl/pcl_config.h> 42 #include <pcl/common/io.h> 43 #include <pcl/io/grabber.h> 44 #include <pcl/io/file_grabber.h> 45 #include <pcl/common/time_trigger.h> 46 #include <pcl/conversions.h> 50 #include <pcl/io/openni_camera/openni_image.h> 51 #include <pcl/io/openni_camera/openni_image_rgb24.h> 52 #include <pcl/io/openni_camera/openni_depth_image.h> 71 PCDGrabberBase (
const std::string& pcd_file,
float frames_per_second,
bool repeat);
78 PCDGrabberBase (
const std::vector<std::string>& pcd_files,
float frames_per_second,
bool repeat);
99 isRunning ()
const override;
103 getName ()
const override;
111 getFramesPerSecond ()
const override;
119 getCloudAt (std::size_t idx,
121 Eigen::Vector4f &origin,
122 Eigen::Quaternionf &orientation)
const;
130 publish (
const pcl::PCLPointCloud2& blob,
const Eigen::Vector4f& origin,
const Eigen::Quaternionf& orientation,
const std::string& file_name)
const = 0;
133 struct PCDGrabberImpl;
134 PCDGrabberImpl* impl_;
142 using Ptr = shared_ptr<PCDGrabber>;
145 PCDGrabber (
const std::string& pcd_path,
float frames_per_second = 0,
bool repeat =
false);
146 PCDGrabber (
const std::vector<std::string>& pcd_files,
float frames_per_second = 0,
bool repeat =
false);
160 size ()
const override;
164 publish (
const pcl::PCLPointCloud2& blob,
const Eigen::Vector4f& origin,
const Eigen::Quaternionf& orientation,
const std::string& file_name)
const override;
166 boost::signals2::signal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>*
signal_;
171 boost::signals2::signal<void (const openni_wrapper::Image::Ptr&)>*
image_signal_;
172 boost::signals2::signal<void (const openni_wrapper::Image::Ptr&, const openni_wrapper::DepthImage::Ptr&, float constant)>*
image_depth_image_signal_;
177 template<
typename Po
intT>
181 signal_ = createSignal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>();
185 image_signal_ = createSignal <void (const openni_wrapper::Image::Ptr&)> ();
186 image_depth_image_signal_ = createSignal <void (const openni_wrapper::Image::Ptr&, const openni_wrapper::DepthImage::Ptr&, float constant)> ();
191 template<
typename Po
intT>
193 :
PCDGrabberBase (pcd_files, frames_per_second, repeat), signal_ ()
195 signal_ = createSignal<void (const typename pcl::PointCloud<PointT>::ConstPtr&)>();
199 image_signal_ = createSignal <void (const openni_wrapper::Image::Ptr&)> ();
200 image_depth_image_signal_ = createSignal <void (const openni_wrapper::Image::Ptr&, const openni_wrapper::DepthImage::Ptr&, float constant)> ();
209 Eigen::Vector4f origin;
210 Eigen::Quaternionf orientation;
211 getCloudAt (idx, blob, origin, orientation);
220 template <
typename Po
intT> std::size_t
223 return (numFrames ());
227 template<
typename Po
intT>
void 235 signal_->operator () (cloud);
236 if (file_name_signal_->num_slots() > 0)
237 file_name_signal_->operator()(file_name);
244 shared_ptr<xn::DepthMetaData> depth_meta_data (
new xn::DepthMetaData);
245 depth_meta_data->AllocateData (cloud->
width, cloud->
height);
246 XnDepthPixel* depth_map = depth_meta_data->WritableData ();
251 depth_map[k] =
static_cast<XnDepthPixel
> ((*cloud)[k].z * 1000);
256 if (depth_image_signal_->num_slots() > 0)
257 depth_image_signal_->operator()(depth_image);
260 std::vector<pcl::PCLPointField> fields;
261 int rgba_index = pcl::getFieldIndex<PointT> (
"rgb", fields);
262 if (rgba_index == -1)
263 rgba_index = pcl::getFieldIndex<PointT> (
"rgba", fields);
266 rgba_index = fields[rgba_index].offset;
268 shared_ptr<xn::ImageMetaData> image_meta_data (
new xn::ImageMetaData);
269 image_meta_data->AllocateData (cloud->
width, cloud->
height, XN_PIXEL_FORMAT_RGB24);
270 XnRGB24Pixel* image_map = image_meta_data->WritableRGB24Data ();
278 memcpy (&rgb, reinterpret_cast<const char*> (&(*cloud)[k]) + rgba_index,
sizeof (
RGB));
279 image_map[k].nRed =
static_cast<XnUInt8
> (rgb.r);
280 image_map[k].nGreen =
static_cast<XnUInt8
> (rgb.g);
281 image_map[k].nBlue =
static_cast<XnUInt8
> (rgb.b);
287 if (image_signal_->num_slots() > 0)
288 image_signal_->operator()(image);
290 if (image_depth_image_signal_->num_slots() > 0)
291 image_depth_image_signal_->operator()(image, depth_image, 1.0f / 525.0f);
shared_ptr< PCDGrabber > Ptr
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...
boost::signals2::signal< void(const openni_wrapper::DepthImage::Ptr &)> * depth_image_signal_
shared_ptr< PointCloud< PointT > > Ptr
Defines functions, macros and traits for allocating and using memory.
boost::signals2::signal< void(const openni_wrapper::Image::Ptr &, const openni_wrapper::DepthImage::Ptr &, float constant)> * image_depth_image_signal_
boost::signals2::signal< void(const openni_wrapper::Image::Ptr &)> * image_signal_
boost::signals2::signal< void(const typename pcl::PointCloud< PointT >::ConstPtr &)> * signal_
~PCDGrabber() noexcept
Virtual destructor.
void stop() override
Stops playing the list of PCD files if frames_per_second is > 0.
Grabber interface for PCL 1.x device drivers.
PCDGrabber(const std::string &pcd_path, float frames_per_second=0, bool repeat=false)
std::size_t size() const override
size Returns the number of clouds currently loaded by the grabber
This class provides methods to fill a depth or disparity image.
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
std::uint32_t width
The point cloud width (if organized as an image-structure).
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
A structure representing RGB color information.
void publish(const pcl::PCLPointCloud2 &blob, const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation, const std::string &file_name) const override
const pcl::PointCloud< PointT >::ConstPtr operator[](std::size_t idx) const override
operator[] Returns the idx-th cloud in the dataset, without bounds checking.
FileGrabber provides a container-style interface for grabbers which operate on fixed-size input...
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
std::uint32_t height
The point cloud height (if organized as an image-structure).
Base class for PCD file grabber.
This class provides methods to fill a RGB or Grayscale image buffer from underlying RGB24 image...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::shared_ptr< Image > Ptr
pcl::shared_ptr< DepthImage > Ptr
boost::signals2::signal< void(const std::string &)> * file_name_signal_
shared_ptr< const PointCloud< PointT > > ConstPtr
shared_ptr< const PCDGrabber > ConstPtr