u/vive: Remove old code and tidy camera calibration extraction

This commit is contained in:
Jakob Bornecrantz 2021-09-20 13:59:22 +01:00 committed by Moses Turner
parent 08721858dc
commit 6f66ada6d9
2 changed files with 7 additions and 189 deletions
src/xrt/auxiliary/vive

View file

@ -194,9 +194,6 @@ _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);
_get_pose_from_pos_x_z(extrinsics, &cam->trackref);
@ -285,9 +282,9 @@ _get_cameras(struct vive_config *d, const cJSON *cameras_json)
}
bool
vive_get_stereo_camera_calibration_jakob(struct vive_config *d,
struct t_stereo_camera_calibration **out_calibration,
struct xrt_pose *head_in_left_camera)
vive_get_stereo_camera_calibration(struct vive_config *d,
struct t_stereo_camera_calibration **calibration_ptr_to_ref,
struct xrt_pose *out_head_in_left_camera)
{
if (!d->cameras.valid) {
U_LOG_E("Camera config not loaded, can not produce camera calibration.");
@ -345,194 +342,15 @@ vive_get_stereo_camera_calibration_jakob(struct vive_config *d,
calib->camera_rotation[2][1] = z.y;
calib->camera_rotation[2][2] = z.z;
math_pose_invert(&d->cameras.view[0].headref, head_in_left_camera);
math_pose_invert(&d->cameras.view[0].headref, out_head_in_left_camera);
// Correctly reference count.
t_stereo_camera_calibration_reference(out_calibration, calib);
t_stereo_camera_calibration_reference(calibration_ptr_to_ref, calib);
t_stereo_camera_calibration_reference(&calib, NULL);
return true;
}
#if 1
bool
vive_get_stereo_camera_calibration_moses(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;
}
#endif
bool
vive_get_stereo_camera_calibration(struct vive_config *d,
struct t_stereo_camera_calibration **out_calibration,
struct xrt_pose *head_in_left_camera)
{
struct t_stereo_camera_calibration *calib[2] = {NULL, NULL};
struct xrt_pose pose[2];
struct xrt_pose TR[2];
vive_get_stereo_camera_calibration_jakob(d, &calib[0], &pose[0]);
vive_get_stereo_camera_calibration_moses(d, &calib[1], &pose[1]);
for (size_t i = 0; i < 2; i++) {
struct xrt_matrix_3x3 mat;
for (size_t y = 0; y < 3; y++) {
for (size_t x = 0; x < 3; x++) {
mat.v[y * 3 + x] = calib[i]->camera_rotation[y][x];
}
}
math_quat_from_matrix_3x3(&mat, &TR[i].orientation);
TR[i].position.x = calib[i]->camera_translation[0];
TR[i].position.y = calib[i]->camera_translation[1];
TR[i].position.z = calib[i]->camera_translation[2];
}
printf("Jakobs:\n");
printf_pose(pose[0]);
printf_pose(TR[0]);
printf("Moses:\n");
printf_pose(pose[1]);
printf_pose(TR[1]);
t_stereo_camera_calibration_reference(out_calibration, calib[0]);
*head_in_left_camera = pose[0];
t_stereo_camera_calibration_reference(&calib[0], NULL);
t_stereo_camera_calibration_reference(&calib[1], NULL);
return true;
}
static void
vive_init_defaults(struct vive_config *d)
{

View file

@ -218,5 +218,5 @@ vive_config_parse_controller(struct vive_controller_config *d, char *json_string
bool
vive_get_stereo_camera_calibration(struct vive_config *d,
struct t_stereo_camera_calibration **out_calibration,
struct xrt_pose *head_in_left_camera);
struct t_stereo_camera_calibration **calibration_ptr_to_ref,
struct xrt_pose *out_head_in_left_camera);