38 #include <pcl/pcl_config.h>
40 #ifndef PCL_IO_PCD_GRABBER_H_
41 #define PCL_IO_PCD_GRABBER_H_
43 #include <pcl/common/io.h>
44 #include <pcl/io/grabber.h>
45 #include <pcl/io/file_grabber.h>
46 #include <pcl/common/time_trigger.h>
49 #include <pcl/conversions.h>
52 #include <pcl/io/openni_camera/openni_image.h>
53 #include <pcl/io/openni_camera/openni_image_rgb24.h>
54 #include <pcl/io/openni_camera/openni_depth_image.h>
70 PCDGrabberBase (
const std::string& pcd_file,
float frames_per_second,
bool repeat);
77 PCDGrabberBase (
const std::vector<std::string>& pcd_files,
float frames_per_second,
bool repeat);
128 getFramesPerSecond () const;
136 getCloudAt (
size_t idx,
138 Eigen::Vector4f &origin,
139 Eigen::Quaternionf &orientation) const;
147 publish (const pcl::
PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const = 0;
150 struct PCDGrabberImpl;
151 PCDGrabberImpl* impl_;
159 PCDGrabber (
const std::string& pcd_path,
float frames_per_second = 0,
bool repeat =
false);
160 PCDGrabber (
const std::vector<std::string>& pcd_files,
float frames_per_second = 0,
bool repeat =
false);
166 const boost::shared_ptr< const pcl::PointCloud<PointT> >
167 operator[] (
size_t idx)
const;
175 publish (
const pcl::PCLPointCloud2& blob,
const Eigen::Vector4f& origin,
const Eigen::Quaternionf& orientation)
const;
177 boost::signals2::signal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>* signal_;
180 boost::signals2::signal<void (const boost::shared_ptr<openni_wrapper::DepthImage>&)>* depth_image_signal_;
181 boost::signals2::signal<void (const boost::shared_ptr<openni_wrapper::Image>&)>* image_signal_;
182 boost::signals2::signal<void (const boost::shared_ptr<openni_wrapper::Image>&,
const boost::shared_ptr<openni_wrapper::DepthImage>&,
float constant)>* image_depth_image_signal_;
187 template<
typename Po
intT>
191 signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
193 depth_image_signal_ = createSignal <void (const boost::shared_ptr<openni_wrapper::DepthImage>&)> ();
194 image_signal_ = createSignal <void (const boost::shared_ptr<openni_wrapper::Image>&)> ();
195 image_depth_image_signal_ = createSignal <void (const boost::shared_ptr<openni_wrapper::Image>&,
const boost::shared_ptr<openni_wrapper::DepthImage>&,
float constant)> ();
200 template<
typename Po
intT>
202 :
PCDGrabberBase (pcd_files, frames_per_second, repeat), signal_ ()
204 signal_ = createSignal<void (const boost::shared_ptr<const pcl::PointCloud<PointT> >&)>();
206 depth_image_signal_ = createSignal <void (const boost::shared_ptr<openni_wrapper::DepthImage>&)> ();
207 image_signal_ = createSignal <void (const boost::shared_ptr<openni_wrapper::Image>&)> ();
208 image_depth_image_signal_ = createSignal <void (const boost::shared_ptr<openni_wrapper::Image>&,
const boost::shared_ptr<openni_wrapper::DepthImage>&,
float constant)> ();
213 template<
typename Po
intT>
const boost::shared_ptr< const pcl::PointCloud<PointT> >
217 Eigen::Vector4f origin;
218 Eigen::Quaternionf orientation;
219 getCloudAt (idx, blob, origin, orientation);
228 template <
typename Po
intT>
size_t
231 return (numFrames ());
235 template<
typename Po
intT>
void
243 signal_->operator () (cloud);
250 boost::shared_ptr<xn::DepthMetaData> depth_meta_data (
new xn::DepthMetaData);
251 depth_meta_data->AllocateData (cloud->
width, cloud->
height);
252 XnDepthPixel* depth_map = depth_meta_data->WritableData ();
254 for (uint32_t i = 0; i < cloud->
height; ++i)
255 for (uint32_t j = 0; j < cloud->
width; ++j)
257 depth_map[k] =
static_cast<XnDepthPixel
> ((*cloud)[k].z * 1000);
261 boost::shared_ptr<openni_wrapper::DepthImage> depth_image (
new openni_wrapper::DepthImage (depth_meta_data, 0.075f, 525, 0, 0));
262 if (depth_image_signal_->num_slots() > 0)
263 depth_image_signal_->operator()(depth_image);
266 std::vector<pcl::PCLPointField> fields;
268 if (rgba_index == -1)
272 rgba_index = fields[rgba_index].offset;
274 boost::shared_ptr<xn::ImageMetaData> image_meta_data (
new xn::ImageMetaData);
275 image_meta_data->AllocateData (cloud->
width, cloud->
height, XN_PIXEL_FORMAT_RGB24);
276 XnRGB24Pixel* image_map = image_meta_data->WritableRGB24Data ();
278 for (uint32_t i = 0; i < cloud->
height; ++i)
280 for (uint32_t j = 0; j < cloud->
width; ++j)
284 memcpy (&rgb, reinterpret_cast<const char*> (&cloud->
points[k]) + rgba_index, sizeof (
RGB));
285 image_map[k].nRed =
static_cast<XnUInt8
> (rgb.r);
286 image_map[k].nGreen =
static_cast<XnUInt8
> (rgb.g);
287 image_map[k].nBlue =
static_cast<XnUInt8
> (rgb.b);
293 if (image_signal_->num_slots() > 0)
294 image_signal_->operator()(image);
296 if (image_depth_image_signal_->num_slots() > 0)
297 image_depth_image_signal_->operator()(image, depth_image, 1.0f / 525.0f);