Struct SO3 

pub struct SO3 { /* private fields */ }

SO(3) group element representing rotations in 3D.

Internally represented using nalgebra’s UnitQuaternion for efficient rotations.

Implementations

impl SO3

pub const DIM: usize = 3

Space dimension - dimension of the ambient space that the group acts on

pub const DOF: usize = 3

Degrees of freedom - dimension of the tangent space

pub const REP_SIZE: usize = 4

Representation size - size of the underlying data representation

pub fn identity() -> SO3

Get the identity element of the group.

Returns the neutral element e such that e ∘ g = g ∘ e = g for any group element g.

pub fn jacobian_identity() -> Matrix<f64, Const<3>, Const<3>, ArrayStorage<f64, 3, 3>>

Get the identity matrix for Jacobians.

Returns the identity matrix in the appropriate dimension for Jacobian computations.

pub fn new(quaternion: Unit<Quaternion<f64>>) -> SO3

Create a new SO(3) element from a unit quaternion.

Arguments

• quaternion - Unit quaternion representing rotation

pub fn from_quaternion_coeffs(x: f64, y: f64, z: f64, w: f64) -> SO3

Create SO(3) from quaternion coefficients in G2O convention [x, y, z, w].

This parameter order matches the G2O file format where quaternion components are stored as (qx, qy, qz, qw). For nalgebra’s (w, x, y, z) convention, use from_quaternion_wxyz().

Arguments

• x - i component of quaternion
• y - j component of quaternion
• z - k component of quaternion
• w - w (real) component of quaternion

pub fn from_quaternion_wxyz(w: f64, x: f64, y: f64, z: f64) -> SO3

Create SO(3) from quaternion coefficients in nalgebra convention [w, x, y, z].

This parameter order matches nalgebra’s Quaternion::new(w, x, y, z). For G2O file format order (qx, qy, qz, qw), use from_quaternion_coeffs().

Arguments

• w - w (real) component of quaternion
• x - i component of quaternion
• y - j component of quaternion
• z - k component of quaternion

pub fn from_euler_angles(roll: f64, pitch: f64, yaw: f64) -> SO3

Create SO(3) from Euler angles (roll, pitch, yaw).

pub fn from_axis_angle( axis: &Matrix<f64, Const<3>, Const<1>, ArrayStorage<f64, 3, 1>>, angle: f64, ) -> SO3

Create SO(3) from axis-angle representation.

pub fn from_scaled_axis( axis_angle: Matrix<f64, Const<3>, Const<1>, ArrayStorage<f64, 3, 1>>, ) -> SO3

Create SO(3) from scaled axis (axis-angle vector).

pub fn quaternion(&self) -> Unit<Quaternion<f64>>

Get the quaternion representation.

pub fn from_quaternion(quaternion: Unit<Quaternion<f64>>) -> SO3

Create SO3 from quaternion (alias for new)

pub fn to_quaternion(&self) -> Unit<Quaternion<f64>>

Get the quaternion representation (alias for quaternion)

pub fn quat(&self) -> Quaternion<f64>

Get the raw quaternion coefficients.

pub fn x(&self) -> f64

Get the x component of the quaternion.

pub fn y(&self) -> f64

Get the y component of the quaternion.

pub fn z(&self) -> f64

Get the z component of the quaternion.

pub fn w(&self) -> f64

Get the w component of the quaternion.

pub fn rotation_matrix( &self, ) -> Matrix<f64, Const<3>, Const<3>, ArrayStorage<f64, 3, 3>>

Get the rotation matrix (3x3).

pub fn transform( &self, ) -> Matrix<f64, Const<4>, Const<4>, ArrayStorage<f64, 4, 4>>

Get the homogeneous transformation matrix (4x4).

pub fn set_quaternion(&mut self, coeffs: &[f64; 4])

Set the quaternion from coefficients array [w, x, y, z].

pub fn coeffs(&self) -> [f64; 4]

Get coefficients as array [w, x, y, z].

pub fn distance(&self, other: &SO3) -> f64

Calculate the distance between two SO3 elements

Computes the geodesic distance, which is the norm of the log map of the relative rotation between the two elements.

Trait Implementations

impl Clone for SO3

fn clone(&self) -> SO3

Returns a duplicate of the value.

fn clone_from(&mut self, source: &Self)

Performs copy-assignment from source.

impl Display for SO3

fn fmt(&self, f: &mut Formatter<'_>) -> Result<(), Error>

Formats the value using the given formatter.

impl From<Matrix<f64, Dyn, Const<1>, VecStorage<f64, Dyn, Const<1>>>> for SO3

fn from(data: Matrix<f64, Dyn, Const<1>, VecStorage<f64, Dyn, Const<1>>>) -> SO3

Converts to this type from the input type.

impl LieGroup for SO3

fn inverse( &self, jacobian: Option<&mut <SO3 as LieGroup>::JacobianMatrix>, ) -> SO3

SO3 inverse.

Arguments

• jacobian - Optional Jacobian matrix of the inverse wrt self.

Notes

R⁻¹ = Rᵀ, for quaternions: q⁻¹ = q*

Equation 140: Jacobian of Inverse for SO(3)

J_R⁻¹_R = -Adj(R) = -R

fn compose( &self, other: &SO3, jacobian_self: Option<&mut <SO3 as LieGroup>::JacobianMatrix>, jacobian_other: Option<&mut <SO3 as LieGroup>::JacobianMatrix>, ) -> SO3

SO3 composition.

Arguments

• other - Another SO3 element.
• jacobian_self - Optional Jacobian matrix of the composition wrt self.
• jacobian_other - Optional Jacobian matrix of the composition wrt other.

Notes

Equation 141: Jacobian of the composition wrt self.

J_QR_R = Adj(R⁻¹) = Rᵀ

Equation 142: Jacobian of the composition wrt other.

J_QR_Q = I

fn log( &self, jacobian: Option<&mut <SO3 as LieGroup>::JacobianMatrix>, ) -> <SO3 as LieGroup>::TangentVector

Get the SO3 corresponding Lie algebra element in vector form.

Arguments

• jacobian - Optional Jacobian matrix of the tangent wrt to self.

Notes

Equation 133: Logarithmic map for unit quaternions (S³)

θu = Log(q) = (2 / ||v||) * v * arctan(||v||, w) ∈ R³

Equation 144: Inverse of Right Jacobian for SO(3) Exp map

J_R⁻¹(θ) = I + (1/2) [θ]ₓ + (1/θ² - (1 + cos θ)/(2θ sin θ)) [θ]ₓ²

fn vee(&self) -> <SO3 as LieGroup>::TangentVector

Vee operator: log(g)^∨.

Maps a group element g ∈ G to its tangent vector log(g)^∨ ∈ 𝔤. For SO(3), this is the same as log().

fn is_approx(&self, other: &SO3, tolerance: f64) -> bool

Check if the element is approximately equal to another element.

Arguments

• other - The other element to compare with
• tolerance - The tolerance for the comparison

type TangentVector = SO3Tangent

The tangent space vector type

type JacobianMatrix = Matrix<f64, Const<3>, Const<3>, ArrayStorage<f64, 3, 3>>

The Jacobian matrix type

type LieAlgebra = Matrix<f64, Const<3>, Const<3>, ArrayStorage<f64, 3, 3>>

Associated Lie algebra type

fn act( &self, vector: &Matrix<f64, Const<3>, Const<1>, ArrayStorage<f64, 3, 1>>, jacobian_self: Option<&mut <SO3 as LieGroup>::JacobianMatrix>, jacobian_vector: Option<&mut Matrix<f64, Const<3>, Const<3>, ArrayStorage<f64, 3, 3>>>, ) -> Matrix<f64, Const<3>, Const<1>, ArrayStorage<f64, 3, 1>>

Act on a vector v: g ⊙ v.

fn adjoint(&self) -> <SO3 as LieGroup>::JacobianMatrix

Adjoint matrix Ad(g).

fn random() -> SO3

Generate a random element (useful for testing and initialization).

fn jacobian_identity() -> <SO3 as LieGroup>::JacobianMatrix

Get the identity matrix for Jacobians.

fn zero_jacobian() -> <SO3 as LieGroup>::JacobianMatrix

Get a zero Jacobian matrix.

fn normalize(&mut self)

Normalize/project the element to the manifold.

fn is_valid(&self, tolerance: f64) -> bool

Check if the element is approximately on the manifold.

fn right_plus( &self, tangent: &Self::TangentVector, jacobian_self: Option<&mut Self::JacobianMatrix>, jacobian_tangent: Option<&mut Self::JacobianMatrix>, ) -> Self

Right plus operation: g ⊞ φ = g ∘ exp(φ^∧).

fn right_minus( &self, other: &Self, jacobian_self: Option<&mut Self::JacobianMatrix>, jacobian_other: Option<&mut Self::JacobianMatrix>, ) -> Self::TangentVector

Right minus operation: g₁ ⊟ g₂ = log(g₂⁻¹ ∘ g₁)^∨.

fn left_plus( &self, tangent: &Self::TangentVector, jacobian_tangent: Option<&mut Self::JacobianMatrix>, jacobian_self: Option<&mut Self::JacobianMatrix>, ) -> Self

Left plus operation: φ ⊞ g = exp(φ^∧) ∘ g.

fn left_minus( &self, other: &Self, jacobian_self: Option<&mut Self::JacobianMatrix>, jacobian_other: Option<&mut Self::JacobianMatrix>, ) -> Self::TangentVector

Left minus operation: g₁ ⊟ g₂ = log(g₁ ∘ g₂⁻¹)^∨.

fn plus( &self, tangent: &Self::TangentVector, jacobian_self: Option<&mut Self::JacobianMatrix>, jacobian_tangent: Option<&mut Self::JacobianMatrix>, ) -> Self

Convenience method for right_plus. Equivalent to g ⊞ φ.

fn minus( &self, other: &Self, jacobian_self: Option<&mut Self::JacobianMatrix>, jacobian_other: Option<&mut Self::JacobianMatrix>, ) -> Self::TangentVector

Convenience method for right_minus. Equivalent to g₁ ⊟ g₂.

fn between( &self, other: &Self, jacobian_self: Option<&mut Self::JacobianMatrix>, jacobian_other: Option<&mut Self::JacobianMatrix>, ) -> Self

Compute g₁⁻¹ ∘ g₂ (relative transformation).

fn tangent_dim(&self) -> usize

Get the dimension of the tangent space for this manifold element.

impl PartialEq for SO3

fn eq(&self, other: &SO3) -> bool

Tests for self and other values to be equal, and is used by ==.

fn ne(&self, other: &Rhs) -> bool

Tests for !=. The default implementation is almost always sufficient, and should not be overridden without very good reason.

impl Tangent<SO3> for SO3Tangent

const DIM: usize = 3

Dimension of the tangent space

fn exp(&self, jacobian: Option<&mut <SO3 as LieGroup>::JacobianMatrix>) -> SO3

SO3 exponential map.

Arguments

• tangent - Tangent vector [θx, θy, θz]
• jacobian - Optional Jacobian matrix of the SO3 element wrt self.

Notes

Equation 132: Exponential map for unit quaternions (S³)

q = Exp(θu) = cos(θ/2) + u sin(θ/2) ∈ H

Equation 143: Right Jacobian for SO(3) Exp map

J_R(θ) = I - (1 - cos θ)/θ² [θ]ₓ + (θ - sin θ)/θ³ [θ]ₓ²

fn right_jacobian(&self) -> <SO3 as LieGroup>::JacobianMatrix

Right Jacobian for SO(3)

Notes

Equation 143: Right Jacobian for SO(3) Exp map

J_R(θ) = I - (1 - cos θ)/θ² [θ]ₓ + (θ - sin θ)/θ³ [θ]ₓ²

fn left_jacobian(&self) -> <SO3 as LieGroup>::JacobianMatrix

Left Jacobian for SO(3)

Notes

Equation 144: Left Jacobian for SO(3) Exp map

J_R⁻¹(θ) = I + (1 - cos θ)/θ² [θ]ₓ + (θ - sin θ)/θ³ [θ]ₓ²

fn right_jacobian_inv(&self) -> <SO3 as LieGroup>::JacobianMatrix

Right Jacobian inverse for SO(3)

Notes

Equation 145: Right Jacobian inverse for SO(3) Exp map

J_R⁻¹(θ) = I + (1 - cos θ)/θ² [θ]ₓ + (θ - sin θ)/θ³ [θ]ₓ²

Numerical Conditioning Warning

This function has inherent numerical conditioning issues that cannot be eliminated.

The formula contains the term (1 + cos θ) / (2θ sin θ) which:

• Becomes indeterminate (0/0) as θ → 0
• Has a singularity as θ → π (sin θ → 0)
• Amplifies floating-point errors for θ > 0.5 rad

Expected Precision:

• θ < 0.01 rad: Jr * Jr⁻¹ ≈ I within ~1e-6
• 0.01 < θ < 0.1 rad: Jr * Jr⁻¹ ≈ I within ~1e-4
• θ > 0.1 rad: Jr * Jr⁻¹ ≈ I within ~0.01

This is mathematically unavoidable and is consistent with all production Lie group libraries (manif, Sophus, GTSAM). See module documentation for references.

fn left_jacobian_inv(&self) -> <SO3 as LieGroup>::JacobianMatrix

Left Jacobian inverse for SO(3)

Notes

Equation 146: Left Jacobian inverse for SO(3) Exp map

J_L⁻¹(θ) = I - (1/2) [θ]ₓ + (1/θ² - (1 + cos θ)/(2θ sin θ)) [θ]ₓ²

Numerical Conditioning Warning

This function has inherent numerical conditioning issues that cannot be eliminated.

The problematic term 1/θ² - (1 + cos θ)/(2θ sin θ) exhibits:

• Catastrophic cancellation as θ → 0 (requires Taylor series expansion)
• Division by zero as θ → π (sin θ → 0)
• Poor conditioning for θ > 0.5 rad (~29°)

Expected Precision (same as right Jacobian inverse):

• θ < 0.01 rad: Jl * Jl⁻¹ ≈ I within ~1e-6
• 0.01 < θ < 0.1 rad: Jl * Jl⁻¹ ≈ I within ~1e-4
• θ > 0.1 rad: Jl * Jl⁻¹ ≈ I within ~0.01

This is a fundamental mathematical limitation documented in:

• Nurlanov et al. (2021): SO(3) log map degeneracies
• Sophus library: https://github.com/strasdat/Sophus/issues/179

fn hat(&self) -> <SO3 as LieGroup>::LieAlgebra

Hat map for SO(3)

Notes

[θ]ₓ = [0 -θz θy; θz 0 -θx; -θy θx 0]

fn small_adj(&self) -> <SO3 as LieGroup>::JacobianMatrix

Small adjoint matrix for SO(3).

For SO(3), the small adjoint is the skew-symmetric matrix (hat operator).

fn lie_bracket(&self, other: &SO3Tangent) -> <SO3 as LieGroup>::TangentVector

Lie bracket for SO(3).

Computes the Lie bracket [this, other] = this.small_adj() * other.

fn is_approx(&self, other: &SO3Tangent, tolerance: f64) -> bool

Check if this tangent vector is approximately equal to another.

Arguments

• other - The other tangent vector to compare with
• tolerance - The tolerance for the comparison

fn generator(&self, i: usize) -> <SO3 as LieGroup>::LieAlgebra

Get the ith generator of the SO(3) Lie algebra.

Arguments

• i - Index of the generator (0, 1, or 2 for SO(3))

Returns

The generator matrix

fn zero() -> <SO3 as LieGroup>::TangentVector

Zero tangent vector.

fn random() -> <SO3 as LieGroup>::TangentVector

Random tangent vector (useful for testing).

fn is_zero(&self, tolerance: f64) -> bool

Check if the tangent vector is approximately zero.

fn normalize(&mut self)

Normalize the tangent vector to unit norm.

fn normalized(&self) -> <SO3 as LieGroup>::TangentVector

Return a unit tangent vector in the same direction.

fn is_dynamic() -> bool

Whether this tangent type has dynamic (runtime-determined) dimension.

impl StructuralPartialEq for SO3