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

apex_manifolds/
lib.rs
