From 22857e786cd7462eb087cf039cefd36076784280 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sat, 17 Oct 2015 17:15:53 +0200 Subject: pt: reformat more --- ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | 42 ++++++++++++++++----------------- 1 file changed, 21 insertions(+), 21 deletions(-) (limited to 'ftnoir_tracker_pt/ftnoir_tracker_pt.cpp') diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp index 19d1bd7f..11fe64cc 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp @@ -19,8 +19,8 @@ Tracker_PT::Tracker_PT() : mutex(QMutex::Recursive), commands(0), - video_widget(NULL), - video_frame(NULL), + video_widget(NULL), + video_frame(NULL), ever_success(false) { connect(s.b.get(), SIGNAL(saving()), this, SLOT(apply_settings())); @@ -28,8 +28,8 @@ Tracker_PT::Tracker_PT() Tracker_PT::~Tracker_PT() { - set_command(ABORT); - wait(); + set_command(ABORT); + wait(); delete video_widget; video_widget = NULL; if (video_frame->layout()) delete video_frame->layout(); @@ -39,13 +39,13 @@ Tracker_PT::~Tracker_PT() void Tracker_PT::set_command(Command command) { //QMutexLocker lock(&mutex); - commands |= command; + commands |= command; } void Tracker_PT::reset_command(Command command) { //QMutexLocker lock(&mutex); - commands &= ~command; + commands &= ~command; } bool Tracker_PT::get_focal_length(float& ret) @@ -69,13 +69,13 @@ bool Tracker_PT::get_focal_length(float& ret) void Tracker_PT::run() { #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); + QFile log_file(QCoreApplication::applicationDirPath() + "/PointTrackerPerformance.txt"); + if (!log_file.open(QIODevice::WriteOnly | QIODevice::Text)) return; + QTextStream log_stream(&log_file); #endif apply_settings(); - + while((commands & ABORT) == 0) { const double dt = time.elapsed() * 1e-9; @@ -97,20 +97,20 @@ void Tracker_PT::run() // blobs are sorted in order of circularity if (points.size() > PointModel::N_POINTS) points.resize(PointModel::N_POINTS); - + bool success = points.size() == PointModel::N_POINTS; - + ever_success |= success; float fx; if (!get_focal_length(fx)) continue; - + if (success) { point_tracker.track(points, PointModel(s), fx, s.dynamic_pose, s.init_phase_timeout); } - + { Affine X_CM = pose(); Affine X_MH(cv::Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z)); // just copy pasted these lines from below @@ -119,7 +119,7 @@ void Tracker_PT::run() cv::Vec2f p_(p[0] / p[2] * fx, p[1] / p[2] * fx); // projected to screen points.push_back(p_); } - + for (unsigned i = 0; i < points.size(); i++) { auto& p = points[i]; @@ -138,7 +138,7 @@ void Tracker_PT::run() color, 4); } - + video_widget->update_image(frame); } #ifdef PT_PERF_LOG @@ -181,26 +181,26 @@ void Tracker_PT::data(double *data) if (ever_success) { Affine X_CM = pose(); - + Affine X_MH(cv::Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z)); Affine X_GH = X_CM * X_MH; - + cv::Matx33f R = X_GH.R; cv::Vec3f t = X_GH.t; - + // translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame // -z -> x, y -> z, x -> -y cv::Matx33f R_EG(0, 0,-1, -1, 0, 0, 0, 1, 0); R = R_EG * R * R_EG.t(); - + // extract rotation angles float 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)); - + // extract rotation angles data[Yaw] = rad2deg * alpha; data[Pitch] = -rad2deg * beta; -- cgit v1.2.3 From c89a1d04630cf7e334fa36b01c9240f0bc0b82c6 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 18 Oct 2015 09:32:46 +0200 Subject: pt: use previous pose on NaN result from POSIT This will reset dynamic pose resolution if the error persists. --- ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | 35 ++++++++++++++++++++++++++++++--- ftnoir_tracker_pt/point_tracker.h | 4 ++++ 2 files changed, 36 insertions(+), 3 deletions(-) (limited to 'ftnoir_tracker_pt/ftnoir_tracker_pt.cpp') diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp index 11fe64cc..29b32dad 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp @@ -66,6 +66,11 @@ bool Tracker_PT::get_focal_length(float& ret) return false; } +static inline bool nanp(double value) +{ + return std::isnan(value) || std::isinf(value); +} + void Tracker_PT::run() { #ifdef PT_PERF_LOG @@ -100,19 +105,43 @@ void Tracker_PT::run() bool success = points.size() == PointModel::N_POINTS; - ever_success |= success; - float fx; if (!get_focal_length(fx)) continue; + Affine X_CM_ = pose(); + if (success) { point_tracker.track(points, PointModel(s), fx, s.dynamic_pose, s.init_phase_timeout); } + + Affine X_CM = pose(); + + { + int j = 0; + + for (int i = 0; i < 3; i++) + { + if (nanp(X_CM.t(i))) + goto nannan; + for (; j < 3; j++) + if (nanp(X_CM.R(i, j))) + { +nannan: success = false; + X_CM = X_CM_; + { + QMutexLocker lock(&mutex); + point_tracker.reset(X_CM_); + } + goto nannannan; + } + } + } + +nannannan: ever_success |= success; { - Affine X_CM = pose(); Affine X_MH(cv::Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z)); // just copy pasted these lines from below Affine X_GH = X_CM * X_MH; cv::Vec3f p = X_GH.t; // head (center?) position in global space diff --git a/ftnoir_tracker_pt/point_tracker.h b/ftnoir_tracker_pt/point_tracker.h index cdcf2998..fd002948 100644 --- a/ftnoir_tracker_pt/point_tracker.h +++ b/ftnoir_tracker_pt/point_tracker.h @@ -118,6 +118,10 @@ public: void track(const std::vector& projected_points, const PointModel& model, float f, bool dynamic_pose, int init_phase_timeout); Affine pose() const { return X_CM; } cv::Vec2f project(const cv::Vec3f& v_M, float f); + void reset(const Affine& pose) + { + X_CM = pose; + } private: // the points in model order struct PointOrder -- cgit v1.2.3 From d9746e7d4c4f7ed687c23b746914a9c5af8e9b49 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Tue, 20 Oct 2015 03:24:30 +0200 Subject: all: update copyright where appropriate --- ftnoir_protocol_sc/ftnoir_protocol_sc.cpp | 26 ++++++++++++++----------- ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | 1 + ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp | 1 + ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h | 1 + ftnoir_tracker_pt/point_extractor.cpp | 1 + ftnoir_tracker_pt/pt_video_widget.cpp | 1 + opentrack-compat/process-list.hpp | 7 +++++++ opentrack/shortcuts.cpp | 2 +- opentrack/win32-shortcuts.cpp | 8 ++++++++ qfunctionconfigurator/functionconfig.cpp | 8 ++++++++ qfunctionconfigurator/functionconfig.h | 2 +- qfunctionconfigurator/qfunctionconfigurator.cpp | 7 +++++++ qfunctionconfigurator/qfunctionconfigurator.h | 2 +- 13 files changed, 53 insertions(+), 14 deletions(-) (limited to 'ftnoir_tracker_pt/ftnoir_tracker_pt.cpp') diff --git a/ftnoir_protocol_sc/ftnoir_protocol_sc.cpp b/ftnoir_protocol_sc/ftnoir_protocol_sc.cpp index 5d7ab778..0c6cb486 100644 --- a/ftnoir_protocol_sc/ftnoir_protocol_sc.cpp +++ b/ftnoir_protocol_sc/ftnoir_protocol_sc.cpp @@ -1,9 +1,13 @@ -/* Copyright (c) 2015 Stanislaw Halik - * Copyright (c) 2015 Wim Vriend - * - * 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. +/* Homepage http://facetracknoir.sourceforge.net/home/default.htm * + * * + * ISC License (ISC) * + * * + * Copyright (c) 2015, Wim Vriend + * Copyright (c) 2014, Stanislaw Halik + * * + * 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_protocol_sc.h" #include "opentrack/plugin-api.hpp" @@ -80,7 +84,7 @@ public: # define PREFIX "" #else # define PREFIX "lib" -#endif +#endif QString path = QCoreApplication::applicationDirPath() + "/" PREFIX "opentrack-proto-simconnect.dll"; QByteArray name = QFile::encodeName(path); actx.lpSource = name.constData(); @@ -113,11 +117,11 @@ private: }; bool FTNoIR_Protocol::correct() -{ - if (!SCClientLib.isLoaded()) +{ + if (!SCClientLib.isLoaded()) { ActivationContext ctx(142 + static_cast(s.sxs_manifest)); - + if (ctx.is_ok()) { SCClientLib.setFileName("SimConnect.dll"); @@ -171,7 +175,7 @@ void FTNoIR_Protocol::handle() void CALLBACK FTNoIR_Protocol::processNextSimconnectEvent(SIMCONNECT_RECV* pData, DWORD, void *self_) { FTNoIR_Protocol& self = *reinterpret_cast(self_); - + switch(pData->dwID) { default: diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp index 29b32dad..2db5006c 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp @@ -1,4 +1,5 @@ /* Copyright (c) 2012 Patrick Ruoff + * Copyright (c) 2014-2015 Stanislaw Halik * * Permission to use, copy, modify, and/or distribute this software for any * purpose with or without fee is hereby granted, provided that the above diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp index da7a6113..b1ae2238 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp @@ -1,4 +1,5 @@ /* Copyright (c) 2012 Patrick Ruoff + * Copyright (c) 2014-2015 Stanislaw Halik * * Permission to use, copy, modify, and/or distribute this software for any * purpose with or without fee is hereby granted, provided that the above diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h index d7ddb6a9..85eec8f9 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h @@ -1,4 +1,5 @@ /* Copyright (c) 2012 Patrick Ruoff + * Copyright (c) 2014-2015 Stanislaw Halik * * Permission to use, copy, modify, and/or distribute this software for any * purpose with or without fee is hereby granted, provided that the above diff --git a/ftnoir_tracker_pt/point_extractor.cpp b/ftnoir_tracker_pt/point_extractor.cpp index a1e61f28..ec37dd00 100644 --- a/ftnoir_tracker_pt/point_extractor.cpp +++ b/ftnoir_tracker_pt/point_extractor.cpp @@ -1,4 +1,5 @@ /* Copyright (c) 2012 Patrick Ruoff + * Copyright (c) 2014-2015 Stanislaw Halik * * Permission to use, copy, modify, and/or distribute this software for any * purpose with or without fee is hereby granted, provided that the above diff --git a/ftnoir_tracker_pt/pt_video_widget.cpp b/ftnoir_tracker_pt/pt_video_widget.cpp index 9f2b90f6..cbb7c268 100644 --- a/ftnoir_tracker_pt/pt_video_widget.cpp +++ b/ftnoir_tracker_pt/pt_video_widget.cpp @@ -1,4 +1,5 @@ /* Copyright (c) 2012 Patrick Ruoff + * Copyright (c) 2015 Stanislaw Halik * * Permission to use, copy, modify, and/or distribute this software for any * purpose with or without fee is hereby granted, provided that the above diff --git a/opentrack-compat/process-list.hpp b/opentrack-compat/process-list.hpp index 65735740..ef3b325f 100644 --- a/opentrack-compat/process-list.hpp +++ b/opentrack-compat/process-list.hpp @@ -1,3 +1,10 @@ +/* Copyright (c) 2015 Stanislaw Halik + * + * 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. + */ + #pragma once #include diff --git a/opentrack/shortcuts.cpp b/opentrack/shortcuts.cpp index 3ad8e9be..718dd778 100644 --- a/opentrack/shortcuts.cpp +++ b/opentrack/shortcuts.cpp @@ -1,4 +1,4 @@ -/* Copyright (c) 2014, Stanislaw Halik +/* Copyright (c) 2014-2015, Stanislaw Halik * Permission to use, copy, modify, and/or distribute this * software for any purpose with or without fee is hereby granted, diff --git a/opentrack/win32-shortcuts.cpp b/opentrack/win32-shortcuts.cpp index feaf6036..96232631 100644 --- a/opentrack/win32-shortcuts.cpp +++ b/opentrack/win32-shortcuts.cpp @@ -1,3 +1,11 @@ +/* Copyright (c) 2015, Stanislaw Halik + + * 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. + */ + #if defined(_WIN32) # ifndef DIRECTINPUT_VERSION # define DIRECTINPUT_VERSION 0x800 diff --git a/qfunctionconfigurator/functionconfig.cpp b/qfunctionconfigurator/functionconfig.cpp index 8c0bbc9f..cac8121c 100644 --- a/qfunctionconfigurator/functionconfig.cpp +++ b/qfunctionconfigurator/functionconfig.cpp @@ -1,3 +1,11 @@ +/* Copyright (c) 2012-2015, Stanislaw Halik + + * 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 #include #include diff --git a/qfunctionconfigurator/functionconfig.h b/qfunctionconfigurator/functionconfig.h index 31aebdf6..6d76d0de 100644 --- a/qfunctionconfigurator/functionconfig.h +++ b/qfunctionconfigurator/functionconfig.h @@ -1,4 +1,4 @@ -/* Copyright (c) 2011-2014, Stanislaw Halik +/* Copyright (c) 2012-2015, Stanislaw Halik * Permission to use, copy, modify, and/or distribute this * software for any purpose with or without fee is hereby granted, diff --git a/qfunctionconfigurator/qfunctionconfigurator.cpp b/qfunctionconfigurator/qfunctionconfigurator.cpp index e62049db..bcb895ec 100644 --- a/qfunctionconfigurator/qfunctionconfigurator.cpp +++ b/qfunctionconfigurator/qfunctionconfigurator.cpp @@ -1,3 +1,10 @@ +/* Copyright (c) 2012-2015 Stanislaw Halik + * + * 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 "opentrack/options.hpp" using namespace options; #include "qfunctionconfigurator/qfunctionconfigurator.h" diff --git a/qfunctionconfigurator/qfunctionconfigurator.h b/qfunctionconfigurator/qfunctionconfigurator.h index 8957c898..667886cd 100644 --- a/qfunctionconfigurator/qfunctionconfigurator.h +++ b/qfunctionconfigurator/qfunctionconfigurator.h @@ -1,4 +1,4 @@ -/* Copyright (c) 2011-2014 Stanislaw Halik +/* Copyright (c) 2012-2015 Stanislaw Halik * * Permission to use, copy, modify, and/or distribute this software for any * purpose with or without fee is hereby granted, provided that the above -- cgit v1.2.3