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 evident 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}$ be, respectively, the measurement and the precision matrix of the relative observation between nodes $i$ and $j$.

The objective is to obtain an estimate of 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.106)

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, it is possible to define a global cost function

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

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})$ provides an estimate of the absolute poses consistent with the relative observations and their corresponding precision matrices, without explicitly introducing into the problem the individual three-dimensional points that generated those observations.