The open source OpenXR runtime
d/wmr: Load IMU calibration
+82
-17
+23
-1
src/xrt/drivers/wmr/wmr_config.c
+23
-1
src/xrt/drivers/wmr/wmr_config.c
···
193
193
}
194
194
195
195
wmr_config_compute_pose(&c->pose, &translation, &rotation);
196
+
c->translation = translation;
197
+
c->rotation = rotation;
196
198
197
199
/* compute the bias offsets and mix matrix by taking the constant
198
200
* coefficients from the configuration */
199
201
cJSON *mix_model = cJSON_GetObjectItem(sensor, "MixingMatrixTemperatureModel");
200
202
cJSON *bias_model = cJSON_GetObjectItem(sensor, "BiasTemperatureModel");
203
+
cJSON *bias_var = cJSON_GetObjectItem(sensor, "BiasUncertainty");
204
+
cJSON *noise_std = cJSON_GetObjectItem(sensor, "Noise");
205
+
201
206
float mix_model_values[3 * 3 * 4];
202
207
float bias_model_values[12];
208
+
float bias_var_values[3];
209
+
float noise_std_values[3 * 2];
203
210
204
-
if (mix_model == NULL || bias_model == NULL) {
211
+
if (mix_model == NULL || bias_model == NULL || noise_std == NULL || bias_var == NULL) {
205
212
WMR_WARN(log_level, "Missing Inertial Sensor calibration");
206
213
return false;
207
214
}
···
222
229
c->bias_offsets.y = bias_model_values[4];
223
230
c->bias_offsets.z = bias_model_values[8];
224
231
232
+
if (u_json_get_float_array(bias_var, bias_var_values, 3) != 3) {
233
+
WMR_WARN(log_level, "Invalid Inertial Sensor calibration (invalid BiasUncertainty)");
234
+
return false;
235
+
}
236
+
c->bias_var.x = bias_var_values[0];
237
+
c->bias_var.y = bias_var_values[1];
238
+
c->bias_var.z = bias_var_values[2];
239
+
240
+
if (u_json_get_float_array(noise_std, noise_std_values, 3 * 2) != 3 * 2) {
241
+
WMR_WARN(log_level, "Invalid Inertial Sensor calibration (invalid Noise)");
242
+
return false;
243
+
}
244
+
c->noise_std.x = noise_std_values[0];
245
+
c->noise_std.y = noise_std_values[1];
246
+
c->noise_std.z = noise_std_values[2];
225
247
return true;
226
248
}
227
249
+10
-2
src/xrt/drivers/wmr/wmr_config.h
+10
-2
src/xrt/drivers/wmr/wmr_config.h
···
107
107
struct xrt_rect roi;
108
108
109
109
struct xrt_vec3 translation; //!< Raw translation (to HT0)
110
-
struct xrt_matrix_3x3 rotation; //!< Raw rotation (to HT0)
110
+
struct xrt_matrix_3x3 rotation; //!< Raw rotation (to HT0), row major
111
111
struct xrt_pose pose; //!< Corrected pose from translation and rotation fields
112
112
113
113
struct wmr_distortion_6KT distortion6KT;
···
116
116
/* Configuration for a single inertial sensor */
117
117
struct wmr_inertial_sensor_config
118
118
{
119
-
struct xrt_pose pose;
119
+
struct xrt_vec3 translation; //!< Raw translation (to HT0). Usually non-zero only on accelerometers.
120
+
struct xrt_matrix_3x3 rotation; //!< Raw rotation (to HT0), row major
121
+
struct xrt_pose pose; //!< Corrected pose from translation and rotation fields
120
122
121
123
/* Current bias and mix matrix extracted from
122
124
* the configuration, which provides coefficients
···
124
126
* so we just take the constant coefficient */
125
127
struct xrt_vec3 bias_offsets;
126
128
struct xrt_matrix_3x3 mix_matrix;
129
+
130
+
//! Bias random walk variance. @see slam_tracker::inertial_calibration.
131
+
struct xrt_vec3 bias_var;
132
+
133
+
//! Measurement noise standard deviation. @see slam_tracker::inertial_calibration.
134
+
struct xrt_vec3 noise_std;
127
135
};
128
136
129
137
/* Configuration for the set of inertial sensors */
+49
-14
src/xrt/drivers/wmr/wmr_hmd.c
+49
-14
src/xrt/drivers/wmr/wmr_hmd.c
···
15
15
#include "xrt/xrt_config_have.h"
16
16
#include "xrt/xrt_config_build.h"
17
17
#include "xrt/xrt_config_os.h"
18
+
#include "xrt/xrt_defines.h"
18
19
#include "xrt/xrt_device.h"
19
20
#include "xrt/xrt_tracking.h"
20
21
···
1248
1249
tcc->use_fisheye = false;
1249
1250
}
1250
1251
1251
-
// Extrinsics (transform HT1 to HT0; or equivalently HT0 space into HT1 space)
1252
-
struct wmr_camera_config *cam2 = &wh->config.cameras[1];
1253
-
calib->camera_translation[0] = cam2->translation.x;
1254
-
calib->camera_translation[1] = cam2->translation.y;
1255
-
calib->camera_translation[2] = cam2->translation.z;
1256
-
calib->camera_rotation[0][0] = cam2->rotation.v[0];
1257
-
calib->camera_rotation[0][1] = cam2->rotation.v[1];
1258
-
calib->camera_rotation[0][2] = cam2->rotation.v[2];
1259
-
calib->camera_rotation[1][0] = cam2->rotation.v[3];
1260
-
calib->camera_rotation[1][1] = cam2->rotation.v[4];
1261
-
calib->camera_rotation[1][2] = cam2->rotation.v[5];
1262
-
calib->camera_rotation[2][0] = cam2->rotation.v[6];
1263
-
calib->camera_rotation[2][1] = cam2->rotation.v[7];
1264
-
calib->camera_rotation[2][2] = cam2->rotation.v[8];
1252
+
// Extrinsics
1253
+
1254
+
// Compute transform from HT1 to HT0 (HT0 space into HT1 space)
1255
+
struct wmr_camera_config *ht1 = &wh->config.cameras[1];
1256
+
calib->camera_translation[0] = ht1->translation.x;
1257
+
calib->camera_translation[1] = ht1->translation.y;
1258
+
calib->camera_translation[2] = ht1->translation.z;
1259
+
calib->camera_rotation[0][0] = ht1->rotation.v[0];
1260
+
calib->camera_rotation[0][1] = ht1->rotation.v[1];
1261
+
calib->camera_rotation[0][2] = ht1->rotation.v[2];
1262
+
calib->camera_rotation[1][0] = ht1->rotation.v[3];
1263
+
calib->camera_rotation[1][1] = ht1->rotation.v[4];
1264
+
calib->camera_rotation[1][2] = ht1->rotation.v[5];
1265
+
calib->camera_rotation[2][0] = ht1->rotation.v[6];
1266
+
calib->camera_rotation[2][1] = ht1->rotation.v[7];
1267
+
calib->camera_rotation[2][2] = ht1->rotation.v[8];
1265
1268
1269
+
return calib;
1270
+
}
1271
+
1272
+
XRT_MAYBE_UNUSED static struct t_imu_calibration
1273
+
wmr_hmd_create_imu_calib(struct wmr_hmd *wh)
1274
+
{
1275
+
float *at = wh->config.sensors.accel.mix_matrix.v;
1276
+
struct xrt_vec3 ao = wh->config.sensors.accel.bias_offsets;
1277
+
struct xrt_vec3 ab = wh->config.sensors.accel.bias_var;
1278
+
struct xrt_vec3 an = wh->config.sensors.accel.noise_std;
1279
+
1280
+
float *gt = wh->config.sensors.gyro.mix_matrix.v;
1281
+
struct xrt_vec3 go = wh->config.sensors.gyro.bias_offsets;
1282
+
struct xrt_vec3 gb = wh->config.sensors.gyro.bias_var;
1283
+
struct xrt_vec3 gn = wh->config.sensors.gyro.noise_std;
1284
+
1285
+
struct t_imu_calibration calib = {
1286
+
.accel =
1287
+
{
1288
+
.transform = {{at[0], at[1], at[2]}, {at[3], at[4], at[5]}, {at[6], at[7], at[8]}},
1289
+
.offset = {-ao.x, -ao.y, -ao.z}, // negative because slam system will add, not subtract
1290
+
.bias_std = {sqrt(ab.x), sqrt(ab.y), sqrt(ab.z)}, // sqrt because we want stdev not variance
1291
+
.noise_std = {an.x, an.y, an.z},
1292
+
},
1293
+
.gyro =
1294
+
{
1295
+
.transform = {{gt[0], gt[1], gt[2]}, {gt[3], gt[4], gt[5]}, {gt[6], gt[7], gt[8]}},
1296
+
.offset = {-go.x, -go.y, -go.z},
1297
+
.bias_std = {sqrt(gb.x), sqrt(gb.y), sqrt(gb.z)},
1298
+
.noise_std = {gn.x, gn.y, gn.z},
1299
+
},
1300
+
};
1266
1301
return calib;
1267
1302
}
1268
1303