From 2e903b45a387e1daa912ffe3dbd001e6eac2f1cd Mon Sep 17 00:00:00 2001 From: Alamoris Date: Fri, 6 Sep 2019 18:25:49 +0300 Subject: [PATCH] aruco_pose: improve processing of axis visualization in aruco_map/image (#167) * 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 --- aruco_pose/src/draw.cpp | 30 +++++++++++++++++++++++++++--- 1 file changed, 27 insertions(+), 3 deletions(-) diff --git a/aruco_pose/src/draw.cpp b/aruco_pose/src/draw.cpp index 004e918a..d9cd0e7d 100644 --- a/aruco_pose/src/draw.cpp +++ b/aruco_pose/src/draw.cpp @@ -2,6 +2,7 @@ // with some improvements and fixes #include "draw.h" +#include 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); + } } }