pointcloud_to_image_projector_halcon.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 <tf/transform_listener.h>
00008 #include <boost/foreach.hpp>
00009 #include <tf/transform_broadcaster.h>
00010 #include <angles/angles.h>
00011 #include <sensor_msgs/PointCloud.h>
00012 //boost
00013 #include <boost/thread/mutex.hpp>
00014 
00015 class PointCloudToImageProjector
00016 {
00017   //subscribers/publishers
00018   ros::NodeHandle nh_;
00019   image_transport::Publisher image_pub_;
00020   image_transport::Subscriber image_sub_;
00021   ros::Subscriber cloud_sub_;
00022 
00023   tf::TransformListener tf_listener_;
00024   //opencv bridge
00025   image_transport::ImageTransport it_;
00026   sensor_msgs::CvBridge bridge_;
00027   std::string input_image_topic_, input_cloud_topic_, output_cluster_topic_, output_image_topic_;
00028   //calibration parameters for svistec cameras
00029   double focal_length_, proj_center_x_, proj_center_y_, pix_size_x_, pix_size_y_;
00030   //ROS msgs
00031   sensor_msgs::PointCloud cloud_in_;
00032   IplImage* image_;
00033   std::string origin_, interim_, child_;
00034   boost::mutex  cloud_lock_;
00035   std::vector <sensor_msgs::PointCloud> cloud_queue_;
00036 
00037 public:
00038   PointCloudToImageProjector(ros::NodeHandle &n)
00039     :  nh_(n), it_(nh_)
00040   {
00041     nh_.param("origin", origin_, std::string("/LeftEyeCalc"));
00042     nh_.param("child", child_, std::string("/sr4"));
00043     nh_.param("input_image_topic", input_image_topic_, std::string("/cop/left/camera"));
00044     nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/swissranger_test/cloud_sr"));
00045     nh_.param("output_cluster_topic", output_cluster_topic_, std::string("output_cluster"));
00046     nh_.param("output_image_topic", output_image_topic_, std::string("image_with_projected_cluster"));
00047     nh_.param("focal_length", focal_length_, 0.00641331974023884);
00048     nh_.param("proj_center_x", proj_center_x_, 833.248646581603);
00049     nh_.param("proj_center_y", proj_center_y_, 661.107370424523);
00050     nh_.param("pix_size_x", pix_size_x_, 7.43100103980579e-06);
00051     nh_.param("pix_size_y", pix_size_y_, 7.4e-06);
00052     image_sub_ = it_.subscribe(input_image_topic_, 1, &PointCloudToImageProjector::image_cb, this);
00053     cloud_sub_= nh_.subscribe (input_cloud_topic_, 10, &PointCloudToImageProjector::cloud_cb, this);
00054     image_pub_ = it_.advertise(output_image_topic_, 1);
00055   }
00056 
00057   void Project3DPoint(const double &x, const double &y, const double &z, int &row, int &column)
00058   {
00059     double temp1,temp2;
00060     if (focal_length_ > 0.0)
00061     {
00062       temp1 = focal_length_ * (x / z );
00063       temp2 = focal_length_ * (y / z );
00064     }
00065     else
00066     {
00067       temp1 = x;
00068       temp2 = y;
00069     }
00070     column = ( temp1 / pix_size_x_ ) + proj_center_x_;
00071     row = ( temp2 / pix_size_y_ ) + proj_center_y_;
00072   }
00073 
00074    void cloud_cb (const sensor_msgs::PointCloudConstPtr& pc)
00075     {
00076       ROS_INFO("[PointCloudToImageProjector: ] cloud received with points: %ld", pc->points.size());
00077       try
00078       {
00079         tf_listener_.transformPointCloud(origin_, *pc, cloud_in_);
00080       }
00081       catch (tf::TransformException ex)
00082       {
00083         ROS_ERROR("[PointCloudToImageProjector:] %s",ex.what());
00084       }
00085       ROS_INFO("[PointCloudToImageProjector:] cloud transformed to %s", origin_.c_str());
00086       boost::mutex::scoped_lock lock(cloud_lock_);
00087       cloud_queue_.push_back(cloud_in_);
00088     }
00089 
00090 
00091   void image_cb(const sensor_msgs::ImageConstPtr& image_msg)
00092   {
00093     if (!cloud_queue_.empty())
00094     {
00095       image_ = NULL;
00096       try 
00097       {
00098         image_ = bridge_.imgMsgToCv(image_msg, "rgb8");
00099       }
00100       catch (sensor_msgs::CvBridgeException& ex) {
00101         ROS_ERROR("[PointCloudToImageProjector:] Failed to convert image");
00102         return;
00103       }
00104       for (unsigned long i = 0; i < cloud_queue_.size(); i++)
00105         process(cloud_queue_[i], image_);
00106       cloud_queue_.clear();
00107     }
00108   }
00109 
00110   void process (sensor_msgs::PointCloud &cloud_in, IplImage *image)
00111   {
00112     //create a sequence storage for projected points 
00113     CvMemStorage* stor = cvCreateMemStorage (0);
00114     CvSeq* seq = cvCreateSeq (CV_SEQ_ELTYPE_POINT, sizeof (CvSeq), sizeof (CvPoint), stor);
00115     if (cloud_in.points.size() == 0)
00116     {
00117       ROS_WARN("[PointCloudToImageProjector:] Point cloud points with size 0!!! Returning.");
00118       return;
00119     }
00120     for (unsigned long i = 0; i < cloud_in.points.size(); i++)
00121     {
00122       cv::Point3d pt_cv(cloud_in.points[i].x, cloud_in.points[i].y, cloud_in.points[i].z);
00123       CvPoint uv;
00124       Project3DPoint(pt_cv.x, pt_cv.y, pt_cv.z, uv.y, uv.x);
00125       if (uv.x < 0 || uv.x > image->width)
00126       {
00127         ROS_WARN("[PointCloudToImageProjector:] width out of bounds");
00128         continue;
00129       }
00130       if (uv.y < 0 || uv.y > image->height)
00131       {
00132         ROS_WARN("[PointCloudToImageProjector:] height out of bounds");
00133         continue;
00134       }
00135       //static const int RADIUS = 3;
00136       //cvCircle(image, uv, RADIUS, CV_RGB(255,0,0), -1);
00137       cvSeqPush( seq, &uv );
00138     }
00139     
00140     CvRect rect = cvBoundingRect(seq);
00141     if (rect.width == 0 || rect.height == 0)
00142     {
00143       ROS_WARN("[PointCloudToImageProjector:] Rectangle's width = height = 0. This is impossible. Skipping ROI extraction.");
00144       return;
00145     }
00146     CvPoint pt1, pt2;
00147     pt1.x = rect.x;
00148     //check boundary, increase ROI for 10%
00149     if ((rect.x + (rect.width + 0.1*rect.width)) > image->width)
00150     {
00151       rect.width =  image->width + - rect.x;
00152     }
00153     else 
00154       rect.width = rect.width + 0.1 * rect.width;
00155     
00156     pt2.x = rect.x + rect.width;
00157     pt1.y = rect.y;
00158     
00159     //check boundary, increase ROI for 10%
00160     if ((rect.y + (rect.height + 0.1*rect.height)) > image->height)
00161     {
00162       rect.height =  image->height +  - rect.y;
00163     }
00164     else
00165       rect.height = rect.height + 0.1 * rect.height;
00166     pt2.y = rect.y+rect.height;
00167     
00168     //draw rectangle around the points
00169     //cvRectangle(image, pt1, pt2, CV_RGB(255,0,0), 3, 8, 0 );
00170     cvClearMemStorage( stor );
00171     
00172     //get subimage, aka region of interest
00173     IplImage *subimage;
00174     cvSetImageROI(image,rect);
00175     // sub-image
00176     subimage = cvCreateImage( cvSize(rect.width, rect.height), image->depth, image->nChannels );
00177     cvCopy(image, subimage);
00178     cvResetImageROI(image); // release image ROI
00179     image_pub_.publish(bridge_.cvToImgMsg(subimage, "rgb8"));
00180     ROS_INFO("[PointCloudToImageProjector:] image published to %s", output_image_topic_.c_str());
00181     cvReleaseImage(&subimage);
00182   }
00183 };
00184 
00185 int main(int argc, char** argv)
00186 {
00187   ros::init(argc, argv, "pointcloud_to_image_projector_node");
00188   ros::NodeHandle n("~");
00189   PointCloudToImageProjector 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