From a430e297d011295131a24419fda4a9b28f22c539 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Fri, 25 Oct 2013 23:53:00 +0200 Subject: aruco: show reprojection Signed-off-by: Stanislaw Halik --- ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) (limited to 'ftnoir_tracker_aruco') diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp index ad4643d0..2bb3ad77 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp @@ -191,6 +191,7 @@ void Tracker::run() auto prev_time = last_time; int fps = 0; int last_fps = 0; + std::vector reprojection; while (!stop) { @@ -223,7 +224,16 @@ void Tracker::run() if (markers.size() == 1 && markers[0].size() == 4) { const aruco::Marker& m = markers.at(0); for (int i = 0; i < 4; i++) - cv::line(color, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), 4); + cv::line(color, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), 3); + } + + for (int i = 0; i < reprojection.size(); i++) + { + cv::circle(frame, + reprojection[i], + 6, + cv::Scalar(0, 255, 128), + 3); } auto time = cv::getTickCount(); @@ -287,7 +297,11 @@ void Tracker::run() pose[Yaw] = foo[1]; pose[Pitch] = -foo[0]; pose[Roll] = foo[2]; - + + reprojection.clear(); + reprojection.resize(4); + cv::projectPoints(obj_points, rvec, tvec, intrinsics, dist_coeffs, reprojection); + //pose[Yaw] -= atan(pose[TX] / pose[TZ]) * 180 / HT_PI; //pose[Pitch] -= atan(pose[TY] / pose[TZ]) * 180 / HT_PI; } -- cgit v1.2.3