d/wmr: Load IMU calibration

This commit is contained in:
Mateo de Mayo 2022-05-23 10:58:43 -03:00
parent 02abc49313
commit 3bf07a2711
3 changed files with 82 additions and 17 deletions

View file

@ -193,15 +193,22 @@ wmr_inertial_sensor_config_parse(struct wmr_inertial_sensor_config *c, cJSON *se
}
wmr_config_compute_pose(&c->pose, &translation, &rotation);
c->translation = translation;
c->rotation = rotation;
/* compute the bias offsets and mix matrix by taking the constant
* coefficients from the configuration */
cJSON *mix_model = cJSON_GetObjectItem(sensor, "MixingMatrixTemperatureModel");
cJSON *bias_model = cJSON_GetObjectItem(sensor, "BiasTemperatureModel");
cJSON *bias_var = cJSON_GetObjectItem(sensor, "BiasUncertainty");
cJSON *noise_std = cJSON_GetObjectItem(sensor, "Noise");
float mix_model_values[3 * 3 * 4];
float bias_model_values[12];
float bias_var_values[3];
float noise_std_values[3 * 2];
if (mix_model == NULL || bias_model == NULL) {
if (mix_model == NULL || bias_model == NULL || noise_std == NULL || bias_var == NULL) {
WMR_WARN(log_level, "Missing Inertial Sensor calibration");
return false;
}
@ -222,6 +229,21 @@ wmr_inertial_sensor_config_parse(struct wmr_inertial_sensor_config *c, cJSON *se
c->bias_offsets.y = bias_model_values[4];
c->bias_offsets.z = bias_model_values[8];
if (u_json_get_float_array(bias_var, bias_var_values, 3) != 3) {
WMR_WARN(log_level, "Invalid Inertial Sensor calibration (invalid BiasUncertainty)");
return false;
}
c->bias_var.x = bias_var_values[0];
c->bias_var.y = bias_var_values[1];
c->bias_var.z = bias_var_values[2];
if (u_json_get_float_array(noise_std, noise_std_values, 3 * 2) != 3 * 2) {
WMR_WARN(log_level, "Invalid Inertial Sensor calibration (invalid Noise)");
return false;
}
c->noise_std.x = noise_std_values[0];
c->noise_std.y = noise_std_values[1];
c->noise_std.z = noise_std_values[2];
return true;
}

View file

@ -107,7 +107,7 @@ struct wmr_camera_config
struct xrt_rect roi;
struct xrt_vec3 translation; //!< Raw translation (to HT0)
struct xrt_matrix_3x3 rotation; //!< Raw rotation (to HT0)
struct xrt_matrix_3x3 rotation; //!< Raw rotation (to HT0), row major
struct xrt_pose pose; //!< Corrected pose from translation and rotation fields
struct wmr_distortion_6KT distortion6KT;
@ -116,7 +116,9 @@ struct wmr_camera_config
/* Configuration for a single inertial sensor */
struct wmr_inertial_sensor_config
{
struct xrt_pose pose;
struct xrt_vec3 translation; //!< Raw translation (to HT0). Usually non-zero only on accelerometers.
struct xrt_matrix_3x3 rotation; //!< Raw rotation (to HT0), row major
struct xrt_pose pose; //!< Corrected pose from translation and rotation fields
/* Current bias and mix matrix extracted from
* the configuration, which provides coefficients
@ -124,6 +126,12 @@ struct wmr_inertial_sensor_config
* so we just take the constant coefficient */
struct xrt_vec3 bias_offsets;
struct xrt_matrix_3x3 mix_matrix;
//! Bias random walk variance. @see slam_tracker::inertial_calibration.
struct xrt_vec3 bias_var;
//! Measurement noise standard deviation. @see slam_tracker::inertial_calibration.
struct xrt_vec3 noise_std;
};
/* Configuration for the set of inertial sensors */

View file

@ -15,6 +15,7 @@
#include "xrt/xrt_config_have.h"
#include "xrt/xrt_config_build.h"
#include "xrt/xrt_config_os.h"
#include "xrt/xrt_defines.h"
#include "xrt/xrt_device.h"
#include "xrt/xrt_tracking.h"
@ -1248,21 +1249,55 @@ wmr_hmd_create_stereo_camera_calib(struct wmr_hmd *wh)
tcc->use_fisheye = false;
}
// Extrinsics (transform HT1 to HT0; or equivalently HT0 space into HT1 space)
struct wmr_camera_config *cam2 = &wh->config.cameras[1];
calib->camera_translation[0] = cam2->translation.x;
calib->camera_translation[1] = cam2->translation.y;
calib->camera_translation[2] = cam2->translation.z;
calib->camera_rotation[0][0] = cam2->rotation.v[0];
calib->camera_rotation[0][1] = cam2->rotation.v[1];
calib->camera_rotation[0][2] = cam2->rotation.v[2];
calib->camera_rotation[1][0] = cam2->rotation.v[3];
calib->camera_rotation[1][1] = cam2->rotation.v[4];
calib->camera_rotation[1][2] = cam2->rotation.v[5];
calib->camera_rotation[2][0] = cam2->rotation.v[6];
calib->camera_rotation[2][1] = cam2->rotation.v[7];
calib->camera_rotation[2][2] = cam2->rotation.v[8];
// Extrinsics
// Compute transform from HT1 to HT0 (HT0 space into HT1 space)
struct wmr_camera_config *ht1 = &wh->config.cameras[1];
calib->camera_translation[0] = ht1->translation.x;
calib->camera_translation[1] = ht1->translation.y;
calib->camera_translation[2] = ht1->translation.z;
calib->camera_rotation[0][0] = ht1->rotation.v[0];
calib->camera_rotation[0][1] = ht1->rotation.v[1];
calib->camera_rotation[0][2] = ht1->rotation.v[2];
calib->camera_rotation[1][0] = ht1->rotation.v[3];
calib->camera_rotation[1][1] = ht1->rotation.v[4];
calib->camera_rotation[1][2] = ht1->rotation.v[5];
calib->camera_rotation[2][0] = ht1->rotation.v[6];
calib->camera_rotation[2][1] = ht1->rotation.v[7];
calib->camera_rotation[2][2] = ht1->rotation.v[8];
return calib;
}
XRT_MAYBE_UNUSED static struct t_imu_calibration
wmr_hmd_create_imu_calib(struct wmr_hmd *wh)
{
float *at = wh->config.sensors.accel.mix_matrix.v;
struct xrt_vec3 ao = wh->config.sensors.accel.bias_offsets;
struct xrt_vec3 ab = wh->config.sensors.accel.bias_var;
struct xrt_vec3 an = wh->config.sensors.accel.noise_std;
float *gt = wh->config.sensors.gyro.mix_matrix.v;
struct xrt_vec3 go = wh->config.sensors.gyro.bias_offsets;
struct xrt_vec3 gb = wh->config.sensors.gyro.bias_var;
struct xrt_vec3 gn = wh->config.sensors.gyro.noise_std;
struct t_imu_calibration calib = {
.accel =
{
.transform = {{at[0], at[1], at[2]}, {at[3], at[4], at[5]}, {at[6], at[7], at[8]}},
.offset = {-ao.x, -ao.y, -ao.z}, // negative because slam system will add, not subtract
.bias_std = {sqrt(ab.x), sqrt(ab.y), sqrt(ab.z)}, // sqrt because we want stdev not variance
.noise_std = {an.x, an.y, an.z},
},
.gyro =
{
.transform = {{gt[0], gt[1], gt[2]}, {gt[3], gt[4], gt[5]}, {gt[6], gt[7], gt[8]}},
.offset = {-go.x, -go.y, -go.z},
.bias_std = {sqrt(gb.x), sqrt(gb.y), sqrt(gb.z)},
.noise_std = {gn.x, gn.y, gn.z},
},
};
return calib;
}