$search
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 }