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.cpp126
1 files changed, 54 insertions, 72 deletions
diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp
index 0d99ccd0..b717e4a1 100644
--- a/tracker-pt/ftnoir_tracker_pt.cpp
+++ b/tracker-pt/ftnoir_tracker_pt.cpp
@@ -7,14 +7,12 @@
*/
#include "ftnoir_tracker_pt.h"
-#include "cv/video-widget.hpp"
+#include "video/video-widget.hpp"
#include "compat/camera-names.hpp"
#include "compat/math-imports.hpp"
#include "pt-api.hpp"
-#include <cmath>
-
#include <QHBoxLayout>
#include <QDebug>
#include <QFile>
@@ -49,11 +47,7 @@ Tracker_PT::~Tracker_PT()
void Tracker_PT::run()
{
-#ifdef PT_PERF_LOG
- QFile log_file(OPENTRACK_BASE_PATH + "/PointTrackerPerformance.txt");
- if (!log_file.open(QIODevice::WriteOnly | QIODevice::Text)) return;
- QTextStream log_stream(&log_file);
-#endif
+ maybe_reopen_camera();
while(!isInterruptionRequested())
{
@@ -69,58 +63,50 @@ void Tracker_PT::run()
if (new_frame)
{
- while (center_flag.test_and_set())
- (void)0;
-
*preview_frame = *frame;
point_extractor->extract_points(*frame, *preview_frame, points);
point_count = points.size();
- const double fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y);
-
+ const f fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y);
const bool success = points.size() >= PointModel::N_POINTS;
- if (success)
- {
- point_tracker.track(points,
- PointModel(s),
- 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();
+ point_tracker.track(points,
+ PointModel(s),
+ info,
+ s.dynamic_pose ? s.init_phase_timeout : 0);
+ ever_success = true;
}
- // just copy pasted these lines from below
- 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
-
- preview_frame->draw_head_center((p[0] * fx) / p[2], (p[1] * fx) / p[2]);
+ QMutexLocker l2(&data_lock);
+ X_CM = point_tracker.pose();
}
- video_widget->update_image(preview_frame->get_bitmap());
+ 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
+
+ preview_frame->draw_head_center((p[0] * fx) / p[2], (p[1] * fx) / p[2]);
+ widget->update_image(preview_frame->get_bitmap());
{
int w = -1, h = -1;
- video_widget->get_preview_size(w, h);
+ widget->get_preview_size(w, h);
if (w != preview_width || h != preview_height)
{
- preview_width = w, preview_height = h;
+ preview_width = w; preview_height = h;
preview_frame = traits->make_preview(w, h);
}
}
-
- center_flag.clear();
}
}
- qDebug() << "pt: thread stopped";
}
bool Tracker_PT::maybe_reopen_camera()
@@ -141,17 +127,14 @@ module_status Tracker_PT::start_tracker(QFrame* video_frame)
{
//video_frame->setAttribute(Qt::WA_NativeWindow);
- video_widget = std::make_unique<cv_video_widget>(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.get());
+ layout->addWidget(widget.get());
video_frame->setLayout(layout.get());
//video_widget->resize(video_frame->width(), video_frame->height());
video_frame->show();
- if (!maybe_reopen_camera())
- return { tr("Can't open camera") };
-
start(QThread::HighPriority);
return {};
@@ -161,71 +144,70 @@ void Tracker_PT::data(double *data)
{
if (ever_success)
{
- 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();
+ 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));
-
- data[Yaw] = rad2deg * alpha;
- data[Pitch] = -rad2deg * beta;
- data[Roll] = rad2deg * gamma;
- }
+ double alpha, beta, gamma;
+ beta = atan2( (double)-R(2,0), (double)sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
+ alpha = atan2( (double)R(1,0), (double)R(0,0) );
+ gamma = atan2( (double)R(2,1), (double)R(2,2) );
+
+ 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;
}
}
bool Tracker_PT::center()
{
- while (center_flag.test_and_set())
- (void)0;
+ QMutexLocker l(&center_lock);
point_tracker.reset_state();
-
- center_flag.clear();
-
return false;
}
-Affine Tracker_PT::pose()
-{
- QMutexLocker l(&data_mtx);
-
- return point_tracker.pose();
-}
-
int Tracker_PT::get_n_points()
{
return int(point_count);
}
-bool Tracker_PT::get_cam_info(pt_camera_info* info)
+bool Tracker_PT::get_cam_info(pt_camera_info& info)
{
- QMutexLocker lock(&camera_mtx);
+ QMutexLocker l(&camera_mtx);
bool ret;
- std::tie(ret, *info) = camera->get_info();
+ std::tie(ret, info) = camera->get_info();
return ret;
}
+Affine Tracker_PT::pose() const
+{
+ QMutexLocker l(&data_lock);
+ return point_tracker.pose();
+}
+
} // ns pt_module
generated by cgit v1.2.3 (git 2.39.1) at 2025-05-21 09:47:37 +0000