mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2024-12-29 11:06:18 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

d/wmr: make sensor transform precomputations reuseable

Browse source 
Not only the sensor values from the HMD need to be transformed from WMR
to OpenXR but also the sensor values from the controllers. Therefore
restructuring the according code to be useable by both the WMR HMD and
WMR controller code.

No functional changes.

Signed-off-by: Linus Lüssing <linus.luessing@c0d3.blue>
This commit is contained in:
Linus Lüssing 2023-06-08 13:55:14 +02:00 committed by Jakob Bornecrantz
parent e5de8cee27
commit c95be33072
4 changed files with 95 additions and 78 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

73
src/xrt/drivers/wmr/wmr_config.c
View file

@ -7,7 +7,7 @@
* @author Jan Schmidt <jan@centricular.com> * @author Jan Schmidt <jan@centricular.com>
* @ingroup drv_wmr * @ingroup drv_wmr
*/ */
#include <string.h>
#include "math/m_api.h" #include "math/m_api.h"
#include "util/u_debug.h" #include "util/u_debug.h"
@ -16,6 +16,10 @@
#include "wmr_config.h" #include "wmr_config.h"
#include <assert.h>
#include <string.h>
#define WMR_TRACE(log_level, ...) U_LOG_IFL_T(log_level, __VA_ARGS__) #define WMR_TRACE(log_level, ...) U_LOG_IFL_T(log_level, __VA_ARGS__)
#define WMR_DEBUG(log_level, ...) U_LOG_IFL_D(log_level, __VA_ARGS__) #define WMR_DEBUG(log_level, ...) U_LOG_IFL_D(log_level, __VA_ARGS__)
#define WMR_INFO(log_level, ...) U_LOG_IFL_I(log_level, __VA_ARGS__) #define WMR_INFO(log_level, ...) U_LOG_IFL_I(log_level, __VA_ARGS__)
@ -584,3 +588,70 @@ wmr_controller_config_parse(struct wmr_controller_config *c, char *json_string,
return true; return true;
} }
/*!
* Precompute transforms to convert between OpenXR and WMR coordinate systems.
*
* OpenXR: X: Right, Y: Up, Z: Backward
* WMR: X: Right, Y: Down, Z: Forward
* ┌────────────────────┐
* │ OXR WMR │
* │ │
* │ ▲ y │
* │ │ ▲ z │
* │ │ x │ x │
* │ ├──────► ├──────► │
* │ │ │ │
* │ ▼ z │ │
* │ ▼ y │
* └────────────────────┘
*/
void
wmr_config_precompute_transforms(struct wmr_inertial_sensors_config *sensors,
struct wmr_distortion_eye_config *eye_params)
{
// P_A_B is such that B = P_A_B * A. See conventions.md
struct xrt_pose P_oxr_wmr = {{.x = 1.0, .y = 0.0, .z = 0.0, .w = 0.0}, XRT_VEC3_ZERO};
struct xrt_pose P_wmr_oxr = {0};
struct xrt_pose P_acc_ht0 = sensors->accel.pose;
struct xrt_pose P_gyr_ht0 = sensors->gyro.pose;
struct xrt_pose P_ht0_acc = {0};
struct xrt_pose P_ht0_gyr = {0};
struct xrt_pose P_me_ht0 = {0}; // "me" == "middle of the eyes"
struct xrt_pose P_me_acc = {0};
struct xrt_pose P_me_gyr = {0};
struct xrt_pose P_ht0_me = {0};
struct xrt_pose P_acc_me = {0};
struct xrt_pose P_oxr_ht0_me = {0}; // P_ht0_me in OpenXR coordinates
struct xrt_pose P_oxr_acc_me = {0}; // P_acc_me in OpenXR coordinates
// All of the observed headsets have reported a zero translation for its gyro
assert(m_vec3_equal_exact(P_gyr_ht0.position, (struct xrt_vec3){0, 0, 0}));
// Initialize transforms
// All of these are in WMR coordinates.
math_pose_invert(&P_oxr_wmr, &P_wmr_oxr); // P_wmr_oxr == P_oxr_wmr
math_pose_invert(&P_acc_ht0, &P_ht0_acc);
math_pose_invert(&P_gyr_ht0, &P_ht0_gyr);
if (eye_params)
math_pose_interpolate(&eye_params[0].pose, &eye_params[1].pose, 0.5, &P_me_ht0);
else
math_pose_identity(&P_me_ht0);
math_pose_transform(&P_me_ht0, &P_ht0_acc, &P_me_acc);
math_pose_transform(&P_me_ht0, &P_ht0_gyr, &P_me_gyr);
math_pose_invert(&P_me_ht0, &P_ht0_me);
math_pose_invert(&P_me_acc, &P_acc_me);
// Express P_*_me pose in OpenXR coordinates through sandwich products.
math_pose_transform(&P_acc_me, &P_wmr_oxr, &P_oxr_acc_me);
math_pose_transform(&P_oxr_wmr, &P_oxr_acc_me, &P_oxr_acc_me);
math_pose_transform(&P_ht0_me, &P_wmr_oxr, &P_oxr_ht0_me);
math_pose_transform(&P_oxr_wmr, &P_oxr_ht0_me, &P_oxr_ht0_me);
// Save transforms
math_pose_transform(&P_oxr_wmr, &P_me_acc, &sensors->transforms.P_oxr_acc);
math_pose_transform(&P_oxr_wmr, &P_me_gyr, &sensors->transforms.P_oxr_gyr);
sensors->transforms.P_ht0_me = P_oxr_ht0_me;
sensors->transforms.P_imu_me = P_oxr_acc_me; // Assume accel pose is IMU pose
}

15
src/xrt/drivers/wmr/wmr_config.h
View file

@ -137,12 +137,23 @@ struct wmr_inertial_sensor_config
struct xrt_vec3 noise_std; struct xrt_vec3 noise_std;
}; };
/* Precomputed transforms to convert between OpenXR and WMR coordinate systems */
struct wmr_sensor_transforms_config
{
struct xrt_pose P_oxr_acc; //!< Converts accel samples into OpenXR coordinates
struct xrt_pose P_oxr_gyr; //!< Converts gyro samples into OpenXR coordinates
struct xrt_pose P_ht0_me; //!< ME="middle of the eyes". HT0-to-ME transform but in OpenXR coordinates
struct xrt_pose P_imu_me; //!< IMU=accel. IMU-to-ME transform but in OpenXR coordinates
};
/* Configuration for the set of inertial sensors */ /* Configuration for the set of inertial sensors */
struct wmr_inertial_sensors_config struct wmr_inertial_sensors_config
{ {
struct wmr_inertial_sensor_config accel; struct wmr_inertial_sensor_config accel;
struct wmr_inertial_sensor_config gyro; struct wmr_inertial_sensor_config gyro;
struct wmr_inertial_sensor_config mag; struct wmr_inertial_sensor_config mag;
struct wmr_sensor_transforms_config transforms;
}; };
struct wmr_led_config struct wmr_led_config
@ -181,6 +192,10 @@ struct wmr_controller_config
bool bool
wmr_controller_config_parse(struct wmr_controller_config *c, char *json_string, enum u_logging_level log_level); wmr_controller_config_parse(struct wmr_controller_config *c, char *json_string, enum u_logging_level log_level);
void
wmr_config_precompute_transforms(struct wmr_inertial_sensors_config *sensors,
struct wmr_distortion_eye_config *eye_params);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

79
src/xrt/drivers/wmr/wmr_hmd.c
View file

@ -395,8 +395,8 @@ hololens_handle_sensors_avg(struct wmr_hmd *wh, const unsigned char *buffer, int
math_matrix_3x3_transform_vec3(&wh->config.sensors.gyro.mix_matrix, &avg_raw_gyro, &avg_calib_gyro); math_matrix_3x3_transform_vec3(&wh->config.sensors.gyro.mix_matrix, &avg_raw_gyro, &avg_calib_gyro);
math_vec3_accum(&wh->config.sensors.accel.bias_offsets, &avg_calib_accel); math_vec3_accum(&wh->config.sensors.accel.bias_offsets, &avg_calib_accel);
math_vec3_accum(&wh->config.sensors.gyro.bias_offsets, &avg_calib_gyro); math_vec3_accum(&wh->config.sensors.gyro.bias_offsets, &avg_calib_gyro);
math_quat_rotate_vec3(&wh->P_oxr_acc.orientation, &avg_calib_accel, &avg_calib_accel); math_quat_rotate_vec3(&wh->config.sensors.transforms.P_oxr_acc.orientation, &avg_calib_accel, &avg_calib_accel);
math_quat_rotate_vec3(&wh->P_oxr_gyr.orientation, &avg_calib_gyro, &avg_calib_gyro); math_quat_rotate_vec3(&wh->config.sensors.transforms.P_oxr_gyr.orientation, &avg_calib_gyro, &avg_calib_gyro);
// Fusion tracking // Fusion tracking
os_mutex_lock(&wh->fusion.mutex); os_mutex_lock(&wh->fusion.mutex);
@ -431,14 +431,14 @@ hololens_handle_sensors_all(struct wmr_hmd *wh, const unsigned char *buffer, int
vec3_from_hololens_gyro(wh->packet.gyro, i, rg); vec3_from_hololens_gyro(wh->packet.gyro, i, rg);
math_matrix_3x3_transform_vec3(&wh->config.sensors.gyro.mix_matrix, rg, cg); math_matrix_3x3_transform_vec3(&wh->config.sensors.gyro.mix_matrix, rg, cg);
math_vec3_accum(&wh->config.sensors.gyro.bias_offsets, cg); math_vec3_accum(&wh->config.sensors.gyro.bias_offsets, cg);
math_quat_rotate_vec3(&wh->P_oxr_gyr.orientation, cg, cg); math_quat_rotate_vec3(&wh->config.sensors.transforms.P_oxr_gyr.orientation, cg, cg);
struct xrt_vec3 *ra = &raw_accel[i]; struct xrt_vec3 *ra = &raw_accel[i];
struct xrt_vec3 *ca = &calib_accel[i]; struct xrt_vec3 *ca = &calib_accel[i];
vec3_from_hololens_accel(wh->packet.accel, i, ra); vec3_from_hololens_accel(wh->packet.accel, i, ra);
math_matrix_3x3_transform_vec3(&wh->config.sensors.accel.mix_matrix, ra, ca); math_matrix_3x3_transform_vec3(&wh->config.sensors.accel.mix_matrix, ra, ca);
math_vec3_accum(&wh->config.sensors.accel.bias_offsets, ca); math_vec3_accum(&wh->config.sensors.accel.bias_offsets, ca);
math_quat_rotate_vec3(&wh->P_oxr_acc.orientation, ca, ca); math_quat_rotate_vec3(&wh->config.sensors.transforms.P_oxr_acc.orientation, ca, ca);
} }
// Fusion tracking // Fusion tracking
@ -1145,7 +1145,7 @@ wmr_hmd_get_slam_tracked_pose(struct xrt_device *xdev,
} }
if (wh->tracking.imu2me) { if (wh->tracking.imu2me) {
math_pose_transform(&wh->pose, &wh->P_imu_me, &wh->pose); math_pose_transform(&wh->pose, &wh->config.sensors.transforms.P_imu_me, &wh->pose);
} }
out_relation->pose = wh->pose; out_relation->pose = wh->pose;
@ -1613,7 +1613,7 @@ wmr_hmd_slam_track(struct wmr_hmd *wh)
static enum t_camera_orientation static enum t_camera_orientation
wmr_hmd_guess_camera_orientation(struct wmr_hmd *wh) wmr_hmd_guess_camera_orientation(struct wmr_hmd *wh)
{ {
struct xrt_quat Q_ht0_me = wh->P_ht0_me.orientation; struct xrt_quat Q_ht0_me = wh->config.sensors.transforms.P_ht0_me.orientation;
struct xrt_vec2 swing = {0}; struct xrt_vec2 swing = {0};
float twist = 0; float twist = 0;
math_quat_to_swing_twist(&Q_ht0_me, &swing, &twist); math_quat_to_swing_twist(&Q_ht0_me, &swing, &twist);
@ -1684,7 +1684,7 @@ wmr_hmd_hand_track(struct wmr_hmd *wh,
} }
device = multi_create_tracking_override(XRT_TRACKING_OVERRIDE_ATTACHED, device, &wh->base, device = multi_create_tracking_override(XRT_TRACKING_OVERRIDE_ATTACHED, device, &wh->base,
XRT_INPUT_GENERIC_HEAD_POSE, &wh->P_ht0_me); XRT_INPUT_GENERIC_HEAD_POSE, &wh->config.sensors.transforms.P_ht0_me);
WMR_DEBUG(wh, "WMR HMD hand tracker successfully created"); WMR_DEBUG(wh, "WMR HMD hand tracker successfully created");
#endif #endif
@ -1851,69 +1851,6 @@ wmr_hmd_setup_trackers(struct wmr_hmd *wh, struct xrt_slam_sinks *out_sinks, str
return true; return true;
} }
/*!
* Precompute transforms to convert between OpenXR and WMR coordinate systems.
*
* OpenXR: X: Right, Y: Up, Z: Backward
* WMR: X: Right, Y: Down, Z: Forward
* ┌────────────────────┐
* │ OXR WMR │
* │ │
* │ ▲ y │
* │ │ ▲ z │
* │ │ x │ x │
* │ ├──────► ├──────► │
* │ │ │ │
* │ ▼ z │ │
* │ ▼ y │
* └────────────────────┘
*/
static void
precompute_sensor_transforms(struct wmr_hmd *wh)
{
// P_A_B is such that B = P_A_B * A. See conventions.md
struct xrt_pose P_oxr_wmr = {{.x = 1.0, .y = 0.0, .z = 0.0, .w = 0.0}, XRT_VEC3_ZERO};
struct xrt_pose P_wmr_oxr = {0};
struct xrt_pose P_acc_ht0 = wh->config.sensors.accel.pose;
struct xrt_pose P_gyr_ht0 = wh->config.sensors.gyro.pose;
struct xrt_pose P_ht0_acc = {0};
struct xrt_pose P_ht0_gyr = {0};
struct xrt_pose P_me_ht0 = {0}; // "me" == "middle of the eyes"
struct xrt_pose P_me_acc = {0};
struct xrt_pose P_me_gyr = {0};
struct xrt_pose P_ht0_me = {0};
struct xrt_pose P_acc_me = {0};
struct xrt_pose P_oxr_ht0_me = {0}; // P_ht0_me in OpenXR coordinates
struct xrt_pose P_oxr_acc_me = {0}; // P_acc_me in OpenXR coordinates
// All of the observed headsets have reported a zero translation for its gyro
assert(m_vec3_equal_exact(P_gyr_ht0.position, (struct xrt_vec3){0, 0, 0}));
// Initialize transforms
// All of these are in WMR coordinates.
math_pose_invert(&P_oxr_wmr, &P_wmr_oxr); // P_wmr_oxr == P_oxr_wmr
math_pose_invert(&P_acc_ht0, &P_ht0_acc);
math_pose_invert(&P_gyr_ht0, &P_ht0_gyr);
math_pose_interpolate(&wh->config.eye_params[0].pose, &wh->config.eye_params[1].pose, 0.5, &P_me_ht0);
math_pose_transform(&P_me_ht0, &P_ht0_acc, &P_me_acc);
math_pose_transform(&P_me_ht0, &P_ht0_gyr, &P_me_gyr);
math_pose_invert(&P_me_ht0, &P_ht0_me);
math_pose_invert(&P_me_acc, &P_acc_me);
// Express P_*_me pose in OpenXR coordinates through sandwich products.
math_pose_transform(&P_acc_me, &P_wmr_oxr, &P_oxr_acc_me);
math_pose_transform(&P_oxr_wmr, &P_oxr_acc_me, &P_oxr_acc_me);
math_pose_transform(&P_ht0_me, &P_wmr_oxr, &P_oxr_ht0_me);
math_pose_transform(&P_oxr_wmr, &P_oxr_ht0_me, &P_oxr_ht0_me);
// Save transforms
math_pose_transform(&P_oxr_wmr, &P_me_acc, &wh->P_oxr_acc);
math_pose_transform(&P_oxr_wmr, &P_me_gyr, &wh->P_oxr_gyr);
wh->P_ht0_me = P_oxr_ht0_me;
wh->P_imu_me = P_oxr_acc_me; // Assume accel pose is IMU pose
}
static bool static bool
wmr_hmd_request_controller_status(struct wmr_hmd *wh) wmr_hmd_request_controller_status(struct wmr_hmd *wh)
{ {
@ -2044,7 +1981,7 @@ wmr_hmd_create(enum wmr_headset_type hmd_type,
WMR_INFO(wh, "Found WMR headset type: %s", wh->hmd_desc->debug_name); WMR_INFO(wh, "Found WMR headset type: %s", wh->hmd_desc->debug_name);
precompute_sensor_transforms(wh); wmr_config_precompute_transforms(&wh->config.sensors, wh->config.eye_params);
struct u_extents_2d exts; struct u_extents_2d exts;
exts.w_pixels = (uint32_t)wh->config.eye_params[0].display_size.x; exts.w_pixels = (uint32_t)wh->config.eye_params[0].display_size.x;

6
src/xrt/drivers/wmr/wmr_hmd.h
View file

@ -115,12 +115,6 @@ struct wmr_hmd
//! Distortion related parameters //! Distortion related parameters
struct wmr_hmd_distortion_params distortion_params[2]; struct wmr_hmd_distortion_params distortion_params[2];
//! Precomputed transforms, @see precompute_sensor_transforms.
struct xrt_pose P_oxr_acc; //!< Converts accel samples into OpenXR coordinates
struct xrt_pose P_oxr_gyr; //!< Converts gyro samples into OpenXR coordinates
struct xrt_pose P_ht0_me; //!< ME="middle of the eyes". HT0-to-ME transform but in OpenXR coordinates
struct xrt_pose P_imu_me; //!< IMU=accel. IMU-to-ME transform but in OpenXR coordinates
struct hololens_sensors_packet packet; struct hololens_sensors_packet packet;
struct struct