tracker/tobii-eyex: initial revision

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, &params) == 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, &params) == 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)