mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2024-12-29 11:06:18 +00:00
aux/vive: Parse camera calibration
This commit is contained in:
Moses Turner
Moses Turner
parent
efbc4cd9e5
commit
cef922946a
|
|
@ -291,7 +291,7 @@ if (XRT_BUILD_DRIVER_VIVE OR XRT_BUILD_DRIVER_SURVIVE)
|
|||
vive/vive_config.c
|
||||
)
|
||||
add_library(aux_vive STATIC ${VIVE_CONFIG_SOURCE_FILES})
|
||||
target_link_libraries(aux_vive PRIVATE xrt-interfaces aux_util aux_math xrt-external-cjson)
|
||||
target_link_libraries(aux_vive PRIVATE xrt-interfaces aux_util aux_math aux_tracking xrt-external-cjson)
|
||||
target_link_libraries(aux_vive PRIVATE ${ZLIB_LIBRARIES})
|
||||
target_include_directories(aux_vive PRIVATE ${ZLIB_INCLUDE_DIRS})
|
||||
endif()
|
||||
|
|
|
|||
|
|
@ -247,7 +247,7 @@ lib_aux_vive = static_library(
|
|||
xrt_include,
|
||||
cjson_include,
|
||||
],
|
||||
dependencies: [zlib],
|
||||
dependencies: [zlib, aux_tracking],
|
||||
)
|
||||
|
||||
aux_vive = declare_dependency(
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@
|
|||
* @file
|
||||
* @brief Vive json implementation
|
||||
* @author Lubosz Sarnecki <lubosz.sarnecki@collabora.com>
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @ingroup drv_vive
|
||||
*/
|
||||
|
||||
|
|
@ -17,6 +18,10 @@
|
|||
|
||||
#include "math/m_api.h"
|
||||
|
||||
#include "tracking/t_tracking.h"
|
||||
#include "math/m_vec3.h"
|
||||
#include "math/m_space.h"
|
||||
|
||||
|
||||
#define VIVE_TRACE(d, ...) U_LOG_IFL_T(d->ll, __VA_ARGS__)
|
||||
#define VIVE_DEBUG(d, ...) U_LOG_IFL_D(d->ll, __VA_ARGS__)
|
||||
|
|
@ -31,6 +36,10 @@
|
|||
#define JSON_MATRIX_3X3(a, b, c) u_json_get_matrix_3x3(u_json_get(a, b), c)
|
||||
#define JSON_STRING(a, b, c) u_json_get_string_into_array(u_json_get(a, b), c, sizeof(c))
|
||||
|
||||
#define printf_pose(pose) \
|
||||
printf("%f %f %f %f %f %f %f\n", pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, \
|
||||
pose.orientation.y, pose.orientation.z, pose.orientation.w);
|
||||
|
||||
static void
|
||||
_get_color_coeffs(struct u_vive_values *values, const cJSON *coeffs, uint8_t eye, uint8_t channel)
|
||||
{
|
||||
|
|
@ -180,6 +189,170 @@ _print_vec3(const char *title, struct xrt_vec3 *vec)
|
|||
U_LOG_D("%s = %f %f %f", title, (double)vec->x, (double)vec->y, (double)vec->z);
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
_get_camera(struct index_camera *cam, const cJSON *cam_json)
|
||||
{
|
||||
bool succeeded = true;
|
||||
const cJSON *extrinsics = u_json_get(cam_json, "extrinsics");
|
||||
succeeded = succeeded && JSON_VEC3(extrinsics, "plus_x", &cam->extrinsics.plus_x);
|
||||
succeeded = succeeded && JSON_VEC3(extrinsics, "plus_z", &cam->extrinsics.plus_z);
|
||||
succeeded = succeeded && JSON_VEC3(extrinsics, "position", &cam->extrinsics.position);
|
||||
|
||||
|
||||
const cJSON *intrinsics = u_json_get(cam_json, "intrinsics");
|
||||
|
||||
succeeded = succeeded && u_json_get_double_array(u_json_get(u_json_get(intrinsics, "distort"), "coeffs"),
|
||||
cam->intrinsics.distortion, 4);
|
||||
|
||||
succeeded = succeeded && u_json_get_double(u_json_get(intrinsics, "center_x"), &cam->intrinsics.center_x);
|
||||
succeeded = succeeded && u_json_get_double(u_json_get(intrinsics, "center_y"), &cam->intrinsics.center_y);
|
||||
|
||||
succeeded = succeeded && u_json_get_double(u_json_get(intrinsics, "focal_x"), &cam->intrinsics.focal_x);
|
||||
succeeded = succeeded && u_json_get_double(u_json_get(intrinsics, "focal_y"), &cam->intrinsics.focal_y);
|
||||
succeeded = succeeded && u_json_get_int(u_json_get(intrinsics, "height"), &cam->intrinsics.image_size_pixels.h);
|
||||
succeeded = succeeded && u_json_get_int(u_json_get(intrinsics, "width"), &cam->intrinsics.image_size_pixels.w);
|
||||
|
||||
if (!succeeded) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool
|
||||
vive_get_stereo_camera_calibration(struct vive_config *d,
|
||||
struct t_stereo_camera_calibration **out_calibration,
|
||||
struct xrt_pose *head_in_left_camera)
|
||||
{
|
||||
// Note! This doesn't feel exactly correct - some parts of the math seem weird, and when undistorting the
|
||||
// images with the data produced by this function, things don't quite line up along epipolar lines. If you're
|
||||
// really good at this kind of math and very bored, you could try to figure out what the problem is here. I for
|
||||
// one have to accept that I don't have time for this and have to merge it as-is.
|
||||
if (!d->cameras.valid) {
|
||||
U_LOG_W("Why did you call me?");
|
||||
return false;
|
||||
}
|
||||
struct index_camera *cameras = d->cameras.view;
|
||||
bool print_debug = false;
|
||||
struct t_stereo_camera_calibration *out_calib = NULL;
|
||||
|
||||
t_stereo_camera_calibration_alloc(&out_calib, 5);
|
||||
|
||||
*out_calibration = out_calib;
|
||||
|
||||
struct xrt_matrix_3x3 rotate_hmd_to_camera[2];
|
||||
struct xrt_matrix_3x3 rotate_camera_to_hmd[2];
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
out_calib->view[i].use_fisheye = true;
|
||||
out_calib->view[i].image_size_pixels.h = 960;
|
||||
out_calib->view[i].image_size_pixels.w = 960;
|
||||
|
||||
// This better be row-major!
|
||||
out_calib->view[i].intrinsics[0][0] = cameras[i].intrinsics.focal_x;
|
||||
out_calib->view[i].intrinsics[0][1] = 0.0f;
|
||||
out_calib->view[i].intrinsics[0][2] = cameras[i].intrinsics.center_x;
|
||||
|
||||
out_calib->view[i].intrinsics[1][0] = 0.0f;
|
||||
out_calib->view[i].intrinsics[1][1] = cameras[i].intrinsics.focal_y;
|
||||
out_calib->view[i].intrinsics[1][2] = cameras[i].intrinsics.center_y;
|
||||
|
||||
out_calib->view[i].intrinsics[2][0] = 0.0f;
|
||||
out_calib->view[i].intrinsics[2][1] = 0.0f;
|
||||
out_calib->view[i].intrinsics[2][2] = 1.0f;
|
||||
|
||||
|
||||
out_calib->view[i].distortion_fisheye[0] = cameras[i].intrinsics.distortion[0];
|
||||
out_calib->view[i].distortion_fisheye[1] = cameras[i].intrinsics.distortion[1];
|
||||
out_calib->view[i].distortion_fisheye[2] = cameras[i].intrinsics.distortion[2];
|
||||
out_calib->view[i].distortion_fisheye[3] = cameras[i].intrinsics.distortion[3];
|
||||
|
||||
|
||||
struct xrt_vec3 plus_x = m_vec3_mul_scalar(cameras[i].extrinsics.plus_x, -1.0f);
|
||||
struct xrt_vec3 plus_z = m_vec3_mul_scalar(cameras[i].extrinsics.plus_z, -1.0f);
|
||||
struct xrt_vec3 plus_y;
|
||||
math_vec3_cross(&plus_z, &plus_x, &plus_y);
|
||||
|
||||
// I will have a seziure if you ask me if this is row-major or col-major. It works, okay? *OKAY?*
|
||||
rotate_hmd_to_camera[i].v[0] = plus_x.x;
|
||||
rotate_hmd_to_camera[i].v[1] = plus_y.x;
|
||||
rotate_hmd_to_camera[i].v[2] = plus_z.x;
|
||||
|
||||
rotate_hmd_to_camera[i].v[3] = plus_x.y;
|
||||
rotate_hmd_to_camera[i].v[4] = plus_y.y;
|
||||
rotate_hmd_to_camera[i].v[5] = plus_z.y;
|
||||
|
||||
rotate_hmd_to_camera[i].v[6] = plus_x.z;
|
||||
rotate_hmd_to_camera[i].v[7] = plus_y.z;
|
||||
rotate_hmd_to_camera[i].v[8] = plus_z.z;
|
||||
|
||||
math_matrix_3x3_inverse(&rotate_hmd_to_camera[i], &rotate_camera_to_hmd[i]);
|
||||
}
|
||||
|
||||
// The Index has hard-coded position intrinsics: relative to the HMD origin, both cameras have the exact same Y
|
||||
// and Z coordinate, and their X-coordinates have the same magnitude but opposite signs. In other words their
|
||||
// positions are mirrored across the YZ plane. So, I can do cheesy shortcuts like this.
|
||||
float negative_baseline = cameras[1].extrinsics.position.x - cameras[0].extrinsics.position.x;
|
||||
|
||||
// Doing rotate_camera_to_hmd * (baseline, 0, 0)
|
||||
out_calib->camera_translation[0] = negative_baseline * rotate_camera_to_hmd[1].v[0];
|
||||
out_calib->camera_translation[1] = negative_baseline * rotate_camera_to_hmd[1].v[1];
|
||||
out_calib->camera_translation[2] = negative_baseline * rotate_camera_to_hmd[1].v[2];
|
||||
|
||||
|
||||
struct xrt_matrix_3x3 rot_left_camera_to_right_camera;
|
||||
|
||||
math_matrix_3x3_multiply(&rotate_hmd_to_camera[1], &rotate_camera_to_hmd[0], &rot_left_camera_to_right_camera);
|
||||
|
||||
out_calib->camera_rotation[0][0] = rot_left_camera_to_right_camera.v[0];
|
||||
out_calib->camera_rotation[0][1] = rot_left_camera_to_right_camera.v[1];
|
||||
out_calib->camera_rotation[0][2] = rot_left_camera_to_right_camera.v[2];
|
||||
|
||||
out_calib->camera_rotation[1][0] = rot_left_camera_to_right_camera.v[3];
|
||||
out_calib->camera_rotation[1][1] = rot_left_camera_to_right_camera.v[4];
|
||||
out_calib->camera_rotation[1][2] = rot_left_camera_to_right_camera.v[5];
|
||||
|
||||
out_calib->camera_rotation[2][0] = rot_left_camera_to_right_camera.v[6];
|
||||
out_calib->camera_rotation[2][1] = rot_left_camera_to_right_camera.v[7];
|
||||
out_calib->camera_rotation[2][2] = rot_left_camera_to_right_camera.v[8];
|
||||
|
||||
|
||||
struct xrt_space_graph xsg_hmd_in_left_cam = {0};
|
||||
|
||||
// For some reason xrt_space_graph wants things in the opposite order as you'd expect.
|
||||
// Second, we apply the position:
|
||||
struct xrt_pose just_translation = {0};
|
||||
just_translation.orientation.w = 1.0f;
|
||||
just_translation.orientation.x = 0.0f;
|
||||
just_translation.orientation.y = 0.0f;
|
||||
just_translation.orientation.z = 0.0f;
|
||||
|
||||
// Weird that only the y-component has to be negative, right? I, the person writing this, don't really
|
||||
// understand it. The Index calibration sure uses weird coordinate spaces.
|
||||
just_translation.position.x = cameras[0].extrinsics.position.x;
|
||||
just_translation.position.y = -cameras[0].extrinsics.position.y;
|
||||
just_translation.position.z = cameras[0].extrinsics.position.z;
|
||||
|
||||
m_space_graph_add_pose(&xsg_hmd_in_left_cam, &just_translation);
|
||||
|
||||
// First, we add the rotation:
|
||||
struct xrt_pose just_rotation = {0};
|
||||
math_quat_from_matrix_3x3(&rotate_camera_to_hmd[0], &just_rotation.orientation);
|
||||
|
||||
m_space_graph_add_pose(&xsg_hmd_in_left_cam, &just_rotation);
|
||||
|
||||
struct xrt_space_relation head_in_left_cam = {0};
|
||||
|
||||
m_space_graph_resolve(&xsg_hmd_in_left_cam, &head_in_left_cam);
|
||||
|
||||
if (print_debug) {
|
||||
printf_pose(head_in_left_cam.pose);
|
||||
}
|
||||
|
||||
*head_in_left_camera = head_in_left_cam.pose;
|
||||
return true;
|
||||
}
|
||||
|
||||
static void
|
||||
vive_init_defaults(struct vive_config *d)
|
||||
{
|
||||
|
|
@ -200,6 +373,8 @@ vive_init_defaults(struct vive_config *d)
|
|||
d->imu.gyro_scale.y = 1.0f;
|
||||
d->imu.gyro_scale.z = 1.0f;
|
||||
|
||||
d->cameras.valid = false;
|
||||
|
||||
for (int view = 0; view < 2; view++) {
|
||||
d->distortion[view].aspect_x_over_y = 0.89999997615814209f;
|
||||
d->distortion[view].grow_for_undistort = 0.5f;
|
||||
|
|
@ -279,6 +454,45 @@ vive_config_parse(struct vive_config *d, char *json_string, enum u_logging_level
|
|||
math_pose_transform(&trackref_to_head, &d->imu.trackref, &imu_to_head);
|
||||
|
||||
d->display.imuref = imu_to_head;
|
||||
|
||||
|
||||
// Hey! Moses wrote this part in a pretty big hurry; you will definitely see some suspect things here.
|
||||
// If you're bored, please fix them!
|
||||
const cJSON *cms = u_json_get(json, "tracked_cameras");
|
||||
const cJSON *cmr;
|
||||
|
||||
bool found_camera_json = false;
|
||||
bool succeeded_parsing_json = false;
|
||||
|
||||
cJSON_ArrayForEach(cmr, cms)
|
||||
{
|
||||
found_camera_json = true;
|
||||
|
||||
const cJSON *name_json = u_json_get(cmr, "name");
|
||||
const char *name = name_json->valuestring;
|
||||
bool is_left = !strcmp("left", name);
|
||||
bool is_right = !strcmp("right", name);
|
||||
|
||||
if (!is_left && !is_right) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (!_get_camera(&d->cameras.view[is_right], cmr)) {
|
||||
succeeded_parsing_json = false;
|
||||
break;
|
||||
}
|
||||
succeeded_parsing_json = true;
|
||||
}
|
||||
|
||||
|
||||
if (!found_camera_json) {
|
||||
U_LOG_W("HMD is Index, but no cameras in json file!");
|
||||
} else if (!succeeded_parsing_json) {
|
||||
U_LOG_E("Failed to parse Index camera calibration!");
|
||||
} else {
|
||||
d->cameras.valid = true;
|
||||
}
|
||||
|
||||
} break;
|
||||
default:
|
||||
VIVE_ERROR(d, "Unknown Vive variant.");
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@
|
|||
* @file
|
||||
* @brief vive json header
|
||||
* @author Lubosz Sarnecki <lubosz.sarnecki@collabora.com>
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @ingroup drv_vive
|
||||
*/
|
||||
|
||||
|
|
@ -14,6 +15,7 @@
|
|||
#include "xrt/xrt_defines.h"
|
||||
#include "util/u_logging.h"
|
||||
#include "util/u_distortion_mesh.h"
|
||||
#include "tracking/t_tracking.h"
|
||||
|
||||
// public documentation
|
||||
#define INDEX_MIN_IPD 0.058
|
||||
|
|
@ -41,6 +43,36 @@ enum VIVE_CONTROLLER_VARIANT
|
|||
CONTROLLER_UNKNOWN
|
||||
};
|
||||
|
||||
/*!
|
||||
* A calibrated camera on an Index.
|
||||
*/
|
||||
struct index_camera
|
||||
{
|
||||
// Note! All the values in this struct are directly pasted in from the JSON values.
|
||||
// As such, in my opinion, plus_x, plus_z and position are all "wrong" - all the code I've had to write that
|
||||
// uses this struct flips the signs of plus_x, plus_z, and the signs of the X- and Z-components of position.
|
||||
// I have no idea why those sign flips were necessary - I suppose Valve/HTC just made some weird decisions when
|
||||
// making the config file schemas. I figure it would be very confusing to try to "fix" these values as I'm
|
||||
// parsing them, so if you're writing code downstream of this, beware and expect the values in here to be
|
||||
// exactly the same as those in the compressed JSON. -Moses
|
||||
struct
|
||||
{
|
||||
struct xrt_vec3 plus_x;
|
||||
struct xrt_vec3 plus_z;
|
||||
struct xrt_vec3 position; // looks like from head pose
|
||||
} extrinsics;
|
||||
struct
|
||||
{
|
||||
double distortion[4]; // Kannala-Brandt
|
||||
double center_x;
|
||||
double center_y;
|
||||
|
||||
double focal_x;
|
||||
double focal_y;
|
||||
struct xrt_size image_size_pixels;
|
||||
} intrinsics;
|
||||
};
|
||||
|
||||
/*!
|
||||
* A single lighthouse senosor point and normal, in IMU space.
|
||||
*/
|
||||
|
|
@ -113,6 +145,12 @@ struct vive_config
|
|||
|
||||
struct u_vive_values distortion[2];
|
||||
|
||||
struct
|
||||
{
|
||||
struct index_camera view[2];
|
||||
bool valid;
|
||||
} cameras;
|
||||
|
||||
struct lh_model lh;
|
||||
};
|
||||
|
||||
|
|
@ -161,3 +199,8 @@ struct vive_controller_device;
|
|||
|
||||
bool
|
||||
vive_config_parse_controller(struct vive_controller_config *d, char *json_string, enum u_logging_level ll);
|
||||
|
||||
bool
|
||||
vive_get_stereo_camera_calibration(struct vive_config *d,
|
||||
struct t_stereo_camera_calibration **out_calibration,
|
||||
struct xrt_pose *head_in_left_camera);
|
||||
|
|
|
|||
Loading…
Reference in a new issue