CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 21:19:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

aruco_pose: improve processing of axis visualization in aruco_map/image (#167)

Browse Source 
* aruco_pose: improved processing of axis visualization in the topic /aruco_map/image

* aruco_pose: reworked method for drawing axis arrow

* aruco_pose: fix indentation error

* Fix style
This commit is contained in:
Alamoris
2019-09-06 18:25:49 +03:00
committed by Oleg Kalachev
parent 50739a3ea8
commit 2e903b45a3
1 changed files with 27 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
aruco_pose/src/draw.cpp
View File

@@ -2,6 +2,7 @@
// with some improvements and fixes
#include "draw.h"
#include <math.h>
using namespace cv;
using namespace cv::aruco;
@@ -112,9 +113,32 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
auto out_copy = _img.getMat();
cv::Point center(ofs.x - minX / sizeX * float(out.cols), ofs.y + out.rows + minY / sizeY * float(out.rows));
line(out_copy, center, center + Point(300, 0), Scalar(255, 0, 0), 10); // x axis
line(out_copy, center, center + Point(0, -300), Scalar(0, 255, 0), 10); // y axis
line(out_copy, center, center + Point(-200, 200), Scalar(0, 0, 255), 10); // z axis
int axis_points[3][2] = {{300, 0}, {0, -300}, {-150, 150}};
Point axis_names[3] = {Point(270, 50), Point(25, -270), Point(-160, 115)};
Scalar colors[] = {Scalar(255, 0, 0), Scalar(0, 255, 0), Scalar(0, 0, 255)};
String names[] = {"X", "Y", "Z"};
int r_half = 14;
int height = 55;
for(int poly = 2; poly >= 0; poly--){
double alpha = atan2(0 - axis_points[poly][0], 0 - axis_points[poly][1]);
float x_delta = r_half * cos(alpha);
float y_delta = r_half * sin(alpha);
Point polygon_vertices[1][3];
polygon_vertices[0][0] = center + Point(axis_points[poly][0] + x_delta, axis_points[poly][1] - y_delta);
polygon_vertices[0][1] = center + Point(axis_points[poly][0] - x_delta, axis_points[poly][1] + y_delta);
polygon_vertices[0][2] = center + Point(axis_points[poly][0] - sin(alpha) * height, axis_points[poly][1] - cos(alpha) * height);
const Point* ppt[1] = {polygon_vertices[0]};
int npt[] = {3};
fillPoly(out_copy, ppt, npt, 1, colors[poly]);
putText(out_copy, names[poly], center + axis_names[poly], FONT_HERSHEY_SIMPLEX, 1.2, colors[poly], 7);
line(out_copy, center, center + Point(axis_points[poly][0], axis_points[poly][1]), colors[poly], 10);
}
}
}