Graph-slam(二)

1.3 Building Up the Graph
Suppose we are given a set of measurements $z_{1:t}$ with associated correspondence variables $c_{1:t}$, and a set of controls $u_{1:t}$. GraphSLAM turns this data into a graph. The nodes of this graph are the robot poses $x_{1:t}$ and the features in the map m =${m_j}$. Each edge in the graph corresponds to an event: a motion event generates an edge between two robot poses, and a measurement event creates a link between a pose and a feature in the map. Edges represent soft constraints between poses and features in GraphSLAM.
For a linear system, these constraints are equivalent to entries in an information matrix and an information vector of a large system of equations. As usual, we will denote the information matrix by $\Omega$ and the information vector by $\epsilon$. As we shall see below, each measurement and each control leads to a local update of $\Omega$ and $\epsilon$, which corresponds to a local addition of an edge to the graph in GraphSLAM. In fact, the rule for incorporating a control or a measurement into $\Omega$ and $\epsilon$ is a local addition, paying tribute to the important fact that information is an additive quantity.
Figure 2 illustrates the process of constructing the graph along with the corresponding information matrix. First consider a measurement ${z_i}^t$. This measurement provides information between the location of the feature $j = {c_i}^t$ and the robot pose $x_t$ at time t. In GraphSLAM, this information is mapped into a constraint between ${x_t}$ and $m_j$.We can think of this edge as a (possibly degenerate) “spring” in a spring-mass model. As we shall see below, the constraint is of the type:

$${({{z_t}^i}-h(x_t,m_j,i))}^T{{Q_t}^{-1}}({{z_t}^i}-h(x_t,m_j,i))$$
Here h is the measurement function, and $Q_t$ is the covariance of the measurement noise. Figure 2(a) shows the addition of such a link into the graph maintained by GraphSLAM. Note that the constraint may be degenerate, that is, it may not constrain all dimensions of the robot pose $x_t$. This will be of no concern for the material yet to come.
In information form, the constraint is incorporated into $\Omega$ and $\epsilon$ by adding values between the rows and columns connecting $x_{t−1}$ and $x_t$. The magnitude of these values corresponds to the stiffness of the constraint, as governed by the uncertainty covariance $Q_t$ of the motion model. This is illustrated in Figure 2(b), which shows the link between two robot poses along with the corresponding element in the information matrix.
Now consider robot motion. The control $u_t$ provides information about the relative value of the robot pose at time t −1 and the pose at time t. Again, this information induces a constraint in the graph, which will be of the form:
$${({{x_t}}-g(u_t,x_{t-1}))}^T{{R_t}^{-1}}({{x_t}}-g(u_t,x_{t-1}))$$
Here g is the kinematic motion model of the robot, and $R_t$ is the covariance of the motion noise. Figure 2(b) illustrates the addition of such a link in the graph. It also shows the addition of a new element in the information matrix, between the pose $x_t$ and the measurement ${z_t}^i$. This update is again additive. As before, the magnitude of these values reflects the residual uncertainty $R_t$ due to the measurement noise; the less noisy the sensor, the larger the value added to $\Omega$ and $\epsilon$. After incorporating all measurements $z_{1:t}$ and controls $u_{1:t}$, we obtain a sparse graph of soft constraints. The number of constraints in the graph is linear in the time elapsed, hence the graph is sparse. The sum of all constraints in the graph will be of the form:
$$J_{GraphSLAM}={{x_0}^T}{{\Omega}_0}{x_0} + {\sum_{t}}{(x_t-g(u_t,x_{t-1}))}^T{{R_t}^{-1}}{(x_t-g(u_t,x_{t-1}))} + {\sum_{t}}{\sum_{i}}{({{z_t}^i}-h(y_t,{c_t}^i,i))}^T{{Q_t}^{-1}}({{z_t}^i}-h(y_t,{c_t}^i,i))$$
It is a function defined over pose variables $x_{1:t}$ and all feature locations in the map m. Notice that this expression also features an anchoring constraint of the form ${{x_0}^T}{{\Omega}_0}{x_0}$. This constraint anchors the absolute coordinates of the map by initializing the very first pose of the robot as $(0,0,0)^T$ .
In the associated information matrix , the off-diagonal elements are all zero with two exceptions: between any two consecutive poses $x_{t−1}$ and $x_t$ will be a non-zero value that represents the information link introduced by the control $u_t$. Also non-zero will be any element between a map feature $m_j$ and a pose $x_t$, if $m_j$ was observed when the robot was at $x_t$.All elements between pairs of different features remain zero.This reflects the fact that we never receive information pertaining to their relative location—all we receive in SLAM are measurements that constrain the location of a feature relative to a robot pose. Thus, the information matrix is equally sparse; all but a linear number of its elements are zero.