// RotatedRectを指定線色で描画する
void IPBase::draw_rect(cv::Mat img, cv::RotatedRect rect, cv::Scalar color) {
cv::Point2f vertices[4];
rect.points(vertices);
for (int i = 0; i < 4; i++) {
cv::line(img, vertices[i], vertices[(i+1)%4], color);
}
}
bool rotatedRectangleContainPoint(cv::RotatedRect rectangle, cv::Point2f point) {
cv::Point2f corners[4];
rectangle.points(corners);
cv::Point2f *lastItemPointer = (corners + sizeof corners / sizeof corners[0]);
std::vector<cv::Point2f> contour(corners, lastItemPointer);
double indicator = pointPolygonTest(contour, point, false);
return indicator >= 0;
}
cv::Mat drawRotatedRectangle(cv::Mat mat, cv::RotatedRect rotated) {
try {
cv::Point2f vertices[4];
rotated.points(vertices);
for (int i = 0; i < 4; i++)
line(mat, vertices[i], vertices[(i+1)%4], cv::Scalar(0,255,0), 2);
} catch (std::exception& e) {
std::cout << "Exception draw rotated rect" << std::endl;
}
return mat;
}
void stretch(cv::RotatedRect box)
{ // Stretch the canvas so that the rectangle box is on the canvas.
cv::Point2f min = box.center;
cv::Point2f max = box.center;
cv::Point2f vtx[4];
box.points(vtx);
for( int i = 0; i < 4; i++ ){
cv::Point2f pnt = vtx[i];
if(max.x < pnt.x){max.x = pnt.x;};
if(max.y < pnt.y){max.y = pnt.y;};
if(min.x > pnt.x){min.x = pnt.x;};
if(min.y > pnt.y){min.y = pnt.y;};
}
stretch(min, max);
}
//from opencv-feature-detector, ported to opencv c++
void rotated_rect(cv::Mat im, const cv::RotatedRect & rot_rect, cv::Scalar color) {
cv::Point2f box_vtx[4];
rot_rect.points(box_vtx);
// copied shamelessly from minarea.c
// it initialize to the last point, then connect to point 0, point 1, point 2 pair-wise
cv::Point pt0, pt;
pt0.x = round(box_vtx[3].x);
pt0.y = round(box_vtx[3].y);
for(int i = 0; i < 4; i++ )
{
pt.x = round(box_vtx[i].x);
pt.y = round(box_vtx[i].y);
cv::line(im, pt0, pt, color, 1, CV_AA, 0);
pt0 = pt;
}
}
void QStasm::__makeComposition(const float *pt, cv::RotatedRect rRect)
{
float pX, pY, alpha = std::cos( -rRect.angle * PI_VALUE/180.0), betha = std::sin( -rRect.angle * PI_VALUE/180.0);
cv::Point2f cp = rRect.center;
cv::Point2f vP[4];
rRect.points(vP);
float shiftX = std::sqrt( (vP[2].x-vP[1].x)*(vP[2].x-vP[1].x) + (vP[2].y-vP[1].y)*(vP[2].y-vP[1].y) ) / 2.0 ;
float shiftY = std::sqrt( (vP[1].x-vP[0].x)*(vP[1].x-vP[0].x) + (vP[1].y-vP[0].y)*(vP[1].y-vP[0].y) ) / 2.0 ;
for(int i = 0; i < stasm_NLANDMARKS; i ++) {
pX = pt[i*2] - shiftX;
pY = pt[i*2+1] - shiftY;
pt_compPts[i*2] = alpha * pX + betha * pY + cp.x;
pt_compPts[i*2+1] = alpha * pY - betha * pX + cp.y;
}
emit pointsComposed(pt_compPts, stasm_NLANDMARKS);
if(!m_Mat.empty())
emit imageComposed(m_Mat, pt_compPts, stasm_NLANDMARKS*2);
}
bool ImageSample::boxInRange(cv::Mat& img, cv::RotatedRect& r)
{
Point2f rect_points[4];
r.points(rect_points);
cv::Rect img_r = cv::Rect(0, 0, img.cols - 1, img.rows - 1);
bool result = true;
for (int i = 0; i < 4; ++i)
{
if (!img_r.contains(rect_points[i]))
{
result = false;
break;
}
}
return result;
}
void RobotIdentifier::identifyTeam(RobotData &data, cv::RotatedRect robot, cv::Mat rgb_frame)
{
cv::Mat hsv;
cv::Point2f test_points[4];
cv::Point2f vertices[4];
robot.points(vertices);
cv::cvtColor(rgb_frame, hsv, CV_BGR2HSV);
for (int i = 0; i < 4; ++i)
{
test_points[i] = calculatePointAtMiddle(robot.center, vertices[i]);
cv::Vec3b hsv_value = hsv.at<cv::Vec3b>(test_points[i].y, test_points[i].x);
if ((team_color_ == "Blue" and isPointBlue(hsv_value)) or
(team_color_ == "Yellow" and isPointYellow(hsv_value)))
{
data.team = RobotData::ALLY;
data.orientation = robot.angle+(90*((i+1)%4))*2*M_PI/360.0;
cv::Point2f id_test_point = calculatePointAtMiddle(robot.center, vertices[(i+2)%4]);
cv::Vec3b hsv_id_point = hsv.at<cv::Vec3b>(id_test_point.y, id_test_point.x);
if (isPointRed(hsv_id_point))
{
data.id = 0;
data.robot_color = cv::Scalar(0, 0, 255);
}
else if (isPointGreen(hsv_id_point))
{
data.id = 1;
data.robot_color = cv::Scalar(0, 255, 0);
}
else if (isPointPurple(hsv_id_point))
{
data.id = 2;
data.robot_color = cv::Scalar(255, 0, 0);
}
else
{
ROS_ERROR("Could not identify this robot");
}
return;
}
}
data.team = RobotData::OPPONENT;
data.id = 0;
data.orientation = 0.0;
}
void drawEllipseWithBox(cv::RotatedRect box, cv::Scalar color, int lineThickness)
{
if(img.empty()){
stretch(box);
img = cv::Mat::zeros(rows,cols,CV_8UC3);
}
box.center = scale * cv::Point2f(box.center.x - origin.x, box.center.y - origin.y);
box.size.width = (float)(scale * box.size.width);
box.size.height = (float)(scale * box.size.height);
ellipse(img, box, color, lineThickness, LINE_AA);
Point2f vtx[4];
box.points(vtx);
for( int j = 0; j < 4; j++ ){
line(img, vtx[j], vtx[(j+1)%4], color, lineThickness, LINE_AA);
}
}
// 将旋转矩形的角度转换为 0-180度
// 参数: box 矩形
// 返回值: 角度 (0-180)
double getRcDegree(const cv::RotatedRect &box)
{
using namespace cv;
double degree = 0.0f;
Point2f vertVect[4];
box.points(vertVect);
//line 1
const double firstLineLen = (vertVect[1].x - vertVect[0].x)*(vertVect[1].x - vertVect[0].x) +
(vertVect[1].y - vertVect[0].y)*(vertVect[1].y - vertVect[0].y);
//line 2
const double secondLineLen = (vertVect[2].x - vertVect[1].x)*(vertVect[2].x - vertVect[1].x) +
(vertVect[2].y - vertVect[1].y)*(vertVect[2].y - vertVect[1].y);
if (firstLineLen > secondLineLen)
{
degree = calcLineDegree(vertVect[0], vertVect[1]);
}
else
{
degree = calcLineDegree(vertVect[2], vertVect[1]);
}
return degree;
}
// 遍历旋转矩形内部的所有像素点
// 参数: rect 旋转矩形
// 参数: xmin x坐标最大值
// 参数: ymin y坐标最大值
// 参数: xmax x坐标最大值
// 参数: ymax y坐标最大值
// 参数: func 回调函数
// 参数: xstep x坐标方向跳过点数
// 参数: ystep y坐标方向跳过点数
void for_each(const cv::RotatedRect &rect, float xmin, float ymin, float xmax, float ymax, const std::function<void(int, int)> &func, int xstep, int ystep)
{
// 获取矩形四个顶点的坐标
const unsigned int sizeof_vertices = 4;
cv::Point2f vertices[sizeof_vertices];
rect.points(vertices);
// 对坐标进行排序
std::sort(std::begin(vertices), std::end(vertices), [](const cv::Point2f &point1, const cv::Point2f &point2)
{
return point1.y < point2.y;
});
// 最上面的点与距离其最近的点的一元一次方程系数和截距
auto tsparam = get_linear_parameters<float>(vertices[0].x, vertices[0].y, vertices[1].x, vertices[1].y);
// 最上面的点与距离其第二进的点的一元一次方程系数和截距
auto tlparam = get_linear_parameters(vertices[0].x, vertices[0].y, vertices[2].x, vertices[2].y);
// 最下面的点与距离其第二进的点的一元一次方程系数和截距
auto blparam = get_linear_parameters(vertices[3].x, vertices[3].y, vertices[1].x, vertices[1].y);
// 最下面的点与距离其最近的点的一元一次方程系数和截距
auto bsparam = get_linear_parameters(vertices[3].x, vertices[3].y, vertices[2].x, vertices[2].y);
// 正矩形
if (std::abs(tsparam.first) < 0.0001 || std::abs(tlparam.first) < 0.0001)
{
auto data = std::minmax(vertices[0].x, vertices[1].x);
xmin = std::max(data.first, xmin);
xmax = std::min(data.second, xmax);
ymin = std::max(vertices[0].y, ymin);
ymax = std::min(vertices[2].y, ymax);
for (float y = ymin; y < ymax; y += ystep)
{
for (float x = xmin; x < xmax; x += xstep)
{
func(static_cast<int>(x), static_cast<int>(y));
}
}
return;
}
// 遍历上三角形
auto yminimum = std::max(vertices[0].y, ymin);
auto ymaximum = std::min(vertices[1].y, ymax);
for (float y = yminimum; y < ymaximum; y += ystep)
{
auto x1 = get_linear_x(tsparam.first, tsparam.second, y);
auto x2 = get_linear_x(tlparam.first, tlparam.second, y);
auto data = std::minmax(x1, x2);
auto xminimum = std::max(data.first, xmin);
auto xmaximum = std::min(data.second, xmax);
for (float x = xminimum; x < xmaximum; x += xstep)
{
func(static_cast<int>(x), static_cast<int>(y));
}
}
// 遍历平行四边形
yminimum = std::max(vertices[1].y, ymin);
ymaximum = std::min(vertices[2].y, ymax);
for (float y = yminimum; y < ymaximum; y += ystep)
{
auto x1 = get_linear_x(blparam.first, blparam.second, y);
auto x2 = get_linear_x(tlparam.first, tlparam.second, y);
auto data = std::minmax(x1, x2);
auto xminimum = std::max(data.first, xmin);
auto xmaximum = std::min(data.second, xmax);
for (float x = xminimum; x < xmaximum; x += xstep)
{
func(static_cast<int>(x), static_cast<int>(y));
}
}
// 遍历下三角形
yminimum = std::max(vertices[2].y, ymin);
ymaximum = std::min(vertices[3].y, ymax);
for (float y = yminimum; y < ymaximum; y += ystep)
{
auto x1 = get_linear_x(blparam.first, blparam.second, y);
auto x2 = get_linear_x(bsparam.first, bsparam.second, y);
auto data = std::minmax(x1, x2);
auto xminimum = std::max(data.first, xmin);
auto xmaximum = std::min(data.second, xmax);
for (float x = xminimum; x < xmaximum; x += xstep)
{
func(static_cast<int>(x), static_cast<int>(y));
}
}
}
ofPolyline toOf(cv::RotatedRect rect) {
vector<cv::Point2f> corners(4);
rect.points(&corners[0]);
ofPolyline polyline = toOf(corners);
return polyline;
}
void cv::boxPoints(cv::RotatedRect box, OutputArray _pts)
{
_pts.create(4, 2, CV_32F);
Mat pts = _pts.getMat();
box.points(pts.ptr<Point2f>());
}