iSAM is an incremental optimizer based on the GTSAM framework, designed for full-trajectory SLAM. It builds upon the Square Root SAM method, enabling incremental optimization for landmark-based SLAM.

Core Principles

The primary concept involves linearizing the nonlinear SLAM problem in to the form (A\theta = b). Through factorization, this is transformed into (R\theta = d), where (R) is an upper-triangular matrix, allowing solution via back-substitution. Key techniques include:

• Block Ordering: Using a column approximate minimum degree (COLAMD) algorithm, treating each pose and landmark as a single block varible to accelerate factorization.
• Incremental Updates: When new observations arrive, the (R) matrix is augmented. Givens rotations are applied to restore its upper-triangular form, enabling continued back-substitution.
• Periodic Variable Reordering: Combined with incremental updates, this maintains the sparsity of the (R) matrix and computational efficiency over time.

Problem Formulation

Consider a SLAM system represented by a Bayesian belief network. Let (X = {x_i}) for (i \in 0, \dots, M) be robot poses, (L = {l_j}) for (j \in 1, \dots, N) be landmark coordinates, (U = {u_i}) be control inputs, and (Z = {z_k}) be landmark observations.

The joint probability is: [ P(X, L, U, Z) \propto P(x_0) \prod_{i=1}^{M} P(x_i | x_{i-1}, u_i) \prod_{k=1}^{K} P(z_k | x_{i_k}, l_{j_k}) ] where (P(x_0)) is the prior, (P(x_i | x_{i-1}, u_i)) is the motion model, and (P(z_k | x_{i_k}, l_{j_k})) is the measurement model.

Assuming Gaussian noise models: [ x_i = f_i(x_{i-1}, u_i) + w_i ] [ z_k = h_k(x_{i_k}, l_{j_k}) + v_k ]

The maximum a posteriori (MAP) estimation problem converts to a nonlinear least-squares minimization: [ X^, L^ = \arg\min_{X,L} \left{ \sum_{i=1}^{M} | f_i(x_{i-1}, u_i) - x_i |{\Lambda_i}^2 + \sum{k=1}^{K} | h_k(x_{i_k}, l_{j_k}) - z_k |{\Gamma_k}^2 \right} ] where (|e|{\Sigma}^2 = e^T \Sigma^{-1} e) is the squared Mahalanobis distance.

Linearizing around a good operating point (or iterative as in Gauss-Newton) yields a standard linear least-squares problem: [ \theta^* = \arg\min_{\theta} | A\theta - b |^2 ]

Solution Method

iSAM employs QR factorization of the Jacobian matrix (A) instead of Cholesky decomposition on (A^TA) to avoid explicitly computing the information matrix. [ A = Q \begin{bmatrix} R \ 0 \end{bmatrix} ] Here, (A \in \mathbb{R}^{m \times n}) (m observations, n variables), (R \in \mathbb{R}^{n \times n}) is upper-triangular with (R^T R = A^T A), and (Q \in \mathbb{R}^{m \times m}) is orthogonal.

The minimization simplifies: [ | A\theta - b |^2 = | R\theta - d |^2 + | e |^2 ] Thus, the solution is found by solving: [ R\theta^* = d ] via back-substitution.

Incremental Solving

As a full-trajectory method, the Jacobian (A) grows over time. iSAM incrementally updates the (R) matrix when new observations arrive.

Incremental Update

A new observation with Jacobian row (w^T) and residual (\gamma) augments the system: [ \begin{bmatrix} A \ w^T \end{bmatrix} \theta = \begin{bmatrix} b \ \gamma \end{bmatrix} ] Applying the existing orthogonal factor (Q) yields: [ \begin{bmatrix} R \ w^T \end{bmatrix} \theta = \begin{bmatrix} d \ \gamma \end{bmatrix} ] The new matrix is not upper-triangular due to non-zero entries in (w^T).

Incremental Factorization via Givens Rotations

Exploiting sparsity, only a few entries in the new row are non-zero. Givens rotations are applied to these entries to eliminate sub-diagonal elements, restoring the upper-triangular form of (R). A Givens rotation matrix (\Psi) is constructed for elements (\alpha = a_{kk}) and (\beta = a_{ik}): [ \Psi = (\cos\phi, \sin\phi) = \begin{cases} (1, 0), & \text{if } \beta = 0 \ \left( \frac{-\alpha}{\beta \sqrt{1 + (\alpha/\beta)^2}}, \frac{1}{\sqrt{1 + (\alpha/\beta)^2}} \right), & \text{if } |\beta| > |\alpha| \ \left( \frac{1}{\sqrt{1 + (\beta/\alpha)^2}}, \frac{-\beta}{\alpha \sqrt{1 + (\beta/\alpha)^2}} \right), & \text{otherwise} \end{cases} ] Sequential rotations zero out the sub-diagonal elements. Only rows and columns associated with the new observation (typically latest pose and observed landmarks) are modified. Back-substitution then only needs to solve for these affected variables.

Loop Closure Handling

iSAM uses periodic variable reordering (e.g., COLAMD) to prevent excessive fill-in in the (R) matrix. The order of columns in the information matrix affects the elimination order and thus the number of non-zeros in (R). Combining periodic reordering with incremental updates maintains sparsity and efficiency. Relinearization is performed during reordering steps since new Jacobians must be built anyway.

Data Association

Data association involves determining correspondences between new observations and existing landmarks.

Variable Correspondence

This is formulated as a maximum likelihoood (ML) assignment problem. A cost matrix (\Xi) for the assignment is built using Mahalanobis distance: [ \Xi = J \Sigma J^T + \Gamma ] where (\Gamma) is measurement noise, (J) is the observation Jacobian, and (\Sigma) is the covariance matrix for involved poses and landmarks. The Jonker–Volgenant–Castanon (JVC) algorithm solves this assignment problem.

Covariance Estimation

The covariance matrix (\Sigma) represents uncertainty and correlations. A full recomputation is expensive. iSAM efficiently computes only the necessary parts:

1. Marginal Covariances (Latest Pose Column Block): Let (d_x) be the pose dimension. Define selector matrix (B = [0_{(n-d_x) \times d_x}; I_{d_x \times d_x}]). The last column block (X) of (\Sigma = (R^T R)^{-1}) satisfies (R^T R X = B). This is solved via: [ R^T Y = B, \quad R X = Y ] using forward and backward substitution.
2. Conservative Landmark Uncertainty Estimates: A conservative estimate for a new landmark's uncertainty is derived from the initial uncertainty of the observing pose, as uncertainty never increases with new data: [ \tilde{\Sigma}{jj} = \bar{J} \begin{bmatrix} \Sigma{ii} & \ & \Gamma \end{bmatrix} \bar{J}^T ] where (\bar{J}) is the Jacobian of the inverse observation (back-projection) function, (\Sigma_{ii}) is the marginal covariance of the observing pose, and (\Gamma) is measurement noise.

Efficient formulas exist to extract specific covariance entries (\sigma_{ij}) corresponding to non-zero entries in (R): [ \sigma_{ll} = \frac{1}{r_{ll}} \left( \frac{1}{r_{ll}} - \sum_{j=l+1, r_{lj} \neq 0}^{n} r_{lj} \sigma_{jl} \right) ] [ \sigma_{il} = \frac{1}{r_{ii}} \left( -\sum_{j=i+1, r_{ij} \neq 0}^{l} r_{ij} \sigma_{jl} - \sum_{j=l+1, r_{ij} \neq 0}^{n} r_{ij} \sigma_{lj} \right) ] Conservative estimation is fastest, followed by this efficient extraction; full covariance recovery is (O(n^2)) and prohibitive.

Supplementary Algorithms

COLAMD (Column Approximate Minimum Degree) Ordering

COLAMD is a heuristic to find a good column permutation for sparse matrix factorization to minimize fill-in. It works by:

1. Computing the degree (number of non-zeros) for each column and row.
2. Selecting a pivot column with the smallest degree.
3. Performing symbolic elimination, updating the degrees of adjacent columns/rows.
4. Repeating steps 2-3. The final column order is used for the QR factorization in iSAM.

JVC Assignment Algorithm

The Jonker-Volgenant-Castanon algorithm solves the linear assignment problem. A Python example using scipy is shown below:

import numpy as np
from scipy.optimize import linear_sum_assignment

# Cost matrix: workers (rows) to tasks (columns)
cost_matrix = np.array([
    [9, 2, 7],
    [6, 4, 3],
    [5, 8, 1]
])

# Solve assignment
worker_idx, task_idx = linear_sum_assignment(cost_matrix)

print("Optimal Assignment:")
for w, t in zip(worker_idx, task_idx):
    print(f"  Worker {w} -> Task {t}")
print(f"Total Cost: {cost_matrix[worker_idx, task_idx].sum()}")

Output:

Optimal Assignment:
  Worker 0 -> Task 1
  Worker 1 -> Task 2
  Worker 2 -> Task 0
Total Cost: 9