rectangle_detector.cpp
Go to the documentation of this file.
00001 
00006 // https://raw.github.com/bsdnoobz/opencv-code/master/quad-segmentation.cpp
00007 
00008 #include <ros/ros.h>
00009 #include <jsk_topic_tools/log_utils.h>
00010 #include <message_filters/subscriber.h>
00011 #include <message_filters/time_synchronizer.h>
00012 #include <jsk_recognition_msgs/LineArray.h>
00013 #include <jsk_perception/RectangleDetectorConfig.h>
00014 #include <image_transport/image_transport.h>
00015 #include <cv_bridge/cv_bridge.h>
00016 #include <sensor_msgs/image_encodings.h>
00017 #include <opencv2/imgproc/imgproc.hpp>
00018 #include "opencv2/highgui/highgui.hpp"
00019 
00020 #include <dynamic_reconfigure/server.h>
00021 
00022 namespace enc = sensor_msgs::image_encodings;
00023 
00024 using namespace message_filters;
00025 
00026 class RectangleDetector
00027 {
00028     jsk_perception::RectangleDetectorConfig config_;
00029     dynamic_reconfigure::Server<jsk_perception::RectangleDetectorConfig> srv;
00030 
00031     //image_transport::Publisher image_pub_;
00032     ros::Publisher image_pub_;
00033     image_transport::ImageTransport it_;
00034     ros::NodeHandle nh_;
00035     //int subscriber_count_;
00036 
00037     message_filters::Subscriber<sensor_msgs::Image> *image_sub;
00038     message_filters::Subscriber<jsk_recognition_msgs::LineArray> *line_sub;
00039     TimeSynchronizer<sensor_msgs::Image, jsk_recognition_msgs::LineArray> *sync;
00040 
00041     double _threshold1;
00042     double _threshold2;
00043     int _apertureSize;
00044     bool _L2gradient;
00045 
00046     void reconfigureCallback(jsk_perception::RectangleDetectorConfig &new_config, uint32_t level)
00047     {
00048         config_ = new_config;
00049         _threshold1 = config_.threshold1;
00050         _threshold2 = config_.threshold2;
00051         _apertureSize = config_.apertureSize;
00052         _L2gradient = config_.L2gradient;
00053     }
00054 
00055     cv::Point2f computeIntersect(cv::Vec4i a,
00056                                  cv::Vec4i b)
00057     {
00058         int x1 = a[0], y1 = a[1], x2 = a[2], y2 = a[3], x3 = b[0], y3 = b[1], x4 = b[2], y4 = b[3];
00059         float denom;
00060 
00061         if (float d = ((float)(x1 - x2) * (y3 - y4)) - ((y1 - y2) * (x3 - x4)))
00062             {
00063                 cv::Point2f pt;
00064                 pt.x = ((x1 * y2 - y1 * x2) * (x3 - x4) - (x1 - x2) * (x3 * y4 - y3 * x4)) / d;
00065                 pt.y = ((x1 * y2 - y1 * x2) * (y3 - y4) - (y1 - y2) * (x3 * y4 - y3 * x4)) / d;
00066                 return pt;
00067             }
00068         else
00069             return cv::Point2f(-1, -1);
00070     }
00071 
00072     void sortCorners(std::vector<cv::Point2f>& corners, 
00073                      cv::Point2f center)
00074     {
00075         std::vector<cv::Point2f> top, bot;
00076 
00077         for (int i = 0; i < corners.size(); i++)
00078             {
00079                 if (corners[i].y < center.y)
00080                     top.push_back(corners[i]);
00081                 else
00082                     bot.push_back(corners[i]);
00083             }
00084 
00085         cv::Point2f tl = top[0].x > top[1].x ? top[1] : top[0];
00086         cv::Point2f tr = top[0].x > top[1].x ? top[0] : top[1];
00087         cv::Point2f bl = bot[0].x > bot[1].x ? bot[1] : bot[0];
00088         cv::Point2f br = bot[0].x > bot[1].x ? bot[0] : bot[1];
00089 
00090         corners.clear();
00091         corners.push_back(tl);
00092         corners.push_back(tr);
00093         corners.push_back(br);
00094         corners.push_back(bl);
00095     }
00096 
00097     void callback(const sensor_msgs::ImageConstPtr& image,
00098                   const jsk_recognition_msgs::LineArrayConstPtr& line)
00099     {
00100         // Transform the image.
00101         try
00102             {
00103                 if ( line->lines.size() < 4 ) return;
00104 
00105                 // Convert the image into something opencv can handle.
00106                 cv::Mat src = cv_bridge::toCvShare(image, image->encoding)->image;
00107                 std::vector<cv::Vec4i> lines;
00108                 lines.resize(line->lines.size());
00109 
00110                 // Expand the lines
00111                 for (int i = 0; i < lines.size(); i++)
00112                     {
00113                         cv::Vec4i v;
00114                         v[0] = line->lines[i].x1; v[1] = line->lines[i].y1;
00115                         v[2] = line->lines[i].x2; v[3] = line->lines[i].y2;
00116                         lines[i][0] = 0;
00117                         lines[i][1] = ((float)v[1] - v[3]) / (v[0] - v[2]) * -v[0] + v[1];
00118                         lines[i][2] = src.cols;
00119                         lines[i][3] = ((float)v[1] - v[3]) / (v[0] - v[2]) * (src.cols - v[2]) + v[3];
00120                     }
00121 
00122                 std::vector<cv::Point2f> corners;
00123                 for (int i = 0; i < lines.size(); i++)
00124                     {
00125                         for (int j = i+1; j < lines.size(); j++)
00126                             {
00127                                 cv::Point2f pt = computeIntersect(lines[i], lines[j]);
00128                                 if (pt.x >= 0 && pt.y >= 0)
00129                                     corners.push_back(pt);
00130                             }
00131                     }
00132 
00133                 std::vector<cv::Point2f> approx;
00134                 cv::approxPolyDP(cv::Mat(corners), approx, cv::arcLength(cv::Mat(corners), true) * 0.02, true);
00135 
00136                 if (approx.size() != 4)
00137                     {
00138                         JSK_ROS_ERROR("The object is not quadrilateral!");
00139                         return ;
00140                     }
00141 
00142                 // Get mass center
00143                 cv::Point2f center(0,0);
00144                 for (int i = 0; i < corners.size(); i++)
00145                     center += corners[i];
00146                 center *= (1. / corners.size());
00147 
00148                 sortCorners(corners, center);
00149 
00150                 cv::Mat dst = src.clone();
00151 
00152                 // Draw lines
00153                 for (int i = 0; i < lines.size(); i++)
00154                     {
00155                         cv::Vec4i v = lines[i];
00156                         cv::line(dst, cv::Point(v[0], v[1]), cv::Point(v[2], v[3]), CV_RGB(0,255,0));
00157                     }
00158 
00159                 // Draw corner points
00160                 cv::circle(dst, corners[0], 3, CV_RGB(255,0,0), 2);
00161                 cv::circle(dst, corners[1], 3, CV_RGB(0,255,0), 2);
00162                 cv::circle(dst, corners[2], 3, CV_RGB(0,0,255), 2);
00163                 cv::circle(dst, corners[3], 3, CV_RGB(255,255,255), 2);
00164 
00165                 // Draw mass center
00166                 cv::circle(dst, center, 3, CV_RGB(255,255,0), 2);
00167 
00168                 cv::Mat quad = cv::Mat::zeros(300, 220, CV_8UC3);
00169 
00170                 std::vector<cv::Point2f> quad_pts;
00171                 quad_pts.push_back(cv::Point2f(0, 0));
00172                 quad_pts.push_back(cv::Point2f(quad.cols, 0));
00173                 quad_pts.push_back(cv::Point2f(quad.cols, quad.rows));
00174                 quad_pts.push_back(cv::Point2f(0, quad.rows));
00175 
00176                 cv::Mat transmtx = cv::getPerspectiveTransform(corners, quad_pts);
00177                 cv::warpPerspective(src, quad, transmtx, quad.size());
00178 
00179                 sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(image->header, enc::RGB8, dst).toImageMsg();
00180                 //sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(image->header, enc::RGB8, quad).toImageMsg();
00181                 image_pub_.publish(out_img);
00182             }
00183         catch (cv::Exception &e)
00184             {
00185                 JSK_ROS_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
00186             }
00187     }
00188 
00189 public:
00190     RectangleDetector(ros::NodeHandle nh = ros::NodeHandle()) : it_(nh), nh_(nh)
00191     {
00192 
00193         image_sub = new message_filters::Subscriber<sensor_msgs::Image>(nh_, "image", 1);
00194         line_sub = new message_filters::Subscriber<jsk_recognition_msgs::LineArray>(nh_, "lines", 1);
00195         sync = new TimeSynchronizer<sensor_msgs::Image, jsk_recognition_msgs::LineArray>(*image_sub, *line_sub, 10);
00196         sync->registerCallback(boost::bind(&RectangleDetector::callback, this, _1, _2));
00197         image_pub_ = nh_.advertise<sensor_msgs::Image>("rectangle/image", 1);
00198 
00199         dynamic_reconfigure::Server<jsk_perception::RectangleDetectorConfig>::CallbackType f =
00200             boost::bind(&RectangleDetector::reconfigureCallback, this, _1, _2);
00201         srv.setCallback(f);
00202     }
00203 };
00204 
00205 int main(int argc, char **argv)
00206 {
00207     ros::init(argc, argv, "edge_detector", ros::init_options::AnonymousName);
00208 
00209     RectangleDetector rd;
00210     ros::spin();
00211 }


jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Wed Sep 16 2015 04:36:15