>

## Graph-based SLAM

### 图优化（后端）

• 通过相邻点之间的位置关系计算出 $T_{b,c,...,f}$
• 通过回环检测构建出来的边： $T_{bf}$

## 非线性最小二乘原理

### 问题引入

• $x$ 为该系统的状态向量，也是我们重点关注需要估计的值
• $z$ 为该系统的观测值，通常由传感器可以进行直接观测获得
• $f(x)$ 为该系统的关于状态向量 $x$ 的一组映射非线性函数，也称为观测函数，表示 $x$$z$ 之间的对应关系

\begin{aligned} f_1(\mathbf{x}) &= \mathbf{z}_1'\\ f_2(\mathbf{x}) &= \mathbf{z}_2'\\ ...\\ f_n(\mathbf{x}) &= \mathbf{z}_n' \end{aligned}\rightarrow \begin{aligned} \mathbf{z}_1\\ \mathbf{z}_2\\ ...\\ \mathbf{z}_n \end{aligned}


### 误差函数和目标函数

$$e_i(\mathbf{x}) = f_i(\mathbf{x}) - z_i$$


$$E_i(\mathbf{x}) = e_i(\mathbf{x})^T\Sigma^{-1}_ie_i(\mathbf{x})$$


$$F(\mathbf{x}) = \sum_i^N E_i(\mathbf{x}) = \sum_i^N e_i(\mathbf{x})^T\Sigma^{-1}_ie_i(\mathbf{x})$$


### 求解方法

#### 方程线性化

$$e_i(\mathbf{x} + \Delta\mathbf{x}) = e_i(\mathbf{x}) + J_i(\mathbf{x})\Delta\mathbf{x}$$


$$J_i(\mathbf{x}) = (\frac{\partial f_i(\mathbf{x})}{\partial x_1}, \frac{\partial f_i(\mathbf{x})}{\partial x_2}, ..., \frac{\partial f_i(\mathbf{x})}{\partial x_n})$$


$$F(\mathbf{x} + \Delta \mathbf{x}) = \sum e_i(\mathbf{x} + \Delta \mathbf{x})^T\Sigma^{-1}_i e_i(\mathbf{x} + \Delta \mathbf{x})$$


#### 化简求解

\begin{aligned} F(\mathbf{x} + \Delta \mathbf{x}) &= \sum e_i(\mathbf{x} + \Delta \mathbf{x})^T\Sigma^{-1}_i e_i(\mathbf{x} + \Delta \mathbf{x}) \\ &= \sum (e_i(\mathbf{x}) + J_i \Delta \mathbf{x})^T \Sigma^{-1}_i (e_i(\mathbf{x}) + J_i\Delta \mathbf{x}) \\ &= \sum e_i^T\Sigma^{-1}_ie_i + e_i^T\Sigma^{-1}_iJ_i\Delta \mathbf{x} + \Delta \mathbf{x}^T J_i^T\Sigma^{-1}_i e_i + \Delta \mathbf{x}^TJ_i^T\Sigma^{-1}_iJ_i\Delta \mathbf{x} \\ &= \sum e_i^T\Sigma^{-1}_ie_i + 2e_i^T\Sigma^{-1}_iJ_i\Delta \mathbf{x} + \Delta \mathbf{x}^TJ_i^T\Sigma^{-1}_iJ_i\Delta \mathbf{x} \\ &= c_i + 2b_i^T\Delta \mathbf{x} + \Delta \mathbf{x}^T H_i \Delta \mathbf{x} \end{aligned}


\begin{aligned} \frac{\partial F(\mathbf{x} + \Delta \mathbf{x})}{\partial \Delta \mathbf{x}} &= 2b + (H + H^T)\Delta \mathbf{x} = 0 \\ &= 2b + (J_i^T\Sigma^{-1}_iJ_i + J_i\Sigma^{-1}_i^TJ_i^T)\Delta \mathbf{x} = 0 \\ &= 2b + 2H\Delta \mathbf{x} = 0 \\ H\Delta \mathbf{x} &= -b \\ \Delta \mathbf{x}^* &= -H^{-1}b \end{aligned}


#### 总结

• 线性化误差函数： $e_i(\mathbf{x} + \Delta \mathbf{x}) = e_i(\mathbf{x}) + J_i\Delta \mathbf{x}$
• 构建线性系统：
• $b^T = \sum e_i^T\Sigma^{-1}_iJ_i$
• $H = \sum J_i^T\Sigma^{-1}_iJ_i$
• $H\Delta \mathbf{x} = b$
• 求解线性系统：$\Delta \mathbf{x}^* = -H^{-1}b$
• 更新解直至收敛：$\mathbf{x} = \mathbf{x} + \Delta \mathbf{x}^*$

## 非线性最小二乘在 SLAM 中的应用

### 图的构建

• 相邻节点之间的边通过里程计测量构建，这里的里程计可以是轮式里程计或者帧间匹配里程计（激光里程计），在视觉 SLAM 中也可以是视觉里程计；
• 不相邻节点之间的边要通过回环检测来构建，节点 i 和节点 j 观测到同样的环境信息,两者进行匹配得到相对位姿。并在对应的节点中连一条边,边为匹配的相对位姿。用信息矩阵来描述本次匹配的可靠性。

### 误差函数

• 预测观测值 $Z'_{ij}$：通过轮式里程计得到的相对位置关系可以作为相邻节点之间的预测值
• 实际观测值 $Z_{ij}$：通过两帧之间的帧间匹配得到的相对位姿可以作为实际观测值
• 因此误差函数 $e_{ij}(\mathbf{x}) = t2v(Z^{-1}Z')$, $t2v$ 在这里表示位姿矩阵转换为位姿向量

$$e_{ij}(\mathbf{x}) = \begin{bmatrix} R_{ij}^T(R_i^T(t_j - t_i) - t_{ij}) \\ \theta_j - \theta_i - \theta_{ij} \end{bmatrix}$$


\begin{aligned} \frac{\partial e_{ij}(\mathbf{x})}{\partial x_i} &= \begin{bmatrix} -R_{ij}^TR_i^T & R_{ij}^T\frac{\partial R_i^T}{\partial \theta}(t_j - t_i) \\ 0 & -1 \end{bmatrix} \\ \frac{\partial e_{ij}(\mathbf{x})}{\partial x_j} &= \begin{bmatrix} R_{ij}^TR_i^T & 0 \\ 0 & 1 \end{bmatrix} \end{aligned}


### 线性化

\begin{aligned} e_{ij}(\mathbf{x} + \Delta \mathbf{x}) &= e_{ij}(\mathbf{x}) + J_{ij}\Delta \mathbf{x} \\ J_{ij} = \frac{\partial e_{ij}(\mathbf{x})}{\partial \mathbf{x}} \end{aligned}


\begin{aligned} \frac{\partial e_{ij}(\mathbf{x})}{\partial \mathbf{x}} &= (0 ...\frac{\partial e_{ij}(\mathbf{x})}{\partial \mathbf{x}_i} ... \frac{\partial e_{ij}(\mathbf{x})}{\partial \mathbf{x}_j} ... 0) \\ J_{ij} &= (\mathbf{0} ... \mathbf{A}_{ij} ... \mathbf{B}_{ij} ... \mathbf{0}) \end{aligned}


### 坐标系固定

$$\begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \Delta x = \begin{bmatrix} 0 \\ 0 \\ 0 \\ \end{bmatrix}$$


$$H_{11} += \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}$$


### 构建线性系统

\begin{aligned} b_i^T &+= e_{ij}^T\Omega_{ij}A_{ij} \\ b_j^T &+= e_{ij}^T\Omega_{ij}B_{ij} \end{aligned}


\begin{aligned} H_{ii} += A_{ij}^T\Omega_{ij}A_{ij} \\ H_{ij} += A_{ij}^T\Omega_{ij}B_{ij} \\ H_{ji} += B_{ij}^T\Omega_{ij}A_{ij} \\ H_{jj} += B_{ij}^T\Omega_{ij}B_{ij} \end{aligned}


### 求解

• 已知矩阵 H 和 向量 b
• 求解 $H\Delta x = -b$，这里要利用稀疏矩阵的特殊性进行矩阵，不要直接求逆
• 进行迭代，直至收敛

## 参考资料

Built with Hugo
Theme Stack designed by Jimmy