From cef922946a820257305c0ec773ca58c9d93c1ce0 Mon Sep 17 00:00:00 2001 From: Moses Turner Date: Thu, 2 Sep 2021 01:41:21 -0500 Subject: [PATCH] aux/vive: Parse camera calibration --- src/xrt/auxiliary/CMakeLists.txt | 2 +- src/xrt/auxiliary/meson.build | 2 +- src/xrt/auxiliary/vive/vive_config.c | 214 +++++++++++++++++++++++++++ src/xrt/auxiliary/vive/vive_config.h | 43 ++++++ 4 files changed, 259 insertions(+), 2 deletions(-) diff --git a/src/xrt/auxiliary/CMakeLists.txt b/src/xrt/auxiliary/CMakeLists.txt index e1b5c842d..8dd019f17 100644 --- a/src/xrt/auxiliary/CMakeLists.txt +++ b/src/xrt/auxiliary/CMakeLists.txt @@ -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() diff --git a/src/xrt/auxiliary/meson.build b/src/xrt/auxiliary/meson.build index 751ca9bfe..ef207d5fe 100644 --- a/src/xrt/auxiliary/meson.build +++ b/src/xrt/auxiliary/meson.build @@ -247,7 +247,7 @@ lib_aux_vive = static_library( xrt_include, cjson_include, ], - dependencies: [zlib], + dependencies: [zlib, aux_tracking], ) aux_vive = declare_dependency( diff --git a/src/xrt/auxiliary/vive/vive_config.c b/src/xrt/auxiliary/vive/vive_config.c index f133d3b08..e166f63a7 100644 --- a/src/xrt/auxiliary/vive/vive_config.c +++ b/src/xrt/auxiliary/vive/vive_config.c @@ -4,6 +4,7 @@ * @file * @brief Vive json implementation * @author Lubosz Sarnecki + * @author Moses Turner * @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."); diff --git a/src/xrt/auxiliary/vive/vive_config.h b/src/xrt/auxiliary/vive/vive_config.h index 9b81cf6b7..2d2b1e536 100644 --- a/src/xrt/auxiliary/vive/vive_config.h +++ b/src/xrt/auxiliary/vive/vive_config.h @@ -4,6 +4,7 @@ * @file * @brief vive json header * @author Lubosz Sarnecki + * @author Moses Turner * @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);