45 ConnectionBasedNodelet::onInit();
56 async_ = boost::make_shared<message_filters::Synchronizer<ASyncPolicy> >(1000);
62 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(1000);
77 const geometry_msgs::PointStamped::ConstPtr& uv_point,
78 const sensor_msgs::CameraInfo::ConstPtr& camera_info)
82 cv::Mat cv_depth_image = cv_ptr->image;
83 double depth_from_depth_sensor = cv_depth_image.at<
float>((int)uv_point->point.y, (
int)uv_point->point.x);
84 NODELET_INFO(
"timestamp diff is %f", (depth_image->header.stamp - uv_point->header.stamp).toSec());
85 NODELET_INFO(
"(u, v) = (%d, %d)", (
int)uv_point->point.x, (
int)uv_point->point.y);
86 NODELET_INFO(
"(z, d) = (%f, %f)", uv_point->point.z, depth_from_depth_sensor);
87 if (! isnan(depth_from_depth_sensor)) {
88 jsk_recognition_msgs::DepthErrorResult result;
89 result.header.frame_id = depth_image->header.frame_id;
90 result.header.stamp = depth_image->header.stamp;
91 result.u = (int)uv_point->point.x;
92 result.v = (
int)uv_point->point.y;
93 result.center_u = camera_info->P[2];
94 result.center_v = camera_info->P[6];
95 result.true_depth = uv_point->point.z;
96 result.observed_depth = depth_from_depth_sensor;
virtual void unsubscribe()
void publish(const boost::shared_ptr< M > &message) const
virtual void calcError(const sensor_msgs::Image::ConstPtr &depth_image, const geometry_msgs::PointStamped::ConstPtr &uv_point, const sensor_msgs::CameraInfo::ConstPtr &camera_info)
message_filters::Subscriber< sensor_msgs::Image > sub_image_
ros::Publisher depth_error_publisher_
boost::shared_ptr< message_filters::Synchronizer< ASyncPolicy > > async_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_camera_info_
const std::string TYPE_32FC1
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::DepthImageError, nodelet::Nodelet)
#define NODELET_INFO(...)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
message_filters::Subscriber< geometry_msgs::PointStamped > sub_point_