Name	Name	Last commit message	Last commit date
parent directory ..
images	images
lsslam_framework	lsslam_framework
odom_calib_framework	odom_calib_framework
README.md	README.md
robotMappingSheet09.pdf	robotMappingSheet09.pdf
robotMappingSheet10.pdf	robotMappingSheet10.pdf

Intro

This folder is to fulfill the Least-square SLAM framework implemented in Sheet 10 of the course Robot Mapping

The Least-square SLAM is a graph-based SLAM algorithm whose graph comprises of robot pose at diffrenet time instance and landmarks position (Fig. 1). A node of this graph is either a robot pose or a landmark's position. An edge is a constraint between two poses or between one pose and a landmark.

Fig.1 An example of graph in graph-based SLAM

The formation of such a pose graph given the sequence of control signal $\textbf{U} = \left\{u_1, u_2, \dots \right\}$ and sequence of observation $\textbf{Z} = \left\{z_1, z_2, \dots \right\}$ is carried out by the Front-end of a graph-based SLAM algorithm, while the Back-end interprets the resulted graph into a cost function and optimize this function to find the optimal nodes position (Fig.2).

Fig.2 The interaction between two ends of graph-based SLAM

The implementation here just covers the Back-end while assume that the poses graph is already available.

Graph Optimization

Before progressing further, the state vector needs to be defined so that the target of the algorithm is clear. Since graph SLAM aims to address the full SLAM which is estimatation of robot trajectory and environment map, the state vector is made of all robot poses and location of all landmarks position.

$\textbf{x} = \left[\textbf{x}_1, \textbf{x}_2, \dots, \textbf{x}_T, m_1, m_2, \dots, m_n \right]$

Here $\textbf{x}_t$ is robot's 2D pose in world frame at time instance .

$\textbf{x}_t = \left[x_t, y_t, \theta_t \right]^T$

The map in this implementation consists of point landmarks. Therefore, a landmark is described by solely its position in is 2D location of landmark in world frame.

$m_j = \left[m_{j, x}, m_{j, y} \right]^T$

1. Cost Function Construction

An edge connecting node and node stems from the measurement $z_{ij}$ . This measurement can be produced by the motion model, the measurement model or the loop closure procedure. Since the generation of graph edges is handled by the Front-end, at this point, an edge is regarded as the homogeneous transformation from one node to another (assume node position is expressed in homogeneous coordinate). The uncertainty of measurement $z_{ij}$ is encoded in its infomation matrix $\Omega_{ij}$ .

Depend on the nature of 2 nodes that an edge connects, the constraint introduced by this edge is classified as a pose-pose constraint or pose-landmark constraint.

1.1 Pose-pose Constraint

Let $z_{ij} = \left[t_{ij}^T, \theta_{ij} \right]^T$ and $\Omega_ij$ be the measurement and the corresponding information matrix of the edge connects node $\textbf{x}_i$ and node $\textbf{x}_j$ (Fig.3).

Fig.3 Pose-pose constraint (image in coursety of Robot Mapping)

Let be the homogeneous transformation matrices represent pose of $\textbf{x}_i$ and $\textbf{x}_j$ in the world frame, respectively. $Z_{ij}$ is the transformation matrix representing $z_{ij}$ .

$\begin{matrix} X_i = \begin{bmatrix}R_i & t_i \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} \cos\theta_i & -\sin\theta_i & x_i \\ \sin\theta_i & \cos\theta_i & y_i \\ 0 & 0 & 1 \end{bmatrix}, &&& Z_{ij} = \begin{bmatrix}R_{ij} & t_{ij} \\ 0 & 1\end{bmatrix} \end{matrix}$

According to Fig.3, estimation of based on and $z_{ij}$ is

$\hat{X}_j = X_i \cdot Z_{ij}$

Let $E_{ij}$ be the transformation from $\hat{X}_j$ to ; hence,

$\hat{X}_j \cdot E_{ij} = X_j$

As a result, the error introduces by this edge is

$e_{ij} = t2v(E_{ij}) = t2v(Z^{-1}_{ij} \cdot X_i^{-1} \cdot X_j)$

Here, $t2v(\cdot)$ is the function that extracts the translation vector and the angle of rotation from a 2D transformation matrix. Expand equation above to get

$e_{ij} = \begin{bmatrix} R_{ij}^T\left[R_i^T\left(t_j - t_i \right) - t_{ij} \right] \\ \theta_j - \theta_i - \theta_{ij} \end{bmatrix}$

1.2 Pose-landmark Constraint

The constraint of an edge connecting a pose $\textbf{x}_i$ and a landmark position can be derived in the same manner as pose-pose constraint. Notice that the state of a landmark does not containt its orientation, this means the measurement $z_{ij}$ is just the translation from $\textbf{x}_i$ to . The error equation in the presvious section becomes

$e_{ij} = R_i^T\left(m_j - t_i \right) - z_{ij}$

1.3 Global Cost Function

With the error introduced by every edge in the graph being defined, the global cost function is the sum of all errors' $\Omega$ -norm.

$\textbf{F}(\textbf{x}) = \sum_{ij}e_{ij}^T\left(\textbf{x}_i, \textbf{x}_j \right) \cdot \Omega_{ij} \cdot e_{ij}\left(\textbf{x}_i, \textbf{x}_j \right)$

Rewrite $e_{ij}$ as a function of the whole state vector , meaning

$e_{ij}\left(\textbf{x}\right) = e_{ij}\left(\textbf{x}_i, \textbf{x}_j \right)$

Note that though the argument of $e_{ij}$ is changed from just two state $\textbf{x}_i$ and $\textbf{x}_j$ to the full state $\textbf{x}$ , the fact that $e_{ij}$ does not depend on the other state keeps the value of $e_{ij}$ exactly the same.

With new definition of $e_{ij}$ , the cost function becomes

$\textbf{F}(\textbf{x}) = \sum_{ij}e_{ij}^T\left(\textbf{x}\right) \cdot \Omega_{ij} \cdot e_{ij}\left(\textbf{x}\right)$

2. Cost Function Linearization

Apply Taylor expandsion on $e_{ij}\left(\hat{\textbf{x}} + \Delta \textbf{x} \right)$ with $\hat{\textbf{x}}$ is an arbitrary value of $\textbf{x}$ and $\Delta \textbf{x}$ is a small step from this value

$e_{ij}\left(\hat{\textbf{x}} + \Delta \textbf{x} \right) = e_{ij}\left(\hat{\textbf{x}}\right) + J_{ij}\Delta \textbf{x}$

Here $J_{ij}$ is the Jacobian of $e_{ij}(\textbf{x})$

$J_{ij}(\textbf{x}) = \frac{\partial e_{ij}(\textbf{x})}{\partial \textbf{x}}$

The structure of $J_{ij}$ is elaborarted in the following section. Substitue the Taylor expandsion of $e_{ij}$ into the cost function,

$\textbf{F}(\hat{\textbf{x}} + \Delta \textbf{x}) = \sum_{ij}e_{ij}^T\left(\hat{\textbf{x}}\right) \Omega_{ij} e_{ij}(\hat{\textbf{x}}) + 2 \Delta \textbf{x}^T \sum_{ij} J_{ij}^T(\hat{\textbf{x}}) \Omega_{ij} e_{ij}(\hat{\textbf{x}}) + \Delta \textbf{x}^T \left(\sum_{ij} J_{ij}^T(\hat{\textbf{x}}) \Omega_{ij} J_{ij}(\hat{\textbf{x}}) \right) \Delta \textbf{x}$

Let,

$b = \sum_{ij} J_{ij}^T(\hat{\textbf{x}}) \Omega_{ij} e_{ij}$

$H = \sum_{ij} J_{ij}^T(\hat{\textbf{x}}) \Omega_{ij} J_{ij}(\hat{\textbf{x}})$

A quadratic form emerge from the cost function

$\textbf{F}(\hat{\textbf{x}} + \Delta \textbf{x}) = \textit{const} + 2 \Delta \textbf{x}^T b + \Delta \textbf{x}^T H \Delta \textbf{x}$

For $\hat{\textbf{x}} + \Delta \textbf{x}$ to be a local minimum of $\textbf{F}$ , $\Delta \textbf{x}$ needs to satisfy the equation

$\frac{\partial \textbf{F}(\hat{\textbf{x}} + \Delta \textbf{x})}{\partial \Delta \textbf{x}} = 0$

The quation above results in the optimal step from $\hat{\textbf{x}}$

$\Delta \textbf{x} = -H^{-1} b$

To find $\Delta \textbf{x}$ and solve the graph optimization, the contribution of each edge to and need to be investigated.

2.1 Structure of Jacobian

Considering an arbitrary edge connecting node to node , the Jacobian of the error introduces by this edge $e_{ij}(\textbf{x})$ is

$J_{ij} = \frac{\partial e_{ij}(\textbf{x})}{\textbf{x}} = \left(0 \dots \frac{\partial e_{ij}\left(\textbf{x}_i, \textbf{x}_j \right)}{\partial \textbf{x}_i} \dots \frac{\partial e_{ij}\left(\textbf{x}_i, \textbf{x}_j \right)}{\partial \textbf{x}_j} \dots 0 \right)$

Let $A_{ij}, B_{ij}$ respectively denote $\frac{\partial e_{ij}\left(\textbf{x}_i, \textbf{x}_j \right)}{\partial \textbf{x}_i}$ and $\frac{\partial e_{ij}\left(\textbf{x}_i, \textbf{x}_j \right)}{\partial \textbf{x}_j}$ , $J_{ij}$ becomes

$J_{ij} = \left(0 \dots A_{ij} \dots B_{ij} \dots 0 \right)$

As can be seen from this equation, $J_{k}$ is sparse.

2.2 Consequence of Sparseness

Substitute $J_{ij}$ established above into the formula of $H_{ij}$ (an element in the sum resulting in ) and $b_{ij}$

$H_{ij} = J_{ij}^T \Omega_{ij} J_{ij} = \begin{bmatrix} 0 \\ \dots \\ A^T_{ij} \\ \vdots \\ B^T_{ij} \\ \vdots \\ 0 \end{bmatrix} \Omega_{ij} \begin{bmatrix} 0 & \dots & A_{ij} & \dots & B_{ij} & \dots & 0 \end{bmatrix} \\=\begin{bmatrix} 0 & \dots & 0 & \dots & 0 & \dots & 0 \\ \vdots & \dots & \vdots & \dots & \vdots & \dots & \vdots \\ 0 & \dots & A^T_{ij}\Omega_{ij}A_{ij} & \dots & A^T_{ij}\Omega_{ij}B_{ij} & \dots & 0 \\ \vdots & \dots & \vdots & \dots & \vdots & \dots & \vdots \\ 0 & \dots & B^T_{ij}\Omega_{ij}A_{ij} & \dots & B^T_{ij}\Omega_{ij}B_{ij} & \dots & 0 \\ \vdots & \dots & \vdots & \dots & \vdots & \dots & \vdots \\ 0 & \dots & 0 & \dots & 0 & \dots & 0 \end{bmatrix}$

$b_{ij} = J_{ij}^T \Omega_{ij} e_{ij} = \begin{bmatrix} 0 \\ \vdots \\ A^T_{ij} \\ \vdots \\ B^T_{ij} \\ \vdots \\ 0 \end{bmatrix} \Omega_{ij} e_{ij} = \begin{bmatrix} 0 \\ \vdots \\ A^T_{ij} \Omega_{ij} e_{ij} \\ \vdots \\ B^T_{ij} \Omega_{ij} e_{ij} \\ \vdots \\ 0 \end{bmatrix}$

2 equations above shows that edge connecting node to node only contributes to entries corresponding to these nodes in both $H_{ij}$ and $b_{ij}$ .

3. Building the linearized system

With the structure of the linearized cost function dervied above, this section shows how and are constructed by iterating through all edge of the pose graph.

3.1 Computing the Jacobian

Consider an arbitrary edge connecting node to node , as mentioned in Sec.1, the contribution to cost function of this edge can becomputed according to pose-pose constraint or pose-landmark constraint.

If the edge is induced by a pose-pose constraint, the corresponding Jacobian is

$A_{ij} = \frac{\partial e_{ij}(\textbf{x})}{\partial \textbf{x}_i} = \begin{bmatrix} -R_{ij}^T R_i^T & R_{ij}^T \begin{bmatrix}-s\theta_i & c\theta_i \\ -c\theta_i & -s\theta_i\end{bmatrix} \left(t_j - t_i\right) \\ 0_{1 \times 2} & -1 \end{bmatrix}$

$B_{ij} = \frac{\partial e_{ij}(\textbf{x})}{\partial \textbf{x}_j} = \begin{bmatrix} R_{ij}^T R_i^T & 0_{2 \times 1} \\ 0_{1 \times 2} & 1 \end{bmatrix}$

If the edge is induced by a pose-landmark constraint, the error's Jacobian is

$A_{ij} = \frac{\partial e_{ij}(\textbf{x})}{\partial \textbf{x}_i} = \begin{bmatrix} -R_i^T & &\begin{bmatrix}-s\theta_i & c\theta_i \\ -c\theta_i & -s\theta_i\end{bmatrix}\left(m_j - t_i\right) \end{bmatrix}$

$B_{ij} = \frac{\partial e_{ij}(\textbf{x})}{\partial m_j} = R_i^T$

3.2 Update Coefficient vector and Hessian

Because of the sparseness of the Jacobian, an edge connecting node to node only contributes to some certaint blocks in and which corresponding to index and

The coefficient vector written in block form as

$b = \begin{bmatrix}b_1^T & b_2^T & \dots & b_n^T\end{bmatrix}$