summaryrefslogtreecommitdiffhomepage
path: root/tracker-easy/ftnoir_tracker_pt.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'tracker-easy/ftnoir_tracker_pt.cpp')
-rw-r--r--tracker-easy/ftnoir_tracker_pt.cpp374
1 files changed, 374 insertions, 0 deletions
diff --git a/tracker-easy/ftnoir_tracker_pt.cpp b/tracker-easy/ftnoir_tracker_pt.cpp
new file mode 100644
index 00000000..947970c3
--- /dev/null
+++ b/tracker-easy/ftnoir_tracker_pt.cpp
@@ -0,0 +1,374 @@
+/* Copyright (c) 2012 Patrick Ruoff
+ * Copyright (c) 2014-2016 Stanislaw Halik <sthalik@misaki.pl>
+ * Copyright (c) 2019 Stephane Lenclud
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ */
+
+#include "ftnoir_tracker_pt.h"
+#include "video/video-widget.hpp"
+#include "compat/math-imports.hpp"
+#include "compat/check-visible.hpp"
+
+#include "pt-api.hpp"
+
+#include <QHBoxLayout>
+#include <QDebug>
+#include <QFile>
+#include <QCoreApplication>
+
+#include <opencv2\calib3d.hpp>
+
+#include <iostream>
+
+using namespace options;
+
+namespace pt_impl {
+
+EasyTracker::EasyTracker(pointer<pt_runtime_traits> const& traits) :
+ traits { traits },
+ s { traits->get_module_name() },
+ point_extractor { traits->make_point_extractor() },
+ camera { traits->make_camera() },
+ frame { traits->make_frame() },
+ preview_frame { traits->make_preview(preview_width, preview_height) }
+{
+ cv::setBreakOnError(true);
+ cv::setNumThreads(1);
+
+ connect(s.b.get(), &bundle_::saving, this, &EasyTracker::maybe_reopen_camera, Qt::DirectConnection);
+ connect(s.b.get(), &bundle_::reloading, this, &EasyTracker::maybe_reopen_camera, Qt::DirectConnection);
+
+ connect(&s.fov, value_::value_changed<int>(), this, &EasyTracker::set_fov, Qt::DirectConnection);
+ set_fov(s.fov);
+}
+
+EasyTracker::~EasyTracker()
+{
+ requestInterruption();
+ wait();
+
+ QMutexLocker l(&camera_mtx);
+ camera->stop();
+}
+
+
+// Compute Euler angles from ratation matrix
+cv::Vec3f EulerAngles(cv::Mat &R)
+{
+
+ float sy = sqrt(R.at<double>(0, 0) * R.at<double>(0, 0) + R.at<double>(1, 0) * R.at<double>(1, 0));
+
+ bool singular = sy < 1e-6; // If
+
+ float x, y, z;
+ if (!singular)
+ {
+ x = atan2(R.at<double>(2, 1), R.at<double>(2, 2));
+ y = atan2(-R.at<double>(2, 0), sy);
+ z = atan2(R.at<double>(1, 0), R.at<double>(0, 0));
+ }
+ else
+ {
+ x = atan2(-R.at<double>(1, 2), R.at<double>(1, 1));
+ y = atan2(-R.at<double>(2, 0), sy);
+ z = 0;
+ }
+
+ // Convert to degrees
+ return cv::Vec3f(x* 180 / CV_PI, y* 180 / CV_PI, z* 180 / CV_PI);
+}
+
+
+void getEulerAngles(cv::Mat &rotCamerMatrix, cv::Vec3d &eulerAngles)
+{
+
+ cv::Mat cameraMatrix, rotMatrix, transVect, rotMatrixX, rotMatrixY, rotMatrixZ;
+ double* _r = rotCamerMatrix.ptr<double>();
+ double projMatrix[12] = { _r[0],_r[1],_r[2],0,
+ _r[3],_r[4],_r[5],0,
+ _r[6],_r[7],_r[8],0 };
+
+ cv::decomposeProjectionMatrix(cv::Mat(3, 4, CV_64FC1, projMatrix),
+ cameraMatrix,
+ rotMatrix,
+ transVect,
+ rotMatrixX,
+ rotMatrixY,
+ rotMatrixZ,
+ eulerAngles);
+}
+
+
+void EasyTracker::run()
+{
+ maybe_reopen_camera();
+
+ while(!isInterruptionRequested())
+ {
+ pt_camera_info info;
+ bool new_frame = false;
+
+ {
+ QMutexLocker l(&camera_mtx);
+
+ if (camera)
+ std::tie(new_frame, info) = camera->get_frame(*frame);
+ }
+
+ if (new_frame)
+ {
+ const bool preview_visible = check_is_visible();
+
+ if (preview_visible)
+ *preview_frame = *frame;
+
+ iImagePoints.clear();
+ point_extractor->extract_points(*frame, *preview_frame, points, iImagePoints);
+ point_count.store(points.size(), std::memory_order_relaxed);
+
+ const bool success = points.size() >= KPointCount;
+
+ int topPointIndex = -1;
+
+ {
+ QMutexLocker l(&center_lock);
+
+ if (success)
+ {
+ ever_success.store(true, std::memory_order_relaxed);
+
+ // Solve P3P problem with OpenCV
+
+ // Construct the points defining the object we want to detect based on settings.
+ // We are converting them from millimeters to centimeters.
+ // TODO: Need to support clip too. That's cap only for now.
+ // s.active_model_panel != PointModel::Clip
+
+ std::vector<cv::Point3f> objectPoints;
+ objectPoints.push_back(cv::Point3f(s.cap_x/10.0, s.cap_z / 10.0, -s.cap_y / 10.0)); // Right
+ objectPoints.push_back(cv::Point3f(-s.cap_x/10.0, s.cap_z / 10.0, -s.cap_y / 10.0)); // Left
+ objectPoints.push_back(cv::Point3f(0, 0, 0)); // Top
+
+ //Bitmap origin is top left
+ std::vector<cv::Point2f> trackedPoints;
+ // Stuff bitmap point in there making sure they match the order of the object point
+ // Find top most point, that's the one with min Y as we assume our guy's head is not up side down
+
+ int minY = std::numeric_limits<int>::max();
+ for (int i = 0; i < 3; i++)
+ {
+ if (iImagePoints[i][1]<minY)
+ {
+ minY = iImagePoints[i][1];
+ topPointIndex = i;
+ }
+ }
+
+ int rightPointIndex = -1;
+ int maxX = 0;
+
+ // Find right most point
+ for (int i = 0; i < 3; i++)
+ {
+ // Excluding top most point
+ if (i!=topPointIndex && iImagePoints[i][0] > maxX)
+ {
+ maxX = iImagePoints[i][0];
+ rightPointIndex = i;
+ }
+ }
+
+ // Find left most point
+ int leftPointIndex = -1;
+ for (int i = 0; i < 3; i++)
+ {
+ // Excluding top most point
+ if (i != topPointIndex && i != rightPointIndex)
+ {
+ leftPointIndex = i;
+ break;
+ }
+ }
+
+ //
+ trackedPoints.push_back(cv::Point2f(iImagePoints[rightPointIndex][0], iImagePoints[rightPointIndex][1]));
+ trackedPoints.push_back(cv::Point2f(iImagePoints[leftPointIndex][0], iImagePoints[leftPointIndex][1]));
+ trackedPoints.push_back(cv::Point2f(iImagePoints[topPointIndex][0], iImagePoints[topPointIndex][1]));
+
+ std::cout << "Object: " << objectPoints << "\n";
+ std::cout << "Points: " << trackedPoints << "\n";
+
+
+ // Create our camera matrix
+ // TODO: Just do that once, use data member instead
+ // Double or Float?
+ cv::Mat cameraMatrix;
+ cameraMatrix.create(3, 3, CV_64FC1);
+ cameraMatrix.setTo(cv::Scalar(0));
+ cameraMatrix.at<double>(0, 0) = camera->info.focalLengthX;
+ cameraMatrix.at<double>(1, 1) = camera->info.focalLengthY;
+ cameraMatrix.at<double>(0, 2) = camera->info.principalPointX;
+ cameraMatrix.at<double>(1, 2) = camera->info.principalPointY;
+ cameraMatrix.at<double>(2, 2) = 1;
+
+ // Create distortion cooefficients
+ cv::Mat distCoeffs = cv::Mat::zeros(8, 1, CV_64FC1);
+ // As per OpenCV docs they should be thus: k1, k2, p1, p2, k3, k4, k5, k6
+ distCoeffs.at<double>(0, 0) = 0; // Radial first order
+ distCoeffs.at<double>(1, 0) = camera->info.radialDistortionSecondOrder; // Radial second order
+ distCoeffs.at<double>(2, 0) = 0; // Tangential first order
+ distCoeffs.at<double>(3, 0) = 0; // Tangential second order
+ distCoeffs.at<double>(4, 0) = 0; // Radial third order
+ distCoeffs.at<double>(5, 0) = camera->info.radialDistortionFourthOrder; // Radial fourth order
+ distCoeffs.at<double>(6, 0) = 0; // Radial fith order
+ distCoeffs.at<double>(7, 0) = camera->info.radialDistortionSixthOrder; // Radial sixth order
+
+ // Define our solution arrays
+ // They will receive up to 4 solutions for our P3P problem
+
+
+ // TODO: try SOLVEPNP_AP3P too
+ iAngles.clear();
+ iBestSolutionIndex = -1;
+ int solutionCount = cv::solveP3P(objectPoints, trackedPoints, cameraMatrix, distCoeffs, iRotations, iTranslations, cv::SOLVEPNP_P3P);
+
+ if (solutionCount > 0)
+ {
+ std::cout << "Solution count: " << solutionCount << "\n";
+ int minPitch = std::numeric_limits<int>::max();
+ // Find the solution we want
+ for (int i = 0; i < solutionCount; i++)
+ {
+ std::cout << "Translation:\n";
+ std::cout << iTranslations.at(i);
+ std::cout << "\n";
+ std::cout << "Rotation:\n";
+ //std::cout << rvecs.at(i);
+ cv::Mat rotationCameraMatrix;
+ cv::Rodrigues(iRotations[i], rotationCameraMatrix);
+ cv::Vec3d angles;
+ getEulerAngles(rotationCameraMatrix,angles);
+ iAngles.push_back(angles);
+
+ // Check if pitch is closest to zero
+ int absolutePitch = std::abs(angles[0]);
+ if (minPitch > absolutePitch)
+ {
+ minPitch = absolutePitch;
+ iBestSolutionIndex = i;
+ }
+
+ //cv::Vec3f angles=EulerAngles(quaternion);
+ std::cout << angles;
+ std::cout << "\n";
+ }
+
+ std::cout << "\n";
+
+ }
+
+ }
+
+ // Send solution data back to main thread
+ QMutexLocker l2(&data_lock);
+ if (iBestSolutionIndex != -1)
+ {
+ iBestAngles = iAngles[iBestSolutionIndex];
+ iBestTranslation = iTranslations[iBestSolutionIndex];
+ }
+
+ }
+
+ if (preview_visible)
+ {
+ if (topPointIndex != -1)
+ {
+ // Render a cross to indicate which point is the head
+ preview_frame->draw_head_center(points[topPointIndex][0], points[topPointIndex][1]);
+ }
+
+ widget->update_image(preview_frame->get_bitmap());
+
+ auto [ w, h ] = widget->preview_size();
+ if (w != preview_width || h != preview_height)
+ {
+ preview_width = w; preview_height = h;
+ preview_frame = traits->make_preview(w, h);
+ }
+ }
+ }
+ }
+}
+
+bool EasyTracker::maybe_reopen_camera()
+{
+ QMutexLocker l(&camera_mtx);
+
+ return camera->start(s.camera_name,
+ s.cam_fps, s.cam_res_x, s.cam_res_y);
+}
+
+void EasyTracker::set_fov(int value)
+{
+ QMutexLocker l(&camera_mtx);
+ camera->set_fov(value);
+}
+
+module_status EasyTracker::start_tracker(QFrame* video_frame)
+{
+ //video_frame->setAttribute(Qt::WA_NativeWindow);
+
+ widget = std::make_unique<video_widget>(video_frame);
+ layout = std::make_unique<QHBoxLayout>(video_frame);
+ layout->setContentsMargins(0, 0, 0, 0);
+ layout->addWidget(widget.get());
+ video_frame->setLayout(layout.get());
+ //video_widget->resize(video_frame->width(), video_frame->height());
+ video_frame->show();
+
+ start(QThread::HighPriority);
+
+ return {};
+}
+
+void EasyTracker::data(double *data)
+{
+ if (ever_success.load(std::memory_order_relaxed))
+ {
+ // Get data back from tracker thread
+ QMutexLocker l(&data_lock);
+ data[Yaw] = iBestAngles[1];
+ data[Pitch] = iBestAngles[0];
+ data[Roll] = iBestAngles[2];
+ data[TX] = iBestTranslation[0];
+ data[TY] = iBestTranslation[1];
+ data[TZ] = iBestTranslation[2];
+ }
+}
+
+bool EasyTracker::center()
+{
+ QMutexLocker l(&center_lock);
+ //TODO: Do we need to do anything there?
+ return false;
+}
+
+int EasyTracker::get_n_points()
+{
+ return (int)point_count.load(std::memory_order_relaxed);
+}
+
+bool EasyTracker::get_cam_info(pt_camera_info& info)
+{
+ QMutexLocker l(&camera_mtx);
+ bool ret;
+
+ std::tie(ret, info) = camera->get_info();
+ return ret;
+}
+
+
+} // ns pt_impl
generated by cgit v1.2.3 (git 2.39.1) at 2025-10-31 16:33:52 +0000