diff options
Diffstat (limited to 'tracker-aruco/ftnoir_tracker_aruco.cpp')
| -rw-r--r-- | tracker-aruco/ftnoir_tracker_aruco.cpp | 429 |
1 files changed, 233 insertions, 196 deletions
diff --git a/tracker-aruco/ftnoir_tracker_aruco.cpp b/tracker-aruco/ftnoir_tracker_aruco.cpp index b222ebbb..65056a63 100644 --- a/tracker-aruco/ftnoir_tracker_aruco.cpp +++ b/tracker-aruco/ftnoir_tracker_aruco.cpp @@ -5,12 +5,6 @@ * copyright notice and this permission notice appear in all copies. */ -#include <vector> -#include <cstdio> -#include <cmath> -#include <algorithm> -#include <QMutexLocker> -#include "./include/markerdetector.h" #include "ftnoir_tracker_aruco.h" #include "opentrack/plugin-api.hpp" #include <opencv2/core/core.hpp> @@ -19,10 +13,20 @@ #include "opentrack-compat/camera-names.hpp" #include "opentrack-compat/sleep.hpp" -typedef struct { +#include <QMutexLocker> +#include <QDebug> + +#include <vector> +#include <cstdio> +#include <cmath> +#include <algorithm> +#include <iterator> + +struct resolution_tuple +{ int width; int height; -} resolution_tuple; +}; static resolution_tuple resolution_choices[] = { @@ -35,8 +39,21 @@ static resolution_tuple resolution_choices[] = Tracker::Tracker() : stop(false), layout(nullptr), - videoWidget(nullptr) + videoWidget(nullptr), + obj_points(4), + intrinsics(decltype(intrinsics)::eye()), + dist_coeffs(decltype(dist_coeffs)::zeros()), + centroid { cv::Point3f(0, 0, 0) }, + rmat(decltype(rmat)::eye()), + roi_points(4), + last_roi(65535, 65535, 0, 0), + freq(cv::getTickFrequency()), // XXX change to timer.hpp + cur_fps(0) { + // param 2 ignored for Otsu thresholding. it's required to use our fork of Aruco. + detector.setThresholdParams(5, -1); + detector.setDesiredSpeed(3); + detector._thresMethod = aruco::MarkerDetector::FIXED_THRES; } Tracker::~Tracker() @@ -79,10 +96,40 @@ void Tracker::getRT(cv::Matx33d& r_, cv::Vec3d& t_) t_ = t; } -void Tracker::run() +bool Tracker::detect_with_roi() { - cv::setNumThreads(0); + if (last_roi.width > 0) + { + detector.setMinMaxSize(std::min(1.f, std::max(0.01f, size_min * grayscale.cols / last_roi.width)), + std::max(.01f, std::min(1.0f, size_max * grayscale.cols / last_roi.width))); + + detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false); + + if (markers.size() == 1 && markers[0].size() == 4) + { + auto& m = markers.at(0); + for (unsigned i = 0; i < 4; i++) + { + auto& p = m.at(i); + p.x += last_roi.x; + p.y += last_roi.y; + } + return true; + } + } + last_roi = cv::Rect(65535, 65535, 0, 0); + return false; +} +bool Tracker::detect_without_roi() +{ + detector.setMinMaxSize(size_min, size_max); + detector.detect(grayscale, markers, cv::Mat(), cv::Mat(), -1, false); + return markers.size() == 1 && markers[0].size() == 4; +} + +bool Tracker::open_camera() +{ int rint = s.resolution; if (rint < 0 || rint >= (int)(sizeof(resolution_choices) / sizeof(resolution_tuple))) rint = 0; @@ -108,238 +155,226 @@ void Tracker::run() break; } - { - QMutexLocker l(&camera_mtx); + QMutexLocker l(&camera_mtx); - camera = cv::VideoCapture(camera_name_to_index(s.camera_name)); - if (res.width) - { - camera.set(cv::CAP_PROP_FRAME_WIDTH, res.width); - camera.set(cv::CAP_PROP_FRAME_HEIGHT, res.height); - } - if (fps) - camera.set(cv::CAP_PROP_FPS, fps); + camera = cv::VideoCapture(camera_name_to_index(s.camera_name)); + if (res.width) + { + camera.set(cv::CAP_PROP_FRAME_WIDTH, res.width); + camera.set(cv::CAP_PROP_FRAME_HEIGHT, res.height); + } + if (fps) + camera.set(cv::CAP_PROP_FPS, fps); - if (!camera.isOpened()) - { - qDebug() << "aruco tracker: can't open camera"; - return; - } + if (!camera.isOpened()) + { + qDebug() << "aruco tracker: can't open camera"; + return false; } + return true; +} - aruco::MarkerDetector detector; - detector.setDesiredSpeed(3); - detector._thresMethod = aruco::MarkerDetector::FIXED_THRES; +void Tracker::set_intrinsics() +{ + static constexpr double pi = 3.1415926f; + const int w = grayscale.cols, h = grayscale.rows; + const double diag_fov = static_cast<int>(s.fov) * pi / 180.; + const double fov_w = 2.*atan(tan(diag_fov/2.)/sqrt(1. + h/(double)w * h/(double)w)); + const double fov_h = 2.*atan(tan(diag_fov/2.)/sqrt(1. + w/(double)h * w/(double)h)); + const double focal_length_w = .5 * w / tan(.5 * fov_w); + const double focal_length_h = .5 * h / tan(.5 * fov_h); + + intrinsics(0, 0) = focal_length_w; + intrinsics(0, 2) = grayscale.cols/2; + intrinsics(1, 1) = focal_length_h; + intrinsics(1, 2) = grayscale.rows/2; +} - cv::Rect last_roi(65535, 65535, 0, 0); +void Tracker::update_fps(double alpha) +{ + std::uint64_t time = std::uint64_t(cv::getTickCount()); + const double dt = std::max(0., (time - last_time) / freq); + last_time = time; - // XXX change to timer.hpp - auto freq = cv::getTickFrequency(); - auto last_time = cv::getTickCount(); - double cur_fps = 0; - cv::Vec3d rvec, tvec; - cv::Mat intrinsics = cv::Mat::eye(3, 3, CV_32FC1); - cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1); + cur_fps = cur_fps * alpha + (1-alpha) * (fabs(dt) < 1e-6 ? 0 : 1./dt); +} - while (!stop) +void Tracker::draw_ar(bool ok) +{ + if (ok) { - cv::Mat color; - { - QMutexLocker l(&camera_mtx); + const auto& m = markers.at(0); + for (unsigned i = 0; i < 4; i++) + cv::line(frame, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), 2, 8); + } - if (!camera.read(color)) - continue; - } - cv::Mat grayscale; - cv::cvtColor(color, grayscale, cv::COLOR_RGB2GRAY); + char buf[32]; + ::sprintf(buf, "Hz: %d", (unsigned short)cur_fps); + cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(0, 255, 0), 1); +} - const int scale = grayscale.cols > 480 ? 2 : 1; - // param 2 ignored for Otsu thresholding. it's required to use our fork of Aruco. - detector.setThresholdParams(5, -1); +void Tracker::clamp_last_roi() +{ + if (last_roi.x < 0) + last_roi.x = 0; + if (last_roi.y < 0) + last_roi.y = 0; + if (last_roi.width < 0) + last_roi.width = 0; + if (last_roi.height < 0) + last_roi.height = 0; + if (last_roi.x >= color.cols-1) + last_roi.x = color.cols-1; + if (last_roi.width >= color.cols-1) + last_roi.width = color.cols-1; + if (last_roi.y >= color.rows-1) + last_roi.y = color.rows-1; + if (last_roi.height >= color.rows-1) + last_roi.height = color.rows-1; +} - static constexpr double pi = 3.1415926f; - const int w = grayscale.cols, h = grayscale.rows; - const double diag_fov = static_cast<int>(s.fov) * pi / 180.; - const double fov_w = 2.*atan(tan(diag_fov/2.)/sqrt(1. + h/(double)w * h/(double)w)); - const double fov_h = 2.*atan(tan(diag_fov/2.)/sqrt(1. + w/(double)h * w/(double)h)); - const float focal_length_w = .5 * w / tan(.5 * fov_w); - const float focal_length_h = .5 * h / tan(.5 * fov_h); +void Tracker::set_points() +{ + using f = float; + const f hx = f(s.headpos_x), hy = f(s.headpos_y), hz = f(s.headpos_z); - intrinsics.at<float> (0, 0) = focal_length_w; - intrinsics.at<float> (1, 1) = focal_length_h; - intrinsics.at<float> (0, 2) = grayscale.cols/2; - intrinsics.at<float> (1, 2) = grayscale.rows/2; + static constexpr float size = 40; - std::vector< aruco::Marker > markers; + const int x1=1, x2=2, x3=3, x4=0; - const double size_min = 0.05; - const double size_max = 0.3; + obj_points[x1] = cv::Point3f(-size + hx, -size + hy, 0 + hz); + obj_points[x2] = cv::Point3f(size + hx, -size + hy, 0 + hz); + obj_points[x3] = cv::Point3f(size + hx, size + hy, 0 + hz); + obj_points[x4] = cv::Point3f(-size + hx, size + hy, 0 + hz); +} - bool roi_valid = false; +void Tracker::draw_centroid() +{ + repr2.clear(); + cv::projectPoints(centroid, rvec, tvec, intrinsics, dist_coeffs, repr2); - auto time = cv::getTickCount(); + auto s = cv::Scalar(255, 0, 255); + cv::circle(frame, repr2.at(0), 4, s, -1); +} - const double dt = (time - last_time) / freq; - last_time = time; - cur_fps = cur_fps * 0.97 + 0.03 * (dt == 0 ? 0 : 1./dt); +bool Tracker::set_last_roi() +{ + roi_projection.clear(); - if (last_roi.width > 0 && last_roi.height) - { - detector.setMinMaxSize(std::min(1., std::max(0.01, size_min * grayscale.cols / last_roi.width)), - std::max(0.01, std::min(1.0, size_max * grayscale.cols / last_roi.width))); + if (!cv::solvePnP(obj_points, markers[0], intrinsics, dist_coeffs, rvec_, tvec_, false, cv::SOLVEPNP_ITERATIVE)) + return false; - cv::Mat grayscale_ = grayscale(last_roi).clone(); - if (detector.detect(grayscale_, markers, cv::Mat(), cv::Mat(), -1, false), - markers.size() == 1 && markers[0].size() == 4) - { - auto& m = markers.at(0); - for (unsigned i = 0; i < 4; i++) - { - auto& p = m.at(i); - p.x += last_roi.x; - p.y += last_roi.y; - } - roi_valid = true; - } - } + for (unsigned i = 0; i < 4; i++) + { + using f = float; + cv::Point3f pt(obj_points[i] - cv::Point3f(f(s.headpos_x), f(s.headpos_y), f(s.headpos_z))); + roi_points[i] = pt * c_search_window; + } + cv::projectPoints(roi_points, rvec_, tvec_, intrinsics, dist_coeffs, roi_projection); - if (!roi_valid) - { - detector.setMinMaxSize(size_min, size_max); - detector.detect(grayscale, markers, cv::Mat(), cv::Mat(), -1, false); - } + set_roi_from_projection(); - if (markers.size() == 1 && markers[0].size() == 4) { - const 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, 8); - } + return true; +} - char buf[128]; +void Tracker::set_rmat() +{ + cv::Rodrigues(rvec, rmat); - frame = color.clone(); + euler = cv::RQDecomp3x3(rmat, m_r, m_q); - ::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); + QMutexLocker lck(&mtx); - const float hx = s.headpos_x, hy = s.headpos_y, hz = s.headpos_z; + for (int i = 0; i < 3; i++) + pose[i] = tvec(i) * .1; - if (markers.size() == 1 && markers[0].size() == 4) { - const auto& m = markers.at(0); - constexpr float size = 40; + pose[Yaw] = euler[1]; + pose[Pitch] = -euler[0]; + pose[Roll] = euler[2]; - cv::Mat obj_points(4,3,CV_32FC1); - const int x1=1, x2=2, x3=3, x4=0; - obj_points.at<float>(x1,0)=-size + hx; - obj_points.at<float>(x1,1)=-size + hy; - obj_points.at<float>(x1,2)= 0 + hz; + r = rmat; + t = cv::Vec3d(tvec[0], -tvec[1], tvec[2]); +} - obj_points.at<float>(x2,0)=size + hx; - obj_points.at<float>(x2,1)=-size + hy; - obj_points.at<float>(x2,2)= 0 + hz; +void Tracker::set_roi_from_projection() +{ + last_roi = cv::Rect(color.cols-1, color.rows-1, 0, 0); - obj_points.at<float>(x3,0)=size + hx; - obj_points.at<float>(x3,1)=size + hy; - obj_points.at<float>(x3,2)= 0 + hz; + for (unsigned i = 0; i < 4; i++) + { + const auto& proj = roi_projection[i]; + int min_x = std::min(int(proj.x), last_roi.x), + min_y = std::min(int(proj.y), last_roi.y); - obj_points.at<float>(x4,0)= -size + hx; - obj_points.at<float>(x4,1)= size + hy; - obj_points.at<float>(x4,2)= 0 + hz; + int max_x = std::max(int(proj.x), last_roi.width), + max_y = std::max(int(proj.y), last_roi.height); - std::vector<cv::Point2f> img_points = m; - if (!cv::solvePnP(obj_points, img_points, intrinsics, dist_coeffs, rvec, tvec, false, cv::SOLVEPNP_ITERATIVE)) - goto fail; + last_roi.x = min_x; + last_roi.y = min_y; - { - std::vector<cv::Point2f> repr2; - std::vector<cv::Point3f> centroid; - centroid.push_back(cv::Point3f(0, 0, 0)); - cv::projectPoints(centroid, rvec, tvec, intrinsics, dist_coeffs, repr2); - - { - auto s = cv::Scalar(255, 0, 255); - cv::circle(frame, repr2.at(0), 4, s, -1); - } - } + last_roi.width = max_x; + last_roi.height = max_y; + } - for (int i = 0; i < 4; i++) - { - obj_points.at<float>(i, 0) -= hx; - obj_points.at<float>(i, 1) -= hy; - obj_points.at<float>(i, 2) -= hz; - } + last_roi.width -= last_roi.x; + last_roi.height -= last_roi.y; - cv::Mat rvec_, tvec_; - cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec_, tvec_, false, cv::SOLVEPNP_ITERATIVE); + clamp_last_roi(); +} - cv::Mat roi_points = obj_points * c_search_window; - std::vector<cv::Point2f> roi_projection(4); - cv::projectPoints(roi_points, rvec_, tvec_, intrinsics, dist_coeffs, roi_projection); +void Tracker::run() +{ + cv::setNumThreads(0); - last_roi = cv::Rect(color.cols-1, color.rows-1, 0, 0); + using std::fabs; + using std::atan; + using std::tan; + using std::sqrt; - for (int i = 0; i < 4; i++) - { - auto proj = roi_projection[i]; - int min_x = std::min<int>(proj.x, last_roi.x), - min_y = std::min<int>(proj.y, last_roi.y); + if (!open_camera()) + return; - int max_x = std::max<int>(proj.x, last_roi.width), - max_y = std::max<int>(proj.y, last_roi.height); + last_time = std::uint64_t(cv::getTickCount()); - last_roi.x = min_x; - last_roi.y = min_y; + while (!stop) + { + { + QMutexLocker l(&camera_mtx); - last_roi.width = max_x; - last_roi.height = max_y; - } + if (!camera.read(color)) + continue; + } - if (last_roi.x < 0) - last_roi.x = 0; - if (last_roi.y < 0) - last_roi.y = 0; - if (last_roi.width < 0) - last_roi.width = 0; - if (last_roi.height < 0) - last_roi.height = 0; - if (last_roi.x >= color.cols-1) - last_roi.x = color.cols-1; - if (last_roi.width >= color.cols-1) - last_roi.width = color.cols-1; - if (last_roi.y >= color.rows-1) - last_roi.y= color.rows-1; - if (last_roi.height >= color.rows-1) - last_roi.height = color.rows-1; - - last_roi.width -= last_roi.x; - last_roi.height -= last_roi.y; - - auto rmat = cv::Matx33d::zeros(); - cv::Matx33d m_r(3, 3, CV_64FC1), m_q(3, 3, CV_64FC1); - cv::Rodrigues(rvec, rmat); + cv::cvtColor(color, grayscale, cv::COLOR_RGB2GRAY); + color.copyTo(frame); - { - cv::Vec3d euler = cv::RQDecomp3x3(rmat, m_r, m_q); + set_intrinsics(); + update_fps(alpha_); - QMutexLocker lck(&mtx); + markers.clear(); - for (int i = 0; i < 3; i++) - pose[i] = tvec(i); - pose[Yaw] = euler[1]; - pose[Pitch] = -euler[0]; - pose[Roll] = euler[2]; + const bool ok = detect_with_roi() || detect_without_roi(); - r = rmat; - t = cv::Vec3d(tvec[0], -tvec[1], tvec[2]); - } + if (ok) + { + set_points(); - if (roi_valid) - cv::rectangle(frame, last_roi, cv::Scalar(255, 0, 255), 1); + if (!cv::solvePnP(obj_points, markers[0], intrinsics, dist_coeffs, rvec, tvec, false, cv::SOLVEPNP_ITERATIVE)) + goto fail; + + if (!set_last_roi()) + goto fail; + + draw_centroid(); + set_rmat(); } else fail: + // no marker found, reset search region last_roi = cv::Rect(65535, 65535, 0, 0); + draw_ar(ok); + if (frame.rows > 0) videoWidget->update_image(frame); } @@ -355,9 +390,9 @@ void Tracker::data(double *data) data[Yaw] = pose[Yaw]; data[Pitch] = pose[Pitch]; data[Roll] = pose[Roll]; - data[TX] = pose[TX] * .1; - data[TY] = pose[TY] * .1; - data[TZ] = pose[TZ] * .1; + data[TX] = pose[TX]; + data[TY] = pose[TY]; + data[TZ] = pose[TZ]; } TrackerControls::TrackerControls() @@ -391,7 +426,9 @@ void TrackerControls::toggleCalibrate() s.headpos_z = 0; calibrator.reset(); calib_timer.start(); - } else { + } + else + { cleanupCalib(); auto pos = calibrator.get_estimate(); |