Struct SE3 

pub struct SE3 { /* private fields */ }

SE(3) group element representing rigid body transformations in 3D.

Represented as a combination of SO(3) rotation and Vector3 translation.

Implementations

impl SE3

pub const DIM: usize = 3

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

pub const DOF: usize = 6

Degrees of freedom - dimension of the tangent space

pub const REP_SIZE: usize = 7

Representation size - size of the underlying data representation

pub fn identity() -> SE3

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<6>, Const<6>, ArrayStorage<f64, 6, 6>>

Get the identity matrix for Jacobians.

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

pub fn new( translation: Matrix<f64, Const<3>, Const<1>, ArrayStorage<f64, 3, 1>>, rotation: Unit<Quaternion<f64>>, ) -> SE3

Create a new SE3 element from translation and rotation.

Arguments

• translation - Translation vector [x, y, z]
• rotation - Unit quaternion representing rotation

pub fn from_translation_quaternion( translation: Matrix<f64, Const<3>, Const<1>, ArrayStorage<f64, 3, 1>>, quaternion: Quaternion<f64>, ) -> SE3

Create SE3 from translation components and Euler angles.

pub fn from_translation_euler( x: f64, y: f64, z: f64, roll: f64, pitch: f64, yaw: f64, ) -> SE3

Create SE3 from translation components and Euler angles.

pub fn from_isometry(isometry: Isometry<f64, Unit<Quaternion<f64>>, 3>) -> SE3

Create SE3 directly from an Isometry3.

pub fn from_translation_so3( translation: Matrix<f64, Const<3>, Const<1>, ArrayStorage<f64, 3, 1>>, rotation: SO3, ) -> SE3

Create SE3 from SO3 and Vector3 components.

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

Get the translation part as a Vector3.

pub fn rotation_so3(&self) -> SO3

Get the rotation part as SO3.

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

Get the rotation part as a UnitQuaternion.

pub fn isometry(&self) -> Isometry<f64, Unit<Quaternion<f64>>, 3>

Get as an Isometry3 (convenience method).

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

Get the transformation matrix (4x4 homogeneous matrix).

pub fn x(&self) -> f64

Get the x component of translation.

pub fn y(&self) -> f64

Get the y component of translation.

pub fn z(&self) -> f64

Get the z component of translation.

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

Get coefficients as array [tx, ty, tz, qw, qx, qy, qz].

Trait Implementations

impl Clone for SE3

fn clone(&self) -> SE3

Returns a duplicate of the value.

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

Performs copy-assignment from source.

impl Display for SE3

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 SE3

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

Converts to this type from the input type.

impl LieGroup for SE3

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

Get the inverse.

Arguments

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

Notes

Equation 170: Inverse of SE(3) matrix

M⁻¹ = [ Rᵀ -Rᵀt ] [ 0 1 ]

Equation 176: Jacobian of inverse operation

J_M⁻¹_M = - [ R [t]ₓ R ] [ 0 R ]

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

Composition of this and another SE3 element.

Arguments

• other - Another SE3 element.
• jacobian_self - Optional Jacobian matrix of the composition wrt this.
• jacobian_other - Optional Jacobian matrix of the composition wrt other.

Notes

Equation 171: Composition of SE(3) matrices

M_a M_b = [ R_aR_b R_at_b + t_a ] [ 0 1 ]

Equation 177: Jacobian of the composition wrt self.

J_MaMb_Ma = [ R_bᵀ -R_bᵀ*[t_b]ₓ ] [ 0 R_bᵀ ]

Equation 178: Jacobian of the composition wrt other.

J_MaMb_Mb = I_6

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

Get the SE3 corresponding Lie algebra element in vector form.

Arguments

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

Notes

Equation 173: SE(3) logarithmic map

τ = log(M) = [ V⁻¹(θ) t ] [ Log(R) ]

Equation 174: V(θ) function for SE(3) Log/Exp maps

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

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

Vee operator: log(g)^∨.

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

fn is_approx(&self, other: &SE3, 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 = SE3Tangent

The tangent space vector type

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

The Jacobian matrix type

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

Associated Lie algebra type

fn act( &self, vector: &Matrix<f64, Const<3>, Const<1>, ArrayStorage<f64, 3, 1>>, jacobian_self: Option<&mut <SE3 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) -> <SE3 as LieGroup>::JacobianMatrix

Adjoint matrix Ad(g).

fn random() -> SE3

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

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

Get the identity matrix for Jacobians.

fn zero_jacobian() -> <SE3 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 SE3

fn eq(&self, other: &SE3) -> 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<SE3> for SE3Tangent

const DIM: usize = 6

Dimension of the tangent space

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

Get the SE3 element.

Arguments

• tangent - Tangent vector [rho, theta]
• jacobian - Optional Jacobian matrix of the SE3 element wrt this.

Notes

Equation 172: SE(3) exponential map

M = exp(τ) = [ R(θ) t(ρ) ] [ 0 1 ]

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

Right Jacobian Jr.

Computes the right Jacobian matrix such that for small δφ: exp((φ + δφ)^∧) ≈ exp(φ^∧) ∘ exp((Jr δφ)^∧)

For SE(3), this involves computing Jacobians for both translation and rotation parts.

Returns

The right Jacobian matrix (6x6)

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

Left Jacobian Jl.

Computes the left Jacobian matrix such that for small δφ: exp((φ + δφ)^∧) ≈ exp((Jl δφ)^∧) ∘ exp(φ^∧)

Following manif conventions for SE(3) left Jacobian computation.

Returns

The left Jacobian matrix (6x6)

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

Inverse of right Jacobian Jr⁻¹.

Computes the inverse of the right Jacobian. This is used for computing perturbations and derivatives.

Numerical Conditioning Warning

This 6×6 Jacobian inverse inherits SO(3) conditioning issues plus scale effects.

Sources of numerical error:

• Embedded SO(3) rotation: (1 + cos θ) / (2θ sin θ) singularity
• Q-block coupling: rotation errors propagate to translation
• Scale amplification: large translations increase absolute error

Expected Precision:

• Small rotations (θ < 0.01): Jr * Jr⁻¹ ≈ I within ~1e-6
• Moderate rotations (θ < 0.1): Jr * Jr⁻¹ ≈ I within ~1e-4
• Larger rotations: Jr * Jr⁻¹ ≈ I within ~0.01

This is a fundamental mathematical limitation consistent with production SLAM libraries. See module documentation for references.

Returns

The inverse right Jacobian matrix (6x6)

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

Inverse of left Jacobian Jl⁻¹.

Computes the inverse of the left Jacobian following manif conventions.

Numerical Conditioning Warning

This 6×6 Jacobian inverse has the same conditioning issues as the right Jacobian inverse.

Sources of numerical error:

• Embedded SO(3) rotation: (1 + cos θ) / (2θ sin θ) singularity
• Q-block coupling: rotation errors propagate to translation
• Scale amplification: large translations increase absolute error

Expected Precision: Same as right Jacobian inverse (see above).

This is a fundamental mathematical limitation consistent with production SLAM libraries. See module documentation for references.

Returns

The inverse left Jacobian matrix (6x6)

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

Hat operator: φ^∧ (vector to matrix).

Converts the SE(3) tangent vector to its 4x4 matrix representation in the Lie algebra. Following manif conventions, the structure is: [ theta_× rho ] [ 0 0 ] where theta_× is the skew-symmetric matrix of the rotational part.

Returns

The 4x4 matrix representation in the SE(3) Lie algebra

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

Zero tangent vector.

Returns the zero element of the SE(3) tangent space.

Returns

A 6-dimensional zero vector

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

Random tangent vector (useful for testing).

Generates a random tangent vector with reasonable bounds. Translation components are in [-1, 1] and rotation components in [-0.1, 0.1].

Returns

A random 6-dimensional tangent vector

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

Check if the tangent vector is approximately zero.

Compares the norm of the tangent vector to the given tolerance.

Arguments

• tolerance - Tolerance for zero comparison

Returns

True if the norm is below the tolerance

fn normalize(&mut self)

Normalize the tangent vector to unit norm.

Modifies this tangent vector to have unit norm. If the vector is near zero, it remains unchanged.

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

Return a unit tangent vector in the same direction.

Returns a new tangent vector with unit norm in the same direction. If the original vector is near zero, returns zero.

Returns

A normalized copy of the tangent vector

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

Small adjoint matrix for SE(3).

For SE(3), the small adjoint matrix has the structure: [ Omega V ] [ 0 Omega ] where Omega is the skew-symmetric matrix of the angular part and V is the skew-symmetric matrix of the linear part.

fn lie_bracket(&self, other: &SE3Tangent) -> <SE3 as LieGroup>::TangentVector

Lie bracket for SE(3).

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

fn is_approx(&self, other: &SE3Tangent, 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) -> <SE3 as LieGroup>::LieAlgebra

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

Arguments

• i - Index of the generator (0-5 for SE(3))

Returns

The generator matrix

fn is_dynamic() -> bool

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

impl StructuralPartialEq for SE3