Past, Present, and Future of Simultaneous Localization And Mapping 论文阅读（三）

SLAM with camera and laser

我们描述一个装有3D激光雷达和未校正的单目相机的SLAM问题，待评估状态量 $\chi = \lbrace x_1, ... , x_t, l_t, ..., l_n, K \rbrace$，这里 $x_i$是机器人在时刻i的位姿， $l_j$是路标点， $K$是相机内参。

雷达前端提供扫描数据给ICP算法去计算两个连续雷达位置的相对运动（里程计），用里程计评估，里程计后端定义了里程计因子 $u$， 每个里程计因子 $u$，联系了相邻的两个位姿，并且响应的评估被 $z^u_i$被以下的评估模型描述：
$z_i^u = h_i^u(x_i, x_{i+1})\,\oplus \epsilon_i^u \tag1$
$h_i^u$计算相对位姿； $\epsilon_i^u$ 是协方差矩阵为 $\Omega_i^u$的零均值高斯噪声。在（1）中，标准求和被扰动取代， $\bigoplus$映射一个向量到SE(3)元素。3D位姿的流形。闭环检测被标记为c，他们评估机器人非序列位姿
$z_{ij}^c = h_{ij}^c(x_i, x_j)\oplus \epsilon_{ij}^u \tag2$
视觉前端在时间点 $i$ 从每个相机图像提取视觉特征，然后数据关联模块匹配每个特征观测到3D路标点 $l_j$。通过这些信息，后端定义了视觉因子 $v$ 每个视觉因子包含了评估 $z_{ij}^v$ ，评估模型为：
$z_{ij}^v = h_{ij}^v(x_i, l_j, K) + \epsilon_{ij}^v \tag3$
$z_{ij}$ 是时间 $i$ 时刻路标点 $l_j$在图像平面的投影评估。函数 $h_{ij}^v(.)$ 是标准的透视投影。
最后，先验因子 $p$ 编码了机器人初始位姿的先验。
$z_p = x_1 \oplus \epsilon_{ij}^v \tag4$
由于本例中相机没有先验，所以我们通常设置其先验为单位矩阵（先验使得MAP的评估是唯一的，一些类似EVO之类的评估软件应该就是通过对应单位矩阵然后在评估后面的位姿的吧）。

给定所有那些因子，SLAM后端就可以通过最小化接下来的非线性最小二乘问题评估 $\chi$：
$\chi_k^* = \underset{\chi_k}{\operatorname{min}} ||z^p \ominus x_1||_{\Omega_p }^2 + \sum_{i \in O}||z_{ij}^c\ominus h_{ij}^c(x_i, x_j)||^2_{\Omega_i^u} + \sum_{ij\in L}||z_{ij}^c \ominus h_{ij}^v(x_i,x_j)||^2_{\Omega_{ij}^c} + \sum_{ij \in P}||z_{ij}^v - h_{ij}^v(x_i, l_j,K)||_{\Omega_{{ij}}^v}^2 \tag5$
这里的 $\ominus$ 可以简单的理解为在流形SE(3) 上的差分。