void OpenNINode::publishRgbImage(const openni_wrapper::Image& rgb_img, image_transport::CameraPublisher img_pub) const {
sensor_msgs::ImagePtr rgb_msg = boost::make_shared<sensor_msgs::Image>();
ros::Time time = ros::Time::now();
rgb_msg->header.stamp = time;
rgb_msg->encoding = enc::RGB8;
rgb_msg->width = depth_width_;
rgb_msg->height = depth_height_;
rgb_msg->step = rgb_msg->width * 3;
rgb_msg->data.resize(rgb_msg->height * rgb_msg->step);
rgb_img.fillRGB(rgb_msg->width, rgb_msg->height, reinterpret_cast<unsigned char*>(&rgb_msg->data[0]), rgb_msg->step);
img_pub.publish(rgb_msg, getDepthCameraInfo(rgb_msg->width, rgb_msg->height, time, 1));
}
cv::Mat3b Inspector::oniToCV(const openni_wrapper::Image& oni) const
{
cv::Mat3b img(oni.getHeight(), oni.getWidth());
uchar data[img.rows * img.cols * 3];
oni.fillRGB(img.cols, img.rows, data);
int i = 0;
for(int y = 0; y < img.rows; ++y) {
for(int x = 0; x < img.cols; ++x, i+=3) {
img(y, x)[0] = data[i+2];
img(y, x)[1] = data[i+1];
img(y, x)[2] = data[i];
}
}
return img;
}
void
DataCapture::saveRgb (const std::string &filename, const openni_wrapper::Image &image) const
{
// get the meta data associated with the image
xn::ImageMetaData& md = const_cast<xn::ImageMetaData&> (image.getMetaData());
/* cout << "image data:" << endl;
cout << " - size = " << image.getWidth() << "x" << image.getHeight() << endl;
cout << " - size (md) = " << md.FullXRes() << "x" << md.FullYRes() << endl;
cout << " - encoding = " << image.getEncoding() << endl;
cout << " - bytes/pixel = " << md.BytesPerPixel() << endl;
cout << " - data size = " << md.DataSize() << endl;
cout << " - pixel format (enum) = " << md.PixelFormat() << endl;
*/
// copy the rgb values in a buffer
int width = image.getWidth(), height = image.getHeight();
unsigned char* buffer = new unsigned char[width*height*3];
image.fillRGB (width, height, buffer, width*3);
cv::Mat img (height, width, CV_8UC3);
// openCV saves color values in order BGR, so rearrange
int idx = 0;
for (int r = 0; r < height; ++r) {
for (int c = 0; c < width; ++c, idx += 3) {
img.data[idx+2] = buffer[idx];
img.data[idx+1] = buffer[idx+1];
img.data[idx] = buffer[idx+2];
}
}
// save as PNG image
cv::imwrite(filename + "_rgb.png", img);
// std::cout << "RGB image saved as PNG to " << filename << "_rgb.png" << std::endl;
}