12 #include <jsk_recognition_msgs/LineArray.h> 13 #include <jsk_perception/RectangleDetectorConfig.h> 17 #include <opencv2/imgproc/imgproc.hpp> 18 #include "opencv2/highgui/highgui.hpp" 20 #include <dynamic_reconfigure/server.h> 28 jsk_perception::RectangleDetectorConfig
config_;
29 dynamic_reconfigure::Server<jsk_perception::RectangleDetectorConfig>
srv;
49 _threshold1 = config_.threshold1;
50 _threshold2 = config_.threshold2;
51 _apertureSize = config_.apertureSize;
52 _L2gradient = config_.L2gradient;
58 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];
61 if (
float d = ((
float)(x1 - x2) * (y3 - y4)) - ((y1 - y2) * (x3 - x4)))
64 pt.x = ((x1 * y2 - y1 * x2) * (x3 - x4) - (x1 - x2) * (x3 * y4 - y3 * x4)) /
d;
65 pt.y = ((x1 * y2 - y1 * x2) * (y3 - y4) - (y1 - y2) * (x3 * y4 - y3 * x4)) /
d;
69 return cv::Point2f(-1, -1);
75 std::vector<cv::Point2f> top, bot;
77 for (
int i = 0; i < corners.size(); i++)
79 if (corners[i].
y < center.y)
80 top.push_back(corners[i]);
82 bot.push_back(corners[i]);
85 cv::Point2f tl = top[0].x > top[1].x ? top[1] : top[0];
86 cv::Point2f tr = top[0].x > top[1].x ? top[0] : top[1];
87 cv::Point2f bl = bot[0].x > bot[1].x ? bot[1] : bot[0];
88 cv::Point2f
br = bot[0].x > bot[1].x ? bot[0] : bot[1];
91 corners.push_back(tl);
92 corners.push_back(tr);
93 corners.push_back(br);
94 corners.push_back(bl);
97 void callback(
const sensor_msgs::ImageConstPtr& image,
98 const jsk_recognition_msgs::LineArrayConstPtr&
line)
103 if ( line->lines.size() < 4 )
return;
107 std::vector<cv::Vec4i> lines;
108 lines.resize(line->lines.size());
111 for (
int i = 0; i < lines.size(); i++)
114 v[0] = line->lines[i].x1; v[1] = line->lines[i].y1;
115 v[2] = line->lines[i].x2; v[3] = line->lines[i].y2;
117 lines[i][1] = ((
float)v[1] - v[3]) / (v[0] - v[2]) * -v[0] + v[1];
118 lines[i][2] = src.cols;
119 lines[i][3] = ((
float)v[1] - v[3]) / (v[0] - v[2]) * (src.cols - v[2]) + v[3];
122 std::vector<cv::Point2f> corners;
123 for (
int i = 0; i < lines.size(); i++)
125 for (
int j = i+1; j < lines.size(); j++)
127 cv::Point2f pt = computeIntersect(lines[i], lines[j]);
128 if (pt.x >= 0 && pt.y >= 0)
129 corners.push_back(pt);
133 std::vector<cv::Point2f> approx;
134 cv::approxPolyDP(cv::Mat(corners), approx, cv::arcLength(cv::Mat(corners),
true) * 0.02,
true);
136 if (approx.size() != 4)
138 ROS_ERROR(
"The object is not quadrilateral!");
143 cv::Point2f center(0,0);
144 for (
int i = 0; i < corners.size(); i++)
145 center += corners[i];
146 center *= (1. / corners.size());
148 sortCorners(corners, center);
150 cv::Mat dst = src.clone();
153 for (
int i = 0; i < lines.size(); i++)
155 cv::Vec4i
v = lines[i];
156 cv::line(dst, cv::Point(v[0], v[1]), cv::Point(v[2], v[3]), CV_RGB(0,255,0));
160 cv::circle(dst, corners[0], 3, CV_RGB(255,0,0), 2);
161 cv::circle(dst, corners[1], 3, CV_RGB(0,255,0), 2);
162 cv::circle(dst, corners[2], 3, CV_RGB(0,0,255), 2);
163 cv::circle(dst, corners[3], 3, CV_RGB(255,255,255), 2);
166 cv::circle(dst, center, 3, CV_RGB(255,255,0), 2);
168 cv::Mat quad = cv::Mat::zeros(300, 220, CV_8UC3);
170 std::vector<cv::Point2f> quad_pts;
171 quad_pts.push_back(cv::Point2f(0, 0));
172 quad_pts.push_back(cv::Point2f(quad.cols, 0));
173 quad_pts.push_back(cv::Point2f(quad.cols, quad.rows));
174 quad_pts.push_back(cv::Point2f(0, quad.rows));
176 cv::Mat transmtx = cv::getPerspectiveTransform(corners, quad_pts);
177 cv::warpPerspective(src, quad, transmtx, quad.size());
183 catch (cv::Exception &e)
185 ROS_ERROR(
"Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
197 image_pub_ = nh_.advertise<sensor_msgs::Image>(
"rectangle/image", 1);
199 dynamic_reconfigure::Server<jsk_perception::RectangleDetectorConfig>::CallbackType
f =
205 int main(
int argc,
char **argv)
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
ros::Publisher image_pub_
cv::Point2f computeIntersect(cv::Vec4i a, cv::Vec4i b)
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void sortCorners(std::vector< cv::Point2f > &corners, cv::Point2f center)
message_filters::Subscriber< jsk_recognition_msgs::LineArray > * line_sub
ROSCPP_DECL void spin(Spinner &spinner)
void callback(const sensor_msgs::ImageConstPtr &image, const jsk_recognition_msgs::LineArrayConstPtr &line)
TimeSynchronizer< sensor_msgs::Image, jsk_recognition_msgs::LineArray > * sync
RectangleDetector(ros::NodeHandle nh=ros::NodeHandle())
image_transport::ImageTransport it_
int main(int argc, char **argv)
dynamic_reconfigure::Server< jsk_perception::RectangleDetectorConfig > srv
void reconfigureCallback(jsk_perception::RectangleDetectorConfig &new_config, uint32_t level)
jsk_perception::RectangleDetectorConfig config_
message_filters::Subscriber< sensor_msgs::Image > * image_sub
sensor_msgs::ImagePtr toImageMsg() const
Connection registerCallback(const C &callback)