apex_manifolds/
lib.rs

1//! Manifold representations for optimization on non-Euclidean spaces.
2//!
3//! This module provides manifold representations commonly used in computer vision and robotics:
4//! - **SE(3)**: Special Euclidean group (rigid body transformations)
5//! - **SO(3)**: Special Orthogonal group (rotations)
6//! - **Sim(3)**: Similarity transformations (rotation + translation + scale)
7//! - **SGal(3)**: Special Galilean group (rotation + translation + velocity + time)
8//! - **SE_2(3)**: Extended Special Euclidean group (rotation + translation + velocity)
9//! - **SE(2)**: Rigid transformations in 2D
10//! - **SO(2)**: Rotations in 2D
11//!
12//! ```text
13//! Lie group M,° | size   | dim | X ∈ M                   | Constraint      | T_E M             | T_X M                 | Exp(T)             | Comp. | Action
14//! ------------- | ------ | --- | ----------------------- | --------------- | ----------------- | --------------------- | ------------------ | ----- | ------
15//! n-D vector    | Rⁿ,+   | n   | n   | v ∈ Rⁿ            | |v-v|=0         | v ∈ Rⁿ            | v ∈ Rⁿ                | v = exp(v)         | v₁+v₂ | v + x
16//! Circle        | S¹,.   | 2   | 1   | z ∈ C             | z*z = 1         | iθ ∈ iR           | θ ∈ R                 | z = exp(iθ)        | z₁z₂  | zx
17//! `Rotation      | SO(2),.| 4   | 1   | R                 | RᵀR = I         | [θ]x ∈ so(2)      | [θ] ∈ R²              | R = exp([θ]x)      | R₁R₂  | Rx`
18//! `Rigid motion  | SE(2),.| 9   | 3   | M = [R t; 0 1]    | RᵀR = I         | [v̂] ∈ se(2)       | [v̂] ∈ R³              | Exp([v̂])           | M₁M₂  | Rx+t`
19//! 3-sphere      | S³,.   | 4   | 3   | q ∈ H             | q*q = 1         | θ/2 ∈ Hp          | θ ∈ R³                | q = exp(uθ/2)      | q₁q₂  | qxq*
20//! `Rotation      | SO(3),.| 9   | 3   | R                 | RᵀR = I         | [θ]x ∈ so(3)      | [θ] ∈ R³              | R = exp([θ]x)      | R₁R₂  | Rx`
21//! `Rigid motion  | SE(3),.| 16  | 6   | M = [R t; 0 1]    | RᵀR = I         | [v̂] ∈ se(3)       | [v̂] ∈ R⁶              | Exp([v̂])           | M₁M₂  | Rx+t`
22//! `Similarity    | Sim(3),.| 16 | 7   | M = [sR t; 0 1]   | RᵀR=I, s>0     | [v̂] ∈ sim(3)      | [ρ,θ,σ] ∈ R⁷          | Exp([v̂])           | M₁M₂  | sRx+t`
23//! `Galilean      | SGal(3),.| 25| 10  | (R,t,v,s)         | RᵀR = I         | [v̂] ∈ sgal(3)     | [ρ,ν,θ,s] ∈ R¹⁰       | Exp([v̂])           | M₁M₂  | Rx+t+sv`
24//! `Extended pose | SE_2(3),.| 25| 9   | (R,t,v)           | RᵀR = I         | [v̂] ∈ se_2_3      | [ρ,θ,ν] ∈ R⁹          | Exp([v̂])           | M₁M₂  | Rx+t`
25//! ```
26//!
27//! The design is inspired by the [manif](https://github.com/artivis/manif) C++ library
28//! and provides:
29//! - Analytic Jacobian computations for all operations
30//! - Right and left perturbation models
31//! - Composition and inverse operations
32//! - Exponential and logarithmic maps
33//! - Tangent space operations
34//!
35//! # Mathematical Background
36//!
37//! This module implements Lie group theory for robotics applications. Each manifold
38//! represents a Lie group with its associated tangent space (Lie algebra).
39//! Operations are differentiated with respect to perturbations on the local tangent space.
40//!
41
42use nalgebra::{Matrix3, Vector3};
43use std::ops::{Mul, Neg};
44use std::{
45    error, fmt,
46    fmt::{Display, Formatter},
47};
48
49/// Threshold for switching between exact formulas and Taylor approximations
50/// in small-angle computations.
51///
52/// `f64::EPSILON` (~2.2e-16) is too tight for small-angle detection — angles of ~1e-8 radians
53/// are well within Taylor approximation validity but would fall through to the exact formula
54/// path, where division by near-zero values causes numerical issues.
55///
56/// `1e-10` is chosen because:
57/// - Taylor expansions for sin(θ)/θ, (1-cos(θ))/θ² are accurate to ~1e-20 at this scale
58/// - Avoids catastrophic cancellation in exact formulas near zero
59/// - Consistent with production SLAM libraries (Sophus, GTSAM)
60///
61/// **Note:** This threshold is compared against `θ²` (not `θ`), so the effective angle
62/// threshold is `√(1e-10) ≈ 1e-5` radians (~0.00057°).
63pub const SMALL_ANGLE_THRESHOLD: f64 = 1e-10;
64
65pub mod rn;
66pub mod se2;
67pub mod se23;
68pub mod se3;
69pub mod sgal3;
70pub mod sim3;
71pub mod so2;
72pub mod so3;
73
74/// Errors that can occur during manifold operations.
75#[derive(Debug, Clone, PartialEq)]
76pub enum ManifoldError {
77    /// Invalid tangent vector dimension
78    InvalidTangentDimension { expected: usize, actual: usize },
79    /// Numerical instability in computation
80    NumericalInstability(String),
81    /// Invalid manifold element
82    InvalidElement(String),
83    /// Dimension validation failed during conversion
84    DimensionMismatch { expected: usize, actual: usize },
85    /// NaN or Inf detected in manifold element
86    InvalidNumber,
87    /// Normalization failed for manifold element
88    NormalizationFailed(String),
89}
90
91#[derive(Debug, Clone, PartialEq)]
92pub enum ManifoldType {
93    RN,
94    SE2,
95    SE3,
96    SE23,
97    SGal3,
98    Sim3,
99    SO2,
100    SO3,
101}
102
103impl Display for ManifoldError {
104    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
105        match self {
106            ManifoldError::InvalidTangentDimension { expected, actual } => {
107                write!(
108                    f,
109                    "Invalid tangent dimension: expected {expected}, got {actual}"
110                )
111            }
112            ManifoldError::NumericalInstability(msg) => {
113                write!(f, "Numerical instability: {msg}")
114            }
115            ManifoldError::InvalidElement(msg) => {
116                write!(f, "Invalid manifold element: {msg}")
117            }
118            ManifoldError::DimensionMismatch { expected, actual } => {
119                write!(f, "Dimension mismatch: expected {expected}, got {actual}")
120            }
121            ManifoldError::InvalidNumber => {
122                write!(f, "Invalid number: NaN or Inf detected")
123            }
124            ManifoldError::NormalizationFailed(msg) => {
125                write!(f, "Normalization failed: {msg}")
126            }
127        }
128    }
129}
130
131impl error::Error for ManifoldError {}
132
133/// Result type for manifold operations.
134pub type ManifoldResult<T> = Result<T, ManifoldError>;
135
136/// Core trait for Lie group operations.
137///
138/// Provides group operations, exponential/logarithmic maps, plus/minus with Jacobians,
139/// and adjoint representations.
140///
141/// # Associated Types
142///
143/// - `TangentVector`: Tangent space (Lie algebra) vector type
144/// - `JacobianMatrix`: Jacobian matrix type for this group
145/// - `LieAlgebra`: Matrix representation of the Lie algebra
146pub trait LieGroup: Clone + PartialEq {
147    /// Human-readable name for serialization and logging.
148    const NAME: &'static str;
149
150    /// The tangent space vector type
151    type TangentVector: Tangent<Self>;
152
153    /// The Jacobian matrix type
154    type JacobianMatrix: Clone
155        + PartialEq
156        + Neg<Output = Self::JacobianMatrix>
157        + Mul<Output = Self::JacobianMatrix>
158        + std::ops::Index<(usize, usize), Output = f64>;
159
160    /// Associated Lie algebra type
161    type LieAlgebra: Clone + PartialEq;
162
163    // Core group operations
164
165    /// Compute the inverse of this manifold element.
166    ///
167    /// For a group element g, returns g⁻¹ such that g ∘ g⁻¹ = e.
168    ///
169    /// # Arguments
170    /// * `jacobian` - Optional mutable reference to store the Jacobian ∂(g⁻¹)/∂g
171    fn inverse(&self, jacobian: Option<&mut Self::JacobianMatrix>) -> Self;
172
173    /// Compose this element with another (group multiplication).
174    ///
175    /// Computes g₁ ∘ g₂ where ∘ is the group operation.
176    ///
177    /// # Arguments
178    /// * `other` - The right operand for composition
179    /// * `jacobian_self` - Optional Jacobian ∂(g₁ ∘ g₂)/∂g₁
180    /// * `jacobian_other` - Optional Jacobian ∂(g₁ ∘ g₂)/∂g₂
181    fn compose(
182        &self,
183        other: &Self,
184        jacobian_self: Option<&mut Self::JacobianMatrix>,
185        jacobian_other: Option<&mut Self::JacobianMatrix>,
186    ) -> Self;
187
188    /// Logarithmic map from manifold to tangent space.
189    ///
190    /// Maps a group element g ∈ G to its tangent vector log(g)^∨ ∈ 𝔤.
191    ///
192    /// # Arguments
193    /// * `jacobian` - Optional Jacobian ∂log(g)^∨/∂g
194    fn log(&self, jacobian: Option<&mut Self::JacobianMatrix>) -> Self::TangentVector;
195
196    /// Vee operator: log(g)^∨.
197    ///
198    /// Maps a group element g ∈ G to its tangent vector log(g)^∨ ∈ 𝔤.
199    ///
200    /// # Arguments
201    /// * `jacobian` - Optional Jacobian ∂log(g)^∨/∂g
202    fn vee(&self) -> Self::TangentVector;
203
204    /// Act on a vector v: g ⊙ v.
205    ///
206    /// Group action on vectors (e.g., rotation for SO(3), transformation for SE(3)).
207    ///
208    /// # Arguments
209    /// * `vector` - Vector to transform
210    /// * `jacobian_self` - Optional Jacobian ∂(g ⊙ v)/∂g
211    /// * `jacobian_vector` - Optional Jacobian ∂(g ⊙ v)/∂v
212    fn act(
213        &self,
214        vector: &Vector3<f64>,
215        jacobian_self: Option<&mut Self::JacobianMatrix>,
216        jacobian_vector: Option<&mut Matrix3<f64>>,
217    ) -> Vector3<f64>;
218
219    // Adjoint operations
220
221    /// Adjoint matrix Ad(g).
222    ///
223    /// The adjoint representation maps the group to linear transformations
224    /// on the Lie algebra: Ad(g) φ = log(g ∘ exp(φ^∧) ∘ g⁻¹)^∨.
225    fn adjoint(&self) -> Self::JacobianMatrix;
226
227    // Utility operations
228
229    /// Generate a random element (useful for testing and initialization).
230    fn random() -> Self;
231
232    /// Get the identity matrix for Jacobians.
233    ///
234    /// Returns the identity matrix in the appropriate dimension for Jacobian computations.
235    /// This is used to initialize Jacobian matrices in optimization algorithms.
236    fn jacobian_identity() -> Self::JacobianMatrix;
237
238    /// Get a zero Jacobian matrix.
239    ///
240    /// Returns a zero matrix in the appropriate dimension for Jacobian computations.
241    /// This is used to initialize Jacobian matrices before optimization computations.
242    fn zero_jacobian() -> Self::JacobianMatrix;
243
244    /// Normalize/project the element to the manifold.
245    ///
246    /// Ensures the element satisfies manifold constraints (e.g., orthogonality for rotations).
247    fn normalize(&mut self);
248
249    /// Check if the element is approximately on the manifold.
250    fn is_valid(&self, tolerance: f64) -> bool;
251
252    /// Check if the element is approximately equal to another element.
253    ///
254    /// # Arguments
255    /// * `other` - The other element to compare with
256    /// * `tolerance` - The tolerance for the comparison
257    fn is_approx(&self, other: &Self, tolerance: f64) -> bool;
258
259    /// Returns the manifold parameters as a contiguous flat slice.
260    ///
261    /// Enables zero-copy faer views: `faer::col::from_slice(g.as_param_slice())`.
262    fn as_param_slice(&self) -> &[f64];
263
264    /// Mutably borrows the raw parameter storage for in-place retraction updates.
265    fn as_param_slice_mut(&mut self) -> &mut [f64];
266
267    /// Constructs from a raw parameter slice (same layout as `as_param_slice`).
268    fn from_param_slice(s: &[f64]) -> Self;
269
270    // Manifold plus/minus operations
271
272    /// Right plus operation: g ⊞ φ = g ∘ exp(φ^∧).
273    ///
274    /// Applies a tangent space perturbation to this manifold element.
275    ///
276    /// # Arguments
277    /// * `tangent` - Tangent vector perturbation
278    /// * `jacobian_self` - Optional Jacobian ∂(g ⊞ φ)/∂g
279    /// * `jacobian_tangent` - Optional Jacobian ∂(g ⊞ φ)/∂φ
280    ///
281    /// # Notes
282    /// # Equation 148:
283    /// J_R⊕θ_R = R(θ)ᵀ
284    /// J_R⊕θ_θ = J_r(θ)
285    fn right_plus(
286        &self,
287        tangent: &Self::TangentVector,
288        jacobian_self: Option<&mut Self::JacobianMatrix>,
289        jacobian_tangent: Option<&mut Self::JacobianMatrix>,
290    ) -> Self {
291        let exp_tangent = tangent.exp(None);
292
293        if let Some(jac_tangent) = jacobian_tangent {
294            *jac_tangent = tangent.right_jacobian();
295        }
296
297        self.compose(&exp_tangent, jacobian_self, None)
298    }
299
300    /// Right minus operation: g₁ ⊟ g₂ = log(g₂⁻¹ ∘ g₁)^∨.
301    ///
302    /// Computes the tangent vector that transforms g₂ to g₁.
303    ///
304    /// # Arguments
305    /// * `other` - The reference element g₂
306    /// * `jacobian_self` - Optional Jacobian ∂(g₁ ⊟ g₂)/∂g₁
307    /// * `jacobian_other` - Optional Jacobian ∂(g₁ ⊟ g₂)/∂g₂
308    ///
309    /// # Notes
310    /// # Equation 149:
311    /// J_Q⊖R_Q = J_r⁻¹(θ)
312    /// J_Q⊖R_R = -J_l⁻¹(θ)
313    fn right_minus(
314        &self,
315        other: &Self,
316        jacobian_self: Option<&mut Self::JacobianMatrix>,
317        jacobian_other: Option<&mut Self::JacobianMatrix>,
318    ) -> Self::TangentVector {
319        let other_inverse = other.inverse(None);
320        let result_group = other_inverse.compose(self, None, None);
321        let result = result_group.log(None);
322
323        if let Some(jac_self) = jacobian_self {
324            *jac_self = result.right_jacobian_inv();
325        }
326
327        if let Some(jac_other) = jacobian_other {
328            *jac_other = -result.left_jacobian_inv();
329        }
330
331        result
332    }
333
334    /// Left plus operation: φ ⊞ g = exp(φ^∧) ∘ g.
335    ///
336    /// # Arguments
337    /// * `tangent` - Tangent vector perturbation
338    /// * `jacobian_tangent` - Optional Jacobian ∂(φ ⊞ g)/∂φ
339    /// * `jacobian_self` - Optional Jacobian ∂(φ ⊞ g)/∂g
340    fn left_plus(
341        &self,
342        tangent: &Self::TangentVector,
343        jacobian_tangent: Option<&mut Self::JacobianMatrix>,
344        jacobian_self: Option<&mut Self::JacobianMatrix>,
345    ) -> Self {
346        let exp_tangent = tangent.exp(None);
347        let result = exp_tangent.compose(self, None, None);
348
349        if let Some(jac_self) = jacobian_self {
350            *jac_self = self.adjoint();
351        }
352
353        if let Some(jac_tangent) = jacobian_tangent {
354            *jac_tangent = self.inverse(None).adjoint() * tangent.right_jacobian();
355        }
356
357        result
358    }
359
360    /// Left minus operation: g₁ ⊟ g₂ = log(g₁ ∘ g₂⁻¹)^∨.
361    ///
362    /// # Arguments
363    /// * `other` - The reference element g₂
364    /// * `jacobian_self` - Optional Jacobian ∂(g₁ ⊟ g₂)/∂g₁
365    /// * `jacobian_other` - Optional Jacobian ∂(g₁ ⊟ g₂)/∂g₂
366    fn left_minus(
367        &self,
368        other: &Self,
369        jacobian_self: Option<&mut Self::JacobianMatrix>,
370        jacobian_other: Option<&mut Self::JacobianMatrix>,
371    ) -> Self::TangentVector {
372        let other_inverse = other.inverse(None);
373        let result_group = self.compose(&other_inverse, None, None);
374        let result = result_group.log(None);
375
376        if let Some(jac_self) = jacobian_self {
377            *jac_self = result.right_jacobian_inv() * other.adjoint();
378        }
379
380        if let Some(jac_other) = jacobian_other {
381            *jac_other = -(result.right_jacobian_inv() * other.adjoint());
382        }
383
384        result
385    }
386
387    // Convenience methods (use right operations by default)
388
389    /// Convenience method for right_plus. Equivalent to g ⊞ φ.
390    fn plus(
391        &self,
392        tangent: &Self::TangentVector,
393        jacobian_self: Option<&mut Self::JacobianMatrix>,
394        jacobian_tangent: Option<&mut Self::JacobianMatrix>,
395    ) -> Self {
396        self.right_plus(tangent, jacobian_self, jacobian_tangent)
397    }
398
399    /// Convenience method for right_minus. Equivalent to g₁ ⊟ g₂.
400    fn minus(
401        &self,
402        other: &Self,
403        jacobian_self: Option<&mut Self::JacobianMatrix>,
404        jacobian_other: Option<&mut Self::JacobianMatrix>,
405    ) -> Self::TangentVector {
406        self.right_minus(other, jacobian_self, jacobian_other)
407    }
408
409    // Additional operations
410
411    /// Compute g₁⁻¹ ∘ g₂ (relative transformation).
412    ///
413    /// # Arguments
414    /// * `other` - The target element g₂
415    /// * `jacobian_self` - Optional Jacobian with respect to g₁
416    /// * `jacobian_other` - Optional Jacobian with respect to g₂
417    fn between(
418        &self,
419        other: &Self,
420        jacobian_self: Option<&mut Self::JacobianMatrix>,
421        jacobian_other: Option<&mut Self::JacobianMatrix>,
422    ) -> Self {
423        let self_inverse = self.inverse(None);
424        let result = self_inverse.compose(other, None, None);
425
426        if let Some(jac_self) = jacobian_self {
427            *jac_self = -result.inverse(None).adjoint();
428        }
429
430        if let Some(jac_other) = jacobian_other {
431            *jac_other = Self::jacobian_identity();
432        }
433
434        result
435    }
436
437    /// Get the dimension of the tangent space for this manifold element.
438    ///
439    /// For most manifolds, this returns the compile-time constant from the TangentVector type.
440    /// For dynamically-sized manifolds like Rⁿ, this method should be overridden to return
441    /// the actual runtime dimension.
442    ///
443    /// # Returns
444    /// The dimension of the tangent space (degrees of freedom)
445    ///
446    /// # Default Implementation
447    /// Returns `Self::TangentVector::DIM` which works for fixed-size manifolds
448    /// (SE2=3, SE3=6, SO2=1, SO3=3).
449    fn tangent_dim(&self) -> usize {
450        Self::TangentVector::DIM
451    }
452}
453
454/// Trait for Lie algebra operations.
455///
456/// This trait provides operations for vectors in the Lie algebra of a Lie group,
457/// including vector space operations, adjoint actions, and conversions to matrix form.
458///
459/// # Type Parameters
460///
461/// - `G`: The associated Lie group type
462pub trait Tangent<Group: LieGroup>: Clone + PartialEq {
463    // Dimension constants
464
465    /// Dimension of the tangent space.
466    ///
467    /// For fixed-size manifolds (SE2, SE3, SO2, SO3), this is the compile-time constant.
468    /// For dynamic-size manifolds (Rn), this is `0` as a sentinel value — use the
469    /// `is_dynamic()` method to check, and the `LieGroup::tangent_dim()` instance method
470    /// to get the actual runtime dimension.
471    const DIM: usize;
472
473    /// Whether this tangent type has dynamic (runtime-determined) dimension.
474    ///
475    /// Returns `true` for `RnTangent` where `DIM == 0` is used as a sentinel.
476    /// Returns `false` for all fixed-size tangent types (SE2, SE3, SO2, SO3).
477    fn is_dynamic() -> bool {
478        Self::DIM == 0
479    }
480
481    // Exponential map and Jacobians
482
483    /// Exponential map to Lie group: exp(φ^∧).
484    ///
485    /// # Arguments
486    /// * `jacobian` - Optional Jacobian ∂exp(φ^∧)/∂φ
487    fn exp(&self, jacobian: Option<&mut Group::JacobianMatrix>) -> Group;
488
489    /// Right Jacobian Jr.
490    ///
491    /// Matrix Jr such that for small δφ:
492    /// exp((φ + δφ)^∧) ≈ exp(φ^∧) ∘ exp((Jr δφ)^∧)
493    fn right_jacobian(&self) -> Group::JacobianMatrix;
494
495    /// Left Jacobian Jl.
496    ///
497    /// Matrix Jl such that for small δφ:
498    /// exp((φ + δφ)^∧) ≈ exp((Jl δφ)^∧) ∘ exp(φ^∧)
499    fn left_jacobian(&self) -> Group::JacobianMatrix;
500
501    /// Inverse of right Jacobian Jr⁻¹.
502    fn right_jacobian_inv(&self) -> Group::JacobianMatrix;
503
504    /// Inverse of left Jacobian Jl⁻¹.
505    fn left_jacobian_inv(&self) -> Group::JacobianMatrix;
506
507    // Matrix representations
508
509    /// Hat operator: φ^∧ (vector to matrix).
510    ///
511    /// Maps the tangent vector to its matrix representation in the Lie algebra.
512    /// For SO(3): 3×1 vector → 3×3 skew-symmetric matrix
513    /// For SE(3): 6×1 vector → 4×4 transformation matrix
514    fn hat(&self) -> Group::LieAlgebra;
515
516    /// Small adjugate operator: adj(φ) = φ^∧.
517    ///
518    /// Maps the tangent vector to its matrix representation in the Lie algebra.
519    /// For SO(3): 3×1 vector → 3×3 skew-symmetric matrix
520    /// For SE(3): 6×1 vector → 4×4 transformation matrix
521    fn small_adj(&self) -> Group::JacobianMatrix;
522
523    /// Lie bracket: [φ, ψ] = φ ∘ ψ - ψ ∘ φ.
524    ///
525    /// Computes the Lie bracket of two tangent vectors in the Lie algebra.
526    /// For SO(3): 3×1 vector → 3×1 vector
527    /// For SE(3): 6×1 vector → 6×1 vector
528    fn lie_bracket(&self, other: &Self) -> Group::TangentVector;
529
530    /// Check if the tangent vector is approximately equal to another tangent vector.
531    ///
532    /// # Arguments
533    /// * `other` - The other tangent vector to compare with
534    /// * `tolerance` - The tolerance for the comparison
535    fn is_approx(&self, other: &Self, tolerance: f64) -> bool;
536
537    /// Get the i-th generator of the Lie algebra.
538    fn generator(&self, i: usize) -> Group::LieAlgebra;
539
540    // Utility functions
541
542    /// Zero tangent vector.
543    fn zero() -> Group::TangentVector;
544
545    /// Random tangent vector (useful for testing).
546    fn random() -> Group::TangentVector;
547
548    /// Check if the tangent vector is approximately zero.
549    fn is_zero(&self, tolerance: f64) -> bool;
550
551    /// Normalize the tangent vector to unit norm.
552    fn normalize(&mut self);
553
554    /// Return a unit tangent vector in the same direction.
555    fn normalized(&self) -> Group::TangentVector;
556
557    /// Borrows the raw tangent data as a flat slice. Zero allocation.
558    fn as_slice(&self) -> &[f64];
559
560    /// Constructs from a raw slice (same layout as `as_slice`).
561    fn from_slice(s: &[f64]) -> Self;
562}
563
564/// Trait for Lie groups that support interpolation.
565pub trait Interpolatable: LieGroup {
566    /// Linear interpolation in the manifold.
567    ///
568    /// For parameter t ∈ `[0,1]`: interp(g₁, g₂, 0) = g₁, interp(g₁, g₂, 1) = g₂.
569    ///
570    /// # Arguments
571    /// * `other` - Target element for interpolation
572    /// * `t` - Interpolation parameter in `[0,1]`
573    fn interp(&self, other: &Self, t: f64) -> Self;
574
575    /// Spherical linear interpolation (when applicable).
576    fn slerp(&self, other: &Self, t: f64) -> Self;
577}
578
579#[cfg(test)]
580mod tests {
581    use crate::LieGroup;
582    use crate::Tangent;
583    use crate::so2::{SO2, SO2Tangent};
584    use crate::so3::{SO3, SO3Tangent};
585    use crate::{ManifoldError, ManifoldType};
586    use nalgebra::Matrix1;
587
588    fn make_so2(angle: f64) -> SO2 {
589        SO2::from_angle(angle)
590    }
591
592    fn make_so2_tangent(angle: f64) -> SO2Tangent {
593        SO2Tangent::new(angle)
594    }
595
596    #[test]
597    fn manifold_error_display_invalid_tangent_dimension() {
598        let e = ManifoldError::InvalidTangentDimension {
599            expected: 3,
600            actual: 6,
601        };
602        let s = e.to_string();
603        assert!(s.contains("3"), "got: {s}");
604        assert!(s.contains("6"), "got: {s}");
605    }
606
607    #[test]
608    fn manifold_error_display_numerical_instability() {
609        let e = ManifoldError::NumericalInstability("singularity".to_string());
610        assert!(e.to_string().contains("singularity"));
611    }
612
613    #[test]
614    fn manifold_error_display_invalid_element() {
615        let e = ManifoldError::InvalidElement("bad quaternion".to_string());
616        assert!(e.to_string().contains("bad quaternion"));
617    }
618
619    #[test]
620    fn manifold_error_display_dimension_mismatch() {
621        let e = ManifoldError::DimensionMismatch {
622            expected: 4,
623            actual: 3,
624        };
625        let s = e.to_string();
626        assert!(s.contains("4") && s.contains("3"), "got: {s}");
627    }
628
629    #[test]
630    fn manifold_error_display_invalid_number() {
631        let e = ManifoldError::InvalidNumber;
632        assert!(!e.to_string().is_empty());
633    }
634
635    #[test]
636    fn manifold_error_display_normalization_failed() {
637        let e = ManifoldError::NormalizationFailed("zero vector".to_string());
638        assert!(e.to_string().contains("zero vector"));
639    }
640
641    #[test]
642    fn manifold_error_is_std_error() {
643        let e = ManifoldError::InvalidNumber;
644        let _: &dyn std::error::Error = &e;
645    }
646
647    #[test]
648    fn manifold_type_variants_are_distinct() {
649        let types = [
650            ManifoldType::RN,
651            ManifoldType::SE2,
652            ManifoldType::SE3,
653            ManifoldType::SE23,
654            ManifoldType::SGal3,
655            ManifoldType::Sim3,
656            ManifoldType::SO2,
657            ManifoldType::SO3,
658        ];
659        assert_eq!(types.len(), 8);
660        assert_eq!(ManifoldType::SO3, ManifoldType::SO3);
661        assert_ne!(ManifoldType::SO2, ManifoldType::SO3);
662    }
663
664    #[test]
665    fn default_right_plus_no_jacobians() {
666        let g = make_so2(0.3);
667        let t = make_so2_tangent(0.1);
668        let result = g.right_plus(&t, None, None);
669        assert!(result.is_valid(1e-9));
670    }
671
672    #[test]
673    fn default_right_plus_with_jacobians() {
674        let g = make_so2(0.3);
675        let t = make_so2_tangent(0.1);
676        let mut j_self = Matrix1::zeros();
677        let mut j_tan = Matrix1::zeros();
678        let result = g.right_plus(&t, Some(&mut j_self), Some(&mut j_tan));
679        assert!(result.is_valid(1e-9));
680        assert!(j_tan[0].is_finite());
681    }
682
683    #[test]
684    fn default_right_minus_no_jacobians() {
685        let g1 = make_so2(0.5);
686        let g2 = make_so2(0.2);
687        let _t = g1.right_minus(&g2, None, None);
688    }
689
690    #[test]
691    fn default_right_minus_with_jacobians() {
692        let g1 = make_so2(0.5);
693        let g2 = make_so2(0.2);
694        let mut j_self = Matrix1::zeros();
695        let mut j_other = Matrix1::zeros();
696        let _t = g1.right_minus(&g2, Some(&mut j_self), Some(&mut j_other));
697        assert!(j_self[0].is_finite());
698        assert!(j_other[0].is_finite());
699    }
700
701    #[test]
702    fn default_left_plus_no_jacobians() {
703        let g = make_so2(0.3);
704        let t = make_so2_tangent(0.1);
705        let result = g.left_plus(&t, None, None);
706        assert!(result.is_valid(1e-9));
707    }
708
709    #[test]
710    fn default_left_plus_with_jacobians() {
711        let g = make_so2(0.3);
712        let t = make_so2_tangent(0.1);
713        let mut j_tan = Matrix1::zeros();
714        let mut j_self = Matrix1::zeros();
715        let result = g.left_plus(&t, Some(&mut j_tan), Some(&mut j_self));
716        assert!(result.is_valid(1e-9));
717        assert!(j_tan[0].is_finite());
718        assert!(j_self[0].is_finite());
719    }
720
721    #[test]
722    fn default_left_minus_no_jacobians() {
723        let g1 = make_so2(0.5);
724        let g2 = make_so2(0.2);
725        let _t = g1.left_minus(&g2, None, None);
726    }
727
728    #[test]
729    fn default_left_minus_with_jacobians() {
730        let g1 = make_so2(0.5);
731        let g2 = make_so2(0.2);
732        let mut j_self = Matrix1::zeros();
733        let mut j_other = Matrix1::zeros();
734        let _t = g1.left_minus(&g2, Some(&mut j_self), Some(&mut j_other));
735        assert!(j_self[0].is_finite());
736        assert!(j_other[0].is_finite());
737    }
738
739    #[test]
740    fn default_plus_delegates_to_right_plus() {
741        let g = make_so2(0.3);
742        let t = make_so2_tangent(0.1);
743        let r1 = g.plus(&t, None, None);
744        let r2 = g.right_plus(&t, None, None);
745        assert!(r1.is_approx(&r2, 1e-9));
746    }
747
748    #[test]
749    fn default_minus_delegates_to_right_minus() {
750        let g1 = make_so2(0.5);
751        let g2 = make_so2(0.2);
752        let t1 = g1.minus(&g2, None, None);
753        let t2 = g1.right_minus(&g2, None, None);
754        assert!(t1.is_approx(&t2, 1e-9));
755    }
756
757    #[test]
758    fn default_between_no_jacobians() {
759        let g1 = make_so2(0.3);
760        let g2 = make_so2(0.7);
761        let b = g1.between(&g2, None, None);
762        assert!(b.is_valid(1e-9));
763    }
764
765    #[test]
766    fn default_between_with_jacobians() {
767        let g1 = make_so2(0.3);
768        let g2 = make_so2(0.7);
769        let mut j_self = Matrix1::zeros();
770        let mut j_other = Matrix1::zeros();
771        let b = g1.between(&g2, Some(&mut j_self), Some(&mut j_other));
772        assert!(b.is_valid(1e-9));
773        assert!(j_self[0].is_finite());
774        assert!(j_other[0].is_finite());
775    }
776
777    #[test]
778    fn default_tangent_dim_returns_dof() {
779        let g = make_so2(0.0);
780        assert_eq!(g.tangent_dim(), 1); // SO2 has 1 DOF
781    }
782
783    #[test]
784    fn tangent_is_dynamic_false_for_so2() {
785        assert!(!SO2Tangent::is_dynamic());
786    }
787
788    #[test]
789    fn manifold_error_clone_and_partial_eq() {
790        let e = ManifoldError::InvalidTangentDimension {
791            expected: 1,
792            actual: 2,
793        };
794        let e2 = e.clone();
795        assert_eq!(e, e2);
796
797        let e3 = ManifoldError::NumericalInstability("x".to_string());
798        let e4 = e3.clone();
799        assert_eq!(e3, e4);
800
801        let e5 = ManifoldError::InvalidElement("y".to_string());
802        let e6 = e5.clone();
803        assert_eq!(e5, e6);
804
805        let e7 = ManifoldError::DimensionMismatch {
806            expected: 3,
807            actual: 4,
808        };
809        let e8 = e7.clone();
810        assert_eq!(e7, e8);
811
812        let e9 = ManifoldError::InvalidNumber;
813        let e10 = e9.clone();
814        assert_eq!(e9, e10);
815
816        let e11 = ManifoldError::NormalizationFailed("z".to_string());
817        let e12 = e11.clone();
818        assert_eq!(e11, e12);
819    }
820
821    #[test]
822    fn manifold_type_clone_and_eq() {
823        let all_types = [
824            ManifoldType::RN,
825            ManifoldType::SE2,
826            ManifoldType::SE3,
827            ManifoldType::SE23,
828            ManifoldType::SGal3,
829            ManifoldType::Sim3,
830            ManifoldType::SO2,
831            ManifoldType::SO3,
832        ];
833        for t in &all_types {
834            let t2 = t.clone();
835            assert_eq!(t, &t2);
836        }
837        // Ensure different variants are not equal
838        assert_ne!(ManifoldType::RN, ManifoldType::SE2);
839        assert_ne!(ManifoldType::SE2, ManifoldType::SE3);
840        assert_ne!(ManifoldType::SE3, ManifoldType::SE23);
841        assert_ne!(ManifoldType::SE23, ManifoldType::SGal3);
842        assert_ne!(ManifoldType::SGal3, ManifoldType::Sim3);
843        assert_ne!(ManifoldType::Sim3, ManifoldType::SO2);
844        assert_ne!(ManifoldType::SO2, ManifoldType::SO3);
845    }
846
847    #[test]
848    fn manifold_type_debug() {
849        let s = format!("{:?}", ManifoldType::RN);
850        assert!(!s.is_empty());
851        let s2 = format!("{:?}", ManifoldType::SE23);
852        assert!(!s2.is_empty());
853        let s3 = format!("{:?}", ManifoldType::SGal3);
854        assert!(!s3.is_empty());
855        let s4 = format!("{:?}", ManifoldType::Sim3);
856        assert!(!s4.is_empty());
857    }
858
859    #[test]
860    fn manifold_error_debug() {
861        let e = ManifoldError::InvalidNumber;
862        let s = format!("{e:?}");
863        assert!(!s.is_empty());
864    }
865
866    // Test SO3-based defaults to exercise more code paths
867
868    #[test]
869    fn so3_default_right_plus_with_jacobians() {
870        use crate::LieGroup;
871        let r = SO3::from_euler_angles(0.1, 0.2, 0.3);
872        let t = SO3Tangent::new(nalgebra::Vector3::new(0.05, 0.0, 0.0));
873        let mut j_self = nalgebra::Matrix3::zeros();
874        let mut j_tan = nalgebra::Matrix3::zeros();
875        let result = r.right_plus(&t, Some(&mut j_self), Some(&mut j_tan));
876        assert!(result.is_valid(1e-6));
877        assert!(j_self[(0, 0)].is_finite());
878        assert!(j_tan[(0, 0)].is_finite());
879    }
880
881    #[test]
882    fn so3_default_left_plus_with_jacobians() {
883        use crate::LieGroup;
884        let r = SO3::from_euler_angles(0.1, 0.2, 0.3);
885        let t = SO3Tangent::new(nalgebra::Vector3::new(0.05, 0.0, 0.0));
886        let mut j_tan = nalgebra::Matrix3::zeros();
887        let mut j_self = nalgebra::Matrix3::zeros();
888        let result = r.left_plus(&t, Some(&mut j_tan), Some(&mut j_self));
889        assert!(result.is_valid(1e-6));
890        assert!(j_tan[(0, 0)].is_finite());
891        assert!(j_self[(0, 0)].is_finite());
892    }
893
894    #[test]
895    fn so3_default_right_minus_with_jacobians() {
896        use crate::LieGroup;
897        let r1 = SO3::from_euler_angles(0.3, 0.1, 0.2);
898        let r2 = SO3::from_euler_angles(0.1, 0.0, 0.1);
899        let mut j_self = nalgebra::Matrix3::zeros();
900        let mut j_other = nalgebra::Matrix3::zeros();
901        let _t = r1.right_minus(&r2, Some(&mut j_self), Some(&mut j_other));
902        assert!(j_self[(0, 0)].is_finite());
903        assert!(j_other[(0, 0)].is_finite());
904    }
905
906    #[test]
907    fn so3_default_left_minus_with_jacobians() {
908        use crate::LieGroup;
909        let r1 = SO3::from_euler_angles(0.3, 0.1, 0.2);
910        let r2 = SO3::from_euler_angles(0.1, 0.0, 0.1);
911        let mut j_self = nalgebra::Matrix3::zeros();
912        let mut j_other = nalgebra::Matrix3::zeros();
913        let _t = r1.left_minus(&r2, Some(&mut j_self), Some(&mut j_other));
914        assert!(j_self[(0, 0)].is_finite());
915        assert!(j_other[(0, 0)].is_finite());
916    }
917
918    #[test]
919    fn so3_default_between_with_jacobians() {
920        use crate::LieGroup;
921        let r1 = SO3::from_euler_angles(0.1, 0.2, 0.3);
922        let r2 = SO3::from_euler_angles(0.4, 0.1, 0.2);
923        let mut j_self = nalgebra::Matrix3::zeros();
924        let mut j_other = nalgebra::Matrix3::zeros();
925        let b = r1.between(&r2, Some(&mut j_self), Some(&mut j_other));
926        assert!(b.is_valid(1e-6));
927        assert!(j_self[(0, 0)].is_finite());
928        assert!(j_other[(0, 0)].is_finite());
929    }
930
931    #[test]
932    fn so3_default_tangent_dim() {
933        use crate::LieGroup;
934        let r = SO3::identity();
935        assert_eq!(r.tangent_dim(), 3);
936    }
937}
apex_manifolds/lib.rs

apex_manifolds/
lib.rs
