diff options
| -rw-r--r-- | ftnoir_tracker_aruco/ar_video_widget.cpp | 12 | ||||
| -rw-r--r-- | ftnoir_tracker_aruco/ar_video_widget.h | 22 | ||||
| -rw-r--r-- | ftnoir_tracker_aruco/aruco-trackercontrols.ui | 91 | ||||
| -rw-r--r-- | ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 198 | ||||
| -rw-r--r-- | ftnoir_tracker_aruco/ftnoir_tracker_aruco.h | 30 | ||||
| -rw-r--r-- | ftnoir_tracker_aruco/trans_calib.cpp | 4 | ||||
| -rw-r--r-- | ftnoir_tracker_aruco/trans_calib.h | 2 |
7 files changed, 209 insertions, 150 deletions
diff --git a/ftnoir_tracker_aruco/ar_video_widget.cpp b/ftnoir_tracker_aruco/ar_video_widget.cpp index 9a089213..61a611ea 100644 --- a/ftnoir_tracker_aruco/ar_video_widget.cpp +++ b/ftnoir_tracker_aruco/ar_video_widget.cpp @@ -40,3 +40,15 @@ void ArucoVideoWidget::update_and_repaint() texture = qframe2; update(); } + +void ArucoVideoWidget::paintEvent(QPaintEvent* e) +{ + QMutexLocker foo(&mtx); + QPainter(this).drawImage(e->rect(), texture); +} + +ArucoVideoWidget::ArucoVideoWidget(QWidget* parent): QWidget(parent) +{ + connect(&timer, SIGNAL(timeout()), this, SLOT(update_and_repaint())); + timer.start(60); +}
\ No newline at end of file diff --git a/ftnoir_tracker_aruco/ar_video_widget.h b/ftnoir_tracker_aruco/ar_video_widget.h index e2cf4d9f..820ba7d0 100644 --- a/ftnoir_tracker_aruco/ar_video_widget.h +++ b/ftnoir_tracker_aruco/ar_video_widget.h @@ -22,26 +22,18 @@ class ArucoVideoWidget : public QWidget { Q_OBJECT - -public: - ArucoVideoWidget(QWidget *parent) : QWidget(parent) { - connect(&timer, SIGNAL(timeout()), this, SLOT(update_and_repaint())); - timer.start(60); - } - void update_image(const cv::Mat& frame); -protected slots: - void paintEvent( QPaintEvent* e ) { - QMutexLocker foo(&mtx); - QPainter painter(this); - painter.drawImage(e->rect(), texture); - } - void update_and_repaint(); - + private: QMutex mtx; QImage texture; QTimer timer; cv::Mat _frame; +private slots: + void update_and_repaint(); +public: + ArucoVideoWidget(QWidget *parent); + void update_image(const cv::Mat& frame); + void paintEvent( QPaintEvent*) override; }; #endif // VIDEOWIDGET_H diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui index 240ef5f8..1898d15b 100644 --- a/ftnoir_tracker_aruco/aruco-trackercontrols.ui +++ b/ftnoir_tracker_aruco/aruco-trackercontrols.ui @@ -9,7 +9,7 @@ <rect> <x>0</x> <y>0</y> - <width>560</width> + <width>562</width> <height>214</height> </rect> </property> @@ -40,35 +40,6 @@ </property> </widget> </item> - <item row="0" column="1"> - <widget class="QDoubleSpinBox" name="cameraFOV"> - <property name="sizePolicy"> - <sizepolicy hsizetype="Preferred" vsizetype="Preferred"> - <horstretch>0</horstretch> - <verstretch>0</verstretch> - </sizepolicy> - </property> - <property name="locale"> - <locale language="English" country="UnitedStates"/> - </property> - <property name="minimum"> - <double>35.000000000000000</double> - </property> - <property name="maximum"> - <double>180.000000000000000</double> - </property> - <property name="value"> - <double>52.000000000000000</double> - </property> - </widget> - </item> - <item row="1" column="0"> - <widget class="QLabel" name="label_2"> - <property name="text"> - <string>Frames per second</string> - </property> - </widget> - </item> <item row="1" column="1"> <widget class="QComboBox" name="cameraFPS"> <property name="sizePolicy"> @@ -104,10 +75,39 @@ </item> </widget> </item> - <item row="2" column="0"> - <widget class="QLabel" name="label_3"> + <item row="3" column="0"> + <widget class="QLabel" name="label_4"> <property name="text"> - <string>Camera name</string> + <string>Resolution</string> + </property> + </widget> + </item> + <item row="0" column="1"> + <widget class="QDoubleSpinBox" name="cameraFOV"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Preferred" vsizetype="Preferred"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> + <property name="locale"> + <locale language="English" country="UnitedStates"/> + </property> + <property name="minimum"> + <double>35.000000000000000</double> + </property> + <property name="maximum"> + <double>180.000000000000000</double> + </property> + <property name="value"> + <double>52.000000000000000</double> + </property> + </widget> + </item> + <item row="1" column="0"> + <widget class="QLabel" name="label_2"> + <property name="text"> + <string>Frames per second</string> </property> </widget> </item> @@ -121,10 +121,10 @@ </property> </widget> </item> - <item row="3" column="0"> - <widget class="QLabel" name="label_4"> + <item row="2" column="0"> + <widget class="QLabel" name="label_3"> <property name="text"> - <string>Resolution</string> + <string>Camera name</string> </property> </widget> </item> @@ -159,16 +159,25 @@ </widget> </item> <item row="4" column="0"> - <widget class="QLabel" name="label_10"> + <widget class="QLabel" name="label_6"> <property name="text"> - <string>Red channel only</string> + <string>Sun glare removal</string> </property> </widget> </item> <item row="4" column="1"> - <widget class="QCheckBox" name="red_only"> - <property name="text"> - <string>Mileage may vary</string> + <widget class="QSlider" name="desaturate_slider"> + <property name="maximum"> + <number>100</number> + </property> + <property name="singleStep"> + <number>10</number> + </property> + <property name="pageStep"> + <number>20</number> + </property> + <property name="orientation"> + <enum>Qt::Horizontal</enum> </property> </widget> </item> 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; diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h index c53a49a4..03fff844 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h @@ -5,8 +5,7 @@ * copyright notice and this permission notice appear in all copies. */ -#ifndef FTNOIR_TRACKER_HT_H -#define FTNOIR_TRACKER_HT_H +#pragma once #include "ui_aruco-trackercontrols.h" #include "ar_video_widget.h" @@ -19,7 +18,9 @@ #include "facetracknoir/options.h" #include "ftnoir_tracker_aruco/trans_calib.h" #include "facetracknoir/plugin-api.hpp" -#include "facetracknoir/gain-control.hpp" + +#include <opencv2/core/core.hpp> +#include <opencv2/highgui/highgui.hpp> using namespace options; @@ -27,7 +28,7 @@ struct settings { pbundle b; value<double> fov, headpos_x, headpos_y, headpos_z; value<int> camera_index, force_fps, resolution; - value<bool> red_only; + value<int> desaturate; settings() : b(bundle("aruco-tracker")), fov(b, "field-of-view", 56), @@ -37,13 +38,14 @@ struct settings { camera_index(b, "camera-index", 0), force_fps(b, "force-fps", 0), resolution(b, "force-resolution", 0), - red_only(b, "red-only", false) + desaturate(b, "desaturate", 0) {} }; class Tracker : protected QThread, public ITracker { Q_OBJECT + static constexpr double c_search_window = 2.9; public: Tracker(); ~Tracker() override; @@ -51,7 +53,7 @@ public: void GetHeadPoseData(double *data); void run(); void reload() { s.b->reload(); } - void getRT(cv::Matx33f& r, cv::Vec3f& t); + void getRT(cv::Matx33d &r, cv::Vec3d &t); private: QMutex mtx; volatile bool stop; @@ -61,9 +63,8 @@ private: double pose[6]; cv::Mat frame; cv::VideoCapture camera; - cv::Matx33f r; - cv::Vec3f t; - Gain gain; + cv::Matx33d r; + cv::Vec3d t; }; class TrackerControls : public QWidget, public ITrackerDialog @@ -71,12 +72,8 @@ class TrackerControls : public QWidget, public ITrackerDialog Q_OBJECT public: TrackerControls(); - void registerTracker(ITracker * x) { - tracker = dynamic_cast<Tracker*>(x); - } - void unRegisterTracker() { - tracker = nullptr; - } + void registerTracker(ITracker * x) { tracker = dynamic_cast<Tracker*>(x); } + void unRegisterTracker() { tracker = nullptr; } private: Ui::Form ui; Tracker* tracker; @@ -90,6 +87,3 @@ private slots: void cleanupCalib(); void update_tracker_calibration(); }; - -#endif - diff --git a/ftnoir_tracker_aruco/trans_calib.cpp b/ftnoir_tracker_aruco/trans_calib.cpp index b1f956b4..369de449 100644 --- a/ftnoir_tracker_aruco/trans_calib.cpp +++ b/ftnoir_tracker_aruco/trans_calib.cpp @@ -21,9 +21,9 @@ void TranslationCalibrator::reset() y = Vec6f(0,0,0, 0,0,0); } -void TranslationCalibrator::update(const Matx33f& R_CM_k, const Vec3f& t_CM_k) +void TranslationCalibrator::update(const Matx33d& R_CM_k, const Vec3d& t_CM_k) { - Matx<float, 6,3> H_k_T = Matx<float, 6,3>::zeros(); + Matx<double, 6,3> H_k_T = Matx<double, 6,3>::zeros(); for (int i=0; i<3; ++i) { for (int j=0; j<3; ++j) { H_k_T(i,j) = R_CM_k(j,i); diff --git a/ftnoir_tracker_aruco/trans_calib.h b/ftnoir_tracker_aruco/trans_calib.h index c2c02b38..5a2d7c0f 100644 --- a/ftnoir_tracker_aruco/trans_calib.h +++ b/ftnoir_tracker_aruco/trans_calib.h @@ -26,7 +26,7 @@ public: void reset(); // update the current estimate - void update(const cv::Matx33f& R_CM_k, const cv::Vec3f& t_CM_k); + void update(const cv::Matx33d& R_CM_k, const cv::Vec3d& t_CM_k); // get the current estimate for t_MH cv::Vec3f get_estimate(); |