summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
Diffstat
-rw-r--r--ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui54
-rw-r--r--ftnoir_filter_accela/ftnoir_filter_accela.cpp2
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp73
3 files changed, 78 insertions, 51 deletions
diff --git a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
index 324acdf9..c7bd2808 100644
--- a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
+++ b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
@@ -14,15 +14,15 @@
</rect>
</property>
<property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>0</width>
- <height>0</height>
+ <width>347</width>
+ <height>268</height>
</size>
</property>
<property name="maximumSize">
@@ -47,26 +47,20 @@
<property name="styleSheet">
<string notr="true"/>
</property>
- <layout class="QFormLayout" name="formLayout_3">
- <property name="fieldGrowthPolicy">
- <enum>QFormLayout::ExpandingFieldsGrow</enum>
- </property>
- <property name="horizontalSpacing">
- <number>4</number>
- </property>
- <property name="verticalSpacing">
- <number>4</number>
- </property>
+ <layout class="QGridLayout" name="gridLayout">
<property name="margin">
<number>6</number>
</property>
+ <property name="spacing">
+ <number>4</number>
+ </property>
<item row="0" column="0">
<widget class="QGroupBox" name="groupbox2">
<property name="enabled">
<bool>true</bool>
</property>
<property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
@@ -84,24 +78,6 @@
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
<layout class="QFormLayout" name="formLayout_2">
- <property name="fieldGrowthPolicy">
- <enum>QFormLayout::ExpandingFieldsGrow</enum>
- </property>
- <property name="rowWrapPolicy">
- <enum>QFormLayout::DontWrapRows</enum>
- </property>
- <property name="labelAlignment">
- <set>Qt::AlignRight|Qt::AlignTop|Qt::AlignTrailing</set>
- </property>
- <property name="formAlignment">
- <set>Qt::AlignRight|Qt::AlignTop|Qt::AlignTrailing</set>
- </property>
- <property name="horizontalSpacing">
- <number>6</number>
- </property>
- <property name="verticalSpacing">
- <number>2</number>
- </property>
<item row="0" column="0">
<widget class="QLabel" name="label_2">
<property name="sizePolicy">
@@ -121,7 +97,7 @@
<item row="0" column="1">
<widget class="QDoubleSpinBox" name="rotation_alpha">
<property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Minimum">
+ <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
@@ -162,7 +138,7 @@
<item row="1" column="1">
<widget class="QDoubleSpinBox" name="translation_alpha">
<property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Minimum">
+ <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
@@ -216,14 +192,14 @@ background:none;</string>
<item row="2" column="1">
<widget class="QSpinBox" name="spinZoom">
<property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Minimum">
+ <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>96</width>
+ <width>20</width>
<height>22</height>
</size>
</property>
@@ -259,7 +235,7 @@ background:none;</string>
<item row="0" column="1">
<widget class="QGroupBox" name="groupBox">
<property name="sizePolicy">
- <sizepolicy hsizetype="Maximum" vsizetype="Fixed">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
@@ -529,7 +505,7 @@ background:none;</string>
</layout>
</widget>
</item>
- <item row="3" column="1">
+ <item row="1" column="1">
<widget class="QLabel" name="label_9">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
@@ -574,7 +550,7 @@ background:none;</string>
</property>
</widget>
</item>
- <item row="4" column="0" colspan="2">
+ <item row="2" column="1">
<widget class="QDialogButtonBox" name="buttonBox">
<property name="standardButtons">
<set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
index abde4d46..29fe9067 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
@@ -64,7 +64,7 @@ void FTNoIR_Filter::receiveSettings(double rot, double trans, double zoom_fac)
static inline double parabola(const double a, const double x)
{
const double a1 = 1./a;
- return a1 * x * x;
+ return a1 * pow(x, 2.2);
}
void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position,
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 43ffc717..b532ac9c 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -185,18 +185,28 @@ void Tracker::run()
fprintf(stderr, "aruco tracker: can't open camera\n");
return;
}
-
+
+ auto freq = cv::getTickFrequency();
+ auto last_time = cv::getTickCount();
+ auto prev_time = last_time;
+ int fps = 0;
+ int last_fps = 0;
+ double error = 0;
+ std::vector<cv::Point2f> reprojection;
+ auto kernel = cv::createGaussianFilter(CV_8U, cv::Size(5, 5), 0);
while (!stop)
{
if (!camera.read(color_))
continue;
color_.copyTo(color);
cv::cvtColor(color, grayscale2, cv::COLOR_BGR2GRAY);
- const int kernel = grayscale2.cols > 480 ? 7 : 5;
- int kernel2 = kernel * grayscale2.rows / grayscale2.cols - 1;
- if ((kernel2 % 2) == 0)
- kernel2++;
- cv::GaussianBlur(grayscale2, grayscale, cv::Size(kernel, kernel2), 0, 0);
+ if (grayscale.empty())
+ grayscale = grayscale2.clone();
+
+ kernel->apply(grayscale2, grayscale);
+
+ const int scale = frame.cols > 480 ? 2 : 1;
+
const float focal_length_w = 0.5 * grayscale.cols / tan(0.5 * fov * HT_PI / 180);
const float focal_length_h = 0.5 * grayscale.rows / tan(0.5 * fov * grayscale.rows / grayscale.cols * HT_PI / 180.0);
cv::Mat intrinsics = cv::Mat::eye(3, 3, CV_32FC1);
@@ -204,7 +214,7 @@ void Tracker::run()
intrinsics.at<float> (1, 1) = focal_length_h;
intrinsics.at<float> (0, 2) = grayscale.cols/2;
intrinsics.at<float> (1, 2) = grayscale.rows/2;
-
+
cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1);
for (int i = 0; i < 5; i++)
@@ -217,11 +227,40 @@ void Tracker::run()
if (markers.size() == 1 && markers[0].size() == 4) {
const aruco::Marker& m = markers.at(0);
for (int i = 0; i < 4; i++)
- cv::line(color, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), 4);
+ cv::line(color, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), scale);
}
-
+
+ for (int i = 0; i < reprojection.size(); i++)
+ {
+ cv::circle(frame,
+ reprojection[i],
+ 6,
+ cv::Scalar(0, 255, 128),
+ 3);
+ }
+
+ auto time = cv::getTickCount();
+
+ if ((long) (time / freq) != (long) (last_time / freq))
+ {
+ last_fps = fps;
+ fps = 0;
+ last_time = time;
+ }
+
+ fps++;
+
+ char buf[128];
+
+ std::sprintf(buf, "Hz: %ld", last_fps);
+ cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(0, 255, 0), scale);
+ std::sprintf(buf, "Delay: %ld ms", (long) (1000 * (time - prev_time) / freq));
+ cv::putText(frame, buf, cv::Point(10, 54), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(80, 255, 0), scale);
+ std::sprintf(buf, "Error: %f px", error);
+ cv::putText(frame, buf, cv::Point(10, 76), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(80, 255, 0), scale);
+ prev_time = time;
+
frame = color;
-
if (frame.rows > 0)
{
videoWidget->update_image(frame.data, frame.cols, frame.rows);
@@ -263,7 +302,19 @@ void Tracker::run()
pose[Yaw] = foo[1];
pose[Pitch] = -foo[0];
pose[Roll] = foo[2];
-
+
+ reprojection.clear();
+ reprojection.resize(4);
+ cv::projectPoints(obj_points, rvec, tvec, intrinsics, dist_coeffs, reprojection);
+
+ error = 0;
+ for (int i = 0; i < 4; i++)
+ {
+ double x = reprojection[i].x - m[i].x;
+ double y = reprojection[i].y - m[i].y;
+ error += std::sqrt(x * x + y * y);
+ }
+
//pose[Yaw] -= atan(pose[TX] / pose[TZ]) * 180 / HT_PI;
//pose[Pitch] -= atan(pose[TY] / pose[TZ]) * 180 / HT_PI;
}
generated by cgit v1.2.3 (git 2.39.1) at 2025-03-02 20:29:55 +0000