rpx_benchmark.pose_conventions

Coordinate conventions shared by RPX camera-pose benchmarks.

 1"""Coordinate conventions shared by RPX camera-pose benchmarks."""
 2
 3from __future__ import annotations
 4
 5import numpy as np
 6
 7from .exceptions import ConfigError
 8
 9# librealsense T265: X right, Y up, Z back.
10# OpenCV/VGGT:       X right, Y down, Z forward.
11T265_TO_OPENCV = np.diag([1.0, -1.0, -1.0, 1.0])
12
13
14def t265_c2w_to_opencv(pose: np.ndarray) -> np.ndarray:
15    """Express a raw T265 camera-to-world pose in OpenCV coordinates.
16
17    RPX stores the unmodified librealsense position and quaternion. Changing
18    the basis of both sides of the camera-to-world transform gives ``S T S``.
19    This is a fixed convention conversion, not an estimated calibration.
20    """
21
22    transform = np.asarray(pose, dtype=np.float64)
23    if transform.shape != (4, 4):
24        raise ConfigError(f"expected a 4x4 pose, got {transform.shape}")
25    return T265_TO_OPENCV @ transform @ T265_TO_OPENCV
26
27
28def relative_pose_from_raw_t265(pose_a: np.ndarray, pose_b: np.ndarray) -> np.ndarray:
29    """Return frame B in frame A, expressed in the OpenCV camera basis."""
30
31    a = t265_c2w_to_opencv(pose_a)
32    b = t265_c2w_to_opencv(pose_b)
33    return np.linalg.inv(a) @ b
34
35
36__all__ = ["T265_TO_OPENCV", "relative_pose_from_raw_t265", "t265_c2w_to_opencv"]
T265_TO_OPENCV = array([[ 1., 0., 0., 0.], [ 0., -1., 0., 0.], [ 0., 0., -1., 0.], [ 0., 0., 0., 1.]])
def relative_pose_from_raw_t265(pose_a: numpy.ndarray, pose_b: numpy.ndarray) -> numpy.ndarray: 
29def relative_pose_from_raw_t265(pose_a: np.ndarray, pose_b: np.ndarray) -> np.ndarray:
30    """Return frame B in frame A, expressed in the OpenCV camera basis."""
31
32    a = t265_c2w_to_opencv(pose_a)
33    b = t265_c2w_to_opencv(pose_b)
34    return np.linalg.inv(a) @ b

Return frame B in frame A, expressed in the OpenCV camera basis.

def t265_c2w_to_opencv(pose: numpy.ndarray) -> numpy.ndarray: 
15def t265_c2w_to_opencv(pose: np.ndarray) -> np.ndarray:
16    """Express a raw T265 camera-to-world pose in OpenCV coordinates.
17
18    RPX stores the unmodified librealsense position and quaternion. Changing
19    the basis of both sides of the camera-to-world transform gives ``S T S``.
20    This is a fixed convention conversion, not an estimated calibration.
21    """
22
23    transform = np.asarray(pose, dtype=np.float64)
24    if transform.shape != (4, 4):
25        raise ConfigError(f"expected a 4x4 pose, got {transform.shape}")
26    return T265_TO_OPENCV @ transform @ T265_TO_OPENCV

Express a raw T265 camera-to-world pose in OpenCV coordinates.

RPX stores the unmodified librealsense position and quaternion. Changing the basis of both sides of the camera-to-world transform gives S T S. This is a fixed convention conversion, not an estimated calibration.