void
processRGBD()
{
USE_TIMES_START( start );
struct timeval process_start, process_end;
USE_TIMES_START( process_start );
int stream_depth_state = 0;
int stream_rgb_state = 0;
int count = 0;
stream_depth_state = true;
while ((stream_depth_state) && (count < 1000))
{
stream_depth_state = processDepth();
++count;
}
if(stream_depth_state)
{
printf("\nstream state error depth = %d, rgb = %d\n", stream_depth_state, stream_rgb_state);
return;
}
count = 0;
stream_rgb_state = true;
while((stream_rgb_state) && (count < 100))
{
stream_rgb_state = processRGB();
++count;
}
USE_TIMES_END_SHOW ( process_start, process_end, "get RGBD time" );
if(stream_rgb_state)
{
printf("\nstream state error depth = %d, rgb = %d\n", stream_depth_state, stream_rgb_state);
return;
}
USE_TIMES_START( process_start );
cv::Mat depth_frame(depth_stream.height, depth_stream.width, CV_8UC1, depth_frame_buffer);
//added
cv::Mat cv_img_depth(depth_stream.height, depth_stream.width, CV_32FC1);
/*img_depth.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
img_depth.is_bigendian = 0;
img_depth.step = sizeof(float) * depth_stream.width;
img_depth.data.resize(depth_stream.width * depth_stream.height * sizeof(float));*/
//
#ifdef V4L2_PIX_FMT_INZI
cv::Mat ir_frame(depth_stream.height, depth_stream.width, CV_8UC1, ir_frame_buffer);
#endif
cv::Mat rgb_frame;
#if USE_BGR24
rgb_frame = cv::Mat(rgb_stream.height, rgb_stream.width, CV_8UC3, rgb_frame_buffer);
memcpy(rgb_frame_buffer, rgb_stream.fillbuf, rgb_stream.buflen);
#else
cv::Mat rgb_frame_yuv(rgb_stream.height, rgb_stream.width, CV_8UC2, rgb_frame_buffer);
memcpy(rgb_frame_buffer, rgb_stream.fillbuf, rgb_stream.buflen);
struct timeval cvt_start, cvt_end;
USE_TIMES_START( cvt_start );
cv::cvtColor(rgb_frame_yuv,rgb_frame,CV_YUV2BGR_YUYV);
USE_TIMES_END_SHOW ( cvt_start, cvt_end, "CV_YUV2BGR_YUYV time" );
#endif
USE_TIMES_END_SHOW ( process_start, process_end, "new cv::Mat object RGBD time" );
USE_TIMES_START( process_start );
if(!resize_point_cloud)
{
realsense_xyz_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
realsense_xyz_cloud->width = depth_stream.width;
realsense_xyz_cloud->height = depth_stream.height;
realsense_xyz_cloud->is_dense = false;
realsense_xyz_cloud->points.resize(depth_stream.width * depth_stream.height);
if(isHaveD2RGBUVMap)
{
realsense_xyzrgb_cloud.reset(new pcl::PointCloud<PointType>());
realsense_xyzrgb_cloud->width = depth_stream.width;
realsense_xyzrgb_cloud->height = depth_stream.height;
realsense_xyzrgb_cloud->is_dense = false;
realsense_xyzrgb_cloud->points.resize(depth_stream.width * depth_stream.height);
}
resize_point_cloud = true;
}
USE_TIMES_END_SHOW ( process_start, process_end, "new point cloud object time" );
USE_TIMES_START( process_start );
cv::MatIterator_<float> it;
it = cv_img_depth.begin<float>();
//depth value
//#pragma omp parallel for
for(int i=0; i<depth_stream.width * depth_stream.height; ++i)
{
float depth = 0.0f;
#ifdef V4L2_PIX_FMT_INZI
unsigned short* depth_ptr = (unsigned short*)((unsigned char*)(depth_stream.fillbuf) + i*3);
unsigned char* ir_ptr = (unsigned char*)(depth_stream.fillbuf) + i*3+2;
unsigned char ir_raw = *ir_ptr;
ir_frame_buffer[i] = ir_raw;
unsigned short depth_raw = *depth_ptr;
depth = (float)depth_raw / depth_unit;
#else
unsigned short depth_raw = *((unsigned short*)(depth_stream.fillbuf) + i);
depth = (float)depth_raw / depth_unit;
#endif
depth_frame_buffer[i] = depth ? 255 * (sensor_depth_max - depth) / sensor_depth_max : 0;
if (it != cv_img_depth.end<float>())
{
if(depth>0.000001f)
*it = depth * depth_scale;
else
*it = std::numeric_limits<float>::quiet_NaN();
++it;
}
else
std::cout << " ********** out of data matrix" << std::endl;
float uvx = -1.0f;
float uvy = -1.0f;
if(depth>0.000001f)
{
float pixel_x = (i % depth_stream.width) - depth_cx;
float pixel_y = (i / depth_stream.width) - depth_cy;
float zz = depth * depth_scale;
realsense_xyz_cloud->points[i].x = pixel_x * zz * depth_fxinv;
realsense_xyz_cloud->points[i].y = pixel_y * zz * depth_fyinv;
realsense_xyz_cloud->points[i].z = zz;
if(isHaveD2RGBUVMap)
{
realsense_xyzrgb_cloud->points[i].x = realsense_xyz_cloud->points[i].x;
realsense_xyzrgb_cloud->points[i].y = realsense_xyz_cloud->points[i].y;
realsense_xyzrgb_cloud->points[i].z = realsense_xyz_cloud->points[i].z;
if(!getUVWithDXY(depth, i, uvx, uvy))
{
int cx = (int)(uvx * rgb_stream.width + 0.5f);
int cy = (int)(uvy * rgb_stream.height + 0.5f);
if (cx >= 0 && cx < rgb_stream.width && cy >= 0 && cy < rgb_stream.height)
{
unsigned char *rgb = rgb_frame.data + (cx+cy*rgb_stream.width)*3;
unsigned char r = rgb[2];
unsigned char g = rgb[1];
unsigned char b = rgb[0];
if(debug_depth_unit &&
pixel_x > -center_offset_pixel &&
pixel_x < center_offset_pixel &&
pixel_y > -center_offset_pixel &&
pixel_y < center_offset_pixel )
{
center_z += zz;
center_z_count++;
realsense_xyzrgb_cloud->points[i].rgba = (0 << 24) | (0 << 16) | (0 << 8) | 255;
}
else
{
realsense_xyzrgb_cloud->points[i].rgba = (0 << 24) | (r << 16) | (g << 8) | b;
}
}
else
{
realsense_xyzrgb_cloud->points[i].x = std::numeric_limits<float>::quiet_NaN();
realsense_xyzrgb_cloud->points[i].y = std::numeric_limits<float>::quiet_NaN();
realsense_xyzrgb_cloud->points[i].z = std::numeric_limits<float>::quiet_NaN();
realsense_xyzrgb_cloud->points[i].rgba = 0;
}
}
else
{
realsense_xyzrgb_cloud->points[i].x = std::numeric_limits<float>::quiet_NaN();
realsense_xyzrgb_cloud->points[i].y = std::numeric_limits<float>::quiet_NaN();
realsense_xyzrgb_cloud->points[i].z = std::numeric_limits<float>::quiet_NaN();
realsense_xyzrgb_cloud->points[i].rgba = 0;
}
}
}
else
{
realsense_xyz_cloud->points[i].x = std::numeric_limits<float>::quiet_NaN();
realsense_xyz_cloud->points[i].y = std::numeric_limits<float>::quiet_NaN();
realsense_xyz_cloud->points[i].z = std::numeric_limits<float>::quiet_NaN();
if(isHaveD2RGBUVMap)
{
realsense_xyzrgb_cloud->points[i].x = std::numeric_limits<float>::quiet_NaN();
realsense_xyzrgb_cloud->points[i].y = std::numeric_limits<float>::quiet_NaN();
realsense_xyzrgb_cloud->points[i].z = std::numeric_limits<float>::quiet_NaN();
realsense_xyzrgb_cloud->points[i].rgba = 0;
}
}
}
USE_TIMES_END_SHOW ( process_start, process_end, "fill point cloud data time" );
if(debug_depth_unit && center_z_count)
{
if(usbContext && usbHandle)
{
realsenseTemperature = getRealsenseTemperature(usbHandle);
}
center_z /= center_z_count;
printf("average center z value = %f temp = %d depth_unit = %f\n", center_z, realsenseTemperature, depth_unit);
center_z_count = 0;
}
USE_TIMES_END_SHOW ( start, end, "process result time" );
#if SHOW_RGBD_FRAME
cv::imshow("depth frame view", depth_frame);
#ifdef V4L2_PIX_FMT_INZI
cv::imshow("ir frame view", ir_frame);
#endif
cv::imshow("RGB frame view", rgb_frame);
#endif
//pub msgs
head_sequence_id++;
head_time_stamp = ros::Time::now();
pubRealSenseRGBImageMsg(rgb_frame);
#ifdef V4L2_PIX_FMT_INZI
pubRealSenseInfraredImageMsg(ir_frame);
#endif
//pubRealSenseDepthImageMsg(depth_frame);
pubRealSenseDepthImageMsg32F(cv_img_depth);
pubRealSensePointsXYZCloudMsg(realsense_xyz_cloud);
if(isHaveD2RGBUVMap)
{
pubRealSensePointsXYZRGBCloudMsg(realsense_xyzrgb_cloud);
}
}
void RGBDImageProc::RGBDCallback(
const ImageMsg::ConstPtr& rgb_msg,
const ImageMsg::ConstPtr& depth_msg,
const CameraInfoMsg::ConstPtr& rgb_info_msg,
const CameraInfoMsg::ConstPtr& depth_info_msg)
{
boost::mutex::scoped_lock(mutex_);
// for profiling
double dur_unwarp, dur_rectify, dur_reproject, dur_cloud, dur_allocate;
// **** initialize if needed
if (!initialized_)
{
initMaps(rgb_info_msg, depth_info_msg);
initialized_ = true;
}
// **** convert ros images to opencv Mat
cv_bridge::CvImageConstPtr rgb_ptr = cv_bridge::toCvShare(rgb_msg);
cv_bridge::CvImageConstPtr depth_ptr = cv_bridge::toCvShare(depth_msg);
const cv::Mat& rgb_img = rgb_ptr->image;
const cv::Mat& depth_img = depth_ptr->image;
//cv::imshow("RGB", rgb_img);
//cv::imshow("Depth", depth_img);
//cv::waitKey(1);
// **** rectify
ros::WallTime start_rectify = ros::WallTime::now();
cv::Mat rgb_img_rect, depth_img_rect;
cv::remap(rgb_img, rgb_img_rect, map_rgb_1_, map_rgb_2_, cv::INTER_LINEAR);
cv::remap(depth_img, depth_img_rect, map_depth_1_, map_depth_2_, cv::INTER_NEAREST);
dur_rectify = getMsDuration(start_rectify);
//cv::imshow("RGB Rect", rgb_img_rect);
//cv::imshow("Depth Rect", depth_img_rect);
//cv::waitKey(1);
// **** unwarp
if (unwarp_)
{
ros::WallTime start_unwarp = ros::WallTime::now();
unwarpDepthImage(depth_img_rect, coeff_0_rect_, coeff_1_rect_, coeff_2_rect_, fit_mode_);
dur_unwarp = getMsDuration(start_unwarp);
}
else dur_unwarp = 0.0;
// **** reproject
ros::WallTime start_reproject = ros::WallTime::now();
cv::Mat depth_img_rect_reg;
buildRegisteredDepthImage(intr_rect_depth_, intr_rect_rgb_, ir2rgb_,
depth_img_rect, depth_img_rect_reg);
dur_reproject = getMsDuration(start_reproject);
// **** point cloud
if (publish_cloud_)
{
ros::WallTime start_cloud = ros::WallTime::now();
PointCloudT::Ptr cloud_ptr;
cloud_ptr.reset(new PointCloudT());
buildPointCloud(depth_img_rect_reg, rgb_img_rect, intr_rect_rgb_, *cloud_ptr);
cloud_ptr->header = rgb_info_msg->header;
cloud_publisher_.publish(cloud_ptr);
dur_cloud = getMsDuration(start_cloud);
}
else dur_cloud = 0.0;
// **** allocate registered rgb image
ros::WallTime start_allocate = ros::WallTime::now();
cv_bridge::CvImage cv_img_rgb(rgb_msg->header, rgb_msg->encoding, rgb_img_rect);
ImageMsg::Ptr rgb_out_msg = cv_img_rgb.toImageMsg();
// **** allocate registered depth image
cv_bridge::CvImage cv_img_depth(depth_msg->header, depth_msg->encoding, depth_img_rect_reg);
ImageMsg::Ptr depth_out_msg = cv_img_depth.toImageMsg();
// **** update camera info (single, since both images are in rgb frame)
rgb_rect_info_msg_.header = rgb_info_msg->header;
dur_allocate = getMsDuration(start_allocate);
ROS_INFO("Rect: %.1f Reproj: %.1f Unwarp: %.1f Cloud %.1f Alloc: %.1f ms",
dur_rectify, dur_reproject, dur_unwarp, dur_cloud, dur_allocate);
// **** publish
rgb_publisher_.publish(rgb_out_msg);
depth_publisher_.publish(depth_out_msg);
info_publisher_.publish(rgb_rect_info_msg_);
}
void RGBDImageProc::RGBDCallback(
const ImageMsg::ConstPtr& rgb_msg,
const ImageMsg::ConstPtr& depth_msg,
const CameraInfoMsg::ConstPtr& rgb_info_msg,
const CameraInfoMsg::ConstPtr& depth_info_msg)
{
boost::mutex::scoped_lock(mutex_);
// for profiling
double dur_unwarp, dur_rectify, dur_reproject, dur_cloud, dur_allocate;
// **** images need to be the same size
if (rgb_msg->height != depth_msg->height ||
rgb_msg->width != depth_msg->width)
{
ROS_WARN("RGB and depth images have different sizes, skipping");
return;
}
// **** initialize if needed
if (size_in_.height != (int)rgb_msg->height ||
size_in_.width != (int)rgb_msg->width)
{
ROS_INFO("Initializing");
size_in_.height = (int)rgb_msg->height;
size_in_.width = (int)rgb_msg->width;
initMaps(rgb_info_msg, depth_info_msg);
}
// **** convert ros images to opencv Mat
cv_bridge::CvImageConstPtr rgb_ptr = cv_bridge::toCvShare(rgb_msg);
cv_bridge::CvImageConstPtr depth_ptr = cv_bridge::toCvShare(depth_msg);
const cv::Mat& rgb_img = rgb_ptr->image;
const cv::Mat& depth_img = depth_ptr->image;
//cv::imshow("RGB", rgb_img);
//cv::imshow("Depth", depth_img);
//cv::waitKey(1);
// **** rectify
ros::WallTime start_rectify = ros::WallTime::now();
cv::Mat rgb_img_rect, depth_img_rect;
cv::remap(rgb_img, rgb_img_rect, map_rgb_1_, map_rgb_2_, cv::INTER_LINEAR);
cv::remap(depth_img, depth_img_rect, map_depth_1_, map_depth_2_, cv::INTER_NEAREST);
dur_rectify = getMsDuration(start_rectify);
//cv::imshow("RGB Rect", rgb_img_rect);
//cv::imshow("Depth Rect", depth_img_rect);
//cv::waitKey(1);
// **** unwarp
if (unwarp_)
{
ros::WallTime start_unwarp = ros::WallTime::now();
rgbdtools::unwarpDepthImage(
depth_img_rect, coeff_0_rect_, coeff_1_rect_, coeff_2_rect_, fit_mode_);
dur_unwarp = getMsDuration(start_unwarp);
}
else dur_unwarp = 0.0;
// **** reproject
ros::WallTime start_reproject = ros::WallTime::now();
cv::Mat depth_img_rect_reg;
rgbdtools::buildRegisteredDepthImage(
intr_rect_depth_, intr_rect_rgb_, ir2rgb_, depth_img_rect, depth_img_rect_reg);
dur_reproject = getMsDuration(start_reproject);
// **** point cloud
if (publish_cloud_)
{
ros::WallTime start_cloud = ros::WallTime::now();
PointCloudT::Ptr cloud_ptr;
cloud_ptr.reset(new PointCloudT());
rgbdtools::buildPointCloud(
depth_img_rect_reg, rgb_img_rect, intr_rect_rgb_, *cloud_ptr);
cloud_ptr->header = rgb_info_msg->header;
cloud_publisher_.publish(cloud_ptr);
dur_cloud = getMsDuration(start_cloud);
}
else dur_cloud = 0.0;
// **** allocate registered rgb image
ros::WallTime start_allocate = ros::WallTime::now();
cv_bridge::CvImage cv_img_rgb(rgb_msg->header, rgb_msg->encoding, rgb_img_rect);
ImageMsg::Ptr rgb_out_msg = cv_img_rgb.toImageMsg();
// **** allocate registered depth image
cv_bridge::CvImage cv_img_depth(depth_msg->header, depth_msg->encoding, depth_img_rect_reg);
ImageMsg::Ptr depth_out_msg = cv_img_depth.toImageMsg();
// **** update camera info (single, since both images are in rgb frame)
rgb_rect_info_msg_.header = rgb_info_msg->header;
dur_allocate = getMsDuration(start_allocate);
// **** print diagnostics
double dur_total = dur_rectify + dur_reproject + dur_unwarp + dur_cloud + dur_allocate;
if(verbose_)
{
ROS_INFO("Rect %.1f Reproj %.1f Unwarp %.1f Cloud %.1f Alloc %.1f Total %.1f ms",
dur_rectify, dur_reproject, dur_unwarp, dur_cloud, dur_allocate,
dur_total);
}
// **** publish
rgb_publisher_.publish(rgb_out_msg);
depth_publisher_.publish(depth_out_msg);
info_publisher_.publish(rgb_rect_info_msg_);
}
