diff options
Diffstat (limited to 'opentrack/tracker.cpp')
| -rw-r--r-- | opentrack/tracker.cpp | 216 |
1 files changed, 133 insertions, 83 deletions
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp index 8f1854e9..f70dd819 100644 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -12,9 +12,8 @@ * originally written by Wim Vriend. */ -#include <opencv2/core/core.hpp> -#include "./tracker.h" +#include "tracker.h" #include <cmath> #include <algorithm> @@ -25,10 +24,13 @@ Tracker::Tracker(main_settings& s, Mappings &m, SelectedLibraries &libs) : s(s), m(m), - centerp(false), + centerp(s.center_at_startup), enabledp(true), + zero_(false), should_quit(false), - libs(libs) + libs(libs), + r_b(dmat<3,3>::eye()), + t_b {0,0,0} { } @@ -38,112 +40,145 @@ Tracker::~Tracker() wait(); } -double Tracker::map(double pos, Mapping& axis) { +double Tracker::map(double pos, Mapping& axis) +{ bool altp = (pos < 0) && axis.opts.altp; axis.curve.setTrackingActive( !altp ); axis.curveAlt.setTrackingActive( altp ); auto& fc = altp ? axis.curveAlt : axis.curve; - double invert = axis.opts.invert ? -1 : 1; - return invert * (fc.getValue(pos) + axis.opts.zero); -} - -static cv::Matx33d euler_to_rmat(const double* input) -{ - static constexpr double pi = 3.141592653; - const auto H = input[0] * pi / -180; - const auto P = input[1] * pi / -180; - const auto B = input[2] * pi / 180; - - const auto cosH = cos(H); - const auto sinH = sin(H); - const auto cosP = cos(P); - const auto sinP = sin(P); - const auto cosB = cos(B); - const auto sinB = sin(B); - - double foo[] = { - cosH * cosB - sinH * sinP * sinB, - - sinB * cosP, - sinH * cosB + cosH * sinP * sinB, - cosH * sinB + sinH * sinP * cosB, - cosB * cosP, - sinB * sinH - cosH * sinP * cosB, - - sinH * cosP, - - sinP, - cosH * cosP, - }; - - return cv::Matx33d(foo); + return fc.getValue(pos) + axis.opts.zero; } -void Tracker::t_compensate(const double* input, double* output, bool rz) +void Tracker::t_compensate(const rmat& rmat, const double* xyz, double* output, bool rz) { - const cv::Matx33d rmat = euler_to_rmat(&input[Yaw]); - const cv::Vec3d tvec(input); - const cv::Vec3d ret = rmat * tvec; - - const int max = !rz ? 3 : 2; - - for (int i = 0; i < max; i++) - output[i] = ret(i); + // TY is really yaw axis. need swapping accordingly. + dmat<3, 1> tvec({ xyz[2], -xyz[0], -xyz[1] }); + const dmat<3, 1> ret = rmat * tvec; + if (!rz) + output[2] = ret(0, 0); + else + output[2] = xyz[2]; + output[1] = -ret(2, 0); + output[0] = -ret(1, 0); } void Tracker::logic() { - libs.pTracker->data(newpose); + bool inverts[6] = { + m(0).opts.invert, + m(1).opts.invert, + m(2).opts.invert, + m(3).opts.invert, + m(4).opts.invert, + m(5).opts.invert, + }; - Pose final_raw_; - - if (enabledp) - { + static constexpr double pi = 3.141592653; + static constexpr double r2d = 180. / pi; + + Pose value, raw; + + if (!zero_) for (int i = 0; i < 6; i++) { - auto& axis = m(i); - int k = axis.opts.src; - if (k < 0 || k >= 6) - { - final_raw_(i) = 0; - continue; - } - // not really raw, after axis remap -sh - final_raw_(i) = newpose[k]; + value(i) = newpose[i]; + raw(i) = newpose[i]; + } + else + { + auto mat = rmat::rmat_to_euler(r_b); + + for (int i = 0; i < 3; i++) + { + raw(i+3) = value(i+3) = mat(i, 0) * r2d; + raw(i) = value(i) = t_b[i]; } - final_raw = final_raw_; } + + const double off[] = { + s.camera_yaw, + s.camera_pitch, + 0. + }; + const rmat cam = rmat::euler_to_rmat(off); + rmat r = rmat::euler_to_rmat(&value[Yaw]); + dmat<3, 1> t { value(0), value(1), value(2) }; - Pose filtered_pose; + r = cam * r; + t = cam * t; - if (libs.pFilter) - libs.pFilter->filter(final_raw, filtered_pose); - else - filtered_pose = final_raw; + bool can_center = false; if (centerp) { + for (int i = 0; i < 6; i++) + if (fabs(newpose[i]) != 0) + { + can_center = true; + break; + } + } + + if (can_center) + { centerp = false; - raw_center = final_raw; + for (int i = 0; i < 3; i++) + t_b[i] = t(i, 0); + r_b = r; } - Pose raw_centered = filtered_pose & raw_center; + { + double tmp[3] = { t(0, 0) - t_b[0], t(1, 0) - t_b[1], t(2, 0) - t_b[2] }; + t_compensate(cam, tmp, tmp, false); + const rmat m_ = r * r_b.t(); + const dmat<3, 1> euler = rmat::rmat_to_euler(m_); + for (int i = 0; i < 3; i++) + { + value(i) = tmp[i]; + value(i+3) = euler(i, 0) * r2d; + } + } - Pose mapped_pose_precomp; + for (int i = 3; i < 6; i++) + value(i) = map(value(i), m(i)); - for (int i = 0; i < 6; i++) - mapped_pose_precomp(i) = map(raw_centered(i), m(i)); + { + Pose tmp = value; + + if (libs.pFilter) + libs.pFilter->filter(tmp, value); + } + + if (s.tcomp_p) + t_compensate(rmat::euler_to_rmat(&value[Yaw]), + value, + value, + s.tcomp_tz); - Pose mapped_pose; + for (int i = 0; i < 3; i++) + value(i) = map(value(i), m(i)); - mapped_pose = mapped_pose_precomp; - if (s.tcomp_p) - t_compensate(mapped_pose_precomp, mapped_pose, s.tcomp_tz); + for (int i = 0; i < 6; i++) + value[i] *= inverts[i] ? -1. : 1.; - libs.pProtocol->pose(mapped_pose); + Pose output_pose_; + for (int i = 0; i < 6; i++) { - QMutexLocker foo(&mtx); - output_pose = mapped_pose; - raw_6dof = final_raw; + auto& axis = m(i); + int k = axis.opts.src; + if (k < 0 || k >= 6) + output_pose_(i) = 0; + else + output_pose_(i) = value(k); } + + + libs.pProtocol->pose(output_pose_); + + QMutexLocker foo(&mtx); + output_pose = output_pose_; + raw_6dof = raw; } void Tracker::run() { @@ -156,13 +191,28 @@ void Tracker::run() { while (!should_quit) { t.start(); - + + double tmp[6] {0,0,0, 0,0,0}; + libs.pTracker->data(tmp); + + if (enabledp) + for (int i = 0; i < 6; i++) + newpose[i] = tmp[i]; + logic(); - double q = sleep_ms * 1000L; - q -= t.elapsed(); - q = std::max(0., q); - usleep((long)q); + long q = sleep_ms * 1000L - t.elapsed()/1000L; + usleep(std::max(1L, q)); + } + + { + // do one last pass with origin pose + for (int i = 0; i < 6; i++) + newpose[i] = 0; + logic(); + // filter may inhibit exact origin + Pose p; + libs.pProtocol->pose(p); } #if defined(_WIN32) |