mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-02-03 12:28:07 +00:00
d/wmr: Add camera configuration block parsing
Read camera configuration info from the HMD JSON block
This commit is contained in:
parent
7fb94889a8
commit
e422be89fa
src/xrt/drivers/wmr
|
@ -211,6 +211,126 @@ wmr_config_parse_inertial_sensor(struct wmr_hmd_config *c, cJSON *sensor, enum u
|
|||
return true;
|
||||
}
|
||||
|
||||
static bool
|
||||
wmr_config_parse_camera_config(struct wmr_hmd_config *c, cJSON *camera, enum u_logging_level ll)
|
||||
{
|
||||
if (c->n_cameras == WMR_MAX_CAMERAS) {
|
||||
WMR_ERROR(ll, "Too many camera entries. Enlarge WMR_MAX_CAMERAS");
|
||||
return false;
|
||||
}
|
||||
|
||||
struct wmr_camera_config *cam_config = c->cameras + c->n_cameras;
|
||||
|
||||
/* Camera purpose */
|
||||
cJSON *json_purpose = cJSON_GetObjectItem(camera, "Purpose");
|
||||
char *json_purpose_name = cJSON_GetStringValue(json_purpose);
|
||||
if (json_purpose_name == NULL) {
|
||||
WMR_ERROR(ll, "Invalid camera calibration block %d - unknown camera purpose %s", c->n_cameras,
|
||||
json_purpose_name);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!strcmp(json_purpose_name, "CALIBRATION_CameraPurposeHeadTracking")) {
|
||||
cam_config->purpose = WMR_CAMERA_PURPOSE_HEAD_TRACKING;
|
||||
} else if (!strcmp(json_purpose_name, "CALIBRATION_CameraPurposeDisplayObserver")) {
|
||||
cam_config->purpose = WMR_CAMERA_PURPOSE_DISPLAY_OBSERVER;
|
||||
} else {
|
||||
WMR_ERROR(ll, "Unknown camera purpose: \"%s\" (camera %d)", json_purpose_name, c->n_cameras);
|
||||
return false;
|
||||
}
|
||||
|
||||
cJSON *json_location = cJSON_GetObjectItem(camera, "Location");
|
||||
char *json_location_name = cJSON_GetStringValue(json_location);
|
||||
if (json_location_name == NULL) {
|
||||
WMR_ERROR(ll, "Invalid camera calibration block %d - location", c->n_cameras);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!strcmp(json_location_name, "CALIBRATION_CameraLocationHT0")) {
|
||||
cam_config->location = WMR_CAMERA_LOCATION_HT0;
|
||||
} else if (!strcmp(json_location_name, "CALIBRATION_CameraLocationHT1")) {
|
||||
cam_config->location = WMR_CAMERA_LOCATION_HT1;
|
||||
} else if (!strcmp(json_location_name, "CALIBRATION_CameraLocationHT2")) {
|
||||
cam_config->location = WMR_CAMERA_LOCATION_HT2;
|
||||
} else if (!strcmp(json_location_name, "CALIBRATION_CameraLocationHT3")) {
|
||||
cam_config->location = WMR_CAMERA_LOCATION_HT3;
|
||||
} else if (!strcmp(json_location_name, "CALIBRATION_CameraLocationDO0")) {
|
||||
cam_config->location = WMR_CAMERA_LOCATION_DO0;
|
||||
} else if (!strcmp(json_location_name, "CALIBRATION_CameraLocationDO1")) {
|
||||
cam_config->location = WMR_CAMERA_LOCATION_DO1;
|
||||
} else {
|
||||
WMR_ERROR(ll, "Unknown camera location: \"%s\" (camera %d)", json_location_name, c->n_cameras);
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Camera pose */
|
||||
struct xrt_vec3 translation;
|
||||
struct xrt_matrix_3x3 rotation;
|
||||
|
||||
cJSON *rt = cJSON_GetObjectItem(camera, "Rt");
|
||||
cJSON *rx = cJSON_GetObjectItem(rt, "Rotation");
|
||||
if (rt == NULL || rx == NULL) {
|
||||
WMR_ERROR(ll, "Invalid camera calibration block %d - pose", c->n_cameras);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!JSON_VEC3(rt, "Translation", &translation) || u_json_get_float_array(rx, rotation.v, 9) != 9) {
|
||||
WMR_ERROR(ll, "Invalid camera calibration block %d - pose", c->n_cameras);
|
||||
return false;
|
||||
}
|
||||
|
||||
wmr_config_compute_pose(&cam_config->pose, &translation, &rotation);
|
||||
|
||||
if (!JSON_INT(camera, "SensorWidth", &cam_config->sensor_width) ||
|
||||
!JSON_INT(camera, "SensorHeight", &cam_config->sensor_height)) {
|
||||
WMR_ERROR(ll, "Invalid camera calibration block %d - sensor size", c->n_cameras);
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Distortion information */
|
||||
cJSON *dist = cJSON_GetObjectItemCaseSensitive(camera, "Intrinsics");
|
||||
if (!dist) {
|
||||
WMR_ERROR(ll, "Invalid camera calibration block %d - distortion", c->n_cameras);
|
||||
return false;
|
||||
}
|
||||
|
||||
const char *model_type = cJSON_GetStringValue(cJSON_GetObjectItemCaseSensitive(dist, "ModelType"));
|
||||
if (model_type == NULL) {
|
||||
WMR_ERROR(ll, "Invalid camera calibration block %d - missing distortion type", c->n_cameras);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!strcmp(model_type, "CALIBRATION_LensDistortionModelRational6KT")) {
|
||||
} else {
|
||||
WMR_ERROR(ll, "Invalid camera calibration block %d - unknown distortion type %s", c->n_cameras,
|
||||
model_type);
|
||||
return false;
|
||||
}
|
||||
|
||||
struct wmr_distortion_6KT *distortion6KT = &cam_config->distortion6KT;
|
||||
|
||||
int param_count;
|
||||
if (!JSON_INT(dist, "ModelParameterCount", ¶m_count)) {
|
||||
WMR_ERROR(ll, "Invalid camera calibration block %d - no ModelParameterCount", c->n_cameras);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (param_count != 15) {
|
||||
WMR_ERROR(ll, "Invalid camera calibration block %d - wrong ModelParameterCount %d", c->n_cameras,
|
||||
param_count);
|
||||
return false;
|
||||
}
|
||||
|
||||
cJSON *params_json = cJSON_GetObjectItemCaseSensitive(dist, "ModelParameters");
|
||||
if (params_json == NULL ||
|
||||
u_json_get_float_array(params_json, distortion6KT->v, param_count) != (size_t)param_count) {
|
||||
WMR_ERROR(ll, "Invalid camera calibration block %d - missing distortion parameters", c->n_cameras);
|
||||
return false;
|
||||
}
|
||||
|
||||
c->n_cameras++;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool
|
||||
wmr_config_parse_calibration(struct wmr_hmd_config *c, cJSON *calib_info, enum u_logging_level ll)
|
||||
|
@ -245,6 +365,18 @@ wmr_config_parse_calibration(struct wmr_hmd_config *c, cJSON *calib_info, enum u
|
|||
}
|
||||
}
|
||||
|
||||
cJSON *cameras = cJSON_GetObjectItemCaseSensitive(calib_info, "Cameras");
|
||||
if (!cJSON_IsArray(cameras)) {
|
||||
WMR_ERROR(ll, "Cameras: not found or not an Array");
|
||||
return false;
|
||||
}
|
||||
|
||||
cJSON_ArrayForEach(item, cameras)
|
||||
{
|
||||
if (!wmr_config_parse_camera_config(c, item, ll))
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
@ -13,16 +13,38 @@
|
|||
#include "math/m_vec2.h"
|
||||
#include "util/u_logging.h"
|
||||
|
||||
enum wmr_distortion_model
|
||||
{
|
||||
WMR_DISTORTION_MODEL_UNKNOWN = 0,
|
||||
WMR_DISTORTION_MODEL_POLYNOMIAL_3K
|
||||
};
|
||||
/* Increase this number if anyone releases a headset with
|
||||
* more cameras */
|
||||
#define WMR_MAX_CAMERAS 4
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
enum wmr_distortion_model
|
||||
{
|
||||
WMR_DISTORTION_MODEL_UNKNOWN = 0,
|
||||
WMR_DISTORTION_MODEL_POLYNOMIAL_3K,
|
||||
WMR_DISTORTION_MODEL_POLYNOMIAL_6KT,
|
||||
};
|
||||
|
||||
/* Location is used as camera_id for setting gain */
|
||||
enum wmr_camera_location
|
||||
{
|
||||
WMR_CAMERA_LOCATION_HT0 = 0,
|
||||
WMR_CAMERA_LOCATION_HT1 = 1,
|
||||
WMR_CAMERA_LOCATION_DO0 = 2,
|
||||
WMR_CAMERA_LOCATION_DO1 = 3,
|
||||
WMR_CAMERA_LOCATION_HT2 = 4,
|
||||
WMR_CAMERA_LOCATION_HT3 = 5,
|
||||
};
|
||||
|
||||
enum wmr_camera_purpose
|
||||
{
|
||||
WMR_CAMERA_PURPOSE_HEAD_TRACKING,
|
||||
WMR_CAMERA_PURPOSE_DISPLAY_OBSERVER,
|
||||
};
|
||||
|
||||
struct wmr_distortion_3K
|
||||
{
|
||||
enum wmr_distortion_model model;
|
||||
|
@ -35,6 +57,25 @@ struct wmr_distortion_3K
|
|||
double k[3];
|
||||
};
|
||||
|
||||
struct wmr_distortion_6KT
|
||||
{
|
||||
enum wmr_distortion_model model;
|
||||
|
||||
/* The config provides 15 float values: */
|
||||
union {
|
||||
struct
|
||||
{
|
||||
float cx, cy; /* Principal point */
|
||||
float fx, fy; /* Focal length */
|
||||
float k[6]; /* Radial distortion coefficients */
|
||||
float dist_x, dist_y; /* Center of distortion */
|
||||
float p2, p1; /* Tangential distortion parameters */
|
||||
float metric_radius; /* Metric radius */
|
||||
} params;
|
||||
float v[15];
|
||||
};
|
||||
};
|
||||
|
||||
struct wmr_distortion_eye_config
|
||||
{
|
||||
/* 3x3 camera matrix that moves from normalised camera coords (X/Z & Y/Z) to undistorted pixels */
|
||||
|
@ -53,6 +94,17 @@ struct wmr_distortion_eye_config
|
|||
struct wmr_distortion_3K distortion3K[3];
|
||||
};
|
||||
|
||||
struct wmr_camera_config
|
||||
{
|
||||
enum wmr_camera_location location;
|
||||
enum wmr_camera_purpose purpose;
|
||||
|
||||
int sensor_width, sensor_height;
|
||||
struct xrt_pose pose;
|
||||
|
||||
struct wmr_distortion_6KT distortion6KT;
|
||||
};
|
||||
|
||||
struct wmr_hmd_config
|
||||
{
|
||||
/* Left and Right eye mapping and distortion params */
|
||||
|
@ -61,6 +113,9 @@ struct wmr_hmd_config
|
|||
struct xrt_pose accel_pose;
|
||||
struct xrt_pose gyro_pose;
|
||||
struct xrt_pose mag_pose;
|
||||
|
||||
int n_cameras;
|
||||
struct wmr_camera_config cameras[WMR_MAX_CAMERAS];
|
||||
};
|
||||
|
||||
bool
|
||||
|
|
Loading…
Reference in a new issue