Core Concepts of iSAM

Paper: "iSAM: Incremental Smoothing and Mapping" The incremental optimizer GTSAM employs an iSAM-based approach that enables full-trajectory SLAM optimization through incremental updates.

iSAM is an incremental smoothing and mapping method derived from "Square Root SAM Simultaneous Localization and Mapping via Square Root Information Smoothing". It represents a full-trajectory SLAM technique based on incremental SAM principles.

Main Concept

The fundamental idea of iSAM revolves around landmark-based SLAM using incremental SAM techniques. It linearizes nonlinear problems into Ax=b form, then performs factorization to convert them into Rx=d form where R is an upper triangular matrix, enabling back-substitution for solution computation.

Key aspects include:

• Utilizing block ordering methods (based on colamd for block element sorting, treating poses and points as single blocks) to accelerate factorization.
• For new measurements, augmenting R matrix and applying Givens rotations to restore upper triangular form for continued back-substitution.
• Periodic variable reordering combined with incremental SAM to maintain sparsity and computational efficiency over time.

Problem Statement

Consider an SLAM system represented by a Bayesian belief network as illustrated below:

In this diagram, the Bayesian network representation of the SLAM problem shows:

• $ x_i $: robot state (pose) at time i,
• $ l_j $: landmark coordinates at index j,
• $ u_i $: control input at time i,
• $ z_k $: observation of landmark k.

Variables defined as: $$ X = {x_i}, i \in 0,\dots,M; \ L = {l_j}, j \in 1,\dots,N; \ Z = {z_k}, k \in 1,\dots,K. $$

Joint probability distribution: $$ 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) $: prior on initial state,
• $ P(x_i|x_{i-1},u_i) $: motion model parameterized by control input,
• $ P(z_k|x_{i_k},l_{j_k}) $: landmark measurement model.

Both transition and observation equations follow Gaussian models: $$ x_i = f_i(x_{i-1}, u_i) + w_i \ z_k = h_k(x_{i_k}, l_{j_k}) + v_k $$

Using maximum a posteriori estimation (MAP), we solve for robot poses and landmark coordinates. This leads to a least squares formulation: $$ \theta^* = \arg\min_{\theta} |A\theta - b|^2 $$

Solution Approach

iSAM solves the above using QR decomposition of Jacobian matrix A.

Why not Cholesky? Cholesky decomposes information matrix $ A^T A $, which although smaller in size and faster to compute, requires explicit calculation of $ A^T A $. QR directly operates on A instead.

$$ A = Q \begin{bmatrix} R \ 0 \end{bmatrix} $$

With $ A \in \mathbb{R}^{m \times n} $, rows represent observations, columns represent unknowns.

The resulting upper triangular matrix $ R \in \mathbb{R}^{n \times n} $ satisfies $ R^T R = A^T A $, and orthogonal matrix $ Q \in \mathbb{R}^{m \times m} $. Thus, the problem becomes: $$ |A\theta - b|^2 \Rightarrow |\begin{bmatrix} R \ 0 \end{bmatrix} \theta - \begin{bmatrix} d \ e \end{bmatrix} |^2 = |R\theta - d|^2 + |e|^2 $$

Therefore: $$ R\theta^* = d $$

Back-substitution can then be applied to solve for variables.

Incremental Updates

Since SAM processes all data without marginalization, matrix A grows continuously. iSAM introduces incremental update techniques for R matrix. When new measurements arrive, only relevant rows/columns of R are updated, followed by Givens rotations to re-triangulate the matrix.

Incremental Update

Adding a new measurement results in: $$ \begin{bmatrix} Q^T & \ & 1 \end{bmatrix} \begin{bmatrix} A \ w^T \end{bmatrix} = \begin{bmatrix} R \ w^T \end{bmatrix}, \quad \text{new RHS: } \begin{bmatrix} d \ \gamma \end{bmatrix} $$

This yields: $$ \begin{bmatrix} R \ w^T \end{bmatrix} \theta = \begin{bmatrix} d \ \gamma \end{bmatrix} $$

Incremental Factorization Using Givens Rotations

Due to SLAM's sparsity, newly added rows relate only to observed landmarks and current poses. Few Givens rotations suffice to zero out lower triangular elements.

Let $ a_{ik} = x $. Choose Givens rotation: $$ \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} $$

Where $ \alpha := a_{kk}, \beta := a_{ik} $

After one rotation, further elements like $ a_{ik+1} $ are cleared using additional rotations until complete upper triangulation.

Assuming landmark variables precede pose variables in storage order, re-factorization affects only those related to the new measurement — landmark diagonal blocks and last 6D pose column.

Back-substitution then solves only affected variables.

Note: Variable reordering should place landmarks first, then poses.

iSAM primarily uses landmark-based SLAM, so landmark assignments are handled probabilistically.

$$ \begin{bmatrix} Q^T & \ & 1 \end{bmatrix} \begin{bmatrix} A \ w^T \end{bmatrix} = \begin{bmatrix} R \ w^T \end{bmatrix} $$

Sparsity allows efficient zeroing of fill-in elements via Givens rotations.

Loop Closure Handling

iSAM mitigates fill-in by variable reordering using heuristic COLAMD algorithm, which finds optimal block ordering. Column order impacts elimination sequence and thus number of nonzeros in R matrix.

Combining periodic reordering with incremental updates maintains sparsity and improves efficiency.

As shown in diagrams, these two strategies together significantly enhance matrix sparsity and reduce computation time.

Additionally, relinearization combined with periodic reordering reduces fill-in. Measurements require recomputation of Jacobians regardless.

Data Association

Data association involves:

1. Matching new poses with existing landmarks using JVC.
2. Computing and updating covariances between poses and landmarks for Mahalanobis distance calculation, forming assignment problem.

Assignment Problem

This is solved using Maximum Likelihood (ML) formulation: $$ \Xi = J\Sigma J^T + \Gamma $$

Where:

• $ \Gamma $: measurement noise,
• $ J $: Jacobian of observation function,
• $ \Sigma $: covariacne matrix of pose and landmark variables.

Jonker–Volgenant–Castanon (JVC) algorithm assigns each observation to corresponding landmark.

Covariance Computation

Covariance matrices reflect uncertainty and correlations among variables:

• Diagonal entries: individual uncertainties,
• Off-diagonal entries: correlations.

$$ \Sigma = \begin{bmatrix} \Sigma_{jj} & \Sigma_{ij}^T \ \Sigma_{ij} & \Sigma_{ii} \end{bmatrix} $$

New measurements typically interact with only a few landmarks and current pose. Therefore, only relevant covariance sub-blocks need updating.

Current relationships involve specific covariance entries as illustrated. For instance, recent pose $ x_2 $ and landmarks $ I_1 $ and $ I_3 $ have marginal covariance.

Assuming landmarks added first, followed by optional reordering, then next pose:

Due to symmetry, only diagonal triangle and rightmost column need computation.

Covariance computation splits into two parts:

1. Marginal Covariances: Last column block extracted from inverse information matrix via back-substitution.
2. Landmark Uncertainties: Conservative estimates from initial values and iterative updates. iSAM uses only initial estimates for landmarks.

Marginal Covariances Extraction

Given $ (R^T R)^{-1} = \Sigma $, extract last column block $ X $: $$ B = \begin{bmatrix} 0_{(n-d_x)\times d_x} \ I_{d_x\times d_x} \end{bmatrix} \Rightarrow R^T R X = B \Rightarrow R^T Y = B, RX = Y $$

Solve via forward-backward substitution.

Conservative Estimates

Initial conservative estimates stem from initial uncertainties. As uncertainty never increases with new measurements, initial values provide safe bounds.

Initial estimate: $$ \tilde{\Sigma}{jj} = \bar{J} \begin{bmatrix} \Sigma{ii} & \ & \Gamma \end{bmatrix} \bar{J}^T $$

Where:

• $ \hat{J} $: Jacobian of inverse projection function,
• $ \Sigma_{ii} $: previous pose uncertainty,
• $ \Gamma $: noise.

Exact Covariance Recovery

From $ R^T R \Sigma = I $, solve: $$ R^T Y = I, R \Sigma = Y $$

Apply forward-backward substitution for desired covariance entries.

However, since covariance is dense, direct computation is $ O(n^2) $. Efficiently recover only needed blocks using recursive formulas: $$ \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) $$

Comparisons show conservative estimates are fastest, followed by efficient extraction, and full recovery is computationally expensive.

Supplementary Knowledge

COLAMD Sorting Algorithm

COLAMD (Column Approximate Minimum Degree) optimizes sparse matrix factorizations by minimizing fill-in during LU decomposition.

Principle

COLAMD minimizes fill-in by reordering matrix columns and rows to reduce new nonzeros generated during factorization.

Steps

1. Compute column and row degrees.
2. Initialize with degrees calculated.
3. Select pivot column with minimum column degree; break ties with row degree.
4. Eliminate selected column to minimize future fill-in.
5. Update degrees of involved rows and columns.
6. Repeat steps 3–5 until all columns processed.
7. Final column ordering used for factorization.

By choosing optimal column orders, COLAMD improves LU decomposition performance and memory usage, especially for large sparse matrices.

JVC Assignment

Jonker-Volgenant-Castanon (JVC) algorithm efficiently solves assignment problems by optimizing cost matrices to find minimal total cost assignments.

Algorithm Steps:

1. Initialize cost matrix (C).
2. Row reduction: subtract row minima to make row zeros.
3. Column reduction: subtract column minima to make column zeros.
4. Mark zeros: cover all zeros with minimum lines.
5. Adjust matrix:

• If line count equals dimensions, optimal solution found.
• Else adjust matrix to generate more zeros, repeat step 4.

Example: Three tasks, three workers:

	Task 1	Task 2	Task 3
Worker 1	9	2	7
Worker 2	6	4	3
Worker 3	5	8	1

Python Implementation:

import numpy as np
from scipy.optimize import linear_sum_assignment

C = np.array([[9, 2, 7],
              [6, 4, 3],
              [5, 8, 1]])

row_ind, col_ind = linear_sum_assignment(C)

print("Assignment:")
for i, j in zip(row_ind, col_ind):
    print(f"Worker {i+1} -> Task {j+1}")

print("Minimum cost:", C[row_ind, col_ind].sum())

Result:

Assignment:
Worker 1 -> Task 2
Worker 2 -> Task 3
Worker 3 -> Task 1
Minimum cost: 9

This approach efficiently solves larger assignment problems.