DHB Decoding

dhb_dr

DHB-DR decoder: reconstruct SE(3) trajectory from invariants. Quaternion convention: wxyz (scalar-first).

Robustness features: - Initial pose validation - Divergence detection with optional warnings - NaN/Inf handling for degenerate invariants

Classes

DecodingDiagnostics dataclass

Diagnostics from decoding.

Source code in src/dhb_xr/decoder/dhb_dr.py

@dataclass
class DecodingDiagnostics:
    """Diagnostics from decoding."""
    initial_pose_valid: bool = True
    num_nan_positions: int = 0
    num_nan_quaternions: int = 0
    max_step_size: float = 0.0
    trajectory_length: float = 0.0

Functions

decode_dhb_dr

decode_dhb_dr(
    linear_motion_invariants,
    angular_motion_invariants,
    initial_pose,
    method=EncodingMethod.POSITION,
    dhb_method=DHBMethod.DOUBLE_REFLECTION,
    drop_padded=True,
    robust_mode=False,
    validate_initial_pose=False,
    return_diagnostics=False,
)

Reconstruct poses from DHB invariants.

Parameters:

Name	Type	Description	Default
linear_motion_invariants	ndarray	(N, k) linear invariants	required
angular_motion_invariants	ndarray	(N, k) angular invariants	required
initial_pose	Dict[str, ndarray]	{'position': (3,), 'quaternion': (4,) wxyz}	required
method	EncodingMethod	'pos' or 'vel'	POSITION
dhb_method	DHBMethod	DHBMethod.ORIGINAL (3 inv) or DHBMethod.DOUBLE_REFLECTION (4 inv)	DOUBLE_REFLECTION
drop_padded	bool	if True, return positions/quaternions[2:] to match encoder padding	True
robust_mode	bool	Enable robustness features (NaN handling, normalization)	False
validate_initial_pose	bool	Check initial pose validity before decoding	False
return_diagnostics	bool	Include decoding diagnostics in output	False

Returns:

Type	Description
Dict[str, Any]	dict with 'positions' (N,3), 'quaternions' (N,4) wxyz,
Dict[str, Any]	and optionally 'diagnostics' if return_diagnostics=True.

Source code in src/dhb_xr/decoder/dhb_dr.py

def decode_dhb_dr(
    linear_motion_invariants: np.ndarray,
    angular_motion_invariants: np.ndarray,
    initial_pose: Dict[str, np.ndarray],
    method: EncodingMethod = EncodingMethod.POSITION,
    dhb_method: DHBMethod = DHBMethod.DOUBLE_REFLECTION,
    drop_padded: bool = True,
    robust_mode: bool = False,
    validate_initial_pose: bool = False,
    return_diagnostics: bool = False,
) -> Dict[str, Any]:
    """
    Reconstruct poses from DHB invariants.

    Parameters:
        linear_motion_invariants: (N, k) linear invariants
        angular_motion_invariants: (N, k) angular invariants
        initial_pose: {'position': (3,), 'quaternion': (4,) wxyz}
        method: 'pos' or 'vel'
        dhb_method: DHBMethod.ORIGINAL (3 inv) or DHBMethod.DOUBLE_REFLECTION (4 inv)
        drop_padded: if True, return positions/quaternions[2:] to match encoder padding
        robust_mode: Enable robustness features (NaN handling, normalization)
        validate_initial_pose: Check initial pose validity before decoding
        return_diagnostics: Include decoding diagnostics in output

    Returns:
        dict with 'positions' (N,3), 'quaternions' (N,4) wxyz,
        and optionally 'diagnostics' if return_diagnostics=True.
    """
    diagnostics = DecodingDiagnostics() if return_diagnostics else None

    # Validate initial pose if requested
    if validate_initial_pose or robust_mode:
        if not _validate_initial_pose(initial_pose):
            if diagnostics:
                diagnostics.initial_pose_valid = False
            if robust_mode:
                warnings.warn("Invalid initial pose detected. Using identity pose.")
                initial_pose = {
                    "position": np.zeros(3),
                    "quaternion": np.array([1.0, 0.0, 0.0, 0.0])
                }
            else:
                raise ValueError("Invalid initial pose: position or quaternion is malformed or contains NaN/Inf")
    linear_inv = np.asarray(linear_motion_invariants, dtype=np.float64)
    angular_inv = np.asarray(angular_motion_invariants, dtype=np.float64)
    num_samples = linear_inv.shape[0]
    assert angular_inv.shape[0] == num_samples

    if dhb_method == DHBMethod.ORIGINAL:
        linear_magnitude = linear_inv[:, 0]
        linear_angle_y = linear_inv[:, 1]
        linear_angle_x = linear_inv[:, 2]
        angular_magnitude = angular_inv[:, 0]
        angular_angle_y = angular_inv[:, 1]
        angular_angle_x = angular_inv[:, 2]
    else:
        linear_magnitude = linear_inv[:, 0]
        linear_angles = linear_inv[:, 1:4]
        angular_magnitude = angular_inv[:, 0]
        angular_angles = angular_inv[:, 1:4]

    position_mode = method == EncodingMethod.POSITION
    init_pos = np.asarray(initial_pose["position"]).reshape(3)
    init_quat = np.asarray(initial_pose["quaternion"]).reshape(4)

    trajectory_position = np.zeros((num_samples, 3))
    trajectory_quaternions = np.zeros((num_samples, 4))
    trajectory_quaternions[0] = init_quat

    linear_frame = np.eye(4)
    linear_frame[:3, 3] = init_pos
    linear_frame[3, 3] = float(position_mode)

    angular_frame = np.eye(4)
    angular_rot_temp = geom.quat_to_rot(init_quat)

    prev_pos = init_pos.copy()

    for i in range(num_samples):
        if position_mode:
            trajectory_position[i] = linear_frame[:3, 3]

        if dhb_method == DHBMethod.ORIGINAL:
            R_lin = geom.y_rot(linear_angle_y[i]) @ geom.x_rot(linear_angle_x[i])
        else:
            R_lin = geom.euler_to_rot(linear_angles[i])

        trans = np.array([linear_magnitude[i], 0.0, 0.0])
        T = np.eye(4)
        T[:3, :3] = R_lin
        T[:3, 3] = trans
        T[3, 3] = float(position_mode)
        linear_frame = linear_frame @ T

        if not position_mode:
            trajectory_position[i] = linear_frame[:3, 3]

        # Handle NaN in robust mode
        if robust_mode:
            if not np.all(np.isfinite(trajectory_position[i])):
                if diagnostics:
                    diagnostics.num_nan_positions += 1
                trajectory_position[i] = prev_pos.copy()
            else:
                # Track step size for diagnostics
                step_size = np.linalg.norm(trajectory_position[i] - prev_pos)
                if diagnostics:
                    diagnostics.max_step_size = max(diagnostics.max_step_size, step_size)
                    diagnostics.trajectory_length += step_size
                prev_pos = trajectory_position[i].copy()

        rvec = angular_frame[:3, :3] @ np.array([angular_magnitude[i], 0.0, 0.0])

        if dhb_method == DHBMethod.ORIGINAL:
            R_ang = geom.y_rot(angular_angle_y[i]) @ geom.x_rot(angular_angle_x[i])
        else:
            R_ang = geom.euler_to_rot(angular_angles[i])

        angular_rot_temp = angular_rot_temp @ geom.axis_angle_to_rot(rvec).T
        if i < num_samples - 1:
            quat = geom.rot_to_quat(angular_rot_temp)
            # Normalize quaternion in robust mode
            if robust_mode:
                quat = _normalize_quaternion(quat)
                if not np.all(np.isfinite(quat)):
                    if diagnostics:
                        diagnostics.num_nan_quaternions += 1
                    quat = trajectory_quaternions[i].copy()  # Use previous
            trajectory_quaternions[i + 1] = quat
        angular_frame[:3, :3] = angular_frame[:3, :3] @ R_ang

    if drop_padded:
        trajectory_position = trajectory_position[2:]
        trajectory_quaternions = trajectory_quaternions[2:]

    result = {
        "positions": trajectory_position,
        "quaternions": trajectory_quaternions,
    }

    if return_diagnostics and diagnostics is not None:
        result["diagnostics"] = diagnostics

    return result

reconstruct_trajectory_single_step

reconstruct_trajectory_single_step(
    linear_invariants,
    angular_invariants,
    current_linear_frame,
    current_angular_frame,
    method,
    dhb_method=DHBMethod.DOUBLE_REFLECTION,
)

Single-step reconstruction for use in optimization. Returns (new_linear_frame, new_angular_frame, new_position, new_rotation_vec).

Source code in src/dhb_xr/decoder/dhb_dr.py

def reconstruct_trajectory_single_step(
    linear_invariants: np.ndarray,
    angular_invariants: np.ndarray,
    current_linear_frame: np.ndarray,
    current_angular_frame: np.ndarray,
    method: str,
    dhb_method: DHBMethod = DHBMethod.DOUBLE_REFLECTION,
) -> tuple:
    """Single-step reconstruction for use in optimization. Returns (new_linear_frame, new_angular_frame, new_position, new_rotation_vec)."""
    if dhb_method == DHBMethod.ORIGINAL:
        linear_magnitude = linear_invariants[0]
        linear_angle_y = linear_invariants[1]
        linear_angle_x = linear_invariants[2]
        angular_magnitude = angular_invariants[0]
        angular_angle_y = angular_invariants[1]
        angular_angle_x = angular_invariants[2]
    else:
        linear_magnitude = float(linear_invariants[0])
        linear_angles = linear_invariants[1:4]
        angular_magnitude = float(angular_invariants[0])
        angular_angles = angular_invariants[1:4]

    position_mode = method == EncodingMethod.POSITION
    if position_mode:
        new_position = current_linear_frame[:3, 3].copy()

    if dhb_method == DHBMethod.ORIGINAL:
        R_lin = geom.y_rot(linear_angle_y) @ geom.x_rot(linear_angle_x)
    else:
        R_lin = geom.euler_to_rot(linear_angles)

    T = np.eye(4)
    T[:3, :3] = R_lin
    T[:3, 3] = np.array([linear_magnitude, 0.0, 0.0])
    T[3, 3] = float(position_mode)
    new_linear_frame = current_linear_frame @ T

    if not position_mode:
        new_position = new_linear_frame[:3, 3].copy()

    new_rotation = current_angular_frame[:3, :3] @ np.array(
        [angular_magnitude, 0.0, 0.0]
    )

    if dhb_method == DHBMethod.ORIGINAL:
        R_ang = geom.y_rot(angular_angle_y) @ geom.x_rot(angular_angle_x)
    else:
        R_ang = geom.euler_to_rot(angular_angles)

    new_angular_frame = np.eye(4)
    new_angular_frame[:3, :3] = current_angular_frame[:3, :3] @ R_ang

    return new_linear_frame, new_angular_frame, new_position, new_rotation

Main Functions

decode_dhb_dr

def decode_dhb_dr(
    linear_motion_invariants: np.ndarray,
    angular_motion_invariants: np.ndarray,
    initial_pose: Dict[str, np.ndarray],
    method: EncodingMethod = EncodingMethod.POSITION,
    dhb_method: DHBMethod = DHBMethod.DOUBLE_REFLECTION,
    drop_padded: bool = True,
    robust_mode: bool = False,
    validate_initial_pose: bool = False,
    return_diagnostics: bool = False,
) -> Dict[str, Any]:

Decode DHB-DR invariants back to trajectory.

Parameters:

• linear_motion_invariants: Linear motion invariants (M, 4) or (M, 3)
• angular_motion_invariants: Angular motion invariants (M, 4) or (M, 3)
• initial_pose: Initial pose dictionary with 'position' and 'quaternion' keys
• method: Decoding method (must match encoding method)
• dhb_method: DHB variant (must match encoding)
• drop_padded: Remove padding frames from output
• robust_mode: Enable robustness features
• validate_initial_pose: Validate initial pose format
• return_diagnostics: Return diagnostic information

Returns:

Dictionary containing: - positions: Reconstructed positions (N, 3) - quaternions: Reconstructed quaternions (N, 4) - Optional diagnostics if return_diagnostics=True

Round-trip Example

import numpy as np
from dhb_xr.encoder.dhb_dr import encode_dhb_dr
from dhb_xr.decoder.dhb_dr import decode_dhb_dr
from dhb_xr.core.types import DHBMethod, EncodingMethod

# Original trajectory
positions = np.random.randn(50, 3) * 0.01
positions = np.cumsum(positions, axis=0)
quaternions = np.tile([1, 0, 0, 0], (50, 1))

# Encode
result = encode_dhb_dr(
    positions, quaternions,
    method=EncodingMethod.POSITION,
    dhb_method=DHBMethod.DOUBLE_REFLECTION
)

# Decode
decoded = decode_dhb_dr(
    result['linear_motion_invariants'],
    result['angular_motion_invariants'],
    result['initial_pose'],
    method=EncodingMethod.POSITION,
    dhb_method=DHBMethod.DOUBLE_REFLECTION,
    drop_padded=True
)

# Check accuracy
error = np.linalg.norm(positions - decoded['positions'], axis=1)
rmse = np.sqrt(np.mean(error**2))
print(f"RMSE: {rmse * 1e6:.2f} μm")