summaryrefslogtreecommitdiffhomepage
path: root/tracker-pt/ftnoir_tracker_pt.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'tracker-pt/ftnoir_tracker_pt.cpp')
-rw-r--r--tracker-pt/ftnoir_tracker_pt.cpp273
1 files changed, 125 insertions, 148 deletions
diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp
index e35acc0e..0f8495d9 100644
--- a/tracker-pt/ftnoir_tracker_pt.cpp
+++ b/tracker-pt/ftnoir_tracker_pt.cpp
@@ -6,244 +6,221 @@
* copyright notice and this permission notice appear in all copies.
*/
+#undef NDEBUG
#include "ftnoir_tracker_pt.h"
-#include "compat/camera-names.hpp"
+#include "pt-api.hpp"
+#include "cv/init.hpp"
+#include "video/video-widget.hpp"
+#include "compat/math-imports.hpp"
+#include "compat/check-visible.hpp"
+#include "compat/thread-name.hpp"
+#include "compat/qt-dpi.hpp"
+
+#include <cassert>
#include <QHBoxLayout>
-#include <cmath>
#include <QDebug>
#include <QFile>
#include <QCoreApplication>
-#include <functional>
-//#define PT_PERF_LOG //log performance
+using namespace options;
-//-----------------------------------------------------------------------------
-Tracker_PT::Tracker_PT() :
- point_count(0),
- commands(0),
- ever_success(false)
+namespace pt_impl {
+
+Tracker_PT::Tracker_PT(pointer<pt_runtime_traits> const& traits) :
+ traits { traits },
+ s { traits->get_module_name() },
+ point_extractor { traits->make_point_extractor() },
+ frame { traits->make_frame() }
{
- connect(s.b.get(), SIGNAL(saving()), this, SLOT(maybe_reopen_camera()), Qt::DirectConnection);
- connect(&s.fov, SIGNAL(valueChanged(int)), this, SLOT(set_fov(int)), Qt::DirectConnection);
- set_fov(s.fov);
+ opencv_init();
+
+ connect(&*s.b, &bundle_::saving, this, [this]{ reopen_camera_flag = true; }, Qt::DirectConnection);
}
Tracker_PT::~Tracker_PT()
{
- set_command(ABORT);
+ requestInterruption();
wait();
- QMutexLocker l(&camera_mtx);
- camera.stop();
+ if (camera)
+ camera->stop();
}
-void Tracker_PT::set_command(Command command)
+bool Tracker_PT::check_camera()
{
- //QMutexLocker lock(&mutex);
- commands |= command;
-}
+ if (reopen_camera_flag)
+ {
+ reopen_camera_flag = false;
-void Tracker_PT::reset_command(Command command)
-{
- //QMutexLocker lock(&mutex);
- commands &= ~command;
+ camera = nullptr;
+ camera = traits->make_camera();
+ if (!camera || !camera->start(s))
+ return false;
+ }
+ assert(camera);
+ if (progn(bool x = true; return open_camera_dialog_flag.compare_exchange_strong(x, false);))
+ run_in_thread_sync(qApp->thread(), [this] { camera->show_camera_settings(); });
+ return true;
}
void Tracker_PT::run()
{
- cv::setNumThreads(0);
+ portable::set_curthread_name("tracker/pt");
-#ifdef PT_PERF_LOG
- QFile log_file(QCoreApplication::applicationDirPath() + "/PointTrackerPerformance.txt");
- if (!log_file.open(QIODevice::WriteOnly | QIODevice::Text)) return;
- QTextStream log_stream(&log_file);
-#endif
-
- maybe_reopen_camera();
-
- while((commands & ABORT) == 0)
+ while(!isInterruptionRequested())
{
- CamInfo cam_info;
- bool new_frame;
+ if (!check_camera())
+ break;
+
+ pt_camera_info info;
+ bool new_frame = false;
{
- QMutexLocker l(&camera_mtx);
- std::tie(new_frame, cam_info) = camera.get_frame(frame);
+ camera->set_fov(s.fov);
+ std::tie(new_frame, info) = camera->get_frame(*frame);
}
if (new_frame)
{
- cv::resize(frame, preview_frame, cv::Size(preview_size.width(), preview_size.height()), 0, 0, cv::INTER_NEAREST);
+ const bool preview_visible = check_is_visible();
- point_extractor.extract_points(frame, preview_frame, points);
- point_count = points.size();
+ if (preview_visible && !widget->fresh())
+ preview_frame->set_last_frame(*frame);
- f fx;
- cam_info.get_focal_length(fx);
+ point_extractor->extract_points(*frame, *preview_frame, preview_visible && !widget->fresh(), points);
+ point_count.store(points.size(), std::memory_order_relaxed);
const bool success = points.size() >= PointModel::N_POINTS;
- if (success)
- {
- point_tracker.track(points,
- PointModel(s),
- cam_info,
- s.dynamic_pose ? s.init_phase_timeout : 0);
- ever_success = true;
- }
+ Affine X_CM;
{
- Affine X_CM;
+ QMutexLocker l(&center_lock);
+
+ if (success)
{
- QMutexLocker l(&data_mtx);
- X_CM = point_tracker.pose();
+ int dynamic_pose_ms = s.dynamic_pose ? s.init_phase_timeout : 0;
+
+ point_tracker.track(points, PointModel(s), info, dynamic_pose_ms, filter, camera->deadzone_amount());
+ ever_success.store(true, std::memory_order_relaxed);
}
- Affine X_MH(mat33::eye(), vec3(s.t_MH_x, s.t_MH_y, s.t_MH_z)); // just copy pasted these lines from below
+ QMutexLocker l2(&data_lock);
+ X_CM = point_tracker.pose();
+ }
+
+ if (preview_visible && !widget->fresh())
+ {
+ const f fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y);
+ Affine X_MH(mat33::eye(), vec3(s.t_MH_x, s.t_MH_y, s.t_MH_z));
Affine X_GH = X_CM * X_MH;
vec3 p = X_GH.t; // head (center?) position in global space
- vec2 p_(p[0] / p[2] * fx, p[1] / p[2] * fx); // projected to screen
-
- static constexpr int len = 9;
-
- cv::Point p2(iround(p_[0] * preview_frame.cols + preview_frame.cols/2),
- iround(-p_[1] * preview_frame.cols + preview_frame.rows/2));
- static const cv::Scalar color(0, 0, 255);
- cv::line(preview_frame,
- cv::Point(p2.x - len, p2.y),
- cv::Point(p2.x + len, p2.y),
- color,
- 1);
- cv::line(preview_frame,
- cv::Point(p2.x, p2.y - len),
- cv::Point(p2.x, p2.y + len),
- color,
- 1);
- }
- video_widget->update_image(preview_frame);
+ if (p[2] > f(.1))
+ preview_frame->draw_head_center((p[0] * fx) / p[2], (p[1] * fx) / p[2]);
+
+ widget->update_image(preview_frame->get_bitmap());
+ }
}
}
- qDebug() << "pt: thread stopped";
}
-void Tracker_PT::maybe_reopen_camera()
+module_status Tracker_PT::start_tracker(QFrame* video_frame)
{
- QMutexLocker l(&camera_mtx);
-
- Camera::open_status status = camera.start(camera_name_to_index(s.camera_name), s.cam_fps, s.cam_res_x, s.cam_res_y);
-
- switch (status)
{
- case Camera::open_error:
- qDebug() << "can't start camera" << s.camera_name;
- break;
- case Camera::open_ok_change:
- frame = cv::Mat();
- break;
- case Camera::open_ok_no_change:
- break;
+ auto camera = traits->make_camera();
+ if (!camera || !camera->start(s))
+ return error(tr("Failed to open camera '%1'").arg(s.camera_name));
}
-}
-void Tracker_PT::set_fov(int value)
-{
- QMutexLocker l(&camera_mtx);
- camera.set_fov(value);
-}
-
-void Tracker_PT::start_tracker(QFrame* video_frame)
-{
- //video_frame->setAttribute(Qt::WA_NativeWindow);
- preview_size = video_frame->size();
-
- preview_frame = cv::Mat(video_frame->height(), video_frame->width(), CV_8UC3);
- preview_frame.setTo(cv::Scalar(0, 0, 0));
-
- video_widget = qptr<cv_video_widget>(video_frame);
- layout = qptr<QHBoxLayout>(video_frame);
+ widget = std::make_unique<video_widget>(video_frame);
+ layout = std::make_unique<QHBoxLayout>(video_frame);
layout->setContentsMargins(0, 0, 0, 0);
- layout->addWidget(video_widget.data());
- video_frame->setLayout(layout.data());
+ layout->addWidget(&*widget);
+ video_frame->setLayout(&*layout);
//video_widget->resize(video_frame->width(), video_frame->height());
video_frame->show();
- start();
+
+ double dpi = screen_dpi(video_frame);
+ preview_frame = traits->make_preview(iround(preview_width * dpi),
+ iround(preview_height * dpi));
+
+ start(QThread::HighPriority);
+
+ return {};
}
void Tracker_PT::data(double *data)
{
- if (ever_success)
+ if (ever_success.load(std::memory_order_relaxed))
{
- Affine X_CM = pose();
+ Affine X_CM;
+ {
+ QMutexLocker l(&data_lock);
+ X_CM = point_tracker.pose();
+ }
Affine X_MH(mat33::eye(), vec3(s.t_MH_x, s.t_MH_y, s.t_MH_z));
- Affine X_GH = X_CM * X_MH;
+ Affine X_GH(X_CM * X_MH);
// translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame
// -z -> x, y -> z, x -> -y
mat33 R_EG(0, 0,-1,
-1, 0, 0,
0, 1, 0);
- mat33 R = R_EG * X_GH.R * R_EG.t();
-
- using std::atan2;
- using std::sqrt;
- using std::atan;
- using std::fabs;
- using std::copysign;
+ mat33 R(R_EG * X_GH.R * R_EG.t());
// get translation(s)
const vec3& t = X_GH.t;
// extract rotation angles
- {
- f alpha, beta, gamma;
- beta = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
- alpha = atan2( R(1,0), R(0,0));
- gamma = atan2( R(2,1), R(2,2));
+ auto r00 = (double)R(0, 0);
+ auto r10 = (double)R(1,0), r20 = (double)R(2,0);
+ auto r21 = (double)R(2,1), r22 = (double)R(2,2);
-#if 0
- if (t[2] > 1e-4)
- {
- alpha += copysign(atan(t[0] / t[2]), t[0]);
- // pitch is skewed anyway due to only one focal length value
- //beta -= copysign(atan(t[1] / t[2]), t[1]);
- }
-#endif
+ double beta = atan2(-r20, sqrt(r21*r21 + r22*r22));
+ double alpha = atan2(r10, r00);
+ double gamma = atan2(r21, r22);
- data[Yaw] = rad2deg * alpha;
- data[Pitch] = -rad2deg * beta;
- data[Roll] = rad2deg * gamma;
- }
+ constexpr double rad2deg = 180/M_PI;
+
+ data[Yaw] = rad2deg * alpha;
+ data[Pitch] = -rad2deg * beta;
+ data[Roll] = rad2deg * gamma;
// convert to cm
- data[TX] = t[0] / 10;
- data[TY] = t[1] / 10;
- data[TZ] = t[2] / 10;
+ data[TX] = (double)t[0] / 10;
+ data[TY] = (double)t[1] / 10;
+ data[TZ] = (double)t[2] / 10;
}
}
-Affine Tracker_PT::pose()
+bool Tracker_PT::center()
{
- QMutexLocker l(&data_mtx);
+ QMutexLocker l(&center_lock);
- return point_tracker.pose();
+ point_tracker.reset_state();
+ return false;
}
int Tracker_PT::get_n_points()
{
- return int(point_count);
+ return (int)point_count.load(std::memory_order_relaxed);
}
-bool Tracker_PT::get_cam_info(CamInfo* info)
+bool Tracker_PT::get_cam_info(pt_camera_info& info)
{
- QMutexLocker lock(&camera_mtx);
- bool ret;
+ bool ret = false;
- std::tie(ret, *info) = camera.get_info();
+ if (camera)
+ std::tie(ret, info) = camera->get_info();
return ret;
}
-#include "ftnoir_tracker_pt_dialog.h"
-OPENTRACK_DECLARE_TRACKER(Tracker_PT, TrackerDialog_PT, PT_metadata)
+Affine Tracker_PT::pose() const
+{
+ QMutexLocker l(&data_lock);
+ return point_tracker.pose();
+}
+} // ns pt_impl
generated by cgit v1.2.3 (git 2.39.1) at 2025-12-17 08:09:31 +0000