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>
47 #include <pcl/make_shared.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);
117 isRunning () const override;
121 getName () const override;
129 getFramesPerSecond () const override;
137 getCloudAt (std::
size_t idx,
139 Eigen::Vector4f &origin,
140 Eigen::Quaternionf &orientation) const;
148 publish (const
pcl::
PCLPointCloud2& blob, const
Eigen::Vector4f& origin, const
Eigen::Quaternionf& orientation, const std::
string& file_name) const = 0;
151 struct PCDGrabberImpl;
152 PCDGrabberImpl* impl_;
163 PCDGrabber (
const std::string& pcd_path,
float frames_per_second = 0,
bool repeat =
false);
164 PCDGrabber (
const std::vector<std::string>& pcd_files,
float frames_per_second = 0,
bool repeat =
false);
174 operator[] (std::size_t idx)
const override;
178 size ()
const override;
182 publish (
const pcl::PCLPointCloud2& blob,
const Eigen::Vector4f& origin,
const Eigen::Quaternionf& orientation,
const std::string& file_name)
const override;
195 template<
typename Po
intT>
203 image_signal_ = createSignal <void (const openni_wrapper::Image::Ptr&)> ();
204 image_depth_image_signal_ = createSignal <void (const openni_wrapper::Image::Ptr&, const openni_wrapper::DepthImage::Ptr&, float constant)> ();
209 template<
typename Po
intT>
211 :
PCDGrabberBase (pcd_files, frames_per_second, repeat), signal_ ()
217 image_signal_ = createSignal <void (const openni_wrapper::Image::Ptr&)> ();
218 image_depth_image_signal_ = createSignal <void (const openni_wrapper::Image::Ptr&, const openni_wrapper::DepthImage::Ptr&, float constant)> ();
227 Eigen::Vector4f origin;
228 Eigen::Quaternionf orientation;
229 getCloudAt (idx, blob, origin, orientation);
238 template <
typename Po
intT> std::size_t
241 return (numFrames ());
245 template<
typename Po
intT>
void
253 signal_->operator () (cloud);
254 if (file_name_signal_->num_slots() > 0)
255 file_name_signal_->operator()(file_name);
263 depth_meta_data->AllocateData (cloud->
width, cloud->
height);
264 XnDepthPixel* depth_map = depth_meta_data->WritableData ();
266 for (std::uint32_t i = 0; i < cloud->
height; ++i)
267 for (std::uint32_t j = 0; j < cloud->
width; ++j)
269 depth_map[k] =
static_cast<XnDepthPixel
> ((*cloud)[k].z * 1000);
274 if (depth_image_signal_->num_slots() > 0)
275 depth_image_signal_->operator()(depth_image);
278 std::vector<pcl::PCLPointField> fields;
279 int rgba_index = pcl::getFieldIndex<PointT> (
"rgb", fields);
280 if (rgba_index == -1)
281 rgba_index = pcl::getFieldIndex<PointT> (
"rgba", fields);
284 rgba_index = fields[rgba_index].offset;
287 image_meta_data->AllocateData (cloud->
width, cloud->
height, XN_PIXEL_FORMAT_RGB24);
288 XnRGB24Pixel* image_map = image_meta_data->WritableRGB24Data ();
290 for (std::uint32_t i = 0; i < cloud->
height; ++i)
292 for (std::uint32_t j = 0; j < cloud->
width; ++j)
296 memcpy (&rgb,
reinterpret_cast<const char*
> (&cloud->
points[k]) + rgba_index, sizeof (
RGB));
297 image_map[k].nRed =
static_cast<XnUInt8
> (rgb.r);
298 image_map[k].nGreen =
static_cast<XnUInt8
> (rgb.g);
299 image_map[k].nBlue =
static_cast<XnUInt8
> (rgb.b);
305 if (image_signal_->num_slots() > 0)
306 image_signal_->operator()(image);
308 if (image_depth_image_signal_->num_slots() > 0)
309 image_depth_image_signal_->operator()(image, depth_image, 1.0f / 525.0f);