rigidity_core/lie/
se3.rs

1//! The group of rigid motions `SE(3)` and its Lie algebra `se(3)`.
2//!
3//! Algebra vectors are ordered `ξ = [ρ; φ]`: translational part first,
4//! rotational part second. The same order indexes the rows and columns of
5//! the adjoint and, further up the stack, the degrees of freedom in the
6//! conditioning report.
7
8use nalgebra::{Matrix3, Matrix4, Matrix6, Vector3, Vector6};
9
10use super::jacobian::{inverse_left_jacobian_so3, left_jacobian_so3};
11use super::so3::{So3, hat};
12
13/// A rigid motion: a rotation and a translation.
14#[derive(Debug, Clone, Copy, PartialEq)]
15pub struct Se3 {
16    rotation: So3,
17    translation: Vector3<f64>,
18}
19
20impl Se3 {
21    /// The identity transform.
22    pub fn identity() -> Self {
23        Self {
24            rotation: So3::identity(),
25            translation: Vector3::zeros(),
26        }
27    }
28
29    /// Builds a transform from a rotation and a translation.
30    pub fn from_parts(rotation: So3, translation: Vector3<f64>) -> Self {
31        Self {
32            rotation,
33            translation,
34        }
35    }
36
37    /// Builds a transform from a homogeneous 4×4 matrix.
38    ///
39    /// The rotational block is not checked for orthogonality. This exists
40    /// for reading poses out of files, where the matrix is already given
41    /// and vouched for by the data source rather than by us.
42    pub fn from_matrix_unchecked(matrix: Matrix4<f64>) -> Self {
43        Self {
44            rotation: So3::from_matrix_unchecked(matrix.fixed_view::<3, 3>(0, 0).into()),
45            translation: Vector3::new(matrix[(0, 3)], matrix[(1, 3)], matrix[(2, 3)]),
46        }
47    }
48
49    /// The rotational part.
50    pub fn rotation(&self) -> &So3 {
51        &self.rotation
52    }
53
54    /// The translational part.
55    pub fn translation(&self) -> &Vector3<f64> {
56        &self.translation
57    }
58
59    /// The homogeneous 4×4 matrix.
60    pub fn matrix(&self) -> Matrix4<f64> {
61        let mut m = Matrix4::identity();
62        m.fixed_view_mut::<3, 3>(0, 0)
63            .copy_from(self.rotation.matrix());
64        m.fixed_view_mut::<3, 1>(0, 3).copy_from(&self.translation);
65        m
66    }
67
68    /// The exponential map `se(3) → SE(3)`.
69    ///
70    /// `exp([ρ; φ]) = (exp(φ), J_l(φ)·ρ)`. The translation passes through
71    /// the left Jacobian; the naive `t = ρ` is only a first-order truth.
72    pub fn exp(xi: &Vector6<f64>) -> Self {
73        let rho = Vector3::new(xi[0], xi[1], xi[2]);
74        let phi = Vector3::new(xi[3], xi[4], xi[5]);
75        Self {
76            rotation: So3::exp(&phi),
77            translation: left_jacobian_so3(&phi) * rho,
78        }
79    }
80
81    /// The logarithm `SE(3) → se(3)`.
82    pub fn log(&self) -> Vector6<f64> {
83        let phi = self.rotation.log();
84        let rho = inverse_left_jacobian_so3(&phi) * self.translation;
85        Vector6::new(rho[0], rho[1], rho[2], phi[0], phi[1], phi[2])
86    }
87
88    /// The inverse transform.
89    pub fn inverse(&self) -> Self {
90        let inv_rotation = self.rotation.inverse();
91        Self {
92            rotation: inv_rotation,
93            translation: -(inv_rotation * self.translation),
94        }
95    }
96
97    /// The 6×6 adjoint representation.
98    ///
99    /// `Adj(T) = [[R, t^ R], [0, R]]` in the `ξ = [ρ; φ]` ordering. Its
100    /// defining property is `Adj(T)·ξ = vee(T · ξ^ · T⁻¹)`.
101    pub fn adjoint(&self) -> Matrix6<f64> {
102        let r = *self.rotation.matrix();
103        let t_hat = hat(&self.translation);
104        let mut adj = Matrix6::zeros();
105        adj.fixed_view_mut::<3, 3>(0, 0).copy_from(&r);
106        adj.fixed_view_mut::<3, 3>(0, 3).copy_from(&(t_hat * r));
107        adj.fixed_view_mut::<3, 3>(3, 3).copy_from(&r);
108        adj
109    }
110
111    /// Applies the transform to a point.
112    pub fn transform_point(&self, p: &Vector3<f64>) -> Vector3<f64> {
113        self.rotation * *p + self.translation
114    }
115}
116
117impl std::ops::Mul for Se3 {
118    type Output = Se3;
119    fn mul(self, rhs: Se3) -> Se3 {
120        Se3 {
121            rotation: self.rotation * rhs.rotation,
122            translation: self.rotation * rhs.translation + self.translation,
123        }
124    }
125}
126
127impl Default for Se3 {
128    fn default() -> Self {
129        Self::identity()
130    }
131}
132
133/// The embedding of `se(3)` into 4×4 matrices: `ξ^ = [[φ^, ρ], [0, 0]]`.
134pub fn hat_se3(xi: &Vector6<f64>) -> Matrix4<f64> {
135    let phi = Vector3::new(xi[3], xi[4], xi[5]);
136    let mut m = Matrix4::zeros();
137    m.fixed_view_mut::<3, 3>(0, 0).copy_from(&hat(&phi));
138    m[(0, 3)] = xi[0];
139    m[(1, 3)] = xi[1];
140    m[(2, 3)] = xi[2];
141    m
142}
143
144/// The inverse of [`hat_se3`].
145pub fn vee_se3(m: &Matrix4<f64>) -> Vector6<f64> {
146    let rot: Matrix3<f64> = m.fixed_view::<3, 3>(0, 0).into();
147    Vector6::new(
148        m[(0, 3)],
149        m[(1, 3)],
150        m[(2, 3)],
151        rot[(2, 1)],
152        rot[(0, 2)],
153        rot[(1, 0)],
154    )
155}
rigidity_core/lie/se3.rs

rigidity_core/lie/
se3.rs
