论文阅读《UV-SLAM: Unconstrained Line-Based SLAM Using Vanishing Points for Structural Mapping》R-AL 2022

Motivation

使用线特征的SLAM方法主要使用线特征的重投影模型，但由于线特征的投影只与Plücker表示中的normal vector有关而与direction vector无关，直线的方向无法被修正，难以解决线特征的退化问题。

本文提出了利用灭点进行结构化建图的无约束的线特征SLAM.重点是不使用任何结构化假设，如Manhattan world假设。算法从线特征中提取灭点，将当前帧中的灭点观测和通过direction vector计算的灭点估计的差异作为BA的残差。通过使用Fisher信息矩阵秩分析，证明了灭点测量约束可以得到唯一的建图结果。

Contribution

1. 第一个使用灭点测量进行结构化建图的单目SLAM系统，不受相机运动和环境的限制，且不适用结构化假设，如Manhattan world假设。
2. 基于3D直线的Plücker表示和正交表示，定义了vanishing point测量的残差和雅可比形式。
3. 通过Fisher信息矩阵秩分析(FIM)，证明了提出的方法保证了3D线特征的可观性。

Methodology

算法框架

采用LSD提取直线，LBD匹配直线

采用Plücker表示$\mathbf{L}(\mathbf{n},\mathbf{d}{)}^{\top }\in {\mathbb{R}}^6$进行直线的三角化和重投影

采用正交表示$\mathbf{o}=[\mathbf{\psi },\varphi ]$用于后端优化，其中$\psi$是3D直线相对相机坐标系的旋转的欧拉角表示，$\varphi$是相机中心到线的最小距离

使用J-linkage算法对直线进行聚类，判断每一直线是否具有结构化约束

优化状态

待估计的变量如下
$$\begin{array}{rl}\mathcal{X}=& [{\mathbf{x}}_0,{\mathbf{x}}_1,\dots ,{\mathbf{x}}_{I-1},\\ & {\lambda }_0,{\lambda }_1,\dots ,{\lambda }_{J-1},\\ & {\mathbf{o}}_0,{\mathbf{o}}_1,\dots ,{\mathbf{o}}_{K-1}],\\ {\mathbf{x}}_i=& \left[{\mathbf{p}}_{b_i}^w,{\mathbf{q}}_{b_i}^w,{\mathbf{v}}_{b_i}^w,{\mathbf{b}}_a,{\mathbf{b}}_g\right],i\in [0,I-1],\\ {\mathbf{o}}_k=& \left[{\mathbf{\psi }}_k,{\varphi }_k\right],k\in [0,K-1],\end{array}$$
其中
${\mathbf{x}}_i$代表滑动窗口中的第i帧对应的body系状态，包括位置、四元数、速度、陀螺仪bias、加速度计bias

${\lambda }_j,j\in [0,J-1]$代表特征点的逆深度，$\mathbf{o}$代表线特征的正交表示

Cost function

$$\begin{array}{rl}& \underset{\mathcal{X}}{\min }\{{\Vert {\mathbf{r}}_0-{\mathbf{J}}_0\mathcal{X}\Vert }^2+\sum _{i\in \mathcal{B}}{\Vert {\mathbf{r}}_I\left({\mathbf{z}}_{b_{i+1}}^{b_i},\mathcal{X}\right)\Vert }_{{\Sigma }_{b_{i+1}}^{b_i}}^2\\ & +\sum _{(i,j)\in \mathcal{P}}{\rho }_p{\Vert {\mathbf{r}}_p\left({\mathbf{z}}_{p_j}^{c_i},\mathcal{X}\right)\Vert }_{{\Sigma }_{p_j}^{c_i}}^2+\sum _{(i,k)\in \mathcal{L}}{\rho }_l{\Vert {\mathbf{r}}_l\left({\mathbf{z}}_{l_k}^{c_i},\mathcal{X}\right)\Vert }_{{\Sigma }_{l_k}^{c_i}}^2\\ & +\sum _{(i,k)\in \mathcal{V}}{\rho }_v{\Vert {\mathbf{r}}_v\left({\mathbf{z}}_{v_k}^{c_i},\mathcal{X}\right)\Vert }_{{\Sigma }_{v_k}^{c_i}}^2\}\end{array}$$
其中
${\mathbf{r}}_0$是边缘化残差
${\mathbf{r}}_I$是IMU预积分残差
${\mathbf{r}}_p$是特征点残差
${\mathbf{r}}_l$是线特征残差
${\mathbf{r}}_v$灭点残差

特征点残差和线特征残差使用Huber核函数
由于灭点残差有无界问题，使用inverse tangent核函数

cost function对应的因子图如下所示

Line Measurement Model

3D直线的投影方程如下
$$\begin{array}{rl}{\mathbf{l}}^c& =\left[\begin{array}{l}{l}_1\\ l_2\\ l_3\end{array}\right]={\mathbf{K}}^{\prime }{\mathbf{n}}^c=f_x{f}_y{\left({\mathbf{K}}^{-1}\right)}^{\top }{\mathbf{n}}^c\\ & =\left[\begin{array}{ccc}{f}_y& 0& 0\\ 0& f_x& 0\\ -f_y{c}_x& -f_x{c}_y& f_x{f}_y\end{array}\right]{\mathbf{n}}^c={\mathbf{n}}^c\end{array}$$
本文提出的算法在归一化平面上进行，因此，K和K’是单位阵。投影直线方程等于Plücker表示中的normal vector。

线特征重投影残差如下
$${\mathbf{r}}_l=\left[\begin{array}{l}d\left({\mathbf{p}}_s,{\mathbf{l}}^c\right)\\ d\left({\mathbf{p}}_e,{\mathbf{l}}^c\right)\end{array}\right]$$
其中
$$\begin{gathered} d\left(\mathbf{p},1^c\right)=\frac{{\mathbf{p}}^{\top }{\mathbf{l}}^c}{l_d},l_d=\sqrt{l_1^2+l_2^2} \\ {\mathbf{p}}_s=\left(u_s,v_s,1\right),{\mathbf{p}}_e=\left(u_e,v_e,1\right) \end{gathered}$$

对应Jacobian如下


其中

${\mathcal{T}}_c^b$是Plücker的变换矩阵

Vanishing point measurement model

将直线上的3D点的齐次坐标表示为如下形式

其中

灭点相当于该直线上的无穷远点在像素平面上的投影

其中$\mathbf{P}=\mathbf{K}[\mathbf{I}\mid 0]$是相机投影矩阵

在UV-SLAM中，灭点对应直线的direction vector

灭点的估计值是${\mathbf{v}}^c$和图像平面的交点，如下图所示

灭点残差如下
$${\mathbf{r}}_v={\mathbf{p}}_v-\frac{1}{v_3}\left[\begin{array}{l}{v}_1\\ v_2\end{array}\right]$$
其中${\mathbf{p}}_v$是灭点的观测值

Jacobian矩阵如下
$$

其中

可观性分析

判断标准：如果FIM奇异，则系统不可观。该结果等价于：如果Jacobian列满秩，则系统可观。
首先，直线观测模型对正交表示的FIM如下

其中

${\Omega }_l^{\delta o}$是直线观测的协方差矩阵的逆

由于$\frac{\partial {\mathbf{l}}^c}{\partial {\mathbf{L}}^c},\frac{\partial {\mathbf{L}}^c}{\partial {\mathbf{L}}^w},\frac{\partial {\mathbf{L}}^w}{\partial \delta \mathbf{o}}$均为满秩矩阵，Jacobian的秩由$\frac{\partial {\mathbf{r}}_l}{\partial {\mathbf{l}}^c}$确定

$\frac{\partial {\mathbf{r}}_l}{\partial {\mathbf{l}}^c}$是一个2x3矩阵，最大秩为2.考虑如下退化情况

此时其秩为1

由于正交表示中的直线具有4个参数，仅依靠直线的测量模型无法保证直线的可观性，为了解决这一问题，使用了新的观测，如下
$${\mathbf{p}}_l=\alpha {\mathbf{p}}_s+(1-\alpha ){\mathbf{p}}_e,\alpha \in (0,1)$$
其中${\mathbf{p}}_l$是新的观测，添加该观测可以使Jacobian的秩变为2，直线仍不可观

考虑灭点测量的FIM如下

其中

类似地，只需分析$\frac{\partial {\mathbf{r}}_v}{\partial {\mathbf{v}}^c}$，其秩为2，因此Jacobian的秩为2

通过特征值分解，可以得到同时考虑直线测量和灭点测量的FIM如下

其中

在新的Jacobian矩阵中，直线测量和灭点测量对应的行是相互独立的，因此除上文提到的退化情况外，新的Jacobian矩阵的秩为4.可以得出结论，加入灭点观测可以使线特征完全可观。

Evaluation

在Euroc数据集上进行了测试

Conclusion

提出了 UV-SLAM，一种使用灭点测量的无结构化约束的点线SLAM.通过FIM秩分析证明了直线的可观性。