diff options
Diffstat (limited to 'tracker-tobii-eyex/tobii-eyex.cpp')
| -rw-r--r-- | tracker-tobii-eyex/tobii-eyex.cpp | 445 |
1 files changed, 445 insertions, 0 deletions
diff --git a/tracker-tobii-eyex/tobii-eyex.cpp b/tracker-tobii-eyex/tobii-eyex.cpp new file mode 100644 index 00000000..d668a4e2 --- /dev/null +++ b/tracker-tobii-eyex/tobii-eyex.cpp @@ -0,0 +1,445 @@ +#include "tobii-eyex.hpp" +#include <cstdlib> +#include <cstdio> +#include <cmath> +#include <functional> +#include <vector> +#include <algorithm> +#include <iterator> +#include <QDebug> +#include <QMutexLocker> +#include <QMessageBox> + +// XXX TODO whole opentrack needs different debug levels -sh 20160801 + +//#define TOBII_EYEX_DEBUG_PRINTF +#define TOBII_EYEX_VERBOSE_PRINTF + +#ifdef TOBII_EYEX_VERBOSE_PRINTF +# define dbg_verbose(msg) (qDebug() << "tobii-eyex:" << (msg)) +#else +# define dbg_verbose(msg) (QMessageLogger().noDebug() << (msg)) +#endif + +#ifdef TOBII_EYEX_DEBUG_PRINTF +# define dbg_debug(msg) (qDebug() << "tobii-eyex:" << (msg)) +#else +# define dbg_debug(msg) (QMessageLogger().noDebug() << (msg)) +#endif + +#define dbg_notice(msg) (qDebug() << "tobii-eyex:" << (msg)) + +std::atomic_flag tobii_eyex_tracker::atexit_done = ATOMIC_FLAG_INIT; + +static inline tobii_eyex_tracker& to_self(TX_USERPARAM param) +{ + return *reinterpret_cast<tobii_eyex_tracker*>(param); +} + +template<typename t> +static constexpr t clamp(t datum, t min, t max) +{ + return ((datum > max) ? max : ((datum < min) ? min : datum)); +} + +rel_settings::rel_settings() : + opts("tobii-eyex-relative-mode"), + speed(b, "speed", s(5, .1, 10)), + dz_end_pt(b, "deadzone-length", s(.05, 0, .2)), + expt_val(b, "exponent", s(1.75, 1.25, 2.25)), + log_base(b, "logarithm-base", s(1.75, 1.1, 5)) +{} + +tobii_eyex_tracker::tobii_eyex_tracker() : + dev_ctx(TX_EMPTY_HANDLE), + conn_state_changed_ticket(TX_INVALID_TICKET), + event_handler_ticket(TX_INVALID_TICKET), + state_snapshot(TX_EMPTY_HANDLE), + display_state(TX_EMPTY_HANDLE), + yaw(0), + pitch(0), + do_center(false) +{ +} + +void tobii_eyex_tracker::call_tx_deinit() +{ + dbg_notice("uninitialize in atexit at _fini time"); + (void) txUninitializeEyeX(); +} + +tobii_eyex_tracker::~tobii_eyex_tracker() +{ + dbg_verbose("dtor"); + + (void) txDisableConnection(dev_ctx); + (void) txReleaseObject(&state_snapshot); + + bool status = true; + status &= txShutdownContext(dev_ctx, TX_CLEANUPTIMEOUT_FORCEIMMEDIATE, TX_FALSE) == TX_RESULT_OK; + status &= txReleaseContext(&dev_ctx) == TX_RESULT_OK; + + // the API cleanup function needs to be called exactly once over image lifetime. + // client software communicates with a service and a desktop program. + // API is ambiguous as to what happens if the image doesn't call it or crashes. + if (!atexit_done.test_and_set()) + std::atexit(call_tx_deinit); + + if (!status) + dbg_notice("tobii-eyex: can't shutdown properly"); +} + +bool tobii_eyex_tracker::register_state_snapshot(TX_CONTEXTHANDLE dev_ctx, TX_HANDLE* state_snapshot_ptr) +{ + TX_HANDLE handle = TX_EMPTY_HANDLE; + TX_GAZEPOINTDATAPARAMS params = { TX_GAZEPOINTDATAMODE_LIGHTLYFILTERED }; + + bool status = true; + + status &= txCreateGlobalInteractorSnapshot(dev_ctx, client_id, state_snapshot_ptr, &handle) == TX_RESULT_OK; + status &= txCreateGazePointDataBehavior(handle, ¶ms) == TX_RESULT_OK; + + (void) txReleaseObject(&handle); + + return status; +} + +void tobii_eyex_tracker::process_display_state(TX_HANDLE display_state_handle) +{ + TX_SIZE2 screen_res; + + if (txGetStateValueAsSize2(display_state_handle, TX_STATEPATH_EYETRACKINGSCREENBOUNDS, &screen_res) == TX_RESULT_OK) + { + dbg_verbose("got display resolution") << screen_res.Width << screen_res.Height; + + QMutexLocker l(&global_state_mtx); + + dev_state.display_res_x = screen_res.Width; + dev_state.display_res_y = screen_res.Height; + } + else + dbg_notice("can't get display resolution"); +} + +void tobii_eyex_tracker::display_state_handler(TX_CONSTHANDLE async_data_handle, TX_USERPARAM param) +{ + tobii_eyex_tracker& self = to_self(param); + + TX_RESULT result = TX_RESULT_UNKNOWN; + TX_HANDLE state = TX_EMPTY_HANDLE; + + if (txGetAsyncDataResultCode(async_data_handle, &result) == TX_RESULT_OK && + txGetAsyncDataContent(async_data_handle, &state) == TX_RESULT_OK) + { + self.process_display_state(state); + txReleaseObject(&state); + } + else + dbg_notice("error in display state handler"); +} + +void tobii_eyex_tracker::snapshot_committed_handler(TX_CONSTHANDLE async_data_handle, TX_USERPARAM) +{ + TX_RESULT result = TX_RESULT_UNKNOWN; + txGetAsyncDataResultCode(async_data_handle, &result); + + if (!(result == TX_RESULT_OK || result == TX_RESULT_CANCELLED)) + dbg_notice("snapshot bad result code") << result; +} + +void tobii_eyex_tracker::connection_state_change_handler(TX_CONNECTIONSTATE state, TX_USERPARAM param) +{ + tobii_eyex_tracker& self = to_self(param); + + switch (state) + { + case TX_CONNECTIONSTATE_CONNECTED: + { + bool status = txCommitSnapshotAsync(self.state_snapshot, snapshot_committed_handler, param) == TX_RESULT_OK; + if (!status) + dbg_notice("connected but failed to initialize data stream"); + else + { + txGetStateAsync(self.dev_ctx, TX_STATEPATH_EYETRACKINGSCREENBOUNDS, display_state_handler, param); + dbg_notice("connected, data stream ok"); + } + } + break; + case TX_CONNECTIONSTATE_DISCONNECTED: + dbg_notice("connection state is now disconnected"); + break; + case TX_CONNECTIONSTATE_TRYINGTOCONNECT: + dbg_verbose("trying to establish connection"); + break; + case TX_CONNECTIONSTATE_SERVERVERSIONTOOLOW: + dbg_notice("installed driver version too low"); + break; + case TX_CONNECTIONSTATE_SERVERVERSIONTOOHIGH: + dbg_notice("new driver came up, we need to update sdk"); + break; + } +} + +void tobii_eyex_tracker::gaze_data_handler(TX_HANDLE gaze_data_handle) +{ + TX_GAZEPOINTDATAEVENTPARAMS params; + + if (txGetGazePointDataEventParams(gaze_data_handle, ¶ms) == TX_RESULT_OK) + { + { + QMutexLocker l(&global_state_mtx); + + if (params.Timestamp > dev_state.last_timestamp && + dev_state.display_res_x > 0 && + // the API allows for events outside screen bounds to e.g. detect looking at keyboard. + // closer to the screen bounds, the values get less accurate. + // ignore events outside the screen bounds. + params.X >= 0 && params.X < dev_state.display_res_x && + params.Y >= 0 && params.Y < dev_state.display_res_y) + { + dev_state.last_timestamp = params.Timestamp; + dev_state.px = params.X; + dev_state.py = params.Y; + +#ifdef TOBII_EYEX_DEBUG_PRINTF + char buf[256] = {0}; + (void) std::sprintf(buf, "gaze data: (%.1f, %.1f)", params.X, params.Y); + dbg_debug(buf); +#endif + + dev_state.fresh = true; + } + } + } + else + { + dbg_notice("failed to interpret gaze data event packet"); + } +} + +void tobii_eyex_tracker::event_handler(TX_CONSTHANDLE async_data_handle, TX_USERPARAM param) +{ + tobii_eyex_tracker& self = to_self(param); + + TX_HANDLE event_handle = TX_EMPTY_HANDLE; + TX_HANDLE behavior_handle = TX_EMPTY_HANDLE; + + txGetAsyncDataContent(async_data_handle, &event_handle); + + if (txGetEventBehavior(event_handle, &behavior_handle, TX_BEHAVIORTYPE_GAZEPOINTDATA) == TX_RESULT_OK) + { + self.gaze_data_handler(behavior_handle); + txReleaseObject(&behavior_handle); + } + + txReleaseObject(&event_handle); +} + +void tobii_eyex_tracker::start_tracker(QFrame*) +{ + dbg_verbose("start tracker"); + + bool status = true; + + status &= txInitializeEyeX(TX_EYEXCOMPONENTOVERRIDEFLAG_NONE, nullptr, nullptr, nullptr, nullptr) == TX_RESULT_OK; + status &= txCreateContext(&dev_ctx, TX_FALSE) == TX_RESULT_OK; + status &= register_state_snapshot(dev_ctx, &state_snapshot); + status &= txRegisterConnectionStateChangedHandler(dev_ctx, &conn_state_changed_ticket, connection_state_change_handler, reinterpret_cast<TX_USERPARAM>(this)) == TX_RESULT_OK; + status &= txRegisterEventHandler(dev_ctx, &event_handler_ticket, event_handler, reinterpret_cast<TX_USERPARAM>(this)) == TX_RESULT_OK; + status &= txEnableConnection(dev_ctx) == TX_RESULT_OK; + + if (!status) + dbg_verbose("connection can't be established. device missing?"); + else + dbg_verbose("api initialized"); +} + +// the gain function was prototyped in python with jupyter qtconsole. +// you can use qtconsole's inline matplotlib support to see the gain function. +// the `piecewise' function assumes monotonic growth or constant value for all functions. +/* + +from math import * +from itertools import izip +import matplotlib +import matplotlib.pyplot as plt + +try: + import IPython + IPython.get_ipython().magic(u'matplotlib inline') +except: + pass + +def frange(from_, to_, step_=1e-4): + i = from_ + while True: + yield i + i += step_ + if i >= to_: + break + +def plot_fn(fn, from_=0., to_=1., step=None): + if step is None: + step = max(1e-4, (to_-from_)*1e-4) + xs = [i for i in frange(from_, to_, step)] + plt.plot(xs, map(fn, xs)) + +def piecewise(x, funs, bounds): + y = 0. + last_bound = 0. + norm = 0. + for fun in funs: + norm += fun(1.) + for fun, bound in izip(funs, bounds): + if x > bound: + y += fun(1.) + else: + b = bound - last_bound + x_ = (x - last_bound) / b + y += fun(x_) + break + last_bound = bound + return y / norm + +def f(x): return x**1.75 +def g(x): return 1.75*1.75*x +def h(x): return log(1+x)/log(2.5) +def zero(x): return 0. + +plot_fn(lambda x: piecewise(x, [zero, f, g, h], [.05, .25, .7, 1.])) + +*/ + +template<typename funs_seq, typename bounds_seq> +tobii_eyex_tracker::num tobii_eyex_tracker::piecewise(num x, const funs_seq& funs, const bounds_seq& bounds) +{ + using fn = std::function<num(num)>; + + auto funs_it = std::begin(funs); + auto bounds_it = std::begin(bounds); + + auto funs_end = std::end(funs); + auto bounds_end = std::end(bounds); + + num norm = 0; + + for (const fn& f : funs) + { + norm += f(1); + } + + norm = std::max(num(1e-4), norm); + + num last_bound = 0, y = 0; + + for (; + bounds_it != bounds_end && funs_it != funs_end; + bounds_it++, funs_it++) + { + const fn& fun = *funs_it; + const num bound = *bounds_it; + + if (x > bound) + { + y += fun(1); + last_bound = bound; + } + else + { + const num b = bound - last_bound; + // rescale x to 0->1 + const num x_ = (x - last_bound) / b; + y += fun(x_); + break; + } + } + return clamp(y / norm, num(0), num(1)); +} + +tobii_eyex_tracker::num tobii_eyex_tracker::gain(num x_) +{ + const num x = std::fabs(x_); + + static const fun_t funs[] = + { + [](num) -> num { return num(0); }, + [](num x) -> num { return std::pow(x, 1.1)*.08; }, + [](num x) -> num { return x*.5; }, + }; + + static constexpr num dz_l = .1, expt_l = .3; + + static constexpr num ends[] = + { + dz_l, + expt_l, + 1, + }; + + const num ret = piecewise(x, funs, ends); + return std::copysign(clamp(ret, num(0), num(1)), x_); +} + +void tobii_eyex_tracker::data(double* data) +{ + TX_REAL px, py, dw, dh, x_, y_; + bool fresh; + + { + QMutexLocker l(&global_state_mtx); + + if (!dev_state.is_valid()) + return; + + px = dev_state.px; + py = dev_state.py; + dw = dev_state.display_res_x; + dh = dev_state.display_res_y; + + fresh = dev_state.fresh; + dev_state.fresh = false; + } + + x_ = (px-dw/2.) / (dw/2.); + y_ = (py-dh/2.) / (dh/2.); + + data[TX] = x_ * 50; + data[TY] = y_ * -50; + + if (fresh) + { + const double dt = t.elapsed_seconds(); + t.start(); + // XXX TODO make slider + static constexpr double v = 300; + + const double x = gain(x_); + const double y = gain(y_); + + const double yaw_delta = (x * v) * dt; + const double pitch_delta = (y * -v) * dt; + + yaw += yaw_delta; + pitch += pitch_delta; + + yaw = clamp(yaw, -180., 180.); + pitch = clamp(pitch, -60., 60.); + } + + if (do_center) + { + do_center = false; + yaw = 0; + pitch = 0; + } + + data[Yaw] = yaw; + data[Pitch] = pitch; + data[Roll] = 0; + data[TZ] = 0; // XXX TODO + + // tan(x) in 0->.7 is almost linear. we don't need to adjust. + // .7 is 40 degrees which is already quite a lot from the monitor. +} + +OPENTRACK_DECLARE_TRACKER(tobii_eyex_tracker, tobii_eyex_dialog, tobii_eyex_metadata) |