d/wmr + a/util: Extract out WMR Poly3k distortion to shared place

Part-of: <https://gitlab.freedesktop.org/monado/monado/-/merge_requests/2590>
This commit is contained in:
Beyley Cardellio authored and Marge Bot committed 2025-10-01 14:44:07 +00:00
1 parent de6c4bff7a
commit b2ffa637f0
6 files changed
+226 -213

No files matched your search

+138
View File
@@ -399,6 +399,144 @@ u_compute_distortion_ns_meshgrid(
}
/*
*
* Windows Mixed Reality distortion
*
*/
void
u_compute_distortion_poly_3k(
struct u_poly_3k_eye_values *values, uint32_t view, float u, float v, struct xrt_uv_triplet *result)
{
assert(view == 0 || view == 1);
const struct xrt_matrix_3x3 *inv_affine_xform = &values->inv_affine_xform;
// Results r/g/b.
struct xrt_vec2 tc[3];
// Dear compiler, please vectorize.
for (int channel = 0; channel < 3; channel++) {
const struct xrt_vec2_i32 display_size = values->channels[channel].display_size;
const struct xrt_vec2 eye_center = values->channels[channel].eye_center;
const double *k = values->channels[channel].k;
/* Scale the 0..1 input UV back to pixels relative to the distortion center,
* accounting for the right eye starting at X = panel_width / 2.0 */
struct xrt_vec2 pix_coord = {(u + 1.0f * view) * (display_size.x / 2.0f) - eye_center.x,
v * display_size.y - eye_center.y};
pix_coord.y += (float)values->y_offset;
float r2 = m_vec2_dot(pix_coord, pix_coord);
float k1 = (float)k[0];
float k2 = (float)k[1];
float k3 = (float)k[2];
float d = 1.0f + r2 * (k1 + r2 * (k2 + r2 * k3));
/* Map the distorted pixel coordinate back to normalised view plane coords using the inverse affine
* xform */
struct xrt_vec3 p = {(pix_coord.x * d + eye_center.x), (pix_coord.y * d + eye_center.y), 1.0f};
struct xrt_vec3 vp;
math_matrix_3x3_transform_vec3(inv_affine_xform, &p, &vp);
/* Finally map back to the input texture 0..1 range based on the render FoV (from tex_N_range.x ..
* tex_N_range.y) */
tc[channel].x =
((vp.x / vp.z) - values->tex_x_range.x) / (values->tex_x_range.y - values->tex_x_range.x);
tc[channel].y =
((vp.y / vp.z) - values->tex_y_range.x) / (values->tex_y_range.y - values->tex_y_range.x);
}
result->r = tc[0];
result->g = tc[1];
result->b = tc[2];
}
void
u_compute_distortion_bounds_poly_3k(const struct xrt_matrix_3x3 *inv_affine_xform,
struct u_poly_3k_distortion_values *values,
int view,
struct xrt_fov *out_fov,
struct xrt_vec2 *out_tex_x_range,
struct xrt_vec2 *out_tex_y_range)
{
assert(view == 0 || view == 1);
float tanangle_left = 0.0f;
float tanangle_right = 0.0f;
float tanangle_up = 0.0f;
float tanangle_down = 0.0f;
for (int channel = 0; channel < 3; channel++) {
const struct xrt_vec2 eye_center = values[channel].eye_center;
const double *k = values[channel].k;
/* The X coords start at 0 for the left eye, and display_size.x / 2.0 for the right */
const struct xrt_vec2 pix_coords[4] = {
/* -eye_center_x, 0 */
{(1.0f * view) * (values->display_size.x / 2.0f) - eye_center.x, 0.0f},
/* 0, -eye_center_y */
{0.0f, -eye_center.y},
/* width-eye_center_x, 0 */
{(1.0f + 1.0f * view) * (values->display_size.x / 2.0f) - eye_center.x, 0.0f},
/* 0, height-eye_center_y */
{0.0f, values->display_size.y - eye_center.y},
};
for (int c = 0; c < 4; c++) {
const struct xrt_vec2 pix_coord = pix_coords[c];
float k1 = (float)k[0];
float k2 = (float)k[1];
float k3 = (float)k[2];
float r2 = m_vec2_dot(pix_coord, pix_coord);
/* distort the pixel */
float d = 1.0f + r2 * (k1 + r2 * (k2 + r2 * k3));
/* Map the distorted pixel coordinate back to normalised view plane coords using the inverse
* affine xform */
struct xrt_vec3 p = {(pix_coord.x * d + eye_center.x), (pix_coord.y * d + eye_center.y), 1.0f};
struct xrt_vec3 vp;
math_matrix_3x3_transform_vec3(inv_affine_xform, &p, &vp);
vp.x /= vp.z;
vp.y /= vp.z;
if (pix_coord.x < 0.0f) {
if (vp.x < tanangle_left)
tanangle_left = vp.x;
} else {
if (vp.x > tanangle_right)
tanangle_right = vp.x;
}
if (pix_coord.y < 0.0f) {
if (vp.y < tanangle_up)
tanangle_up = vp.y;
} else {
if (vp.y > tanangle_down)
tanangle_down = vp.y;
}
}
}
out_fov->angle_left = atanf(tanangle_left);
out_fov->angle_right = atanf(tanangle_right);
out_fov->angle_down = -atanf(tanangle_down);
out_fov->angle_up = -atanf(tanangle_up);
out_tex_x_range->x = tanf(out_fov->angle_left);
out_tex_x_range->y = tanf(out_fov->angle_right);
out_tex_y_range->x = tanf(out_fov->angle_down);
out_tex_y_range->y = tanf(out_fov->angle_up);
}
/*
*
* No distortion.
@@ -157,6 +157,57 @@ void
u_compute_distortion_ns_meshgrid(
struct u_ns_meshgrid_values *values, int view, float u, float v, struct xrt_uv_triplet *result);
/*
*
* Windows Mixed Reality distortion
*
*/
struct u_poly_3k_distortion_values
{
struct xrt_vec2_i32 display_size;
/* X/Y center of the distortion (pixels) */
struct xrt_vec2 eye_center;
/* k1,k2,k3 params for radial distortion as
* per the radial distortion model in
* https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html */
double k[3];
};
struct u_poly_3k_eye_values
{
//! Inverse affine transform to move from (undistorted) pixels
//! to image plane / normalised image coordinates
struct xrt_matrix_3x3 inv_affine_xform;
//! tan(angle) FoV min/max for X and Y in the input texture
struct xrt_vec2 tex_x_range;
struct xrt_vec2 tex_y_range;
//! Hack values for WMR devices with weird distortions
int32_t y_offset;
struct u_poly_3k_distortion_values channels[3];
};
void
u_compute_distortion_poly_3k(
struct u_poly_3k_eye_values *values, uint32_t view, float u, float v, struct xrt_uv_triplet *result);
/*
* Compute the visible area bounds by calculating the X/Y limits of a
* crosshair through the distortion center, and back-project to the render FoV,
*/
void
u_compute_distortion_bounds_poly_3k(const struct xrt_matrix_3x3 *inv_affine_xform,
struct u_poly_3k_distortion_values *values,
int view,
struct xrt_fov *out_fov,
struct xrt_vec2 *out_tex_x_range,
struct xrt_vec2 *out_tex_y_range);
/*
*
+13 -12
View File
@@ -90,13 +90,17 @@ wmr_config_parse_display(struct wmr_hmd_config *c, cJSON *display, enum u_loggin
return false;
}
struct xrt_matrix_3x3 affine_xform;
/* Extract display panel parameters */
cJSON *affine = cJSON_GetObjectItem(display, "Affine");
if (affine == NULL || u_json_get_float_array(affine, eye->affine_xform.v, 9) != 9) {
if (affine == NULL || u_json_get_float_array(affine, affine_xform.v, 9) != 9) {
WMR_ERROR(log_level, "Missing affine transform for AssignedEye \"%s\"", json_eye_name);
return false;
}
math_matrix_3x3_inverse(&affine_xform, &eye->poly_3k.inv_affine_xform);
if (!JSON_FLOAT(display, "DisplayWidth", &eye->display_size.x) ||
!JSON_FLOAT(display, "DisplayHeight", &eye->display_size.y))
return false;
@@ -133,7 +137,7 @@ wmr_config_parse_display(struct wmr_hmd_config *c, cJSON *display, enum u_loggin
const char *channel_names[] = {"DistortionRed", "DistortionGreen", "DistortionBlue"};
for (int channel = 0; channel < 3; ++channel) {
struct wmr_distortion_3K *distortion3K = &eye->distortion3K[channel];
struct u_poly_3k_eye_values *distortion3K = &eye->poly_3k;
cJSON *dist = cJSON_GetObjectItemCaseSensitive(display, channel_names[channel]);
if (!dist) {
@@ -147,11 +151,8 @@ wmr_config_parse_display(struct wmr_hmd_config *c, cJSON *display, enum u_loggin
return false;
}
if (!strcmp(model_type, "CALIBRATION_DisplayDistortionModelPolynomial3K")) {
distortion3K->model = WMR_DISTORTION_MODEL_POLYNOMIAL_3K;
} else {
distortion3K->model = WMR_DISTORTION_MODEL_UNKNOWN;
WMR_ERROR(log_level, "Unknown distortion model %s", model_type);
if (strcmp(model_type, "CALIBRATION_DisplayDistortionModelPolynomial3K") != 0) {
WMR_ERROR(log_level, "Unknown display distortion model %s", model_type);
return false;
}
@@ -170,12 +171,12 @@ wmr_config_parse_display(struct wmr_hmd_config *c, cJSON *display, enum u_loggin
return false;
}
distortion3K->eye_center.x = parameters[0];
distortion3K->eye_center.y = parameters[1];
distortion3K->channels[channel].eye_center.x = parameters[0];
distortion3K->channels[channel].eye_center.y = parameters[1];
distortion3K->k[0] = parameters[2];
distortion3K->k[1] = parameters[3];
distortion3K->k[2] = parameters[4];
distortion3K->channels[channel].k[0] = parameters[2];
distortion3K->channels[channel].k[1] = parameters[3];
distortion3K->channels[channel].k[2] = parameters[4];
}
return true;
+3 -17
View File
@@ -12,7 +12,9 @@
#include "math/m_vec2.h"
#include "math/m_vec3.h"
#include "util/u_logging.h"
#include "util/u_distortion_mesh.h"
/* Increase this number if anyone releases a headset with
* more cameras */
@@ -50,18 +52,6 @@ enum wmr_camera_purpose
WMR_CAMERA_PURPOSE_DISPLAY_OBSERVER,
};
struct wmr_distortion_3K
{
enum wmr_distortion_model model;
/* X/Y center of the distortion (pixels) */
struct xrt_vec2 eye_center;
/* k1,k2,k3 params for radial distortion as
* per the radial distortion model in
* https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html */
double k[3];
};
struct wmr_distortion_6KT
{
enum wmr_distortion_model model;
@@ -83,9 +73,6 @@ struct wmr_distortion_6KT
struct wmr_distortion_eye_config
{
/* 3x3 camera matrix that moves from normalised camera coords (X/Z & Y/Z) to undistorted pixels */
struct xrt_matrix_3x3 affine_xform;
struct xrt_vec3 translation; //!< Raw translation (to HT0)
struct xrt_matrix_3x3 rotation; //!< Raw rotation (to HT0), row major
struct xrt_pose pose; //!< Pose from `translation` and `rotation`
@@ -98,8 +85,7 @@ struct wmr_distortion_eye_config
/* Center for the eye viewport visibility (pixels) */
struct xrt_vec2 visible_center;
/* RGB distortion params */
struct wmr_distortion_3K distortion3K[3];
struct u_poly_3k_eye_values poly_3k; //!< Distortion parameters for each channel
};
struct wmr_camera_config
+21 -168
View File
@@ -1247,154 +1247,6 @@ wmr_hmd_destroy(struct xrt_device *xdev)
u_device_free(&wh->base);
}
static xrt_result_t
compute_distortion_wmr(struct xrt_device *xdev, uint32_t view, float u, float v, struct xrt_uv_triplet *result)
{
DRV_TRACE_MARKER();
struct wmr_hmd *wh = wmr_hmd(xdev);
assert(view == 0 || view == 1);
const struct wmr_distortion_eye_config *ec = wh->config.eye_params + view;
struct wmr_hmd_distortion_params *distortion_params = wh->distortion_params + view;
// Results r/g/b.
struct xrt_vec2 tc[3];
// Dear compiler, please vectorize.
for (int i = 0; i < 3; i++) {
const struct wmr_distortion_3K *distortion3K = ec->distortion3K + i;
/* Scale the 0..1 input UV back to pixels relative to the distortion center,
* accounting for the right eye starting at X = panel_width / 2.0 */
struct xrt_vec2 pix_coord = {(u + 1.0f * view) * (ec->display_size.x / 2.0f) -
distortion3K->eye_center.x,
v * ec->display_size.y - distortion3K->eye_center.y};
if (view == 0) {
pix_coord.y += (float)wh->left_view_y_offset;
} else if (view == 1) {
pix_coord.y += (float)wh->right_view_y_offset;
}
float r2 = m_vec2_dot(pix_coord, pix_coord);
float k1 = (float)distortion3K->k[0];
float k2 = (float)distortion3K->k[1];
float k3 = (float)distortion3K->k[2];
float d = 1.0f + r2 * (k1 + r2 * (k2 + r2 * k3));
/* Map the distorted pixel coordinate back to normalised view plane coords using the inverse affine
* xform */
struct xrt_vec3 p = {(pix_coord.x * d + distortion3K->eye_center.x),
(pix_coord.y * d + distortion3K->eye_center.y), 1.0f};
struct xrt_vec3 vp;
math_matrix_3x3_transform_vec3(&distortion_params->inv_affine_xform, &p, &vp);
/* Finally map back to the input texture 0..1 range based on the render FoV (from tex_N_range.x ..
* tex_N_range.y) */
tc[i].x = ((vp.x / vp.z) - distortion_params->tex_x_range.x) /
(distortion_params->tex_x_range.y - distortion_params->tex_x_range.x);
tc[i].y = ((vp.y / vp.z) - distortion_params->tex_y_range.x) /
(distortion_params->tex_y_range.y - distortion_params->tex_y_range.x);
}
result->r = tc[0];
result->g = tc[1];
result->b = tc[2];
return XRT_SUCCESS;
}
/*
* Compute the visible area bounds by calculating the X/Y limits of a
* crosshair through the distortion center, and back-project to the render FoV,
*/
static void
compute_distortion_bounds(struct wmr_hmd *wh,
int view,
float *out_angle_left,
float *out_angle_right,
float *out_angle_down,
float *out_angle_up)
{
DRV_TRACE_MARKER();
assert(view == 0 || view == 1);
float tanangle_left = 0.0f;
float tanangle_right = 0.0f;
float tanangle_up = 0.0f;
float tanangle_down = 0.0f;
const struct wmr_distortion_eye_config *ec = wh->config.eye_params + view;
struct wmr_hmd_distortion_params *distortion_params = wh->distortion_params + view;
for (int i = 0; i < 3; i++) {
const struct wmr_distortion_3K *distortion3K = ec->distortion3K + i;
/* The X coords start at 0 for the left eye, and display_size.x / 2.0 for the right */
const struct xrt_vec2 pix_coords[4] = {
/* -eye_center_x, 0 */
{(1.0f * view) * (ec->display_size.x / 2.0f) - distortion3K->eye_center.x, 0.0f},
/* 0, -eye_center_y */
{0.0f, -distortion3K->eye_center.y},
/* width-eye_center_x, 0 */
{(1.0f + 1.0f * view) * (ec->display_size.x / 2.0f) - distortion3K->eye_center.x, 0.0f},
/* 0, height-eye_center_y */
{0.0f, ec->display_size.y - distortion3K->eye_center.y},
};
for (int c = 0; c < 4; c++) {
const struct xrt_vec2 pix_coord = pix_coords[c];
float k1 = distortion3K->k[0];
float k2 = distortion3K->k[1];
float k3 = distortion3K->k[2];
float r2 = m_vec2_dot(pix_coord, pix_coord);
/* distort the pixel */
float d = 1.0f + r2 * (k1 + r2 * (k2 + r2 * k3));
/* Map the distorted pixel coordinate back to normalised view plane coords using the inverse
* affine xform */
struct xrt_vec3 p = {(pix_coord.x * d + distortion3K->eye_center.x),
(pix_coord.y * d + distortion3K->eye_center.y), 1.0f};
struct xrt_vec3 vp;
math_matrix_3x3_transform_vec3(&distortion_params->inv_affine_xform, &p, &vp);
vp.x /= vp.z;
vp.y /= vp.z;
if (pix_coord.x < 0.0f) {
if (vp.x < tanangle_left)
tanangle_left = vp.x;
} else {
if (vp.x > tanangle_right)
tanangle_right = vp.x;
}
if (pix_coord.y < 0.0f) {
if (vp.y < tanangle_up)
tanangle_up = vp.y;
} else {
if (vp.y > tanangle_down)
tanangle_down = vp.y;
}
WMR_DEBUG(wh, "channel %d delta coord %f, %f d pixel %f %f, %f -> %f, %f", i, pix_coord.x,
pix_coord.y, d, p.x, p.y, vp.x, vp.y);
}
}
*out_angle_left = atanf(tanangle_left);
*out_angle_right = atanf(tanangle_right);
*out_angle_down = -atanf(tanangle_down);
*out_angle_up = -atanf(tanangle_up);
}
XRT_MAYBE_UNUSED static struct t_camera_calibration
wmr_hmd_get_cam_calib(struct wmr_hmd *wh, int cam_index)
{
@@ -1928,6 +1780,16 @@ wmr_hmd_request_controller_status(struct wmr_hmd *wh)
return wmr_hmd_send_controller_packet(wh, cmd, sizeof(cmd));
}
static xrt_result_t
compute_distortion_wmr(struct xrt_device *xdev, uint32_t view, float u, float v, struct xrt_uv_triplet *out_result)
{
struct wmr_hmd *wh = wmr_hmd(xdev);
u_compute_distortion_poly_3k(&wh->config.eye_params[view].poly_3k, view, u, v, out_result);
return XRT_SUCCESS;
}
void
wmr_hmd_create(enum wmr_headset_type hmd_type,
struct os_hid_device *hid_holo,
@@ -1961,9 +1823,6 @@ wmr_hmd_create(enum wmr_headset_type hmd_type,
wh->base.device_type = XRT_DEVICE_TYPE_HMD;
wh->log_level = log_level;
wh->left_view_y_offset = debug_get_num_option_left_view_y_offset();
wh->right_view_y_offset = debug_get_num_option_right_view_y_offset();
wh->hid_hololens_sensors_dev = hid_holo;
wh->hid_control_dev = hid_ctrl;
@@ -2065,28 +1924,22 @@ wmr_hmd_create(enum wmr_headset_type hmd_type,
wh->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_OPAQUE;
wh->base.hmd->blend_mode_count = idx;
wh->config.eye_params[0].poly_3k.y_offset = debug_get_num_option_left_view_y_offset();
wh->config.eye_params[1].poly_3k.y_offset = debug_get_num_option_right_view_y_offset();
// Distortion information, fills in xdev->compute_distortion().
for (eye = 0; eye < 2; eye++) {
math_matrix_3x3_inverse(&wh->config.eye_params[eye].affine_xform,
&wh->distortion_params[eye].inv_affine_xform);
struct xrt_fov *fov = &wh->base.hmd->distortion.fov[eye];
struct u_poly_3k_eye_values *poly_3k = &wh->config.eye_params[eye].poly_3k;
compute_distortion_bounds(wh, eye, &wh->base.hmd->distortion.fov[eye].angle_left,
&wh->base.hmd->distortion.fov[eye].angle_right,
&wh->base.hmd->distortion.fov[eye].angle_down,
&wh->base.hmd->distortion.fov[eye].angle_up);
u_compute_distortion_bounds_poly_3k(&poly_3k->inv_affine_xform, poly_3k->channels, eye, fov,
&poly_3k->tex_x_range, &poly_3k->tex_y_range);
WMR_INFO(wh, "FoV eye %d angles left %f right %f down %f up %f", eye,
wh->base.hmd->distortion.fov[eye].angle_left, wh->base.hmd->distortion.fov[eye].angle_right,
wh->base.hmd->distortion.fov[eye].angle_down, wh->base.hmd->distortion.fov[eye].angle_up);
WMR_INFO(wh, "FoV eye %d angles left %f right %f down %f up %f", eye, fov->angle_left, fov->angle_right,
fov->angle_down, fov->angle_up);
wh->distortion_params[eye].tex_x_range.x = tanf(wh->base.hmd->distortion.fov[eye].angle_left);
wh->distortion_params[eye].tex_x_range.y = tanf(wh->base.hmd->distortion.fov[eye].angle_right);
wh->distortion_params[eye].tex_y_range.x = tanf(wh->base.hmd->distortion.fov[eye].angle_down);
wh->distortion_params[eye].tex_y_range.y = tanf(wh->base.hmd->distortion.fov[eye].angle_up);
WMR_INFO(wh, "Render texture range %f, %f to %f, %f", wh->distortion_params[eye].tex_x_range.x,
wh->distortion_params[eye].tex_y_range.x, wh->distortion_params[eye].tex_x_range.y,
wh->distortion_params[eye].tex_y_range.y);
WMR_INFO(wh, "Render texture range %f, %f to %f, %f", poly_3k->tex_x_range.x, poly_3k->tex_y_range.x,
poly_3k->tex_x_range.y, poly_3k->tex_y_range.y);
}
wh->base.hmd->distortion.models = XRT_DISTORTION_MODEL_COMPUTE;
-16
View File
@@ -54,17 +54,6 @@ struct wmr_headset_descriptor
void (*screen_enable_func)(struct wmr_hmd *wh, bool enable);
};
struct wmr_hmd_distortion_params
{
//! Inverse affine transform to move from (undistorted) pixels
//! to image plane / normalised image coordinates
struct xrt_matrix_3x3 inv_affine_xform;
//! tan(angle) FoV min/max for X and Y in the input texture
struct xrt_vec2 tex_x_range;
struct xrt_vec2 tex_y_range;
};
/*!
* @implements xrt_device
*/
@@ -85,8 +74,6 @@ struct wmr_hmd
enum u_logging_level log_level;
int32_t left_view_y_offset, right_view_y_offset;
/*!
* This is the Hololens Sensors device, this is where we get all of the
* IMU data and read the config from.
@@ -114,9 +101,6 @@ struct wmr_hmd
//! Latest proximity sensor value read from the device.
uint8_t proximity_sensor;
//! Distortion related parameters
struct wmr_hmd_distortion_params distortion_params[2];
struct hololens_sensors_packet packet;
struct