85 cv::cvtColor(img, temp, CV_BGR2HSV);
86 inRange(temp, cv::Scalar(0, 0, 200, 0), cv::Scalar(180, 255, 255, 0), temp);
88 std::vector<std::vector<cv::Point> > contours;
89 std::vector<cv::Vec4i> hierarchy;
90 findContours(temp, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE,
93 std::vector<std::vector<cv::Point> > contours_poly(contours.size());
94 std::vector<cv::Rect> boundRect(contours.size());
95 cv::Mat roi(img.rows, img.cols, CV_8UC3, cv::Scalar(0, 0, 0));
96 cv::Mat dst = cv::Mat::zeros(roi.size(), CV_32FC1);
97 cv::Mat dst_norm, dst_norm_scaled;
99 std::vector<cv::Point2f> src;
103 for (
int i = 0; i < contours.size(); i++) {
104 if (contourArea(contours[i]) > 40000) {
105 drawContours(roi, contours, i, cv::Scalar(255, 255, 255), CV_FILLED);
106 cv::cvtColor(roi, roi, CV_BGR2GRAY);
107 cornerHarris(roi, dst, 2, 3, 0.06);
108 normalize(dst, dst_norm, 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat());
109 convertScaleAbs(dst_norm, dst_norm_scaled);
110 for (
int x = 0; x < dst_norm.rows; x++) {
111 for (
int y = 0; y < dst_norm.cols; y++) {
112 if (static_cast<int>(dst_norm.at<
float>(x, y)) > 200) {
113 src.push_back(cv::Point2f(y, x));
119 ROS_INFO(
"Number of corner points detected: %d",
120 static_cast<int>(src.size()));
121 std::vector<cv::Point2f> srcPoints;
122 cv::Point2f topLeft, topRight, bottomRight, bottomLeft;
124 if (src.size() == 4) {
126 src.begin(), src.end(),
127 [](
const cv::Point2f& a,
const cv::Point2f& b) {
return a.x < b.x; });
129 if (src.at(0).x + src.at(0).y < src.at(1).x + src.at(1).y) {
131 bottomLeft = src.at(1);
134 bottomLeft = src.at(0);
136 if (src.at(2).x + src.at(2).y < src.at(3).x + src.at(3).y) {
137 topRight = src.at(2);
138 bottomRight = src.at(3);
140 topRight = src.at(3);
141 bottomRight = src.at(2);
143 srcPoints = {topLeft, topRight, bottomLeft, bottomRight};
145 std::vector<cv::Point2f> dstPoints = {
146 cv::Point2f(0.0, 0.0), cv::Point2f(199.0, 0.0), cv::Point2f(0.0, 199.0),
147 cv::Point2f(199.0, 199.0)};
148 cv::Mat transform = getPerspectiveTransform(srcPoints, dstPoints);
149 warpPerspective(img, img, transform, cv::Size(200, 200), CV_INTER_LINEAR);