summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
Diffstat
-rw-r--r--ftnoir_tracker_aruco/aruco-trackercontrols.ui455
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp18
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco.h4
3 files changed, 249 insertions, 228 deletions
diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
index e699eccc..7dadb58c 100644
--- a/ftnoir_tracker_aruco/aruco-trackercontrols.ui
+++ b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
@@ -10,7 +10,7 @@
<x>0</x>
<y>0</y>
<width>630</width>
- <height>315</height>
+ <height>326</height>
</rect>
</property>
<property name="sizePolicy">
@@ -32,169 +32,42 @@
<property name="spacing">
<number>7</number>
</property>
- <item row="0" column="3" rowspan="3" colspan="2">
- <widget class="QGroupBox" name="groupBox">
- <property name="title">
- <string>Enable axes</string>
- </property>
- <layout class="QFormLayout" name="formLayout">
- <property name="horizontalSpacing">
- <number>7</number>
+ <item row="2" column="1" colspan="2">
+ <widget class="QComboBox" name="cameraName"/>
+ </item>
+ <item row="1" column="1" colspan="2">
+ <widget class="QComboBox" name="cameraFPS">
+ <item>
+ <property name="text">
+ <string notr="true">Default</string>
</property>
- <property name="leftMargin">
- <number>6</number>
+ </item>
+ <item>
+ <property name="text">
+ <string>30</string>
</property>
- <property name="topMargin">
- <number>0</number>
+ </item>
+ <item>
+ <property name="text">
+ <string>60</string>
</property>
- <property name="rightMargin">
- <number>6</number>
+ </item>
+ <item>
+ <property name="text">
+ <string>120</string>
</property>
- <property name="bottomMargin">
- <number>0</number>
+ </item>
+ <item>
+ <property name="text">
+ <string>180</string>
</property>
- <item row="0" column="0">
- <widget class="QCheckBox" name="rx">
- <property name="text">
- <string>RX</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QCheckBox" name="tx">
- <property name="text">
- <string>TX</string>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QCheckBox" name="ry">
- <property name="text">
- <string>RY</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QCheckBox" name="ty">
- <property name="text">
- <string>TY</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QCheckBox" name="rz">
- <property name="text">
- <string>RZ</string>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QCheckBox" name="tz">
- <property name="text">
- <string>TZ</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item row="9" column="1" rowspan="3">
- <widget class="QGroupBox" name="groupBox_2">
- <property name="title">
- <string>Head position</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- <layout class="QGridLayout" name="gridLayout_2">
- <item row="0" column="0">
- <widget class="QLabel" name="label_5">
- <property name="text">
- <string>TX</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QDoubleSpinBox" name="cx">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimum">
- <double>-200.000000000000000</double>
- </property>
- <property name="maximum">
- <double>200.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLabel" name="label_7">
- <property name="text">
- <string>TY</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QDoubleSpinBox" name="cy">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimum">
- <double>-200.000000000000000</double>
- </property>
- <property name="maximum">
- <double>200.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_8">
- <property name="text">
- <string>TZ</string>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QDoubleSpinBox" name="cz">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimum">
- <double>-200.000000000000000</double>
- </property>
- <property name="maximum">
- <double>200.000000000000000</double>
- </property>
- </widget>
- </item>
- </layout>
+ </item>
</widget>
</item>
- <item row="2" column="1" colspan="2">
- <widget class="QComboBox" name="cameraName"/>
- </item>
- <item row="0" column="1" colspan="2">
- <widget class="QDoubleSpinBox" name="cameraFOV">
- <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>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Frames per second</string>
</property>
</widget>
</item>
@@ -205,14 +78,7 @@
</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="6" column="2" rowspan="5" colspan="3">
+ <item row="6" column="2" rowspan="6" colspan="3">
<widget class="QLabel" name="label_6">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Preferred">
@@ -243,33 +109,41 @@
</property>
</widget>
</item>
- <item row="1" column="1" colspan="2">
- <widget class="QComboBox" name="cameraFPS">
- <item>
- <property name="text">
- <string notr="true">Default</string>
- </property>
- </item>
- <item>
- <property name="text">
- <string>30</string>
- </property>
- </item>
- <item>
- <property name="text">
- <string>60</string>
- </property>
- </item>
- <item>
- <property name="text">
- <string>120</string>
- </property>
- </item>
- <item>
- <property name="text">
- <string>180</string>
- </property>
- </item>
+ <item row="0" column="1" colspan="2">
+ <widget class="QDoubleSpinBox" name="cameraFOV">
+ <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="2" column="0">
+ <widget class="QLabel" name="label_3">
+ <property name="text">
+ <string>Camera name</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="0">
+ <widget class="QLabel" name="label_10">
+ <property name="text">
+ <string>Red channel only</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Horizontal FOV</string>
+ </property>
</widget>
</item>
<item row="6" column="1">
@@ -296,28 +170,7 @@
</item>
</widget>
</item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_3">
- <property name="text">
- <string>Camera name</string>
- </property>
- </widget>
- </item>
- <item row="7" column="0">
- <widget class="QLabel" name="label_10">
- <property name="text">
- <string>Red channel only</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Horizontal FOV</string>
- </property>
- </widget>
- </item>
- <item row="11" column="4">
+ <item row="12" column="4">
<widget class="QDialogButtonBox" name="buttonBox">
<property name="standardButtons">
<set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
@@ -331,6 +184,16 @@
</property>
</widget>
</item>
+ <item row="8" column="1">
+ <widget class="QDoubleSpinBox" name="marker_pitch">
+ <property name="minimum">
+ <double>-180.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>180.000000000000000</double>
+ </property>
+ </widget>
+ </item>
<item row="8" column="0">
<widget class="QLabel" name="label_9">
<property name="text">
@@ -338,13 +201,164 @@
</property>
</widget>
</item>
- <item row="8" column="1">
- <widget class="QDoubleSpinBox" name="marker_pitch">
- <property name="minimum">
- <double>-180.000000000000000</double>
+ <item row="10" column="1" rowspan="3">
+ <widget class="QGroupBox" name="groupBox_2">
+ <property name="title">
+ <string>Head position</string>
</property>
- <property name="maximum">
- <double>180.000000000000000</double>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_7">
+ <property name="text">
+ <string>TY</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_5">
+ <property name="text">
+ <string>TX</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QDoubleSpinBox" name="cx">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimum">
+ <double>-200.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>200.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="label_8">
+ <property name="text">
+ <string>TZ</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QDoubleSpinBox" name="cz">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimum">
+ <double>-200.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>200.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QDoubleSpinBox" name="cy">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimum">
+ <double>-200.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>200.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item row="0" column="3" rowspan="3" colspan="2">
+ <widget class="QGroupBox" name="groupBox">
+ <property name="title">
+ <string>Enable axes</string>
+ </property>
+ <layout class="QFormLayout" name="formLayout">
+ <property name="horizontalSpacing">
+ <number>7</number>
+ </property>
+ <property name="leftMargin">
+ <number>6</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>6</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item row="0" column="0">
+ <widget class="QCheckBox" name="rx">
+ <property name="text">
+ <string>RX</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QCheckBox" name="tx">
+ <property name="text">
+ <string>TX</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QCheckBox" name="ry">
+ <property name="text">
+ <string>RY</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QCheckBox" name="ty">
+ <property name="text">
+ <string>TY</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QCheckBox" name="rz">
+ <property name="text">
+ <string>RZ</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QCheckBox" name="tz">
+ <property name="text">
+ <string>TZ</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item row="9" column="1">
+ <widget class="QCheckBox" name="fisheye_correction">
+ <property name="text">
+ <string>Check if yaw/X interconnect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="9" column="0">
+ <widget class="QLabel" name="label_11">
+ <property name="text">
+ <string>Fisheye correction</string>
</property>
</widget>
</item>
@@ -357,6 +371,7 @@
<tabstop>resolution</tabstop>
<tabstop>red_only</tabstop>
<tabstop>marker_pitch</tabstop>
+ <tabstop>fisheye_correction</tabstop>
<tabstop>cx</tabstop>
<tabstop>cy</tabstop>
<tabstop>cz</tabstop>
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 776d443a..67c88e3e 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -18,7 +18,8 @@
#include <cstdio>
#if defined(_WIN32)
-#define NOMINMAX
+# undef NOMINMAX
+# define NOMINMAX
# define NO_DSHOW_STRSAFE
# include <dshow.h>
#else
@@ -322,17 +323,13 @@ void Tracker::run()
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, !first, cv::ITERATIVE);
+ cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, !first, cv::ITERATIVE);
first = false;
-
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);
-
+
{
cv::Vec3d euler = cv::RQDecomp3x3(rotation_matrix, junk1, junk2);
@@ -350,6 +347,12 @@ void Tracker::run()
pose[Yaw] = euler[1];
pose[Pitch] = -euler[0];
pose[Roll] = euler[2];
+
+ if (s.fisheye_correction)
+ {
+ pose[Yaw] -= atan(pose[TX] / pose[TZ]) * 180 / HT_PI;
+ pose[Pitch] -= atan(pose[TY] / pose[TZ]) * 180 / HT_PI;
+ }
}
std::vector<cv::Point2f> repr2;
@@ -473,6 +476,7 @@ TrackerControls::TrackerControls()
tie_setting(s.headpos_z, ui.cz);
tie_setting(s.red_only, ui.red_only);
tie_setting(s.marker_pitch, ui.marker_pitch);
+ tie_setting(s.fisheye_correction, ui.fisheye_correction);
connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
ui.cameraName->addItems(get_camera_names());
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
index 4cab84b5..e32e304a 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
@@ -28,6 +28,7 @@ struct settings {
value<bool> red_only;
value<bool> eyaw, epitch, eroll, ex, ey, ez;
value<double> marker_pitch;
+ value<bool> fisheye_correction;
settings() :
b(bundle("aruco-tracker")),
fov(b, "field-of-view", 56),
@@ -44,7 +45,8 @@ struct settings {
ex(b, "enable-x", true),
ey(b, "enable-y", true),
ez(b, "enable-z", true),
- marker_pitch(b, "marker-pitch", 0)
+ marker_pitch(b, "marker-pitch", 0),
+ fisheye_correction(b, "fisheye-correction", false)
{}
};
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-06 23:48:57 +0000