Module problem 

Optimization problem definition and sparse Jacobian computation.

The Problem struct is the central component that defines a factor graph optimization problem. It manages residual blocks (constraints), variables, and the construction of sparse Jacobian matrices for efficient nonlinear least squares optimization.

Factor Graph Representation

The optimization problem is represented as a bipartite factor graph:

Variables:  x0 --- x1 --- x2 --- x3
             |      |      |      |
Factors:    f0     f1     f2     f3 (constraints/measurements)

Each factor connects one or more variables and contributes a residual (error) term to the overall cost function:

minimize Σ_i ρ(||r_i(x)||²)

where r_i(x) is the residual for factor i, and ρ is an optional robust loss function.

Key Responsibilities

1. Residual Block Management: Add/remove factors and track their structure
2. Variable Management: Initialize variables with manifold types and constraints
3. Sparsity Pattern: Build symbolic structure for efficient sparse linear algebra
4. Linearization: Compute residuals and Jacobians in parallel
5. Covariance: Extract per-variable uncertainty estimates after optimization

Sparse Jacobian Structure

The Jacobian matrix J = ∂r/∂x is sparse because each factor only depends on a small subset of variables. For example, a between factor connecting x0 and x1 contributes a 3×6 block (SE2) or 6×12 block (SE3) to the Jacobian, leaving the rest as zeros.

The Problem pre-computes the sparsity pattern once, then efficiently fills in the numerical values during each iteration.

Mixed Manifold Support

The Problem supports mixed manifold types in a single optimization problem via VariableEnum. This allows:

• SE2 and SE3 poses in the same graph
• SO3 rotations with R³ landmarks
• Any combination of supported manifolds

Example: Building a Problem

use apex_solver::core::problem::Problem;
use apex_solver::factors::{BetweenFactor, PriorFactor};
use apex_solver::core::loss_functions::HuberLoss;
use apex_solver::manifold::ManifoldType;
use nalgebra::{DVector, dvector};
use std::collections::HashMap;
use apex_solver::manifold::se2::SE2;

let mut problem = Problem::new();

// Add prior factor to anchor the first pose
let prior = Box::new(PriorFactor {
    data: dvector![0.0, 0.0, 0.0],
});
problem.add_residual_block(&["x0"], prior, None);

// Add between factor with robust loss
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)?));
problem.add_residual_block(&["x0", "x1"], between, loss);

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

let variables = problem.initialize_variables(&initial_values);

Structs

Problem

The optimization problem definition for factor graph optimization.

SymbolicStructure

Symbolic structure for sparse matrix operations.

Enums

VariableEnum

Enum to handle mixed manifold variable types