summaryrefslogtreecommitdiffhomepage
path: root/ftnoir_tracker_aruco
diff options
context:
space:
mode:
Diffstat (limited to 'ftnoir_tracker_aruco')
-rw-r--r--ftnoir_tracker_aruco/aruco-trackercontrols.ui217
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp84
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco.h2
3 files changed, 124 insertions, 179 deletions
diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
index c007e93b..76d750af 100644
--- a/ftnoir_tracker_aruco/aruco-trackercontrols.ui
+++ b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
@@ -32,154 +32,7 @@
<property name="spacing">
<number>-1</number>
</property>
- <item row="7" column="0">
- <widget class="QLabel" name="label_5">
- <property name="text">
- <string>Distortion coefficients</string>
- </property>
- </widget>
- </item>
- <item row="7" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBox">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="frame">
- <bool>true</bool>
- </property>
- <property name="buttonSymbols">
- <enum>QAbstractSpinBox::NoButtons</enum>
- </property>
- <property name="decimals">
- <number>11</number>
- </property>
- <property name="minimum">
- <double>-1000.000000000000000</double>
- </property>
- <property name="maximum">
- <double>1000.000000000000000</double>
- </property>
- <property name="singleStep">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="9" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBox_3">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="frame">
- <bool>true</bool>
- </property>
- <property name="buttonSymbols">
- <enum>QAbstractSpinBox::NoButtons</enum>
- </property>
- <property name="decimals">
- <number>11</number>
- </property>
- <property name="minimum">
- <double>-1000.000000000000000</double>
- </property>
- <property name="maximum">
- <double>1000.000000000000000</double>
- </property>
- <property name="singleStep">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="10" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBox_4">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="frame">
- <bool>true</bool>
- </property>
- <property name="buttonSymbols">
- <enum>QAbstractSpinBox::NoButtons</enum>
- </property>
- <property name="decimals">
- <number>11</number>
- </property>
- <property name="minimum">
- <double>-1000.000000000000000</double>
- </property>
- <property name="maximum">
- <double>1000.000000000000000</double>
- </property>
- <property name="singleStep">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="11" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBox_5">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="frame">
- <bool>true</bool>
- </property>
- <property name="buttonSymbols">
- <enum>QAbstractSpinBox::NoButtons</enum>
- </property>
- <property name="decimals">
- <number>11</number>
- </property>
- <property name="minimum">
- <double>-1000.000000000000000</double>
- </property>
- <property name="maximum">
- <double>1000.000000000000000</double>
- </property>
- <property name="singleStep">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="8" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBox_2">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="frame">
- <bool>true</bool>
- </property>
- <property name="buttonSymbols">
- <enum>QAbstractSpinBox::NoButtons</enum>
- </property>
- <property name="decimals">
- <number>11</number>
- </property>
- <property name="minimum">
- <double>-1000.000000000000000</double>
- </property>
- <property name="maximum">
- <double>1000.000000000000000</double>
- </property>
- <property name="singleStep">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="6" column="2" rowspan="5" colspan="3">
+ <item row="6" column="2" rowspan="2" colspan="3">
<widget class="QLabel" name="label_6">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Preferred">
@@ -267,7 +120,7 @@
</property>
</widget>
</item>
- <item row="11" column="3" colspan="2">
+ <item row="8" column="3" colspan="2">
<widget class="QDialogButtonBox" name="buttonBox">
<property name="standardButtons">
<set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
@@ -378,6 +231,72 @@
</layout>
</widget>
</item>
+ <item row="7" column="1" rowspan="2">
+ <widget class="QGroupBox" name="groupBox_2">
+ <property name="title">
+ <string>Head centroid position</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ <layout class="QFormLayout" name="formLayout_2">
+ <property name="formAlignment">
+ <set>Qt::AlignHCenter|Qt::AlignTop</set>
+ </property>
+ <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="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="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="minimum">
+ <double>-200.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>200.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
</layout>
</widget>
<resources/>
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index b532ac9c..7ad80b46 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -120,8 +120,11 @@ void Tracker::load_settings()
enableTX = iniFile.value("enable-tx", true).toBool();
enableTY = iniFile.value("enable-ty", true).toBool();
enableTZ = iniFile.value("enable-tz", true).toBool();
- for (int i = 0; i < 5; i++)
- dc[i] = iniFile.value(QString("dc%1").arg(i), 0).toFloat();
+
+ for (int i = 0; i < 3; i++)
+ {
+ headpos[i] = iniFile.value(QString("headpos-%1").arg(i), 0).toDouble();
+ }
iniFile.endGroup();
}
@@ -194,6 +197,7 @@ void Tracker::run()
double error = 0;
std::vector<cv::Point2f> reprojection;
auto kernel = cv::createGaussianFilter(CV_8U, cv::Size(5, 5), 0);
+ cv::Point2f last_centroid;
while (!stop)
{
if (!camera.read(color_))
@@ -218,7 +222,7 @@ void Tracker::run()
cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1);
for (int i = 0; i < 5; i++)
- dist_coeffs.at<float>(i) = dc[i];
+ dist_coeffs.at<float>(i) = 0;
std::vector< aruco::Marker > markers;
@@ -239,6 +243,8 @@ void Tracker::run()
3);
}
+ cv::circle(frame, last_centroid, 4, cv::Scalar(0, 0, 0), -1);
+
auto time = cv::getTickCount();
if ((long) (time / freq) != (long) (last_time / freq))
@@ -271,18 +277,18 @@ void Tracker::run()
const float size = 7;
cv::Mat obj_points(4,3,CV_32FC1);
- obj_points.at<float>(1,0)=-size;
- obj_points.at<float>(1,1)=-size;
- obj_points.at<float>(1,2)=0;
- obj_points.at<float>(2,0)=size;
- obj_points.at<float>(2,1)=-size;
- obj_points.at<float>(2,2)=0;
- obj_points.at<float>(3,0)=size;
- obj_points.at<float>(3,1)=size;
- obj_points.at<float>(3,2)=0;
- obj_points.at<float>(0,0)=-size;
- obj_points.at<float>(0,1)=size;
- obj_points.at<float>(0,2)=0;
+ obj_points.at<float>(1,0)=-size + headpos[0];
+ obj_points.at<float>(1,1)=-size + headpos[1];
+ obj_points.at<float>(1,2)=0 + headpos[2];
+ obj_points.at<float>(2,0)=size + headpos[0];
+ obj_points.at<float>(2,1)=-size + headpos[1];
+ obj_points.at<float>(2,2)=0 + headpos[2];
+ obj_points.at<float>(3,0)=size + headpos[0];
+ obj_points.at<float>(3,1)=size + headpos[1];
+ obj_points.at<float>(3,2)=0 + headpos[2];
+ obj_points.at<float>(0,0)=-size + headpos[0];
+ obj_points.at<float>(0,1)=size + headpos[1];
+ obj_points.at<float>(0,2)=0 + headpos[2];
cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, false, cv::ITERATIVE);
@@ -315,6 +321,14 @@ void Tracker::run()
error += std::sqrt(x * x + y * y);
}
+ reprojection.clear();
+ reprojection.resize(1);
+ std::vector<cv::Point3f> centroid;
+ centroid.push_back(cv::Point3f(0, 0, 0));
+ cv::projectPoints(centroid, rvec, tvec, intrinsics, dist_coeffs, reprojection);
+
+ last_centroid = reprojection[0];
+
//pose[Yaw] -= atan(pose[TX] / pose[TZ]) * 180 / HT_PI;
//pose[Pitch] -= atan(pose[TY] / pose[TZ]) * 180 / HT_PI;
}
@@ -414,6 +428,9 @@ TrackerControls::TrackerControls()
connect(ui.tx, SIGNAL(stateChanged(int)), this, SLOT(settingChanged(int)));
connect(ui.ty, SIGNAL(stateChanged(int)), this, SLOT(settingChanged(int)));
connect(ui.tz, SIGNAL(stateChanged(int)), this, SLOT(settingChanged(int)));
+ connect(ui.cx, SIGNAL(valueChanged(double)), this, SLOT(settingChanged(double)));
+ connect(ui.cy, SIGNAL(valueChanged(double)), this, SLOT(settingChanged(double)));
+ connect(ui.cz, SIGNAL(valueChanged(double)), this, SLOT(settingChanged(double)));
//connect(ui.buttonCancel, SIGNAL(clicked()), this, SLOT(doCancel()));
//connect(ui.buttonOK, SIGNAL(clicked()), this, SLOT(doOK()));
//connect(ui.buttonSettings, SIGNAL(clicked()), this, SLOT(cameraSettings()));
@@ -474,13 +491,18 @@ void TrackerControls::loadSettings()
ui.ty->setCheckState(iniFile.value("enable-ty", true).toBool() ? Qt::Checked : Qt::Unchecked);
ui.tz->setCheckState(iniFile.value("enable-tz", true).toBool() ? Qt::Checked : Qt::Unchecked);
ui.resolution->setCurrentIndex(iniFile.value("resolution", 0).toInt());
-
- ui.doubleSpinBox->setValue(iniFile.value("dc0").toDouble());
- ui.doubleSpinBox_2->setValue(iniFile.value("dc1").toDouble());
- ui.doubleSpinBox_3->setValue(iniFile.value("dc2").toDouble());
- ui.doubleSpinBox_4->setValue(iniFile.value("dc3").toDouble());
- ui.doubleSpinBox_5->setValue(iniFile.value("dc4").toDouble());
-
+
+ QDoubleSpinBox* headpos[] = {
+ ui.cx,
+ ui.cy,
+ ui.cz
+ };
+
+ for (int i = 0; i < 3; i++)
+ {
+ headpos[i]->setValue(iniFile.value(QString("headpos-%1").arg(i)).toDouble());
+ }
+
iniFile.endGroup();
settingsDirty = false;
}
@@ -519,13 +541,17 @@ void TrackerControls::save()
iniFile.setValue("enable-ty", ui.ty->checkState() != Qt::Unchecked ? true : false);
iniFile.setValue("enable-tz", ui.tz->checkState() != Qt::Unchecked ? true : false);
iniFile.setValue("resolution", ui.resolution->currentIndex());
-
- iniFile.setValue("dc0", ui.doubleSpinBox->value());
- iniFile.setValue("dc1", ui.doubleSpinBox_2->value());
- iniFile.setValue("dc2", ui.doubleSpinBox_3->value());
- iniFile.setValue("dc3", ui.doubleSpinBox_4->value());
- iniFile.setValue("dc4", ui.doubleSpinBox_5->value());
-
+
+ QDoubleSpinBox* headpos[] = {
+ ui.cx,
+ ui.cy,
+ ui.cz
+ };
+
+ for (int i = 0; i < 3; i++)
+ {
+ iniFile.setValue(QString("headpos-%1").arg(i), headpos[i]->value());
+ }
iniFile.endGroup();
settingsDirty = false;
}
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
index 8d532b4c..5d3d4ed6 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
@@ -35,11 +35,11 @@ private:
volatile bool stop;
float fov;
int camera_index;
- float dc[5];
int force_fps, force_width, force_height;
void load_settings();
double pose[6];
cv::Mat frame;
+ double headpos[3];
};
// Widget that has controls for FTNoIR protocol client-settings.
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-11 18:15:17 +0000