From 1f506d88bc1ab38ecbb32084da7bdce6bdecdb7d Mon Sep 17 00:00:00 2001 From: Beyley Cardellio Date: Sun, 23 Aug 2026 21:58:11 -0700 Subject: [PATCH] a/math: Add vec3 functions for converting between OpenCV and OpenXR coordinate spaces Signed-off-by: Beyley Cardellio Part-of: --- src/xrt/auxiliary/math/m_api.h | 30 +++++++++++++++++-- src/xrt/auxiliary/math/m_base.cpp | 18 ++++++++++- .../constellation_debug_scribble.cpp | 2 +- .../constellation_tracker_rerun.cpp | 4 +-- .../constellation/t_constellation_tracker.cpp | 15 +++++----- 5 files changed, 55 insertions(+), 14 deletions(-) diff --git a/src/xrt/auxiliary/math/m_api.h b/src/xrt/auxiliary/math/m_api.h index b3ba0464b..3da13c87c 100644 --- a/src/xrt/auxiliary/math/m_api.h +++ b/src/xrt/auxiliary/math/m_api.h @@ -193,6 +193,18 @@ math_vec3_translation_from_isometry(const struct xrt_matrix_4x4 *isometry, struc void math_vec3_normalize(struct xrt_vec3 *in); +/*! + * Convert a vec3 from the OpenCV coordinate system to the OpenXR coordinate system and back. OpenCV camera space + * coordinates has +Y down and +Z away from the user. + * + * The input and output may be the same pointer. + * + * @relates xrt_vec3 + * @ingroup aux_math + */ +void +math_vec3_convert_from_opencv(const struct xrt_vec3 *in, struct xrt_vec3 *out); + /* * @@ -218,6 +230,18 @@ math_vec3_f64_cross(const struct xrt_vec3_f64 *l, const struct xrt_vec3_f64 *r, void math_vec3_f64_normalize(struct xrt_vec3_f64 *in); +/*! + * Convert a vec3_f64 from the OpenCV coordinate system to the OpenXR coordinate system. OpenCV camera space coordinates + * has +Y down and +Z away from the user. + * + * The input and output may be the same pointer. + * + * @relates xrt_vec3 + * @ingroup aux_math + */ +void +math_vec3_f64_convert_opencv(const struct xrt_vec3_f64 *in, struct xrt_vec3_f64 *out); + /* * @@ -829,8 +853,8 @@ void math_pose_transform_point(const struct xrt_pose *transform, const struct xrt_vec3 *point, struct xrt_vec3 *out_point); /*! - * Convert a pose from the OpenCV coordinate system to the OpenXR coordinate system. OpenCV camera space coordinates has - * +Y down and +Z away from the user. + * Convert a pose from the OpenCV coordinate system to the OpenXR coordinate system and back. OpenCV camera space + * coordinates has +Y down and +Z away from the user. * * The input and output may be the same pointer. * @@ -838,7 +862,7 @@ math_pose_transform_point(const struct xrt_pose *transform, const struct xrt_vec * @ingroup aux_math */ void -math_pose_convert_opencv(const struct xrt_pose *in, struct xrt_pose *out); +math_pose_convert_from_opencv(const struct xrt_pose *in, struct xrt_pose *out); /* diff --git a/src/xrt/auxiliary/math/m_base.cpp b/src/xrt/auxiliary/math/m_base.cpp index 9247820e2..5208d0490 100644 --- a/src/xrt/auxiliary/math/m_base.cpp +++ b/src/xrt/auxiliary/math/m_base.cpp @@ -182,6 +182,14 @@ math_vec3_translation_from_isometry(const struct xrt_matrix_4x4 *transform, stru map_vec3(*result) = isometry.translation(); } +extern "C" void +math_vec3_convert_from_opencv(const struct xrt_vec3 *in, struct xrt_vec3 *out) +{ + out->x = in->x; + out->y = -in->y; + out->z = -in->z; +} + /* * @@ -201,6 +209,14 @@ math_vec3_f64_normalize(struct xrt_vec3_f64 *in) map_vec3_f64(*in) = map_vec3_f64(*in).normalized(); } +extern "C" void +math_vec3_f64_convert_opencv(const struct xrt_vec3_f64 *in, struct xrt_vec3_f64 *out) +{ + out->x = in->x; + out->y = -in->y; + out->z = -in->z; +} + /* * @@ -1004,7 +1020,7 @@ math_pose_transform_point(const struct xrt_pose *transform, const struct xrt_vec } extern "C" void -math_pose_convert_opencv(const struct xrt_pose *in, struct xrt_pose *out) +math_pose_convert_from_opencv(const struct xrt_pose *in, struct xrt_pose *out) { const struct xrt_pose yz_flip = { {1.0, 0.0, 0.0, 0.0}, diff --git a/src/xrt/tracking/constellation/constellation_debug_scribble.cpp b/src/xrt/tracking/constellation/constellation_debug_scribble.cpp index d0571d52c..9206c7ec3 100644 --- a/src/xrt/tracking/constellation/constellation_debug_scribble.cpp +++ b/src/xrt/tracking/constellation/constellation_debug_scribble.cpp @@ -189,7 +189,7 @@ Camera::debugScribbleSample(CameraSample &sample, bool fast) &Txr_cam_device_prior); xrt_pose Tcv_cam_device_prior; - math_pose_convert_opencv(&Txr_cam_device_prior, &Tcv_cam_device_prior); + math_pose_convert_from_opencv(&Txr_cam_device_prior, &Tcv_cam_device_prior); scribble_led_model(frame, // *this, // diff --git a/src/xrt/tracking/constellation/constellation_tracker_rerun.cpp b/src/xrt/tracking/constellation/constellation_tracker_rerun.cpp index 26c40852f..9cefc31ae 100644 --- a/src/xrt/tracking/constellation/constellation_tracker_rerun.cpp +++ b/src/xrt/tracking/constellation/constellation_tracker_rerun.cpp @@ -341,7 +341,7 @@ RerunContext::logSample(const ConstellationTracker &tracker, const CameraSample std::optional Tcv_world_cam = std::nullopt; if (camera_sample.Txr_world_cam.has_value()) { xrt_pose Tcv_world_cam_value; - math_pose_convert_opencv(&camera_sample.Txr_world_cam.value(), &Tcv_world_cam_value); + math_pose_convert_from_opencv(&camera_sample.Txr_world_cam.value(), &Tcv_world_cam_value); Tcv_world_cam = Tcv_world_cam_value; this->stream->log(camera_entity, ToRerunTransform(Tcv_world_cam_value)); @@ -393,7 +393,7 @@ RerunContext::logSample(const ConstellationTracker &tracker, const CameraSample &Txr_cam_device_prior); // xrt_pose Tcv_cam_device_prior; - math_pose_convert_opencv(&Txr_cam_device_prior, &Tcv_cam_device_prior); + math_pose_convert_from_opencv(&Txr_cam_device_prior, &Tcv_cam_device_prior); this->stream->log(camera_device_prior_entity, ToRerunTransform(Tcv_cam_device_prior)); this->logLedModel(camera_device_prior_entity, device_id, device->params.led_model, true); diff --git a/src/xrt/tracking/constellation/t_constellation_tracker.cpp b/src/xrt/tracking/constellation/t_constellation_tracker.cpp index 69e59cd9c..cb6bfcc47 100644 --- a/src/xrt/tracking/constellation/t_constellation_tracker.cpp +++ b/src/xrt/tracking/constellation/t_constellation_tracker.cpp @@ -449,7 +449,7 @@ Camera::processSampleSlow(CameraSample &sample) math_pose_transform(&Txr_cam_world, &device_state->Txr_world_device_prior.value(), &Txr_cam_device); - math_pose_convert_opencv(&Txr_cam_device, &Tcv_cam_device); + math_pose_convert_from_opencv(&Txr_cam_device, &Tcv_cam_device); search_flags = (correspondence_search_flags)(search_flags | CS_FLAG_HAVE_POSE_PRIOR); } @@ -465,7 +465,7 @@ Camera::processSampleSlow(CameraSample &sample) // (required by correspondence for search gravity matching) if (Txr_world_cam.has_value()) { xrt_pose Tcv_world_cam; - math_pose_convert_opencv(&Txr_world_cam.value(), &Tcv_world_cam); + math_pose_convert_from_opencv(&Txr_world_cam.value(), &Tcv_world_cam); // Acquire the camera's gravity vector under the processing lock get_pose_gravity_vector(Tcv_world_cam, cv_camera_gravity_vector); @@ -534,7 +534,7 @@ Camera::processSampleFast(CameraSample &sample) } xrt_pose Tcv_world_cam; - math_pose_convert_opencv(&Txr_world_cam.value(), &Tcv_world_cam); + math_pose_convert_from_opencv(&Txr_world_cam.value(), &Tcv_world_cam); xrt_pose Tcv_cam_world; math_pose_invert(&Tcv_world_cam, &Tcv_cam_world); @@ -555,7 +555,8 @@ Camera::processSampleFast(CameraSample &sample) (XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT)) == (XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT)) { Tcv_world_device_predicted.emplace(xrt_pose{}); // initialize it to a value - math_pose_convert_opencv(&device_predicted_relation.pose, &Tcv_world_device_predicted.value()); + math_pose_convert_from_opencv(&device_predicted_relation.pose, + &Tcv_world_device_predicted.value()); } auto &device_state = sample.putDeviceState(device->id); @@ -587,8 +588,8 @@ Camera::processSampleFast(CameraSample &sample) std::unique_lock lock(device->data_lock); if (auto last_known_pose = device->locked_data.last_known_pose) { - math_pose_convert_opencv(&last_known_pose->Txr_world_device, - &Tcv_world_device_last_known); + math_pose_convert_from_opencv(&last_known_pose->Txr_world_device, + &Tcv_world_device_last_known); has_last_known = true; } } @@ -686,7 +687,7 @@ Camera::pushPose(CameraSample &camera_sample, // Move to OpenXR space xrt_pose Txr_cam_device; - math_pose_convert_opencv(&Tcv_cam_device, &Txr_cam_device); + math_pose_convert_from_opencv(&Tcv_cam_device, &Txr_cam_device); CT_DEBUG(tracker, "Pose: orient %f %f %f %f pos %f %f %f", Txr_cam_device.orientation.x, Txr_cam_device.orientation.y, Txr_cam_device.orientation.z, Txr_cam_device.orientation.w, -- 2.51.2