The open source OpenXR runtime
d/rs: apply offset to t265 pose, d/ns: fix up some minor things, load realsense offset from json file
+255
-48
+3
src/xrt/drivers/north_star/exampleconfigs/v1_deckx_50cm.json.license
+3
src/xrt/drivers/north_star/exampleconfigs/v1_deckx_50cm.json.license
+96
src/xrt/drivers/north_star/exampleconfigs/v2_deckx_50cm.json
+96
src/xrt/drivers/north_star/exampleconfigs/v2_deckx_50cm.json
···
1
+
{
2
+
"baseline": 64.0,
3
+
"left_fov_radians_left": -0.8,
4
+
"left_fov_radians_right": 0.8,
5
+
"left_fov_radians_up": 0.8,
6
+
"left_fov_radians_down": -0.8,
7
+
"right_fov_radians_left": -0.8,
8
+
"right_fov_radians_right": 0.8,
9
+
"right_fov_radians_up": 0.8,
10
+
"right_fov_radians_down": -0.8,
11
+
"t265_to_nose_bridge": {
12
+
"translation_meters": {
13
+
"x": 0,
14
+
"y": -0.04824,
15
+
"z": 0.08609
16
+
},
17
+
"rotation_quaternion": {
18
+
"x": -0.105,
19
+
"y": 0,
20
+
"z": 0,
21
+
"w": 0.995
22
+
}
23
+
},
24
+
"left_uv_to_rect_x": [
25
+
-0.7801988839098829,
26
+
1.5908068712799723,
27
+
-0.9827824328911254,
28
+
0.5263358892466966,
29
+
0.18664841081239514,
30
+
-0.5246668149574745,
31
+
1.4048420164308815,
32
+
-0.9330577906613924,
33
+
-0.15392100230529293,
34
+
1.106540330862997,
35
+
-2.87151330241086,
36
+
2.0315485110726668,
37
+
-0.06427965195537277,
38
+
-0.3146940276594198,
39
+
1.5763979534056192,
40
+
-1.1327966252705322
41
+
],
42
+
"left_uv_to_rect_y": [
43
+
-0.4409536269756768,
44
+
0.2888996614465091,
45
+
-0.3848014983107932,
46
+
-0.1223362997546647,
47
+
0.9670157125483582,
48
+
-0.3256816024594271,
49
+
0.533926353628004,
50
+
-0.23310084856690347,
51
+
-0.17082017203715041,
52
+
-0.059688885588560045,
53
+
-0.9941995171671563,
54
+
1.0721046441395208,
55
+
0.2547327106222003,
56
+
-0.25419242442834455,
57
+
1.0488819862049827,
58
+
-0.9515005068294279
59
+
],
60
+
"right_uv_to_rect_x": [
61
+
-0.6710613688416264,
62
+
1.1362689645641848,
63
+
0.3956957345987107,
64
+
-0.21326295396259742,
65
+
-0.5335462524956397,
66
+
4.546071135245812,
67
+
-10.207643227900032,
68
+
5.846282899290887,
69
+
1.3039161130160792,
70
+
-10.66129593656746,
71
+
22.158725872611857,
72
+
-12.912719005943362,
73
+
-0.9563087974847321,
74
+
6.870750205887958,
75
+
-13.828097816218262,
76
+
8.10122851327323
77
+
],
78
+
"right_uv_to_rect_y": [
79
+
-0.36496330390320814,
80
+
-0.34773830007498274,
81
+
1.1607448604832324,
82
+
-0.5639076806706572,
83
+
0.5129009330129037,
84
+
4.133391411431411,
85
+
-9.131958871130214,
86
+
5.601050711809029,
87
+
1.1111421772677867,
88
+
-10.247117862137081,
89
+
20.539322154891007,
90
+
-12.003706549179878,
91
+
-0.671733941602777,
92
+
6.353458803843707,
93
+
-12.784556822147636,
94
+
7.548883311524131
95
+
]
96
+
}
+4
src/xrt/drivers/north_star/exampleconfigs/v2_deckx_50cm.json.license
+4
src/xrt/drivers/north_star/exampleconfigs/v2_deckx_50cm.json.license
+117
-37
src/xrt/drivers/north_star/ns_hmd.c
+117
-37
src/xrt/drivers/north_star/ns_hmd.c
···
35
35
36
36
DEBUG_GET_ONCE_LOG_OPTION(ns_log, "NS_LOG", U_LOGGING_INFO)
37
37
38
+
struct xrt_pose t265_to_nose_bridge = {.orientation = {0, 0, 0, 1},
39
+
.position = {0, 0, 0}};
40
+
38
41
/*
39
42
*
40
43
* Common functions
41
44
*
42
45
*/
43
46
44
-
45
-
static double map(double value, double fromLow, double fromHigh, double toLow, double toHigh) {
46
-
return (value - fromLow) * (toHigh - toLow) / (fromHigh - fromLow) + toLow;
47
-
} // Math copied from https://www.arduino.cc/reference/en/language/functions/math/map/
47
+
static double
48
+
map(double value, double fromLow, double fromHigh, double toLow, double toHigh)
49
+
{
50
+
return (value - fromLow) * (toHigh - toLow) / (fromHigh - fromLow) +
51
+
toLow;
52
+
} // Math copied from
53
+
// https://www.arduino.cc/reference/en/language/functions/math/map/
48
54
// This is pure math so it is still under the Boost Software License.
49
55
50
56
static void
···
230
236
fov->angle_left =
231
237
projection.z; // atanf(fabsf(projection.z) / near_plane);
232
238
fov->angle_right =
233
-
projection.w; // atanf(fabsf(projection.w) / near_plane);
239
+
projection.w; // atanf(fabsf(projection.w) / near_plane);
234
240
}
235
241
236
242
/*
237
-
*
243
+
*
238
244
* V2 optics.
239
-
*
240
-
*
241
-
*/
245
+
*
246
+
*
247
+
*/
242
248
243
249
244
250
static void
···
312
318
float v,
313
319
struct xrt_uv_triplet *result)
314
320
{
315
-
/*! @todo (Moses Turner) It should not be necessary to reverse the U and V coords.
316
-
* I have no idea why it is like this. It shouldn't be like this.
317
-
* It must be something wrong with the undistortion calibrator.
321
+
/*! @todo (Moses Turner) It should not be necessary to reverse the U and
322
+
* V coords. I have no idea why it is like this. It shouldn't be like
323
+
* this. It must be something wrong with the undistortion calibrator.
318
324
* The V2 undistortion calibrator software is here if you want to look:
319
325
* https://github.com/BryanChrisBrown/ProjectNorthStar/tree/feat-gen-2-software/Software/North%20Star%20Gen%202/North%20Star%20Calibrator
320
326
*/
321
-
//u = 1.0 - u;
327
+
// u = 1.0 - u;
322
328
v = 1.0 - v;
323
329
324
330
···
335
341
float up_ray_bound = tan(ns->eye_configs_v2[view].fov.angle_up);
336
342
float down_ray_bound = tan(ns->eye_configs_v2[view].fov.angle_down);
337
343
338
-
float u_eye = map(x_ray, left_ray_bound, right_ray_bound, 0, 1);
344
+
float u_eye = map(x_ray, left_ray_bound, right_ray_bound, 0, 1);
339
345
340
346
float v_eye = map(y_ray, down_ray_bound, up_ray_bound, 0, 1);
341
347
···
407
413
cJSON_GetObjectItemCaseSensitive(config_json, "baseline"),
408
414
&ns->ipd);
409
415
ns->ipd = ns->ipd / 1000.0f; // converts from mm to m
410
-
/*! @todo (Moses Turner) Next four u_json_get_float_array calls don't make any sense.
411
-
* They put the X coefficients from the JSON file into the Y coefficients in the structs, which is totally wrong, but the distortion looks totally wrong if we don't do this.
416
+
/*! @todo (Moses Turner) Next four u_json_get_float_array calls
417
+
* don't make any sense. They put the X coefficients from the
418
+
* JSON file into the Y coefficients in the structs, which is
419
+
* totally wrong, but the distortion looks totally wrong if we
420
+
* don't do this.
412
421
*/
413
422
u_json_get_float_array(cJSON_GetObjectItemCaseSensitive(
414
423
config_json, "left_uv_to_rect_x"),
···
426
435
config_json, "right_uv_to_rect_y"),
427
436
ns->eye_configs_v2[1].x_coefficients,
428
437
16);
429
-
438
+
bool said_first_thing = false;
430
439
if (!u_json_get_float(
431
-
cJSON_GetObjectItemCaseSensitive(config_json,
432
-
"left_fov_radians_left"),
440
+
cJSON_GetObjectItemCaseSensitive(
441
+
config_json, "left_fov_radians_left"),
433
442
&ns->eye_configs_v2[0]
434
443
.fov
435
444
.angle_left)) { // not putting this directly in
···
442
451
"parameters to your v2 json file. There's an "
443
452
"example in "
444
453
"src/xrt/drivers/north_star/"
445
-
"v2_example_config.json.");
454
+
"v2_example_config.json.\n");
455
+
said_first_thing = true;
446
456
ns->eye_configs_v2[0].fov.angle_left = -0.8;
447
457
ns->eye_configs_v2[0].fov.angle_right = 0.8;
448
458
ns->eye_configs_v2[0].fov.angle_up = 0.8;
···
457
467
458
468
} else {
459
469
460
-
461
-
u_json_get_float(cJSON_GetObjectItemCaseSensitive(
470
+
/*! @todo (Moses Turner) Something's wrong with either
471
+
* ns_v2_mesh_calc or this code, because when you have
472
+
* uneven FOV bounds, the distortion looks totally
473
+
* wrong.*/
474
+
u_json_get_float(
475
+
cJSON_GetObjectItemCaseSensitive(
462
476
config_json, "left_fov_radians_left"),
463
-
&ns->eye_configs_v2[0].fov.angle_left);
464
-
u_json_get_float(cJSON_GetObjectItemCaseSensitive(
477
+
&ns->eye_configs_v2[0].fov.angle_left);
478
+
u_json_get_float(
479
+
cJSON_GetObjectItemCaseSensitive(
465
480
config_json, "left_fov_radians_right"),
466
-
&ns->eye_configs_v2[0].fov.angle_right);
467
-
u_json_get_float(cJSON_GetObjectItemCaseSensitive(
481
+
&ns->eye_configs_v2[0].fov.angle_right);
482
+
u_json_get_float(
483
+
cJSON_GetObjectItemCaseSensitive(
468
484
config_json, "left_fov_radians_up"),
469
-
&ns->eye_configs_v2[0].fov.angle_up);
470
-
u_json_get_float(cJSON_GetObjectItemCaseSensitive(
485
+
&ns->eye_configs_v2[0].fov.angle_up);
486
+
u_json_get_float(
487
+
cJSON_GetObjectItemCaseSensitive(
471
488
config_json, "left_fov_radians_down"),
472
-
&ns->eye_configs_v2[0].fov.angle_down);
489
+
&ns->eye_configs_v2[0].fov.angle_down);
473
490
474
-
u_json_get_float(cJSON_GetObjectItemCaseSensitive(
491
+
u_json_get_float(
492
+
cJSON_GetObjectItemCaseSensitive(
475
493
config_json, "right_fov_radians_left"),
476
-
&ns->eye_configs_v2[1].fov.angle_left);
477
-
u_json_get_float(cJSON_GetObjectItemCaseSensitive(
494
+
&ns->eye_configs_v2[1].fov.angle_left);
495
+
u_json_get_float(
496
+
cJSON_GetObjectItemCaseSensitive(
478
497
config_json, "right_fov_radians_right"),
479
-
&ns->eye_configs_v2[1].fov.angle_right);
480
-
u_json_get_float(cJSON_GetObjectItemCaseSensitive(
498
+
&ns->eye_configs_v2[1].fov.angle_right);
499
+
u_json_get_float(
500
+
cJSON_GetObjectItemCaseSensitive(
481
501
config_json, "right_fov_radians_up"),
482
-
&ns->eye_configs_v2[1].fov.angle_up);
483
-
u_json_get_float(cJSON_GetObjectItemCaseSensitive(
502
+
&ns->eye_configs_v2[1].fov.angle_up);
503
+
u_json_get_float(
504
+
cJSON_GetObjectItemCaseSensitive(
484
505
config_json, "right_fov_radians_down"),
485
-
&ns->eye_configs_v2[1].fov.angle_down);
506
+
&ns->eye_configs_v2[1].fov.angle_down);
507
+
}
508
+
509
+
struct cJSON *offset = cJSON_GetObjectItemCaseSensitive(
510
+
config_json, "t265_to_nose_bridge");
511
+
if (offset == NULL) {
512
+
if (said_first_thing) {
513
+
NS_INFO(
514
+
ns,
515
+
"Also, you should put an offset parameter "
516
+
"into the json file to transform your head "
517
+
"pose from the realsense to your nose "
518
+
"bridge. There are some examples in "
519
+
"src/xrt/drivers/north_star/"
520
+
"example_configs/ .\n");
521
+
} else {
522
+
NS_INFO(
523
+
ns,
524
+
"You should put an offset parameter into "
525
+
"the json file to transform your head pose "
526
+
"from the realsense to your nose bridge. "
527
+
"There are some examples in "
528
+
"src/xrt/drivers/north_star/"
529
+
"example_configs/ .\n");
530
+
}
531
+
} else {
532
+
struct cJSON *translation_meters =
533
+
cJSON_GetObjectItemCaseSensitive(
534
+
offset, "translation_meters");
535
+
u_json_get_float(cJSON_GetObjectItemCaseSensitive(
536
+
translation_meters, "x"),
537
+
&t265_to_nose_bridge.position.x);
538
+
u_json_get_float(cJSON_GetObjectItemCaseSensitive(
539
+
translation_meters, "y"),
540
+
&t265_to_nose_bridge.position.y);
541
+
u_json_get_float(cJSON_GetObjectItemCaseSensitive(
542
+
translation_meters, "z"),
543
+
&t265_to_nose_bridge.position.z);
544
+
545
+
struct cJSON *rotation_quaternion =
546
+
cJSON_GetObjectItemCaseSensitive(
547
+
offset, "rotation_quaternion");
548
+
549
+
u_json_get_float(cJSON_GetObjectItemCaseSensitive(
550
+
rotation_quaternion, "x"),
551
+
&t265_to_nose_bridge.orientation.x);
552
+
u_json_get_float(cJSON_GetObjectItemCaseSensitive(
553
+
rotation_quaternion, "y"),
554
+
&t265_to_nose_bridge.orientation.y);
555
+
u_json_get_float(cJSON_GetObjectItemCaseSensitive(
556
+
rotation_quaternion, "z"),
557
+
&t265_to_nose_bridge.orientation.z);
558
+
u_json_get_float(cJSON_GetObjectItemCaseSensitive(
559
+
rotation_quaternion, "w"),
560
+
&t265_to_nose_bridge.orientation.w);
486
561
}
487
562
488
563
ns->is_v2 = true;
···
608
683
// If built, try to load the realsense tracker.
609
684
#ifdef XRT_BUILD_DRIVER_RS
610
685
ns->tracker = rs_6dof_create();
686
+
if (ns->tracker == NULL) {
687
+
NS_ERROR(ns, "Couldn't create realsense device!");
688
+
} else {
689
+
rs_update_offset(t265_to_nose_bridge, ns->tracker);
690
+
}
611
691
#endif
612
692
// Setup variable tracker.
613
693
u_var_add_root(ns, "North Star", true);
-3
src/xrt/drivers/north_star/v1_example_config.json.license
-3
src/xrt/drivers/north_star/v1_example_config.json.license
src/xrt/drivers/north_star/v1_example_config_deckx_50cm.json
src/xrt/drivers/north_star/exampleconfigs/v1_deckx_50cm.json
src/xrt/drivers/north_star/v1_example_config_deckx_50cm.json
src/xrt/drivers/north_star/exampleconfigs/v1_deckx_50cm.json
+6
-6
src/xrt/drivers/north_star/v2_example_config_lonestar_50cm.json
src/xrt/drivers/north_star/exampleconfigs/v2_lonestar_50cm.json
+6
-6
src/xrt/drivers/north_star/v2_example_config_lonestar_50cm.json
src/xrt/drivers/north_star/exampleconfigs/v2_lonestar_50cm.json
···
1
1
{
2
2
"baseline": 64.0,
3
-
"left_fov_radians_left": -0.8,
4
-
"left_fov_radians_right": 0.8,
5
-
"left_fov_radians_up": 0.8,
6
-
"left_fov_radians_down": -0.8,
3
+
"left_fov_radians_left": -0.8,
4
+
"left_fov_radians_right": 0.8,
5
+
"left_fov_radians_up": 0.8,
6
+
"left_fov_radians_down": -0.8,
7
7
"right_fov_radians_left": -0.8,
8
8
"right_fov_radians_right": 0.8,
9
-
"right_fov_radians_up": 0.8,
9
+
"right_fov_radians_up": 0.8,
10
10
"right_fov_radians_down": -0.8,
11
-
"t265_to_eyes_center": {
11
+
"t265_to_nose_bridge": {
12
12
"translation_meters": {
13
13
"x": 0,
14
14
"y": -0.0683,
src/xrt/drivers/north_star/v2_example_config_lonestar_50cm.json.license
src/xrt/drivers/north_star/exampleconfigs/v2_lonestar_50cm.json.license
src/xrt/drivers/north_star/v2_example_config_lonestar_50cm.json.license
src/xrt/drivers/north_star/exampleconfigs/v2_lonestar_50cm.json.license
+26
-2
src/xrt/drivers/realsense/rs_6dof.c
+26
-2
src/xrt/drivers/realsense/rs_6dof.c
···
11
11
12
12
#include "xrt/xrt_defines.h"
13
13
#include "xrt/xrt_device.h"
14
+
#include "math/m_api.h"
15
+
#include "math/m_space.h"
14
16
15
17
#include "os/os_time.h"
16
18
#include "os/os_threading.h"
···
30
32
#include <inttypes.h>
31
33
32
34
35
+
/*!
36
+
* Stupid convenience macro to print out a pose, only used for debugging
37
+
*/
38
+
#define print_pose(msg, pose) \
39
+
U_LOG_E(msg " %f %f %f %f %f %f %f", pose.position.x, \
40
+
pose.position.y, pose.position.z, pose.orientation.x, \
41
+
pose.orientation.y, pose.orientation.z, pose.orientation.w)
33
42
34
43
/*!
35
44
* @implements xrt_device
···
40
49
41
50
uint64_t relation_timestamp_ns;
42
51
struct xrt_space_relation relation;
52
+
53
+
// arbitrary offset to apply to the pose the t265 gives us
54
+
struct xrt_pose offset;
43
55
44
56
struct os_thread_helper oth;
45
57
···
153
165
return 0;
154
166
}
155
167
168
+
169
+
void
170
+
rs_update_offset(struct xrt_pose offset, struct xrt_device *xdev)
171
+
{
172
+
struct rs_6dof *rs = rs_6dof(xdev);
173
+
memcpy(&rs->offset, &offset, sizeof(struct xrt_pose));
174
+
}
175
+
176
+
156
177
/*!
157
178
* Process a frame as 6DOF data, does not assume ownership of the frame.
158
179
*/
···
196
217
197
218
// Adjust the timestamp to monotonic time.
198
219
timestamp_ns = now_monotonic_ns - diff_ns;
199
-
200
220
201
221
/*
202
222
* Transfer the data to the struct.
···
316
336
struct xrt_space_relation relation = rs->relation;
317
337
os_thread_helper_unlock(&rs->oth);
318
338
319
-
*out_relation = relation;
339
+
struct xrt_space_graph xsg = {0};
340
+
341
+
m_space_graph_add_pose(&xsg, &rs->offset);
342
+
m_space_graph_add_relation(&xsg, &relation);
343
+
m_space_graph_resolve(&xsg, out_relation);
320
344
}
321
345
static void
322
346
rs_6dof_get_view_pose(struct xrt_device *xdev,