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;
177 template<
typename Po
intT>
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_ ()
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 ();
248 for (std::uint32_t i = 0; i < cloud->
height; ++i)
249 for (std::uint32_t j = 0; j < cloud->
width; ++j)
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 ();
272 for (std::uint32_t i = 0; i < cloud->
height; ++i)
274 for (std::uint32_t j = 0; j < cloud->
width; ++j)
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);