The open source OpenXR runtime
d/ht: Change everything
authored by
Moses Turner
and committed by
Moses Turner
624a676f
80689129
+3215
-267
+18
-12
src/xrt/drivers/CMakeLists.txt
+18
-12
src/xrt/drivers/CMakeLists.txt
···
5
5
set(ENABLED_HEADSET_DRIVERS)
6
6
set(ENABLED_DRIVERS)
7
7
8
-
9
8
if(XRT_BUILD_DRIVER_ARDUINO)
10
9
set(ARDUINO_SOURCE_FILES
11
10
arduino/arduino_device.c
···
149
148
list(APPEND ENABLED_HEADSET_DRIVERS openhmd)
150
149
endif()
151
150
152
-
if(XRT_BUILD_DRIVER_HANDTRACKING)
153
-
set(HT_SOURCE_FILES
154
-
ht/ht_driver.c
155
-
ht/ht_driver.h
156
-
ht/ht_interface.h
157
-
ht/ht_prober.c
158
-
)
159
151
160
-
add_library(drv_ht STATIC ${HT_SOURCE_FILES})
161
-
target_link_libraries(drv_ht PRIVATE xrt-interfaces aux_util aux_math)
162
-
list(APPEND ENABLED_DRIVERS ht)
163
-
endif()
164
152
165
153
if(XRT_BUILD_DRIVER_PSMV)
166
154
set(PSMOVE_SOURCE_FILES
···
258
246
target_link_libraries(drv_vf PRIVATE xrt-interfaces aux_os aux_util ${GST_LIBRARIES})
259
247
target_include_directories(drv_vf PRIVATE ${GST_INCLUDE_DIRS})
260
248
list(APPEND ENABLED_DRIVERS vf)
249
+
endif()
250
+
251
+
if(XRT_BUILD_DRIVER_HANDTRACKING)
252
+
set(HT_SOURCE_FILES
253
+
ht/ht_driver.cpp
254
+
ht/ht_driver.hpp
255
+
ht/ht_interface.h
256
+
ht/ht_models.hpp
257
+
ht/ht_hand_math.hpp
258
+
ht/ht_image_math.hpp
259
+
ht/ht_nms.hpp
260
+
ht/templates/DiscardLastBuffer.hpp
261
+
ht/templates/NaivePermutationSort.hpp
262
+
)
263
+
add_library(drv_ht STATIC ${HT_SOURCE_FILES})
264
+
target_link_libraries(drv_ht PRIVATE xrt-interfaces aux_os aux_util aux_math ONNXRuntime::ONNXRuntime ${OpenCV_LIBRARIES})
265
+
target_include_directories(drv_ht PRIVATE ${OpenCV_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
266
+
list(APPEND ENABLED_DRIVERS ht)
261
267
endif()
262
268
263
269
if (XRT_BUILD_DRIVER_SURVIVE)
+638
src/xrt/drivers/ht/ht_algorithm.hpp
+638
src/xrt/drivers/ht/ht_algorithm.hpp
···
1
+
// Copyright 2021, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Camera based hand tracking mainloop algorithm.
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @ingroup drv_ht
8
+
*/
9
+
10
+
#pragma once
11
+
12
+
#include "ht_driver.hpp"
13
+
14
+
#include "util/u_frame.h"
15
+
16
+
#include "ht_image_math.hpp"
17
+
#include "ht_models.hpp"
18
+
#include "ht_hand_math.hpp"
19
+
#include "templates/NaivePermutationSort.hpp"
20
+
#include <opencv2/imgproc.hpp>
21
+
22
+
23
+
// Flags to tell state tracker that these are indeed valid joints
24
+
static enum xrt_space_relation_flags valid_flags_ht = (enum xrt_space_relation_flags)(
25
+
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
26
+
XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
27
+
28
+
29
+
static void
30
+
htProcessJoint(struct ht_device *htd,
31
+
struct xrt_vec3 model_out,
32
+
struct xrt_hand_joint_set *hand,
33
+
enum xrt_hand_joint idx)
34
+
{
35
+
hand->values.hand_joint_set_default[idx].relation.relation_flags = valid_flags_ht;
36
+
hand->values.hand_joint_set_default[idx].relation.pose.position.x = model_out.x;
37
+
hand->values.hand_joint_set_default[idx].relation.pose.position.y = model_out.y;
38
+
hand->values.hand_joint_set_default[idx].relation.pose.position.z = model_out.z;
39
+
}
40
+
41
+
static float
42
+
errHistory2D(HandHistory2DBBox *past, Palm7KP *present)
43
+
{
44
+
return (m_vec2_len(*past->wrist_unfiltered[0] - present->kps[WRIST_7KP]) +
45
+
m_vec2_len(*past->middle_unfiltered[0] - present->kps[MIDDLE_7KP]));
46
+
}
47
+
48
+
static std::vector<Hand2D>
49
+
htImageToKeypoints(struct ht_view *htv)
50
+
{
51
+
int view = htv->view;
52
+
struct ht_device *htd = htv->htd;
53
+
54
+
55
+
cv::Mat raw_input = htv->run_model_on_this;
56
+
57
+
// Get a list of palms - drop confidences and ssd bounding boxes, just keypoints.
58
+
59
+
std::vector<Palm7KP> hand_detections = htv->run_detection_model(htv, raw_input);
60
+
61
+
std::vector<bool> used_histories;
62
+
std::vector<bool> used_detections;
63
+
64
+
std::vector<size_t> history_indices;
65
+
std::vector<size_t> detection_indices;
66
+
std::vector<float> dontuse;
67
+
68
+
69
+
// Strategy here is: We have a big list of palms. Match 'em up to previous palms.
70
+
naive_sort_permutation_by_error<HandHistory2DBBox, Palm7KP>(htv->bbox_histories, hand_detections,
71
+
72
+
// bools
73
+
used_histories, used_detections,
74
+
75
+
history_indices, detection_indices, dontuse,
76
+
errHistory2D);
77
+
78
+
// Here's the trick - we use the associated bbox_filter to get an output but *never commit* the noisy 128x128
79
+
// detection; instead later on we commit the (hopefully) nicer palm and wrist from the 224x224 keypoint
80
+
// estimation.
81
+
82
+
// Add extra detections!
83
+
for (size_t i = 0; i < used_detections.size(); i++) {
84
+
if (used_detections[i] == false) {
85
+
HandHistory2DBBox hist_new = {};
86
+
m_filter_euro_vec2_init(&hist_new.m_filter_middle, FCMIN_BBOX, FCMIN_D_BB0X, BETA_BB0X);
87
+
m_filter_euro_vec2_init(&hist_new.m_filter_wrist, FCMIN_BBOX, FCMIN_D_BB0X, BETA_BB0X);
88
+
89
+
// this leaks, on august 24
90
+
htv->bbox_histories.push_back(hist_new);
91
+
history_indices.push_back(htv->bbox_histories.size() - 1);
92
+
detection_indices.push_back(i);
93
+
}
94
+
}
95
+
96
+
// Do the things for each active bbox history!
97
+
for (size_t i = 0; i < history_indices.size(); i++) {
98
+
HandHistory2DBBox *hist_of_interest = &htv->bbox_histories[history_indices[i]];
99
+
hist_of_interest->wrist_unfiltered.push(hand_detections[detection_indices[i]].kps[WRIST_7KP]);
100
+
hist_of_interest->middle_unfiltered.push(hand_detections[detection_indices[i]].kps[MIDDLE_7KP]);
101
+
// Eh do the rest later
102
+
}
103
+
104
+
// Prune stale detections! (After we don't need {history,detection}_indices to be correct)
105
+
int bob = 0;
106
+
for (size_t i = 0; i < used_histories.size(); i++) {
107
+
if (used_histories[i] == false) {
108
+
// history never got assigned a present hand to it. treat it as stale delete it.
109
+
110
+
HT_TRACE(htv->htd, "Removing bbox from history!\n");
111
+
htv->bbox_histories.erase(htv->bbox_histories.begin() + i + bob);
112
+
bob--;
113
+
}
114
+
}
115
+
if (htv->bbox_histories.size() == 0) {
116
+
return {}; // bail early
117
+
}
118
+
119
+
120
+
121
+
std::vector<Hand2D> list_of_hands_in_bbox(
122
+
htv->bbox_histories.size()); // all of these are same size as htv->bbox_histories
123
+
124
+
std::vector<std::future<Hand2D>> await_list_of_hand_in_bbox; //(htv->bbox_histories.size());
125
+
126
+
std::vector<DetectionModelOutput> blah(htv->bbox_histories.size());
127
+
128
+
std::vector<Hand2D> output;
129
+
130
+
if (htv->bbox_histories.size() > 2) {
131
+
HT_DEBUG(htd, "More than two hands (%zu) in 2D view %i", htv->bbox_histories.size(), htv->view);
132
+
}
133
+
134
+
135
+
for (size_t i = 0; i < htv->bbox_histories.size(); i++) { //(BBoxHistory * entry : htv->bbox_histories) {
136
+
HandHistory2DBBox *entry = &htv->bbox_histories[i];
137
+
cv::Mat hand_rect = cv::Mat(224, 224, CV_8UC3);
138
+
xrt_vec2 goodenough_middle;
139
+
xrt_vec2 goodenough_wrist;
140
+
141
+
m_filter_euro_vec2_run_no_commit(&entry->m_filter_middle, htv->htd->current_frame_timestamp,
142
+
entry->middle_unfiltered[0], &goodenough_middle);
143
+
m_filter_euro_vec2_run_no_commit(&entry->m_filter_wrist, htv->htd->current_frame_timestamp,
144
+
entry->wrist_unfiltered[0], &goodenough_wrist);
145
+
146
+
rotatedRectFromJoints(htv, goodenough_middle, goodenough_wrist, &blah[i]);
147
+
148
+
149
+
150
+
warpAffine(raw_input, hand_rect, blah[i].warp_there, hand_rect.size());
151
+
152
+
await_list_of_hand_in_bbox.push_back(
153
+
std::async(std::launch::async, htd->views[view].run_keypoint_model, &htd->views[view], hand_rect));
154
+
}
155
+
156
+
// cut here
157
+
158
+
for (size_t i = 0; i < htv->bbox_histories.size(); i++) {
159
+
160
+
Hand2D in_bbox = await_list_of_hand_in_bbox[i].get();
161
+
162
+
cv::Matx23f warp_back = blah[i].warp_back;
163
+
164
+
Hand2D in_image_ray_coords;
165
+
Hand2D in_image_px_coords;
166
+
167
+
for (int i = 0; i < 21; i++) {
168
+
struct xrt_vec3 vec = in_bbox.kps[i];
169
+
170
+
#if 1
171
+
xrt_vec3 rr = transformVecBy2x3(vec, warp_back);
172
+
rr.z = vec.z;
173
+
#else
174
+
xrt_vec3 rr;
175
+
rr.x = (vec.x * warp_back(0, 0)) + (vec.y * warp_back(0, 1)) + warp_back(0, 2);
176
+
rr.y = (vec.x * warp_back(1, 0)) + (vec.y * warp_back(1, 1)) + warp_back(1, 2);
177
+
rr.z = vec.z;
178
+
#endif
179
+
in_image_px_coords.kps[i] = rr;
180
+
181
+
in_image_ray_coords.kps[i] = raycoord(htv, rr);
182
+
if (htd->debug_scribble) {
183
+
handDot(htv->debug_out_to_this, {rr.x, rr.y}, fmax((-vec.z + 100 - 20) * .08, 2),
184
+
((float)i) / 21.0f, cv::FILLED);
185
+
}
186
+
}
187
+
xrt_vec2 middle_in_px_coords = {in_image_px_coords.kps[MIDL_PXM].x, in_image_px_coords.kps[MIDL_PXM].y};
188
+
xrt_vec2 wrist_in_px_coords = {in_image_px_coords.kps[WRIST].x, in_image_px_coords.kps[WRIST].y};
189
+
xrt_vec2 dontuse;
190
+
m_filter_euro_vec2_run(&htv->bbox_histories[i].m_filter_wrist, htv->htd->current_frame_timestamp,
191
+
&wrist_in_px_coords, &dontuse);
192
+
193
+
m_filter_euro_vec2_run(&htv->bbox_histories[i].m_filter_middle, htv->htd->current_frame_timestamp,
194
+
&middle_in_px_coords, &dontuse);
195
+
output.push_back(in_image_ray_coords);
196
+
}
197
+
return output;
198
+
}
199
+
200
+
#if defined(JSON_OUTPUT)
201
+
202
+
static void
203
+
jsonAddJoint(cJSON *into_this, xrt_pose loc, const char *name)
204
+
{
205
+
cJSON *container = cJSON_CreateObject();
206
+
cJSON *joint_loc = cJSON_CreateArray();
207
+
cJSON_AddItemToArray(joint_loc, cJSON_CreateNumber(loc.position.x));
208
+
cJSON_AddItemToArray(joint_loc, cJSON_CreateNumber(loc.position.y));
209
+
cJSON_AddItemToArray(joint_loc, cJSON_CreateNumber(loc.position.z));
210
+
211
+
cJSON_AddItemToObject(container, "position", joint_loc);
212
+
213
+
cJSON *joint_rot = cJSON_CreateArray();
214
+
215
+
216
+
cJSON_AddItemToArray(joint_rot, cJSON_CreateNumber(loc.orientation.x));
217
+
cJSON_AddItemToArray(joint_rot, cJSON_CreateNumber(loc.orientation.y));
218
+
cJSON_AddItemToArray(joint_rot, cJSON_CreateNumber(loc.orientation.z));
219
+
cJSON_AddItemToArray(joint_rot, cJSON_CreateNumber(loc.orientation.w));
220
+
221
+
cJSON_AddItemToObject(container, "rotation_quat_xyzw", joint_rot);
222
+
223
+
cJSON_AddItemToObject(into_this, name, container);
224
+
}
225
+
226
+
227
+
static void
228
+
jsonAddSet(struct ht_device *htd)
229
+
{
230
+
cJSON *two_hand_container = cJSON_CreateObject();
231
+
static const char *keys[] = {
232
+
"wrist", "palm",
233
+
234
+
"thumb_mcp", "thumb_pxm", "thumb_dst", "thumb_tip",
235
+
236
+
"index_mcp", "index_pxm", "index_int", "index_dst", "index_tip",
237
+
238
+
"middle_mcp", "middle_pxm", "middle_int", "middle_dst", "middle_tip",
239
+
240
+
"ring_mcp", "ring_pxm", "ring_int", "ring_dst", "ring_tip",
241
+
242
+
"little_mcp", "little_pxm", "little_int", "little_dst", "little_tip",
243
+
};
244
+
static const char *sides_names[] = {
245
+
"left",
246
+
"right",
247
+
};
248
+
for (int side = 0; side < 2; side++) {
249
+
struct xrt_hand_joint_set *set = &htd->hands_for_openxr[side];
250
+
if (!set->is_active) {
251
+
cJSON_AddNullToObject(two_hand_container, sides_names[side]);
252
+
} else {
253
+
cJSON *hand_obj = cJSON_CreateObject();
254
+
for (int i = 0; i < 26; i++) {
255
+
const char *key = keys[i];
256
+
xrt_pose pose = set->values.hand_joint_set_default[i].relation.pose;
257
+
jsonAddJoint(hand_obj, pose, key);
258
+
}
259
+
cJSON_AddItemToObject(two_hand_container, sides_names[side], hand_obj);
260
+
}
261
+
}
262
+
263
+
#if defined(JSON_OUTPUT)
264
+
cJSON_AddItemToArray(htd->output_array, two_hand_container);
265
+
#endif
266
+
}
267
+
268
+
#endif
269
+
270
+
271
+
static void
272
+
htBailThisFrame(struct ht_device *htd)
273
+
{
274
+
275
+
os_mutex_lock(&htd->openxr_hand_data_mediator);
276
+
htd->hands_for_openxr[0].is_active = false;
277
+
htd->hands_for_openxr[1].is_active = false;
278
+
#if defined(JSON_OUTPUT)
279
+
json_add_set(htd);
280
+
#endif
281
+
os_mutex_unlock(&htd->openxr_hand_data_mediator);
282
+
}
283
+
284
+
int64_t last_frame, this_frame;
285
+
286
+
static void
287
+
htRunAlgorithm(struct ht_device *htd)
288
+
{
289
+
XRT_TRACE_MARKER();
290
+
291
+
htd->current_frame_timestamp = htd->frame_for_process->timestamp;
292
+
293
+
294
+
int64_t start, end;
295
+
start = os_monotonic_get_ns();
296
+
297
+
298
+
299
+
cv::Mat full_frame(960, 960 * 2, CV_8UC3, htd->frame_for_process->data, htd->frame_for_process->stride);
300
+
htd->views[0].run_model_on_this =
301
+
full_frame(cv::Rect(0, 0, htd->camera.one_view_size_px.w, htd->camera.one_view_size_px.h));
302
+
htd->views[1].run_model_on_this = full_frame(cv::Rect(
303
+
htd->camera.one_view_size_px.w, 0, htd->camera.one_view_size_px.w, htd->camera.one_view_size_px.h));
304
+
305
+
// Check this every frame. We really, really, really don't want it to ever suddenly be null.
306
+
htd->debug_scribble = htd->debug_sink != nullptr;
307
+
308
+
cv::Mat debug_output;
309
+
xrt_frame *debug_frame = nullptr; // only use if htd->debug_scribble
310
+
311
+
312
+
if (htd->debug_scribble) {
313
+
u_frame_clone(htd->frame_for_process, &debug_frame);
314
+
debug_output = cv::Mat(960, 960 * 2, CV_8UC3, debug_frame->data, debug_frame->stride);
315
+
htd->views[0].debug_out_to_this =
316
+
debug_output(cv::Rect(0, 0, htd->camera.one_view_size_px.w, htd->camera.one_view_size_px.h));
317
+
htd->views[1].debug_out_to_this = debug_output(cv::Rect(
318
+
htd->camera.one_view_size_px.w, 0, htd->camera.one_view_size_px.w, htd->camera.one_view_size_px.h));
319
+
}
320
+
321
+
std::future<std::vector<Hand2D>> future_left =
322
+
std::async(std::launch::async, htImageToKeypoints, &htd->views[0]);
323
+
std::future<std::vector<Hand2D>> future_right =
324
+
std::async(std::launch::async, htImageToKeypoints, &htd->views[1]);
325
+
std::vector<Hand2D> hands_in_left_view = future_left.get();
326
+
std::vector<Hand2D> hands_in_right_view = future_right.get();
327
+
end = os_monotonic_get_ns();
328
+
329
+
330
+
this_frame = os_monotonic_get_ns();
331
+
332
+
double time_ms = (double)(end - start) / (double)U_TIME_1MS_IN_NS;
333
+
double _1_time = 1 / (time_ms * 0.001);
334
+
335
+
char t[64];
336
+
char t2[64];
337
+
sprintf(t, "% 8.2f ms", time_ms);
338
+
sprintf(t2, "% 8.2f fps", _1_time);
339
+
last_frame = this_frame;
340
+
341
+
342
+
if (htd->debug_scribble) {
343
+
cv::putText(debug_output, t, cv::Point(30, 60), cv::FONT_HERSHEY_SIMPLEX, 1.0f, cv::Scalar(0, 255, 0),
344
+
4);
345
+
cv::putText(debug_output, t2, cv::Point(30, 100), cv::FONT_HERSHEY_SIMPLEX, 1.0f, cv::Scalar(0, 255, 0),
346
+
4);
347
+
} else {
348
+
HT_DEBUG(htd, "%s", t);
349
+
HT_DEBUG(htd, "%s", t2);
350
+
}
351
+
352
+
353
+
// Convenience
354
+
uint64_t timestamp = htd->frame_for_process->timestamp;
355
+
356
+
if (htd->debug_scribble) {
357
+
htd->debug_sink->push_frame(htd->debug_sink, debug_frame);
358
+
xrt_frame_reference(&debug_frame, NULL);
359
+
}
360
+
361
+
// Bail early this frame if no hands were detected.
362
+
// In the long run, this'll be a silly thing - we shouldn't always take the detection model's word for it
363
+
// especially when part of the pipeline is an arbitrary confidence threshold.
364
+
if (hands_in_left_view.size() == 0 || hands_in_right_view.size() == 0) {
365
+
htBailThisFrame(htd);
366
+
return;
367
+
}
368
+
369
+
// Figure out how to match hands up across views.
370
+
// Construct a matrix, where the rows are left view hands and the cols are right view hands.
371
+
// For each cell, compute an error that's just the difference in Y ray coordinates of all the 21 keypoints. With
372
+
// perfect cameras + models, these differences will be zero. Anything with a high difference is not the same
373
+
// hand observed across views. For each cell, make a datatype that is: the error, the left view hand index, the
374
+
// right view hand index. Put these in an array, sort them by lowest error. Iterate over this sorted list (not
375
+
// in matrix-land anymore), assigning left view hands to right view hands as you go. For any elements that are
376
+
// trying to assign an already-assigned hand, skip them. At the end, check for any hands that went un-assigned;
377
+
// forget about those.
378
+
379
+
// In the future, maybe we should go forward with several hand associations if there are two that are close,
380
+
// keep track of which associations are mutually exclusive, and drop the one that fits the kinematic model less
381
+
// well? Or drop the one that matches with previous measurements less well? Getting raw 3D poses out of line
382
+
// intersection is not expensive.
383
+
384
+
// Known issue: If you put your hands at both exactly the same height it will not do the right thing. Won't fix
385
+
// right now; need to upstream *something* first.
386
+
387
+
std::vector<bool> left_hands_taken;
388
+
std::vector<bool> right_hands_taken;
389
+
390
+
std::vector<size_t> l_indices_in_order;
391
+
std::vector<size_t> r_indices_in_order;
392
+
std::vector<float> y_disparity_error_in_order;
393
+
394
+
naive_sort_permutation_by_error<Hand2D, Hand2D>(
395
+
// Inputs
396
+
hands_in_left_view, hands_in_right_view,
397
+
398
+
// Outputs
399
+
left_hands_taken, right_hands_taken,
400
+
401
+
l_indices_in_order, r_indices_in_order, y_disparity_error_in_order, errHandDisparity);
402
+
403
+
std::vector<Hand2D> associated_in_left;
404
+
std::vector<Hand2D> associated_in_right;
405
+
406
+
407
+
for (size_t i = 0; i < l_indices_in_order.size(); i++) {
408
+
associated_in_left.push_back(hands_in_left_view[i]);
409
+
associated_in_right.push_back(hands_in_right_view[i]);
410
+
}
411
+
412
+
413
+
std::vector<Hand3D> hands_unfiltered; //(associated_in_left.size());
414
+
415
+
for (size_t hand_idx = 0; hand_idx < associated_in_left.size(); hand_idx++) {
416
+
417
+
Hand3D cur_hand;
418
+
419
+
for (int i = 0; i < 21; i++) {
420
+
float t = htd->baseline /
421
+
(associated_in_left[hand_idx].kps[i].x - associated_in_right[hand_idx].kps[i].x);
422
+
// float x, y;
423
+
424
+
cur_hand.kps[i].z = -t;
425
+
426
+
cur_hand.kps[i].x = (associated_in_left[hand_idx].kps[i].x * t); //-(htd->baseline * 0.5f);
427
+
cur_hand.kps[i].y = -associated_in_left[hand_idx].kps[i].y * t;
428
+
cur_hand.timestamp = timestamp;
429
+
430
+
// soon! average with hand in right view.
431
+
cur_hand.kps[i].x += htd->baseline + (associated_in_right[hand_idx].kps[i].x * t);
432
+
cur_hand.kps[i].y += -associated_in_right[hand_idx].kps[i].y * t;
433
+
434
+
cur_hand.kps[i].x *= .5;
435
+
cur_hand.kps[i].y *= .5;
436
+
}
437
+
438
+
if (rejectBadHand(&cur_hand)) { // reject hands!!!
439
+
cur_hand.y_disparity_error = y_disparity_error_in_order[hand_idx];
440
+
hands_unfiltered.push_back(cur_hand);
441
+
} else {
442
+
HT_DEBUG(htd, "Rejected bad hand!"); // This probably could be a warn ...
443
+
}
444
+
}
445
+
446
+
// Okay now do the exact same thing but with present and past instead of with left view and right view. Lotsa
447
+
// code but hey this is hard stuff.
448
+
449
+
450
+
std::vector<bool> past_hands_taken;
451
+
std::vector<bool> present_hands_taken;
452
+
453
+
std::vector<size_t> past_indices;
454
+
std::vector<size_t> present_indices;
455
+
std::vector<float> flow_errors;
456
+
457
+
458
+
naive_sort_permutation_by_error<HandHistory3D, Hand3D>(htd->histories_3d, // past
459
+
hands_unfiltered, // present
460
+
461
+
462
+
// outputs
463
+
past_hands_taken, present_hands_taken, past_indices,
464
+
present_indices, flow_errors, errHandHistory
465
+
466
+
);
467
+
468
+
469
+
for (size_t i = 0; i < past_indices.size(); i++) {
470
+
htd->histories_3d[past_indices[i]].last_hands.push(hands_unfiltered[present_indices[i]]);
471
+
}
472
+
// The above may not do anything, because we'll start out with no hand histories! All the numbers of elements
473
+
// should be zero.
474
+
475
+
476
+
for (size_t i = 0; i < present_hands_taken.size(); i++) {
477
+
if (present_hands_taken[i] == false) {
478
+
// if this hand never got assigned to a history
479
+
HandHistory3D history_new;
480
+
handEuroFiltersInit(&history_new, FCMIN_HAND, FCMIN_D_HAND, BETA_HAND);
481
+
history_new.last_hands.push(hands_unfiltered[i]);
482
+
// history_new.
483
+
htd->histories_3d.push_back(
484
+
history_new); // Add something to the end - don't initialize any of it.
485
+
}
486
+
}
487
+
488
+
int bob = 0;
489
+
for (size_t i = 0; i < past_hands_taken.size(); i++) {
490
+
if (past_hands_taken[i] == false) {
491
+
htd->histories_3d.erase(htd->histories_3d.begin() + i + bob);
492
+
bob--;
493
+
}
494
+
}
495
+
496
+
if (htd->histories_3d.size() == 0) {
497
+
HT_DEBUG(htd, "Bailing");
498
+
htBailThisFrame(htd);
499
+
return;
500
+
}
501
+
502
+
size_t num_hands = htd->histories_3d.size();
503
+
if (num_hands > 2) {
504
+
HT_WARN(htd, "More than two hands observed (%zu)! Expect bugginess!",
505
+
num_hands); // this is quite bad, but rarely happens.
506
+
}
507
+
508
+
// Iterate over all hands we're keeping track of, compute their current handedness.
509
+
for (size_t i = 0; i < htd->histories_3d.size(); i++) {
510
+
handednessHandHistory3D(&htd->histories_3d[i]);
511
+
}
512
+
513
+
// Whoo! Okay, now we have some unfiltered hands in htd->histories_3d[i].last_hands[0]! Euro filter them!
514
+
515
+
std::vector<Hand3D> filtered_hands(num_hands);
516
+
517
+
for (size_t hand_index = 0; hand_index < num_hands; hand_index++) {
518
+
filtered_hands[hand_index] = handEuroFiltersRun(&htd->histories_3d[hand_index]);
519
+
applyThumbIndexDrag(&filtered_hands[hand_index]);
520
+
filtered_hands[hand_index].handedness = htd->histories_3d[hand_index].handedness;
521
+
}
522
+
523
+
std::vector<size_t> xr_indices;
524
+
std::vector<Hand3D *> hands_to_use;
525
+
526
+
if (filtered_hands.size() == 1) {
527
+
if (filtered_hands[0].handedness < 0) {
528
+
// Left
529
+
xr_indices = {0};
530
+
hands_to_use = {&filtered_hands[0]};
531
+
} else {
532
+
xr_indices = {1};
533
+
hands_to_use = {&filtered_hands[0]};
534
+
}
535
+
} else {
536
+
// filtered_hands better be two for now.
537
+
if (filtered_hands[0].handedness < filtered_hands[1].handedness) {
538
+
xr_indices = {0, 1};
539
+
hands_to_use = {&filtered_hands[0], &filtered_hands[1]};
540
+
} else {
541
+
xr_indices = {1, 0};
542
+
hands_to_use = {&filtered_hands[0], &filtered_hands[1]};
543
+
}
544
+
}
545
+
546
+
struct xrt_hand_joint_set final_hands_ordered_by_handedness[2];
547
+
memset(&final_hands_ordered_by_handedness[0], 0, sizeof(xrt_hand_joint_set));
548
+
memset(&final_hands_ordered_by_handedness[1], 0, sizeof(xrt_hand_joint_set));
549
+
final_hands_ordered_by_handedness[0].is_active = false;
550
+
final_hands_ordered_by_handedness[1].is_active = false;
551
+
552
+
for (size_t i = 0; (i < xr_indices.size()); i++) {
553
+
Hand3D *hand = hands_to_use[i];
554
+
555
+
556
+
struct xrt_hand_joint_set *put_in_set = &final_hands_ordered_by_handedness[xr_indices[i]];
557
+
558
+
xrt_vec3 wrist = hand->kps[0];
559
+
560
+
xrt_vec3 index_prox = hand->kps[5];
561
+
xrt_vec3 middle_prox = hand->kps[9];
562
+
xrt_vec3 ring_prox = hand->kps[13];
563
+
xrt_vec3 pinky_prox = hand->kps[17];
564
+
565
+
xrt_vec3 middle_to_index = m_vec3_sub(index_prox, middle_prox);
566
+
xrt_vec3 middle_to_ring = m_vec3_sub(ring_prox, middle_prox);
567
+
xrt_vec3 middle_to_pinky = m_vec3_sub(pinky_prox, middle_prox);
568
+
569
+
xrt_vec3 three_fourths_down_middle_mcp =
570
+
m_vec3_add(m_vec3_mul_scalar(wrist, 3.0f / 4.0f), m_vec3_mul_scalar(middle_prox, 1.0f / 4.0f));
571
+
572
+
xrt_vec3 middle_metacarpal = three_fourths_down_middle_mcp;
573
+
574
+
float s = 0.6f;
575
+
576
+
xrt_vec3 index_metacarpal = middle_metacarpal + m_vec3_mul_scalar(middle_to_index, s);
577
+
xrt_vec3 ring_metacarpal = middle_metacarpal + m_vec3_mul_scalar(middle_to_ring, s);
578
+
xrt_vec3 pinky_metacarpal = middle_metacarpal + m_vec3_mul_scalar(middle_to_pinky, s);
579
+
580
+
float palm_ness = 0.33;
581
+
xrt_vec3 palm =
582
+
m_vec3_add(m_vec3_mul_scalar(wrist, palm_ness), m_vec3_mul_scalar(middle_prox, (1.0f - palm_ness)));
583
+
584
+
585
+
586
+
// clang-format off
587
+
588
+
htProcessJoint(htd,palm, put_in_set, XRT_HAND_JOINT_PALM);
589
+
590
+
htProcessJoint(htd,hand->kps[0], put_in_set, XRT_HAND_JOINT_WRIST);
591
+
htProcessJoint(htd,hand->kps[1], put_in_set, XRT_HAND_JOINT_THUMB_METACARPAL);
592
+
htProcessJoint(htd,hand->kps[2], put_in_set, XRT_HAND_JOINT_THUMB_PROXIMAL);
593
+
htProcessJoint(htd,hand->kps[3], put_in_set, XRT_HAND_JOINT_THUMB_DISTAL);
594
+
htProcessJoint(htd,hand->kps[4], put_in_set, XRT_HAND_JOINT_THUMB_TIP);
595
+
596
+
htProcessJoint(htd,index_metacarpal, put_in_set, XRT_HAND_JOINT_INDEX_METACARPAL);
597
+
htProcessJoint(htd,hand->kps[5], put_in_set, XRT_HAND_JOINT_INDEX_PROXIMAL);
598
+
htProcessJoint(htd,hand->kps[6], put_in_set, XRT_HAND_JOINT_INDEX_INTERMEDIATE);
599
+
htProcessJoint(htd,hand->kps[7], put_in_set, XRT_HAND_JOINT_INDEX_DISTAL);
600
+
htProcessJoint(htd,hand->kps[8], put_in_set, XRT_HAND_JOINT_INDEX_TIP);
601
+
602
+
htProcessJoint(htd,middle_metacarpal, put_in_set, XRT_HAND_JOINT_MIDDLE_METACARPAL);
603
+
htProcessJoint(htd,hand->kps[9], put_in_set, XRT_HAND_JOINT_MIDDLE_PROXIMAL);
604
+
htProcessJoint(htd,hand->kps[10], put_in_set, XRT_HAND_JOINT_MIDDLE_INTERMEDIATE);
605
+
htProcessJoint(htd,hand->kps[11], put_in_set, XRT_HAND_JOINT_MIDDLE_DISTAL);
606
+
htProcessJoint(htd,hand->kps[12], put_in_set, XRT_HAND_JOINT_MIDDLE_TIP);
607
+
608
+
htProcessJoint(htd,ring_metacarpal, put_in_set, XRT_HAND_JOINT_RING_METACARPAL);
609
+
htProcessJoint(htd,hand->kps[13], put_in_set, XRT_HAND_JOINT_RING_PROXIMAL);
610
+
htProcessJoint(htd,hand->kps[14], put_in_set, XRT_HAND_JOINT_RING_INTERMEDIATE);
611
+
htProcessJoint(htd,hand->kps[15], put_in_set, XRT_HAND_JOINT_RING_DISTAL);
612
+
htProcessJoint(htd,hand->kps[16], put_in_set, XRT_HAND_JOINT_RING_TIP);
613
+
614
+
htProcessJoint(htd, pinky_metacarpal, put_in_set, XRT_HAND_JOINT_LITTLE_METACARPAL);
615
+
htProcessJoint(htd,hand->kps[17], put_in_set, XRT_HAND_JOINT_LITTLE_PROXIMAL);
616
+
htProcessJoint(htd,hand->kps[18], put_in_set, XRT_HAND_JOINT_LITTLE_INTERMEDIATE);
617
+
htProcessJoint(htd,hand->kps[19], put_in_set, XRT_HAND_JOINT_LITTLE_DISTAL);
618
+
htProcessJoint(htd,hand->kps[20], put_in_set, XRT_HAND_JOINT_LITTLE_TIP);
619
+
put_in_set->is_active = true;
620
+
math_pose_identity(&put_in_set->hand_pose.pose);
621
+
put_in_set->hand_pose.relation_flags = valid_flags_ht;
622
+
// clang-format on
623
+
applyJointWidths(put_in_set);
624
+
applyJointOrientations(put_in_set, xr_indices[i]);
625
+
}
626
+
627
+
628
+
// For some reason, final_hands_ordered_by_handedness[0] is active but the other is inactive.
629
+
630
+
os_mutex_lock(&htd->openxr_hand_data_mediator);
631
+
memcpy(&htd->hands_for_openxr[0], &final_hands_ordered_by_handedness[0], sizeof(struct xrt_hand_joint_set));
632
+
memcpy(&htd->hands_for_openxr[1], &final_hands_ordered_by_handedness[1], sizeof(struct xrt_hand_joint_set));
633
+
634
+
#if defined(JSON_OUTPUT)
635
+
json_add_set(htd);
636
+
#endif
637
+
os_mutex_unlock(&htd->openxr_hand_data_mediator);
638
+
}
-146
src/xrt/drivers/ht/ht_driver.c
-146
src/xrt/drivers/ht/ht_driver.c
···
1
-
// Copyright 2020, Collabora, Ltd.
2
-
// SPDX-License-Identifier: BSL-1.0
3
-
/*!
4
-
* @file
5
-
* @brief Camera based hand tracking driver code.
6
-
* @author Christtoph Haag <christtoph.haag@collabora.com>
7
-
* @ingroup drv_ht
8
-
*/
9
-
10
-
#include "ht_driver.h"
11
-
#include "util/u_device.h"
12
-
#include "util/u_var.h"
13
-
#include "util/u_debug.h"
14
-
#include <string.h>
15
-
#include <stdio.h>
16
-
17
-
struct ht_device
18
-
{
19
-
struct xrt_device base;
20
-
21
-
struct xrt_tracked_hand *tracker;
22
-
23
-
struct xrt_space_relation hand_relation[2];
24
-
struct u_hand_tracking u_tracking[2];
25
-
26
-
struct xrt_tracking_origin tracking_origin;
27
-
28
-
enum u_logging_level ll;
29
-
};
30
-
31
-
DEBUG_GET_ONCE_LOG_OPTION(ht_log, "HT_LOG", U_LOGGING_WARN)
32
-
33
-
#define HT_TRACE(htd, ...) U_LOG_XDEV_IFL_T(&htd->base, htd->ll, __VA_ARGS__)
34
-
#define HT_DEBUG(htd, ...) U_LOG_XDEV_IFL_D(&htd->base, htd->ll, __VA_ARGS__)
35
-
#define HT_INFO(htd, ...) U_LOG_XDEV_IFL_I(&htd->base, htd->ll, __VA_ARGS__)
36
-
#define HT_WARN(htd, ...) U_LOG_XDEV_IFL_W(&htd->base, htd->ll, __VA_ARGS__)
37
-
#define HT_ERROR(htd, ...) U_LOG_XDEV_IFL_E(&htd->base, htd->ll, __VA_ARGS__)
38
-
39
-
static inline struct ht_device *
40
-
ht_device(struct xrt_device *xdev)
41
-
{
42
-
return (struct ht_device *)xdev;
43
-
}
44
-
45
-
static void
46
-
ht_device_update_inputs(struct xrt_device *xdev)
47
-
{
48
-
// Empty
49
-
}
50
-
51
-
static void
52
-
ht_device_get_hand_tracking(struct xrt_device *xdev,
53
-
enum xrt_input_name name,
54
-
uint64_t at_timestamp_ns,
55
-
struct xrt_hand_joint_set *out_value)
56
-
{
57
-
struct ht_device *htd = ht_device(xdev);
58
-
59
-
enum xrt_hand hand;
60
-
int index;
61
-
62
-
if (name == XRT_INPUT_GENERIC_HAND_TRACKING_LEFT) {
63
-
HT_TRACE(htd, "Get left hand tracking data");
64
-
index = 0;
65
-
hand = XRT_HAND_LEFT;
66
-
} else if (name == XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT) {
67
-
HT_TRACE(htd, "Get right hand tracking data");
68
-
index = 1;
69
-
hand = XRT_HAND_RIGHT;
70
-
} else {
71
-
HT_ERROR(htd, "unknown input name for hand tracker");
72
-
return;
73
-
}
74
-
75
-
76
-
77
-
htd->tracker->get_tracked_joints(htd->tracker, name, at_timestamp_ns, &htd->u_tracking[index].joints,
78
-
&htd->hand_relation[index]);
79
-
htd->u_tracking[index].timestamp_ns = at_timestamp_ns;
80
-
81
-
struct xrt_pose identity = XRT_POSE_IDENTITY;
82
-
83
-
u_hand_joints_set_out_data(&htd->u_tracking[index], hand, &htd->hand_relation[index], &identity, out_value);
84
-
}
85
-
86
-
static void
87
-
ht_device_destroy(struct xrt_device *xdev)
88
-
{
89
-
struct ht_device *htd = ht_device(xdev);
90
-
91
-
// Remove the variable tracking.
92
-
u_var_remove_root(htd);
93
-
94
-
u_device_free(&htd->base);
95
-
}
96
-
97
-
struct xrt_device *
98
-
ht_device_create(struct xrt_auto_prober *xap, cJSON *attached_data, struct xrt_prober *xp)
99
-
{
100
-
enum u_device_alloc_flags flags = U_DEVICE_ALLOC_NO_FLAGS;
101
-
102
-
//! @todo 2 hands hardcoded
103
-
int num_hands = 2;
104
-
105
-
struct ht_device *htd = U_DEVICE_ALLOCATE(struct ht_device, flags, num_hands, 0);
106
-
107
-
108
-
htd->base.tracking_origin = &htd->tracking_origin;
109
-
htd->base.tracking_origin->type = XRT_TRACKING_TYPE_RGB;
110
-
htd->base.tracking_origin->offset.position.x = 0.0f;
111
-
htd->base.tracking_origin->offset.position.y = 0.0f;
112
-
htd->base.tracking_origin->offset.position.z = 0.0f;
113
-
htd->base.tracking_origin->offset.orientation.w = 1.0f;
114
-
115
-
htd->ll = debug_get_log_option_ht_log();
116
-
117
-
htd->base.update_inputs = ht_device_update_inputs;
118
-
htd->base.get_hand_tracking = ht_device_get_hand_tracking;
119
-
htd->base.destroy = ht_device_destroy;
120
-
121
-
snprintf(htd->base.str, XRT_DEVICE_NAME_LEN, "Camera based Hand Tracker");
122
-
snprintf(htd->base.serial, XRT_DEVICE_NAME_LEN, "Camera based Hand Tracker");
123
-
124
-
htd->base.inputs[0].name = XRT_INPUT_GENERIC_HAND_TRACKING_LEFT;
125
-
htd->base.inputs[1].name = XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT;
126
-
127
-
htd->base.name = XRT_DEVICE_HAND_TRACKER;
128
-
129
-
if (xp->tracking->create_tracked_hand(xp->tracking, &htd->base, &htd->tracker) < 0) {
130
-
HT_DEBUG(htd, "Failed to create hand tracker module");
131
-
ht_device_destroy(&htd->base);
132
-
return NULL;
133
-
}
134
-
135
-
u_hand_joints_init_default_set(&htd->u_tracking[XRT_HAND_LEFT], XRT_HAND_LEFT, XRT_HAND_TRACKING_MODEL_CAMERA,
136
-
1.0);
137
-
u_hand_joints_init_default_set(&htd->u_tracking[XRT_HAND_RIGHT], XRT_HAND_RIGHT, XRT_HAND_TRACKING_MODEL_CAMERA,
138
-
1.0);
139
-
140
-
u_var_add_root(htd, "Camera based Hand Tracker", true);
141
-
u_var_add_ro_text(htd, htd->base.str, "Name");
142
-
143
-
HT_DEBUG(htd, "Hand Tracker initialized!");
144
-
145
-
return &htd->base;
146
-
}
+534
src/xrt/drivers/ht/ht_driver.cpp
+534
src/xrt/drivers/ht/ht_driver.cpp
···
1
+
// Copyright 2021, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Camera based hand tracking driver code.
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @author Jakob Bornecrantz <jakob@collabora.com>
8
+
* @ingroup drv_ht
9
+
*/
10
+
11
+
#include "ht_driver.hpp"
12
+
#include "math/m_api.h"
13
+
#include "util/u_device.h"
14
+
#include "util/u_frame.h"
15
+
#include "util/u_sink.h"
16
+
#include "util/u_format.h"
17
+
#include "tracking/t_frame_cv_mat_wrapper.hpp"
18
+
#include <cjson/cJSON.h>
19
+
#include <opencv2/core/mat.hpp>
20
+
#include <unistd.h>
21
+
22
+
#include "templates/NaivePermutationSort.hpp"
23
+
24
+
#include "opencv2/calib3d.hpp"
25
+
#include "opencv2/highgui.hpp"
26
+
#include "os/os_threading.h"
27
+
#include "os/os_time.h"
28
+
#include "util/u_time.h"
29
+
#include "util/u_json.h"
30
+
#include "util/u_config_json.h"
31
+
32
+
#include "math/m_eigen_interop.hpp"
33
+
34
+
35
+
36
+
// #include <asm-generic/errno-base.h>
37
+
38
+
#include "vive/vive_config.h"
39
+
40
+
#include "tracking/t_calibration_opencv.hpp"
41
+
42
+
#include "util/u_logging.h"
43
+
#include "util/u_time.h"
44
+
#include "util/u_trace_marker.h"
45
+
#include "xrt/xrt_defines.h"
46
+
47
+
#include "ht_algorithm.hpp"
48
+
#include "xrt/xrt_frameserver.h"
49
+
50
+
// #include <opencv2/imgproc.hpp>
51
+
#include <string.h>
52
+
#include <stdio.h>
53
+
#include <math.h>
54
+
55
+
#include <float.h>
56
+
57
+
58
+
59
+
#include <exception>
60
+
#include <fstream>
61
+
#include <iostream>
62
+
#include <limits>
63
+
#include <numeric>
64
+
#include <cmath>
65
+
#include <sstream>
66
+
#include <algorithm>
67
+
68
+
#include <thread>
69
+
#include <future>
70
+
71
+
/*!
72
+
* Setup helper functions.
73
+
*/
74
+
75
+
static bool
76
+
getCalibration(struct ht_device *htd, t_stereo_camera_calibration *calibration)
77
+
{
78
+
xrt::auxiliary::tracking::StereoCameraCalibrationWrapper wrap(calibration);
79
+
xrt_vec3 trans = {(float)wrap.camera_translation_mat(0, 0), (float)wrap.camera_translation_mat(1, 0),
80
+
(float)wrap.camera_translation_mat(2, 0)};
81
+
htd->baseline = m_vec3_len(trans);
82
+
83
+
#if 0
84
+
std::cout << "\n\nTRANSLATION VECTOR IS\n" << wrap.camera_translation_mat;
85
+
std::cout << "\n\nROTATION FROM LEFT TO RIGHT IS\n" << wrap.camera_rotation_mat << "\n";
86
+
#endif
87
+
88
+
cv::Matx34d P1;
89
+
cv::Matx34d P2;
90
+
91
+
cv::Matx44d Q;
92
+
93
+
// The only reason we're calling stereoRectify is because we want R1 and R2 for the
94
+
cv::stereoRectify(wrap.view[0].intrinsics_mat, // cameraMatrix1
95
+
wrap.view[0].distortion_mat, // distCoeffs1
96
+
wrap.view[1].intrinsics_mat, // cameraMatrix2
97
+
wrap.view[1].distortion_mat, // distCoeffs2
98
+
cv::Size(960, 960), // imageSize
99
+
wrap.camera_rotation_mat, // R
100
+
wrap.camera_translation_mat, // T
101
+
htd->views[0].rotate_camera_to_stereo_camera, // R1
102
+
htd->views[1].rotate_camera_to_stereo_camera, // R2
103
+
P1, // P1
104
+
P2, // P2
105
+
Q, // Q
106
+
0, // flags
107
+
-1.0f, // alpha
108
+
cv::Size(), // newImageSize
109
+
NULL, // validPixROI1
110
+
NULL); // validPixROI2
111
+
112
+
113
+
for (int i = 0; i < 2; i++) {
114
+
htd->views[i].cameraMatrix = wrap.view[i].intrinsics_mat;
115
+
116
+
htd->views[i].distortion = wrap.view[i].distortion_fisheye_mat;
117
+
}
118
+
119
+
cv::Matx33d rotate_stereo_camera_to_left_camera = htd->views[0].rotate_camera_to_stereo_camera.inv();
120
+
121
+
xrt_matrix_3x3 s;
122
+
s.v[0] = rotate_stereo_camera_to_left_camera(0, 0);
123
+
s.v[1] = rotate_stereo_camera_to_left_camera(0, 1);
124
+
s.v[2] = rotate_stereo_camera_to_left_camera(0, 2);
125
+
126
+
s.v[3] = rotate_stereo_camera_to_left_camera(1, 0);
127
+
s.v[4] = rotate_stereo_camera_to_left_camera(1, 1);
128
+
s.v[5] = rotate_stereo_camera_to_left_camera(1, 2);
129
+
130
+
s.v[6] = rotate_stereo_camera_to_left_camera(2, 0);
131
+
s.v[7] = rotate_stereo_camera_to_left_camera(2, 1);
132
+
s.v[8] = rotate_stereo_camera_to_left_camera(2, 2);
133
+
134
+
xrt_quat tmp;
135
+
136
+
math_quat_from_matrix_3x3(&s, &tmp);
137
+
138
+
// Weird that I have to invert this quat, right? I think at some point - like probably just above this - I must
139
+
// have swapped row-major and col-major - remember, if you transpose a rotation matrix, you get its inverse.
140
+
// Doesn't matter that I don't understand - non-inverted looks definitely wrong, inverted looks definitely
141
+
// right.
142
+
math_quat_invert(&tmp, &htd->stereo_camera_to_left_camera);
143
+
144
+
#if 0
145
+
U_LOG_E("%f %f %f %f", htd->stereo_camera_to_left_camera.w, htd->stereo_camera_to_left_camera.x,
146
+
htd->stereo_camera_to_left_camera.y, htd->stereo_camera_to_left_camera.z);
147
+
#endif
148
+
149
+
return true;
150
+
}
151
+
152
+
static void
153
+
getUserConfig(struct ht_device *htd)
154
+
{
155
+
// The game here is to avoid bugs + be paranoid, not to be fast. If you see something that seems "slow" - don't
156
+
// fix it. Any of the tracking code is way stickier than this could ever be.
157
+
158
+
// Set defaults
159
+
// Admit defeat: for now, Mediapipe's are still better than ours.
160
+
htd->runtime_config.palm_detection_use_mediapipe = true;
161
+
htd->runtime_config.keypoint_estimation_use_mediapipe = true;
162
+
163
+
// Make sure you build DebugOptimized!
164
+
htd->runtime_config.desired_format = XRT_FORMAT_YUYV422;
165
+
166
+
struct u_config_json config_json = {};
167
+
168
+
u_config_json_open_or_create_main_file(&config_json);
169
+
if (!config_json.file_loaded) {
170
+
return;
171
+
}
172
+
173
+
cJSON *ht_config_json = cJSON_GetObjectItemCaseSensitive(config_json.root, "config_ht");
174
+
if (ht_config_json == NULL) {
175
+
return;
176
+
}
177
+
178
+
cJSON *palm_detection_type = cJSON_GetObjectItemCaseSensitive(ht_config_json, "palm_detection_model");
179
+
cJSON *keypoint_estimation_type = cJSON_GetObjectItemCaseSensitive(ht_config_json, "keypoint_estimation_model");
180
+
cJSON *uvc_wire_format = cJSON_GetObjectItemCaseSensitive(ht_config_json, "uvc_wire_format");
181
+
182
+
// IsString does its own null-checking
183
+
if (cJSON_IsString(palm_detection_type)) {
184
+
bool is_collabora = (strcmp(palm_detection_type->valuestring, "collabora") == 0);
185
+
bool is_mediapipe = (strcmp(palm_detection_type->valuestring, "mediapipe") == 0);
186
+
if (!is_collabora && !is_mediapipe) {
187
+
HT_WARN(htd, "Unknown palm detection type %s - should be \"collabora\" or \"mediapipe\"",
188
+
palm_detection_type->valuestring);
189
+
}
190
+
htd->runtime_config.palm_detection_use_mediapipe = is_mediapipe;
191
+
}
192
+
193
+
if (cJSON_IsString(keypoint_estimation_type)) {
194
+
bool is_collabora = (strcmp(keypoint_estimation_type->valuestring, "collabora") == 0);
195
+
bool is_mediapipe = (strcmp(keypoint_estimation_type->valuestring, "mediapipe") == 0);
196
+
if (!is_collabora && !is_mediapipe) {
197
+
HT_WARN(htd, "Unknown keypoint estimation type %s - should be \"collabora\" or \"mediapipe\"",
198
+
keypoint_estimation_type->valuestring);
199
+
}
200
+
htd->runtime_config.keypoint_estimation_use_mediapipe = is_mediapipe;
201
+
}
202
+
203
+
if (cJSON_IsString(uvc_wire_format)) {
204
+
bool is_yuv = (strcmp(cJSON_GetStringValue(uvc_wire_format), "yuv") == 0);
205
+
bool is_mjpeg = (strcmp(cJSON_GetStringValue(uvc_wire_format), "mjpeg") == 0);
206
+
if (!is_yuv && !is_mjpeg) {
207
+
HT_WARN(htd, "Unknown wire format type %s - should be \"yuv\" or \"mjpeg\"",
208
+
cJSON_GetStringValue(uvc_wire_format));
209
+
}
210
+
if (is_yuv) {
211
+
HT_DEBUG(htd, "Using YUYV422!");
212
+
htd->runtime_config.desired_format = XRT_FORMAT_YUYV422;
213
+
} else {
214
+
HT_DEBUG(htd, "Using MJPEG!");
215
+
htd->runtime_config.desired_format = XRT_FORMAT_MJPEG;
216
+
}
217
+
}
218
+
219
+
cJSON_Delete(config_json.root);
220
+
return;
221
+
}
222
+
223
+
224
+
static void
225
+
getModelsFolder(struct ht_device *htd)
226
+
{
227
+
// Please bikeshed me on this! I don't know where is the best place to put this stuff!
228
+
#if 0
229
+
char exec_location[1024] = {};
230
+
readlink("/proc/self/exe", exec_location, 1024);
231
+
232
+
HT_DEBUG(htd, "Exec at %s\n", exec_location);
233
+
234
+
int end = 0;
235
+
while (exec_location[end] != '\0') {
236
+
HT_DEBUG(htd, "%d", end);
237
+
end++;
238
+
}
239
+
240
+
while (exec_location[end] != '/' && end != 0) {
241
+
HT_DEBUG(htd, "%d %c", end, exec_location[end]);
242
+
exec_location[end] = '\0';
243
+
end--;
244
+
}
245
+
246
+
strcat(exec_location, "../share/monado/hand-tracking-models/");
247
+
strcpy(htd->runtime_config.model_slug, exec_location);
248
+
#else
249
+
const char *xdg_home = getenv("XDG_CONFIG_HOME");
250
+
const char *home = getenv("HOME");
251
+
if (xdg_home != NULL) {
252
+
strcpy(htd->runtime_config.model_slug, xdg_home);
253
+
} else if (home != NULL) {
254
+
strcpy(htd->runtime_config.model_slug, home);
255
+
} else {
256
+
assert(false);
257
+
}
258
+
strcat(htd->runtime_config.model_slug, "/.local/share/monado/hand-tracking-models/");
259
+
#endif
260
+
}
261
+
262
+
static void
263
+
on_video_device(struct xrt_prober *xp,
264
+
struct xrt_prober_device *pdev,
265
+
const char *product,
266
+
const char *manufacturer,
267
+
const char *serial,
268
+
void *ptr)
269
+
{
270
+
// Stolen from gui_scene_record
271
+
272
+
struct ht_device *htd = (struct ht_device *)ptr;
273
+
274
+
// Hardcoded for the Index.
275
+
if (product != NULL && manufacturer != NULL) {
276
+
if ((strcmp(product, "3D Camera") == 0) && (strcmp(manufacturer, "Etron Technology, Inc.") == 0)) {
277
+
xrt_prober_open_video_device(xp, pdev, &htd->camera.xfctx, &htd->camera.xfs);
278
+
htd->found_camera = true;
279
+
return;
280
+
}
281
+
}
282
+
}
283
+
284
+
/*!
285
+
* xrt_frame_sink function implementations
286
+
*/
287
+
288
+
static void
289
+
ht_sink_push_frame(struct xrt_frame_sink *xs, struct xrt_frame *xf)
290
+
{
291
+
XRT_TRACE_MARKER();
292
+
struct ht_device *htd = container_of(xs, struct ht_device, sink);
293
+
assert(xf != NULL);
294
+
295
+
if (!htd->tracking_should_die) {
296
+
os_mutex_lock(&htd->dying_breath);
297
+
298
+
xrt_frame_reference(&htd->frame_for_process, xf);
299
+
htRunAlgorithm(htd);
300
+
xrt_frame_reference(&htd->frame_for_process, NULL); // Could let go of it a little earlier but nah
301
+
302
+
os_mutex_unlock(&htd->dying_breath);
303
+
}
304
+
}
305
+
306
+
/*!
307
+
* xrt_frame_node function implementations
308
+
*/
309
+
310
+
static void
311
+
ht_node_break_apart(struct xrt_frame_node *node)
312
+
{
313
+
struct ht_device *htd = container_of(node, struct ht_device, node);
314
+
HT_DEBUG(htd, "called!");
315
+
// wrong but don't care
316
+
}
317
+
318
+
static void
319
+
ht_node_destroy(struct xrt_frame_node *node)
320
+
{
321
+
struct ht_device *htd = container_of(node, struct ht_device, node);
322
+
323
+
HT_DEBUG(htd, "called!");
324
+
}
325
+
326
+
/*!
327
+
* xrt_device function implementations
328
+
*/
329
+
330
+
static void
331
+
ht_device_update_inputs(struct xrt_device *xdev)
332
+
{
333
+
// Empty
334
+
}
335
+
336
+
static void
337
+
ht_device_get_hand_tracking(struct xrt_device *xdev,
338
+
enum xrt_input_name name,
339
+
uint64_t at_timestamp_ns,
340
+
struct xrt_hand_joint_set *out_value)
341
+
{
342
+
// Note! Currently, this totally ignores at_timestamp_ns. We need a better interface.
343
+
struct ht_device *htd = ht_device(xdev);
344
+
345
+
if (name != XRT_INPUT_GENERIC_HAND_TRACKING_LEFT && name != XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT) {
346
+
HT_ERROR(htd, "unknown input name for hand tracker");
347
+
return;
348
+
}
349
+
bool hand_index = (name == XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT); // left=0, right=1
350
+
351
+
352
+
353
+
os_mutex_lock(&htd->openxr_hand_data_mediator);
354
+
memcpy(out_value, &htd->hands_for_openxr[hand_index], sizeof(struct xrt_hand_joint_set));
355
+
os_mutex_unlock(&htd->openxr_hand_data_mediator);
356
+
}
357
+
358
+
static void
359
+
ht_device_destroy(struct xrt_device *xdev)
360
+
{
361
+
struct ht_device *htd = ht_device(xdev);
362
+
HT_DEBUG(htd, "called!");
363
+
364
+
365
+
xrt_frame_context_destroy_nodes(&htd->camera.xfctx);
366
+
htd->tracking_should_die = true;
367
+
368
+
// Lock this mutex so we don't try to free things as they're being used on the last iteration
369
+
os_mutex_lock(&htd->dying_breath);
370
+
destroyOnnx(htd);
371
+
#if defined(JSON_OUTPUT)
372
+
const char *string = cJSON_Print(htd->output_root);
373
+
FILE *fp;
374
+
fp = fopen("/1/2handtrack/aug12.json", "w");
375
+
376
+
377
+
fprintf(fp, "%s", string);
378
+
fclose(fp);
379
+
cJSON_Delete(htd->output_root);
380
+
#endif
381
+
382
+
// Remove the variable tracking.
383
+
u_var_remove_root(htd);
384
+
385
+
// Shhhhhhhhhhh, it's okay. It'll all be okay.
386
+
htd->histories_3d.~vector();
387
+
htd->views[0].bbox_histories.~vector();
388
+
htd->views[1].bbox_histories.~vector();
389
+
// Okay, fine, since we're mixing C and C++ idioms here, I couldn't find a clean way to implicitly
390
+
// call the destructors on these (ht_device doesn't have a destructor; neither do most of its members; and if
391
+
// you read u_device_allocate and u_device_free you'll agree it'd be somewhat annoying to write a
392
+
// constructor/destructor for ht_device), so we just manually call the destructors for things like std::vector's
393
+
// that need their destructors to be called to not leak.
394
+
395
+
u_device_free(&htd->base);
396
+
}
397
+
398
+
extern "C" struct xrt_device *
399
+
ht_device_create(struct xrt_prober *xp, struct t_stereo_camera_calibration *calib)
400
+
{
401
+
XRT_TRACE_MARKER();
402
+
enum u_device_alloc_flags flags = U_DEVICE_ALLOC_NO_FLAGS;
403
+
404
+
//! @todo 2 hands hardcoded
405
+
int num_hands = 2;
406
+
407
+
// Allocate device
408
+
struct ht_device *htd = U_DEVICE_ALLOCATE(struct ht_device, flags, num_hands, 0);
409
+
410
+
// Setup logging first. We like logging.
411
+
htd->ll = debug_get_log_option_ht_log();
412
+
413
+
// Get configuration
414
+
assert(calib != NULL);
415
+
getCalibration(htd, calib);
416
+
getUserConfig(htd);
417
+
getModelsFolder(htd);
418
+
419
+
// Add xrt_frame_sink and xrt_frame_node implementations
420
+
htd->sink.push_frame = &ht_sink_push_frame;
421
+
htd->node.break_apart = &ht_node_break_apart;
422
+
htd->node.destroy = &ht_node_destroy;
423
+
424
+
// Add ourselves to the frame context
425
+
xrt_frame_context_add(&htd->camera.xfctx, &htd->node);
426
+
427
+
428
+
htd->camera.one_view_size_px.w = 960;
429
+
htd->camera.one_view_size_px.h = 960;
430
+
htd->camera.prober = xp;
431
+
xrt_prober_list_video_devices(htd->camera.prober, on_video_device, htd);
432
+
433
+
434
+
if (!htd->found_camera) {
435
+
return NULL;
436
+
}
437
+
438
+
439
+
htd->views[0].htd = htd;
440
+
htd->views[1].htd = htd; // :)
441
+
442
+
htd->views[0].view = 0;
443
+
htd->views[1].view = 1;
444
+
445
+
446
+
initOnnx(htd);
447
+
448
+
htd->base.tracking_origin = &htd->tracking_origin;
449
+
htd->base.tracking_origin->type = XRT_TRACKING_TYPE_RGB;
450
+
htd->base.tracking_origin->offset.position.x = 0.0f;
451
+
htd->base.tracking_origin->offset.position.y = 0.0f;
452
+
htd->base.tracking_origin->offset.position.z = 0.0f;
453
+
htd->base.tracking_origin->offset.orientation.w = 1.0f;
454
+
455
+
os_mutex_init(&htd->openxr_hand_data_mediator);
456
+
os_mutex_init(&htd->dying_breath);
457
+
458
+
htd->base.update_inputs = ht_device_update_inputs;
459
+
htd->base.get_hand_tracking = ht_device_get_hand_tracking;
460
+
htd->base.destroy = ht_device_destroy;
461
+
462
+
snprintf(htd->base.str, XRT_DEVICE_NAME_LEN, "Camera based Hand Tracker");
463
+
snprintf(htd->base.serial, XRT_DEVICE_NAME_LEN, "Camera based Hand Tracker");
464
+
465
+
htd->base.inputs[0].name = XRT_INPUT_GENERIC_HAND_TRACKING_LEFT;
466
+
htd->base.inputs[1].name = XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT;
467
+
468
+
// Yes, you need all of these. Yes, I tried disabling them all one at a time. You need all of these.
469
+
htd->base.name = XRT_DEVICE_HAND_TRACKER;
470
+
htd->base.device_type = XRT_DEVICE_TYPE_HAND_TRACKER;
471
+
htd->base.orientation_tracking_supported = true;
472
+
htd->base.position_tracking_supported = true;
473
+
htd->base.hand_tracking_supported = true;
474
+
475
+
#if defined(JSON_OUTPUT)
476
+
htd->output_root = cJSON_CreateObject();
477
+
htd->output_array = cJSON_CreateArray();
478
+
cJSON_AddItemToObject(htd->output_root, "hand_array", htd->output_array);
479
+
#endif
480
+
481
+
struct xrt_frame_sink *tmp = &htd->sink;
482
+
483
+
u_var_add_root(htd, "Camera based Hand Tracker", true);
484
+
u_var_add_ro_text(htd, htd->base.str, "Name");
485
+
486
+
// This puts u_sink_create_to_r8g8b8_or_l8 on its own thread, so that nothing gets backed up if it runs slower
487
+
// than the native camera framerate.
488
+
u_sink_queue_create(&htd->camera.xfctx, tmp, &tmp);
489
+
490
+
// Converts images (we'd expect YUV422 or MJPEG) to R8G8B8. Can take a long time, especially on unoptimized
491
+
// builds. If it's really slow, triple-check that you built Monado with optimizations!
492
+
u_sink_create_to_r8g8b8_or_l8(&htd->camera.xfctx, tmp, &tmp);
493
+
494
+
// Puts the hand tracking code on its own thread, so that nothing upstream of it gets backed up if the hand
495
+
// tracking code runs slower than the upstream framerate.
496
+
u_sink_queue_create(&htd->camera.xfctx, tmp, &tmp);
497
+
498
+
xrt_fs_mode *modes;
499
+
uint32_t count;
500
+
501
+
xrt_fs_enumerate_modes(htd->camera.xfs, &modes, &count);
502
+
503
+
// Index should only have XRT_FORMAT_YUYV422 or XRT_FORMAT_MJPEG.
504
+
505
+
bool found_mode = false;
506
+
uint32_t selected_mode = 0;
507
+
508
+
for (; selected_mode < count; selected_mode++) {
509
+
if (modes[selected_mode].format == htd->runtime_config.desired_format) {
510
+
found_mode = true;
511
+
break;
512
+
}
513
+
}
514
+
515
+
if (!found_mode) {
516
+
selected_mode = 0;
517
+
HT_WARN(htd, "Couldn't find desired camera mode! Something's probably wrong.");
518
+
}
519
+
520
+
free(modes);
521
+
522
+
523
+
xrt_fs_stream_start(htd->camera.xfs, tmp, XRT_FS_CAPTURE_TYPE_TRACKING, selected_mode);
524
+
525
+
#if 1
526
+
u_var_add_sink(htd, &htd->debug_sink, "Debug visualization");
527
+
#endif
528
+
529
+
530
+
HT_DEBUG(htd, "Hand Tracker initialized!");
531
+
532
+
533
+
return &htd->base;
534
+
}
-26
src/xrt/drivers/ht/ht_driver.h
-26
src/xrt/drivers/ht/ht_driver.h
···
1
-
// Copyright 2020, Collabora, Ltd.
2
-
// SPDX-License-Identifier: BSL-1.0
3
-
/*!
4
-
* @file
5
-
* @brief Interface to camera based hand tracking driver code.
6
-
* @author Christtoph Haag <christtoph.haag@collabora.com>
7
-
* @ingroup drv_ht
8
-
*/
9
-
10
-
#pragma once
11
-
12
-
#include "math/m_api.h"
13
-
#include "xrt/xrt_device.h"
14
-
#include "xrt/xrt_prober.h"
15
-
16
-
#ifdef __cplusplus
17
-
extern "C" {
18
-
#endif
19
-
20
-
struct xrt_device *
21
-
ht_device_create();
22
-
23
-
24
-
#ifdef __cplusplus
25
-
}
26
-
#endif
+290
src/xrt/drivers/ht/ht_driver.hpp
+290
src/xrt/drivers/ht/ht_driver.hpp
···
1
+
// Copyright 2021, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Defines and common includes for camera-based hand tracker
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @ingroup drv_ht
8
+
*/
9
+
10
+
#pragma once
11
+
12
+
13
+
#include "math/m_api.h"
14
+
#include "math/m_vec3.h"
15
+
16
+
#include "math/m_filter_one_euro.h"
17
+
18
+
#include "xrt/xrt_device.h"
19
+
#include "xrt/xrt_prober.h"
20
+
#include "xrt/xrt_frame.h"
21
+
#include "xrt/xrt_frameserver.h"
22
+
23
+
#include "util/u_var.h"
24
+
#include "util/u_debug.h"
25
+
#include "util/u_sink.h"
26
+
#include "util/u_device.h"
27
+
28
+
#include "os/os_threading.h"
29
+
30
+
#include <future>
31
+
#include <opencv2/opencv.hpp>
32
+
33
+
#include "core/session/onnxruntime_c_api.h"
34
+
35
+
#include "templates/DiscardLastBuffer.hpp"
36
+
37
+
#include "util/u_json.h"
38
+
39
+
#include <vector>
40
+
41
+
42
+
43
+
DEBUG_GET_ONCE_LOG_OPTION(ht_log, "HT_LOG", U_LOGGING_WARN)
44
+
45
+
#define HT_TRACE(htd, ...) U_LOG_XDEV_IFL_T(&htd->base, htd->ll, __VA_ARGS__)
46
+
#define HT_DEBUG(htd, ...) U_LOG_XDEV_IFL_D(&htd->base, htd->ll, __VA_ARGS__)
47
+
#define HT_INFO(htd, ...) U_LOG_XDEV_IFL_I(&htd->base, htd->ll, __VA_ARGS__)
48
+
#define HT_WARN(htd, ...) U_LOG_XDEV_IFL_W(&htd->base, htd->ll, __VA_ARGS__)
49
+
#define HT_ERROR(htd, ...) U_LOG_XDEV_IFL_E(&htd->base, htd->ll, __VA_ARGS__)
50
+
51
+
// #define ht_
52
+
53
+
54
+
// To make clang-tidy happy
55
+
#define opencv_distortion_param_num 4
56
+
57
+
/*
58
+
*
59
+
* Compile-time defines to choose where to get camera frames from and what kind of output to give out
60
+
*
61
+
*/
62
+
#undef JSON_OUTPUT
63
+
64
+
#define FCMIN_BBOX 3.0f
65
+
#define FCMIN_D_BB0X 10.0f
66
+
#define BETA_BB0X 0.0f
67
+
68
+
69
+
#define FCMIN_HAND 4.0f
70
+
#define FCMIN_D_HAND 12.0f
71
+
#define BETA_HAND 0.05f
72
+
73
+
#ifdef __cplusplus
74
+
extern "C" {
75
+
#endif
76
+
77
+
enum HandJoint7Keypoint
78
+
{
79
+
WRIST_7KP = 0,
80
+
INDEX_7KP = 1,
81
+
MIDDLE_7KP = 2,
82
+
RING_7KP = 3,
83
+
LITTLE_7KP = 4,
84
+
THUMB_METACARPAL_7KP = 5,
85
+
THMB_PROXIMAL_7KP = 6
86
+
};
87
+
88
+
enum HandJoint21Keypoint
89
+
{
90
+
WRIST = 0,
91
+
92
+
THMB_MCP = 1,
93
+
THMB_PXM = 2,
94
+
THMB_DST = 3,
95
+
THMB_TIP = 4,
96
+
97
+
INDX_PXM = 5,
98
+
INDX_INT = 6,
99
+
INDX_DST = 7,
100
+
INDX_TIP = 8,
101
+
102
+
MIDL_PXM = 9,
103
+
MIDL_INT = 10,
104
+
MIDL_DST = 11,
105
+
MIDL_TIP = 12,
106
+
107
+
RING_PXM = 13,
108
+
RING_INT = 14,
109
+
RING_DST = 15,
110
+
RING_TIP = 16,
111
+
112
+
LITL_PXM = 17,
113
+
LITL_INT = 18,
114
+
LITL_DST = 19,
115
+
LITL_TIP = 20
116
+
};
117
+
118
+
struct Palm7KP
119
+
{
120
+
struct xrt_vec2 kps[7];
121
+
};
122
+
123
+
// To keep you on your toes. *Don't* think the 2D hand is the same as the 3D!
124
+
struct Hand2D
125
+
{
126
+
struct xrt_vec3 kps[21];
127
+
// Third value is depth from ML model. Do not believe the depth.
128
+
};
129
+
130
+
struct Hand3D
131
+
{
132
+
struct xrt_vec3 kps[21];
133
+
float y_disparity_error;
134
+
float flow_error;
135
+
float handedness;
136
+
uint64_t timestamp;
137
+
};
138
+
139
+
140
+
struct DetectionModelOutput
141
+
{
142
+
float rotation;
143
+
float size;
144
+
xrt_vec2 center;
145
+
xrt_vec2 wrist;
146
+
cv::Matx23f warp_there;
147
+
cv::Matx23f warp_back;
148
+
};
149
+
150
+
struct HandHistory3D
151
+
{
152
+
// Index 0 is current frame, index 1 is last frame, index 2 is second to last frame.
153
+
// No particular reason to keep the last 5 frames. we only really only use the current and last one.
154
+
float handedness;
155
+
DiscardLastBuffer<Hand3D, 5> last_hands;
156
+
// Euro filter for 21kps.
157
+
m_filter_euro_vec3 filters[21];
158
+
};
159
+
160
+
struct HandHistory2DBBox
161
+
{
162
+
m_filter_euro_vec2 m_filter_wrist;
163
+
m_filter_euro_vec2 m_filter_middle;
164
+
165
+
DiscardLastBuffer<xrt_vec2, 50> wrist_unfiltered;
166
+
DiscardLastBuffer<xrt_vec2, 50> middle_unfiltered;
167
+
};
168
+
169
+
170
+
struct ModelInfo
171
+
{
172
+
OrtSession *session = nullptr;
173
+
OrtMemoryInfo *memoryInfo = nullptr;
174
+
// std::vector's don't make too much sense here, but they're oh so easy
175
+
std::vector<int64_t> input_shape;
176
+
size_t input_size_bytes;
177
+
std::vector<const char *> output_names;
178
+
std::vector<const char *> input_names;
179
+
};
180
+
181
+
182
+
// Forward declaration for ht_view
183
+
struct ht_device;
184
+
185
+
struct ht_view
186
+
{
187
+
ht_device *htd;
188
+
int view; // :)))
189
+
190
+
// Loaded from config file
191
+
cv::Matx<double, opencv_distortion_param_num, 1> distortion;
192
+
cv::Matx<double, 3, 3> cameraMatrix;
193
+
cv::Matx33d rotate_camera_to_stereo_camera; // R1 or R2
194
+
195
+
cv::Mat run_model_on_this;
196
+
cv::Mat debug_out_to_this;
197
+
198
+
std::vector<HandHistory2DBBox> bbox_histories;
199
+
200
+
struct ModelInfo detection_model;
201
+
std::vector<Palm7KP> (*run_detection_model)(struct ht_view *htv, cv::Mat &img);
202
+
203
+
struct ModelInfo keypoint_model;
204
+
// The cv::mat is passed by value, *not* passed by reference or by pointer;
205
+
// in the tight loop that sets these off we reuse that cv::Mat; changing the data pointer as all the models are
206
+
// running is... going to wreak havoc let's say that.
207
+
Hand2D (*run_keypoint_model)(struct ht_view *htv, cv::Mat img);
208
+
};
209
+
210
+
struct ht_device
211
+
{
212
+
struct xrt_device base;
213
+
214
+
struct xrt_tracking_origin tracking_origin; // probably cargo-culted
215
+
216
+
struct xrt_frame_sink sink;
217
+
struct xrt_frame_node node;
218
+
219
+
struct xrt_frame_sink *debug_sink; // this must be bad.
220
+
221
+
222
+
struct
223
+
{
224
+
struct xrt_frame_context xfctx;
225
+
226
+
struct xrt_fs *xfs;
227
+
228
+
struct xrt_fs_mode mode;
229
+
230
+
struct xrt_prober *prober;
231
+
232
+
struct xrt_size one_view_size_px;
233
+
} camera;
234
+
235
+
bool found_camera;
236
+
237
+
const OrtApi *ort_api;
238
+
OrtEnv *ort_env;
239
+
240
+
struct xrt_frame *frame_for_process;
241
+
242
+
struct ht_view views[2];
243
+
244
+
// These are all we need - R and T don't aren't of interest to us.
245
+
// [2];
246
+
float baseline;
247
+
248
+
struct xrt_quat stereo_camera_to_left_camera;
249
+
250
+
uint64_t current_frame_timestamp; // SUPER dumb.
251
+
252
+
std::vector<HandHistory3D> histories_3d;
253
+
254
+
struct os_mutex openxr_hand_data_mediator;
255
+
struct xrt_hand_joint_set hands_for_openxr[2];
256
+
257
+
bool tracking_should_die;
258
+
struct os_mutex dying_breath;
259
+
260
+
bool debug_scribble = true;
261
+
262
+
263
+
#if defined(JSON_OUTPUT)
264
+
cJSON *output_root;
265
+
cJSON *output_array;
266
+
#endif
267
+
268
+
struct
269
+
{
270
+
bool palm_detection_use_mediapipe;
271
+
bool keypoint_estimation_use_mediapipe;
272
+
enum xrt_format desired_format;
273
+
char model_slug[1024];
274
+
} runtime_config;
275
+
276
+
277
+
278
+
enum u_logging_level ll;
279
+
};
280
+
281
+
static inline struct ht_device *
282
+
ht_device(struct xrt_device *xdev)
283
+
{
284
+
return (struct ht_device *)xdev;
285
+
}
286
+
287
+
288
+
#ifdef __cplusplus
289
+
}
290
+
#endif
+307
src/xrt/drivers/ht/ht_hand_math.hpp
+307
src/xrt/drivers/ht/ht_hand_math.hpp
···
1
+
// Copyright 2021, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Helper math to do things with 3D hands for the camera-based hand tracker
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @author Nick Klingensmith <programmerpichu@gmail.com>
8
+
* @ingroup drv_ht
9
+
*/
10
+
11
+
#pragma once
12
+
#include "ht_driver.hpp"
13
+
#include "math/m_api.h"
14
+
#include "math/m_vec3.h"
15
+
16
+
const int num_real_joints = 21;
17
+
18
+
static float
19
+
errHandDisparity(Hand2D *left_rays, Hand2D *right_rays)
20
+
{
21
+
float error = 0.0f;
22
+
for (int i = 0; i < 21; i++) {
23
+
float diff = fabsf(left_rays->kps[i].y - right_rays->kps[i].y);
24
+
// Big question about what's the best loss function. Gut feeling was "I should be using sum of squared
25
+
// errors" but I don't really know. Using just sum of errors for now. Ideally it'd also be not very
26
+
// sensitive to one or two really bad outliers.
27
+
error += diff;
28
+
}
29
+
return error;
30
+
}
31
+
32
+
33
+
static float
34
+
errHandFlow(Hand3D *prev, Hand3D *next)
35
+
{
36
+
float error = 0.0f;
37
+
for (int i = 0; i < num_real_joints; i++) {
38
+
xrt_vec3 first = prev->kps[i];
39
+
xrt_vec3 second = next->kps[i];
40
+
error += m_vec3_len(m_vec3_sub(second, first));
41
+
}
42
+
return error;
43
+
}
44
+
45
+
static float
46
+
errHandHistory(HandHistory3D *history_hand, Hand3D *present_hand)
47
+
{
48
+
// Remember we never have to deal with an empty hand. Can always access the last element.
49
+
return errHandFlow(history_hand->last_hands[0], present_hand);
50
+
}
51
+
52
+
53
+
static void
54
+
applyJointWidths(struct xrt_hand_joint_set *set)
55
+
{
56
+
// Thanks to Nick Klingensmith for this idea
57
+
struct xrt_hand_joint_value *gr = set->values.hand_joint_set_default;
58
+
59
+
const float hand_joint_size[5] = {0.022f, 0.021f, 0.022f, 0.021f, 0.02f};
60
+
const float hand_finger_size[5] = {1.0f, 1.0f, 0.83f, 0.75f};
61
+
62
+
const float thumb_size[4] = {0.016f, 0.014f, 0.012f, 0.012f};
63
+
float mul = 1.0f;
64
+
65
+
for (int i = XRT_HAND_JOINT_THUMB_METACARPAL; i <= XRT_HAND_JOINT_THUMB_TIP; i++) {
66
+
int j = i - XRT_HAND_JOINT_THUMB_METACARPAL;
67
+
gr[i].radius = thumb_size[j] * mul;
68
+
}
69
+
70
+
for (int finger = 0; finger < 4; finger++) {
71
+
for (int joint = 0; joint < 5; joint++) {
72
+
int set_idx = finger * 5 + joint + XRT_HAND_JOINT_INDEX_METACARPAL;
73
+
float val = hand_joint_size[joint] * hand_finger_size[finger] * .5 * mul;
74
+
gr[set_idx].radius = val;
75
+
}
76
+
}
77
+
}
78
+
79
+
static void
80
+
applyThumbIndexDrag(Hand3D *hand)
81
+
{
82
+
// TERRIBLE HACK.
83
+
// Puts the thumb and pointer a bit closer together to be better at triggering XR clients' pinch detection.
84
+
const float max_radius = 0.09; // 9 centimeters.
85
+
const float min_radius = 0.00;
86
+
87
+
// no min drag, min drag always 0.
88
+
const float max_drag = 0.75f;
89
+
90
+
xrt_vec3 thumb = hand->kps[THMB_TIP];
91
+
xrt_vec3 index = hand->kps[INDX_TIP];
92
+
xrt_vec3 ttp = index - thumb;
93
+
float length = m_vec3_len(ttp);
94
+
if ((length > max_radius)) {
95
+
return;
96
+
}
97
+
98
+
99
+
float amount = math_map_ranges(length, min_radius, max_radius, max_drag, 0.0f);
100
+
101
+
hand->kps[THMB_TIP] = m_vec3_lerp(thumb, index, amount * 0.5f);
102
+
hand->kps[INDX_TIP] = m_vec3_lerp(index, thumb, amount * 0.5f);
103
+
}
104
+
105
+
static void
106
+
applyJointOrientations(struct xrt_hand_joint_set *set, bool is_right)
107
+
{
108
+
// The real rule to follow is that each joint's "X" axis is along the axis along which it can bend.
109
+
// The nature of our estimation makes this a bit difficult, but these should work okay-ish under perfect
110
+
// conditions
111
+
if (set->is_active == false) {
112
+
return;
113
+
}
114
+
115
+
#define gl(jt) set->values.hand_joint_set_default[jt].relation.pose.position
116
+
117
+
xrt_vec3 pinky_prox = gl(XRT_HAND_JOINT_LITTLE_PROXIMAL);
118
+
119
+
xrt_vec3 index_prox = gl(XRT_HAND_JOINT_INDEX_PROXIMAL);
120
+
121
+
122
+
xrt_vec3 pinky_to_index_prox = m_vec3_normalize(index_prox - pinky_prox);
123
+
if (is_right) {
124
+
pinky_to_index_prox = m_vec3_mul_scalar(pinky_to_index_prox, -1.0f);
125
+
}
126
+
127
+
std::vector<std::vector<enum xrt_hand_joint>> fingers_with_joints_in_them = {
128
+
129
+
{XRT_HAND_JOINT_INDEX_METACARPAL, XRT_HAND_JOINT_INDEX_PROXIMAL, XRT_HAND_JOINT_INDEX_INTERMEDIATE,
130
+
XRT_HAND_JOINT_INDEX_DISTAL, XRT_HAND_JOINT_INDEX_TIP},
131
+
132
+
{XRT_HAND_JOINT_MIDDLE_METACARPAL, XRT_HAND_JOINT_MIDDLE_PROXIMAL, XRT_HAND_JOINT_MIDDLE_INTERMEDIATE,
133
+
XRT_HAND_JOINT_MIDDLE_DISTAL, XRT_HAND_JOINT_MIDDLE_TIP},
134
+
135
+
{XRT_HAND_JOINT_RING_METACARPAL, XRT_HAND_JOINT_RING_PROXIMAL, XRT_HAND_JOINT_RING_INTERMEDIATE,
136
+
XRT_HAND_JOINT_RING_DISTAL, XRT_HAND_JOINT_RING_TIP},
137
+
138
+
{XRT_HAND_JOINT_LITTLE_METACARPAL, XRT_HAND_JOINT_LITTLE_PROXIMAL, XRT_HAND_JOINT_LITTLE_INTERMEDIATE,
139
+
XRT_HAND_JOINT_LITTLE_DISTAL, XRT_HAND_JOINT_LITTLE_TIP},
140
+
141
+
};
142
+
for (std::vector<enum xrt_hand_joint> finger : fingers_with_joints_in_them) {
143
+
144
+
for (int i = 0; i < 4; i++) {
145
+
// Don't do fingertips. (Fingertip would be index 4.)
146
+
struct xrt_vec3 forwards = m_vec3_normalize(gl(finger[i + 1]) - gl(finger[i]));
147
+
struct xrt_vec3 backwards = m_vec3_mul_scalar(forwards, -1.0f);
148
+
149
+
struct xrt_vec3 left = m_vec3_orthonormalize(forwards, pinky_to_index_prox);
150
+
// float dot = m_vec3_dot(backwards, left);
151
+
// assert((m_vec3_dot(backwards,left) == 0.0f));
152
+
math_quat_from_plus_x_z(
153
+
&left, &backwards,
154
+
&set->values.hand_joint_set_default[finger[i]].relation.pose.orientation);
155
+
}
156
+
// Do fingertip! Per XR_EXT_hand_tracking, just copy the distal joint's orientation. Doing anything else
157
+
// is wrong.
158
+
set->values.hand_joint_set_default[finger[4]].relation.pose.orientation =
159
+
set->values.hand_joint_set_default[finger[3]].relation.pose.orientation;
160
+
}
161
+
162
+
// wrist!
163
+
// Not the best but acceptable. Eventually, probably, do triangle of wrist pinky prox and index prox.
164
+
set->values.hand_joint_set_default[XRT_HAND_JOINT_WRIST].relation.pose.orientation =
165
+
set->values.hand_joint_set_default[XRT_HAND_JOINT_MIDDLE_METACARPAL].relation.pose.orientation;
166
+
167
+
168
+
// palm!
169
+
set->values.hand_joint_set_default[XRT_HAND_JOINT_PALM].relation.pose.orientation =
170
+
set->values.hand_joint_set_default[XRT_HAND_JOINT_MIDDLE_METACARPAL].relation.pose.orientation;
171
+
172
+
// thumb!
173
+
// When I look at Ultraleap tracking, there's like, a "plane" made by the tip, distal and proximal (and kinda
174
+
// MCP, but least squares fitting a plane is too hard for my baby brain) Normal to this plane is the +X, and
175
+
// obviously forwards to the next joint is the -Z.
176
+
xrt_vec3 thumb_prox_to_dist = gl(XRT_HAND_JOINT_THUMB_DISTAL) - gl(XRT_HAND_JOINT_THUMB_PROXIMAL);
177
+
xrt_vec3 thumb_dist_to_tip = gl(XRT_HAND_JOINT_THUMB_TIP) - gl(XRT_HAND_JOINT_THUMB_DISTAL);
178
+
xrt_vec3 plane_normal;
179
+
if (!is_right) {
180
+
math_vec3_cross(&thumb_prox_to_dist, &thumb_dist_to_tip, &plane_normal);
181
+
} else {
182
+
math_vec3_cross(&thumb_dist_to_tip, &thumb_prox_to_dist, &plane_normal);
183
+
}
184
+
std::vector<enum xrt_hand_joint> thumbs = {XRT_HAND_JOINT_THUMB_METACARPAL, XRT_HAND_JOINT_THUMB_PROXIMAL,
185
+
XRT_HAND_JOINT_THUMB_DISTAL, XRT_HAND_JOINT_THUMB_TIP};
186
+
for (int i = 0; i < 3; i++) {
187
+
struct xrt_vec3 backwards =
188
+
m_vec3_mul_scalar(m_vec3_normalize(gl(thumbs[i + 1]) - gl(thumbs[i])), -1.0f);
189
+
190
+
struct xrt_vec3 left = m_vec3_orthonormalize(backwards, plane_normal);
191
+
math_quat_from_plus_x_z(&left, &backwards,
192
+
&set->values.hand_joint_set_default[thumbs[i]].relation.pose.orientation);
193
+
}
194
+
struct xrt_quat *tip = &set->values.hand_joint_set_default[XRT_HAND_JOINT_THUMB_TIP].relation.pose.orientation;
195
+
struct xrt_quat *distal =
196
+
&set->values.hand_joint_set_default[XRT_HAND_JOINT_THUMB_DISTAL].relation.pose.orientation;
197
+
memcpy(tip, distal, sizeof(struct xrt_quat));
198
+
}
199
+
200
+
static float
201
+
handednessJointSet(Hand3D *set)
202
+
{
203
+
// Guess if hand is left or right.
204
+
// Left is negative, right is positive.
205
+
206
+
207
+
// xrt_vec3 middle_mcp = gl(XRT_HAND_JOINT_MIDDLE_METACARPAL);
208
+
209
+
xrt_vec3 pinky_prox = set->kps[LITL_PXM]; // gl(XRT_HAND_JOINT_LITTLE_PROXIMAL);
210
+
211
+
xrt_vec3 index_prox = set->kps[INDX_PXM]; // gl(XRT_HAND_JOINT_INDEX_PROXIMAL);
212
+
213
+
xrt_vec3 pinky_to_index_prox = m_vec3_normalize(index_prox - pinky_prox);
214
+
215
+
float handedness = 0.0f;
216
+
217
+
for (int i : {INDX_PXM, MIDL_PXM, RING_PXM, LITL_PXM}) {
218
+
xrt_vec3 prox = set->kps[i];
219
+
xrt_vec3 intr = set->kps[i + 1];
220
+
xrt_vec3 dist = set->kps[i + 2];
221
+
xrt_vec3 tip = set->kps[i + 3];
222
+
223
+
xrt_vec3 prox_to_int = m_vec3_normalize(intr - prox);
224
+
xrt_vec3 int_to_dist = m_vec3_normalize(dist - intr);
225
+
xrt_vec3 dist_to_tip = m_vec3_normalize(tip - dist);
226
+
227
+
xrt_vec3 checks[2];
228
+
229
+
math_vec3_cross(&prox_to_int, &int_to_dist, &checks[0]);
230
+
math_vec3_cross(&int_to_dist, &dist_to_tip, &checks[1]);
231
+
232
+
handedness += m_vec3_dot(m_vec3_normalize(pinky_to_index_prox), (checks[0]));
233
+
handedness += m_vec3_dot(m_vec3_normalize(pinky_to_index_prox), (checks[1]));
234
+
}
235
+
set->handedness = handedness / (4 * 2);
236
+
return set->handedness;
237
+
}
238
+
239
+
static void
240
+
handednessHandHistory3D(HandHistory3D *history)
241
+
{
242
+
243
+
float inter = handednessJointSet(history->last_hands[0]);
244
+
245
+
if ((fabsf(inter) > 0.3f) || (fabsf(history->handedness) < 0.3f)) {
246
+
history->handedness += inter;
247
+
}
248
+
const int max_handedness = 2.0f;
249
+
if (history->handedness > max_handedness) {
250
+
history->handedness = max_handedness;
251
+
} else if (history->handedness < -max_handedness) {
252
+
history->handedness = -max_handedness;
253
+
}
254
+
}
255
+
256
+
static void
257
+
handEuroFiltersInit(HandHistory3D *history, double fc_min, double fc_min_d, double beta)
258
+
{
259
+
for (int i = 0; i < 21; i++) {
260
+
m_filter_euro_vec3_init(&history->filters[i], fc_min, beta, fc_min_d);
261
+
}
262
+
}
263
+
264
+
static Hand3D
265
+
handEuroFiltersRun(HandHistory3D *history)
266
+
{
267
+
// Assume present hand is in element 0!
268
+
Hand3D hand;
269
+
for (int i = 0; i < 21; i++) {
270
+
m_filter_euro_vec3_run(&history->filters[i], history->last_hands[0]->timestamp,
271
+
&history->last_hands[0]->kps[i], &hand.kps[i]);
272
+
}
273
+
return hand;
274
+
}
275
+
276
+
static bool
277
+
rejectTooFarOrTooClose(Hand3D *hand)
278
+
{
279
+
const float max_dist_from_camera_sqrd =
280
+
2.f * 2.f; // If you ever run into somebody with 2-meter-long arms, let me know!
281
+
const float min_dist_from_camera_sqrd = 0.05f * 0.05f;
282
+
for (int i = 0; i < 21; i++) {
283
+
xrt_vec3 pos = hand->kps[i];
284
+
float len = m_vec3_len_sqrd(pos); // Faster.
285
+
if (len > max_dist_from_camera_sqrd) {
286
+
return false;
287
+
U_LOG_W("Hand is somewhere we wouldn't expect!");
288
+
}
289
+
if (len < min_dist_from_camera_sqrd) {
290
+
return false;
291
+
}
292
+
if (pos.z > 0.0f) { // remember negative-Z is forward!
293
+
return false;
294
+
}
295
+
}
296
+
return true;
297
+
}
298
+
299
+
static bool
300
+
rejectBadHand(Hand3D *hand)
301
+
{
302
+
if (!rejectTooFarOrTooClose(hand)) {
303
+
return false;
304
+
}
305
+
// todo: add lots of checks! finger length, fingers bending backwards, etc.
306
+
return true;
307
+
}
+208
src/xrt/drivers/ht/ht_image_math.hpp
+208
src/xrt/drivers/ht/ht_image_math.hpp
···
1
+
// Copyright 2021, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Helper math to do things with images for the camera-based hand tracker
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @ingroup drv_ht
8
+
*/
9
+
10
+
#pragma once
11
+
#include "ht_driver.hpp"
12
+
#include "math/m_api.h"
13
+
#include "math/m_vec2.h"
14
+
#include "math/m_vec3.h"
15
+
#include "xrt/xrt_defines.h"
16
+
#include <opencv2/calib3d.hpp>
17
+
18
+
#include <opencv2/core/types.hpp>
19
+
#include <opencv2/imgproc.hpp>
20
+
21
+
static cv::Scalar
22
+
hsv2rgb(float fH, float fS, float fV)
23
+
{
24
+
float fC = fV * fS; // Chroma
25
+
float fHPrime = fmod(fH / 60.0, 6);
26
+
float fX = fC * (1 - fabs(fmod(fHPrime, 2) - 1));
27
+
float fM = fV - fC;
28
+
29
+
float fR, fG, fB;
30
+
31
+
if (0 <= fHPrime && fHPrime < 1) {
32
+
fR = fC;
33
+
fG = fX;
34
+
fB = 0;
35
+
} else if (1 <= fHPrime && fHPrime < 2) {
36
+
fR = fX;
37
+
fG = fC;
38
+
fB = 0;
39
+
} else if (2 <= fHPrime && fHPrime < 3) {
40
+
fR = 0;
41
+
fG = fC;
42
+
fB = fX;
43
+
} else if (3 <= fHPrime && fHPrime < 4) {
44
+
fR = 0;
45
+
fG = fX;
46
+
fB = fC;
47
+
} else if (4 <= fHPrime && fHPrime < 5) {
48
+
fR = fX;
49
+
fG = 0;
50
+
fB = fC;
51
+
} else if (5 <= fHPrime && fHPrime < 6) {
52
+
fR = fC;
53
+
fG = 0;
54
+
fB = fX;
55
+
} else {
56
+
fR = 0;
57
+
fG = 0;
58
+
fB = 0;
59
+
}
60
+
61
+
fR += fM;
62
+
fG += fM;
63
+
fB += fM;
64
+
return {fR * 255.0f, fG * 255.0f, fB * 255.0f};
65
+
}
66
+
67
+
static xrt_vec3
68
+
raycoord(struct ht_view *htv, struct xrt_vec3 model_out)
69
+
{
70
+
cv::Mat in_px_coords(1, 1, CV_32FC2);
71
+
float *write_in;
72
+
write_in = in_px_coords.ptr<float>(0);
73
+
write_in[0] = model_out.x;
74
+
write_in[1] = model_out.y;
75
+
cv::Mat out_ray(1, 1, CV_32FC2);
76
+
77
+
cv::fisheye::undistortPoints(in_px_coords, out_ray, htv->cameraMatrix, htv->distortion);
78
+
79
+
80
+
float n_x = out_ray.at<float>(0, 0);
81
+
float n_y = out_ray.at<float>(0, 1);
82
+
83
+
84
+
struct xrt_vec3 n = {n_x, n_y, 1.0f};
85
+
86
+
cv::Matx33f R = htv->rotate_camera_to_stereo_camera;
87
+
88
+
struct xrt_vec3 o = {
89
+
(n.x * R(0, 0)) + (n.y * R(0, 1)) + (n.z * R(0, 2)),
90
+
(n.x * R(1, 0)) + (n.y * R(1, 1)) + (n.z * R(1, 2)),
91
+
(n.x * R(2, 0)) + (n.y * R(2, 1)) + (n.z * R(2, 2)),
92
+
};
93
+
94
+
math_vec3_scalar_mul(1.0f / o.z, &o);
95
+
return o;
96
+
}
97
+
98
+
99
+
/*!
100
+
* Returns a 2x3 transform matrix that takes you back from the blackbarred image to the original image.
101
+
*/
102
+
103
+
static cv::Matx23f
104
+
blackbar(cv::Mat &in, cv::Mat &out, xrt_vec2_i32 out_size)
105
+
{
106
+
float aspect_ratio_input = in.cols / in.rows;
107
+
float aspect_ratio_output = out_size.x / out_size.y;
108
+
109
+
if (aspect_ratio_input == aspect_ratio_output) {
110
+
cv::resize(in, out, {out_size.x, out_size.y});
111
+
cv::Matx23f ret;
112
+
float scale_from_out_to_in = (float)in.cols / (float)out_size.x;
113
+
// clang-format off
114
+
ret(0,0) = scale_from_out_to_in; ret(0,1) = 0.0f; ret(0,2) = 0.0f;
115
+
ret(1,0) = 0.0f; ret(1,1) = scale_from_out_to_in; ret(1,2) = 0.0f;
116
+
// clang-format on
117
+
return ret;
118
+
}
119
+
assert(!"Uh oh! Unimplemented!");
120
+
return {};
121
+
}
122
+
123
+
/*!
124
+
* This is a template so that we can use xrt_vec3 or xrt_vec2.
125
+
* Please don't use this for anything other than xrt_vec3 or xrt_vec2!
126
+
*/
127
+
128
+
template <typename T>
129
+
T
130
+
transformVecBy2x3(T in, cv::Matx23f warp_back)
131
+
{
132
+
T rr;
133
+
rr.x = (in.x * warp_back(0, 0)) + (in.y * warp_back(0, 1)) + warp_back(0, 2);
134
+
rr.y = (in.x * warp_back(1, 0)) + (in.y * warp_back(1, 1)) + warp_back(1, 2);
135
+
return rr;
136
+
}
137
+
138
+
//! Draw some dots. Factors out some boilerplate.
139
+
static void
140
+
handDot(cv::Mat &mat, xrt_vec2 place, float radius, float hue, int type)
141
+
{
142
+
cv::circle(mat, {(int)place.x, (int)place.y}, radius, hsv2rgb(hue * 360.0f, 1.0f, 1.0f), type);
143
+
}
144
+
145
+
static DetectionModelOutput
146
+
rotatedRectFromJoints(struct ht_view *htv, xrt_vec2 middle, xrt_vec2 wrist, DetectionModelOutput *out)
147
+
{
148
+
// Close to what Mediapipe does, but slightly different - just uses the middle proximal instead of "estimating"
149
+
// it from the pinky and index.
150
+
// at the end of the day I should probably do that basis vector filtering thing to get a nicer middle metacarpal
151
+
// from 6 keypoints (not thumb proximal) OR SHOULD I. because distortion. hmm
152
+
153
+
// Feel free to look at the way MP does it, you can see it's different.
154
+
// https://github.com/google/mediapipe/blob/master/mediapipe/modules/holistic_landmark/calculators/hand_detections_from_pose_to_rects_calculator.cc
155
+
156
+
struct xrt_vec2 hand_center = middle; // Middle proximal, straight-up.
157
+
158
+
struct xrt_vec2 wrist_to_middle = middle - wrist;
159
+
160
+
float box_size = m_vec2_len(wrist_to_middle) * 2.0f * 1.7f;
161
+
162
+
double rot = atan2(wrist_to_middle.x, wrist_to_middle.y) * (-180.0f / M_PI);
163
+
164
+
out->rotation = rot;
165
+
out->size = box_size;
166
+
out->center = hand_center;
167
+
168
+
cv::RotatedRect rrect =
169
+
cv::RotatedRect(cv::Point2f(out->center.x, out->center.y), cv::Size2f(out->size, out->size), out->rotation);
170
+
171
+
172
+
cv::Point2f vertices[4];
173
+
rrect.points(vertices);
174
+
if (htv->htd->debug_scribble) {
175
+
for (int i = 0; i < 4; i++)
176
+
line(htv->debug_out_to_this, vertices[i], vertices[(i + 1) % 4], cv::Scalar(i * 63, i * 63, 0),
177
+
2);
178
+
}
179
+
// topright is 0. bottomright is 1. bottomleft is 2. topleft is 3.
180
+
181
+
cv::Point2f src_tri[3] = {vertices[3], vertices[2], vertices[1]}; // top-left, bottom-left, bottom-right
182
+
183
+
cv::Point2f dest_tri[3] = {cv::Point2f(0, 0), cv::Point2f(0, 224), cv::Point2f(224, 224)};
184
+
185
+
out->warp_there = getAffineTransform(src_tri, dest_tri);
186
+
out->warp_back = getAffineTransform(dest_tri, src_tri);
187
+
188
+
out->wrist = wrist;
189
+
190
+
return *out;
191
+
}
192
+
193
+
static void
194
+
planarize(cv::Mat &input, uint8_t *output)
195
+
{
196
+
// output better be the right size, because we are not doing any bounds checking!
197
+
assert(input.isContinuous());
198
+
int lix = input.cols;
199
+
int liy = input.rows;
200
+
cv::Mat planes[3];
201
+
cv::split(input, planes);
202
+
cv::Mat red = planes[0];
203
+
cv::Mat green = planes[1];
204
+
cv::Mat blue = planes[2];
205
+
memcpy(output, red.data, lix * liy);
206
+
memcpy(output + (lix * liy), green.data, lix * liy);
207
+
memcpy(output + (lix * liy * 2), blue.data, lix * liy);
208
+
}
+7
-4
src/xrt/drivers/ht/ht_interface.h
+7
-4
src/xrt/drivers/ht/ht_interface.h
···
1
-
// Copyright 2020, Collabora, Ltd.
1
+
// Copyright 2020-2021, Collabora, Ltd.
2
2
// SPDX-License-Identifier: BSL-1.0
3
3
/*!
4
4
* @file
5
5
* @brief Interface to camera based hand tracking driver code.
6
6
* @author Christoph Haag <christoph.haag@collabora.com>
7
+
* @author Moses Turner <moses@collabora.com>
7
8
* @ingroup drv_ht
8
9
*/
9
10
10
11
#pragma once
11
12
12
13
#include "math/m_api.h"
14
+
#include "xrt/xrt_defines.h"
13
15
#include "xrt/xrt_device.h"
16
+
#include "vive/vive_config.h"
14
17
15
18
#ifdef __cplusplus
16
19
extern "C" {
···
24
27
*/
25
28
26
29
/*!
27
-
* Create a probe for camera based hand tracking.
30
+
* Create a hand tracker device.
28
31
*
29
32
* @ingroup drv_ht
30
33
*/
31
-
struct xrt_auto_prober *
32
-
ht_create_auto_prober();
34
+
struct xrt_device *
35
+
ht_device_create(struct xrt_prober *xp, struct t_stereo_camera_calibration *calib);
33
36
34
37
/*!
35
38
* @dir drivers/ht
+819
src/xrt/drivers/ht/ht_models.hpp
+819
src/xrt/drivers/ht/ht_models.hpp
···
1
+
// Copyright 2021, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Code to run machine learning models for camera-based hand tracker.
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @author Marcus Edel <marcus.edel@collabora.com>
8
+
* @ingroup drv_ht
9
+
*/
10
+
11
+
// Many C api things were stolen from here (MIT license):
12
+
// https://github.com/microsoft/onnxruntime-inference-examples/blob/main/c_cxx/fns_candy_style_transfer/fns_candy_style_transfer.c
13
+
14
+
#pragma once
15
+
16
+
#include "math/m_api.h"
17
+
#include "math/m_vec2.h"
18
+
#include "math/m_vec3.h"
19
+
#include "opencv2/calib3d.hpp"
20
+
#include "opencv2/highgui.hpp"
21
+
#include "os/os_threading.h"
22
+
#include "os/os_time.h"
23
+
#include "util/u_json.h"
24
+
25
+
26
+
#include "../depthai/depthai_interface.h"
27
+
#include "../vf/vf_interface.h"
28
+
29
+
#include <cjson/cJSON.h>
30
+
31
+
#include "ht_driver.hpp"
32
+
#include "ht_nms.hpp"
33
+
#include "util/u_logging.h"
34
+
#include "util/u_time.h"
35
+
#include "util/u_trace_marker.h"
36
+
#include "ht_image_math.hpp"
37
+
38
+
#include <core/session/onnxruntime_c_api.h>
39
+
#include <cstdlib>
40
+
#include <opencv2/core/types.hpp>
41
+
#include <opencv2/imgproc.hpp>
42
+
#include <string.h>
43
+
#include <stdio.h>
44
+
#include <math.h>
45
+
46
+
#include <exception>
47
+
#include <fstream>
48
+
#include <iostream>
49
+
#include <limits>
50
+
#include <numeric>
51
+
#include <cmath>
52
+
#include <sstream>
53
+
#include <vector>
54
+
55
+
#define ORT_CHECK(g_ort, expr) \
56
+
do { \
57
+
OrtStatus *onnx_status = (expr); \
58
+
if (onnx_status != nullptr) { \
59
+
const char *msg = g_ort->GetErrorMessage(onnx_status); \
60
+
U_LOG_E("at %s:%d: %s\n", __FILE__, __LINE__, msg); \
61
+
g_ort->ReleaseStatus(onnx_status); \
62
+
assert(false); \
63
+
} \
64
+
} while (0);
65
+
66
+
static std::vector<std::vector<float>> anchor{
67
+
{0.031250, 0.031250, 1.000000, 1.000000}, {0.031250, 0.031250, 1.000000, 1.000000},
68
+
{0.093750, 0.031250, 1.000000, 1.000000}, {0.093750, 0.031250, 1.000000, 1.000000},
69
+
{0.156250, 0.031250, 1.000000, 1.000000}, {0.156250, 0.031250, 1.000000, 1.000000},
70
+
{0.218750, 0.031250, 1.000000, 1.000000}, {0.218750, 0.031250, 1.000000, 1.000000},
71
+
{0.281250, 0.031250, 1.000000, 1.000000}, {0.281250, 0.031250, 1.000000, 1.000000},
72
+
{0.343750, 0.031250, 1.000000, 1.000000}, {0.343750, 0.031250, 1.000000, 1.000000},
73
+
{0.406250, 0.031250, 1.000000, 1.000000}, {0.406250, 0.031250, 1.000000, 1.000000},
74
+
{0.468750, 0.031250, 1.000000, 1.000000}, {0.468750, 0.031250, 1.000000, 1.000000},
75
+
{0.531250, 0.031250, 1.000000, 1.000000}, {0.531250, 0.031250, 1.000000, 1.000000},
76
+
{0.593750, 0.031250, 1.000000, 1.000000}, {0.593750, 0.031250, 1.000000, 1.000000},
77
+
{0.656250, 0.031250, 1.000000, 1.000000}, {0.656250, 0.031250, 1.000000, 1.000000},
78
+
{0.718750, 0.031250, 1.000000, 1.000000}, {0.718750, 0.031250, 1.000000, 1.000000},
79
+
{0.781250, 0.031250, 1.000000, 1.000000}, {0.781250, 0.031250, 1.000000, 1.000000},
80
+
{0.843750, 0.031250, 1.000000, 1.000000}, {0.843750, 0.031250, 1.000000, 1.000000},
81
+
{0.906250, 0.031250, 1.000000, 1.000000}, {0.906250, 0.031250, 1.000000, 1.000000},
82
+
{0.968750, 0.031250, 1.000000, 1.000000}, {0.968750, 0.031250, 1.000000, 1.000000},
83
+
{0.031250, 0.093750, 1.000000, 1.000000}, {0.031250, 0.093750, 1.000000, 1.000000},
84
+
{0.093750, 0.093750, 1.000000, 1.000000}, {0.093750, 0.093750, 1.000000, 1.000000},
85
+
{0.156250, 0.093750, 1.000000, 1.000000}, {0.156250, 0.093750, 1.000000, 1.000000},
86
+
{0.218750, 0.093750, 1.000000, 1.000000}, {0.218750, 0.093750, 1.000000, 1.000000},
87
+
{0.281250, 0.093750, 1.000000, 1.000000}, {0.281250, 0.093750, 1.000000, 1.000000},
88
+
{0.343750, 0.093750, 1.000000, 1.000000}, {0.343750, 0.093750, 1.000000, 1.000000},
89
+
{0.406250, 0.093750, 1.000000, 1.000000}, {0.406250, 0.093750, 1.000000, 1.000000},
90
+
{0.468750, 0.093750, 1.000000, 1.000000}, {0.468750, 0.093750, 1.000000, 1.000000},
91
+
{0.531250, 0.093750, 1.000000, 1.000000}, {0.531250, 0.093750, 1.000000, 1.000000},
92
+
{0.593750, 0.093750, 1.000000, 1.000000}, {0.593750, 0.093750, 1.000000, 1.000000},
93
+
{0.656250, 0.093750, 1.000000, 1.000000}, {0.656250, 0.093750, 1.000000, 1.000000},
94
+
{0.718750, 0.093750, 1.000000, 1.000000}, {0.718750, 0.093750, 1.000000, 1.000000},
95
+
{0.781250, 0.093750, 1.000000, 1.000000}, {0.781250, 0.093750, 1.000000, 1.000000},
96
+
{0.843750, 0.093750, 1.000000, 1.000000}, {0.843750, 0.093750, 1.000000, 1.000000},
97
+
{0.906250, 0.093750, 1.000000, 1.000000}, {0.906250, 0.093750, 1.000000, 1.000000},
98
+
{0.968750, 0.093750, 1.000000, 1.000000}, {0.968750, 0.093750, 1.000000, 1.000000},
99
+
{0.031250, 0.156250, 1.000000, 1.000000}, {0.031250, 0.156250, 1.000000, 1.000000},
100
+
{0.093750, 0.156250, 1.000000, 1.000000}, {0.093750, 0.156250, 1.000000, 1.000000},
101
+
{0.156250, 0.156250, 1.000000, 1.000000}, {0.156250, 0.156250, 1.000000, 1.000000},
102
+
{0.218750, 0.156250, 1.000000, 1.000000}, {0.218750, 0.156250, 1.000000, 1.000000},
103
+
{0.281250, 0.156250, 1.000000, 1.000000}, {0.281250, 0.156250, 1.000000, 1.000000},
104
+
{0.343750, 0.156250, 1.000000, 1.000000}, {0.343750, 0.156250, 1.000000, 1.000000},
105
+
{0.406250, 0.156250, 1.000000, 1.000000}, {0.406250, 0.156250, 1.000000, 1.000000},
106
+
{0.468750, 0.156250, 1.000000, 1.000000}, {0.468750, 0.156250, 1.000000, 1.000000},
107
+
{0.531250, 0.156250, 1.000000, 1.000000}, {0.531250, 0.156250, 1.000000, 1.000000},
108
+
{0.593750, 0.156250, 1.000000, 1.000000}, {0.593750, 0.156250, 1.000000, 1.000000},
109
+
{0.656250, 0.156250, 1.000000, 1.000000}, {0.656250, 0.156250, 1.000000, 1.000000},
110
+
{0.718750, 0.156250, 1.000000, 1.000000}, {0.718750, 0.156250, 1.000000, 1.000000},
111
+
{0.781250, 0.156250, 1.000000, 1.000000}, {0.781250, 0.156250, 1.000000, 1.000000},
112
+
{0.843750, 0.156250, 1.000000, 1.000000}, {0.843750, 0.156250, 1.000000, 1.000000},
113
+
{0.906250, 0.156250, 1.000000, 1.000000}, {0.906250, 0.156250, 1.000000, 1.000000},
114
+
{0.968750, 0.156250, 1.000000, 1.000000}, {0.968750, 0.156250, 1.000000, 1.000000},
115
+
{0.031250, 0.218750, 1.000000, 1.000000}, {0.031250, 0.218750, 1.000000, 1.000000},
116
+
{0.093750, 0.218750, 1.000000, 1.000000}, {0.093750, 0.218750, 1.000000, 1.000000},
117
+
{0.156250, 0.218750, 1.000000, 1.000000}, {0.156250, 0.218750, 1.000000, 1.000000},
118
+
{0.218750, 0.218750, 1.000000, 1.000000}, {0.218750, 0.218750, 1.000000, 1.000000},
119
+
{0.281250, 0.218750, 1.000000, 1.000000}, {0.281250, 0.218750, 1.000000, 1.000000},
120
+
{0.343750, 0.218750, 1.000000, 1.000000}, {0.343750, 0.218750, 1.000000, 1.000000},
121
+
{0.406250, 0.218750, 1.000000, 1.000000}, {0.406250, 0.218750, 1.000000, 1.000000},
122
+
{0.468750, 0.218750, 1.000000, 1.000000}, {0.468750, 0.218750, 1.000000, 1.000000},
123
+
{0.531250, 0.218750, 1.000000, 1.000000}, {0.531250, 0.218750, 1.000000, 1.000000},
124
+
{0.593750, 0.218750, 1.000000, 1.000000}, {0.593750, 0.218750, 1.000000, 1.000000},
125
+
{0.656250, 0.218750, 1.000000, 1.000000}, {0.656250, 0.218750, 1.000000, 1.000000},
126
+
{0.718750, 0.218750, 1.000000, 1.000000}, {0.718750, 0.218750, 1.000000, 1.000000},
127
+
{0.781250, 0.218750, 1.000000, 1.000000}, {0.781250, 0.218750, 1.000000, 1.000000},
128
+
{0.843750, 0.218750, 1.000000, 1.000000}, {0.843750, 0.218750, 1.000000, 1.000000},
129
+
{0.906250, 0.218750, 1.000000, 1.000000}, {0.906250, 0.218750, 1.000000, 1.000000},
130
+
{0.968750, 0.218750, 1.000000, 1.000000}, {0.968750, 0.218750, 1.000000, 1.000000},
131
+
{0.031250, 0.281250, 1.000000, 1.000000}, {0.031250, 0.281250, 1.000000, 1.000000},
132
+
{0.093750, 0.281250, 1.000000, 1.000000}, {0.093750, 0.281250, 1.000000, 1.000000},
133
+
{0.156250, 0.281250, 1.000000, 1.000000}, {0.156250, 0.281250, 1.000000, 1.000000},
134
+
{0.218750, 0.281250, 1.000000, 1.000000}, {0.218750, 0.281250, 1.000000, 1.000000},
135
+
{0.281250, 0.281250, 1.000000, 1.000000}, {0.281250, 0.281250, 1.000000, 1.000000},
136
+
{0.343750, 0.281250, 1.000000, 1.000000}, {0.343750, 0.281250, 1.000000, 1.000000},
137
+
{0.406250, 0.281250, 1.000000, 1.000000}, {0.406250, 0.281250, 1.000000, 1.000000},
138
+
{0.468750, 0.281250, 1.000000, 1.000000}, {0.468750, 0.281250, 1.000000, 1.000000},
139
+
{0.531250, 0.281250, 1.000000, 1.000000}, {0.531250, 0.281250, 1.000000, 1.000000},
140
+
{0.593750, 0.281250, 1.000000, 1.000000}, {0.593750, 0.281250, 1.000000, 1.000000},
141
+
{0.656250, 0.281250, 1.000000, 1.000000}, {0.656250, 0.281250, 1.000000, 1.000000},
142
+
{0.718750, 0.281250, 1.000000, 1.000000}, {0.718750, 0.281250, 1.000000, 1.000000},
143
+
{0.781250, 0.281250, 1.000000, 1.000000}, {0.781250, 0.281250, 1.000000, 1.000000},
144
+
{0.843750, 0.281250, 1.000000, 1.000000}, {0.843750, 0.281250, 1.000000, 1.000000},
145
+
{0.906250, 0.281250, 1.000000, 1.000000}, {0.906250, 0.281250, 1.000000, 1.000000},
146
+
{0.968750, 0.281250, 1.000000, 1.000000}, {0.968750, 0.281250, 1.000000, 1.000000},
147
+
{0.031250, 0.343750, 1.000000, 1.000000}, {0.031250, 0.343750, 1.000000, 1.000000},
148
+
{0.093750, 0.343750, 1.000000, 1.000000}, {0.093750, 0.343750, 1.000000, 1.000000},
149
+
{0.156250, 0.343750, 1.000000, 1.000000}, {0.156250, 0.343750, 1.000000, 1.000000},
150
+
{0.218750, 0.343750, 1.000000, 1.000000}, {0.218750, 0.343750, 1.000000, 1.000000},
151
+
{0.281250, 0.343750, 1.000000, 1.000000}, {0.281250, 0.343750, 1.000000, 1.000000},
152
+
{0.343750, 0.343750, 1.000000, 1.000000}, {0.343750, 0.343750, 1.000000, 1.000000},
153
+
{0.406250, 0.343750, 1.000000, 1.000000}, {0.406250, 0.343750, 1.000000, 1.000000},
154
+
{0.468750, 0.343750, 1.000000, 1.000000}, {0.468750, 0.343750, 1.000000, 1.000000},
155
+
{0.531250, 0.343750, 1.000000, 1.000000}, {0.531250, 0.343750, 1.000000, 1.000000},
156
+
{0.593750, 0.343750, 1.000000, 1.000000}, {0.593750, 0.343750, 1.000000, 1.000000},
157
+
{0.656250, 0.343750, 1.000000, 1.000000}, {0.656250, 0.343750, 1.000000, 1.000000},
158
+
{0.718750, 0.343750, 1.000000, 1.000000}, {0.718750, 0.343750, 1.000000, 1.000000},
159
+
{0.781250, 0.343750, 1.000000, 1.000000}, {0.781250, 0.343750, 1.000000, 1.000000},
160
+
{0.843750, 0.343750, 1.000000, 1.000000}, {0.843750, 0.343750, 1.000000, 1.000000},
161
+
{0.906250, 0.343750, 1.000000, 1.000000}, {0.906250, 0.343750, 1.000000, 1.000000},
162
+
{0.968750, 0.343750, 1.000000, 1.000000}, {0.968750, 0.343750, 1.000000, 1.000000},
163
+
{0.031250, 0.406250, 1.000000, 1.000000}, {0.031250, 0.406250, 1.000000, 1.000000},
164
+
{0.093750, 0.406250, 1.000000, 1.000000}, {0.093750, 0.406250, 1.000000, 1.000000},
165
+
{0.156250, 0.406250, 1.000000, 1.000000}, {0.156250, 0.406250, 1.000000, 1.000000},
166
+
{0.218750, 0.406250, 1.000000, 1.000000}, {0.218750, 0.406250, 1.000000, 1.000000},
167
+
{0.281250, 0.406250, 1.000000, 1.000000}, {0.281250, 0.406250, 1.000000, 1.000000},
168
+
{0.343750, 0.406250, 1.000000, 1.000000}, {0.343750, 0.406250, 1.000000, 1.000000},
169
+
{0.406250, 0.406250, 1.000000, 1.000000}, {0.406250, 0.406250, 1.000000, 1.000000},
170
+
{0.468750, 0.406250, 1.000000, 1.000000}, {0.468750, 0.406250, 1.000000, 1.000000},
171
+
{0.531250, 0.406250, 1.000000, 1.000000}, {0.531250, 0.406250, 1.000000, 1.000000},
172
+
{0.593750, 0.406250, 1.000000, 1.000000}, {0.593750, 0.406250, 1.000000, 1.000000},
173
+
{0.656250, 0.406250, 1.000000, 1.000000}, {0.656250, 0.406250, 1.000000, 1.000000},
174
+
{0.718750, 0.406250, 1.000000, 1.000000}, {0.718750, 0.406250, 1.000000, 1.000000},
175
+
{0.781250, 0.406250, 1.000000, 1.000000}, {0.781250, 0.406250, 1.000000, 1.000000},
176
+
{0.843750, 0.406250, 1.000000, 1.000000}, {0.843750, 0.406250, 1.000000, 1.000000},
177
+
{0.906250, 0.406250, 1.000000, 1.000000}, {0.906250, 0.406250, 1.000000, 1.000000},
178
+
{0.968750, 0.406250, 1.000000, 1.000000}, {0.968750, 0.406250, 1.000000, 1.000000},
179
+
{0.031250, 0.468750, 1.000000, 1.000000}, {0.031250, 0.468750, 1.000000, 1.000000},
180
+
{0.093750, 0.468750, 1.000000, 1.000000}, {0.093750, 0.468750, 1.000000, 1.000000},
181
+
{0.156250, 0.468750, 1.000000, 1.000000}, {0.156250, 0.468750, 1.000000, 1.000000},
182
+
{0.218750, 0.468750, 1.000000, 1.000000}, {0.218750, 0.468750, 1.000000, 1.000000},
183
+
{0.281250, 0.468750, 1.000000, 1.000000}, {0.281250, 0.468750, 1.000000, 1.000000},
184
+
{0.343750, 0.468750, 1.000000, 1.000000}, {0.343750, 0.468750, 1.000000, 1.000000},
185
+
{0.406250, 0.468750, 1.000000, 1.000000}, {0.406250, 0.468750, 1.000000, 1.000000},
186
+
{0.468750, 0.468750, 1.000000, 1.000000}, {0.468750, 0.468750, 1.000000, 1.000000},
187
+
{0.531250, 0.468750, 1.000000, 1.000000}, {0.531250, 0.468750, 1.000000, 1.000000},
188
+
{0.593750, 0.468750, 1.000000, 1.000000}, {0.593750, 0.468750, 1.000000, 1.000000},
189
+
{0.656250, 0.468750, 1.000000, 1.000000}, {0.656250, 0.468750, 1.000000, 1.000000},
190
+
{0.718750, 0.468750, 1.000000, 1.000000}, {0.718750, 0.468750, 1.000000, 1.000000},
191
+
{0.781250, 0.468750, 1.000000, 1.000000}, {0.781250, 0.468750, 1.000000, 1.000000},
192
+
{0.843750, 0.468750, 1.000000, 1.000000}, {0.843750, 0.468750, 1.000000, 1.000000},
193
+
{0.906250, 0.468750, 1.000000, 1.000000}, {0.906250, 0.468750, 1.000000, 1.000000},
194
+
{0.968750, 0.468750, 1.000000, 1.000000}, {0.968750, 0.468750, 1.000000, 1.000000},
195
+
{0.031250, 0.531250, 1.000000, 1.000000}, {0.031250, 0.531250, 1.000000, 1.000000},
196
+
{0.093750, 0.531250, 1.000000, 1.000000}, {0.093750, 0.531250, 1.000000, 1.000000},
197
+
{0.156250, 0.531250, 1.000000, 1.000000}, {0.156250, 0.531250, 1.000000, 1.000000},
198
+
{0.218750, 0.531250, 1.000000, 1.000000}, {0.218750, 0.531250, 1.000000, 1.000000},
199
+
{0.281250, 0.531250, 1.000000, 1.000000}, {0.281250, 0.531250, 1.000000, 1.000000},
200
+
{0.343750, 0.531250, 1.000000, 1.000000}, {0.343750, 0.531250, 1.000000, 1.000000},
201
+
{0.406250, 0.531250, 1.000000, 1.000000}, {0.406250, 0.531250, 1.000000, 1.000000},
202
+
{0.468750, 0.531250, 1.000000, 1.000000}, {0.468750, 0.531250, 1.000000, 1.000000},
203
+
{0.531250, 0.531250, 1.000000, 1.000000}, {0.531250, 0.531250, 1.000000, 1.000000},
204
+
{0.593750, 0.531250, 1.000000, 1.000000}, {0.593750, 0.531250, 1.000000, 1.000000},
205
+
{0.656250, 0.531250, 1.000000, 1.000000}, {0.656250, 0.531250, 1.000000, 1.000000},
206
+
{0.718750, 0.531250, 1.000000, 1.000000}, {0.718750, 0.531250, 1.000000, 1.000000},
207
+
{0.781250, 0.531250, 1.000000, 1.000000}, {0.781250, 0.531250, 1.000000, 1.000000},
208
+
{0.843750, 0.531250, 1.000000, 1.000000}, {0.843750, 0.531250, 1.000000, 1.000000},
209
+
{0.906250, 0.531250, 1.000000, 1.000000}, {0.906250, 0.531250, 1.000000, 1.000000},
210
+
{0.968750, 0.531250, 1.000000, 1.000000}, {0.968750, 0.531250, 1.000000, 1.000000},
211
+
{0.031250, 0.593750, 1.000000, 1.000000}, {0.031250, 0.593750, 1.000000, 1.000000},
212
+
{0.093750, 0.593750, 1.000000, 1.000000}, {0.093750, 0.593750, 1.000000, 1.000000},
213
+
{0.156250, 0.593750, 1.000000, 1.000000}, {0.156250, 0.593750, 1.000000, 1.000000},
214
+
{0.218750, 0.593750, 1.000000, 1.000000}, {0.218750, 0.593750, 1.000000, 1.000000},
215
+
{0.281250, 0.593750, 1.000000, 1.000000}, {0.281250, 0.593750, 1.000000, 1.000000},
216
+
{0.343750, 0.593750, 1.000000, 1.000000}, {0.343750, 0.593750, 1.000000, 1.000000},
217
+
{0.406250, 0.593750, 1.000000, 1.000000}, {0.406250, 0.593750, 1.000000, 1.000000},
218
+
{0.468750, 0.593750, 1.000000, 1.000000}, {0.468750, 0.593750, 1.000000, 1.000000},
219
+
{0.531250, 0.593750, 1.000000, 1.000000}, {0.531250, 0.593750, 1.000000, 1.000000},
220
+
{0.593750, 0.593750, 1.000000, 1.000000}, {0.593750, 0.593750, 1.000000, 1.000000},
221
+
{0.656250, 0.593750, 1.000000, 1.000000}, {0.656250, 0.593750, 1.000000, 1.000000},
222
+
{0.718750, 0.593750, 1.000000, 1.000000}, {0.718750, 0.593750, 1.000000, 1.000000},
223
+
{0.781250, 0.593750, 1.000000, 1.000000}, {0.781250, 0.593750, 1.000000, 1.000000},
224
+
{0.843750, 0.593750, 1.000000, 1.000000}, {0.843750, 0.593750, 1.000000, 1.000000},
225
+
{0.906250, 0.593750, 1.000000, 1.000000}, {0.906250, 0.593750, 1.000000, 1.000000},
226
+
{0.968750, 0.593750, 1.000000, 1.000000}, {0.968750, 0.593750, 1.000000, 1.000000},
227
+
{0.031250, 0.656250, 1.000000, 1.000000}, {0.031250, 0.656250, 1.000000, 1.000000},
228
+
{0.093750, 0.656250, 1.000000, 1.000000}, {0.093750, 0.656250, 1.000000, 1.000000},
229
+
{0.156250, 0.656250, 1.000000, 1.000000}, {0.156250, 0.656250, 1.000000, 1.000000},
230
+
{0.218750, 0.656250, 1.000000, 1.000000}, {0.218750, 0.656250, 1.000000, 1.000000},
231
+
{0.281250, 0.656250, 1.000000, 1.000000}, {0.281250, 0.656250, 1.000000, 1.000000},
232
+
{0.343750, 0.656250, 1.000000, 1.000000}, {0.343750, 0.656250, 1.000000, 1.000000},
233
+
{0.406250, 0.656250, 1.000000, 1.000000}, {0.406250, 0.656250, 1.000000, 1.000000},
234
+
{0.468750, 0.656250, 1.000000, 1.000000}, {0.468750, 0.656250, 1.000000, 1.000000},
235
+
{0.531250, 0.656250, 1.000000, 1.000000}, {0.531250, 0.656250, 1.000000, 1.000000},
236
+
{0.593750, 0.656250, 1.000000, 1.000000}, {0.593750, 0.656250, 1.000000, 1.000000},
237
+
{0.656250, 0.656250, 1.000000, 1.000000}, {0.656250, 0.656250, 1.000000, 1.000000},
238
+
{0.718750, 0.656250, 1.000000, 1.000000}, {0.718750, 0.656250, 1.000000, 1.000000},
239
+
{0.781250, 0.656250, 1.000000, 1.000000}, {0.781250, 0.656250, 1.000000, 1.000000},
240
+
{0.843750, 0.656250, 1.000000, 1.000000}, {0.843750, 0.656250, 1.000000, 1.000000},
241
+
{0.906250, 0.656250, 1.000000, 1.000000}, {0.906250, 0.656250, 1.000000, 1.000000},
242
+
{0.968750, 0.656250, 1.000000, 1.000000}, {0.968750, 0.656250, 1.000000, 1.000000},
243
+
{0.031250, 0.718750, 1.000000, 1.000000}, {0.031250, 0.718750, 1.000000, 1.000000},
244
+
{0.093750, 0.718750, 1.000000, 1.000000}, {0.093750, 0.718750, 1.000000, 1.000000},
245
+
{0.156250, 0.718750, 1.000000, 1.000000}, {0.156250, 0.718750, 1.000000, 1.000000},
246
+
{0.218750, 0.718750, 1.000000, 1.000000}, {0.218750, 0.718750, 1.000000, 1.000000},
247
+
{0.281250, 0.718750, 1.000000, 1.000000}, {0.281250, 0.718750, 1.000000, 1.000000},
248
+
{0.343750, 0.718750, 1.000000, 1.000000}, {0.343750, 0.718750, 1.000000, 1.000000},
249
+
{0.406250, 0.718750, 1.000000, 1.000000}, {0.406250, 0.718750, 1.000000, 1.000000},
250
+
{0.468750, 0.718750, 1.000000, 1.000000}, {0.468750, 0.718750, 1.000000, 1.000000},
251
+
{0.531250, 0.718750, 1.000000, 1.000000}, {0.531250, 0.718750, 1.000000, 1.000000},
252
+
{0.593750, 0.718750, 1.000000, 1.000000}, {0.593750, 0.718750, 1.000000, 1.000000},
253
+
{0.656250, 0.718750, 1.000000, 1.000000}, {0.656250, 0.718750, 1.000000, 1.000000},
254
+
{0.718750, 0.718750, 1.000000, 1.000000}, {0.718750, 0.718750, 1.000000, 1.000000},
255
+
{0.781250, 0.718750, 1.000000, 1.000000}, {0.781250, 0.718750, 1.000000, 1.000000},
256
+
{0.843750, 0.718750, 1.000000, 1.000000}, {0.843750, 0.718750, 1.000000, 1.000000},
257
+
{0.906250, 0.718750, 1.000000, 1.000000}, {0.906250, 0.718750, 1.000000, 1.000000},
258
+
{0.968750, 0.718750, 1.000000, 1.000000}, {0.968750, 0.718750, 1.000000, 1.000000},
259
+
{0.031250, 0.781250, 1.000000, 1.000000}, {0.031250, 0.781250, 1.000000, 1.000000},
260
+
{0.093750, 0.781250, 1.000000, 1.000000}, {0.093750, 0.781250, 1.000000, 1.000000},
261
+
{0.156250, 0.781250, 1.000000, 1.000000}, {0.156250, 0.781250, 1.000000, 1.000000},
262
+
{0.218750, 0.781250, 1.000000, 1.000000}, {0.218750, 0.781250, 1.000000, 1.000000},
263
+
{0.281250, 0.781250, 1.000000, 1.000000}, {0.281250, 0.781250, 1.000000, 1.000000},
264
+
{0.343750, 0.781250, 1.000000, 1.000000}, {0.343750, 0.781250, 1.000000, 1.000000},
265
+
{0.406250, 0.781250, 1.000000, 1.000000}, {0.406250, 0.781250, 1.000000, 1.000000},
266
+
{0.468750, 0.781250, 1.000000, 1.000000}, {0.468750, 0.781250, 1.000000, 1.000000},
267
+
{0.531250, 0.781250, 1.000000, 1.000000}, {0.531250, 0.781250, 1.000000, 1.000000},
268
+
{0.593750, 0.781250, 1.000000, 1.000000}, {0.593750, 0.781250, 1.000000, 1.000000},
269
+
{0.656250, 0.781250, 1.000000, 1.000000}, {0.656250, 0.781250, 1.000000, 1.000000},
270
+
{0.718750, 0.781250, 1.000000, 1.000000}, {0.718750, 0.781250, 1.000000, 1.000000},
271
+
{0.781250, 0.781250, 1.000000, 1.000000}, {0.781250, 0.781250, 1.000000, 1.000000},
272
+
{0.843750, 0.781250, 1.000000, 1.000000}, {0.843750, 0.781250, 1.000000, 1.000000},
273
+
{0.906250, 0.781250, 1.000000, 1.000000}, {0.906250, 0.781250, 1.000000, 1.000000},
274
+
{0.968750, 0.781250, 1.000000, 1.000000}, {0.968750, 0.781250, 1.000000, 1.000000},
275
+
{0.031250, 0.843750, 1.000000, 1.000000}, {0.031250, 0.843750, 1.000000, 1.000000},
276
+
{0.093750, 0.843750, 1.000000, 1.000000}, {0.093750, 0.843750, 1.000000, 1.000000},
277
+
{0.156250, 0.843750, 1.000000, 1.000000}, {0.156250, 0.843750, 1.000000, 1.000000},
278
+
{0.218750, 0.843750, 1.000000, 1.000000}, {0.218750, 0.843750, 1.000000, 1.000000},
279
+
{0.281250, 0.843750, 1.000000, 1.000000}, {0.281250, 0.843750, 1.000000, 1.000000},
280
+
{0.343750, 0.843750, 1.000000, 1.000000}, {0.343750, 0.843750, 1.000000, 1.000000},
281
+
{0.406250, 0.843750, 1.000000, 1.000000}, {0.406250, 0.843750, 1.000000, 1.000000},
282
+
{0.468750, 0.843750, 1.000000, 1.000000}, {0.468750, 0.843750, 1.000000, 1.000000},
283
+
{0.531250, 0.843750, 1.000000, 1.000000}, {0.531250, 0.843750, 1.000000, 1.000000},
284
+
{0.593750, 0.843750, 1.000000, 1.000000}, {0.593750, 0.843750, 1.000000, 1.000000},
285
+
{0.656250, 0.843750, 1.000000, 1.000000}, {0.656250, 0.843750, 1.000000, 1.000000},
286
+
{0.718750, 0.843750, 1.000000, 1.000000}, {0.718750, 0.843750, 1.000000, 1.000000},
287
+
{0.781250, 0.843750, 1.000000, 1.000000}, {0.781250, 0.843750, 1.000000, 1.000000},
288
+
{0.843750, 0.843750, 1.000000, 1.000000}, {0.843750, 0.843750, 1.000000, 1.000000},
289
+
{0.906250, 0.843750, 1.000000, 1.000000}, {0.906250, 0.843750, 1.000000, 1.000000},
290
+
{0.968750, 0.843750, 1.000000, 1.000000}, {0.968750, 0.843750, 1.000000, 1.000000},
291
+
{0.031250, 0.906250, 1.000000, 1.000000}, {0.031250, 0.906250, 1.000000, 1.000000},
292
+
{0.093750, 0.906250, 1.000000, 1.000000}, {0.093750, 0.906250, 1.000000, 1.000000},
293
+
{0.156250, 0.906250, 1.000000, 1.000000}, {0.156250, 0.906250, 1.000000, 1.000000},
294
+
{0.218750, 0.906250, 1.000000, 1.000000}, {0.218750, 0.906250, 1.000000, 1.000000},
295
+
{0.281250, 0.906250, 1.000000, 1.000000}, {0.281250, 0.906250, 1.000000, 1.000000},
296
+
{0.343750, 0.906250, 1.000000, 1.000000}, {0.343750, 0.906250, 1.000000, 1.000000},
297
+
{0.406250, 0.906250, 1.000000, 1.000000}, {0.406250, 0.906250, 1.000000, 1.000000},
298
+
{0.468750, 0.906250, 1.000000, 1.000000}, {0.468750, 0.906250, 1.000000, 1.000000},
299
+
{0.531250, 0.906250, 1.000000, 1.000000}, {0.531250, 0.906250, 1.000000, 1.000000},
300
+
{0.593750, 0.906250, 1.000000, 1.000000}, {0.593750, 0.906250, 1.000000, 1.000000},
301
+
{0.656250, 0.906250, 1.000000, 1.000000}, {0.656250, 0.906250, 1.000000, 1.000000},
302
+
{0.718750, 0.906250, 1.000000, 1.000000}, {0.718750, 0.906250, 1.000000, 1.000000},
303
+
{0.781250, 0.906250, 1.000000, 1.000000}, {0.781250, 0.906250, 1.000000, 1.000000},
304
+
{0.843750, 0.906250, 1.000000, 1.000000}, {0.843750, 0.906250, 1.000000, 1.000000},
305
+
{0.906250, 0.906250, 1.000000, 1.000000}, {0.906250, 0.906250, 1.000000, 1.000000},
306
+
{0.968750, 0.906250, 1.000000, 1.000000}, {0.968750, 0.906250, 1.000000, 1.000000},
307
+
{0.031250, 0.968750, 1.000000, 1.000000}, {0.031250, 0.968750, 1.000000, 1.000000},
308
+
{0.093750, 0.968750, 1.000000, 1.000000}, {0.093750, 0.968750, 1.000000, 1.000000},
309
+
{0.156250, 0.968750, 1.000000, 1.000000}, {0.156250, 0.968750, 1.000000, 1.000000},
310
+
{0.218750, 0.968750, 1.000000, 1.000000}, {0.218750, 0.968750, 1.000000, 1.000000},
311
+
{0.281250, 0.968750, 1.000000, 1.000000}, {0.281250, 0.968750, 1.000000, 1.000000},
312
+
{0.343750, 0.968750, 1.000000, 1.000000}, {0.343750, 0.968750, 1.000000, 1.000000},
313
+
{0.406250, 0.968750, 1.000000, 1.000000}, {0.406250, 0.968750, 1.000000, 1.000000},
314
+
{0.468750, 0.968750, 1.000000, 1.000000}, {0.468750, 0.968750, 1.000000, 1.000000},
315
+
{0.531250, 0.968750, 1.000000, 1.000000}, {0.531250, 0.968750, 1.000000, 1.000000},
316
+
{0.593750, 0.968750, 1.000000, 1.000000}, {0.593750, 0.968750, 1.000000, 1.000000},
317
+
{0.656250, 0.968750, 1.000000, 1.000000}, {0.656250, 0.968750, 1.000000, 1.000000},
318
+
{0.718750, 0.968750, 1.000000, 1.000000}, {0.718750, 0.968750, 1.000000, 1.000000},
319
+
{0.781250, 0.968750, 1.000000, 1.000000}, {0.781250, 0.968750, 1.000000, 1.000000},
320
+
{0.843750, 0.968750, 1.000000, 1.000000}, {0.843750, 0.968750, 1.000000, 1.000000},
321
+
{0.906250, 0.968750, 1.000000, 1.000000}, {0.906250, 0.968750, 1.000000, 1.000000},
322
+
{0.968750, 0.968750, 1.000000, 1.000000}, {0.968750, 0.968750, 1.000000, 1.000000},
323
+
{0.062500, 0.062500, 1.000000, 1.000000}, {0.062500, 0.062500, 1.000000, 1.000000},
324
+
{0.187500, 0.062500, 1.000000, 1.000000}, {0.187500, 0.062500, 1.000000, 1.000000},
325
+
{0.312500, 0.062500, 1.000000, 1.000000}, {0.312500, 0.062500, 1.000000, 1.000000},
326
+
{0.437500, 0.062500, 1.000000, 1.000000}, {0.437500, 0.062500, 1.000000, 1.000000},
327
+
{0.562500, 0.062500, 1.000000, 1.000000}, {0.562500, 0.062500, 1.000000, 1.000000},
328
+
{0.687500, 0.062500, 1.000000, 1.000000}, {0.687500, 0.062500, 1.000000, 1.000000},
329
+
{0.812500, 0.062500, 1.000000, 1.000000}, {0.812500, 0.062500, 1.000000, 1.000000},
330
+
{0.937500, 0.062500, 1.000000, 1.000000}, {0.937500, 0.062500, 1.000000, 1.000000},
331
+
{0.062500, 0.187500, 1.000000, 1.000000}, {0.062500, 0.187500, 1.000000, 1.000000},
332
+
{0.187500, 0.187500, 1.000000, 1.000000}, {0.187500, 0.187500, 1.000000, 1.000000},
333
+
{0.312500, 0.187500, 1.000000, 1.000000}, {0.312500, 0.187500, 1.000000, 1.000000},
334
+
{0.437500, 0.187500, 1.000000, 1.000000}, {0.437500, 0.187500, 1.000000, 1.000000},
335
+
{0.562500, 0.187500, 1.000000, 1.000000}, {0.562500, 0.187500, 1.000000, 1.000000},
336
+
{0.687500, 0.187500, 1.000000, 1.000000}, {0.687500, 0.187500, 1.000000, 1.000000},
337
+
{0.812500, 0.187500, 1.000000, 1.000000}, {0.812500, 0.187500, 1.000000, 1.000000},
338
+
{0.937500, 0.187500, 1.000000, 1.000000}, {0.937500, 0.187500, 1.000000, 1.000000},
339
+
{0.062500, 0.312500, 1.000000, 1.000000}, {0.062500, 0.312500, 1.000000, 1.000000},
340
+
{0.187500, 0.312500, 1.000000, 1.000000}, {0.187500, 0.312500, 1.000000, 1.000000},
341
+
{0.312500, 0.312500, 1.000000, 1.000000}, {0.312500, 0.312500, 1.000000, 1.000000},
342
+
{0.437500, 0.312500, 1.000000, 1.000000}, {0.437500, 0.312500, 1.000000, 1.000000},
343
+
{0.562500, 0.312500, 1.000000, 1.000000}, {0.562500, 0.312500, 1.000000, 1.000000},
344
+
{0.687500, 0.312500, 1.000000, 1.000000}, {0.687500, 0.312500, 1.000000, 1.000000},
345
+
{0.812500, 0.312500, 1.000000, 1.000000}, {0.812500, 0.312500, 1.000000, 1.000000},
346
+
{0.937500, 0.312500, 1.000000, 1.000000}, {0.937500, 0.312500, 1.000000, 1.000000},
347
+
{0.062500, 0.437500, 1.000000, 1.000000}, {0.062500, 0.437500, 1.000000, 1.000000},
348
+
{0.187500, 0.437500, 1.000000, 1.000000}, {0.187500, 0.437500, 1.000000, 1.000000},
349
+
{0.312500, 0.437500, 1.000000, 1.000000}, {0.312500, 0.437500, 1.000000, 1.000000},
350
+
{0.437500, 0.437500, 1.000000, 1.000000}, {0.437500, 0.437500, 1.000000, 1.000000},
351
+
{0.562500, 0.437500, 1.000000, 1.000000}, {0.562500, 0.437500, 1.000000, 1.000000},
352
+
{0.687500, 0.437500, 1.000000, 1.000000}, {0.687500, 0.437500, 1.000000, 1.000000},
353
+
{0.812500, 0.437500, 1.000000, 1.000000}, {0.812500, 0.437500, 1.000000, 1.000000},
354
+
{0.937500, 0.437500, 1.000000, 1.000000}, {0.937500, 0.437500, 1.000000, 1.000000},
355
+
{0.062500, 0.562500, 1.000000, 1.000000}, {0.062500, 0.562500, 1.000000, 1.000000},
356
+
{0.187500, 0.562500, 1.000000, 1.000000}, {0.187500, 0.562500, 1.000000, 1.000000},
357
+
{0.312500, 0.562500, 1.000000, 1.000000}, {0.312500, 0.562500, 1.000000, 1.000000},
358
+
{0.437500, 0.562500, 1.000000, 1.000000}, {0.437500, 0.562500, 1.000000, 1.000000},
359
+
{0.562500, 0.562500, 1.000000, 1.000000}, {0.562500, 0.562500, 1.000000, 1.000000},
360
+
{0.687500, 0.562500, 1.000000, 1.000000}, {0.687500, 0.562500, 1.000000, 1.000000},
361
+
{0.812500, 0.562500, 1.000000, 1.000000}, {0.812500, 0.562500, 1.000000, 1.000000},
362
+
{0.937500, 0.562500, 1.000000, 1.000000}, {0.937500, 0.562500, 1.000000, 1.000000},
363
+
{0.062500, 0.687500, 1.000000, 1.000000}, {0.062500, 0.687500, 1.000000, 1.000000},
364
+
{0.187500, 0.687500, 1.000000, 1.000000}, {0.187500, 0.687500, 1.000000, 1.000000},
365
+
{0.312500, 0.687500, 1.000000, 1.000000}, {0.312500, 0.687500, 1.000000, 1.000000},
366
+
{0.437500, 0.687500, 1.000000, 1.000000}, {0.437500, 0.687500, 1.000000, 1.000000},
367
+
{0.562500, 0.687500, 1.000000, 1.000000}, {0.562500, 0.687500, 1.000000, 1.000000},
368
+
{0.687500, 0.687500, 1.000000, 1.000000}, {0.687500, 0.687500, 1.000000, 1.000000},
369
+
{0.812500, 0.687500, 1.000000, 1.000000}, {0.812500, 0.687500, 1.000000, 1.000000},
370
+
{0.937500, 0.687500, 1.000000, 1.000000}, {0.937500, 0.687500, 1.000000, 1.000000},
371
+
{0.062500, 0.812500, 1.000000, 1.000000}, {0.062500, 0.812500, 1.000000, 1.000000},
372
+
{0.187500, 0.812500, 1.000000, 1.000000}, {0.187500, 0.812500, 1.000000, 1.000000},
373
+
{0.312500, 0.812500, 1.000000, 1.000000}, {0.312500, 0.812500, 1.000000, 1.000000},
374
+
{0.437500, 0.812500, 1.000000, 1.000000}, {0.437500, 0.812500, 1.000000, 1.000000},
375
+
{0.562500, 0.812500, 1.000000, 1.000000}, {0.562500, 0.812500, 1.000000, 1.000000},
376
+
{0.687500, 0.812500, 1.000000, 1.000000}, {0.687500, 0.812500, 1.000000, 1.000000},
377
+
{0.812500, 0.812500, 1.000000, 1.000000}, {0.812500, 0.812500, 1.000000, 1.000000},
378
+
{0.937500, 0.812500, 1.000000, 1.000000}, {0.937500, 0.812500, 1.000000, 1.000000},
379
+
{0.062500, 0.937500, 1.000000, 1.000000}, {0.062500, 0.937500, 1.000000, 1.000000},
380
+
{0.187500, 0.937500, 1.000000, 1.000000}, {0.187500, 0.937500, 1.000000, 1.000000},
381
+
{0.312500, 0.937500, 1.000000, 1.000000}, {0.312500, 0.937500, 1.000000, 1.000000},
382
+
{0.437500, 0.937500, 1.000000, 1.000000}, {0.437500, 0.937500, 1.000000, 1.000000},
383
+
{0.562500, 0.937500, 1.000000, 1.000000}, {0.562500, 0.937500, 1.000000, 1.000000},
384
+
{0.687500, 0.937500, 1.000000, 1.000000}, {0.687500, 0.937500, 1.000000, 1.000000},
385
+
{0.812500, 0.937500, 1.000000, 1.000000}, {0.812500, 0.937500, 1.000000, 1.000000},
386
+
{0.937500, 0.937500, 1.000000, 1.000000}, {0.937500, 0.937500, 1.000000, 1.000000},
387
+
{0.125000, 0.125000, 1.000000, 1.000000}, {0.125000, 0.125000, 1.000000, 1.000000},
388
+
{0.125000, 0.125000, 1.000000, 1.000000}, {0.125000, 0.125000, 1.000000, 1.000000},
389
+
{0.125000, 0.125000, 1.000000, 1.000000}, {0.125000, 0.125000, 1.000000, 1.000000},
390
+
{0.375000, 0.125000, 1.000000, 1.000000}, {0.375000, 0.125000, 1.000000, 1.000000},
391
+
{0.375000, 0.125000, 1.000000, 1.000000}, {0.375000, 0.125000, 1.000000, 1.000000},
392
+
{0.375000, 0.125000, 1.000000, 1.000000}, {0.375000, 0.125000, 1.000000, 1.000000},
393
+
{0.625000, 0.125000, 1.000000, 1.000000}, {0.625000, 0.125000, 1.000000, 1.000000},
394
+
{0.625000, 0.125000, 1.000000, 1.000000}, {0.625000, 0.125000, 1.000000, 1.000000},
395
+
{0.625000, 0.125000, 1.000000, 1.000000}, {0.625000, 0.125000, 1.000000, 1.000000},
396
+
{0.875000, 0.125000, 1.000000, 1.000000}, {0.875000, 0.125000, 1.000000, 1.000000},
397
+
{0.875000, 0.125000, 1.000000, 1.000000}, {0.875000, 0.125000, 1.000000, 1.000000},
398
+
{0.875000, 0.125000, 1.000000, 1.000000}, {0.875000, 0.125000, 1.000000, 1.000000},
399
+
{0.125000, 0.375000, 1.000000, 1.000000}, {0.125000, 0.375000, 1.000000, 1.000000},
400
+
{0.125000, 0.375000, 1.000000, 1.000000}, {0.125000, 0.375000, 1.000000, 1.000000},
401
+
{0.125000, 0.375000, 1.000000, 1.000000}, {0.125000, 0.375000, 1.000000, 1.000000},
402
+
{0.375000, 0.375000, 1.000000, 1.000000}, {0.375000, 0.375000, 1.000000, 1.000000},
403
+
{0.375000, 0.375000, 1.000000, 1.000000}, {0.375000, 0.375000, 1.000000, 1.000000},
404
+
{0.375000, 0.375000, 1.000000, 1.000000}, {0.375000, 0.375000, 1.000000, 1.000000},
405
+
{0.625000, 0.375000, 1.000000, 1.000000}, {0.625000, 0.375000, 1.000000, 1.000000},
406
+
{0.625000, 0.375000, 1.000000, 1.000000}, {0.625000, 0.375000, 1.000000, 1.000000},
407
+
{0.625000, 0.375000, 1.000000, 1.000000}, {0.625000, 0.375000, 1.000000, 1.000000},
408
+
{0.875000, 0.375000, 1.000000, 1.000000}, {0.875000, 0.375000, 1.000000, 1.000000},
409
+
{0.875000, 0.375000, 1.000000, 1.000000}, {0.875000, 0.375000, 1.000000, 1.000000},
410
+
{0.875000, 0.375000, 1.000000, 1.000000}, {0.875000, 0.375000, 1.000000, 1.000000},
411
+
{0.125000, 0.625000, 1.000000, 1.000000}, {0.125000, 0.625000, 1.000000, 1.000000},
412
+
{0.125000, 0.625000, 1.000000, 1.000000}, {0.125000, 0.625000, 1.000000, 1.000000},
413
+
{0.125000, 0.625000, 1.000000, 1.000000}, {0.125000, 0.625000, 1.000000, 1.000000},
414
+
{0.375000, 0.625000, 1.000000, 1.000000}, {0.375000, 0.625000, 1.000000, 1.000000},
415
+
{0.375000, 0.625000, 1.000000, 1.000000}, {0.375000, 0.625000, 1.000000, 1.000000},
416
+
{0.375000, 0.625000, 1.000000, 1.000000}, {0.375000, 0.625000, 1.000000, 1.000000},
417
+
{0.625000, 0.625000, 1.000000, 1.000000}, {0.625000, 0.625000, 1.000000, 1.000000},
418
+
{0.625000, 0.625000, 1.000000, 1.000000}, {0.625000, 0.625000, 1.000000, 1.000000},
419
+
{0.625000, 0.625000, 1.000000, 1.000000}, {0.625000, 0.625000, 1.000000, 1.000000},
420
+
{0.875000, 0.625000, 1.000000, 1.000000}, {0.875000, 0.625000, 1.000000, 1.000000},
421
+
{0.875000, 0.625000, 1.000000, 1.000000}, {0.875000, 0.625000, 1.000000, 1.000000},
422
+
{0.875000, 0.625000, 1.000000, 1.000000}, {0.875000, 0.625000, 1.000000, 1.000000},
423
+
{0.125000, 0.875000, 1.000000, 1.000000}, {0.125000, 0.875000, 1.000000, 1.000000},
424
+
{0.125000, 0.875000, 1.000000, 1.000000}, {0.125000, 0.875000, 1.000000, 1.000000},
425
+
{0.125000, 0.875000, 1.000000, 1.000000}, {0.125000, 0.875000, 1.000000, 1.000000},
426
+
{0.375000, 0.875000, 1.000000, 1.000000}, {0.375000, 0.875000, 1.000000, 1.000000},
427
+
{0.375000, 0.875000, 1.000000, 1.000000}, {0.375000, 0.875000, 1.000000, 1.000000},
428
+
{0.375000, 0.875000, 1.000000, 1.000000}, {0.375000, 0.875000, 1.000000, 1.000000},
429
+
{0.625000, 0.875000, 1.000000, 1.000000}, {0.625000, 0.875000, 1.000000, 1.000000},
430
+
{0.625000, 0.875000, 1.000000, 1.000000}, {0.625000, 0.875000, 1.000000, 1.000000},
431
+
{0.625000, 0.875000, 1.000000, 1.000000}, {0.625000, 0.875000, 1.000000, 1.000000},
432
+
{0.875000, 0.875000, 1.000000, 1.000000}, {0.875000, 0.875000, 1.000000, 1.000000},
433
+
{0.875000, 0.875000, 1.000000, 1.000000}, {0.875000, 0.875000, 1.000000, 1.000000},
434
+
{0.875000, 0.875000, 1.000000, 1.000000}, {0.875000, 0.875000, 1.000000, 1.000000}};
435
+
436
+
static Hand2D
437
+
runKeypointEstimator(struct ht_view *htv, cv::Mat img)
438
+
{
439
+
const size_t lix = 224;
440
+
const size_t liy = 224;
441
+
cv::Mat planes[3];
442
+
443
+
444
+
uint8_t combined_planes[224 * 224 * 3];
445
+
planarize(img, combined_planes);
446
+
447
+
// Normalize - supposedly, the keypoint estimator wants keypoints in [0,1]
448
+
float real_thing[lix * liy * 3] = {0};
449
+
for (size_t i = 0; i < lix * liy * 3; i++) {
450
+
real_thing[i] = (float)combined_planes[i] / 255.0;
451
+
}
452
+
453
+
const OrtApi *g_ort = htv->htd->ort_api;
454
+
struct ModelInfo *model = &htv->keypoint_model;
455
+
456
+
457
+
OrtValue *input_tensor = nullptr;
458
+
459
+
460
+
ORT_CHECK(g_ort, g_ort->CreateTensorWithDataAsOrtValue(model->memoryInfo, real_thing, model->input_size_bytes,
461
+
model->input_shape.data(), model->input_shape.size(),
462
+
ONNX_TENSOR_ELEMENT_DATA_TYPE_FLOAT, &input_tensor));
463
+
464
+
// Cargo-culted
465
+
assert(input_tensor != nullptr);
466
+
int is_tensor;
467
+
ORT_CHECK(g_ort, g_ort->IsTensor(input_tensor, &is_tensor));
468
+
assert(is_tensor);
469
+
470
+
const char *output_names[] = {"Identity", "Identity_2", "Identity_2"};
471
+
472
+
// If any of these are non-NULL, ONNX will explode and won't tell you why! Be extremely paranoid!
473
+
OrtValue *output_tensor[3] = {nullptr, nullptr, nullptr};
474
+
ORT_CHECK(g_ort, g_ort->Run(model->session, nullptr, model->input_names.data(), &input_tensor, 1, output_names,
475
+
3, output_tensor));
476
+
477
+
ORT_CHECK(g_ort, g_ort->IsTensor(output_tensor[0], &is_tensor));
478
+
assert(is_tensor);
479
+
480
+
float *landmarks = nullptr;
481
+
482
+
// Should give a pointer to data that is freed on g_ort->ReleaseValue(output_tensor[0]);.
483
+
ORT_CHECK(g_ort, g_ort->GetTensorMutableData(output_tensor[0], (void **)&landmarks));
484
+
485
+
int stride = 3;
486
+
Hand2D dumb;
487
+
for (size_t i = 0; i < 21; i++) {
488
+
int rt = i * stride;
489
+
float x = landmarks[rt];
490
+
float y = landmarks[rt + 1];
491
+
float z = landmarks[rt + 2];
492
+
dumb.kps[i].x = x;
493
+
dumb.kps[i].y = y;
494
+
dumb.kps[i].z = z;
495
+
}
496
+
497
+
// We have to get to here, or else we leak a whole lot! If you need to return early, make this into a goto!
498
+
g_ort->ReleaseValue(output_tensor[0]);
499
+
g_ort->ReleaseValue(output_tensor[1]);
500
+
g_ort->ReleaseValue(output_tensor[2]);
501
+
g_ort->ReleaseValue(input_tensor);
502
+
return dumb;
503
+
}
504
+
505
+
506
+
507
+
static std::vector<Palm7KP>
508
+
runHandDetector(struct ht_view *htv, cv::Mat &raw_input)
509
+
{
510
+
const OrtApi *g_ort = htv->htd->ort_api;
511
+
512
+
cv::Mat img;
513
+
514
+
const int hd_size = 128;
515
+
const int inputTensorSize = hd_size * hd_size * 3 * sizeof(float);
516
+
517
+
cv::Matx23f back_from_blackbar = blackbar(raw_input, img, {hd_size, hd_size});
518
+
519
+
float scale_factor = back_from_blackbar(0, 0); // 960/128
520
+
assert(img.isContinuous());
521
+
float mean = 128.0f;
522
+
float std = 128.0f;
523
+
float real_thing[hd_size * hd_size * 3];
524
+
525
+
526
+
if (htv->htd->runtime_config.palm_detection_use_mediapipe) {
527
+
uint8_t combined_planes[hd_size * hd_size * 3] = {0};
528
+
planarize(img, combined_planes);
529
+
for (size_t i = 0; i < hd_size * hd_size * 3; i++) {
530
+
float val = (float)combined_planes[i];
531
+
real_thing[i] = (val - mean) / std;
532
+
}
533
+
// Hope it was worth it...
534
+
} else {
535
+
536
+
assert(img.isContinuous());
537
+
538
+
for (size_t i = 0; i < hd_size * hd_size * 3; i++) {
539
+
int val = img.data[i];
540
+
541
+
real_thing[i] = (val - mean) / std;
542
+
}
543
+
}
544
+
545
+
// Convenience
546
+
struct ModelInfo *model_hd = &htv->detection_model;
547
+
548
+
// If this is non-NULL, ONNX will explode and won't tell you why!
549
+
OrtValue *input_tensor = nullptr;
550
+
551
+
ORT_CHECK(g_ort, g_ort->CreateTensorWithDataAsOrtValue(
552
+
model_hd->memoryInfo, real_thing, inputTensorSize, model_hd->input_shape.data(),
553
+
model_hd->input_shape.size(), ONNX_TENSOR_ELEMENT_DATA_TYPE_FLOAT, &input_tensor));
554
+
555
+
// Cargo-culted
556
+
assert(input_tensor != nullptr);
557
+
int is_tensor;
558
+
ORT_CHECK(g_ort, g_ort->IsTensor(input_tensor, &is_tensor));
559
+
assert(is_tensor);
560
+
561
+
// If any of these are non-NULL, ONNX will explode and won't tell you why! Be extremely paranoid!
562
+
OrtValue *output_tensor[2] = {nullptr, nullptr};
563
+
ORT_CHECK(g_ort, g_ort->Run(model_hd->session, nullptr, model_hd->input_names.data(),
564
+
(const OrtValue *const *)&input_tensor, model_hd->input_names.size(),
565
+
model_hd->output_names.data(), model_hd->output_names.size(), output_tensor));
566
+
567
+
ORT_CHECK(g_ort, g_ort->IsTensor(output_tensor[0], &is_tensor));
568
+
assert(is_tensor);
569
+
570
+
float *classificators = nullptr;
571
+
float *regressors = nullptr;
572
+
573
+
ORT_CHECK(g_ort,
574
+
g_ort->GetTensorMutableData(output_tensor[0], (void **)&classificators)); // Totally won't segfault!
575
+
ORT_CHECK(g_ort, g_ort->GetTensorMutableData(output_tensor[1], (void **)®ressors));
576
+
// We need to free these!
577
+
578
+
float *rg = regressors; // shorter name
579
+
580
+
std::vector<NMSPalm> detections;
581
+
int count = 0;
582
+
size_t i = 0;
583
+
584
+
std::vector<Palm7KP> output;
585
+
std::vector<NMSPalm> nms_palms;
586
+
587
+
588
+
for (std::vector<std::vector<float>>::iterator it = anchor.begin(); it != anchor.end(); ++it, ++i) {
589
+
std::vector<float>::iterator anchorData = it->begin();
590
+
591
+
float score0 = classificators[i];
592
+
float score = 1.0 / (1.0 + exp(-score0));
593
+
594
+
if (score > 0.6) {
595
+
// Boundary box.
596
+
NMSPalm det;
597
+
598
+
float anchx = *(anchorData + 0) * 128;
599
+
float anchy = *(anchorData + 1) * 128;
600
+
601
+
float shiftx = regressors[i * 18];
602
+
float shifty = regressors[i * 18 + 1];
603
+
604
+
float w = regressors[i * 18 + 2];
605
+
float h = regressors[i * 18 + 3];
606
+
607
+
608
+
609
+
float cx = shiftx + anchx;
610
+
float cy = shifty + anchy;
611
+
612
+
struct xrt_vec2 *kps = det.keypoints;
613
+
614
+
kps[0] = {rg[i * 18 + 4], rg[i * 18 + 5]};
615
+
kps[1] = {rg[i * 18 + 6], rg[i * 18 + 7]};
616
+
kps[2] = {rg[i * 18 + 8], rg[i * 18 + 9]};
617
+
kps[3] = {rg[i * 18 + 10], rg[i * 18 + 11]};
618
+
kps[4] = {rg[i * 18 + 12], rg[i * 18 + 13]};
619
+
kps[5] = {rg[i * 18 + 14], rg[i * 18 + 15]};
620
+
kps[6] = {rg[i * 18 + 16], rg[i * 18 + 17]};
621
+
622
+
623
+
for (int i = 0; i < 7; i++) {
624
+
struct xrt_vec2 *b = &kps[i];
625
+
b->x += anchx;
626
+
b->y += anchy;
627
+
}
628
+
629
+
det.bbox.w = w;
630
+
det.bbox.h = h;
631
+
det.bbox.cx = cx;
632
+
det.bbox.cy = cy;
633
+
det.confidence = score;
634
+
detections.push_back(det);
635
+
count++;
636
+
637
+
638
+
639
+
int square = fmax(w, h);
640
+
641
+
square = square / scale_factor;
642
+
}
643
+
}
644
+
645
+
if (count == 0) {
646
+
goto cleanup;
647
+
}
648
+
649
+
nms_palms = filterBoxesWeightedAvg(detections);
650
+
651
+
652
+
653
+
for (NMSPalm cooler : nms_palms) {
654
+
655
+
// Display box
656
+
struct xrt_vec2 tl = {cooler.bbox.cx - cooler.bbox.w / 2, cooler.bbox.cy - cooler.bbox.h / 2};
657
+
struct xrt_vec2 bob = transformVecBy2x3(tl, back_from_blackbar);
658
+
float sz = cooler.bbox.w / scale_factor;
659
+
660
+
if (htv->htd->debug_scribble) {
661
+
cv::rectangle(htv->debug_out_to_this, {(int)bob.x, (int)bob.y, (int)sz, (int)sz}, {0, 0, 255},
662
+
5);
663
+
}
664
+
665
+
666
+
Palm7KP this_element;
667
+
668
+
for (int i = 0; i < 7; i++) {
669
+
struct xrt_vec2 b = cooler.keypoints[i];
670
+
this_element.kps[i] = transformVecBy2x3(b, back_from_blackbar);
671
+
if (htv->htd->debug_scribble) {
672
+
handDot(htv->debug_out_to_this, this_element.kps[i], 5, ((float)i) * (360.0f / 7.0f),
673
+
2);
674
+
}
675
+
}
676
+
677
+
output.push_back(this_element);
678
+
}
679
+
680
+
cleanup:
681
+
g_ort->ReleaseValue(output_tensor[0]);
682
+
g_ort->ReleaseValue(output_tensor[1]);
683
+
g_ort->ReleaseValue(input_tensor);
684
+
return output;
685
+
}
686
+
687
+
688
+
static void
689
+
addSlug(struct ht_device *htd, const char *suffix, char *out)
690
+
{
691
+
strcpy(out, htd->runtime_config.model_slug);
692
+
strcat(out, suffix);
693
+
}
694
+
695
+
static void
696
+
initKeypointEstimator(struct ht_device *htd, ht_view *htv)
697
+
{
698
+
struct ModelInfo *model_ke = &htv->keypoint_model;
699
+
const OrtApi *g_ort = htd->ort_api;
700
+
OrtSessionOptions *opts = nullptr;
701
+
702
+
ORT_CHECK(g_ort, g_ort->CreateSessionOptions(&opts));
703
+
704
+
ORT_CHECK(g_ort, g_ort->SetSessionGraphOptimizationLevel(opts, ORT_ENABLE_ALL));
705
+
ORT_CHECK(g_ort, g_ort->SetIntraOpNumThreads(opts, 1));
706
+
707
+
char modelLocation[1024];
708
+
if (htd->runtime_config.keypoint_estimation_use_mediapipe) {
709
+
addSlug(htd, "hand_landmark_MEDIAPIPE.onnx", modelLocation);
710
+
} else {
711
+
addSlug(htd, "hand_landmark_COLLABORA.onnx", modelLocation);
712
+
}
713
+
ORT_CHECK(g_ort, g_ort->CreateSession(htd->ort_env, modelLocation, opts, &model_ke->session));
714
+
g_ort->ReleaseSessionOptions(opts);
715
+
716
+
model_ke->input_shape.push_back(1);
717
+
model_ke->input_shape.push_back(3);
718
+
model_ke->input_shape.push_back(224);
719
+
model_ke->input_shape.push_back(224);
720
+
721
+
model_ke->input_names.push_back("input_1");
722
+
723
+
model_ke->output_names.push_back("Identity");
724
+
model_ke->output_names.push_back("Identity_1");
725
+
model_ke->output_names.push_back("Identity_2");
726
+
727
+
model_ke->input_size_bytes = 224 * 224 * 3 * sizeof(float);
728
+
729
+
ORT_CHECK(g_ort, g_ort->CreateCpuMemoryInfo(OrtArenaAllocator, OrtMemTypeDefault, &model_ke->memoryInfo));
730
+
731
+
htv->run_keypoint_model = runKeypointEstimator;
732
+
}
733
+
734
+
static void
735
+
initHandDetector(struct ht_device *htd, ht_view *htv)
736
+
{
737
+
struct ModelInfo *model_hd = &htv->detection_model;
738
+
memset(model_hd, 0, sizeof(struct ModelInfo));
739
+
740
+
const OrtApi *g_ort = htd->ort_api;
741
+
OrtSessionOptions *opts = nullptr;
742
+
ORT_CHECK(g_ort, g_ort->CreateSessionOptions(&opts));
743
+
744
+
ORT_CHECK(g_ort, g_ort->SetSessionGraphOptimizationLevel(opts, ORT_ENABLE_ALL));
745
+
ORT_CHECK(g_ort, g_ort->SetIntraOpNumThreads(opts, 1));
746
+
747
+
char modelLocation[1024];
748
+
749
+
// Hard-coded. Even though you can use the ONNX runtime's API to dynamically figure these out, that doesn't make
750
+
// any sense because these don't change between runs, and if you are swapping models you have to do much more
751
+
// than just change the input/output names.
752
+
if (htd->runtime_config.palm_detection_use_mediapipe) {
753
+
addSlug(htd, "palm_detection_MEDIAPIPE.onnx", modelLocation);
754
+
model_hd->input_shape.push_back(1);
755
+
model_hd->input_shape.push_back(3);
756
+
model_hd->input_shape.push_back(128);
757
+
model_hd->input_shape.push_back(128);
758
+
759
+
model_hd->input_names.push_back("input");
760
+
} else {
761
+
addSlug(htd, "palm_detection_COLLABORA.onnx", modelLocation);
762
+
763
+
model_hd->input_shape.push_back(1);
764
+
model_hd->input_shape.push_back(128);
765
+
model_hd->input_shape.push_back(128);
766
+
model_hd->input_shape.push_back(3);
767
+
768
+
model_hd->input_names.push_back("input:0");
769
+
}
770
+
771
+
ORT_CHECK(g_ort, g_ort->CreateSession(htd->ort_env, modelLocation, opts, &model_hd->session));
772
+
g_ort->ReleaseSessionOptions(opts);
773
+
774
+
775
+
776
+
model_hd->output_names.push_back("classificators");
777
+
model_hd->output_names.push_back("regressors");
778
+
779
+
ORT_CHECK(g_ort, g_ort->CreateCpuMemoryInfo(OrtArenaAllocator, OrtMemTypeDefault, &model_hd->memoryInfo));
780
+
781
+
htv->run_detection_model = runHandDetector;
782
+
}
783
+
784
+
785
+
static void
786
+
initOnnx(struct ht_device *htd)
787
+
{
788
+
htd->ort_api = OrtGetApiBase()->GetApi(ORT_API_VERSION);
789
+
ORT_CHECK(htd->ort_api, htd->ort_api->CreateEnv(ORT_LOGGING_LEVEL_WARNING, "moses", &htd->ort_env));
790
+
791
+
initHandDetector(htd, &htd->views[0]);
792
+
initHandDetector(htd, &htd->views[1]);
793
+
794
+
initKeypointEstimator(htd, &htd->views[0]);
795
+
initKeypointEstimator(htd, &htd->views[1]);
796
+
}
797
+
798
+
static void
799
+
destroyModelInfo(struct ht_device *htd, ModelInfo *info)
800
+
{
801
+
const OrtApi *g_ort = htd->ort_api;
802
+
g_ort->ReleaseSession(info->session);
803
+
g_ort->ReleaseMemoryInfo(info->memoryInfo);
804
+
// Same deal as in ht_device - I'm mixing C and C++ idioms, so sometimes it's easier to just manually call their
805
+
// destructors instead of figuring out some way to convince C++ to call them implicitly.
806
+
info->output_names.~vector();
807
+
info->input_names.~vector();
808
+
info->input_shape.~vector();
809
+
}
810
+
811
+
void
812
+
destroyOnnx(struct ht_device *htd)
813
+
{
814
+
destroyModelInfo(htd, &htd->views[0].keypoint_model);
815
+
destroyModelInfo(htd, &htd->views[1].keypoint_model);
816
+
destroyModelInfo(htd, &htd->views[0].detection_model);
817
+
destroyModelInfo(htd, &htd->views[1].detection_model);
818
+
htd->ort_api->ReleaseEnv(htd->ort_env);
819
+
}
+140
src/xrt/drivers/ht/ht_nms.hpp
+140
src/xrt/drivers/ht/ht_nms.hpp
···
1
+
// Copyright 2021, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Code to deal with bounding boxes for camera-based hand-tracking.
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @author Marcus Edel <marcus.edel@collabora.com>
8
+
* @ingroup drv_ht
9
+
*/
10
+
11
+
#pragma once
12
+
#include <math.h>
13
+
#include <stdio.h>
14
+
#include <vector>
15
+
#include <xrt/xrt_defines.h>
16
+
#include "ht_driver.hpp"
17
+
18
+
struct Box
19
+
{
20
+
float cx;
21
+
float cy;
22
+
float w;
23
+
float h;
24
+
};
25
+
26
+
struct NMSPalm
27
+
{
28
+
Box bbox;
29
+
xrt_vec2 keypoints[7];
30
+
float confidence;
31
+
};
32
+
33
+
static float
34
+
overlap(float x1, float w1, float x2, float w2)
35
+
{
36
+
float l1 = x1 - w1 / 2;
37
+
float l2 = x2 - w2 / 2;
38
+
float left = l1 > l2 ? l1 : l2;
39
+
40
+
float r1 = x1 + w1 / 2;
41
+
float r2 = x2 + w2 / 2;
42
+
float right = r1 < r2 ? r1 : r2;
43
+
44
+
return right - left;
45
+
}
46
+
47
+
static float
48
+
boxIntersection(const Box &a, const Box &b)
49
+
{
50
+
float w = overlap(a.cx, a.w, b.cx, b.w);
51
+
float h = overlap(a.cy, a.h, b.cy, b.h);
52
+
53
+
if (w < 0 || h < 0)
54
+
return 0;
55
+
56
+
return w * h;
57
+
}
58
+
59
+
static float
60
+
boxUnion(const Box &a, const Box &b)
61
+
{
62
+
return a.w * a.h + b.w * b.h - boxIntersection(a, b);
63
+
}
64
+
65
+
static float
66
+
boxIOU(const Box &a, const Box &b)
67
+
{
68
+
return boxIntersection(a, b) / boxUnion(a, b);
69
+
}
70
+
71
+
static NMSPalm
72
+
weightedAvgBoxes(std::vector<NMSPalm> &detections)
73
+
{
74
+
float weight = 0.0f;
75
+
float cx = 0.0f;
76
+
float cy = 0.0f;
77
+
float size = 0.0f;
78
+
NMSPalm out = {};
79
+
80
+
for (NMSPalm &detection : detections) {
81
+
weight += detection.confidence;
82
+
cx += detection.bbox.cx * detection.confidence;
83
+
cy += detection.bbox.cy * detection.confidence;
84
+
size += detection.bbox.w * .5 * detection.confidence;
85
+
size += detection.bbox.h * .5 * detection.confidence;
86
+
87
+
for (int i = 0; i < 7; i++) {
88
+
out.keypoints[i].x += detection.keypoints[i].x * detection.confidence;
89
+
out.keypoints[i].y += detection.keypoints[i].y * detection.confidence;
90
+
}
91
+
}
92
+
cx /= weight;
93
+
cy /= weight;
94
+
size /= weight;
95
+
for (int i = 0; i < 7; i++) {
96
+
out.keypoints[i].x /= weight;
97
+
out.keypoints[i].y /= weight;
98
+
}
99
+
out.bbox.cx = cx;
100
+
out.bbox.cy = cy;
101
+
out.bbox.w = size;
102
+
out.bbox.h = size;
103
+
return out;
104
+
}
105
+
106
+
static std::vector<NMSPalm>
107
+
filterBoxesWeightedAvg(std::vector<NMSPalm> &detections)
108
+
{
109
+
std::vector<std::vector<NMSPalm>> overlaps;
110
+
std::vector<NMSPalm> outs;
111
+
112
+
113
+
// U_LOG_D("\n\nStarting filtering boxes. There are %zu boxes to look at.\n", detections.size());
114
+
for (NMSPalm &detection : detections) {
115
+
// U_LOG_D("Starting looking at one detection\n");
116
+
bool foundAHome = false;
117
+
for (size_t i = 0; i < outs.size(); i++) {
118
+
float iou = boxIOU(outs[i].bbox, detection.bbox);
119
+
// U_LOG_D("IOU is %f\n", iou);
120
+
// U_LOG_D("Outs box is %f %f %f %f", outs[i].bbox.cx, outs[i].bbox.cy, outs[i].bbox.w,
121
+
// outs[i].bbox.h)
122
+
if (iou > 0.1f) {
123
+
// This one intersects with the whole thing
124
+
overlaps[i].push_back(detection);
125
+
outs[i] = weightedAvgBoxes(overlaps[i]);
126
+
foundAHome = true;
127
+
break;
128
+
}
129
+
}
130
+
if (!foundAHome) {
131
+
// U_LOG_D("No home\n");
132
+
overlaps.push_back({detection});
133
+
outs.push_back({detection});
134
+
} else {
135
+
// U_LOG_D("Found a home!\n");
136
+
}
137
+
}
138
+
// U_LOG_D("Sizeeeeeeeeeeeeeeeeeeeee is %zu\n", outs.size());
139
+
return outs;
140
+
}
-74
src/xrt/drivers/ht/ht_prober.c
-74
src/xrt/drivers/ht/ht_prober.c
···
1
-
// Copyright 2020, Collabora, Ltd.
2
-
// SPDX-License-Identifier: BSL-1.0
3
-
/*!
4
-
* @file
5
-
* @brief Camera based hand tracking prober code.
6
-
* @author Christoph Haag <christoph.haag@collabora.com>
7
-
* @ingroup drv_ht
8
-
*/
9
-
10
-
11
-
#include "xrt/xrt_prober.h"
12
-
13
-
#include "util/u_misc.h"
14
-
15
-
#include "ht_interface.h"
16
-
#include "ht_driver.h"
17
-
18
-
/*!
19
-
* @implements xrt_auto_prober
20
-
*/
21
-
struct ht_prober
22
-
{
23
-
struct xrt_auto_prober base;
24
-
};
25
-
26
-
//! @private @memberof ht_prober
27
-
static inline struct ht_prober *
28
-
ht_prober(struct xrt_auto_prober *p)
29
-
{
30
-
return (struct ht_prober *)p;
31
-
}
32
-
33
-
//! @public @memberof ht_prober
34
-
static void
35
-
ht_prober_destroy(struct xrt_auto_prober *p)
36
-
{
37
-
struct ht_prober *htp = ht_prober(p);
38
-
39
-
free(htp);
40
-
}
41
-
42
-
//! @public @memberof ht_prober
43
-
static int
44
-
ht_prober_autoprobe(struct xrt_auto_prober *xap,
45
-
cJSON *attached_data,
46
-
bool no_hmds,
47
-
struct xrt_prober *xp,
48
-
struct xrt_device **out_xdevs)
49
-
{
50
-
struct xrt_device *xdev = ht_device_create(xap, attached_data, xp);
51
-
52
-
if (xdev == NULL) {
53
-
return 0;
54
-
}
55
-
56
-
xdev->orientation_tracking_supported = true;
57
-
xdev->position_tracking_supported = true;
58
-
xdev->hand_tracking_supported = true;
59
-
xdev->device_type = XRT_DEVICE_TYPE_HAND_TRACKER;
60
-
61
-
out_xdevs[0] = xdev;
62
-
return 1;
63
-
}
64
-
65
-
struct xrt_auto_prober *
66
-
ht_create_auto_prober()
67
-
{
68
-
struct ht_prober *htp = U_TYPED_CALLOC(struct ht_prober);
69
-
htp->base.name = "Camera Hand Tracking";
70
-
htp->base.destroy = ht_prober_destroy;
71
-
htp->base.lelo_dallas_autoprobe = ht_prober_autoprobe;
72
-
73
-
return &htp->base;
74
-
}
+90
src/xrt/drivers/ht/readme.md
+90
src/xrt/drivers/ht/readme.md
···
1
+
<!--
2
+
Copyright 2021, Collabora, Ltd.
3
+
Authors:
4
+
Moses Turner <moses@collabora.com>
5
+
SPDX-License-Identifier: BSL-1.0
6
+
-->
7
+
8
+
# What is this?
9
+
This is a driver to do optical hand tracking. The actual code mostly written by Moses Turner, with tons of help from Marcus Edel, Jakob Bornecrantz, Ryan Pavlik, and Christoph Haag. Jakob Bornecrantz and Marcus Edel are the main people who gathered training data for the initial Collabora models.
10
+
11
+
Currently, it works with the Valve Index. In the past, it was tested with a Luxonis 1090ffc, and in the future it should work fine with devices like the T265, Leap Motion Controller (w/ LeapUVC), or PS4/PS5 cam, should there be enough interest for any of those.
12
+
13
+
Under good lighting, I would say it's around as good as Oculus Quest 2's hand tracking. Not that I'm trying to make any claims; that's just what I honestly would tell somebody if they are wondering if it's worth testing out.
14
+
15
+
16
+
# How to get started
17
+
## Get dependencies
18
+
### Get OpenCV
19
+
Each distro has its own way to get OpenCV, and it can change at any time; there's no specific reason to trust this documentation over anything else.
20
+
21
+
Having said that, on Ubuntu, it would look something like
22
+
23
+
```
24
+
sudo apt install libopencv-dev libopencv-contrib-dev
25
+
```
26
+
27
+
Or you could build it from source, or get it from one of the other 1000s of package managers. Whatever floats your boat.
28
+
29
+
### Get ONNXRuntime
30
+
I followed the instructions here: https://onnxruntime.ai/docs/how-to/build/inferencing.html#linux
31
+
32
+
then had to do
33
+
```
34
+
cd build/Linux/RelWithDebInfo/
35
+
sudo make install
36
+
```
37
+
38
+
### Get the ML models
39
+
Make sure you have git-lfs installed, then run ./scripts/get-ht-models.sh. Should work fine.
40
+
41
+
## Building the driver
42
+
Once onnxruntime is installed, you should be able to build like normal with CMake or Meson.
43
+
44
+
If it properly found everything, - CMake should say
45
+
46
+
```
47
+
-- Found ONNXRUNTIME: /usr/local/include/onnxruntime
48
+
49
+
[...]
50
+
51
+
-- # DRIVER_HANDTRACKING: ON
52
+
```
53
+
54
+
and Meson should say
55
+
56
+
```
57
+
Run-time dependency libonnxruntime found: YES 1.8.2
58
+
59
+
[...]
60
+
61
+
Message: Configuration done!
62
+
Message: drivers: [...] handtracking, [...]
63
+
```
64
+
65
+
## Running the driver
66
+
Currently, it's only set up to work on Valve Index.
67
+
68
+
So, the two things you can do are
69
+
* Use the `survive` driver with both controllers off - It should automagically start hand tracking upon not finding any controllers.
70
+
* Use the `vive` driver with `VIVE_USE_HANDTRACKING=ON` and it should work the same as the survive driver.
71
+
72
+
You can see if the driver is working with `openxr-simple-playground`, StereoKit, or any other app you know of. Poke me (Moses) if you find any other cool hand-tracking apps; I'm always looking for more!
73
+
74
+
# Tips and tricks
75
+
76
+
This tracking likes to be in a bright, evenly-lit room with multiple light sources. Turn all the lights on, see if you can find any lamps. If the ML models can see well, the tracking quality can get surprisingly nice.
77
+
78
+
Sometimes, the tracking fails when it can see more than one hand. As the tracking gets better (we train better ML models and squash more bugs) this should happen less often or not at all. If it does, put one of your hands down, and it should resume tracking the remaining hand just fine.
79
+
80
+
# Future improvements
81
+
82
+
* Get more training data; train better ML models.
83
+
* Improve the tracking math
84
+
* Be smarter about keeping tracking lock on a hand
85
+
* Try predicting the next bounding box based on the estimated keypoints of the last few frames instead of blindly trusting the detection model, and not run the detection model *every single* frame.
86
+
* Instead of directly doing disparity on the observed keypoints, use a kinematic model of the hand and fit that to the 2D observations - this should get rid of a *lot* of jitter and make it look better to the end user if the ML models fail
87
+
* Make something that also works with non-stereo (mono, trinocular, or N cameras) camera setups
88
+
* Optionally run the ML models on GPU - currently, everything's CPU bound which could be dumb under some circumstances
89
+
* Write a lot of generic code so that you can run this on any stereo camera
90
+
* More advanced prediction/interpolation code that doesn't care at all about the input frame cadence. One-euro filters are pretty good about this, but we can get better!
+64
src/xrt/drivers/ht/templates/DiscardLastBuffer.hpp
+64
src/xrt/drivers/ht/templates/DiscardLastBuffer.hpp
···
1
+
// Copyright 2021, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Camera based hand tracking ringbuffer implementation.
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @ingroup drv_ht
8
+
*/
9
+
10
+
#pragma once
11
+
12
+
#include <stdio.h>
13
+
#include <stdlib.h>
14
+
#include <stdint.h>
15
+
#include <assert.h>
16
+
17
+
18
+
//| -4 | -3 | -2 | -1 | Top | Garbage |
19
+
// OR
20
+
//| -4 | -3 | -2 | -1 | Top | -7 | -6 | -5 |
21
+
22
+
23
+
24
+
template <typename T, int maxSize> struct DiscardLastBuffer
25
+
{
26
+
T internalBuffer[maxSize];
27
+
int topIdx = 0;
28
+
29
+
/* Put something at the top, overwrite whatever was at the back*/
30
+
void
31
+
push(const T inElement);
32
+
33
+
T *operator[](int inIndex);
34
+
};
35
+
36
+
template <typename T, int maxSize>
37
+
void
38
+
DiscardLastBuffer<T, maxSize>::push(const T inElement)
39
+
{
40
+
topIdx++;
41
+
if (topIdx == maxSize) {
42
+
topIdx = 0;
43
+
}
44
+
45
+
memcpy(&internalBuffer[topIdx], &inElement, sizeof(T));
46
+
}
47
+
48
+
template <typename T, int maxSize> T *DiscardLastBuffer<T, maxSize>::operator[](int inIndex)
49
+
{
50
+
assert(inIndex <= maxSize);
51
+
assert(inIndex >= 0);
52
+
53
+
int index = topIdx - inIndex;
54
+
if (index < 0) {
55
+
index = maxSize + index;
56
+
}
57
+
58
+
assert(index >= 0);
59
+
if (index > maxSize) {
60
+
assert(false);
61
+
}
62
+
63
+
return &internalBuffer[index];
64
+
}
+90
src/xrt/drivers/ht/templates/NaivePermutationSort.hpp
+90
src/xrt/drivers/ht/templates/NaivePermutationSort.hpp
···
1
+
// Copyright 2021, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Camera based hand tracking sorting implementation.
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @ingroup drv_ht
8
+
*/
9
+
10
+
#pragma once
11
+
12
+
#include <math.h>
13
+
#include <vector>
14
+
#include <algorithm>
15
+
#include <iostream>
16
+
// Other thing: sort by speed? like, if our thing must have suddenly changed directions, add to error?
17
+
// Easy enough to do using more complicated structs.
18
+
// Like a past thing with position, velocity and timestamp - present thing with position and timestamp.
19
+
20
+
// typedef bool booool;
21
+
22
+
struct psort_atom_t
23
+
{
24
+
size_t idx_1;
25
+
size_t idx_2;
26
+
float err;
27
+
};
28
+
29
+
30
+
bool
31
+
comp_err(psort_atom_t one, psort_atom_t two)
32
+
{
33
+
return (one.err < two.err);
34
+
}
35
+
36
+
37
+
template <typename Tp_1, typename Tp_2>
38
+
void
39
+
naive_sort_permutation_by_error(
40
+
// Inputs - shall be initialized with real data before calling. This function shall not modify them in any way.
41
+
std::vector<Tp_1> &in_1,
42
+
std::vector<Tp_2> &in_2,
43
+
44
+
// Outputs - shall be uninitialized. This function shall initialize them to the right size and fill them with the
45
+
// proper values.
46
+
std::vector<bool> &used_1,
47
+
std::vector<bool> &used_2,
48
+
std::vector<size_t> &out_indices_1,
49
+
std::vector<size_t> &out_indices_2,
50
+
std::vector<float> &out_errs,
51
+
52
+
float (*calc_error)(Tp_1 *one, Tp_2 *two))
53
+
{
54
+
used_1 = std::vector<bool>(in_1.size()); // silly? Unsure.
55
+
used_2 = std::vector<bool>(in_2.size());
56
+
57
+
size_t out_size = std::min(in_1.size(), in_2.size());
58
+
59
+
out_indices_1.reserve(out_size);
60
+
out_indices_2.reserve(out_size);
61
+
62
+
std::vector<psort_atom_t> associations;
63
+
64
+
for (size_t idx_1 = 0; idx_1 < in_1.size(); idx_1++) {
65
+
for (size_t idx_2 = 0; idx_2 < in_2.size(); idx_2++) {
66
+
float err = calc_error(&in_1[idx_1], &in_2[idx_2]);
67
+
if (err > 0.0f) {
68
+
// Negative error means the error calculator thought there was something so bad with
69
+
// these that they shouldn't be considered at all.
70
+
associations.push_back({idx_1, idx_2, err});
71
+
}
72
+
}
73
+
}
74
+
75
+
std::sort(associations.begin(), associations.end(), comp_err);
76
+
77
+
for (size_t i = 0; i < associations.size(); i++) {
78
+
psort_atom_t chonk = associations[i];
79
+
if (used_1[chonk.idx_1] || used_2[chonk.idx_2]) {
80
+
continue;
81
+
}
82
+
used_1[chonk.idx_1] = true;
83
+
used_2[chonk.idx_2] = true;
84
+
85
+
out_indices_1.push_back(chonk.idx_1);
86
+
out_indices_2.push_back(chonk.idx_2);
87
+
88
+
out_errs.push_back(chonk.err);
89
+
}
90
+
}
+10
-5
src/xrt/drivers/meson.build
+10
-5
src/xrt/drivers/meson.build
···
87
87
lib_drv_ht = static_library(
88
88
'drv_ht',
89
89
files(
90
-
'ht/ht_driver.c',
91
-
'ht/ht_driver.h',
90
+
'ht/ht_driver.cpp',
91
+
'ht/ht_driver.hpp',
92
92
'ht/ht_interface.h',
93
-
'ht/ht_prober.c',
93
+
'ht/ht_models.hpp',
94
+
'ht/ht_hand_math.hpp',
95
+
'ht/ht_image_math.hpp',
96
+
'ht/ht_nms.hpp',
97
+
'ht/templates/DiscardLastBuffer.hpp',
98
+
'ht/templates/NaivePermutationSort.hpp',
94
99
),
95
-
include_directories: xrt_include,
96
-
dependencies: [aux],
100
+
include_directories: [xrt_include, cjson_include],
101
+
dependencies: [aux, opencv, onnxruntime, eigen3],
97
102
build_by_default: 'handtracking' in drivers,
98
103
)
99
104