summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2014-01-06 15:35:29 +0100
committerStanislaw Halik <sthalik@misaki.pl>2014-01-06 15:35:29 +0100
commit5f14d59ba285c72682e99c0fb95310f5920898de (patch)
tree7a2e2be2fc13b89d3e88a6d7814170c7f787040a
parent477bf983513fca2674134953fe65d679fc280ab1 (diff)
optimize existing pose when it makes sense
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp12
1 files changed, 11 insertions, 1 deletions
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 70c82c52..9c8cd52b 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -195,6 +195,7 @@ void Tracker::run()
int cur_fps = 0;
int last_fps = 0;
cv::Point2f last_centroid;
+ bool first = true, pitch_sign = true;
while (!stop)
{
@@ -318,7 +319,9 @@ void Tracker::run()
last_roi.height = std::min<int>(grayscale.rows - last_roi.y, last_roi.height);
}
- cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, false, cv::ITERATIVE);
+ cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, !first, cv::ITERATIVE);
+
+ first = false;
cv::Mat rotation_matrix = cv::Mat::zeros(3, 3, CV_64FC1);
@@ -329,6 +332,12 @@ void Tracker::run()
{
cv::Vec3d euler = cv::RQDecomp3x3(rotation_matrix, junk1, junk2);
+ if ((euler[0] > 0) != pitch_sign)
+ {
+ first = true;
+ pitch_sign = euler[0] > 0;
+ }
+
QMutexLocker lck(&mtx);
for (int i = 0; i < 3; i++)
@@ -354,6 +363,7 @@ void Tracker::run()
else
{
last_roi = cv::Rect(65535, 65535, 0, 0);
+ first = true;
}
if (frame.rows > 0)