diff options
Diffstat (limited to 'logic/tracker.cpp')
| -rw-r--r-- | logic/tracker.cpp | 385 |
1 files changed, 385 insertions, 0 deletions
diff --git a/logic/tracker.cpp b/logic/tracker.cpp new file mode 100644 index 00000000..c7580500 --- /dev/null +++ b/logic/tracker.cpp @@ -0,0 +1,385 @@ +/* Copyright (c) 2012-2015 Stanislaw Halik <sthalik@misaki.pl> + * + * 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. + */ + +/* + * this file appeared originally in facetracknoir, was rewritten completely + * following opentrack fork. + * + * originally written by Wim Vriend. + */ + +#include "compat/sleep.hpp" + +#include "tracker.h" +#include <cmath> +#include <algorithm> +#include <cstdio> + +#if defined(_WIN32) +# include <windows.h> +#endif + +Tracker::Tracker(Mappings &m, SelectedLibraries &libs, TrackLogger &logger) : + m(m), + newpose {0,0,0, 0,0,0}, + libs(libs), + logger(logger), + r_b(get_camera_offset_matrix(c_div).t()), + r_b_real(get_camera_offset_matrix(1).t()), + t_b {0,0,0}, + centerp(s.center_at_startup), + enabledp(true), + zero_(false), + should_quit(false) +{ +} + +Tracker::~Tracker() +{ + should_quit = true; + wait(); +} + +Tracker::rmat Tracker::get_camera_offset_matrix(double c) +{ + const double off[] = + { + d2r * c * (double)-s.camera_yaw, + d2r * c * (double)-s.camera_pitch, + d2r * c * (double)-s.camera_roll + }; + + return euler::euler_to_rmat(off); +} + +double Tracker::map(double pos, Map& axis) +{ + bool altp = (pos < 0) && axis.opts.altp; + axis.spline_main.setTrackingActive( !altp ); + axis.spline_alt.setTrackingActive( altp ); + auto& fc = altp ? axis.spline_alt : axis.spline_main; + return fc.getValue(pos); +} + +void Tracker::t_compensate(const rmat& rmat, const euler_t& xyz_, euler_t& output, bool rz) +{ + // TY is really yaw axis. need swapping accordingly. + const euler_t ret = rmat * euler_t(xyz_(TZ), -xyz_(TX), -xyz_(TY)); + if (!rz) + output(2) = ret(0); + else + output(2) = xyz_(2); + output(1) = -ret(2); + output(0) = -ret(1); +} + +#include "compat/nan.hpp" + +static inline double elide_nan(double value, double def) +{ + if (nanp(value)) + { + if (nanp(def)) + return 0; + return def; + } + return value; +} + +static bool is_nan(const dmat<3,3>& r, const dmat<3, 1>& t) +{ + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + if (nanp(r(i, j))) + return true; + + for (int i = 0; i < 3; i++) + if (nanp(t(i))) + return true; + + return false; +} + +static bool is_nan(const Pose& value) +{ + for (int i = 0; i < 6; i++) + if (nanp(value(i))) + return true; + return false; +} + +constexpr double Tracker::c_mult; +constexpr double Tracker::c_div; + +void Tracker::logic() +{ + using namespace euler; + + logger.write_dt(); + logger.reset_dt(); + + Pose value, raw; + + for (int i = 0; i < 6; i++) + { + auto& axis = m(i); + int k = axis.opts.src; + if (k < 0 || k >= 6) + value(i) = 0; + else + value(i) = newpose[k]; + raw(i) = newpose[i]; + } + + logger.write_pose(raw); // raw + + if (is_nan(raw)) + raw = last_raw; + + // TODO fix gimbal lock by dividing euler angle input by >=3. + // maintain the real rmat separately for translation compensation + // TODO split this function, it's too big + rmat r, r_real; + + { + euler_t tmp = d2r * euler_t(&value[Yaw]); + r = euler_to_rmat(c_div * tmp); + r_real = euler_to_rmat(tmp); + } + + euler_t t(value(0), value(1), value(2)); + + bool do_center_now = false; + bool nan = is_nan(r, t); + + if (centerp && !nan) + { + for (int i = 0; i < 6; i++) + if (fabs(newpose[i]) != 0) + { + do_center_now = true; + break; + } + } + + const rmat cam = get_camera_offset_matrix(c_div); + const rmat cam_real = get_camera_offset_matrix(1); + + r = r * cam; + r_real = r_real * cam_real; + + if (do_center_now) + { + centerp = false; + + if (libs.pFilter) + libs.pFilter->center(); + + if (libs.pTracker->center()) + { + r_b = cam.t(); + r_b_real = cam_real.t(); + r = rmat::eye(); + r_real = rmat::eye(); + } + else + { + r_b = r.t(); + r_b_real = r_real.t(); + } + + for (int i = 0; i < 3; i++) + t_b[i] = t(i); + } + + { + switch (s.center_method) + { + // inertial + case 0: + default: + r = r_b * r; + break; + // camera + case 1: + r = r * r_b; + break; + } + + const euler_t rot = r2d * c_mult * rmat_to_euler(r); + euler_t pos(t(0) - t_b[0], t(1) - t_b[1], t(2) - t_b[2]); + + if (s.use_camera_offset_from_centering) + t_compensate(r_b_real.t() * cam_real.t(), pos, pos, false); + else + t_compensate(cam_real.t(), pos, pos, false); + + for (int i = 0; i < 3; i++) + { + value(i) = pos(i); + value(i+3) = rot(i); + } + } + + logger.write_pose(value); // "corrected" - after various transformations to account for camera position + + // whenever something can corrupt its internal state due to nan/inf, elide the call + if (is_nan(value)) + { + nan = true; + logger.write_pose(value); // "filtered" + } + else + { + { + Pose tmp = value; + + if (libs.pFilter) + libs.pFilter->filter(tmp, value); + } + logger.write_pose(value); // "filtered" + + // CAVEAT rotation only, due to tcomp + for (int i = 3; i < 6; i++) + value(i) = map(value(i), m(i)); + + for (int i = 0; i < 6; i++) + value(i) += m(i).opts.zero; + + for (int i = 0; i < 6; i++) + value(i) *= int(m(i).opts.invert) * -2 + 1; + + if (zero_) + for (int i = 0; i < 6; i++) + value(i) = 0; + + if (is_nan(value)) + nan = true; + } + + if (s.tcomp_p) + { + euler_t value_(value(TX), value(TY), value(TZ)); + t_compensate(euler_to_rmat(euler_t(value(Yaw) * d2r, value(Pitch) * d2r, value(Roll) * d2r)), + value_, + value_, + s.tcomp_tz); + if (is_nan(r, value_)) + nan = true; + for (int i = 0; i < 3; i++) + value(i) = value_(i); + } + + // CAVEAT translation only, due to tcomp + for (int i = 0; i < 3; i++) + value(i) = map(value(i), m(i)); + + logger.write_pose(value); // "mapped" + + if (nan) + { + value = last_mapped; + + // for widget last value display + for (int i = 0; i < 6; i++) + (void) map(value(i), m(i)); + } + + libs.pProtocol->pose(value); + + last_mapped = value; + last_raw = raw; + + QMutexLocker foo(&mtx); + output_pose = value; + raw_6dof = raw; + + logger.reset_dt(); + logger.next_line(); +} + +void Tracker::run() +{ +#if defined(_WIN32) + (void) timeBeginPeriod(1); +#endif + + setPriority(QThread::HighPriority); + + { + static constexpr const char* posechannels[6] = { "TX", "TY", "TZ", "Yaw", "Pitch", "Roll" }; + static constexpr const char* datachannels[5] = { "dt", "raw", "corrected", "filtered", "mapped" }; + logger.write(datachannels[0]); + char buffer[128]; + for (unsigned j = 1; j < 5; ++j) + { + for (unsigned i = 0; i < 6; ++i) + { + std::sprintf(buffer, "%s%s", datachannels[j], posechannels[i]); + logger.write(buffer); + } + } + logger.next_line(); + } + + t.start(); + logger.reset_dt(); + + while (!should_quit) + { + double tmp[6] {0,0,0, 0,0,0}; + libs.pTracker->data(tmp); + + if (enabledp) + for (int i = 0; i < 6; i++) + newpose[i] = elide_nan(tmp[i], newpose[i]); + + logic(); + + static constexpr long const_sleep_us = 4000; + + using std::max; + using std::min; + + const long elapsed_usecs = t.elapsed_usecs(); + const long sleep_us = const_sleep_us * 2 - elapsed_usecs; + + const unsigned sleep_time = unsigned(max(1l, min(const_sleep_us * 4, max(1l, (sleep_us + 200l)/1000l)))); + + t.start(); + + portable::sleep(unsigned(max(1u, sleep_time))); + } + + { + // filter may inhibit exact origin + Pose p; + libs.pProtocol->pose(p); + } + +#if defined(_WIN32) + (void) timeEndPeriod(1); +#endif + + for (int i = 0; i < 6; i++) + { + m(i).spline_main.setTrackingActive(false); + m(i).spline_alt.setTrackingActive(false); + } +} + +void Tracker::get_raw_and_mapped_poses(double* mapped, double* raw) const +{ + QMutexLocker foo(&const_cast<Tracker&>(*this).mtx); + + for (int i = 0; i < 6; i++) + { + raw[i] = raw_6dof(i); + mapped[i] = output_pose(i); + } +} + |