Pose Graph Optimization

An alternative to Bundle Adjustment that introduces fewer variables is Pose Graph Optimization (GKSB10). In this case, the three-dimensional points are not explicitly included among the problem variables, and only the poses of the different frames, or nodes in the sequence, are optimized.

The advantage of this formulation is particularly clear when relative observations are available between nonconsecutive poses, for example in the presence of a loop closure. The same scene configuration may be observed again after a certain number of frames, providing an additional relative constraint that can be used to reduce the drift accumulated by odometry.

Let

$$\mathbf{x} = \left( \mathbf{x}_1,\ldots,\mathbf{x}_n \right)$$

be a parameter vector, where element $\mathbf{x}_i$ represents the pose of node $i$-th. Let $\mathbf{z}_{ij}$ and $\boldsymbol\Omega_{ij}$ denote, respectively, the measurement and the precision matrix of the relative observation between nodes $i$ and $j$.

The objective is to estimate the parameters $\mathbf {x}$ from the relative observations $\mathbf{z}_{ij}$. Since the relative pose between two nodes can be obtained by comparing their respective absolute poses, the error can be defined as

$$\mathbf{e}_{ij} = \mathbf{e}_{ij} \left( \mathbf{x}_i,\mathb... ...hat{\mathbf{z}}_{ij} \left( \mathbf{x}_i,\mathbf{x}_j \right),$$ (10.103)

where $\hat{\mathbf{z}}_{ij}$ represents the relative pose predicted from the configurations $\mathbf{x}_i$ and $\mathbf{x}_j$ of the two nodes.

Using the precision information associated with each relative observation, a global cost function can be defined:

$$F(\mathbf{x}) = \sum_{\langle i,j\rangle} \mathbf{e}_{ij}^{\top} \boldsymbol\Omega_{ij} \mathbf{e}_{ij}.$$ (10.104)

The function $F(\mathbf{x})$ is therefore the sum of the Mahalanobis distances associated with the available relative observations between pairs of nodes.

Minimizing $F(\mathbf{x})$ yields an estimate of the absolute poses that is consistent with the relative observations and their respective precision matrices, without explicitly introducing into the problem the individual three-dimensional points that generated those observations.