summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2013-11-03 16:25:28 +0100
committerStanislaw Halik <sthalik@misaki.pl>2013-11-03 17:16:59 +0100
commit9e145b2b92d0f2d8b76599608746bb377af6bd91 (patch)
tree7790bee4f11714fa6fc8185dd46d0e618ff1fd38
parent7e06aaff52974d2dc8035bfb986c42bc8ee9cfa2 (diff)
implement ROI for speed
Signed-off-by: Stanislaw Halik <sthalik@misaki.pl>
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp110
1 files changed, 63 insertions, 47 deletions
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 906e2654..a918d020 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -188,10 +188,13 @@ start:
aruco::MarkerDetector detector;
detector.setDesiredSpeed(3);
- detector.setMinMaxSize(0.06, 0.4);
- detector.setThresholdParams(11, 7);
+ detector.setThresholdParams(11, 6);
+
+ cv::Rect last_roi(65535, 65535, 0, 0);
cv::Mat color, color_, grayscale, rvec, tvec;
+
+ const double stateful_coeff = 0.81;
if (!camera->isOpened())
{
@@ -205,12 +208,8 @@ start:
auto freq = cv::getTickFrequency();
auto last_time = cv::getTickCount();
- auto prev_time = last_time;
- double last_delay = -1;
int fps = 0;
int last_fps = 0;
- double error = 0;
- std::vector<cv::Point2f> reprojection;
cv::Point2f last_centroid;
while (!stop)
{
@@ -241,25 +240,32 @@ start:
dist_coeffs.at<float>(i) = 0;
std::vector< aruco::Marker > markers;
-
- detector.detect(grayscale, markers, cv::Mat(), cv::Mat(), -1, false);
-
- if (markers.size() == 1 && markers[0].size() == 4) {
- const aruco::Marker& m = markers.at(0);
+
+ if (last_roi.width > 0 &&
+ (detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false),
+ markers.size() == 1 && markers[0].size() == 4))
+ {
+ detector.setMinMaxSize(std::max(0.2, 0.08 * grayscale.cols / last_roi.width),
+ std::min(1.0, 0.39 * grayscale.cols / last_roi.width));
+ auto& 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), scale);
+ {
+ auto& p = m.at(i);
+ p.x += last_roi.x;
+ p.y += last_roi.y;
+ }
}
-
- for (int i = 0; i < reprojection.size(); i++)
+ else
{
- cv::circle(frame,
- reprojection[i],
- 6,
- cv::Scalar(0, 255, 128),
- 3);
+ detector.setMinMaxSize(0.09, 0.4);
+ detector.detect(grayscale, markers, cv::Mat(), cv::Mat(), -1, false);
}
- cv::circle(frame, last_centroid, 7, cv::Scalar(255, 255, 0), -1);
+ 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), scale, 8);
+ }
auto time = cv::getTickCount();
@@ -278,15 +284,8 @@ start:
cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(0, 255, 0), scale);
::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);
- ::sprintf(buf, "Error: %f px", error);
- cv::putText(frame, buf, cv::Point(10, 76), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(80, 255, 0), scale);
- prev_time = time;
frame = color;
- if (frame.rows > 0)
- {
- videoWidget->update_image(frame.data, frame.cols, frame.rows);
- }
if (markers.size() == 1 && markers[0].size() == 4) {
const aruco::Marker& m = markers.at(0);
@@ -305,6 +304,29 @@ start:
obj_points.at<float>(0,0)=-size + headpos[0];
obj_points.at<float>(0,1)=size + headpos[1];
obj_points.at<float>(0,2)=0 + headpos[2];
+
+ last_roi = cv::Rect(65535, 65535, 0, 0);
+
+ for (int i = 0; i < 4; i++)
+ {
+ auto foo = m.at(i);
+ last_roi.x = std::min<int>(foo.x, last_roi.x);
+ last_roi.y = std::min<int>(foo.y, last_roi.y);
+ last_roi.width = std::max<int>(foo.x, last_roi.width);
+ last_roi.height = std::max<int>(foo.y, last_roi.height);
+ }
+ {
+ last_roi.width -= last_roi.x;
+ last_roi.height -= last_roi.y;
+ last_roi.x -= last_roi.width * stateful_coeff;
+ last_roi.y -= last_roi.height * stateful_coeff;
+ last_roi.width *= stateful_coeff * 3;
+ last_roi.height *= stateful_coeff * 3;
+ last_roi.x = std::max<int>(0, last_roi.x);
+ last_roi.y = std::max<int>(0, last_roi.y);
+ last_roi.width = std::min<int>(grayscale.cols - last_roi.x, last_roi.width);
+ 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);
@@ -316,25 +338,15 @@ start:
cv::Vec3d foo = cv::RQDecomp3x3(rotation_matrix, junk1, junk2);
- QMutexLocker lck(&mtx);
-
- for (int i = 0; i < 3; i++)
- pose[i] = tvec.at<double>(i);
-
- pose[Yaw] = foo[1];
- pose[Pitch] = -foo[0];
- pose[Roll] = foo[2];
+ {
+ QMutexLocker lck(&mtx);
- reprojection.clear();
- reprojection.resize(4);
- cv::projectPoints(obj_points, rvec, tvec, intrinsics, dist_coeffs, reprojection);
+ for (int i = 0; i < 3; i++)
+ pose[i] = tvec.at<double>(i);
- error = 0;
- for (int i = 0; i < 4; i++)
- {
- double x = reprojection[i].x - m[i].x;
- double y = reprojection[i].y - m[i].y;
- error += std::sqrt(x * x + y * y);
+ pose[Yaw] = foo[1];
+ pose[Pitch] = -foo[0];
+ pose[Roll] = foo[2];
}
std::vector<cv::Point2f> repr2;
@@ -343,10 +355,14 @@ start:
cv::projectPoints(centroid, rvec, tvec, intrinsics, dist_coeffs, repr2);
last_centroid = repr2[0];
-
- //pose[Yaw] -= atan(pose[TX] / pose[TZ]) * 180 / HT_PI;
- //pose[Pitch] -= atan(pose[TY] / pose[TZ]) * 180 / HT_PI;
}
+ else
+ {
+ last_roi = cv::Rect(65535, 65535, 0, 0);
+ }
+
+ if (frame.rows > 0)
+ videoWidget->update_image(frame.data, frame.cols, frame.rows);
}
}