sophus_lie/
lib.rs

1#![cfg_attr(feature = "simd", feature(portable_simd))]
2#![deny(missing_docs)]
3#![allow(clippy::needless_range_loop)]
4#![no_std]
5#![doc = include_str!(concat!("../", std::env!("CARGO_PKG_README")))]
6#![cfg_attr(nightly, feature(doc_auto_cfg))]
7
8#[doc = include_str!(concat!("../",  core::env!("CARGO_PKG_README")))]
9#[cfg(doctest)]
10pub struct ReadmeDoctests;
11
12#[cfg(feature = "std")]
13extern crate std;
14
15mod complex;
16mod groups;
17mod lie_group;
18mod quaternion;
19mod sl2c;
20
21/// sophus_lie prelude.
22///
23/// It is recommended to import this prelude when working with `sophus_lie` types:
24///
25/// ```
26/// use sophus_lie::prelude::*;
27/// ```
28///
29/// or
30///
31/// ```ignore
32/// use sophus::prelude::*;
33/// ```
34///
35/// to import all preludes when using the `sophus` umbrella crate.
36pub mod prelude {
37    pub use sophus_autodiff::prelude::*;
38
39    pub use crate::{
40        IsAffineGroup,
41        IsLieGroup,
42    };
43}
44
45use core::fmt::Debug;
46
47use sophus_autodiff::prelude::*;
48
49pub use crate::{
50    complex::*,
51    groups::{
52        affine_group_template::*,
53        galilean3::*,
54        isometry2::*,
55        isometry3::*,
56        rotation_boost3::*,
57        rotation2::*,
58        rotation3::*,
59    },
60    lie_group::{
61        LieGroup,
62        average::*,
63        factor_lie_group::*,
64        real_lie_group::*,
65    },
66    quaternion::*,
67    sl2c::*,
68};
69
70/// Disambiguate the parameters.
71pub trait HasDisambiguate<
72    S: IsScalar<BATCH, DM, DN>,
73    const PARAMS: usize,
74    const BATCH: usize,
75    const DM: usize,
76    const DN: usize,
77>: Clone + Debug
78{
79    /// Disambiguate the parameters.
80    ///
81    /// Note that that in most cases the default implementation is what you want, and a
82    /// group parameterization SHAll be chosen that does not require disambiguation.
83    ///
84    /// For instance, do not represent 2d rotations as a single angle, since theta and theta + 2pi,
85    /// and theta + 4pi, etc. are the same rotations, and which would require disambiguation.
86    /// Use the following 2d unit vector (cos(theta), sin(theta)) - aka unit complex number -
87    /// instead.
88    ///
89    /// Example where disambiguation is needed: A rotation in 3d represented by a unit quaternion.
90    /// The quaternion (r, (x, y, z)) and (r, (-x, -y, -z)) represent the same rotation. In this
91    /// case, the disambiguation function would return a disambiguated representation, e.g. a
92    /// unit quaternion with the positive r component.
93    fn disambiguate(params: S::Vector<PARAMS>) -> S::Vector<PARAMS> {
94        params
95    }
96}
97
98/// Lie Group implementation trait
99///
100/// Here, the actual manifold is represented by as a N-dimensional parameter tuple.
101/// See LieGroup struct for the concrete implementation.
102///
103/// DOF: Degrees of freedom
104/// NUM_PARAMS:
105/// POINT_DIM:
106/// N: dimension ambient the dimension
107pub trait IsLieGroupImpl<
108    S: IsScalar<BATCH, DM, DN>,
109    const DOF: usize,
110    const PARAMS: usize,
111    const POINT: usize,
112    const AMBIENT: usize,
113    const BATCH: usize,
114    const DM: usize,
115    const DN: usize,
116>:
117    IsParamsImpl<S, PARAMS, BATCH, DM, DN>
118    + IsTangent<S, DOF, BATCH, DM, DN>
119    + HasDisambiguate<S, PARAMS, BATCH, DM, DN>
120    + Clone
121    + Copy
122    + Debug
123{
124    /// Generic scalar, real scalar, and dual scalar
125    type GenG<S2: IsScalar<BATCH, DM, DN>>: IsLieGroupImpl<S2, DOF, PARAMS, POINT, AMBIENT, BATCH, DM, DN>;
126    /// Real scalar
127    type RealG: IsLieGroupImpl<S::RealScalar, DOF, PARAMS, POINT, AMBIENT, BATCH, 0, 0>;
128    /// DualScalar scalar - for automatic differentiation
129    type DualG<const M: usize, const N: usize>: IsLieGroupImpl<S::DualScalar<M, N>, DOF, PARAMS, POINT, AMBIENT, BATCH, M, N>;
130
131    /// is transformation origin preserving?
132    const IS_ORIGIN_PRESERVING: bool;
133    /// is transformation axis direction preserving?
134    const IS_AXIS_DIRECTION_PRESERVING: bool;
135    /// is transformation direction vector preserving?
136    const IS_DIRECTION_VECTOR_PRESERVING: bool;
137    /// is transformation shape preserving?
138    const IS_SHAPE_PRESERVING: bool;
139    /// is transformation distance preserving?
140    const IS_DISTANCE_PRESERVING: bool;
141    /// is transformation angle preserving?
142    const IS_PARALLEL_LINE_PRESERVING: bool;
143
144    /// identity parameters
145    fn identity_params() -> S::Vector<PARAMS>;
146
147    // Manifold / Lie Group concepts
148
149    /// group adjoint
150    fn adj(params: &S::Vector<PARAMS>) -> S::Matrix<DOF, DOF>;
151
152    /// algebra adjoint
153    fn ad(tangent: S::Vector<DOF>) -> S::Matrix<DOF, DOF>;
154
155    /// exponential map
156    fn exp(omega: S::Vector<DOF>) -> S::Vector<PARAMS>;
157
158    /// logarithmic map
159    fn log(params: &S::Vector<PARAMS>) -> S::Vector<DOF>;
160
161    /// hat operator
162    fn hat(omega: S::Vector<DOF>) -> S::Matrix<AMBIENT, AMBIENT>;
163
164    /// vee operator
165    fn vee(hat: S::Matrix<AMBIENT, AMBIENT>) -> S::Vector<DOF>;
166
167    // group operations
168
169    /// group multiplication
170    fn group_mul(params1: &S::Vector<PARAMS>, params2: S::Vector<PARAMS>) -> S::Vector<PARAMS>;
171
172    /// group inverse
173    fn inverse(params: &S::Vector<PARAMS>) -> S::Vector<PARAMS>;
174
175    // Group actions
176
177    /// group action on a point
178    fn transform(params: &S::Vector<PARAMS>, point: S::Vector<POINT>) -> S::Vector<POINT>;
179
180    /// convert minimal manifold representation to ambient manifold representation
181    fn to_ambient(point: S::Vector<POINT>) -> S::Vector<AMBIENT>;
182
183    /// return compact matrix representation
184    fn compact(params: &S::Vector<PARAMS>) -> S::Matrix<POINT, AMBIENT>;
185
186    /// return square matrix representation
187    fn matrix(params: &S::Vector<PARAMS>) -> S::Matrix<AMBIENT, AMBIENT>;
188}
189
190/// Lie Group implementation trait for real scalar, f64
191pub trait IsRealLieGroupImpl<
192    S: IsRealScalar<BATCH>,
193    const DOF: usize,
194    const PARAMS: usize,
195    const POINT: usize,
196    const AMBIENT: usize,
197    const BATCH: usize,
198>: IsLieGroupImpl<S, DOF, PARAMS, POINT, AMBIENT, BATCH, 0, 0>
199{
200    /// derivative of group multiplication with respect to the first argument
201    fn da_a_mul_b(a: S::Vector<PARAMS>, b: S::Vector<PARAMS>) -> S::Matrix<PARAMS, PARAMS>;
202
203    /// derivative of group multiplication with respect to the second argument
204    fn db_a_mul_b(a: S::Vector<PARAMS>, b: S::Vector<PARAMS>) -> S::Matrix<PARAMS, PARAMS>;
205
206    /// derivative of exponential map
207    fn dx_exp(tangent: S::Vector<DOF>) -> S::Matrix<PARAMS, DOF>;
208
209    /// derivative of exponential map at the identity
210    fn dx_exp_x_at_0() -> S::Matrix<PARAMS, DOF>;
211
212    /// derivative of exponential map times a point at the identity
213    fn dx_exp_x_times_point_at_0(point: S::Vector<POINT>) -> S::Matrix<POINT, DOF>;
214
215    /// derivative of matrix representation with respect to the internal parameters
216    ///
217    /// precondition: column index in [0, AMBIENT-1]
218    fn dparams_matrix(params: &S::Vector<PARAMS>, col_idx: usize) -> S::Matrix<POINT, PARAMS>;
219
220    /// are there multiple shortest paths to the identity?
221    fn has_shortest_path_ambiguity(params: &S::Vector<PARAMS>) -> S::Mask;
222
223    /// **Left Jacobian** Jₗ(u) ∈ ℝ^{DOF×DOF}.
224    ///
225    /// Satisfies `exp(u) · exp(v) ≃ exp(Jₗ(u) · v)` to first order, for small `u`s.
226    ///
227    /// Note, for 3d rotations aka SO(3), the left Jacobian happens to be identical to the
228    /// V-matrix (used to calculate the exponential map of SE(3)). This is *not* true in general,
229    /// though. The left Jacobian is a DOFxDOF matrix and the V-matrix is a POINTxPOINT matrix.
230    fn left_jacobian(tangent: S::Vector<DOF>) -> S::Matrix<DOF, DOF>;
231
232    /// Inverse of the left Jacobian, i.e. Jₗ⁻¹(u).
233    fn inv_left_jacobian(tangent: S::Vector<DOF>) -> S::Matrix<DOF, DOF>;
234}
235
236/// Lie Factor Group
237///
238/// Can be a factor of a semi-direct product group
239pub trait IsLieFactorGroupImpl<
240    S: IsScalar<BATCH, DM, DN>,
241    const DOF: usize,
242    const PARAMS: usize,
243    const POINT: usize,
244    const BATCH: usize,
245    const DM: usize,
246    const DN: usize,
247>: IsLieGroupImpl<S, DOF, PARAMS, POINT, POINT, BATCH, DM, DN>
248{
249    /// Generic scalar, real scalar, and dual scalar
250    type GenFactorG<S2: IsScalar<BATCH, M, N>, const M: usize, const N: usize>: IsLieFactorGroupImpl<S2, DOF, PARAMS, POINT, BATCH, M, N>;
251    /// Real scalar
252    type RealFactorG: IsLieFactorGroupImpl<S::RealScalar, DOF, PARAMS, POINT, BATCH, 0, 0>;
253    /// DualScalar scalar - for automatic differentiation
254    type DualFactorG<const M: usize, const N: usize>: IsLieFactorGroupImpl<S::DualScalar<M, N>, DOF, PARAMS, POINT, BATCH, M, N>;
255
256    /// V matrix - used by semi-direct product exponential
257    fn mat_v(tangent: S::Vector<DOF>) -> S::Matrix<POINT, POINT>;
258
259    /// V matrix inverse - used by semi-direct product logarithm
260    fn mat_v_inverse(tangent: S::Vector<DOF>) -> S::Matrix<POINT, POINT>;
261
262    /// group adjoint of translation
263    fn adj_of_translation(
264        params: &S::Vector<PARAMS>,
265        point: S::Vector<POINT>,
266    ) -> S::Matrix<POINT, DOF>;
267
268    /// algebra adjoint of translation
269    fn ad_of_translation(point: S::Vector<POINT>) -> S::Matrix<POINT, DOF>;
270}
271
272/// Lie Factor Group implementation trait for real scalar, f64
273pub trait IsRealLieFactorGroupImpl<
274    S: IsRealScalar<BATCH>,
275    const DOF: usize,
276    const PARAMS: usize,
277    const POINT: usize,
278    const BATCH: usize,
279>:
280    IsLieGroupImpl<S, DOF, PARAMS, POINT, POINT, BATCH, 0, 0>
281    + IsLieFactorGroupImpl<S, DOF, PARAMS, POINT, BATCH, 0, 0>
282    + IsRealLieGroupImpl<S, DOF, PARAMS, POINT, POINT, BATCH>
283{
284    /// derivative of V matrix
285    fn dx_mat_v(tangent: S::Vector<DOF>) -> [S::Matrix<POINT, POINT>; DOF];
286
287    /// derivative of V matrix inverse
288    fn dx_mat_v_inverse(tangent: S::Vector<DOF>) -> [S::Matrix<POINT, POINT>; DOF];
289
290    /// derivative of group transformation times a point with respect to the group parameters
291    fn dparams_matrix_times_point(
292        params: &S::Vector<PARAMS>,
293        point: S::Vector<POINT>,
294    ) -> S::Matrix<POINT, PARAMS>;
295
296    /// Left Jacobian of a translation in the factor group.
297    ///
298    /// This coupling block, called Q matrix by Barfoot (Eq. 7.86) et al., is part of the
299    /// left Jacobian of an affine Lie group:
300    ///
301    /// ```ascii
302    ///                   / J_left(α)   O \
303    /// J_left(α, ν)  =  |                 |
304    ///                   \  Q(α, ν)    V /
305    /// ```
306    ///
307    /// Here, `J_left(α)` is the left Jacobian of the factor group, `V` is the V-matrix.
308    /// `Q(α, ν)` is what this function returns.
309    fn left_jacobian_of_translation(
310        alpha: S::Vector<DOF>,
311        upsilon: S::Vector<POINT>,
312    ) -> S::Matrix<POINT, DOF>;
313}
314
315/// Lie Group trait
316pub trait IsLieGroup<
317    S: IsScalar<BATCH, DM, DN>,
318    const DOF: usize,
319    const PARAMS: usize,
320    const POINT: usize,
321    const AMBIENT: usize,
322    const BATCH: usize,
323    const DM: usize,
324    const DN: usize,
325>: IsTangent<S, DOF, BATCH, DM, DN> + HasParams<S, PARAMS, BATCH, DM, DN>
326{
327    /// This is the actual Lie Group implementation
328    type G: IsLieGroupImpl<S, DOF, PARAMS, POINT, AMBIENT, BATCH, DM, DN>;
329    /// Lie Group implementation with generic scalar, real scalar, and dual scalar
330    type GenG<S2: IsScalar<BATCH, DM, DN>>: IsLieGroupImpl<S2, DOF, PARAMS, POINT, AMBIENT, BATCH, DM, DN>;
331    /// Lie Group implementation- with real scalar
332    type RealG: IsLieGroupImpl<S::RealScalar, DOF, PARAMS, POINT, AMBIENT, BATCH, 0, 0>;
333    /// Lie Group implementation with dual scalar - for automatic differentiation
334    type DualG<const M: usize, const N: usize>: IsLieGroupImpl<S::DualScalar<M, N>, DOF, PARAMS, POINT, AMBIENT, BATCH, M, N>;
335
336    /// degree of freedom
337    const DOF: usize;
338    /// number of parameters
339    const PARAMS: usize;
340    /// point dimension
341    const POINT: usize;
342    /// ambient dimension
343    const AMBIENT: usize;
344
345    /// Get the Lie Group
346    type GenGroup<S2: IsScalar<BATCH, DM, DN>, G2: IsLieGroupImpl<S2, DOF, PARAMS, POINT, AMBIENT, BATCH, DM, DN>>: IsLieGroup<
347        S2,
348        DOF,
349        PARAMS,
350        POINT,
351        AMBIENT,
352        BATCH
353        ,DM,DN
354    >;
355}
356
357/// Affine Lie Group trait.
358pub trait IsAffineGroup<
359    S: IsScalar<BATCH, DM, DN>,
360    const DOF: usize,
361    const PARAMS: usize,
362    const POINT: usize,
363    const AMBIENT: usize,
364    const SDOF: usize,
365    const SPARAMS: usize,
366    const BATCH: usize,
367    const DM: usize,
368    const DN: usize,
369    FactorGroup: IsLieGroup<S, SDOF, SPARAMS, POINT, POINT, BATCH, DM, DN>,
370>: IsLieGroup<S, DOF, PARAMS, POINT, AMBIENT, BATCH, DM, DN>
371{
372    /// This is the actual Lie Factor Group implementation
373    type Impl;
374
375    /// Create from translation and factor group element
376    fn from_factor_and_translation(factor: FactorGroup, translation: S::Vector<POINT>) -> Self;
377
378    /// set translation
379    fn set_translation(&mut self, translation: S::Vector<POINT>);
380    /// get translation
381    fn translation(&self) -> S::Vector<POINT>;
382
383    /// set factor group element
384    fn set_factor(&mut self, factor: FactorGroup);
385
386    /// get factor group element
387    fn factor(&self) -> FactorGroup;
388}
389
390/// slice is empty
391#[derive(Debug)]
392pub struct EmptySliceError;
393
394/// Lie Group trait
395pub trait HasAverage<
396    S: IsSingleScalar<DM, DN>,
397    const DOF: usize,
398    const PARAMS: usize,
399    const POINT: usize,
400    const AMBIENT: usize,
401    const DM: usize,
402    const DN: usize,
403>: IsLieGroup<S, DOF, PARAMS, POINT, AMBIENT, 1, DM, DN> + core::marker::Sized
404{
405    /// Lie group average
406    fn average(parent_from_body_transforms: &[Self]) -> Result<Self, EmptySliceError>;
407}
sophus_lie/lib.rs

sophus_lie/
lib.rs
