pointcloud_to_image_projector_opencv.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 <cv_bridge/cv_bridge.h>
00007 #include <image_geometry/pinhole_camera_model.h>
00008 //for pcl::transformPointCloud
00009 #include <pcl_ros/transforms.h>
00010 #include <tf/transform_listener.h>
00011 #include <tf/transform_broadcaster.h>
00012 #include <angles/angles.h>
00013 #include <sensor_msgs/PointCloud.h>
00014 //boost
00015 #include <boost/thread/mutex.hpp>
00016 
00017 //for point_cloud::fromROSMsg
00018 #include <pcl/ros/conversions.h>
00019 #include <sensor_msgs/point_cloud_conversion.h>
00020 #include <pcl/point_types.h>
00021 #include <pcl/io/pcd_io.h>
00022 
00023 #include "pcl/filters/extract_indices.h"
00024 #include "pcl/segmentation/extract_clusters.h"
00025 #include "pcl/kdtree/kdtree_flann.h"
00026 
00027 #include <boost/algorithm/string.hpp>
00028 #include <boost/filesystem.hpp>
00029 #include "pcl/common/common.h"
00030 
00031 #include <geometry_msgs/PolygonStamped.h>
00032 typedef pcl::PointXYZ Point;
00033 typedef pcl::KdTree<Point>::Ptr KdTreePtr;
00034 
00035 class PointCloudToImageProjector
00036 {
00037   //subscribers/publishers
00038   ros::NodeHandle nh_;
00039   image_transport::Publisher image_pub_;
00040   image_transport::CameraSubscriber image_sub_;
00041   ros::Subscriber cloud_sub_;
00042   ros::Publisher polygon_pub_;
00043 
00044   tf::TransformListener tf_listener_;
00045   //opencv bridge
00046   image_transport::ImageTransport it_;
00047   sensor_msgs::CvBridge bridge_;
00048   std::string input_image_topic_, input_cloud_topic_, output_cluster_topic_, output_image_topic_, output_polygon_points_topic_;
00049   //pcl clouds
00050   pcl::PointCloud<pcl::PointXYZ> cloud_in_, cloud_in_tranformed_;
00051   IplImage* image_;
00052   boost::mutex  cloud_lock_;
00053   std::vector <pcl::PointCloud<pcl::PointXYZ> > cloud_queue_, cluster_queue_;
00054   //pinhole cam model
00055   image_geometry::PinholeCameraModel cam_model_;
00056   int offset_, nr_clusters_;
00057   pcl::EuclideanClusterExtraction<Point> cluster_;
00058   int object_cluster_min_size_, counter_, count_images_;
00059   double object_cluster_tolerance_;
00060   KdTreePtr clusters_tree_;
00061   bool cluster_cloud_, save_data_;
00062   std::string image_cluster_name_, location_;
00063   pcl::PCDWriter pcd_writer_;
00064   pcl::PointXYZ point_center_, point_min_, point_max_;
00065   geometry_msgs::PolygonStamped polygon_points_;
00066 
00067 public:
00068   PointCloudToImageProjector(ros::NodeHandle &n)
00069     :  nh_(n), it_(nh_)
00070   {
00071     nh_.param("input_image_topic", input_image_topic_, std::string("/wide_stereo/right/image_rect_color"));
00072     nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/laser_table_detector/extract_object_clusters/output"));
00073     nh_.param("output_cluster_topic", output_cluster_topic_, std::string("output_cluster"));
00074     nh_.param("output_image_topic", output_image_topic_, std::string("image_with_projected_cluster"));
00075     nh_.param("output_polygon_points_topic", output_polygon_points_topic_, std::string("polygon_points"));
00076     nh_.param("offset", offset_, 0);
00077     nh_.param("object_cluster_min_size", object_cluster_min_size_, 100);
00078     nh_.param("object_cluster_tolerance", object_cluster_tolerance_, 0.03);
00079     nh_.param("cluster_cloud", cluster_cloud_, false);
00080     nh_.param("image_cluster_name", image_cluster_name_, std::string(""));
00081     nh_.param("location", location_, std::string("island"));
00082     nh_.param("save_data", save_data_, false);
00083     nh_.param("nr_clusters_", nr_clusters_, 1);
00084     counter_ = count_images_ = 0;
00085     image_sub_ = it_.subscribeCamera(input_image_topic_, 1, &PointCloudToImageProjector::image_cb, this);
00086     cloud_sub_= nh_.subscribe (input_cloud_topic_, 10, &PointCloudToImageProjector::cloud_cb, this);
00087     image_pub_ = it_.advertise(output_image_topic_, 100);
00088     polygon_pub_ = nh_.advertise<geometry_msgs::PolygonStamped>(output_polygon_points_topic_, 1);
00089     clusters_tree_ = boost::make_shared<pcl::KdTreeFLANN<Point> > ();
00090     clusters_tree_->setEpsilon (1);
00091   }
00092 
00093   void cluster_cloud (pcl::PointCloud<pcl::PointXYZ> &cloud_in, std::vector<pcl::PointCloud<pcl::PointXYZ> > &cluster_queue)
00094   {
00095     std::vector<pcl::PointIndices> clusters;
00096     cluster_.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_in));
00097     cluster_.setClusterTolerance (object_cluster_tolerance_);
00098     cluster_.setMinClusterSize (object_cluster_min_size_);
00099     //    cluster_.setMaxClusterSize (object_cluster_max_size_);
00100     cluster_.setSearchMethod (clusters_tree_);
00101     cluster_.extract (clusters);
00102     
00103     ROS_INFO("[PointCloudToImageProjector: ] Found clusters: %ld", clusters.size());
00104     
00105     cluster_queue.resize(clusters.size());
00106     for (unsigned int i = 0; i < clusters.size(); i++)
00107       {
00108         pcl::copyPointCloud (cloud_in, clusters[i], cluster_queue[i]);
00109       }
00110   }
00111 
00112   void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc)
00113   {
00114     unsigned long size = pc->height*pc->width;
00115     ROS_INFO("[PointCloudToImageProjector: ] cloud received with points: %ld in frame %s", 
00116              size, pc->header.frame_id.c_str());
00117     pcl::fromROSMsg(*pc, cloud_in_);
00118     boost::mutex::scoped_lock lock(cloud_lock_);
00119     cloud_queue_.push_back(cloud_in_);
00120     counter_++;
00121   }
00122 
00123 
00124   void image_cb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg)
00125   {
00126     //ROS_INFO("[PointCloudToImageProjector: ] Image received in frame %s", 
00127     //       image_msg->header.frame_id.c_str());
00128     if (!cloud_queue_.empty())
00129     {
00130       image_ = NULL;
00131       try 
00132       {
00133         image_ = bridge_.imgMsgToCv(image_msg, "rgb8");
00134       }
00135       catch (sensor_msgs::CvBridgeException& ex) {
00136         ROS_ERROR("[PointCloudToImageProjector:] Failed to convert image");
00137         return;
00138       }
00139       cam_model_.fromCameraInfo(info_msg);
00140       for (unsigned long i = 0; i < cloud_queue_.size(); i++)
00141       {
00142         if (cluster_cloud_)
00143         {
00144           cluster_cloud (cloud_queue_[i], cluster_queue_);
00145           for (unsigned long j = 0; j < cluster_queue_.size(); j++)
00146           {
00147             process(cluster_queue_[j], image_);
00148           }
00149           cluster_queue_.clear();
00150         }
00151         else
00152         {
00153           process(cloud_queue_[i], image_);
00154         }
00155       }
00156       cloud_queue_.clear();
00157     }
00158   }
00159 
00160   // bool get_projected_points (image_algos::GetProjectedPoints &req, image_algos::GetProjectedPoints &res)
00161   //   {
00162   //     boost::mutex::scoped_lock lock(polygon_points_lock_);
00163   //     res.points = ;
00164   //     return true;
00165   //   }
00166 
00167   void process (pcl::PointCloud<pcl::PointXYZ> &cloud_in, IplImage *image)
00168   {
00169     //sleep(2.0);
00170     if (cloud_in.width == 1 && cloud_in.height == 1)
00171       {
00172         // cv::Mat token(480, 1, CV_8U);
00173         // cv_bridge::CvImagePtr cv_ptr;
00174         // cv_ptr->image = token;
00175         // image_pub_.publish(cv_ptr->toImageMsg());
00176         // //   image_pub_.publish(bridge_.toImageMsg(token, "rgb8"));
00177         IplImage * token = cvCreateImage(cvSize(2, 480), image->depth, image->nChannels);
00178         cvZero(token);
00179         image_pub_.publish(bridge_.cvToImgMsg(token));
00180         ROS_INFO("[PointCloudToImageProjector:] Published TOKEN Image");
00181         cvReleaseImage(&token);
00182         return;
00183       }
00184     pcl_ros::transformPointCloud(cam_model_.tfFrame(), cloud_in, cloud_in_tranformed_, tf_listener_);
00185     ROS_INFO("[PointCloudToImageProjector:] cloud transformed to %s", cam_model_.tfFrame().c_str());
00186     //create a sequence storage for projected points 
00187     CvMemStorage* stor = cvCreateMemStorage (0);
00188     CvSeq* seq = cvCreateSeq (CV_SEQ_ELTYPE_POINT, sizeof (CvSeq), sizeof (CvPoint), stor);
00189     if (cloud_in_tranformed_.points.size() == 0)
00190     {
00191       ROS_WARN("[PointCloudToImageProjector:] Point cloud points with size 0!!! Returning.");
00192       return;
00193     }
00194     
00195     polygon_points_.polygon.points.clear();
00196     geometry_msgs::Point32 tmp_pt;
00197     for (unsigned long i = 0; i < cloud_in_tranformed_.points.size(); i++)
00198     {
00199       cv::Point3d pt_cv(cloud_in_tranformed_.points[i].x, cloud_in_tranformed_.points[i].y, 
00200                         cloud_in_tranformed_.points[i].z);
00201       cv::Point2d uv;
00202       cam_model_.project3dToPixel(pt_cv, uv);
00203       ROS_DEBUG_STREAM("points projected: " << uv.x << " " << uv.y);
00204       //     Project3DPoint(pt_cv.x, pt_cv.y, pt_cv.z, uv.y, uv.x);
00205       if (uv.x < 0 || uv.x > image->width)
00206       {
00207         //ROS_WARN("[PointCloudToImageProjector:] width out of bounds");
00208         continue;
00209       }
00210       if (uv.y < 0 || uv.y > image->height)
00211       {
00212         //ROS_WARN("[PointCloudToImageProjector:] height out of bounds");
00213         continue;
00214       }
00215       //static const int RADIUS = 3;
00216       //cvCircle(image, uv, RADIUS, CV_RGB(255,0,0), -1);
00217       CvPoint pt;
00218       pt.x = uv.x;
00219       pt.y = uv.y;
00220       tmp_pt.x = uv.x;
00221       tmp_pt.y = uv.y;
00222       tmp_pt.z = 0.0;
00223       polygon_points_.polygon.points.push_back(tmp_pt);
00224       cvSeqPush( seq, &pt );
00225     }
00226     polygon_points_.header.stamp = ros::Time::now();
00227     polygon_points_.header.seq = 0;
00228     polygon_pub_.publish(polygon_points_);
00229     ROS_INFO("Polygon with %ld points published", polygon_points_.polygon.points.size());
00230     
00231     CvRect rect = cvBoundingRect(seq);
00232 
00233     ROS_DEBUG_STREAM("rect: " << rect.x << " " << rect.y << " " << rect.width << " " << rect.height);
00234 
00235     if (rect.width == 0 || rect.height == 0)
00236     {
00237       ROS_WARN("[PointCloudToImageProjector:] Rectangle's width = height = 0. This is impossible. Skipping ROI extraction.");
00238       return;
00239     }
00240     CvPoint pt1, pt2;
00241     pt1.x = rect.x;
00242     //check boundary, increase ROI for 10%
00243     if ((rect.x + (rect.width + 0.1*rect.width)) > image->width)
00244     {
00245       rect.width =  image->width + - rect.x;
00246     }
00247     else 
00248       rect.width = rect.width + 0.1 * rect.width;
00249     
00250     pt2.x = rect.x + rect.width;
00251     pt1.y = rect.y;
00252     
00253     //check boundary, increase ROI for 10%
00254     if ((rect.y + (rect.height + 0.1*rect.height)) > image->height)
00255     {
00256       rect.height =  image->height +  - rect.y;
00257     }
00258     else
00259       rect.height = rect.height + 0.1 * rect.height;
00260     pt2.y = rect.y+rect.height;
00261     
00262     //draw rectangle around the points
00263     //cvRectangle(image, pt1, pt2, CV_RGB(255,0,0), 3, 8, 0 );
00264     //cvClearMemStorage( stor );
00265     
00266     //get subimage, aka region of interest
00267     rect.x = rect.x - offset_;
00268     rect.y = rect.y - offset_;
00269     rect.height = rect.height + 2*offset_;
00270     rect.width = rect.width + 2*offset_;
00271     IplImage *subimage;
00272     cvSetImageROI(image,rect);
00273     // sub-image
00274     subimage = cvCreateImage( cvSize(rect.width, rect.height), image->depth, image->nChannels );
00275     cvCopy(image, subimage);
00276     cvResetImageROI(image); // release image ROI
00277     image_pub_.publish(bridge_.cvToImgMsg(subimage, "rgb8"));
00278 
00279     if (save_data_)
00280     {
00281       char counter_char[100];
00282       sprintf (counter_char, "%04d",  counter_);
00283       std::stringstream ss;
00284       std::stringstream ss_position;
00285       getMinMax3D (cloud_in, point_min_, point_max_);
00286       //Calculate the centroid of the cluster
00287       point_center_.x = (point_max_.x + point_min_.x)/2;
00288       point_center_.y = (point_max_.y + point_min_.y)/2;
00289       point_center_.z = (point_max_.z + point_min_.z)/2;
00290       ss_position << point_center_.x << "_" << point_center_.y << "_" << point_center_.z;
00291 
00292       ss << ros::Time::now();
00293       image_cluster_name_ = location_ + "/" + std::string(counter_char) + "/" +  "NAME" + "_" + ss.str() + "_" + ss_position.str();
00294      
00295       boost::filesystem::path outpath (location_ + "/" + std::string(counter_char));
00296       if (!boost::filesystem::exists (outpath))
00297       {
00298         if (!boost::filesystem::create_directories (outpath))
00299         {
00300           ROS_ERROR ("Error creating directory %s.", (location_ + "/" + std::string(counter_char)).c_str ());
00301           //return (-1);
00302         }
00303         ROS_INFO ("Creating directory %s", (location_ + "/" + std::string(counter_char)).c_str ());
00304       }
00305       
00306       pcd_writer_.write (image_cluster_name_ + ".pcd", cloud_in_tranformed_, true);
00307       cv::imwrite(image_cluster_name_ + ".png", subimage);
00308     }
00309     ROS_INFO("[PointCloudToImageProjector:] image with size %d %d published to %s", subimage->width, subimage->height, output_image_topic_.c_str());
00310     cvReleaseImage(&subimage);
00311   }
00312 };
00313 
00314 int main(int argc, char** argv)
00315 {
00316   ros::init(argc, argv, "pointcloud_to_image_projector_opencv_node");
00317   ros::NodeHandle n("~");
00318   PointCloudToImageProjector pp(n);
00319   ros::spin();
00320   //pp.spin();
00321 }
 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