mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-02-03 12:28:07 +00:00
u/vive: Remove old code and tidy camera calibration extraction
This commit is contained in:
parent
08721858dc
commit
6f66ada6d9
src/xrt/auxiliary/vive
|
@ -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)
|
||||
{
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in a new issue