matrixfurry.com
The open source OpenXR runtime
1// Copyright 2022-2023, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Camera based hand tracking driver code.
6 * @author Moshi Turner <moshiturner@protonmail.com>
7 * @ingroup drv_ht
8 */
9
10#include "os/os_threading.h"
11
12#include "math/m_space.h"
13#include "math/m_relation_history.h"
14
15#include "util/u_var.h"
16#include "util/u_misc.h"
17#include "util/u_debug.h"
18#include "util/u_logging.h"
19#include "util/u_trace_marker.h"
20
21#include "tracking/t_hand_tracking.h"
22
23
24DEBUG_GET_ONCE_BOOL_OPTION(hta_prediction_disable, "HTA_PREDICTION_DISABLE", false)
25DEBUG_GET_ONCE_FLOAT_OPTION(hta_prediction_offset_ms, "HTA_PREDICTION_OFFSET_MS", -40.0f)
26
27
28/*!
29 * A synchronous to asynchronous wrapper around the hand-tracker code.
30 *
31 * @ingroup drv_ht
32 */
33struct ht_async_impl
34{
35 struct t_hand_tracking_async base;
36
37 struct t_hand_tracking_sync *provider;
38
39 struct xrt_frame *frames[2];
40
41 bool use_prediction;
42 struct u_var_draggable_f32 prediction_offset_ms;
43
44 struct
45 {
46 struct xrt_hand_joint_set hands[2];
47 int64_t timestamp;
48 } working;
49
50 struct
51 {
52 struct os_mutex mutex;
53 struct xrt_hand_joint_set hands[2];
54 struct m_relation_history *relation_hist[2];
55 int64_t timestamp;
56 } present;
57
58 // in here:
59 // mutex is so that the mainloop and two push_frames don't fight over referencing frames;
60 // cond is so that we can wake up the mainloop at certain times;
61 // running is so we can stop the thread when Monado exits
62 struct os_thread_helper mainloop;
63
64 volatile bool hand_tracking_work_active;
65};
66
67
68/*
69 *
70 * Misc functions.
71 *
72 */
73
74static inline struct ht_async_impl *
75ht_async_impl(struct t_hand_tracking_async *base)
76{
77 return (struct ht_async_impl *)base;
78}
79
80static void *
81ht_async_mainloop(void *ptr)
82{
83 U_TRACE_SET_THREAD_NAME("Hand Tracking: Async");
84
85 struct ht_async_impl *hta = (struct ht_async_impl *)ptr;
86
87 os_thread_helper_lock(&hta->mainloop);
88
89 while (os_thread_helper_is_running_locked(&hta->mainloop)) {
90
91 // No new frame, wait.
92 if (hta->frames[0] == NULL && hta->frames[1] == NULL) {
93 os_thread_helper_wait_locked(&hta->mainloop);
94
95 /*
96 * Loop back to the top to check if we should stop,
97 * also handles spurious wakeups by re-checking the
98 * condition in the if case. Essentially two loops.
99 */
100 continue;
101 }
102
103 os_thread_helper_unlock(&hta->mainloop);
104
105
106 /*
107 * Do the hand-tracking now.
108 */
109
110 t_ht_sync_process( //
111 hta->provider, //
112 hta->frames[0], //
113 hta->frames[1], //
114 &hta->working.hands[0], //
115 &hta->working.hands[1], //
116 &hta->working.timestamp); //
117
118 xrt_frame_reference(&hta->frames[0], NULL);
119 xrt_frame_reference(&hta->frames[1], NULL);
120
121
122 /*
123 * Post process.
124 */
125
126 os_mutex_lock(&hta->present.mutex);
127
128 hta->present.timestamp = hta->working.timestamp;
129
130 for (int i = 0; i < 2; i++) {
131 hta->present.hands[i] = hta->working.hands[i];
132 }
133
134 os_mutex_unlock(&hta->present.mutex);
135
136 for (int i = 0; i < 2; i++) {
137 struct xrt_space_relation wrist_rel =
138 hta->working.hands[i].values.hand_joint_set_default[XRT_HAND_JOINT_WRIST].relation;
139
140 m_relation_history_push_with_motion_estimation( //
141 hta->present.relation_hist[i], //
142 &wrist_rel, //
143 hta->working.timestamp); //
144 }
145
146 hta->hand_tracking_work_active = false;
147
148 // Have to lock it again.
149 os_thread_helper_lock(&hta->mainloop);
150 }
151
152 os_thread_helper_unlock(&hta->mainloop);
153
154 return NULL;
155}
156
157
158/*
159 *
160 * Sink receive functions.
161 *
162 */
163
164static void
165ht_async_receive_left(struct xrt_frame_sink *sink, struct xrt_frame *frame)
166{
167 struct ht_async_impl *hta = ht_async_impl(container_of(sink, struct t_hand_tracking_async, left));
168
169 // See comment in ht_async_receive_right.
170 if (hta->hand_tracking_work_active) {
171 // Throw away this frame
172 return;
173 }
174
175 // Ensure a strict left then right order of frames.
176 assert(hta->frames[0] == NULL);
177
178 // Keep onto this frame.
179 xrt_frame_reference(&hta->frames[0], frame);
180}
181
182static void
183ht_async_receive_right(struct xrt_frame_sink *sink, struct xrt_frame *frame)
184{
185 struct ht_async_impl *hta = ht_async_impl(container_of(sink, struct t_hand_tracking_async, right));
186
187 /*
188 * Throw away this frame - either the hand tracking work is running now,
189 * or it was a very short time ago, and ht_async_receive_left threw away
190 * its frame or there's some other bug where left isn't pushed before
191 * right.
192 */
193 if (hta->hand_tracking_work_active || hta->frames[0] == NULL) {
194 return;
195 }
196
197 // Just to check the above.
198 assert(hta->frames[0] != NULL);
199 assert(hta->frames[1] == NULL);
200
201 // Keep onto this frame.
202 xrt_frame_reference(&hta->frames[1], frame);
203
204 // We have both frames, now work is active.
205 hta->hand_tracking_work_active = true;
206
207 // Wake up the worker thread.
208 os_thread_helper_lock(&hta->mainloop);
209 os_thread_helper_signal_locked(&hta->mainloop);
210 os_thread_helper_unlock(&hta->mainloop);
211}
212
213
214/*
215 *
216 * Sink node functions.
217 *
218 */
219
220static void
221ht_async_break_apart(struct xrt_frame_node *node)
222{
223 struct ht_async_impl *hta = ht_async_impl(container_of(node, struct t_hand_tracking_async, node));
224
225 // Stop the thread, unsure nothing else is pushed into the tracker.
226 os_thread_helper_stop_and_wait(&hta->mainloop);
227}
228
229static void
230ht_async_destroy(struct xrt_frame_node *node)
231{
232 struct ht_async_impl *hta = ht_async_impl(container_of(node, struct t_hand_tracking_async, node));
233
234 os_thread_helper_destroy(&hta->mainloop);
235 os_mutex_destroy(&hta->present.mutex);
236
237 t_ht_sync_destroy(&hta->provider);
238
239 for (int i = 0; i < 2; i++) {
240 m_relation_history_destroy(&hta->present.relation_hist[i]);
241 }
242
243 free(hta);
244}
245
246
247/*
248 *
249 * Member functions.
250 *
251 */
252
253static void
254ht_async_get_hand(struct t_hand_tracking_async *ht_async,
255 enum xrt_input_name name,
256 int64_t desired_timestamp_ns,
257 struct xrt_hand_joint_set *out_value,
258 int64_t *out_timestamp_ns)
259{
260 struct ht_async_impl *hta = ht_async_impl(ht_async);
261 assert(name == XRT_INPUT_HT_UNOBSTRUCTED_LEFT || name == XRT_INPUT_HT_UNOBSTRUCTED_RIGHT);
262
263 int idx = 0;
264 if (name == XRT_INPUT_HT_UNOBSTRUCTED_RIGHT) {
265 idx = 1;
266 }
267
268 os_mutex_lock(&hta->present.mutex);
269
270 struct xrt_hand_joint_set latest_hand = hta->present.hands[idx];
271
272 if (!hta->use_prediction) {
273 *out_value = latest_hand;
274 *out_timestamp_ns = hta->present.timestamp;
275 os_mutex_unlock(&hta->present.mutex);
276 return;
277 }
278
279 os_mutex_unlock(&hta->present.mutex);
280
281 double prediction_offset_ns = (double)hta->prediction_offset_ms.val * (double)U_TIME_1MS_IN_NS;
282
283 desired_timestamp_ns += (uint64_t)prediction_offset_ns;
284
285 struct xrt_space_relation predicted_wrist;
286 m_relation_history_get(hta->present.relation_hist[idx], desired_timestamp_ns, &predicted_wrist);
287
288 struct xrt_space_relation latest_wrist =
289 latest_hand.values.hand_joint_set_default[XRT_HAND_JOINT_WRIST].relation;
290
291 *out_value = latest_hand;
292
293 // apply the pose change from the latest wrist to the predicted wrist
294 // to all the joints on the hand.
295
296 //! @todo We could slightly reduce the total number of transforms by putting some of this in ht_async_mainloop
297 for (int i = 0; i < XRT_HAND_JOINT_COUNT; i++) {
298 struct xrt_relation_chain xrc = {0};
299 m_relation_chain_push_relation(&xrc, &latest_hand.values.hand_joint_set_default[i].relation);
300 m_relation_chain_push_inverted_relation(&xrc, &latest_wrist);
301 m_relation_chain_push_relation(&xrc, &predicted_wrist);
302 m_relation_chain_resolve(&xrc, &out_value->values.hand_joint_set_default[i].relation);
303 }
304
305 *out_timestamp_ns = desired_timestamp_ns;
306}
307
308
309/*
310 *
311 * 'Exported' functions.
312 *
313 */
314
315struct t_hand_tracking_async *
316t_hand_tracking_async_default_create(struct xrt_frame_context *xfctx, struct t_hand_tracking_sync *sync)
317{
318 struct ht_async_impl *hta = U_TYPED_CALLOC(struct ht_async_impl);
319 hta->base.left.push_frame = ht_async_receive_left;
320 hta->base.right.push_frame = ht_async_receive_right;
321 hta->base.sinks.cam_count = 2;
322 hta->base.sinks.cams[0] = &hta->base.left;
323 hta->base.sinks.cams[1] = &hta->base.right;
324 hta->base.node.break_apart = ht_async_break_apart;
325 hta->base.node.destroy = ht_async_destroy;
326 hta->base.get_hand = ht_async_get_hand;
327 hta->provider = sync;
328
329 for (int i = 0; i < 2; i++) {
330 m_relation_history_create(&hta->present.relation_hist[i]);
331 }
332
333 /*!
334 * @todo We came up with this value just by seeing what worked. With
335 * Index and WMR, we'd be around 40ms late by the time the camera frames
336 * arrived and were processed.
337 *
338 * We _really_ need a way to calibrate this live - something like an
339 * exponential filter that looks at the typical maximum time between the
340 * time at which we were asked for a sample and most recent processed
341 * sample timestamp.
342 */
343 float prediction_offset_ms = debug_get_float_option_hta_prediction_offset_ms();
344
345 hta->use_prediction = !debug_get_bool_option_hta_prediction_disable();
346 hta->prediction_offset_ms = (struct u_var_draggable_f32){
347 .val = prediction_offset_ms,
348 .step = 0.5,
349 // No need to enforce limits, although generally around -40 is what you want.
350 .min = -1000000,
351 .max = 1000000,
352 };
353
354 // In reality never fails.
355 os_mutex_init(&hta->present.mutex);
356 os_thread_helper_init(&hta->mainloop);
357 os_thread_helper_start(&hta->mainloop, ht_async_mainloop, hta);
358
359 // Everything setup, add to frame context.
360 xrt_frame_context_add(xfctx, &hta->base.node);
361
362 // Now that everything initialised add to u_var.
363 u_var_add_root(hta, "Hand-tracking async shim!", 0);
364 u_var_add_bool(hta, &hta->use_prediction, "Predict wrist movement");
365 u_var_add_draggable_f32(hta, &hta->prediction_offset_ms, "Amount to time-travel (ms)");
366
367 return &hta->base;
368}