Graph-slam(三)

In GraphSLAM, the map and the path are obtained from the linearized information matrix $\Omega$ and the information vector $\epsilon$, via the equations ${\Sigma}={\Omega}^{-1}$ and ${\mu} = {\Sigma}{\epsilon}$. This operation requires us to solve a system of linear equations. This raises the question on how efficiently we can recover the map estimate ${\mu}$.
The answer to the complexity question depends on the topology of the world. If each feature is seen only locally in time, the graph represented by the constraints is linear. Thus, can be reordered so that it becomes a band-diagonal matrix,that is,all non-zero values occur near its diagonal.The equation ${\mu} = {\Omega}^{-1}{\epsilon}$ can then be computed in linear time. This intuition carries over to a cycle-free world that is traversed once,so that each feature is seen for a short,consecutive period of time.
The more common case, however, involves features that are observed multiple times, with large time delays in between. This might be the case because the robot goes back and forth through a corridor, or because the world possesses cycles. In either situation, there will exist features $m_j$ that are seen at drastically different time steps $x_{t1}$ and $x_{t2}$, with t2>> t1. In our constraint graph,this introduces a cyclic dependence: $x_{t1}$ and $x_{t2}$ are linked through the sequence of controls $u_{t1+1},u_{t1+2},...,u_{t2}$ and through the joint observation links between $x_{t1}$ and$m_j$,and$x_{t2}$ and$m_j$,respectively.Such link smake our variable reordering trick inapplicable, and recovering the map becomes more complex. In fact, since the inverse of is multiplied with a vector, the result can be computed with optimization techniques such as conjugate gradient, without explicitly computing the full inverse matrix. Since most worlds possess cycles, this is the case of interest.



The GraphSLAM algorithm now employs an important factorization trick, which we can think of as propagating information trough the information matrix (in fact, it is a generalization of the well-known variable elimination algorithm for matrix inversion). Suppose we would like to remove a feature $m_j$ from the information matrix ${\Omega}$ and the information state ${\epsilon}$. In our spring mass model, this is equivalent to removing the node and all springs attached to this node.As we shall see below, this is possible by a remarkably simple operation. we can remove all those springs between $m_j$ and the poses at which $m_j$ was observed, by introducing new springs between any pair of such poses.
This process is illustrated in Figure 3, which shows the removal of two map features, $m_1$ and $m_3$ (the removal of $m_2$ and $m_4$ is trivial in this example). In both cases, the feature removal modifies the link between any pair of poses from which a feature was originally observed.As illustrated in Figure 3(b), this operation may lead to the introduction of new links in the graph. In the example shown there, the removal of $m_3$ leads to a new link between $x_2$ and $x_4$.