The open source OpenXR runtime
d/wmr: Refactor how precomputed transforms work
Hopefully this is an improvement and not just a rewrite. Some ideas were:
1. Keep poses in WMR space as much as possible.
2. Add an explicit function that precomputes a handful of transforms that allow
converting between WMR and OpenXR coordinates.
3. Make naming of variables in line with a relatively common T_A_B notation.
4. I wasn't able to figure out why `wmr_config_compute_pose` worked before,
in any case, hopefully the new notation convention helps understanding
why P_oxr_{acc,gyr} work.
+90
-62
+8
doc/conventions.md
+8
doc/conventions.md
···
111
111
- Constants/constexpr values: `kCamelCase`
112
112
- If a header is only usable from C++ code, it should be named with the
113
113
extension `.hpp` to signify this.
114
+
- Math:
115
+
- For different types of transforms `T` between two entities `A` and `B`, try
116
+
to use variable names like `T_A_B` to express the transform such that `B =
117
+
T_A_B * A`. This is equivalent to "`B` expressed w.r.t. `A`" and "the
118
+
transform that converts a point in `B` coordinates into `A` coordinates".
119
+
`T` can be used for 4x4 isometry matrices, but you can use others like
120
+
`P` for poses, `R` for 3x3 rotations, `Q` for quaternion rotations, `t` for
121
+
translations, etc.
114
122
115
123
## Patterns and Idioms
116
124
+14
-15
src/xrt/drivers/wmr/wmr_config.c
+14
-15
src/xrt/drivers/wmr/wmr_config.c
···
46
46
math_matrix_3x3_identity(&c->sensors.mag.mix_matrix);
47
47
}
48
48
49
-
static void
50
-
wmr_config_compute_pose(struct xrt_pose *out_pose, const struct xrt_vec3 *tx, const struct xrt_matrix_3x3 *rx)
49
+
static struct xrt_pose
50
+
pose_from_rt(const struct xrt_matrix_3x3 rotation_rm, const struct xrt_vec3 translation)
51
51
{
52
-
// Adjust the coordinate system / conventions of the raw Tx and Rx config to yield a usable xrt_pose
53
-
// The config stores a 3x3 rotation matrix and a vec3 translation.
54
-
// Translation is applied after rotation, and the coordinate system is flipped in YZ.
52
+
struct xrt_matrix_3x3 rotation_cm;
53
+
math_matrix_3x3_transpose(&rotation_rm, &rotation_cm);
55
54
56
-
struct xrt_matrix_3x3 coordsys = {.v = {1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, -1.0}};
55
+
struct xrt_matrix_4x4 mat = {0};
56
+
math_matrix_4x4_isometry_from_rt(&rotation_cm, &translation, &mat);
57
57
58
-
struct xrt_matrix_3x3 rx_adj;
59
-
math_matrix_3x3_multiply(&coordsys, rx, &rx_adj);
60
-
math_quat_from_matrix_3x3(&rx_adj, &out_pose->orientation);
58
+
struct xrt_pose pose;
59
+
math_pose_from_isometry(&mat, &pose);
61
60
62
-
struct xrt_vec3 v;
63
-
math_matrix_3x3_transform_vec3(&coordsys, tx, &v);
64
-
math_matrix_3x3_transform_vec3(&rx_adj, &v, &out_pose->position);
61
+
return pose;
65
62
}
66
63
67
64
static bool
···
120
117
if (u_json_get_float_array(rx, rotation.v, 9) != 9)
121
118
return false;
122
119
123
-
wmr_config_compute_pose(&eye->pose, &translation, &rotation);
120
+
eye->pose = pose_from_rt(rotation, translation);
121
+
eye->translation = translation;
122
+
eye->rotation = rotation;
124
123
125
124
/* Parse color distortion channels */
126
125
const char *channel_names[] = {"DistortionRed", "DistortionGreen", "DistortionBlue"};
···
192
191
return false;
193
192
}
194
193
195
-
wmr_config_compute_pose(&c->pose, &translation, &rotation);
194
+
c->pose = pose_from_rt(rotation, translation);
196
195
c->translation = translation;
197
196
c->rotation = rotation;
198
197
···
339
338
return false;
340
339
}
341
340
342
-
wmr_config_compute_pose(&cam_config->pose, &translation, &rotation);
341
+
cam_config->pose = pose_from_rt(rotation, translation);
343
342
cam_config->translation = translation;
344
343
cam_config->rotation = rotation;
345
344
+7
-4
src/xrt/drivers/wmr/wmr_config.h
+7
-4
src/xrt/drivers/wmr/wmr_config.h
···
85
85
{
86
86
/* 3x3 camera matrix that moves from normalised camera coords (X/Z & Y/Z) to undistorted pixels */
87
87
struct xrt_matrix_3x3 affine_xform;
88
-
/* Eye pose in world space */
89
-
struct xrt_pose pose;
88
+
89
+
struct xrt_vec3 translation; //!< Raw translation (to HT0)
90
+
struct xrt_matrix_3x3 rotation; //!< Raw rotation (to HT0), row major
91
+
struct xrt_pose pose; //!< Pose from `translation` and `rotation`
92
+
90
93
/* Radius of the (undistorted) visible area from the center (pixels) (I think) */
91
94
double visible_radius;
92
95
···
108
111
109
112
struct xrt_vec3 translation; //!< Raw translation (to HT0)
110
113
struct xrt_matrix_3x3 rotation; //!< Raw rotation (to HT0), row major
111
-
struct xrt_pose pose; //!< Corrected pose from translation and rotation fields
114
+
struct xrt_pose pose; //!< Pose from `translation` and `rotation`
112
115
113
116
struct wmr_distortion_6KT distortion6KT;
114
117
};
···
118
121
{
119
122
struct xrt_vec3 translation; //!< Raw translation (to HT0). Usually non-zero only on accelerometers.
120
123
struct xrt_matrix_3x3 rotation; //!< Raw rotation (to HT0), row major
121
-
struct xrt_pose pose; //!< Corrected pose from translation and rotation fields
124
+
struct xrt_pose pose; //!< Pose from `translation` and `rotation`
122
125
123
126
/* Current bias and mix matrix extracted from
124
127
* the configuration, which provides coefficients
+58
-37
src/xrt/drivers/wmr/wmr_hmd.c
+58
-37
src/xrt/drivers/wmr/wmr_hmd.c
···
22
22
#include "os/os_time.h"
23
23
#include "os/os_hid.h"
24
24
25
-
#include "math/m_mathinclude.h"
26
25
#include "math/m_api.h"
27
-
#include "math/m_vec2.h"
26
+
#include "math/m_mathinclude.h"
28
27
#include "math/m_predict.h"
28
+
#include "math/m_vec2.h"
29
29
30
30
#include "util/u_var.h"
31
31
#include "util/u_misc.h"
···
323
323
vec3_from_hololens_gyro(wh->packet.gyro, i, rg);
324
324
math_matrix_3x3_transform_vec3(&wh->config.sensors.gyro.mix_matrix, rg, cg);
325
325
math_vec3_accum(&wh->config.sensors.gyro.bias_offsets, cg);
326
-
math_quat_rotate_vec3(&wh->gyro_to_centerline.orientation, cg, cg);
326
+
math_quat_rotate_vec3(&wh->P_oxr_gyr.orientation, cg, cg);
327
327
328
328
struct xrt_vec3 *ra = &raw_accel[i];
329
329
struct xrt_vec3 *ca = &calib_accel[i];
330
330
vec3_from_hololens_accel(wh->packet.accel, i, ra);
331
331
math_matrix_3x3_transform_vec3(&wh->config.sensors.accel.mix_matrix, ra, ca);
332
332
math_vec3_accum(&wh->config.sensors.accel.bias_offsets, ca);
333
-
math_quat_rotate_vec3(&wh->accel_to_centerline.orientation, ca, ca);
333
+
math_quat_rotate_vec3(&wh->P_oxr_acc.orientation, ca, ca);
334
334
}
335
335
336
336
// Fusion tracking
···
1600
1600
return true;
1601
1601
}
1602
1602
1603
+
/*!
1604
+
* Precompute transforms to convert between OpenXR and WMR coordinate systems.
1605
+
*
1606
+
* OpenXR: X: Right, Y: Up, Z: Backward
1607
+
* WMR: X: Right, Y: Down, Z: Forward
1608
+
* ┌────────────────────┐
1609
+
* │ OXR WMR │
1610
+
* │ │
1611
+
* │ ▲ y │
1612
+
* │ │ ▲ z │
1613
+
* │ │ x │ x │
1614
+
* │ ├──────► ├──────► │
1615
+
* │ │ │ │
1616
+
* │ ▼ z │ │
1617
+
* │ ▼ y │
1618
+
* └────────────────────┘
1619
+
*/
1620
+
static void
1621
+
precompute_sensor_transforms(struct wmr_hmd *wh)
1622
+
{
1623
+
// P_A_B is such that B = P_A_B * A. See conventions.md
1624
+
struct xrt_pose P_oxr_wmr = {{.x = 1.0, .y = 0.0, .z = 0.0, .w = 0.0}, XRT_VEC3_ZERO};
1625
+
struct xrt_pose P_wmr_oxr = {0};
1626
+
struct xrt_pose P_acc_ht0 = wh->config.sensors.accel.pose;
1627
+
struct xrt_pose P_gyr_ht0 = wh->config.sensors.gyro.pose;
1628
+
struct xrt_pose P_ht0_acc = {0};
1629
+
struct xrt_pose P_ht0_gyr = {0};
1630
+
struct xrt_pose P_me_ht0 = {0}; // "me" == "middle of the eyes"
1631
+
struct xrt_pose P_me_acc = {0};
1632
+
struct xrt_pose P_me_gyr = {0};
1633
+
struct xrt_pose P_ht0_me = {0};
1634
+
struct xrt_pose P_acc_me = {0};
1635
+
1636
+
// All of the observed headsets have reported a zero translation for its gyro
1637
+
assert(m_vec3_equal_exact(P_gyr_ht0.position, (struct xrt_vec3){0, 0, 0}));
1638
+
1639
+
// Initialize transforms
1640
+
1641
+
// All of these are in WMR coordinates.
1642
+
math_pose_invert(&P_oxr_wmr, &P_wmr_oxr); // P_wmr_oxr == P_oxr_wmr
1643
+
math_pose_invert(&P_acc_ht0, &P_ht0_acc);
1644
+
math_pose_invert(&P_gyr_ht0, &P_ht0_gyr);
1645
+
math_pose_interpolate(&wh->config.eye_params[0].pose, &wh->config.eye_params[1].pose, 0.5, &P_me_ht0);
1646
+
math_pose_transform(&P_me_ht0, &P_ht0_acc, &P_me_acc);
1647
+
math_pose_transform(&P_me_ht0, &P_ht0_gyr, &P_me_gyr);
1648
+
math_pose_invert(&P_me_ht0, &P_ht0_me);
1649
+
math_pose_invert(&P_me_acc, &P_acc_me);
1650
+
1651
+
// Save transforms
1652
+
math_pose_transform(&P_oxr_wmr, &P_me_acc, &wh->P_oxr_acc);
1653
+
math_pose_transform(&P_oxr_wmr, &P_me_gyr, &wh->P_oxr_gyr);
1654
+
}
1655
+
1603
1656
void
1604
1657
wmr_hmd_create(enum wmr_headset_type hmd_type,
1605
1658
struct os_hid_device *hid_holo,
···
1689
1742
1690
1743
WMR_INFO(wh, "Found WMR headset type: %s", wh->hmd_desc->debug_name);
1691
1744
1692
-
// Compute centerline in the HMD's calibration coordinate space as the average of the two display poses,
1693
-
// then rotate around the X axis to convert coordinate system from WMR (X right, Y down, Z away)
1694
-
// to OpenXR (X right, Y up, Z towards)
1695
-
math_quat_slerp(&wh->config.eye_params[0].pose.orientation, &wh->config.eye_params[1].pose.orientation, 0.5f,
1696
-
&wh->centerline.orientation);
1697
-
wh->centerline.position.x =
1698
-
(wh->config.eye_params[0].pose.position.x + wh->config.eye_params[1].pose.position.x) * 0.5f;
1699
-
wh->centerline.position.y =
1700
-
(wh->config.eye_params[0].pose.position.y + wh->config.eye_params[1].pose.position.y) * 0.5f;
1701
-
wh->centerline.position.z =
1702
-
(wh->config.eye_params[0].pose.position.z + wh->config.eye_params[1].pose.position.z) * 0.5f;
1703
-
1704
-
struct xrt_pose wmr_to_openxr_xform = {
1705
-
.position = {0.0, 0.0, 0.0},
1706
-
.orientation = {.x = 1.0, .y = 0.0, .z = 0.0, .w = 0.0},
1707
-
};
1708
-
1709
-
math_pose_transform(&wmr_to_openxr_xform, &wh->centerline, &wh->centerline);
1710
-
1711
-
// Compute display and sensor offsets relative to the centerline
1712
-
for (int dIdx = 0; dIdx < 2; ++dIdx) {
1713
-
math_pose_invert(&wh->config.eye_params[dIdx].pose, &wh->display_to_centerline[dIdx]);
1714
-
math_pose_transform(&wh->centerline, &wh->display_to_centerline[dIdx],
1715
-
&wh->display_to_centerline[dIdx]);
1716
-
}
1717
-
math_pose_invert(&wh->config.sensors.accel.pose, &wh->accel_to_centerline);
1718
-
math_pose_transform(&wh->centerline, &wh->accel_to_centerline, &wh->accel_to_centerline);
1719
-
math_pose_invert(&wh->config.sensors.gyro.pose, &wh->gyro_to_centerline);
1720
-
math_pose_transform(&wh->centerline, &wh->gyro_to_centerline, &wh->gyro_to_centerline);
1721
-
math_pose_invert(&wh->config.sensors.mag.pose, &wh->mag_to_centerline);
1722
-
math_pose_transform(&wh->centerline, &wh->mag_to_centerline, &wh->mag_to_centerline);
1745
+
precompute_sensor_transforms(wh);
1723
1746
1724
1747
struct u_extents_2d exts;
1725
-
1726
1748
exts.w_pixels = (uint32_t)wh->config.eye_params[0].display_size.x;
1727
1749
exts.h_pixels = (uint32_t)wh->config.eye_params[0].display_size.y;
1728
-
1729
1750
u_extents_2d_split_side_by_side(&wh->base, &exts);
1730
1751
1731
1752
// Fill in blend mode - just opqaue, unless we get Hololens support one day.
+3
-6
src/xrt/drivers/wmr/wmr_hmd.h
+3
-6
src/xrt/drivers/wmr/wmr_hmd.h
···
116
116
//! Distortion related parameters
117
117
struct wmr_hmd_distortion_params distortion_params[2];
118
118
119
-
// Config-derived poses
120
-
struct xrt_pose centerline;
121
-
struct xrt_pose display_to_centerline[2];
122
-
struct xrt_pose accel_to_centerline;
123
-
struct xrt_pose gyro_to_centerline;
124
-
struct xrt_pose mag_to_centerline;
119
+
//! Precomputed transforms, @see precompute_sensor_transforms.
120
+
struct xrt_pose P_oxr_acc; //!< Converts accel samples into OpenXR coordinates
121
+
struct xrt_pose P_oxr_gyr; //!< Converts gyro samples into OpenXR coordinates
125
122
126
123
struct hololens_sensors_packet packet;
127
124