Cartographer¶

Concepts:¶

1. Scan: Lidar pointcloud. This is projected on horizontal plane

2. Submap: The world is represented by multiple 2D occupancy grid. A continuos chunk of 3D volume of specific length in the world around the robot motion is converted into a 2D occupancy grid which contains the probability of the grid cell being occupied. This is used to represent the 3D world from hereafter.

Steps:¶

Local refinement (Scan Matching)¶

Each consecutive scan is matched against a small chunk of the world, called a submap M, using a non-linear optimization that aligns the scan with the submap; this process is further referred to as scan matching. Scan matching accumulates error over time.

Scan pose is optimized relative to current local submap using ceres. Its a nonlinear least squares problem:

where scan has K points, $h_{k}$ is the kth point, $\xi$ is the pose between scan and submap, $T_{\xi}$ transforms $h_{k}$ from the scan frame to the submap frame according to the scan pose. The function Msmooth is a smooth version of the probability values in the local submap.

A few consecutive scans are used to build a submap. These submaps take the form of probability grids M : rZ × rZ → [pmin, pmax] which map from discrete grid points at a given resolution r, for example 5 cm, to values. These values can be thought of as the probability that a grid point is obstructed.

Adding scan to submap: When a scan arrives and is to be added to the submap the discrete points in the scan are transformed into submap frame and then hits and misses are calculated. Its just a way to calculate the probability of how probable is it that a grid point is occupied after this scan. Unobserved cells with hits and misses get p_hit and p_miss. Observed ones get their probability (hits and misses) updated like:

Loop closing¶

Local matching works for small spaces. Larger spaces are handled by creating many small sub-maps and doing a global optimization using loop closures.

Getting a valid loop closure candidate:

1. A region around the current global estimate (say a 7m radius) is taken in consideration. All scans and submaps in this region become valid pairs for loop closing pipeline (given the submap no longer changes).

2. A intial guess for the pose between each eligible pair (scan, finished submap) is generated by discretizing delta (translation and rotation wiggle needed around relative pose) around relative pose (say +-7m, +-30 degrees), transform the scan with delta, scoring in the submap and using the highest scored scan to submap matching candidate as the initial guess. Branch and bound is just a optimization to convert the above idea from a grid search over all possible translation and rotation pairs to a computationally efficient algorithm.

For each eligible pair (scan, finished submap):

1. Define a search window around the predicted relative pose (from odometry graph till now). They have an initial guess from the current global estimate of poses. Around that, search over discrete rotation samples and a 2D translation window (grid). This leads to a finite set $W$ forming a search window around an estimate $\xi_0$ (relative pose) placed in its center,

Think of $\bar{W}$ as the coordinates and W has its real world pose equivalent. Here $\xi_0 + (r j_x, r j_y, \delta\theta j_\theta)$ starts from initial guess xi0, then offsets it by rjx, rjy and del_theta*jtheta in rotation.

2. Score candidates with correlative scan matching. Each candidate pose gets a score like: a. transform scan points hk into submap frame b. look up occupancy/probability values in a grid c. sum them up

To reduce the computational complexity of doing an exhaustive grid search over possible initial guesses, branch-and-bound is used. Branch-and-bound is used to find the global best candidate in that window while pruning. The main idea is to represent subsets of possibilities in grid search as nodes in a tree where the root node represents all possible solutions, W in our case. The children of each node form a partition of their parent (every child level breaks the parent partition in at max 4 partitions), so that they together represent the same set of possibilities. The leaf nodes are singletons; each represents a single feasible solution. Branch and bound requires score(c) of inner nodes c is an upper bound on the score of its elements to work.

At coarse resolution (higher “height”), each node represents many translations. There is not one root. The tree starts by grouping the grid of translations and rotations into a few non overlapping nodes. So one node translation has [xi, xj], next node has [xj+1, xk] and so on. All these nodes reside at one level or height of tree. The score of each node is the upper bound on the score you can get from that subtree. This way if the score of a node is smaller than the best score found till now, it can be safely pruned or we dont check those grid of rotations and translations for initial guess. The larger problem to find the initial guess is:

• ($h_k$) = scan points (in scan frame)

• ($T_\xi$) = rigid transform for pose ($\xi$)

• ($M_{\text{nearest}}(\cdot)$) = look up the submap grid value at the nearest cell (occupancy/probability)

Each B&B node is an integer tuple:

where:

• ($c_\theta$) is a fixed rotation index (one discrete angle)

• ($c_x,c_y$) define translation blocks start x and y indices,

• ($c_h$) is the “height” (block size scale) For a node the above tuple can be one reference translation and rotation mostly top left corner in translation.

A node represents a subset of translation indices (but one rotation):

(the above equation can be read as the set of all index triples (jx, jy, jtheta) from the grid search such that jx ranges over a consecutive block of size 2^ch starting at cx, jy ranges over a consecutive block of size 2^ch, starting at cy and jtheta is fixed to exactly ctheta. 2^ch is the block width and height hence this node covers 2^ch x 2^ch translation points.) and they intersect it with the overall window:

$\bar{W_c}$ - refined grid coordinates within node c (only the ones that are in \bar{W} as well)

$\bar{\bar{W_c}}$ - raw grid coordinates within node c

$\bar{W}$ - all grid coordinates

So: at height ($c_h$), a node covers up to ($2^{c_h}\times 2^{c_h}$) translation candidates for a single angle.

Leaf nodes: A leaf has ($c_h=0$) and corresponds to exactly one pose:

Root node: They don’t explicitly store the root node; instead they start with a set ($C_0$) at some height ($h_0$) that tiles the window:

Interpretation: this makes a coarse grid of “blocks” covering the full window, for every discrete rotation. Build an initial list of nodes at height h0 whose translation starts (cx,cy) step in increments of 2^h0 to tile the whole translation window, and include all rotation indices.”

The B&B algo has three stages: node selection, branching, and computation of upper bounds. Node selection: The tree (representing grid search) is searched using DFS. To find which children is visited next from a node during the DFS, we compute the upper bound on the score for each child, visiting the most promising child node with the largest bound first. Branching rule: For a node with ($c_h>1$), they split into up to four children (quadrants in translation), keeping the same rotation index:

A node at height ch is split into four children at height ch−1, with the same rotation cθ, and with translation starts at: (cx,cy) (cx+2^ch−1,cy) (cx,cy+2^ch−1) (cx+2^ch−1,cy+2^ch−1) then clip to the window.

To prune, each node needs a number ($\text{score}(c)$) such that:

They propose an upper bound that’s computed scan-point-wise:

Why this is an upper bound:

• the true best pose ($j^*$) must pick one ($j$) for all points,

• but (15) allows each point ($h_k$) to “cheat” and pick its own best ($j$),

• so it can only be greater or equal than the true best achievable by one single pose.

That’s what makes pruning safe.

Precomputing max-grids

Computing ($\max_{j\in W_c}$) directly would still be expensive. Their trick is: for each finished submap, precompute grids ($M^{h}_{\text{precomp}}$) that already store the maximum value over a ($2^h \times 2^h$) pixel box.

Then they can bound a node simply by:

And the precomputed grid is defined as:

At one specific node, the scan is hence transformed according to the nodes anchor pose ${\xi_c}$ which is the starting indices for x, y and fixed theta. This gives the place in submap where for each point in scan we get a reference point in submap. The precomputed M then has for each grid cell the maximum probability spanning grid cell to grid cell r(2^h-1) apart in space. This is the span of this node and hence the probability serves as the upper bound since for initial guess we would have some points that doesnt coincide with maximum probablity.

Note that Mprecomp has the same pixel structure as Mnearest, but in each pixel storing the maximum of the values of the 2h × 2h box of pixels beginning there.

Interpretation:

• Same pixel layout as the original grid,

• but each pixel stores the max value in the ($2^h\times 2^h$) block starting there.

Final Algorithm: They run a depth-first style B&B (Algorithm 3):

• Keep best_score initialized to a score threshold (they only care about matches above some minimum).

• Maintain a stack of nodes, sorted by node bound (largest bound first).

• Pop the current best-bound node:

  • If its bound ≤ best_score, prune it (“Bound”).

  • If it’s a leaf and its score is better, update the best match.

  • Else, split into children, compute bounds for children, push them, continue. This is why it’s fast:

• good matches raise best_score,

• which causes many nodes to fail the bound test and get pruned.

Using the above candidate, a nonlinear least squres problem is formed thats solved using ceres:

where the submap poses Ξm = {$\xi^{m}_{i}$ }i=1,...,m and the scan poses Ξs = {$\xi^{s}_{j}$} j=1,...,n in the world are optimized given some constraints. These constraints take the form of relative poses $\xi_{ij}$ and associated covariance matrices $\Sigma_{ij}$ . For a pair of submap i and scan j, the pose $\xi_{ij}$ describes where in the submap coordinate frame the scan was matched. The covariance matrices $\Sigma_{ij}$ can be evaluated locally using the covariance estimation feature of Ceres. $\xi_{ij}$ are calculated using two sources, one is the rough estimate we get from branch and bound scan matching other is the relative pose according to global pose data (odometry) The residual E for such a constraint is computed by

A loss function ρ, for example Huber loss, is used to reduce the influence of outlier. This gives the final poses of the scans and submaps which refines the graph to high level of accuracy.