Skip to main content

rust_robotics_core/
lie.rs

1//! Lie-group utilities used by estimation and optimization algorithms.
2//!
3//! Tangent vectors use translation-first ordering throughout this module:
4//! `[vx, vy, omega]` for SE(2), and
5//! `[vx, vy, vz, omega_x, omega_y, omega_z]` for SE(3).
6//!
7//! The implementation follows the closed-form exponential, logarithm, and
8//! Jacobian expressions used by MathematicalRobotics, with explicit series
9//! expansions around zero and a stable SO(3) logarithm around pi.
10
11use nalgebra::{Matrix2, Matrix3, Matrix4, SMatrix, SVector, Vector2, Vector3};
12#[cfg(not(feature = "std"))]
13#[allow(unused_imports)]
14use num_traits::Float;
15
16/// A six-dimensional SE(3) tangent vector in translation-first order.
17pub type Vector6 = SVector<f64, 6>;
18/// A 6x6 matrix acting on an SE(3) tangent vector.
19pub type Matrix6 = SMatrix<f64, 6, 6>;
20
21const SMALL_ANGLE_SQUARED: f64 = 1.0e-12;
22const NEAR_PI: f64 = 1.0e-6;
23
24/// Returns the 3x3 skew-symmetric matrix for a vector.
25pub fn skew(vector: &Vector3<f64>) -> Matrix3<f64> {
26    Matrix3::new(
27        0.0, -vector.z, vector.y, vector.z, 0.0, -vector.x, -vector.y, vector.x, 0.0,
28    )
29}
30
31/// Extracts a vector from the skew-symmetric part of a 3x3 matrix.
32pub fn unskew(matrix: &Matrix3<f64>) -> Vector3<f64> {
33    Vector3::new(matrix[(2, 1)], matrix[(0, 2)], matrix[(1, 0)])
34}
35
36/// Exponential map from so(2) to SO(2).
37pub fn so2_exp(angle: f64) -> Matrix2<f64> {
38    let (sin_angle, cos_angle) = angle.sin_cos();
39    Matrix2::new(cos_angle, -sin_angle, sin_angle, cos_angle)
40}
41
42/// Logarithm map from SO(2) to so(2), returning an angle in `[-pi, pi]`.
43pub fn so2_log(rotation: &Matrix2<f64>) -> f64 {
44    rotation[(1, 0)].atan2(rotation[(0, 0)])
45}
46
47/// Exponential map from so(3) to SO(3).
48pub fn so3_exp(omega: &Vector3<f64>) -> Matrix3<f64> {
49    let theta_squared = omega.norm_squared();
50    let omega_hat = skew(omega);
51    let omega_hat_squared = omega_hat * omega_hat;
52    let (a, b) = rodrigues_coefficients(theta_squared);
53    Matrix3::identity() + a * omega_hat + b * omega_hat_squared
54}
55
56/// Logarithm map from SO(3) to the principal so(3) tangent vector.
57pub fn so3_log(rotation: &Matrix3<f64>) -> Vector3<f64> {
58    let cos_theta = ((rotation.trace() - 1.0) * 0.5).clamp(-1.0, 1.0);
59    let theta = cos_theta.acos();
60
61    if theta * theta < SMALL_ANGLE_SQUARED {
62        return 0.5 * unskew(&(rotation - rotation.transpose()));
63    }
64
65    if core::f64::consts::PI - theta < NEAR_PI {
66        return theta * axis_near_pi(rotation);
67    }
68
69    let scale = theta / (2.0 * theta.sin());
70    scale * unskew(&(rotation - rotation.transpose()))
71}
72
73/// Left Jacobian of SO(3).
74pub fn so3_left_jacobian(omega: &Vector3<f64>) -> Matrix3<f64> {
75    let theta_squared = omega.norm_squared();
76    let omega_hat = skew(omega);
77    let omega_hat_squared = omega_hat * omega_hat;
78    let (b, c) = jacobian_coefficients(theta_squared);
79    Matrix3::identity() + b * omega_hat + c * omega_hat_squared
80}
81
82/// Inverse of the SO(3) left Jacobian.
83pub fn so3_left_jacobian_inverse(omega: &Vector3<f64>) -> Matrix3<f64> {
84    let theta_squared = omega.norm_squared();
85    let omega_hat = skew(omega);
86    let omega_hat_squared = omega_hat * omega_hat;
87    let coefficient = if theta_squared < SMALL_ANGLE_SQUARED {
88        1.0 / 12.0 + theta_squared / 720.0 + theta_squared * theta_squared / 30_240.0
89    } else {
90        let theta = theta_squared.sqrt();
91        1.0 / theta_squared - (1.0 + theta.cos()) / (2.0 * theta * theta.sin())
92    };
93    Matrix3::identity() - 0.5 * omega_hat + coefficient * omega_hat_squared
94}
95
96/// Exponential map from se(2) to a homogeneous SE(2) matrix.
97pub fn se2_exp(tangent: &Vector3<f64>) -> Matrix3<f64> {
98    let translation_tangent = Vector2::new(tangent.x, tangent.y);
99    let angle = tangent.z;
100    let jacobian = so2_left_jacobian(angle);
101    let translation = jacobian * translation_tangent;
102    let rotation = so2_exp(angle);
103
104    Matrix3::new(
105        rotation[(0, 0)],
106        rotation[(0, 1)],
107        translation.x,
108        rotation[(1, 0)],
109        rotation[(1, 1)],
110        translation.y,
111        0.0,
112        0.0,
113        1.0,
114    )
115}
116
117/// Logarithm map from a homogeneous SE(2) matrix.
118pub fn se2_log(transform: &Matrix3<f64>) -> Vector3<f64> {
119    let rotation = transform.fixed_view::<2, 2>(0, 0).into_owned();
120    let translation = Vector2::new(transform[(0, 2)], transform[(1, 2)]);
121    let angle = so2_log(&rotation);
122    let translation_tangent = so2_left_jacobian_inverse(angle) * translation;
123    Vector3::new(translation_tangent.x, translation_tangent.y, angle)
124}
125
126/// Inverse of a homogeneous SE(2) matrix.
127pub fn se2_inverse(transform: &Matrix3<f64>) -> Matrix3<f64> {
128    let rotation = transform.fixed_view::<2, 2>(0, 0).into_owned();
129    let translation = Vector2::new(transform[(0, 2)], transform[(1, 2)]);
130    let inverse_rotation = rotation.transpose();
131    let inverse_translation = -(inverse_rotation * translation);
132    Matrix3::new(
133        inverse_rotation[(0, 0)],
134        inverse_rotation[(0, 1)],
135        inverse_translation.x,
136        inverse_rotation[(1, 0)],
137        inverse_rotation[(1, 1)],
138        inverse_translation.y,
139        0.0,
140        0.0,
141        1.0,
142    )
143}
144
145/// Adjoint matrix of an SE(2) transform for translation-first tangents.
146pub fn se2_adjoint(transform: &Matrix3<f64>) -> Matrix3<f64> {
147    let rotation = transform.fixed_view::<2, 2>(0, 0);
148    let tx = transform[(0, 2)];
149    let ty = transform[(1, 2)];
150    Matrix3::new(
151        rotation[(0, 0)],
152        rotation[(0, 1)],
153        ty,
154        rotation[(1, 0)],
155        rotation[(1, 1)],
156        -tx,
157        0.0,
158        0.0,
159        1.0,
160    )
161}
162
163/// Exponential map from se(3) to a homogeneous SE(3) matrix.
164pub fn se3_exp(tangent: &Vector6) -> Matrix4<f64> {
165    let velocity = tangent.fixed_rows::<3>(0).into_owned();
166    let omega = tangent.fixed_rows::<3>(3).into_owned();
167    let rotation = so3_exp(&omega);
168    let translation = so3_left_jacobian(&omega) * velocity;
169
170    Matrix4::new(
171        rotation[(0, 0)],
172        rotation[(0, 1)],
173        rotation[(0, 2)],
174        translation.x,
175        rotation[(1, 0)],
176        rotation[(1, 1)],
177        rotation[(1, 2)],
178        translation.y,
179        rotation[(2, 0)],
180        rotation[(2, 1)],
181        rotation[(2, 2)],
182        translation.z,
183        0.0,
184        0.0,
185        0.0,
186        1.0,
187    )
188}
189
190/// Logarithm map from a homogeneous SE(3) matrix.
191pub fn se3_log(transform: &Matrix4<f64>) -> Vector6 {
192    let rotation = transform.fixed_view::<3, 3>(0, 0).into_owned();
193    let translation = transform.fixed_view::<3, 1>(0, 3).into_owned();
194    let omega = so3_log(&rotation);
195    let velocity = so3_left_jacobian_inverse(&omega) * translation;
196    Vector6::new(
197        velocity.x, velocity.y, velocity.z, omega.x, omega.y, omega.z,
198    )
199}
200
201/// Inverse of a homogeneous SE(3) matrix.
202pub fn se3_inverse(transform: &Matrix4<f64>) -> Matrix4<f64> {
203    let rotation = transform.fixed_view::<3, 3>(0, 0).into_owned();
204    let translation = transform.fixed_view::<3, 1>(0, 3).into_owned();
205    let inverse_rotation = rotation.transpose();
206    let inverse_translation = -(inverse_rotation * translation);
207    Matrix4::new(
208        inverse_rotation[(0, 0)],
209        inverse_rotation[(0, 1)],
210        inverse_rotation[(0, 2)],
211        inverse_translation.x,
212        inverse_rotation[(1, 0)],
213        inverse_rotation[(1, 1)],
214        inverse_rotation[(1, 2)],
215        inverse_translation.y,
216        inverse_rotation[(2, 0)],
217        inverse_rotation[(2, 1)],
218        inverse_rotation[(2, 2)],
219        inverse_translation.z,
220        0.0,
221        0.0,
222        0.0,
223        1.0,
224    )
225}
226
227/// Adjoint matrix of an SE(3) transform for translation-first tangents.
228pub fn se3_adjoint(transform: &Matrix4<f64>) -> Matrix6 {
229    let rotation = transform.fixed_view::<3, 3>(0, 0).into_owned();
230    let translation = transform.fixed_view::<3, 1>(0, 3).into_owned();
231    let cross_rotation = skew(&translation) * rotation;
232    let mut adjoint = Matrix6::zeros();
233    adjoint.fixed_view_mut::<3, 3>(0, 0).copy_from(&rotation);
234    adjoint
235        .fixed_view_mut::<3, 3>(0, 3)
236        .copy_from(&cross_rotation);
237    adjoint.fixed_view_mut::<3, 3>(3, 3).copy_from(&rotation);
238    adjoint
239}
240
241fn so2_left_jacobian(angle: f64) -> Matrix2<f64> {
242    let angle_squared = angle * angle;
243    let (a, b) = if angle_squared < SMALL_ANGLE_SQUARED {
244        (
245            1.0 - angle_squared / 6.0 + angle_squared * angle_squared / 120.0,
246            angle * (0.5 - angle_squared / 24.0 + angle_squared * angle_squared / 720.0),
247        )
248    } else {
249        (angle.sin() / angle, (1.0 - angle.cos()) / angle)
250    };
251    Matrix2::new(a, -b, b, a)
252}
253
254fn so2_left_jacobian_inverse(angle: f64) -> Matrix2<f64> {
255    let half_angle = 0.5 * angle;
256    let diagonal = if angle * angle < SMALL_ANGLE_SQUARED {
257        1.0 - angle * angle / 12.0 - angle.powi(4) / 720.0
258    } else {
259        half_angle / half_angle.tan()
260    };
261    Matrix2::new(diagonal, half_angle, -half_angle, diagonal)
262}
263
264fn rodrigues_coefficients(theta_squared: f64) -> (f64, f64) {
265    if theta_squared < SMALL_ANGLE_SQUARED {
266        (
267            1.0 - theta_squared / 6.0 + theta_squared * theta_squared / 120.0,
268            0.5 - theta_squared / 24.0 + theta_squared * theta_squared / 720.0,
269        )
270    } else {
271        let theta = theta_squared.sqrt();
272        (theta.sin() / theta, (1.0 - theta.cos()) / theta_squared)
273    }
274}
275
276fn jacobian_coefficients(theta_squared: f64) -> (f64, f64) {
277    if theta_squared < SMALL_ANGLE_SQUARED {
278        (
279            0.5 - theta_squared / 24.0 + theta_squared * theta_squared / 720.0,
280            1.0 / 6.0 - theta_squared / 120.0 + theta_squared * theta_squared / 5_040.0,
281        )
282    } else {
283        let theta = theta_squared.sqrt();
284        (
285            (1.0 - theta.cos()) / theta_squared,
286            (theta - theta.sin()) / (theta_squared * theta),
287        )
288    }
289}
290
291fn axis_near_pi(rotation: &Matrix3<f64>) -> Vector3<f64> {
292    let xx = ((rotation[(0, 0)] + 1.0) * 0.5).max(0.0).sqrt();
293    let yy = ((rotation[(1, 1)] + 1.0) * 0.5).max(0.0).sqrt();
294    let zz = ((rotation[(2, 2)] + 1.0) * 0.5).max(0.0).sqrt();
295
296    let mut axis = if xx >= yy && xx >= zz && xx > f64::EPSILON {
297        Vector3::new(
298            xx,
299            (rotation[(0, 1)] + rotation[(1, 0)]) / (4.0 * xx),
300            (rotation[(0, 2)] + rotation[(2, 0)]) / (4.0 * xx),
301        )
302    } else if yy >= zz && yy > f64::EPSILON {
303        Vector3::new(
304            (rotation[(0, 1)] + rotation[(1, 0)]) / (4.0 * yy),
305            yy,
306            (rotation[(1, 2)] + rotation[(2, 1)]) / (4.0 * yy),
307        )
308    } else if zz > f64::EPSILON {
309        Vector3::new(
310            (rotation[(0, 2)] + rotation[(2, 0)]) / (4.0 * zz),
311            (rotation[(1, 2)] + rotation[(2, 1)]) / (4.0 * zz),
312            zz,
313        )
314    } else {
315        Vector3::x()
316    };
317
318    let antisymmetric = unskew(&(rotation - rotation.transpose()));
319    if axis.dot(&antisymmetric) < 0.0 {
320        axis = -axis;
321    }
322    axis.normalize()
323}
324
325#[cfg(test)]
326mod tests {
327    use super::*;
328
329    const TOLERANCE: f64 = 1.0e-9;
330
331    fn assert_matrix_close<const R: usize, const C: usize>(
332        actual: &SMatrix<f64, R, C>,
333        expected: &SMatrix<f64, R, C>,
334        tolerance: f64,
335    ) {
336        let error = (actual - expected).norm();
337        assert!(
338            error <= tolerance,
339            "matrix error {error} exceeded tolerance {tolerance}\nactual={actual}\nexpected={expected}"
340        );
341    }
342
343    #[test]
344    fn skew_matches_cross_product() {
345        let a = Vector3::new(0.3, -0.7, 1.2);
346        let b = Vector3::new(-0.4, 0.9, 0.2);
347        assert_matrix_close(&(skew(&a) * b), &a.cross(&b), TOLERANCE);
348    }
349
350    #[test]
351    fn so3_round_trips_regular_and_small_rotations() {
352        for omega in [
353            Vector3::new(0.2, -0.3, 0.4),
354            Vector3::new(1.0e-10, -2.0e-10, 3.0e-10),
355            Vector3::zeros(),
356        ] {
357            let recovered = so3_log(&so3_exp(&omega));
358            assert_matrix_close(&recovered, &omega, TOLERANCE);
359        }
360    }
361
362    #[test]
363    fn so3_log_is_stable_near_pi() {
364        let axis = Vector3::new(1.0, -2.0, 0.5).normalize();
365        let omega = axis * (core::f64::consts::PI - 1.0e-8);
366        let recovered_rotation = so3_exp(&so3_log(&so3_exp(&omega)));
367        assert_matrix_close(&recovered_rotation, &so3_exp(&omega), 1.0e-7);
368    }
369
370    #[test]
371    fn so3_jacobian_and_inverse_cancel() {
372        for omega in [
373            Vector3::new(0.4, -0.2, 0.1),
374            Vector3::new(1.0e-10, 2.0e-10, -1.0e-10),
375        ] {
376            let product = so3_left_jacobian(&omega) * so3_left_jacobian_inverse(&omega);
377            assert_matrix_close(&product, &Matrix3::identity(), TOLERANCE);
378        }
379    }
380
381    #[test]
382    fn se2_round_trip_and_inverse() {
383        let tangent = Vector3::new(1.2, -0.7, 0.8);
384        let transform = se2_exp(&tangent);
385        assert_matrix_close(&se2_log(&transform), &tangent, TOLERANCE);
386        assert_matrix_close(
387            &(transform * se2_inverse(&transform)),
388            &Matrix3::identity(),
389            TOLERANCE,
390        );
391    }
392
393    #[test]
394    fn se3_round_trip_and_inverse() {
395        let tangent = Vector6::new(1.2, -0.7, 0.3, 0.2, -0.4, 0.1);
396        let transform = se3_exp(&tangent);
397        assert_matrix_close(&se3_log(&transform), &tangent, TOLERANCE);
398        assert_matrix_close(
399            &(transform * se3_inverse(&transform)),
400            &Matrix4::identity(),
401            TOLERANCE,
402        );
403    }
404
405    #[test]
406    fn adjoints_match_group_conjugation() {
407        let se2_pose = se2_exp(&Vector3::new(0.4, -0.2, 0.7));
408        let se2_delta = Vector3::new(0.2, 0.1, -0.3);
409        assert_matrix_close(
410            &(se2_pose * se2_exp(&se2_delta) * se2_inverse(&se2_pose)),
411            &se2_exp(&(se2_adjoint(&se2_pose) * se2_delta)),
412            TOLERANCE,
413        );
414
415        let se3_pose = se3_exp(&Vector6::new(0.4, -0.2, 0.7, 0.1, 0.3, -0.2));
416        let se3_delta = Vector6::new(0.2, 0.1, -0.3, -0.2, 0.15, 0.1);
417        assert_matrix_close(
418            &(se3_pose * se3_exp(&se3_delta) * se3_inverse(&se3_pose)),
419            &se3_exp(&(se3_adjoint(&se3_pose) * se3_delta)),
420            1.0e-8,
421        );
422    }
423}