pointcloud_colorizer.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <image_transport/image_transport.h>
00003 #include <opencv/cv.h>
00004 #include <opencv/highgui.h>
00005 #include <cv_bridge/CvBridge.h>
00006 #include <image_geometry/pinhole_camera_model.h>
00007 #include <pcl_ros/transforms.h>
00008 #include <tf/transform_listener.h>
00009 #include <tf/transform_broadcaster.h>
00010 #include <angles/angles.h>
00011 #include <sensor_msgs/PointCloud2.h>
00012 //boost
00013 #include <boost/thread/mutex.hpp>
00014 
00015 //for point_cloud::fromROSMsg
00016 #include <pcl/ros/conversions.h>
00017 #include <sensor_msgs/point_cloud_conversion.h>
00018 #include <pcl/point_types.h>
00019 #include <pcl/io/pcd_io.h>
00020 
00021 class PointCloudColorizer
00022 {
00023   //subscribers/publishers
00024   ros::NodeHandle nh_;
00025   image_transport::CameraSubscriber image_sub_;
00026   ros::Subscriber cloud_sub_;
00027 
00028   tf::TransformListener tf_listener_;
00029   //opencv bridge
00030   image_transport::ImageTransport it_;
00031   sensor_msgs::CvBridge bridge_;
00032   std::string input_image_topic_, input_cloud_topic_, cloud_rgb_topic_;
00033   bool monochrome_, save_image_;
00034   //pcl clouds
00035   pcl::PointCloud<pcl::PointXYZ> cloud_in_, cloud_in_tranformed_;
00036   cv::Mat image_;
00037   boost::mutex  cloud_lock_;
00038   std::vector <pcl::PointCloud<pcl::PointXYZ> > cloud_queue_;
00039   //pinhole cam model
00040   image_geometry::PinholeCameraModel cam_model_;
00041   sensor_msgs::PointCloud2::Ptr output_cloud_ptr_;
00042   ros::Publisher pub_output_;
00043 public:
00044   PointCloudColorizer(ros::NodeHandle &n)
00045     :  nh_(n), it_(nh_)
00046   {
00047     //    nh_.param("input_image_topic", input_image_topic_, std::string("/wide_stereo/right/image_rect_color"));
00048     nh_.param("input_image_topic", input_image_topic_, std::string("/wide_stereo/right/image_rect_color_throttle"));
00049     nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/points2_out"));
00050     nh_.param("monochrome", monochrome_, false);
00051     nh_.param("save_image", save_image_, false);
00052     nh_.param("cloud_rgb_topic", cloud_rgb_topic_, std::string("cloud_rgb"));
00053     image_sub_ = it_.subscribeCamera(input_image_topic_, 1, &PointCloudColorizer::image_cb, this);
00054     cloud_sub_= nh_.subscribe (input_cloud_topic_, 10, &PointCloudColorizer::cloud_cb, this);
00055     pub_output_ = nh_.advertise<sensor_msgs::PointCloud2> (cloud_rgb_topic_, 1);
00056     ROS_INFO("[PointCloudColorizer:] Subscribing to image topic %s", input_image_topic_.c_str());
00057     ROS_INFO("[PointCloudColorizer:] Subscribing to cloud topic %s", input_cloud_topic_.c_str());
00058   }
00059 
00060   void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc)
00061   {
00062     unsigned long size = pc->height*pc->width;
00063     ROS_INFO("[PointCloudColorizer: ] cloud received with points: %ld in frame %s", 
00064              size, pc->header.frame_id.c_str());
00065     pcl::fromROSMsg(*pc, cloud_in_);
00066     boost::mutex::scoped_lock lock(cloud_lock_);
00067     cloud_queue_.push_back(cloud_in_);
00068   }
00069 
00070   float
00071   getRGB (float r, float g, float b)
00072   {
00073     int32_t res = (int (r * 255) << 16) | (int (g*255) << 8) | int (b*255);
00074     float rgb = *reinterpret_cast<float*>(&res);
00075     return (rgb);
00076   }
00077 
00078   void image_cb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg)
00079   {
00080     //    ROS_INFO("[PointCloudColorizer: ] Image received in frame %s", 
00081     //        image_msg->header.frame_id.c_str());
00082     if (!cloud_queue_.empty())
00083     {
00084       ROS_INFO("[PointCloudColorizer:] Image encoding %s", image_msg->encoding.c_str());
00085       //image_ = NULL;
00086       try 
00087       {
00088         image_ = bridge_.imgMsgToCv(image_msg, "passthrough");
00089       }
00090       catch (sensor_msgs::CvBridgeException& ex) {
00091         ROS_ERROR("[PointCloudColorizer:] Failed to convert image");
00092         return;
00093       }
00094       cam_model_.fromCameraInfo(info_msg);
00095       for (unsigned long i = 0; i < cloud_queue_.size(); i++)
00096         process(cloud_queue_[i], image_);
00097       cloud_queue_.clear();
00098     }
00099   }
00100 
00101   void process (pcl::PointCloud<pcl::PointXYZ> &cloud_in, cv::Mat image)
00102   {
00103     pcl_ros::transformPointCloud(cam_model_.tfFrame(), cloud_in, cloud_in_tranformed_, tf_listener_);
00104     ROS_INFO("[PointCloudColorizer:] cloud transformed to %s", cam_model_.tfFrame().c_str());
00105     //create a sequence storage for projected points 
00106     CvMemStorage* stor = cvCreateMemStorage (0);
00107     CvSeq* seq = cvCreateSeq (CV_SEQ_ELTYPE_POINT, sizeof (CvSeq), sizeof (CvPoint), stor);
00108     if (cloud_in_tranformed_.points.size() == 0)
00109     {
00110       ROS_WARN("[PointCloudColorizer:] Point cloud points with size 0!!! Returning.");
00111       return;
00112     }
00113 
00114     boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > cloud_xyzrgb = boost::shared_ptr< pcl::PointCloud<pcl::PointXYZRGB> >(new pcl::PointCloud<pcl::PointXYZRGB>());
00115 
00116     cloud_xyzrgb->width    = cloud_in_tranformed_.points.size();
00117     cloud_xyzrgb->height   = 1;
00118     cloud_xyzrgb->is_dense = true;
00119     cloud_xyzrgb->header = cloud_in_tranformed_.header;
00120 
00121     std::cerr <<  cloud_in_tranformed_.points.size() <<  " " << cloud_in.points.size() << std::endl;
00122     pcl::PointXYZRGB point_xyzrgb;
00123     for (unsigned long i = 0; i < cloud_in_tranformed_.points.size(); i++)
00124     {
00125       cv::Point3d pt_cv(cloud_in_tranformed_.points[i].x, cloud_in_tranformed_.points[i].y, 
00126                       cloud_in_tranformed_.points[i].z);
00127       cv::Point2d uv;
00128       cam_model_.project3dToPixel(pt_cv, uv);
00129       ROS_DEBUG_STREAM("points projected: " << round(uv.x) << " " << round(uv.y));
00130       //ROS_INFO_STREAM("uv.x: " << round(uv.x) << "uv.y: " << round(uv.y));
00131    
00132       //if point projection outside of image assign rgb white value
00133       if (uv.x < 0 || uv.x > image.cols || uv.y < 0 || uv.y > image.rows)
00134       {
00135         //        ROS_WARN("[PointCloudColorizer:] width out of bounds");
00136         point_xyzrgb.rgb = getRGB(0.9, 0.0, 0.0);
00137         //continue;
00138       }
00139       else
00140       {
00141         ROS_DEBUG_STREAM("uv.x: " << round(uv.x) << "uv.y: " << round(uv.y));
00142 
00143         //uint8_t g = image.at<uint8_t>(round(uv.x), round(uv.y)*image.channels() + 3);
00144         //int32_t rgb = g | (g << 8) |  (g << 16);
00145         
00146         //VERSION for MONO
00147         if (monochrome_)
00148           {
00149             uint8_t g = image.at<uint8_t>(round(uv.y), round(uv.x));
00150             //int32_t rgb = g | (g << 8) |  (g << 16);
00151             int32_t rgb = (g << 16) | (g << 8) | g;
00152             point_xyzrgb.rgb = *reinterpret_cast<float*>(&rgb);
00153           }
00154         //VERSION for BGR8
00155         else
00156           {
00157             const cv::Vec3b& bgr = image.at<cv::Vec3b>(round(uv.y), round(uv.x));
00158             int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0];
00159             point_xyzrgb.rgb = *reinterpret_cast<float*>(&rgb_packed);
00160           }
00161       }
00162       //fill in points
00163       point_xyzrgb.x = cloud_in_tranformed_.points[i].x;
00164       point_xyzrgb.y = cloud_in_tranformed_.points[i].y;
00165       point_xyzrgb.z = cloud_in_tranformed_.points[i].z;
00166       cloud_xyzrgb->points.push_back(point_xyzrgb);
00167     }
00168     if (save_image_)
00169       {
00170         std::stringstream stamp;
00171         stamp << cloud_in_tranformed_.header.stamp;
00172         cv::imwrite(stamp.str() + ".png", image);
00173       }
00174     cloud_xyzrgb->width =  cloud_xyzrgb->points.size();
00175     boost::shared_ptr< sensor_msgs::PointCloud2> output_cloud = boost::shared_ptr<sensor_msgs::PointCloud2>(new sensor_msgs::PointCloud2()); 
00176     pcl::toROSMsg (*cloud_xyzrgb, *output_cloud);
00177     ROS_INFO("Publishing PointXZYRGB cloud with %ld points in frame %s", cloud_xyzrgb->points.size(), 
00178              output_cloud->header.frame_id.c_str());
00179     pub_output_.publish (output_cloud);
00180     cvClearSeq(seq);
00181     cvClearMemStorage(stor);
00182   } 
00183 };
00184 
00185 int main(int argc, char** argv)
00186 {
00187   ros::init(argc, argv, "pointcloud_colorizer_node");
00188   ros::NodeHandle n("~");
00189   PointCloudColorizer pp(n);
00190   ros::spin();
00191   //pp.spin();
00192 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


image_algos
Author(s): Dejan Pangercic
autogenerated on Thu May 23 2013 18:41:31