apex-solver 1.2.1

High-performance nonlinear least squares optimization with Lie group support for SLAM and bundle adjustment
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110

//! Prior factor for unary constraints on variables.

use super::Factor;
use nalgebra::{DMatrix, DVector};

/// Prior factor (unary constraint) on a single variable.
///
/// Represents a direct measurement or prior belief about a variable's value. This is used
/// to anchor variables to known values or to incorporate prior knowledge into the optimization.
///
/// # Mathematical Formulation
///
/// The residual is simply the difference between the current value and the prior:
///
/// ```text
/// r = x - x_prior
/// ```
///
/// The Jacobian is the identity matrix: `J = I`.
///
/// # Use Cases
///
/// - **Anchoring**: Fix the first pose in SLAM to prevent drift
/// - **GPS measurements**: Constrain a pose to a known global position
/// - **Prior knowledge**: Incorporate measurements from other sensors
/// - **Regularization**: Prevent variables from drifting too far from initial values
///
/// # Example
///
/// ```
/// use apex_solver::factors::{Factor, PriorFactor};
/// use nalgebra::DVector;
///
/// // Prior: first pose should be at origin
/// let prior = PriorFactor {
///     data: DVector::from_vec(vec![0.0, 0.0, 0.0]),
/// };
///
/// // Current estimate (slightly off)
/// let current_pose = DVector::from_vec(vec![0.1, 0.05, 0.02]);
///
/// // Compute residual (shows deviation from prior)
/// let (residual, jacobian) = prior.linearize(&[current_pose], true);
/// ```
///
/// # Implementation Note
///
/// This is a simple "Euclidean" prior that works for any vector space. For manifold
/// variables (SE2, SE3, etc.), consider using manifold-aware priors that respect the
/// geometry (not yet implemented).
#[derive(Debug, Clone)]
pub struct PriorFactor {
    /// The prior value (measurement or known value)
    pub data: DVector<f64>,
}

impl Factor for PriorFactor {
    /// Compute residual and Jacobian for prior factor.
    ///
    /// # Arguments
    ///
    /// * `params` - Single variable value
    /// * `compute_jacobian` - Whether to compute the Jacobian matrix
    ///
    /// # Returns
    ///
    /// - Residual: N×1 vector `(x_current - x_prior)`
    /// - Jacobian: N×N identity matrix
    ///
    /// # Example
    ///
    /// ```
    /// use apex_solver::factors::{Factor, PriorFactor};
    /// use nalgebra::DVector;
    ///
    /// let prior = PriorFactor {
    ///     data: DVector::from_vec(vec![1.0, 2.0]),
    /// };
    ///
    /// let current = DVector::from_vec(vec![1.5, 2.3]);
    /// let (residual, jacobian) = prior.linearize(&[current], true);
    ///
    /// // Residual shows difference
    /// assert!((residual[0] - 0.5).abs() < 1e-10);
    /// assert!((residual[1] - 0.3).abs() < 1e-10);
    ///
    /// // Jacobian is identity
    /// if let Some(jac) = jacobian {
    ///     assert_eq!(jac[(0, 0)], 1.0);
    ///     assert_eq!(jac[(1, 1)], 1.0);
    /// }
    /// ```
    fn linearize(
        &self,
        params: &[DVector<f64>],
        compute_jacobian: bool,
    ) -> (DVector<f64>, Option<DMatrix<f64>>) {
        let residual = &params[0] - &self.data;
        let jacobian = if compute_jacobian {
            Some(DMatrix::<f64>::identity(residual.nrows(), residual.nrows()))
        } else {
            None
        };
        (residual, jacobian)
    }

    fn get_dimension(&self) -> usize {
        self.data.len()
    }
}