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 #define BOOST_PARAMETER_MAX_ARITY 7
00037
00038 #include "jsk_pcl_ros/add_color_from_image.h"
00039 #include <image_geometry/pinhole_camera_model.h>
00040 #include <cv_bridge/cv_bridge.h>
00041 #include <sensor_msgs/image_encodings.h>
00042 #include <pcl_conversions/pcl_conversions.h>
00043
00044 namespace jsk_pcl_ros
00045 {
00046 void AddColorFromImage::onInit()
00047 {
00048 DiagnosticNodelet::onInit();
00049 pub_ = advertise<sensor_msgs::PointCloud2>(
00050 *pnh_, "output", 1);
00051 }
00052
00053 void AddColorFromImage::subscribe()
00054 {
00055 sub_cloud_.subscribe(*pnh_, "input", 1);
00056 sub_image_.subscribe(*pnh_, "input/image", 1);
00057 sub_info_.subscribe(*pnh_, "input/camera_info", 1);
00058 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00059 sync_->connectInput(sub_cloud_, sub_image_, sub_info_);
00060 sync_->registerCallback(boost::bind(&AddColorFromImage::addColor,
00061 this, _1, _2, _3));
00062 }
00063
00064 void AddColorFromImage::unsubscribe()
00065 {
00066 sub_cloud_.unsubscribe();
00067 sub_info_.unsubscribe();
00068 sub_image_.unsubscribe();
00069 }
00070
00071 void AddColorFromImage::addColor(
00072 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00073 const sensor_msgs::Image::ConstPtr& image_msg,
00074 const sensor_msgs::CameraInfo::ConstPtr& info_msg)
00075 {
00076 if ((cloud_msg->header.frame_id != image_msg->header.frame_id) ||
00077 (cloud_msg->header.frame_id != info_msg->header.frame_id)) {
00078 JSK_NODELET_FATAL("frame_id is not collect: [%s, %s, %s",
00079 cloud_msg->header.frame_id.c_str(),
00080 image_msg->header.frame_id.c_str(),
00081 info_msg->header.frame_id.c_str());
00082 return;
00083 }
00084 vital_checker_->poke();
00085 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
00086 (new pcl::PointCloud<pcl::PointXYZ>);
00087 pcl::fromROSMsg(*cloud_msg, *cloud);
00088
00089 pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgb_cloud
00090 (new pcl::PointCloud<pcl::PointXYZRGB>);
00091 rgb_cloud->points.resize(cloud->points.size());
00092 rgb_cloud->is_dense = cloud->is_dense;
00093 rgb_cloud->width = cloud->width;
00094 rgb_cloud->height = cloud->height;
00095 cv::Mat image = cv_bridge::toCvCopy(
00096 image_msg, sensor_msgs::image_encodings::BGR8)->image;
00097 image_geometry::PinholeCameraModel model;
00098 model.fromCameraInfo(info_msg);
00099 for (size_t i = 0; i < cloud->points.size(); i++) {
00100 pcl::PointXYZRGB p;
00101 p.x = cloud->points[i].x;
00102 p.y = cloud->points[i].y;
00103 p.z = cloud->points[i].z;
00104
00105 if (!isnan(p.x) && !isnan(p.y) && !isnan(p.z)) {
00106
00107 cv::Point2d uv = model.project3dToPixel(cv::Point3d(p.x, p.y, p.z));
00108 if (uv.x > 0 && uv.x < image_msg->width &&
00109 uv.y > 0 && uv.y < image_msg->height) {
00110 cv::Vec3b rgb = image.at<cv::Vec3b>(uv.y, uv.x);
00111 p.r = rgb[2];
00112 p.g = rgb[1];
00113 p.b = rgb[0];
00114 }
00115 }
00116 rgb_cloud->points[i] = p;
00117 }
00118 sensor_msgs::PointCloud2 ros_cloud;
00119 pcl::toROSMsg(*rgb_cloud, ros_cloud);
00120 ros_cloud.header = cloud_msg->header;
00121 pub_.publish(ros_cloud);
00122 }
00123 }
00124
00125 #include <pluginlib/class_list_macros.h>
00126 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::AddColorFromImage, nodelet::Nodelet);