void
pubRealSenseRGBImageMsg(cv::Mat& rgb_mat)
{
sensor_msgs::ImagePtr rgb_img(new sensor_msgs::Image);
rgb_img->header.seq = head_sequence_id;
rgb_img->header.stamp = head_time_stamp;
rgb_img->header.frame_id = rgb_frame_id;
rgb_img->width = rgb_mat.cols;
rgb_img->height = rgb_mat.rows;
rgb_img->encoding = sensor_msgs::image_encodings::BGR8;
rgb_img->is_bigendian = 0;
int step = sizeof(unsigned char) * 3 * rgb_img->width;
int size = step * rgb_img->height;
rgb_img->step = step;
rgb_img->data.resize(size);
memcpy(&(rgb_img->data[0]), rgb_mat.data, size);
rgb_camera_info->header.frame_id = rgb_frame_id;
rgb_camera_info->header.stamp = head_time_stamp;
rgb_camera_info->header.seq = head_sequence_id;
realsense_rgb_image_pub.publish(rgb_img, rgb_camera_info);
//pub_rgb_info.publish(rgb_camera_info);
//save rgb img
// static int count = 0;
// count++;
// if(count > 0)
// {
// struct timeval save_time;
// gettimeofday( &save_time, NULL );
// char save_name[256];
// sprintf(save_name, "~/temp/realsense_rgb_%d.jpg", (int)save_time.tv_sec);
// printf("\nsave realsense rgb img: %s\n", save_name);
// cv::imwrite(save_name, rgb_mat);
// count = 0;
// }
}
void GLLUTWidget::sampleImage(const RawImage & img) {
//compute slice it sits on:
ColorFormat source_format=img.getColorFormat();
int n=img.getNumPixels();
yuv color;
int i=0;
if (img.getWidth() > 1 && img.getHeight() > 1) {
if (source_format==COLOR_RGB8) {
rgbImage rgb_img(img);
rgb * color_rgb=rgb_img.getPixelData();
for (int j=0;j<n;j++) {
color=Conversions::rgb2yuv(*color_rgb);
i=_lut->norm2lutX(color.y);
if (i >= 0 && i < (int)slices.size()) {
drawSample(i,_lut->norm2lutY(color.u),_lut->norm2lutZ(color.v));
//slices[i]->sampler->surface.setPixel(_lut->norm2lutY(color.u),_lut->norm2lutZ(color.v),rgba(255,255,255,255));
slices[i]->sampler_update_pending=true;
}
color_rgb++;
}
} else if (source_format==COLOR_YUV444) {
yuvImage yuv_img(img);
yuv * color_yuv=yuv_img.getPixelData();
for (int j=0;j<n;j++) {
color=(*color_yuv);
i=_lut->norm2lutX(color.y);
if (i >= 0 && i < (int)slices.size()) {
//slices[i]->sampler->surface.setPixel(_lut->norm2lutY(color.u),_lut->norm2lutZ(color.v),rgba(255,255,255,255));
drawSample(i,_lut->norm2lutY(color.u),_lut->norm2lutZ(color.v));
slices[i]->sampler_update_pending=true;
}
color_yuv++;
}
} else if (source_format==COLOR_YUV422_UYVY) {
uyvy * color_uyvy = (uyvy*)img.getData();
uyvy color_uyvy_tmp;
for (int j=0;j<n;j+=2) {
color_uyvy_tmp=(*color_uyvy);
color.u=color_uyvy_tmp.u;
color.v=color_uyvy_tmp.v;
color.y=color_uyvy_tmp.y1;
i=_lut->norm2lutX(color.y);
if (i >= 0 && i < (int)slices.size()) {
//slices[i]->sampler->surface.setPixel(_lut->norm2lutY(color.u),_lut->norm2lutZ(color.v),rgba(255,255,255,255));
drawSample(i,_lut->norm2lutY(color.u),_lut->norm2lutZ(color.v));
slices[i]->sampler_update_pending=true;
}
color.y=color_uyvy_tmp.y2;
i=_lut->norm2lutX(color.y);
if (i >= 0 && i < (int)slices.size()) {
//slices[i]->sampler->surface.setPixel(_lut->norm2lutY(color.u),_lut->norm2lutZ(color.v),rgba(255,255,255,255));
drawSample(i,_lut->norm2lutY(color.u),_lut->norm2lutZ(color.v));
slices[i]->sampler_update_pending=true;
}
color_uyvy++;
}
} else {
fprintf(stderr,"Unable to sample colors from frame of format: %s\n",Colors::colorFormatToString(source_format).c_str());
fprintf(stderr,"Currently supported are rgb8, yuv444, and yuv422 (UYVY).\n");
fprintf(stderr,"(Feel free to add more conversions to glLUTwidget.cpp).\n");
}
}
redraw();
}
