DHB-DR Encoding

dhb_dr

DHB-DR encoder: double-reflection (RMF) frame transport with Euler XYZ relative rotations. Quaternion convention: wxyz (scalar-first).

Robustness features (enabled via robust_mode=True): - 180-degree reversal detection and RMF fallback - Zero-motion segment handling with frame preservation - Frame validation to detect degenerate states - Configurable thresholds for edge case detection

Classes

EncodingDiagnostics dataclass

Diagnostics from robust encoding.

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

@dataclass
class EncodingDiagnostics:
    """Diagnostics from robust encoding."""
    num_reversals_detected: int = 0
    num_zero_motion_frames: int = 0
    reversal_indices: List[int] = None
    zero_motion_indices: List[int] = None
    frame_validation_failures: int = 0

    def __post_init__(self):
        if self.reversal_indices is None:
            self.reversal_indices = []
        if self.zero_motion_indices is None:
            self.zero_motion_indices = []

Functions

encode_dhb_dr

encode_dhb_dr(
    positions,
    quaternions,
    init_pose=None,
    method="pos",
    use_default_initial_frames=True,
    dhb_method=DHBMethod.DOUBLE_REFLECTION,
    robust_mode=False,
    reversal_threshold=-0.9,
    zero_motion_threshold=1e-06,
    validate_frames=False,
    return_diagnostics=False,
)

Compute DHB invariants (original or double-reflection).

Parameters:

Name	Type	Description	Default
positions	ndarray	(N, 3) position trajectory	required
quaternions	ndarray	(N, 4) wxyz quaternion trajectory	required
init_pose	Optional[Dict[str, ndarray]]	optional {'position': (3,), 'quaternion': (4,) wxyz}	None
method	str	EncodingMethod for position or velocity-based encoding	'pos'
use_default_initial_frames	bool	if True, pad and use identity-like frames	True
dhb_method	DHBMethod	DHBMethod.ORIGINAL (3 inv) or DHBMethod.DOUBLE_REFLECTION (4 inv)	DOUBLE_REFLECTION
robust_mode	bool	Enable robustness features (reversal detection, zero-motion handling)	False
reversal_threshold	float	Dot product threshold for 180° detection (default -0.9)	-0.9
zero_motion_threshold	float	Threshold for zero-motion detection	1e-06
validate_frames	bool	Check frame orthogonality at each step	False
return_diagnostics	bool	Include encoding diagnostics in output	False

Returns:

Type	Description
Dict[str, Any]	dict with linear_motion_invariants, angular_motion_invariants,
Dict[str, Any]	linear_frame_initial, angular_frame_initial, initial_pose,
Dict[str, Any]	and optionally 'diagnostics' if return_diagnostics=True.

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

def encode_dhb_dr(
    positions: np.ndarray,
    quaternions: np.ndarray,
    init_pose: Optional[Dict[str, np.ndarray]] = None,
    method: str = "pos",
    use_default_initial_frames: bool = True,
    dhb_method: DHBMethod = DHBMethod.DOUBLE_REFLECTION,
    robust_mode: bool = False,
    reversal_threshold: float = -0.9,
    zero_motion_threshold: float = 1e-6,
    validate_frames: bool = False,
    return_diagnostics: bool = False,
) -> Dict[str, Any]:
    """
    Compute DHB invariants (original or double-reflection).

    Parameters:
        positions: (N, 3) position trajectory
        quaternions: (N, 4) wxyz quaternion trajectory
        init_pose: optional {'position': (3,), 'quaternion': (4,) wxyz}
        method: EncodingMethod for position or velocity-based encoding
        use_default_initial_frames: if True, pad and use identity-like frames
        dhb_method: DHBMethod.ORIGINAL (3 inv) or DHBMethod.DOUBLE_REFLECTION (4 inv)
        robust_mode: Enable robustness features (reversal detection, zero-motion handling)
        reversal_threshold: Dot product threshold for 180° detection (default -0.9)
        zero_motion_threshold: Threshold for zero-motion detection
        validate_frames: Check frame orthogonality at each step
        return_diagnostics: Include encoding diagnostics in output

    Returns:
        dict with linear_motion_invariants, angular_motion_invariants,
        linear_frame_initial, angular_frame_initial, initial_pose,
        and optionally 'diagnostics' if return_diagnostics=True.
    """
    positions = np.asarray(positions, dtype=np.float64)
    quaternions = np.asarray(quaternions, dtype=np.float64)
    n = positions.shape[0]
    assert n > 2 and quaternions.shape[0] == n, "Need >2 samples and matching lengths"

    if use_default_initial_frames:
        positions = np.vstack((positions[0], positions[0], positions))
        quaternions = np.vstack((quaternions[0], quaternions[0], quaternions))
    positions = np.vstack((positions, positions[-1], positions[-1], positions[-1]))
    quaternions = np.vstack((quaternions, quaternions[-1], quaternions[-1], quaternions[-1]))

    num_samples = positions.shape[0]
    initial_pose = init_pose or {
        "position": positions[0].copy(),
        "quaternion": quaternions[0].copy(),
    }
    initial_pose["quaternion"] = np.asarray(initial_pose["quaternion"]).reshape(4)
    initial_pose["position"] = np.asarray(initial_pose["position"]).reshape(3)

    position_diff = np.diff(positions, axis=0)
    rotation_diff = np.zeros((num_samples - 1, 3))
    for i in range(1, num_samples):
        R_prev = geom.quat_to_rot(quaternions[i - 1]).T
        R_curr = geom.quat_to_rot(quaternions[i]).T
        R_rel = R_curr @ R_prev.T
        rotation_diff[i - 1] = geom.rot_to_axis_angle(R_rel)

    num_steps = position_diff.shape[0]
    frames = _compute_initial_frames(
        position_diff, rotation_diff, initial_pose, method, use_default_initial_frames
    )

    k = 4 if dhb_method == DHBMethod.DOUBLE_REFLECTION else 3
    linear_inv = np.zeros((num_steps - 2, k))
    angular_inv = np.zeros((num_steps - 2, k))

    # Initialize diagnostics if needed
    diagnostics = EncodingDiagnostics() if (robust_mode or return_diagnostics) else None

    lx = frames["linear_frame_x"].copy()
    lx2 = frames["linear_frame_x2"].copy()
    ly = frames["linear_frame_y"].copy()
    ax = frames["angular_frame_x"].copy()
    ax2 = frames["angular_frame_x2"].copy()
    ay = frames["angular_frame_y"].copy()

    for i in range(num_steps - 2):
        # Handle zero-motion in robust mode
        pos_is_zero = _is_zero_motion(position_diff[i], zero_motion_threshold) if robust_mode else False
        rot_is_zero = _is_zero_motion(rotation_diff[i], zero_motion_threshold) if robust_mode else False

        if robust_mode and pos_is_zero and diagnostics is not None:
            diagnostics.num_zero_motion_frames += 1
            diagnostics.zero_motion_indices.append(i)

        # Compute next frame axes
        if robust_mode and pos_is_zero:
            # For zero motion, preserve previous frame direction
            lx3 = lx2.copy()
        else:
            lx3 = _compute_frame_axis_x(position_diff[i + 2], lx2)

        if robust_mode and rot_is_zero:
            ax3 = ax2.copy()
        else:
            ax3 = _compute_frame_axis_x(rotation_diff[i + 2], ax2)

        if dhb_method == DHBMethod.DOUBLE_REFLECTION:
            ly2, linear_R_rel, lin_fallback = _double_reflection_step(
                lx, ly, lx2, position_diff[i], position_diff[i + 1],
                robust_mode=robust_mode, reversal_threshold=reversal_threshold
            )
            ay2, angular_R_rel, ang_fallback = _double_reflection_step(
                ax, ay, ax2, rotation_diff[i], rotation_diff[i + 1],
                robust_mode=robust_mode, reversal_threshold=reversal_threshold
            )

            # Track reversals in diagnostics
            if diagnostics is not None and lin_fallback:
                if _detect_reversal(position_diff[i], position_diff[i + 1], reversal_threshold):
                    diagnostics.num_reversals_detected += 1
                    diagnostics.reversal_indices.append(i)

            linear_inv[i, 0] = np.dot(lx, position_diff[i])
            angular_inv[i, 0] = np.dot(ax, rotation_diff[i])
            linear_inv[i, 1:4] = geom.rot_to_euler(linear_R_rel)
            angular_inv[i, 1:4] = geom.rot_to_euler(angular_R_rel)
        else:
            ly2 = _compute_frame_axis_y(lx2, lx3, ly)
            ay2 = _compute_frame_axis_y(ax2, ax3, ay)
            if np.dot(ly, ly2) < 0:
                ly2 = -ly2
            if np.dot(ay, ay2) < 0:
                ay2 = -ay2
            linear_inv[i] = _compute_invariants_original(
                position_diff[i], lx, lx2, ly, ly2
            )
            angular_inv[i] = _compute_invariants_original(
                rotation_diff[i], ax, ax2, ay, ay2
            )
            # No fallback tracking for ORIGINAL method
            lin_fallback = False

        # Frame validation in robust mode
        if validate_frames and diagnostics is not None:
            if not _validate_frame(lx2, ly2):
                diagnostics.frame_validation_failures += 1
            if not _validate_frame(ax2, ay2):
                diagnostics.frame_validation_failures += 1

        lx, lx2, ly = lx2, lx3, ly2
        ax, ax2, ay = ax2, ax3, ay2

    result = {
        "linear_motion_invariants": linear_inv,
        "angular_motion_invariants": angular_inv,
        "linear_frame_initial": frames["linear_frame_initial"],
        "angular_frame_initial": frames["angular_frame_initial"],
        "initial_pose": initial_pose,
    }

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

    return result

Main Functions

encode_dhb_dr

def encode_dhb_dr(
    positions: np.ndarray,
    quaternions: np.ndarray,
    init_pose: Optional[Dict[str, np.ndarray]] = None,
    method: EncodingMethod = EncodingMethod.POSITION,
    use_default_initial_frames: bool = True,
    dhb_method: DHBMethod = DHBMethod.DOUBLE_REFLECTION,
    robust_mode: bool = False,
    reversal_threshold: float = -0.9,
    zero_motion_threshold: float = 1e-6,
    validate_frames: bool = False,
    return_diagnostics: bool = False,
) -> Dict[str, Any]:

Encode trajectory to DHB-DR (Double-Reflection) invariants.

Parameters:

• positions: Trajectory positions (N, 3) array
• quaternions: Trajectory quaternions (N, 4) array in wxyz format
• init_pose: Optional initial pose override
• method: Initial frame computation method
• use_default_initial_frames: Use default frame initialization
• dhb_method: DHB variant (DOUBLE_REFLECTION or ORIGINAL)
• robust_mode: Enable robustness features
• reversal_threshold: Threshold for reversal detection
• zero_motion_threshold: Threshold for zero-motion detection
• validate_frames: Validate frame orthonormality
• return_diagnostics: Return diagnostic information

Returns:

Dictionary containing: - 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 used for encoding - Optional diagnostics if return_diagnostics=True

Usage Example

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

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

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

print(f"Linear invariants shape: {result['linear_motion_invariants'].shape}")
print(f"Angular invariants shape: {result['angular_motion_invariants'].shape}")