summaryrefslogtreecommitdiffhomepage
path: root/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2015-06-03 07:57:31 +0200
committerStanislaw Halik <sthalik@misaki.pl>2015-06-03 07:57:31 +0200
commit69e729e4515a085ea4f0cc6943917897ffe61daf (patch)
treeebc9e78d0f7cbfea279ca979da334469001c8c10 /ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
parent37d15b7a32a63994ec292499e500e967c9313803 (diff)
aruco: wait for up to a second to switch box sizesopentrack-2.3-rc12p1
Diffstat (limited to 'ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp')
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp51
1 files changed, 16 insertions, 35 deletions
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 9f85fcce..793469e2 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -119,21 +119,18 @@ void Tracker::run()
auto freq = cv::getTickFrequency();
auto last_time = cv::getTickCount();
- int cur_fps = 0;
- int last_fps = 0;
-
+ double cur_fps = 0;
std::vector<int> box_sizes { 5, 7, 9, 11 };
int box_idx = 0;
double failed = 0;
- const double max_failed = .334;
+ const double max_failed = 1.25;
+ cv::Vec3d rvec, tvec;
while (!stop)
{
cv::Mat color;
if (!camera.read(color))
continue;
- auto tm = cv::getTickCount();
-
cv::Mat grayscale;
cv::cvtColor(color, grayscale, cv::COLOR_RGB2GRAY);
@@ -156,7 +153,13 @@ void Tracker::run()
const double size_max = 0.4;
bool roi_valid = false;
-
+
+ auto time = cv::getTickCount();
+
+ const double dt = (time - last_time) / freq;
+ last_time = time;
+ cur_fps = cur_fps * 0.97 + 0.03 * (dt == 0 ? 0 : 1./dt);
+
if (last_roi.width > 0 && last_roi.height)
{
detector.setThresholdParams(box_sizes[box_idx], 5);
@@ -166,7 +169,7 @@ void Tracker::run()
if (detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false),
markers.size() == 1 && markers[0].size() == 4)
{
- failed = 0;
+ failed = std::max(0., failed - dt);
auto& m = markers.at(0);
for (int i = 0; i < 4; i++)
{
@@ -177,17 +180,6 @@ void Tracker::run()
roi_valid = true;
}
}
-
- auto time = cv::getTickCount();
-
- if ((long) (time / freq) != (long) (last_time / freq))
- {
- last_fps = cur_fps;
- cur_fps = 0;
- last_time = time;
- }
-
- const double dt = (time - last_time) / freq;
if (!roi_valid)
{
@@ -209,16 +201,12 @@ void Tracker::run()
cv::line(color, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), scale, 8);
}
- cur_fps++;
-
char buf[128];
frame = color.clone();
- ::sprintf(buf, "Hz: %d", last_fps);
+ ::sprintf(buf, "Hz: %d", (int)cur_fps);
cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(0, 255, 0), scale*2);
- ::sprintf(buf, "Jiffies: %ld", (long) (10000 * (time - tm) / freq));
- cv::putText(frame, buf, cv::Point(10, 54), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(80, 255, 0), scale*2);
if (markers.size() == 1 && markers[0].size() == 4) {
const auto& m = markers.at(0);
@@ -242,8 +230,6 @@ void Tracker::run()
obj_points.at<float>(x4,1)= size + s.headpos_y;
obj_points.at<float>(x4,2)= 0 + s.headpos_z;
- cv::Vec3d rvec, tvec;
-
cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, false, CV_ITERATIVE);
std::vector<cv::Point2f> roi_projection(4);
@@ -267,14 +253,11 @@ void Tracker::run()
obj_points.at<float>(i, 2) -= s.headpos_z;
}
- {
- cv::Mat rvec_, tvec_;
- cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec_, tvec_, false, CV_ITERATIVE);
- tvec = tvec_;
- }
+ cv::Mat rvec_, tvec_;
+ cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec_, tvec_, false, CV_ITERATIVE);
cv::Mat roi_points = obj_points * c_search_window;
- cv::projectPoints(roi_points, rvec, tvec, intrinsics, dist_coeffs, roi_projection);
+ cv::projectPoints(roi_points, rvec_, tvec_, intrinsics, dist_coeffs, roi_projection);
last_roi = cv::Rect(color.cols-1, color.rows-1, 0, 0);
@@ -317,8 +300,6 @@ void Tracker::run()
QMutexLocker lck(&mtx);
- tvec[1] *= -1;
-
for (int i = 0; i < 3; i++)
pose[i] = tvec(i);
pose[Yaw] = euler[1];
@@ -326,7 +307,7 @@ void Tracker::run()
pose[Roll] = euler[2];
r = rmat;
- t = tvec;
+ t = cv::Vec3d(tvec[0], -tvec[1], tvec[2]);
}
if (roi_valid)