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_); }