ccd_node.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <ros/node_handle.h>
00003 // #include "sensor_msgs/Image.h"
00004 #include <sensor_msgs/image_encodings.h>
00005 #include <image_transport/image_transport.h>
00006 #include <cv_bridge/CvBridge.h>
00007 #include <opencv/cv.h>
00008 #include <opencv2/legacy/legacy.hpp>
00009 #include <opencv/highgui.h>
00010 #include <string.h>
00011 #include <iostream>
00012 #include "ccd/auto_init.h"
00013 #include <ccd/bspline.h>
00014 #include <ccd/ccd.h>
00015 #include <geometry_msgs/PolygonStamped.h>
00016 
00017 //class ImageConverter {
00018 
00019 void on_mouse(int event, int x, int y, int flags, void *param );
00020 
00021 class CCDNode
00022 {
00023  public:
00024   ros::NodeHandle n_;
00025   image_transport::ImageTransport it_;
00026   image_transport::Subscriber image_sub_;
00027   ros::Subscriber polygon_sub_;
00028   sensor_msgs::CvBridge bridge_;
00029   std::string image_topic_, polygon_points_topic_;
00030   std::vector<cv::Point3d> pts_;
00031   CCD ccd;
00032   int count_;
00033   int init_method_;
00034   bool got_polygon_;
00035   CCDNode(ros::NodeHandle &n, int init_method) :
00036       n_(n), it_(n_)
00037   {
00038     // n_.param("image_topic", image_topic_, std::string("/narrow_stereo/left/image_rect"));
00039     // n_.param("image_topic", image_topic_, std::string("/wide_stereo/left/image_rect_color"));
00040     // n_.param("image_topic", image_topic_, std::string("/camera/rgb/image_color"));
00041     n_.param("image_topic", image_topic_, std::string("/prosilica/image_raw"));
00042     // n_.param("polygon_points_topic", polygon_points_topic_, std::string("/pointcloud_to_image_projector_opencv_node/polygon_points"));
00043     //    n_.param("init_method", init_method_, 1);
00044     init_method_ = init_method;
00045     image_sub_ = it_.subscribe(image_topic_, 1, &CCDNode::imageCallback, this);
00046     polygon_sub_ = n_.subscribe(polygon_points_topic_, 1, &CCDNode::polygonCallback, this);
00047     ROS_INFO("CCDNode Ready, listening on topic %s", image_topic_.c_str());
00048     ccd.read_params(std::string("ccd_params.xml"));
00049     ROS_INFO("HELLOOOOOO %d", init_method_);
00050     ccd.init_mat();
00051     count_ = 0;
00052     got_polygon_ = false;
00053   }
00054 
00055   ~CCDNode(){}
00056 
00057 
00058 
00059   cv::Mat readImage(const sensor_msgs::ImageConstPtr& msg_ptr)
00060   {
00061     cv::Mat image;
00062     if (msg_ptr->encoding.find("bayer") != std::string::npos)
00063     {
00064       image = cv::Mat(1024, 768, CV_8UC3);
00065       const std::string& raw_encoding = msg_ptr->encoding;
00066       cv::Mat camera_image;
00067       int raw_type = CV_8UC1;
00068       if (raw_encoding == sensor_msgs::image_encodings::BGR8 || raw_encoding == sensor_msgs::image_encodings::RGB8)
00069         raw_type = CV_8UC3;
00070       const cv::Mat raw(msg_ptr->height, msg_ptr->width, raw_type,
00071                         const_cast<uint8_t*>(&msg_ptr->data[0]), msg_ptr->step);
00072 
00073       // Convert to color BGR
00074       int code = 0;
00075       if (msg_ptr->encoding == sensor_msgs::image_encodings::BAYER_RGGB8)
00076         code = CV_BayerBG2BGR;
00077       else if (msg_ptr->encoding == sensor_msgs::image_encodings::BAYER_BGGR8)
00078         code = CV_BayerRG2BGR;
00079       else if (msg_ptr->encoding == sensor_msgs::image_encodings::BAYER_GBRG8)
00080         code = CV_BayerGR2BGR;
00081       else if (msg_ptr->encoding == sensor_msgs::image_encodings::BAYER_GRBG8)
00082         code = CV_BayerGB2BGR;
00083       else
00084       {
00085         ROS_ERROR("[image_proc] Unsupported encoding '%s'", msg_ptr->encoding.c_str());
00086       }
00087       cv::cvtColor(raw, camera_image, code);
00088 
00089       // cv::Mat tmp2;
00090       // cv::cvtColor(tmpImage, tmp2, CV_BGR2GRAY);
00091       cv::resize(camera_image, image, cv::Size(1024,768),0,0,cv::INTER_LINEAR);
00092     }
00093     else
00094     {
00095       image = bridge_.imgMsgToCv(msg_ptr, "bgr8");
00096     }
00097     return image;
00098   }
00099 
00100   void contourFMP()
00101   {
00102     AutoInit *ai = new AutoInit(0,0, 30);
00103     ai->init(ccd.tpl, ccd.canvas);
00104     double *ptr;
00105     for (int row = 0; row < (ai->control_points).rows; ++row)
00106     {
00107       ptr = (ai->control_points).ptr<double>(row);
00108       pts_.push_back(cv::Point3d(ptr[0]/ptr[2], ptr[1]/ptr[2], 1));
00109     }
00110   }
00111 
00112   
00113   void contourManually()
00114   {
00115     int key;
00116     cv::namedWindow("CCD", 1);
00117     cv::setMouseCallback( "CCD", on_mouse,  (void*)this);
00118     cv::imshow("CCD", ccd.canvas);
00119     while (1)
00120     {
00121       key = cv::waitKey(10);
00122       if (key == 27) break;
00123     }
00124   }
00125 
00126   void imageCallback(const sensor_msgs::ImageConstPtr& msg_ptr)
00127   {
00128     count_++;
00129     cv::Mat cv_image = readImage(msg_ptr);
00130     std::cerr << "col: " <<cv_image.cols << "  rows:" <<  cv_image.rows<< std::endl;
00131     std::cerr << "init_method_: " << init_method_ << std::endl;
00132     cv_image.copyTo(ccd.canvas);
00133     cv_image.copyTo(ccd.image);
00134     if (count_ == 1)
00135     {
00136       // cv::imwrite("book_test.png", ccd.image);
00137       if (init_method_ == 0) //manually 
00138         contourManually();
00139       else if (init_method_ == 1) //initialized from SIFT
00140       {
00141         ccd.tpl = cv::imread("data/book.png", 1);
00142         contourFMP();
00143       }
00144       else if (init_method_ == 2) //initialized from projected Point Cloud
00145       {
00146         while (!got_polygon_)
00147         {
00148           //std::cerr << "in while loop" << std::endl;            
00149           //sleep(1);
00150           count_ = 0;
00151           return;
00152         }
00153       }
00154       else
00155       {
00156         ROS_ERROR("Unknown parameter!");
00157         return;
00158       }
00159         
00160       if((int)pts_.size() > ccd.degree())
00161       {
00162         for (int i = 0; i < ccd.degree(); ++i)
00163           pts_.push_back(pts_[i]);
00164       }
00165         
00166       ccd.pts = pts_;
00167       BSpline bs(ccd.degree(), ccd.resolution(), ccd.pts);    
00168       ccd.init_cov(bs, (int)ccd.degree());
00169     }      
00170     ccd.run_ccd();
00171     // cv::imshow("CCD", ccd.canvas);
00172     // if(count_ == 1)
00173     // {
00174     //   while(1)
00175     //   {
00176     //     key = cv::waitKey(10);
00177     //     if(key == 27) break;
00178     //   }
00179     // }        
00180     std::stringstream name;
00181     name << count_;
00182     cv::imwrite(name.str() + ".png", ccd.canvas);
00183   }
00184   // sleep(1);
00185   //protected:
00186 
00187   void polygonCallback(const geometry_msgs::PolygonStampedPtr& msg_ptr)
00188   {
00189     if (!got_polygon_)
00190     {
00191       for (uint i = 0; i < msg_ptr->polygon.points.size(); i = i+7)
00192       {
00193         //          ROS_INFO("Polygon: %f, %f", msg_ptr->polygon.points[i].x, msg_ptr->polygon.points[i].y);
00194         pts_.push_back(cv::Point3d(msg_ptr->polygon.points[i].x, msg_ptr->polygon.points[i].y, 1));
00195       }
00196       ROS_INFO("pts_: %ld", pts_.size());
00197       got_polygon_ = true;
00198     }
00199   }
00200 };
00201 
00202 void on_mouse(int event, int x, int y, int flags, void *param )
00203 {
00204   CCDNode *my_node = (CCDNode *)param;
00205   cv::Mat image = my_node->ccd.canvas;
00206   if( image.empty())
00207     return ;
00208 
00209   //caution: check
00210   // if( image1.at<double>() )
00211   //   y = image1->height - y;
00212   switch( event )
00213   {
00214     case CV_EVENT_LBUTTONDOWN:
00215       break;
00216     case CV_EVENT_LBUTTONUP:
00217       cv::circle(image,cv::Point(x,y),2,cv::Scalar(0,0,255),2);
00218       my_node->pts_.push_back(cv::Point3d(x,y,1));
00219       cv::imshow("CCD", image);
00220       std::stringstream name;
00221       name << my_node->pts_.size();
00222       cv::imwrite(name.str() + "i.png", image);
00223       break;
00224   }
00225 }
00226 
00227 int main(int argc, char** argv)
00228 {
00229   if(argc != 2)
00230   {
00231     std::cout << "usage: ros_to_openCv init_method (0-manual, 1-SIFT, 2-PointCloud)" << std::endl;
00232     exit(-1);
00233   }
00234 
00235   ros::init(argc, argv, "ros_to_openCv");
00236   ros::NodeHandle n("~");
00237   CCDNode ccd_node(n, atoi(argv[1]));
00238   //  ccd_node.canvas = imread(argv[1], 1);
00239   //  ccd_node.img = imread(argv[1], 1);
00240   ros::spin();
00241   return 0;
00242 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


contracting_curve_density_algorithm
Author(s): Shulei Zhu, Dejan Pangercic
autogenerated on Thu May 23 2013 14:41:45