summaryrefslogtreecommitdiffhomepage
path: root/ftnoir_tracker_aruco
diff options
context:
space:
mode:
Diffstat (limited to 'ftnoir_tracker_aruco')
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp18
1 files changed, 16 insertions, 2 deletions
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<cv::Point2f> 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;
}