diff options
Diffstat (limited to 'ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp')
| -rw-r--r-- | ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 198 |
1 files changed, 125 insertions, 73 deletions
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp index 31aa2372..0974f0f0 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp @@ -8,6 +8,7 @@ #include <vector> #include <cstdio> #include <cmath> +#include <algorithm> #include <QMutexLocker> #include "./include/markerdetector.h" #include "ftnoir_tracker_aruco.h" @@ -135,7 +136,7 @@ void Tracker::StartTracker(QFrame* videoframe) #define HT_PI 3.1415926535 -void Tracker::getRT(cv::Matx33f& r_, cv::Vec3f& t_) +void Tracker::getRT(cv::Matx33d& r_, cv::Vec3d& t_) { QMutexLocker l(&mtx); @@ -183,10 +184,6 @@ void Tracker::run() cv::Rect last_roi(65535, 65535, 0, 0); - cv::Mat color, color_, grayscale, rvec, tvec; - - const double stateful_coeff = 0.88; - if (!camera.isOpened()) { fprintf(stderr, "aruco tracker: can't open camera\n"); @@ -201,19 +198,46 @@ void Tracker::run() while (!stop) { - if (!camera.read(color_)) + cv::Mat color; + if (!camera.read(color)) continue; auto tm = cv::getTickCount(); - color_.copyTo(color); - if (s.red_only) + + const double c = s.desaturate * 16. / 100.; + + if (std::abs(c) > 1e-3) { - cv::Mat channel[3]; - cv::split(color, channel); - grayscale = channel[2]; - } else - cv::cvtColor(color, grayscale, cv::COLOR_BGR2GRAY); - - gain.tick(camera, grayscale); + const int w=color.cols, h=color.rows; + + cv::Mat hsv; + cv::cvtColor(color, hsv, cv::COLOR_BGR2HSV); + vector<cv::Mat> channels; + cv::split(hsv, channels); + cv::Mat sat = channels[1]; + cv::Mat val = channels[2]; + + struct ops { + static double sig(double x) + { + double x_ = -6 + x * 2 * 6; + return 1./(1.+exp(-x_)); + } + }; + + for (int i = 0; i < h; i++) + for (int j = 0; j < w; j++) + { + const double sat_ij = sat.at<unsigned char>(i, j)/255.; + val.at<unsigned char>(i, j) *= std::max(0., 1. - c*ops::sig(sat_ij)); + } + + channels[1] = sat; + channels[2] = val; + cv::merge(channels, hsv); + cv::cvtColor(hsv, color, cv::COLOR_HSV2BGR); + } + cv::Mat grayscale; + cv::cvtColor(color, grayscale, cv::COLOR_BGR2GRAY); const int scale = frame.cols > 480 ? 2 : 1; detector.setThresholdParams(scale > 1 ? 11 : 7, 4); @@ -232,27 +256,33 @@ void Tracker::run() const double size_min = 0.04; const double size_max = 0.38; - - if (last_roi.width > 0 && - (detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false), - markers.size() == 1 && markers[0].size() == 4)) + + bool roi_valid = false; + + if (last_roi.width > 0 && last_roi.height) { detector.setMinMaxSize(std::max(0.01, size_min * grayscale.cols / last_roi.width), std::min(1.0, size_max * grayscale.cols / last_roi.width)); - auto& m = markers.at(0); - for (int i = 0; i < 4; i++) + if (detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false), + markers.size() == 1 && markers[0].size() == 4) { - auto& p = m.at(i); - p.x += last_roi.x; - p.y += last_roi.y; + auto& m = markers.at(0); + for (int i = 0; i < 4; i++) + { + auto& p = m.at(i); + p.x += last_roi.x; + p.y += last_roi.y; + } + roi_valid = true; } } - else + + if (!roi_valid) { detector.setMinMaxSize(size_min, size_max); detector.detect(grayscale, markers, cv::Mat(), cv::Mat(), -1, false); } - + if (markers.size() == 1 && markers[0].size() == 4) { const auto& m = markers.at(0); for (int i = 0; i < 4; i++) @@ -275,70 +305,89 @@ void Tracker::run() frame = color.clone(); ::sprintf(buf, "Hz: %d", last_fps); - cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(0, 255, 0), scale); + cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(0, 255, 0), 1); ::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); + cv::putText(frame, buf, cv::Point(10, 54), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(80, 255, 0), 1); if (markers.size() == 1 && markers[0].size() == 4) { const auto& m = markers.at(0); const float size = 40; cv::Mat obj_points(4,3,CV_32FC1); - obj_points.at<float>(1,0)=-size + s.headpos_x; - obj_points.at<float>(1,1)=-size + s.headpos_y; - obj_points.at<float>(1,2)=-size + s.headpos_z; - obj_points.at<float>(2,0)=size + s.headpos_x; - obj_points.at<float>(2,1)=-size + s.headpos_y; - obj_points.at<float>(2,2)=-size + s.headpos_z; - obj_points.at<float>(3,0)=size + s.headpos_x; - obj_points.at<float>(3,1)=size + s.headpos_y; - obj_points.at<float>(3,2)=size + s.headpos_z; - obj_points.at<float>(0,0)=-size + s.headpos_x; - obj_points.at<float>(0,1)=size + s.headpos_y; - obj_points.at<float>(0,2)=size + s.headpos_z; - - last_roi = cv::Rect(65535, 65535, 0, 0); - + const int x1=1, x2=2, x3=3, x4=0; + obj_points.at<float>(x1,0)=-size + s.headpos_x; + obj_points.at<float>(x1,1)=-size + s.headpos_y; + obj_points.at<float>(x1,2)= 0 + s.headpos_z; + + obj_points.at<float>(x2,0)=size + s.headpos_x; + obj_points.at<float>(x2,1)=-size + s.headpos_y; + obj_points.at<float>(x2,2)= 0 + s.headpos_z; + + obj_points.at<float>(x3,0)=size + s.headpos_x; + obj_points.at<float>(x3,1)=size + s.headpos_y; + obj_points.at<float>(x3,2)= 0 + s.headpos_z; + + obj_points.at<float>(x4,0)= -size + s.headpos_x; + 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); + cv::Mat roi_points = obj_points * c_search_window; + cv::projectPoints(roi_points, rvec, tvec, intrinsics, dist_coeffs, roi_projection); + + last_roi = cv::Rect(color.cols-1, color.rows-1, 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); + 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); + + int max_x = std::max<int>(proj.x, last_roi.width), + max_y = std::max<int>(proj.y, last_roi.height); + + last_roi.x = min_x; + last_roi.y = min_y; + + last_roi.width = max_x; + last_roi.height = max_y; } - - cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, false, cv::ITERATIVE); - cv::Mat rotation_matrix = cv::Mat::zeros(3, 3, CV_64FC1); - cv::Mat junk1(3, 3, CV_64FC1), junk2(3, 3, CV_64FC1); - cv::Rodrigues(rvec, rotation_matrix); + + if (last_roi.x < 0) + last_roi.x = 0; + if (last_roi.y < 0) + last_roi.y = 0; + + if (last_roi.width+1 > color.cols) + last_roi.width = color.cols-1; + + if (last_roi.height+1 > color.rows) + 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::Vec3d euler = cv::RQDecomp3x3(rotation_matrix, junk1, junk2); + cv::Vec3d euler = cv::RQDecomp3x3(rmat, m_r, m_q); QMutexLocker lck(&mtx); for (int i = 0; i < 3; i++) - pose[i] = tvec.at<double>(i); - + pose[i] = tvec(i); pose[Yaw] = euler[1]; pose[Pitch] = -euler[0]; pose[Roll] = euler[2]; - rotation_matrix.convertTo(r, CV_32FC1); - tvec.convertTo(t, CV_32FC1); + r = rmat; + t = tvec; } std::vector<cv::Point2f> repr2; @@ -350,6 +399,9 @@ void Tracker::run() auto s = cv::Scalar(255, 0, 255); cv::circle(frame, repr2.at(0), 4, s, -1); } + + if (roi_valid) + cv::rectangle(frame, last_roi, cv::Scalar(255, 0, 255), 1); last_centroid = repr2[0]; } @@ -438,7 +490,7 @@ TrackerControls::TrackerControls() tie_setting(s.headpos_x, ui.cx); tie_setting(s.headpos_y, ui.cy); tie_setting(s.headpos_z, ui.cz); - tie_setting(s.red_only, ui.red_only); + tie_setting(s.desaturate, ui.desaturate_slider); connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); connect(ui.btn_calibrate, SIGNAL(clicked()), this, SLOT(toggleCalibrate())); @@ -467,8 +519,8 @@ void TrackerControls::update_tracker_calibration() { if (calib_timer.isActive() && tracker) { - cv::Matx33f r; - cv::Vec3f t; + cv::Matx33d r; + cv::Vec3d t; tracker->getRT(r, t); calibrator.update(r, t); auto pos = calibrator.get_estimate() * .1; |