summaryrefslogtreecommitdiffhomepage
path: root/ftnoir_tracker_aruco
diff options
context:
space:
mode:
Diffstat (limited to 'ftnoir_tracker_aruco')
-rw-r--r--ftnoir_tracker_aruco/ar_video_widget.cpp12
-rw-r--r--ftnoir_tracker_aruco/ar_video_widget.h22
-rw-r--r--ftnoir_tracker_aruco/aruco-trackercontrols.ui91
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp198
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco.h30
-rw-r--r--ftnoir_tracker_aruco/trans_calib.cpp4
-rw-r--r--ftnoir_tracker_aruco/trans_calib.h2
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();
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-17 16:22:03 +0000