d/wmr: Add some groundwork for config parsing.

Sketch out some groundwork for parsing WMR config blocks,
using some hard-coded values from my headset for now. Later,
this will be parsed from the headset JSON blob.

src/xrt/drivers/CMakeLists.txt

@@ -309,6 +309,8 @@ list(APPEND ENABLED_HEADSET_DRIVERS drv_multi)
 if(XRT_BUILD_DRIVER_WMR)
 	set(WMR_SOURCE_FILES
 		wmr/wmr_common.h
+		wmr/wmr_config.c
+		wmr/wmr_config.h
 		wmr/wmr_hmd.c
 		wmr/wmr_hmd.h
 		wmr/wmr_interface.h

src/xrt/drivers/meson.build

@@ -288,6 +288,7 @@ lib_drv_wmr = static_library(
 	'drv_wmr',
 	files(
 		'wmr/wmr_common.h',
+		'wmr/wmr_config.c',
 		'wmr/wmr_hmd.c',
 		'wmr/wmr_hmd.h',
 		'wmr/wmr_interface.h',

src/xrt/drivers/wmr/wmr_config.c

@@ -0,0 +1,70 @@
+/* Copyright 2021, Jan Schmidt
+ * SPDX-License-Identifier: BSL-1.0
+ */
+/*!
+ * @file
+ * @brief	Driver code to read WMR config blocks
+ * @author Jan Schmidt <jan@centricular.com>
+ * @ingroup drv_wmr
+ */
+#include "wmr_config.h"
+
+bool
+wmr_config_parse(struct wmr_hmd_config *c)
+{
+	int i, j, k;
+
+	const struct xrt_vec2 display_size[2] = {{4320, 2160}, {4320, 2160}};
+
+	// eye_centers from VisibleAreaCenter X/Y
+	struct xrt_vec2 eye_centers[2] = {
+	    {1171.2011028243148, 1078.7720082720277},
+	    {3154.10490135909, 1085.7209119898746},
+	};
+
+	const double eye_radius[2] = {1500, 1500};
+	const double eye_affine[2][9] = {
+	    {1467.741455078125, -0, 1171.2010498046875, -0, 1467.642333984375, 1078.77197265625, 0, 0, 1},
+	    {1469.2613525390625, -0, 3154.10498046875, -0, 1468.5185546875, 1085.720947265625, 0, 0, 1}};
+
+	// These need to be acquired from the WMR config:
+	/* From DistortionRed/Green/Blue ModelParameters[0] and [1] */
+	const struct xrt_vec2 eye_centers_rgb[2][3][2] = {{{{1173.7816892048809, 1079.9493112867228}},
+	                                                   {{1171.8855282399534, 1078.1630786236972}},
+	                                                   {{1169.6008128643944, 1074.7670304358412}}},
+	                                                  {{{3150.2395019256492, 1086.1749083261475}},
+	                                                   {{3149.7810962925541, 1084.9113795043713}},
+	                                                   {{3150.3983095783842, 1082.3335369357619}}}};
+	/* From DistortionRed/Green/Blue ModelParameters [2,3,4] */
+	const double distortion_params[2][3][3] = {
+	    {
+	        {1.6392082863852426E-7, 4.0096839564631026E-14, 6.6538855737065E-20},   /* Red */
+	        {2.1590946493033866E-7, -4.78028658789357E-14, 1.1929574027716904E-19}, /* Green */
+	        {3.1991456111909366E-7, -2.300137347653273E-13, 2.3405778580046485E-19} /* Blue */
+	    },
+	    {
+	        {1.5982850735663643E-7, 4.990973924637425E-14, 6.0056239395619067E-20},   /* Red */
+	        {2.1206804797012724E-7, -3.5561864117498794E-14, 1.0992145779675043E-19}, /* Green */
+	        {3.1395508877599257E-7, -2.0999418299177255E-13, 2.1828476911150306E-19}  /* Blue */
+	    }};
+
+	for (i = 0; i < 2; i++) {
+		struct wmr_distortion_eye_config *eye = c->eye_params + i;
+
+		eye->display_size = display_size[i];
+		eye->visible_center = eye_centers[i];
+		eye->visible_radius = eye_radius[i];
+		for (j = 0; j < 9; j++) {
+			eye->affine_xform.v[j] = eye_affine[i][j];
+		}
+
+		for (j = 0; j < 3; j++) {
+			/* RGB distortion params */
+			eye->distortion3K[j].eye_center = *eye_centers_rgb[i][j];
+			for (k = 0; k < 3; k++) {
+				eye->distortion3K[j].k[k] = distortion_params[i][j][k];
+			}
+		}
+	}
+	return true;
+}

src/xrt/drivers/wmr/wmr_config.h

@@ -0,0 +1,58 @@
+/* Copyright 2021 Jan Schmidt
+ * SPDX-License-Identifier: BSL-1.0
+ */
+/*!
+ * @file
+ * @brief	WMR and MS HoloLens configuration structures
+ * @author Jan Schmidt <jan@centricular.com>
+ * @ingroup drv_wmr
+ */
+
+#pragma once
+
+#include "math/m_vec2.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct wmr_distortion_3K
+{
+	/* 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/master/d9/d0c/group__calib3d.html */
+	double k[3];
+};
+
+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;
+	/* Radius of the (undistorted) visible area from the center (pixels) (I think) */
+	double visible_radius;
+
+	/* Width, Height (pixels) of the full display */
+	struct xrt_vec2 display_size;
+	/* Center for the eye viewport visibility (pixels) */
+	struct xrt_vec2 visible_center;
+
+	/* RGB distortion params */
+	struct wmr_distortion_3K distortion3K[3];
+};
+
+struct wmr_hmd_config
+{
+	/* Left and Right eye mapping and distortion params */
+	struct wmr_distortion_eye_config eye_params[2];
+};
+
+/* FIXME:	Pass JSON config when we have that: */
+bool
+wmr_config_parse(struct wmr_hmd_config *c);
+
+#ifdef __cplusplus
+}
+#endif

src/xrt/drivers/wmr/wmr_hmd.c

@@ -508,6 +508,13 @@ wmr_hmd_create(struct os_hid_device *hid_holo, struct os_hid_device *hid_ctrl, e
 	wh->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE;
 
 	// TODO: Read config file from HMD, provide guestimate values for now.
+	if (!wmr_config_parse(&wh->config)) {
+		WMR_ERROR(wh, "Failed to load headset configuration!");
+		wmr_hmd_destroy(&wh->base);
+		wh = NULL;
+		return NULL;
+	}
+
 	struct u_device_simple_info info;
 	info.display.w_pixels = 4320;
 	info.display.h_pixels = 2160;

src/xrt/drivers/wmr/wmr_hmd.h

@@ -20,7 +20,7 @@
 #include "util/u_logging.h"
 
 #include "wmr_protocol.h"
-
+#include "wmr_config.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -48,6 +48,9 @@ struct wmr_hmd
 {
 	struct xrt_device base;
 
+	// Config block read from the firmware JSON
+	struct wmr_hmd_config config;
+
 	//! Packet reading thread.
 	struct os_thread_helper oth;