00001 #include <ros/ros.h>
00002
00003 #include <tf/transform_listener.h>
00004 #include <pcl/point_cloud.h>
00005 #include <pcl/point_types.h>
00006 #include <pcl_ros/point_cloud.h>
00007 #include <pcl_ros/transforms.h>
00008 #include <cv_bridge/cv_bridge.h>
00009 #include <opencv/cv.h>
00010 #include <sensor_msgs/image_encodings.h>
00011 #include <image_transport/image_transport.h>
00012 #include <image_geometry/pinhole_camera_model.h>
00013
00014 #include <hrl_head_registration/hsl_rgb_conversions.h>
00015
00016 typedef pcl::PointXYZRGB PRGB;
00017 typedef pcl::PointCloud<PRGB> PCRGB;
00018
00019 PCRGB::Ptr aligned_pc;
00020 ros::Subscriber align_sub;
00021
00022 image_transport::CameraSubscriber camera_sub;
00023 image_transport::Publisher overlay_pub;
00024 image_geometry::PinholeCameraModel cam_model;
00025 boost::shared_ptr<tf::TransformListener> tf_list;
00026
00027 void subAlignCallback(const PCRGB::Ptr& aligned_pc_)
00028 {
00029 tf::StampedTransform transform;
00030 if(!tf_list->waitForTransform("/base_link", aligned_pc_->header.frame_id,
00031 aligned_pc_->header.stamp, ros::Duration(3)))
00032 return;
00033 tf_list->lookupTransform("/base_link", aligned_pc_->header.frame_id,
00034 aligned_pc_->header.stamp, transform);
00035 aligned_pc = boost::shared_ptr<PCRGB>(new PCRGB());
00036 pcl_ros::transformPointCloud<PRGB>(*aligned_pc_, *aligned_pc, transform);
00037 }
00038
00039 void doOverlay(const sensor_msgs::ImageConstPtr& img_msg,
00040 const sensor_msgs::CameraInfoConstPtr& info_msg) {
00041
00042
00043 cam_model.fromCameraInfo(info_msg);
00044 cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::RGB8);
00045
00046 double alpha_mult;
00047 ros::param::param<double>("~alpha_mult", alpha_mult, 0.5);
00048
00049 uint8_t r, g, b;
00050 if(aligned_pc) {
00051 if(!tf_list->waitForTransform(img_msg->header.frame_id, "/base_link",
00052 img_msg->header.stamp, ros::Duration(3)))
00053 return;
00054 tf::StampedTransform transform;
00055 tf_list->lookupTransform(img_msg->header.frame_id, "/base_link",
00056 img_msg->header.stamp, transform);
00057 PCRGB::Ptr tf_pc(new PCRGB());
00058 pcl_ros::transformPointCloud<PRGB>(*aligned_pc, *tf_pc, transform);
00059 for(uint32_t i=0;i<tf_pc->size();i++) {
00060 cv::Point3d proj_pt_cv(tf_pc->points[i].x, tf_pc->points[i].y,
00061 tf_pc->points[i].z);
00062 cv::Point pt2d = cam_model.project3dToPixel(proj_pt_cv);
00063 extractRGB(tf_pc->points[i].rgb, r, g, b);
00064 if(pt2d.x >= 0 && pt2d.y >= 0 &&
00065 pt2d.x < cv_img->image.rows && pt2d.y < cv_img->image.cols) {
00066 double old_r = cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[0];
00067 double old_g = cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[1];
00068 double old_b = cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[2];
00069 cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[0] =
00070 (uint8_t) min(alpha_mult*old_r+r, 255.0);
00071 cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[1] =
00072 (uint8_t) min(alpha_mult*old_g+g, 255.0);
00073 cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[2] =
00074 (uint8_t) min(alpha_mult*old_b+b, 255.0);
00075 }
00076 }
00077 }
00078
00079 overlay_pub.publish(cv_img->toImageMsg());
00080 }
00081
00082 int main(int argc, char **argv)
00083 {
00084 ros::init(argc, argv, "head_registration");
00085 ros::NodeHandle nh;
00086 image_transport::ImageTransport img_trans(nh);
00087 tf_list = boost::shared_ptr<tf::TransformListener>(new tf::TransformListener());
00088
00089 align_sub = nh.subscribe("/head_registration/aligned_pc", 1, &subAlignCallback);
00090 camera_sub = img_trans.subscribeCamera("/kinect_camera", 1,
00091 &doOverlay);
00092 overlay_pub = img_trans.advertise("/head_registration/confirmation", 1);
00093
00094 ros::Rate r(5);
00095 while(ros::ok()) {
00096 cv::Mat image(480, 640, CV_8UC3);
00097 ros::spinOnce();
00098 r.sleep();
00099 }
00100
00101 return 0;
00102 }