apex_solver/core/
problem.rs

1use std::{
2    collections::{HashMap, HashSet},
3    fs::File,
4    io::{Error, Write},
5};
6
7use faer::{Mat, sparse::SparseColMat};
8use nalgebra::DVector;
9use rayon::prelude::*;
10use slotmap::{SecondaryMap, SlotMap};
11use tracing::warn;
12
13use crate::{
14    core::CoreResult,
15    core::{
16        FactorKey, VarKey,
17        corrector::Corrector,
18        loss_functions::LossFunction,
19        residual_block::ResidualBlock,
20        variable::{ManifoldVariable, Variable},
21    },
22    factors::Factor,
23    linalg::{JacobianMode, LinearSolver, SparseMode, extract_variable_covariances},
24};
25use apex_manifolds::{LieGroup, ManifoldType, rn, se2, se3, se23, sgal3, sim3, so2, so3};
26
27pub use crate::linearizer::cpu::sparse::SymbolicStructure;
28
29pub struct Problem {
30    pub(crate) total_residual_dimension: usize,
31    pub(crate) jacobian_mode: JacobianMode,
32    pub(crate) variables: SlotMap<VarKey, Box<dyn ManifoldVariable>>,
33    residual_blocks: SlotMap<FactorKey, ResidualBlock>,
34    pub(crate) fixed_variable_indexes: SecondaryMap<VarKey, HashSet<usize>>,
35    pub(crate) variable_bounds: SecondaryMap<VarKey, HashMap<usize, (f64, f64)>>,
36    pub(crate) schur_landmark_keys: HashSet<VarKey>,
37}
38
39impl Default for Problem {
40    fn default() -> Self {
41        Self::new(JacobianMode::Sparse)
42    }
43}
44
45impl Problem {
46    pub fn new(jacobian_mode: JacobianMode) -> Self {
47        Self {
48            total_residual_dimension: 0,
49            jacobian_mode,
50            variables: SlotMap::with_key(),
51            residual_blocks: SlotMap::with_key(),
52            fixed_variable_indexes: SecondaryMap::new(),
53            variable_bounds: SecondaryMap::new(),
54            schur_landmark_keys: HashSet::new(),
55        }
56    }
57
58    /// Mark a variable as a Schur complement landmark (eliminated block).
59    ///
60    /// Call this for every landmark/point variable when using a Schur complement
61    /// solver. Variables not marked here are treated as camera-block variables.
62    pub fn mark_as_schur_landmark(&mut self, key: VarKey) {
63        self.schur_landmark_keys.insert(key);
64    }
65
66    /// Add a variable with a given manifold type and initial parameter vector.
67    ///
68    /// Returns a stable `VarKey` handle for use in `add_residual_block`, `fix_variable`, etc.
69    pub fn add_variable(&mut self, manifold_type: ManifoldType, params: DVector<f64>) -> VarKey {
70        let var = Self::create_variable(&manifold_type, &params);
71        self.variables.insert(var)
72    }
73
74    /// Add a residual block (factor + optional loss) connecting the given variables.
75    ///
76    /// Returns a `FactorKey` that can be used to remove the block later.
77    pub fn add_residual_block(
78        &mut self,
79        variable_keys: &[VarKey],
80        factor: Box<dyn Factor + Send>,
81        loss_func: Option<Box<dyn LossFunction + Send>>,
82    ) -> FactorKey {
83        let new_residual_dimension = factor.residual_dim();
84        let row_start = self.total_residual_dimension;
85        let fk = self.residual_blocks.insert_with_key(|fk| {
86            ResidualBlock::new(fk, row_start, variable_keys, factor, loss_func)
87        });
88        self.total_residual_dimension += new_residual_dimension;
89        fk
90    }
91
92    pub fn remove_residual_block(&mut self, block_id: FactorKey) -> Option<ResidualBlock> {
93        if let Some(block) = self.residual_blocks.remove(block_id) {
94            self.total_residual_dimension -= block.factor.residual_dim();
95            Some(block)
96        } else {
97            None
98        }
99    }
100
101    pub fn fix_variable(&mut self, var_key: VarKey, idx: usize) {
102        if let Some(set) = self.fixed_variable_indexes.get_mut(var_key) {
103            set.insert(idx);
104        } else {
105            let mut s = HashSet::new();
106            s.insert(idx);
107            self.fixed_variable_indexes.insert(var_key, s);
108        }
109    }
110
111    pub fn unfix_variable(&mut self, var_key: VarKey) {
112        self.fixed_variable_indexes.remove(var_key);
113    }
114
115    pub fn set_variable_bounds(
116        &mut self,
117        var_key: VarKey,
118        idx: usize,
119        lower_bound: f64,
120        upper_bound: f64,
121    ) {
122        if lower_bound > upper_bound {
123            warn!("lower bound is larger than upper bound");
124        } else if let Some(map) = self.variable_bounds.get_mut(var_key) {
125            map.insert(idx, (lower_bound, upper_bound));
126        } else {
127            self.variable_bounds
128                .insert(var_key, HashMap::from([(idx, (lower_bound, upper_bound))]));
129        }
130    }
131
132    pub fn remove_variable_bounds(&mut self, var_key: VarKey) {
133        self.variable_bounds.remove(var_key);
134    }
135
136    fn create_variable(
137        manifold_type: &ManifoldType,
138        params: &DVector<f64>,
139    ) -> Box<dyn ManifoldVariable> {
140        match manifold_type {
141            ManifoldType::SO2 => {
142                Box::new(Variable::new(so2::SO2::from_param_slice(params.as_slice())))
143            }
144            ManifoldType::SO3 => {
145                Box::new(Variable::new(so3::SO3::from_param_slice(params.as_slice())))
146            }
147            ManifoldType::SE2 => {
148                Box::new(Variable::new(se2::SE2::from_param_slice(params.as_slice())))
149            }
150            ManifoldType::SE3 => {
151                Box::new(Variable::new(se3::SE3::from_param_slice(params.as_slice())))
152            }
153            ManifoldType::RN => Box::new(Variable::new(rn::Rn::new(params.clone()))),
154            ManifoldType::SE23 => Box::new(Variable::new(se23::SE23::from_param_slice(
155                params.as_slice(),
156            ))),
157            ManifoldType::SGal3 => Box::new(Variable::new(sgal3::SGal3::from_param_slice(
158                params.as_slice(),
159            ))),
160            ManifoldType::Sim3 => Box::new(Variable::new(sim3::Sim3::from_param_slice(
161                params.as_slice(),
162            ))),
163        }
164    }
165
166    /// Apply fixed indices and bounds to a mutable clone of the variable map.
167    ///
168    /// Optimizers call this after copying `problem.variables` to initialize working state.
169    pub fn apply_constraints_to_variables(
170        &self,
171        variables: &mut SlotMap<VarKey, Box<dyn ManifoldVariable>>,
172    ) {
173        for (key, var) in variables.iter_mut() {
174            if let Some(indexes) = self.fixed_variable_indexes.get(key) {
175                var.set_fixed_indices(indexes.clone());
176            }
177            if let Some(bounds) = self.variable_bounds.get(key) {
178                var.set_bounds(bounds.clone());
179            }
180        }
181    }
182
183    pub fn num_residual_blocks(&self) -> usize {
184        self.residual_blocks.len()
185    }
186
187    pub(crate) fn residual_blocks(&self) -> &SlotMap<FactorKey, ResidualBlock> {
188        &self.residual_blocks
189    }
190
191    /// Compute only the residual vector (no Jacobian) for the given variable values.
192    pub fn compute_residual_sparse(
193        &self,
194        variables: &SlotMap<VarKey, Box<dyn ManifoldVariable>>,
195    ) -> CoreResult<Mat<f64>> {
196        use crate::linearizer::split_by_row_offsets_mut;
197
198        let mut blocks: Vec<&crate::core::residual_block::ResidualBlock> =
199            self.residual_blocks.values().collect();
200        blocks.sort_by_key(|b| b.residual_row_start_idx);
201
202        let mut residual_buf = vec![0.0f64; self.total_residual_dimension];
203        let offsets_lens: Vec<(usize, usize)> = blocks
204            .iter()
205            .map(|b| (b.residual_row_start_idx, b.factor.residual_dim()))
206            .collect();
207        let residual_slices = split_by_row_offsets_mut(&mut residual_buf, &offsets_lens);
208
209        let results: Vec<CoreResult<()>> = residual_slices
210            .into_par_iter()
211            .zip(blocks.par_iter())
212            .map(|(slice, block)| self.compute_residual_block(block, variables, slice))
213            .collect();
214        results.into_iter().collect::<CoreResult<Vec<_>>>()?;
215
216        let n = self.total_residual_dimension;
217        Ok(Mat::from_fn(n, 1, |i, _| residual_buf[i]))
218    }
219
220    /// Compute residuals and sparse Jacobian.
221    pub fn compute_residual_and_jacobian_sparse(
222        &self,
223        variables: &SlotMap<VarKey, Box<dyn ManifoldVariable>>,
224        variable_index_map: &SecondaryMap<VarKey, usize>,
225        symbolic_structure: &SymbolicStructure,
226    ) -> CoreResult<(Mat<f64>, SparseColMat<usize, f64>)> {
227        Ok(crate::linearizer::cpu::sparse::assemble_sparse(
228            self,
229            variables,
230            variable_index_map,
231            symbolic_structure,
232        )?)
233    }
234
235    /// Compute residuals and dense Jacobian.
236    pub fn compute_residual_and_jacobian_dense(
237        &self,
238        variables: &SlotMap<VarKey, Box<dyn ManifoldVariable>>,
239        variable_index_map: &SecondaryMap<VarKey, usize>,
240        total_dof: usize,
241    ) -> CoreResult<(Mat<f64>, Mat<f64>)> {
242        Ok(crate::linearizer::cpu::dense::assemble_dense(
243            self,
244            variables,
245            variable_index_map,
246            total_dof,
247        )?)
248    }
249
250    fn compute_residual_block(
251        &self,
252        residual_block: &ResidualBlock,
253        variables: &SlotMap<VarKey, Box<dyn ManifoldVariable>>,
254        residual_slice: &mut [f64],
255    ) -> CoreResult<()> {
256        let mut param_slices: smallvec::SmallVec<[&[f64]; 8]> = smallvec::SmallVec::new();
257        for &k in &residual_block.variable_keys {
258            if let Some(v) = variables.get(k) {
259                param_slices.push(v.as_param_slice());
260            }
261        }
262
263        residual_block
264            .factor
265            .linearize(&param_slices, residual_slice, None);
266
267        if let Some(loss_func) = &residual_block.loss_func {
268            let squared_norm: f64 = residual_slice.iter().map(|x| x * x).sum();
269            let corrector = Corrector::new(loss_func.as_ref(), squared_norm);
270            corrector.correct_residual_in_place(residual_slice);
271        }
272
273        Ok(())
274    }
275
276    pub fn log_residual_to_file(
277        &self,
278        residual: &nalgebra::DVector<f64>,
279        filename: &str,
280    ) -> Result<(), Error> {
281        let mut file = File::create(filename)?;
282        writeln!(file, "# Residual vector - {} elements", residual.len())?;
283        for (i, &value) in residual.iter().enumerate() {
284            writeln!(file, "{}: {:.12}", i, value)?;
285        }
286        Ok(())
287    }
288
289    pub fn log_sparse_jacobian_to_file(
290        &self,
291        jacobian: &SparseColMat<usize, f64>,
292        filename: &str,
293    ) -> Result<(), Error> {
294        let mut file = File::create(filename)?;
295        writeln!(
296            file,
297            "# Sparse Jacobian matrix - {} x {} ({} non-zeros)",
298            jacobian.nrows(),
299            jacobian.ncols(),
300            jacobian.compute_nnz()
301        )?;
302        writeln!(file, "# Matrix saved as dimensions and non-zero count only")?;
303        Ok(())
304    }
305
306    pub fn log_variables_to_file(
307        &self,
308        variables: &SlotMap<VarKey, Box<dyn ManifoldVariable>>,
309        filename: &str,
310    ) -> Result<(), Error> {
311        let mut file = File::create(filename)?;
312        writeln!(file, "# Variables - {} total", variables.len())?;
313        for (_, var) in variables {
314            let vec = var.to_dvector();
315            write!(file, "[")?;
316            for (i, &v) in vec.iter().enumerate() {
317                write!(file, "{:.12}", v)?;
318                if i < vec.len() - 1 {
319                    write!(file, ", ")?;
320                }
321            }
322            writeln!(file, "]")?;
323        }
324        Ok(())
325    }
326
327    pub fn compute_and_set_covariances(
328        &self,
329        linear_solver: &mut Box<dyn LinearSolver<SparseMode>>,
330        variables: &mut SlotMap<VarKey, Box<dyn ManifoldVariable>>,
331        variable_index_map: &SecondaryMap<VarKey, usize>,
332    ) -> Option<SecondaryMap<VarKey, Mat<f64>>> {
333        linear_solver.compute_covariance_matrix()?;
334        let full_cov = linear_solver.get_covariance_matrix()?.clone();
335        let per_var = extract_variable_covariances(&full_cov, variables, variable_index_map);
336        for (key, cov) in &per_var {
337            if let Some(var) = variables.get_mut(key) {
338                var.set_covariance(cov.clone());
339            }
340        }
341        Some(per_var)
342    }
343
344    pub fn compute_and_set_covariances_generic<M: crate::linalg::LinearizationMode>(
345        &self,
346        linear_solver: &mut dyn crate::linalg::LinearSolver<M>,
347        variables: &mut SlotMap<VarKey, Box<dyn ManifoldVariable>>,
348        variable_index_map: &SecondaryMap<VarKey, usize>,
349    ) -> Option<SecondaryMap<VarKey, Mat<f64>>> {
350        linear_solver.compute_covariance_matrix()?;
351        let full_cov = linear_solver.get_covariance_matrix()?.clone();
352        let per_var = extract_variable_covariances(&full_cov, variables, variable_index_map);
353        for (key, cov) in &per_var {
354            if let Some(var) = variables.get_mut(key) {
355                var.set_covariance(cov.clone());
356            }
357        }
358        Some(per_var)
359    }
360}
361
362#[cfg(test)]
363mod tests {
364    use super::*;
365    use crate::core::loss_functions::HuberLoss;
366    use crate::factors::{BetweenFactor, PriorFactor};
367    use apex_manifolds::{ManifoldType, se2::SE2, se3::SE3};
368    use nalgebra::{Quaternion, Vector3, dvector};
369
370    type TestResult = Result<(), Box<dyn std::error::Error>>;
371
372    fn create_se2_test_problem() -> Result<(Problem, Vec<VarKey>), Box<dyn std::error::Error>> {
373        let mut problem = Problem::new(JacobianMode::Sparse);
374
375        let poses = [
376            (0.0_f64, 0.0, 0.0),
377            (1.0, 0.0, 0.1),
378            (1.5, 1.0, 0.5),
379            (1.0, 2.0, 1.0),
380            (0.0, 2.5, 1.5),
381            (-1.0, 2.0, 2.0),
382            (-1.5, 1.0, 2.5),
383            (-1.0, 0.0, 3.0),
384            (-0.5, -0.5, -2.8),
385            (0.5, -0.5, -2.3),
386        ];
387
388        let keys: Vec<VarKey> = poses
389            .iter()
390            .map(|&(x, y, t)| problem.add_variable(ManifoldType::SE2, dvector![x, y, t]))
391            .collect();
392
393        for i in 0..9 {
394            let (fx, fy, ft) = poses[i];
395            let (tx, ty, tt) = poses[i + 1];
396            problem.add_residual_block(
397                &[keys[i], keys[i + 1]],
398                Box::new(BetweenFactor::new(SE2::from_xy_angle(
399                    tx - fx,
400                    ty - fy,
401                    tt - ft,
402                ))),
403                Some(Box::new(HuberLoss::new(1.0)?)),
404            );
405        }
406
407        let (fx, fy, ft) = poses[9];
408        let (tx, ty, tt) = poses[0];
409        problem.add_residual_block(
410            &[keys[9], keys[0]],
411            Box::new(BetweenFactor::new(SE2::from_xy_angle(
412                tx - fx,
413                ty - fy,
414                tt - ft,
415            ))),
416            Some(Box::new(HuberLoss::new(1.0)?)),
417        );
418
419        problem.add_residual_block(
420            &[keys[0]],
421            Box::new(PriorFactor {
422                data: dvector![0.0, 0.0, 0.0],
423            }),
424            None,
425        );
426
427        Ok((problem, keys))
428    }
429
430    fn create_se3_test_problem() -> Result<(Problem, Vec<VarKey>), Box<dyn std::error::Error>> {
431        let mut problem = Problem::new(JacobianMode::Sparse);
432
433        // (tx, ty, tz, qx, qy, qz, qw)
434        let poses = [
435            (0.0_f64, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0),
436            (1.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.995),
437            (1.0, 1.0, 0.0, 0.0, 0.0, 0.2, 0.98),
438            (0.0, 1.0, 0.0, 0.0, 0.0, 0.3, 0.955),
439            (0.0, 0.0, 1.0, 0.1, 0.0, 0.0, 0.995),
440            (1.0, 0.0, 1.0, 0.1, 0.0, 0.1, 0.99),
441            (1.0, 1.0, 1.0, 0.1, 0.0, 0.2, 0.975),
442            (0.0, 1.0, 1.0, 0.1, 0.0, 0.3, 0.95),
443        ];
444
445        let keys: Vec<VarKey> = poses
446            .iter()
447            .map(|&(tx, ty, tz, qx, qy, qz, qw)| {
448                problem.add_variable(ManifoldType::SE3, dvector![tx, ty, tz, qw, qx, qy, qz])
449            })
450            .collect();
451
452        let edges = [
453            (0, 1),
454            (1, 2),
455            (2, 3),
456            (3, 0),
457            (4, 5),
458            (5, 6),
459            (6, 7),
460            (7, 4),
461            (0, 4),
462            (1, 5),
463            (2, 6),
464            (3, 7),
465        ];
466
467        for (f, t) in edges {
468            let fp = poses[f];
469            let tp = poses[t];
470            let rel = SE3::from_translation_quaternion(
471                Vector3::new(tp.0 - fp.0, tp.1 - fp.1, tp.2 - fp.2),
472                Quaternion::new(1.0, 0.0, 0.0, 0.0),
473            );
474            problem.add_residual_block(
475                &[keys[f], keys[t]],
476                Box::new(BetweenFactor::new(rel)),
477                Some(Box::new(HuberLoss::new(1.0)?)),
478            );
479        }
480
481        problem.add_residual_block(
482            &[keys[0]],
483            Box::new(PriorFactor {
484                data: dvector![0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
485            }),
486            None,
487        );
488
489        Ok((problem, keys))
490    }
491
492    #[test]
493    fn test_problem_construction_se2() -> TestResult {
494        let (problem, keys) = create_se2_test_problem()?;
495        assert_eq!(problem.num_residual_blocks(), 11);
496        assert_eq!(problem.total_residual_dimension, 33);
497        assert_eq!(keys.len(), 10);
498        assert_eq!(problem.variables.len(), 10);
499        Ok(())
500    }
501
502    #[test]
503    fn test_problem_construction_se3() -> TestResult {
504        let (problem, keys) = create_se3_test_problem()?;
505        assert_eq!(problem.num_residual_blocks(), 13);
506        assert_eq!(keys.len(), 8);
507        assert_eq!(problem.variables.len(), 8);
508        Ok(())
509    }
510
511    #[test]
512    fn test_add_variable_returns_distinct_keys() {
513        let mut problem = Problem::new(JacobianMode::Sparse);
514        let k0 = problem.add_variable(ManifoldType::SE2, dvector![0.0, 0.0, 0.0]);
515        let k1 = problem.add_variable(ManifoldType::SE2, dvector![1.0, 0.0, 0.1]);
516        assert_ne!(k0, k1);
517        assert_eq!(problem.variables.len(), 2);
518        assert_eq!(problem.variables[k0].dof(), 3);
519    }
520
521    #[test]
522    fn test_variable_all_manifold_types() {
523        let mut p = Problem::new(JacobianMode::Sparse);
524        let so2 = p.add_variable(ManifoldType::SO2, dvector![0.5]);
525        let so3 = p.add_variable(ManifoldType::SO3, dvector![1.0, 0.0, 0.0, 0.0]);
526        let se2 = p.add_variable(ManifoldType::SE2, dvector![1.0, 2.0, 0.5]);
527        let se3 = p.add_variable(
528            ManifoldType::SE3,
529            dvector![1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
530        );
531        let rn = p.add_variable(ManifoldType::RN, dvector![5.0, 6.0]);
532
533        assert_eq!(p.variables[so2].manifold_type_name(), "SO2");
534        assert_eq!(p.variables[so3].manifold_type_name(), "SO3");
535        assert_eq!(p.variables[se2].manifold_type_name(), "SE2");
536        assert_eq!(p.variables[se3].manifold_type_name(), "SE3");
537        assert_eq!(p.variables[rn].manifold_type_name(), "Rn");
538    }
539
540    #[test]
541    fn test_residual_block_add_remove() -> TestResult {
542        let mut p = Problem::new(JacobianMode::Sparse);
543        let k0 = p.add_variable(ManifoldType::SE2, dvector![0.0, 0.0, 0.0]);
544        let k1 = p.add_variable(ManifoldType::SE2, dvector![1.0, 0.0, 0.1]);
545
546        let fk1 = p.add_residual_block(
547            &[k0, k1],
548            Box::new(BetweenFactor::new(SE2::from_xy_angle(1.0, 0.0, 0.1))),
549            Some(Box::new(HuberLoss::new(1.0)?)),
550        );
551        let fk2 = p.add_residual_block(
552            &[k0],
553            Box::new(PriorFactor {
554                data: dvector![0.0, 0.0, 0.0],
555            }),
556            None,
557        );
558
559        assert_ne!(fk1, fk2);
560        assert_eq!(p.num_residual_blocks(), 2);
561        assert_eq!(p.total_residual_dimension, 6);
562
563        let removed = p.remove_residual_block(fk1);
564        assert!(removed.is_some());
565        assert_eq!(p.num_residual_blocks(), 1);
566        assert_eq!(p.total_residual_dimension, 3);
567
568        assert!(p.remove_residual_block(fk1).is_none());
569        Ok(())
570    }
571
572    #[test]
573    fn test_fix_unfix_variable() {
574        let mut p = Problem::new(JacobianMode::Sparse);
575        let k0 = p.add_variable(ManifoldType::SE2, dvector![0.0, 0.0, 0.0]);
576        let k1 = p.add_variable(ManifoldType::SE2, dvector![1.0, 0.0, 0.1]);
577
578        p.fix_variable(k0, 0);
579        p.fix_variable(k0, 1);
580        p.fix_variable(k1, 2);
581
582        assert_eq!(p.fixed_variable_indexes[k0].len(), 2);
583        assert_eq!(p.fixed_variable_indexes[k1].len(), 1);
584
585        p.unfix_variable(k0);
586        assert!(!p.fixed_variable_indexes.contains_key(k0));
587        assert!(p.fixed_variable_indexes.contains_key(k1));
588    }
589
590    #[test]
591    fn test_variable_bounds_set_remove() {
592        let mut p = Problem::new(JacobianMode::Sparse);
593        let k0 = p.add_variable(ManifoldType::SE2, dvector![0.0, 0.0, 0.0]);
594        let k1 = p.add_variable(ManifoldType::SE2, dvector![0.0, 0.0, 0.0]);
595
596        p.set_variable_bounds(k0, 0, -1.0, 1.0);
597        p.set_variable_bounds(k0, 1, -2.0, 2.0);
598        p.set_variable_bounds(k1, 0, 0.0, 5.0);
599
600        assert_eq!(p.variable_bounds[k0].len(), 2);
601        assert_eq!(p.variable_bounds[k1].len(), 1);
602
603        p.remove_variable_bounds(k0);
604        assert!(!p.variable_bounds.contains_key(k0));
605        assert!(p.variable_bounds.contains_key(k1));
606    }
607
608    #[test]
609    fn test_set_variable_bounds_invalid_order() {
610        let mut p = Problem::new(JacobianMode::Sparse);
611        let k = p.add_variable(ManifoldType::SE2, dvector![0.0, 0.0, 0.0]);
612        p.set_variable_bounds(k, 0, 5.0, 1.0);
613        assert!(!p.variable_bounds.contains_key(k));
614    }
615
616    #[test]
617    fn test_problem_default_equals_new_sparse() {
618        let d = Problem::default();
619        let n = Problem::new(JacobianMode::Sparse);
620        assert_eq!(d.jacobian_mode, n.jacobian_mode);
621        assert_eq!(d.num_residual_blocks(), 0);
622    }
623
624    #[test]
625    fn test_compute_residual_sparse_smoke() -> TestResult {
626        let (problem, _) = create_se2_test_problem()?;
627        let residual = problem.compute_residual_sparse(&problem.variables)?;
628        let norm_sq: f64 = (0..residual.nrows())
629            .map(|i| residual[(i, 0)].powi(2))
630            .sum();
631        assert!(norm_sq >= 0.0);
632        assert_eq!(residual.nrows(), problem.total_residual_dimension);
633        Ok(())
634    }
635
636    #[test]
637    fn test_variable_covariance_lifecycle() -> TestResult {
638        use faer::Mat;
639        let mut p = Problem::new(JacobianMode::Sparse);
640        let k = p.add_variable(ManifoldType::SE2, dvector![0.0, 0.0, 0.0]);
641
642        assert!(p.variables[k].covariance().is_none());
643        p.variables[k].set_covariance(Mat::identity(3, 3));
644        let cov = p.variables[k].covariance().ok_or("no cov")?;
645        assert_eq!(cov.nrows(), 3);
646        p.variables[k].clear_covariance();
647        assert!(p.variables[k].covariance().is_none());
648        Ok(())
649    }
650
651    #[test]
652    fn test_fixed_indices_stored_correctly() {
653        let mut p = Problem::new(JacobianMode::Sparse);
654        let k = p.add_variable(ManifoldType::SE2, dvector![0.0, 0.0, 0.0]);
655        p.fix_variable(k, 0);
656        p.fix_variable(k, 2);
657        assert_eq!(p.fixed_variable_indexes[k].len(), 2);
658        assert!(p.fixed_variable_indexes[k].contains(&0));
659        assert!(p.fixed_variable_indexes[k].contains(&2));
660    }
661
662    #[test]
663    fn test_log_residual_to_file() -> TestResult {
664        let p = Problem::new(JacobianMode::Sparse);
665        let res = nalgebra::dvector![1.0, 2.0, 3.0];
666        let path = std::env::temp_dir().join("apex_test_residual.txt");
667        p.log_residual_to_file(&res, path.to_str().ok_or("bad path")?)?;
668        assert!(path.exists());
669        Ok(())
670    }
671
672    #[test]
673    fn test_log_variables_to_file() -> TestResult {
674        let mut p = Problem::new(JacobianMode::Sparse);
675        p.add_variable(ManifoldType::SE2, dvector![1.0, 2.0, 0.3]);
676        let vars = p.variables.clone();
677        let path = std::env::temp_dir().join("apex_test_variables.txt");
678        p.log_variables_to_file(&vars, path.to_str().ok_or("bad path")?)?;
679        assert!(path.exists());
680        Ok(())
681    }
682
683    #[test]
684    fn test_log_sparse_jacobian_to_file() -> TestResult {
685        use faer::sparse::SparseColMat;
686        let p = Problem::new(JacobianMode::Sparse);
687        let triplets = vec![faer::sparse::Triplet::new(0usize, 0usize, 1.0f64)];
688        let jac =
689            SparseColMat::try_new_from_triplets(1, 1, &triplets).map_err(|e| format!("{e:?}"))?;
690        let path = std::env::temp_dir().join("apex_test_jacobian.txt");
691        p.log_sparse_jacobian_to_file(&jac, path.to_str().ok_or("bad path")?)?;
692        assert!(path.exists());
693        Ok(())
694    }
695
696    #[test]
697    fn test_apply_tangent_step_se2() -> TestResult {
698        let mut p = Problem::new(JacobianMode::Sparse);
699        let k = p.add_variable(ManifoldType::SE2, dvector![0.0, 0.0, 0.0]);
700        p.variables[k].apply_tangent_step(&[1.0, 2.0, 3.0]);
701        assert_eq!(p.variables[k].dof(), 3);
702        Ok(())
703    }
704
705    #[test]
706    fn test_variable_rn_values() -> TestResult {
707        let mut p = Problem::new(JacobianMode::Sparse);
708        let k = p.add_variable(ManifoldType::RN, dvector![5.0, 6.0]);
709        let vec = p.variables[k].to_dvector();
710        assert!((vec[0] - 5.0).abs() < 1e-10);
711        assert!((vec[1] - 6.0).abs() < 1e-10);
712        Ok(())
713    }
714}
apex_solver/core/problem.rs

apex_solver/core/
problem.rs
