Struct Problem 

pub struct Problem { /* private fields */ }

The optimization problem definition for factor graph optimization.

Manages residual blocks (factors/constraints), variables, and the sparse Jacobian structure. Supports mixed manifold types (SE2, SE3, SO2, SO3, Rn) in a single problem and provides efficient parallel residual/Jacobian computation.

Architecture

The Problem acts as a container and coordinator:

• Stores all residual blocks (factors with optional loss functions)
• Tracks the global structure (which variables connect to which factors)
• Builds and maintains the sparse Jacobian pattern
• Provides parallel residual/Jacobian evaluation using rayon
• Manages variable constraints (fixed indices, bounds)

Workflow

1. Construction: Create a new Problem with Problem::new(JacobianMode::Sparse)
2. Add Factors: Use add_residual_block() to add constraints
3. Initialize Variables: Use initialize_variables() with initial values
4. Build Sparsity: Use build_symbolic_structure() once before optimization
5. Linearize: Call compute_residual_and_jacobian_sparse() each iteration
6. Extract Covariance: Use compute_and_set_covariances() after convergence

Example

use apex_solver::core::problem::Problem;
use apex_solver::factors::BetweenFactor;
use apex_solver::manifold::ManifoldType;
use apex_solver::manifold::se2::SE2;
use apex_solver::JacobianMode;
use nalgebra::dvector;
use std::collections::HashMap;

let mut problem = Problem::new(JacobianMode::Sparse);

// Add a between factor
let factor = Box::new(BetweenFactor::new(SE2::from_xy_angle(1.0, 0.0, 0.1)));
problem.add_residual_block(&["x0", "x1"], factor, None);

// Initialize variables
let mut initial = HashMap::new();
initial.insert("x0".to_string(), (ManifoldType::SE2, dvector![0.0, 0.0, 0.0]));
initial.insert("x1".to_string(), (ManifoldType::SE2, dvector![1.0, 0.0, 0.1]));

let variables = problem.initialize_variables(&initial);
assert_eq!(variables.len(), 2);

Implementations

impl Problem

pub fn new(jacobian_mode: JacobianMode) -> Self

Create a new empty optimization problem with the given Jacobian assembly mode.

Arguments

• jacobian_mode - Assembly strategy: JacobianMode::Sparse (default, large problems) or JacobianMode::Dense (small-to-medium problems < ~500 DOF)

Example

use apex_solver::core::problem::Problem;
use apex_solver::JacobianMode;

// Sparse (default for large-scale problems)
let sparse_problem = Problem::new(JacobianMode::Sparse);
assert_eq!(sparse_problem.num_residual_blocks(), 0);

// Dense (for small-to-medium problems)
let dense_problem = Problem::new(JacobianMode::Dense);
assert_eq!(dense_problem.num_residual_blocks(), 0);

pub fn add_residual_block( &mut self, variable_key_size_list: &[&str], factor: Box<dyn Factor + Send>, loss_func: Option<Box<dyn LossFunction + Send>>, ) -> usize

Add a residual block (factor with optional loss function) to the problem.

This is the primary method for building the factor graph. Each call adds one constraint connecting one or more variables.

Arguments

• variable_key_size_list - Names of the variables this factor connects (order matters)
• factor - The factor implementation that computes residuals and Jacobians
• loss_func - Optional robust loss function for outlier rejection

Returns

The unique ID assigned to this residual block

Example

use apex_solver::core::problem::Problem;
use apex_solver::factors::{BetweenFactor, PriorFactor};
use apex_solver::core::loss_functions::HuberLoss;
use apex_solver::JacobianMode;
use nalgebra::dvector;
use apex_solver::manifold::se2::SE2;

let mut problem = Problem::new(JacobianMode::Sparse);

// Add prior factor (unary constraint)
let prior = Box::new(PriorFactor { data: dvector![0.0, 0.0, 0.0] });
let id1 = problem.add_residual_block(&["x0"], prior, None);

// Add between factor with robust loss (binary constraint)
let between = Box::new(BetweenFactor::new(SE2::from_xy_angle(1.0, 0.0, 0.1)));
let loss: Option<Box<dyn apex_solver::core::loss_functions::LossFunction + Send>> =
    Some(Box::new(HuberLoss::new(1.0)?));
let id2 = problem.add_residual_block(&["x0", "x1"], between, loss);

assert_eq!(id1, 0);
assert_eq!(id2, 1);
assert_eq!(problem.num_residual_blocks(), 2);

pub fn remove_residual_block( &mut self, block_id: usize, ) -> Option<ResidualBlock>

pub fn fix_variable(&mut self, var_to_fix: &str, idx: usize)

pub fn unfix_variable(&mut self, var_to_unfix: &str)

pub fn set_variable_bounds( &mut self, var_to_bound: &str, idx: usize, lower_bound: f64, upper_bound: f64, )

pub fn remove_variable_bounds(&mut self, var_to_unbound: &str)

pub fn initialize_variables( &self, initial_values: &HashMap<String, (ManifoldType, DVector<f64>)>, ) -> HashMap<String, VariableEnum>

Initialize variables from initial values with manifold types.

Converts raw initial values into typed Variable<M> instances wrapped in VariableEnum. This method also applies any fixed indices or bounds that were set via fix_variable() or set_variable_bounds().

Arguments

• initial_values - Map from variable name to (manifold type, initial value vector)

Returns

Map from variable name to VariableEnum (typed variables ready for optimization)

Manifold Formats

• SE2: [x, y, theta] (3 elements)
• SE3: [tx, ty, tz, qw, qx, qy, qz] (7 elements)
• SO2: [theta] (1 element)
• SO3: [qw, qx, qy, qz] (4 elements)
• Rn: [x1, x2, ..., xn] (n elements)

Example

use apex_solver::core::problem::Problem;
use apex_solver::manifold::ManifoldType;
use apex_solver::JacobianMode;
use nalgebra::dvector;
use std::collections::HashMap;

let problem = Problem::new(JacobianMode::Sparse);

let mut initial = HashMap::new();
initial.insert("pose0".to_string(), (ManifoldType::SE2, dvector![0.0, 0.0, 0.0]));
initial.insert("pose1".to_string(), (ManifoldType::SE2, dvector![1.0, 0.0, 0.1]));
initial.insert("landmark".to_string(), (ManifoldType::RN, dvector![5.0, 3.0]));

let variables = problem.initialize_variables(&initial);
assert_eq!(variables.len(), 3);

pub fn num_residual_blocks(&self) -> usize

Get the number of residual blocks

pub fn compute_residual_sparse( &self, variables: &HashMap<String, VariableEnum>, ) -> CoreResult<Mat<f64>>

Compute only the residual vector for the current variable values.

This is an optimized version that skips Jacobian computation when only the cost function value is needed (e.g., during initialization or step evaluation).

Arguments

• variables - Current variable values (from initialize_variables() or updated)

Returns

Residual vector as N×1 column matrix (N = total residual dimension)

Performance

Approximately 2x faster than compute_residual_and_jacobian_sparse() since it:

• Skips Jacobian computation for each residual block
• Avoids Jacobian matrix assembly and storage
• Only parallelizes residual evaluation

When to Use

• Initial cost computation: When setting up optimization state
• Step evaluation: When computing new cost after applying parameter updates
• Cost-only queries: When you don’t need gradients

Use compute_residual_and_jacobian_sparse() when you need both residual and Jacobian (e.g., in the main optimization iteration loop for linearization).

Example

// Initial cost evaluation (no Jacobian needed)
let residual = problem.compute_residual_sparse(&variables)?;
let initial_cost = residual.norm_l2() * residual.norm_l2();

pub fn compute_residual_and_jacobian_sparse( &self, variables: &HashMap<String, VariableEnum>, variable_index_sparce_matrix: &HashMap<String, usize>, symbolic_structure: &SymbolicStructure, ) -> CoreResult<(Mat<f64>, SparseColMat<usize, f64>)>

Compute residual vector and sparse Jacobian matrix for the current variable values.

This is the core linearization method called during each optimization iteration. It:

1. Evaluates all residual blocks in parallel using rayon
2. Assembles the full residual vector
3. Constructs the sparse Jacobian matrix using the precomputed symbolic structure

Arguments

• variables - Current variable values (from initialize_variables() or updated)
• variable_index_sparce_matrix - Map from variable name to starting column in Jacobian
• symbolic_structure - Precomputed sparsity pattern (from build_symbolic_structure())

Returns

Tuple (residual, jacobian) where:

• residual: N×1 column matrix (total residual dimension)
• jacobian: N×M sparse matrix (N = residual dim, M = total DOF)

Performance

This method is highly optimized:

• Parallel evaluation: Each residual block is evaluated independently using rayon
• Sparse storage: Only non-zero Jacobian entries are stored and computed
• Memory efficient: Preallocated sparse structure avoids dynamic allocations

Typically accounts for 40-60% of total optimization time (including sparse matrix ops).

When to Use

Use this method in the main optimization loop when you need both residual and Jacobian for linearization. For cost-only evaluation, use compute_residual_sparse() instead.

Example

// Inside optimizer loop, compute both residual and Jacobian for linearization
// let (residual, jacobian) = problem.compute_residual_and_jacobian_sparse(
//     &variables,
//     &variable_index_map,
//     &symbolic_structure,
// )?;
//
// Use for linear system: J^T J dx = -J^T r

Compute residuals and sparse Jacobian.

Delegates to assembly::sparse::assemble_sparse().

pub fn compute_residual_and_jacobian_dense( &self, variables: &HashMap<String, VariableEnum>, variable_index_map: &HashMap<String, usize>, total_dof: usize, ) -> CoreResult<(Mat<f64>, Mat<f64>)>

Compute residuals and dense Jacobian.

Delegates to assembly::dense::assemble_dense().

pub fn log_residual_to_file( &self, residual: &DVector<f64>, filename: &str, ) -> Result<(), Error>

Log residual vector to a text file

pub fn log_sparse_jacobian_to_file( &self, jacobian: &SparseColMat<usize, f64>, filename: &str, ) -> Result<(), Error>

Log sparse Jacobian matrix to a text file

pub fn log_variables_to_file( &self, variables: &HashMap<String, VariableEnum>, filename: &str, ) -> Result<(), Error>

Log variables to a text file

pub fn compute_and_set_covariances( &self, linear_solver: &mut Box<dyn LinearSolver<SparseMode>>, variables: &mut HashMap<String, VariableEnum>, variable_index_map: &HashMap<String, usize>, ) -> Option<HashMap<String, Mat<f64>>>

Compute per-variable covariances and set them in Variable objects

This method computes the full covariance matrix by inverting the Hessian from the linear solver, then extracts per-variable covariance blocks and stores them in the corresponding Variable objects.

Arguments

• linear_solver - Mutable reference to the linear solver containing the cached Hessian
• variables - Mutable map of variables where covariances will be stored
• variable_index_map - Map from variable names to their starting column indices

Returns

Some(HashMap) containing per-variable covariance matrices if successful, None otherwise

pub fn compute_and_set_covariances_generic<M: LinearizationMode>( &self, linear_solver: &mut dyn LinearSolver<M>, variables: &mut HashMap<String, VariableEnum>, variable_index_map: &HashMap<String, usize>, ) -> Option<HashMap<String, Mat<f64>>>

Compute and set covariances using a generic linear solver.

This is the generic version of [compute_and_set_covariances] that works with any assembly mode (sparse or dense).

Trait Implementations

impl Default for Problem

fn default() -> Self

Returns the “default value” for a type.