Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include "jsk_pcl_ros_utils/depth_image_error.h"
00037 #include <pluginlib/class_list_macros.h>
00038 #include <cv_bridge/cv_bridge.h>
00039 #include <sensor_msgs/image_encodings.h>
00040
00041 namespace jsk_pcl_ros_utils
00042 {
00043 void DepthImageError::onInit()
00044 {
00045 ConnectionBasedNodelet::onInit();
00046 pnh_->param("approximate_sync", approximate_sync_, false);
00047 depth_error_publisher_ = advertise<jsk_recognition_msgs::DepthErrorResult>(*pnh_, "output", 1);
00048 }
00049
00050 void DepthImageError::subscribe()
00051 {
00052 sub_image_.subscribe(*pnh_, "image", 1);
00053 sub_point_.subscribe(*pnh_, "point", 1);
00054 sub_camera_info_.subscribe(*pnh_, "camera_info", 1);
00055 if (approximate_sync_) {
00056 async_ = boost::make_shared<message_filters::Synchronizer<ASyncPolicy> >(1000);
00057 async_->connectInput(sub_image_, sub_point_, sub_camera_info_);
00058 async_->registerCallback(boost::bind(&DepthImageError::calcError,
00059 this, _1, _2, _3));
00060 }
00061 else {
00062 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(1000);
00063 sync_->connectInput(sub_image_, sub_point_, sub_camera_info_);
00064 sync_->registerCallback(boost::bind(&DepthImageError::calcError,
00065 this, _1, _2, _3));
00066 }
00067 }
00068
00069 void DepthImageError::unsubscribe()
00070 {
00071 sub_image_.unsubscribe();
00072 sub_point_.unsubscribe();
00073 }
00074
00075
00076 void DepthImageError::calcError(const sensor_msgs::Image::ConstPtr& depth_image,
00077 const geometry_msgs::PointStamped::ConstPtr& uv_point,
00078 const sensor_msgs::CameraInfo::ConstPtr& camera_info)
00079 {
00080 cv_bridge::CvImagePtr cv_ptr;
00081 cv_ptr = cv_bridge::toCvCopy(depth_image, sensor_msgs::image_encodings::TYPE_32FC1);
00082 cv::Mat cv_depth_image = cv_ptr->image;
00083 double depth_from_depth_sensor = cv_depth_image.at<float>((int)uv_point->point.y, (int)uv_point->point.x);
00084 NODELET_INFO("timestamp diff is %f", (depth_image->header.stamp - uv_point->header.stamp).toSec());
00085 NODELET_INFO("(u, v) = (%d, %d)", (int)uv_point->point.x, (int)uv_point->point.y);
00086 NODELET_INFO("(z, d) = (%f, %f)", uv_point->point.z, depth_from_depth_sensor);
00087 if (! isnan(depth_from_depth_sensor)) {
00088 jsk_recognition_msgs::DepthErrorResult result;
00089 result.header.frame_id = depth_image->header.frame_id;
00090 result.header.stamp = depth_image->header.stamp;
00091 result.u = (int)uv_point->point.x;
00092 result.v = (int)uv_point->point.y;
00093 result.center_u = camera_info->P[2];
00094 result.center_v = camera_info->P[6];
00095 result.true_depth = uv_point->point.z;
00096 result.observed_depth = depth_from_depth_sensor;
00097 depth_error_publisher_.publish(result);
00098 }
00099 }
00100 }
00101
00102 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::DepthImageError, nodelet::Nodelet);