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