From 173d55c58b7c4c78036179d3e9d93e3d3541b197 Mon Sep 17 00:00:00 2001 From: Moses Turner Date: Sun, 18 Sep 2022 08:04:34 -0500 Subject: [PATCH] tests: Add test_quat_change_of_basis --- tests/CMakeLists.txt | 5 + tests/tests_quat_change_of_basis.cpp | 199 +++++++++++++++++++++++++++ 2 files changed, 204 insertions(+) create mode 100644 tests/tests_quat_change_of_basis.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index e2e44733f..be7419de3 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -21,6 +21,7 @@ set(tests tests_lowpass_integer tests_pacing tests_quatexpmap + tests_quat_change_of_basis tests_rational tests_vector tests_worker @@ -64,6 +65,10 @@ target_link_libraries(tests_lowpass_integer PRIVATE aux_math) target_link_libraries(tests_quatexpmap PRIVATE aux_math) target_link_libraries(tests_rational PRIVATE aux_math) target_link_libraries(tests_pose PRIVATE aux_math) +target_link_libraries(tests_quat_change_of_basis PRIVATE aux_math) + +target_include_directories(tests_quat_change_of_basis SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR}) + if(XRT_BUILD_DRIVER_HANDTRACKING) target_link_libraries( diff --git a/tests/tests_quat_change_of_basis.cpp b/tests/tests_quat_change_of_basis.cpp new file mode 100644 index 000000000..738f4e828 --- /dev/null +++ b/tests/tests_quat_change_of_basis.cpp @@ -0,0 +1,199 @@ +// Copyright 2022, Collabora, Inc. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Test for change-of-basis transformations between left-handed and right-handed coordinate sytems for + * quaternions. + * @author Moses Turner + */ +#include "math/m_api.h" +#include "xrt/xrt_defines.h" +#include +#include +#include +#include + + +#include "catch/catch.hpp" + +#include + + +#include "math/m_eigen_interop.hpp" +#include + + +// https://stackoverflow.com/questions/28673777/convert-quaternion-from-right-handed-to-left-handed-coordinate-system + + +// unity: x=0.3, w=0.95 . results in +40 rotation, which I think is wrong probably, should probably be -40 +// openxr, according to us: +void +log_quat(xrt_quat q) +{ + U_LOG_E("xyzw: %f %f %f %f", q.x, q.y, q.z, q.w); +} + +float +quat_difference(xrt_quat q1, xrt_quat q2) +{ + // https://math.stackexchange.com/a/90098 + // d(q1,q2)=1−⟨q1,q2⟩2 + + float inner_product = (q1.w * q2.w) + (q1.x * q2.x) + (q1.y * q2.y) + (q1.z * q2.z); + return 1.0 - (inner_product * inner_product); +} + +xrt_quat +random_quat() +{ + std::random_device dev; + + auto mt = std::mt19937(dev()); + auto rd = std::normal_distribution(0, 1); + + struct xrt_quat quat = {rd(mt), rd(mt), rd(mt), rd(mt)}; + + math_quat_normalize(&quat); + return quat; +} + +//! https://stackoverflow.com/questions/28673777/convert-quaternion-from-right-handed-to-left-handed-coordinate-system +// Same as zldtt_ori_right in lm_main. +void +slow_change_of_basis_lh_to_rh(xrt_quat *in, xrt_quat *out) +{ + + xrt_vec3 x = XRT_VEC3_UNIT_X; + xrt_vec3 z = XRT_VEC3_UNIT_Z; + + math_quat_rotate_vec3(in, &x, &x); + math_quat_rotate_vec3(in, &z, &z); + + // This is a very squashed change-of-basis from left-handed coordinate systems to right-handed coordinate + // systems: you multiply everything by (-1 0 0) then negate the X axis. + + x.y *= -1; + x.z *= -1; + + z.x *= -1; + + math_quat_from_plus_x_z(&x, &z, out); +} + +void +fast_change_of_basis_lh_to_rh(xrt_quat *in, xrt_quat *out) +{ + out->x = -in->x; + out->y = in->y; + out->z = in->z; + out->w = -in->w; +} + +/* +OpenXR is +X right, +Y up, -Z forward +Unity is X+ Right, Y+ Up, Z+ Forward + +So we're not swapping axes, we're just flipping them. It's the same change of basis as "left hand" to "right hand" +(indeed we chould have implemented left vs right in our optical hand tracking that way), just the "flip" is on the XY +plane not the YZ plane. + +Vaguely based on `make_joint_at_matrix_right_hand` from ccdik_main but rotated +*/ + +void +slow_change_of_basis_unity_to_oxr(xrt_quat *in, xrt_quat *out) +{ + + Eigen::Quaternionf q; + + q.w() = in->w; + q.x() = in->x; + q.y() = in->y; + q.z() = in->z; + + Eigen::Matrix3f unity_rotation(q); + + Eigen::Matrix3f mirror_unity_to_openxr = Eigen::Matrix3f::Identity(); + mirror_unity_to_openxr(2, 2) = -1; + + Eigen::Matrix3f intermediate = mirror_unity_to_openxr * unity_rotation; + + intermediate(0, 2) *= -1; + intermediate(1, 2) *= -1; + intermediate(2, 2) *= -1; + + Eigen::Quaternionf q_new; + + q_new = intermediate; + + + out->w = q_new.w(); + out->x = q_new.x(); + out->y = q_new.y(); + out->z = q_new.z(); +} + +void +fast_change_of_basis_unity_to_oxr(xrt_quat *in, xrt_quat *out) +{ + out->x = in->x; + out->y = in->y; + out->z = -in->z; + out->w = -in->w; +} + + + +// int +// main() +TEST_CASE("LevenbergMarquardt") + +{ + U_LOG_E("LH to RH!"); + for (int i = 0; i < 3; i++) { + xrt_quat q = random_quat(); + xrt_quat q_prime; + xrt_quat q_prime_fast; + slow_change_of_basis_lh_to_rh(&q, &q_prime); + fast_change_of_basis_lh_to_rh(&q, &q_prime_fast); + log_quat(q); + log_quat(q_prime); + log_quat(q_prime_fast); + // xrt_quat factors; + // factors.w = q_prime.w / q.w; + // factors.x = q_prime.x / q.x; + // factors.y = q_prime.y / q.y; + // factors.z = q_prime.z / q.z; + // log_quat(factors); + + CHECK(quat_difference(q_prime, q_prime_fast) < 0.01); + } + + U_LOG_E("Unity to OpenXR!"); + for (int i = 0; i < 3; i++) { + xrt_quat q = random_quat(); + if (i == 0) { + q.x = 0.31; + q.y = 0; + q.z = 0; + q.w = 0.95; + math_quat_normalize(&q); + } + xrt_quat q_prime; + xrt_quat q_prime_fast; + slow_change_of_basis_unity_to_oxr(&q, &q_prime); + fast_change_of_basis_unity_to_oxr(&q, &q_prime_fast); + log_quat(q); + log_quat(q_prime); + log_quat(q_prime_fast); + // xrt_quat factors; + // factors.w = q_prime.w / q.w; + // factors.x = q_prime.x / q.x; + // factors.y = q_prime.y / q.y; + // factors.z = q_prime.z / q.z; + // log_quat(factors); + + CHECK(quat_difference(q_prime, q_prime_fast) < 0.01); + } +}