cv_core/
so3.rs

1use derive_more::{AsMut, AsRef, Deref, DerefMut, From, Into};
2use nalgebra::{Matrix3, Matrix4, Rotation3, Unit, Vector3};
3use num_traits::Float;
4
5/// Contains a member of the lie algebra so(3), a representation of the tangent space
6/// of 3d rotation. This is also known as the lie algebra of the 3d rotation group SO(3).
7///
8/// This is only intended to be used in optimization problems where it is desirable to
9/// have unconstranied variables representing the degrees of freedom of the rotation.
10/// In all other cases, a rotation matrix should be used to store rotations, since the
11/// conversion to and from a rotation matrix is non-trivial.
12#[derive(Debug, Clone, Copy, PartialEq, PartialOrd, AsMut, AsRef, Deref, DerefMut, From, Into)]
13pub struct Skew3(pub Vector3<f64>);
14
15impl Skew3 {
16    /// Converts the Skew3 to a Rotation3 matrix.
17    pub fn rotation(self) -> Rotation3<f64> {
18        self.into()
19    }
20
21    /// This converts a matrix in skew-symmetric form into a Skew3.
22    ///
23    /// Warning: Does no check to ensure matrix is actually skew-symmetric.
24    pub fn vee(mat: Matrix3<f64>) -> Self {
25        Self(Vector3::new(mat.m32, mat.m13, mat.m21))
26    }
27
28    /// This converts the Skew3 into its skew-symmetric matrix form.
29    #[rustfmt::skip]
30    pub fn hat(self) -> Matrix3<f64> {
31        self.0.cross_matrix()
32    }
33
34    /// This converts the Skew3 into its squared skew-symmetric matrix form efficiently.
35    #[rustfmt::skip]
36    pub fn hat2(self) -> Matrix3<f64> {
37        let w = self.0;
38        let w11 = w.x * w.x;
39        let w12 = w.x * w.y;
40        let w13 = w.x * w.z;
41        let w22 = w.y * w.y;
42        let w23 = w.y * w.z;
43        let w33 = w.z * w.z;
44        Matrix3::new(
45            -w22 - w33,     w12,           w13,
46             w12,          -w11 - w33,     w23,
47             w13,           w23,          -w11 - w22,
48        )
49    }
50
51    /// Computes the lie bracket [self, rhs].
52    pub fn bracket(self, rhs: Self) -> Self {
53        Self::vee(self.hat() * rhs.hat() - rhs.hat() * self.hat())
54    }
55
56    /// The jacobian of the output of a rotation in respect to the
57    /// input of a rotation.
58    ///
59    /// `y = R * x`
60    ///
61    /// `dy/dx = R`
62    ///
63    /// The formula is pretty simple and is just the rotation matrix created
64    /// from the exponential map of this so(3) element into SO(3). The result is converted
65    /// to homogeneous form (by adding a new dimension with a `1` in the diagonal) so
66    /// that it is compatible with homogeneous coordinates.
67    ///
68    /// If you have the rotation matrix already, please use the rotation matrix itself
69    /// rather than calling this method. Calling this method will waste time converting
70    /// the [`Skew3`] back into a [`Rotation3`], which is non-trivial.
71    pub fn jacobian_input(self) -> Matrix4<f64> {
72        let rotation: Rotation3<f64> = self.into();
73        let matrix: Matrix3<f64> = rotation.into();
74        matrix.to_homogeneous()
75    }
76
77    /// The jacobian of the output of a rotation in respect to the
78    /// rotation itself.
79    ///
80    /// `y = R * x`
81    ///
82    /// `dy/dR = -hat(y)`
83    ///
84    /// The derivative is purely based on the current output vector, and thus doesn't take `self`.
85    ///
86    /// Note that when working with homogeneous projective coordinates, only the first three components
87    /// (the bearing) are relevant, hence the resulting matrix is a [`Matrix3`].
88    pub fn jacobian_self(y: Vector3<f64>) -> Matrix3<f64> {
89        y.cross_matrix()
90    }
91}
92
93/// This is the exponential map.
94impl From<Skew3> for Rotation3<f64> {
95    fn from(w: Skew3) -> Self {
96        // This check is done to avoid the degenerate case where the angle is near zero.
97        let theta2 = w.0.norm_squared();
98        if theta2 <= f64::epsilon() {
99            Rotation3::from_matrix(&(Matrix3::identity() + w.hat()))
100        } else {
101            let theta = theta2.sqrt();
102            let axis = Unit::new_unchecked(w.0 / theta);
103            Self::from_axis_angle(&axis, theta)
104        }
105    }
106}
107
108/// This is the log map.
109impl From<Rotation3<f64>> for Skew3 {
110    fn from(r: Rotation3<f64>) -> Self {
111        Self(r.scaled_axis())
112    }
113}
cv_core/so3.rs

cv_core/
so3.rs
