pywayne-vio-so3

Pywayne VIO SO3

Overview

Complete SO(3) rotation matrix toolkit for 3D rotations with Lie group/ Lie algebra operations, rotation representation conversions, skew-symmetric matrix operations, and rotation averaging.

Quick Start

from pywayne.vio.SO3 import SO3_skew, SO3_Exp, SO3_Log, SO3_to_quat
import numpy as np

# Skew-symmetric matrix
vec = np.array([1, 2, 3])
skew = SO3_skew(vec)  # Returns 3x3 skew-symmetric matrix

# Log/Exp mapping
R = np.eye(3)
rotvec = SO3_Log(R)  # Rotation vector (Lie algebra)
R_recon = SO3_Exp(rotvec)  # Back to rotation matrix

# Quaternion conversion
quat = SO3_to_quat(R)  # Returns [w, x, y, z]

Core Functions

Basic Operations

check_SO3(R)

Check if matrix is a valid SO(3) rotation matrix.

• Validates shape (3, 3)
• Checks R.T @ R = I (orthogonality)

SO3_mul(R1, R2)

Multiply two rotation matrices: R1 @ R2.

SO3_diff(R1, R2, from_1_to_2=True)

Compute relative rotation between two matrices.

• from_1_to_2=True: Returns R1.T @ R2
• from_1_to_2=False: Returns R2.T @ R1

SO3_inv(R)

Compute inverse of rotation matrix (transpose).

• Supports single (3, 3) or batch (N, 3, 3) inputs

Skew-Symmetric Matrices

SO3_skew(vec)

Convert 3D vector to skew-symmetric matrix.

vec = [x, y, z] -> [[ 0, -z,  y],
                    [ z,  0, -x],
                    [-y,  x,  0]]

• Supports single vector (3,) or batch (N, 3)

SO3_unskew(skew)

Extract vector from skew-symmetric matrix.

• Single matrix (3, 3) -> vector (3,)
• Batch (N, 3, 3) -> vectors (N, 3)

Rotation Representation Conversions

Quaternion

• SO3_from_quat(q) - Quaternion [w, x, y, z] to rotation matrix
• SO3_to_quat(R) - Rotation matrix to quaternion [w, x, y, z]
• Uses Hamilton convention (wxyz)

Axis-Angle

• SO3_from_axis_angle(axis, angle) - Axis-angle to rotation matrix
• SO3_to_axis_angle(R) - Returns (axis, angle) tuple

Euler Angles

• SO3_from_euler(euler_angles, axes='zyx', intrinsic=True) - Euler to matrix
• SO3_to_euler(R, axes='zyx', intrinsic=True) - Matrix to Euler
• Supports all rotation sequences