论文阅读: Building a 3-D Line-Based Map Using Stereo SLAM

Abstract

一个把直线用作feature的SLAM系统.

跟点相比, 直线对于环境的结构提供了更丰富的信息, 也让其鞥有可能推断地图的空间语义.

• 使用了Plucker line coordiantes来高效的初始化新观测的线特征,以及3D线的投影.
• 用orthonormal representation做图优化.

1. Introduction

线和线段提供了更多环境里的结构信息. 用线特征建立的环境一般都有更少的纹理

用两个端点的三维线表示方式明显不满足SLAM的要求, 因为提取和跟踪线的端点的准确性和可靠性都差那么点那意思. 但也没有没有很统治级的3D线的参数法方法.

我们用了plucker coordinate和orthonormal representation.

线的关联不会提供任何的相对运动的约束. 解决方案是用第三张图来限制gauge freedoms.

2. Related Work

3. Preliminaries

4. 3D Line Feature

A. Line Representation

1) Plucker Line Coordinate \(\mathcal{L} = (n^T, v^T)^T\)

注意, \(n\)和\(v\)不一定要是unit vector, 从原点到线最近点\(Q\)的计算是\(||n||/||v||\).

还有一个约束是 \(n^Tv =0\).

plucker coordinate的transformation也是个线性转换:
\[
\left[\begin{array}{c}{\mathbf{n}_{j}} \\ {\mathbf{v}_{j}}\end{array}\right]=\left[\begin{array}{cc}{\mathrm{R}_{j i}} & {\left[\mathbf{t}_{j i}\right]_{ \times} \mathbf{R}_{j i}} \\ {\mathbf{0}} & {\mathrm{R}_{j i}}\end{array}\right]\left[\begin{array}{l}{\mathbf{n}_{i}} \\ {\mathbf{v}_{i}}\end{array}\right]
\]
2) Orthonormal Representation

我们的实验表明这个表达形式有最小的4个参数,在准确度上和收敛性上变现都很好.

我们可以用plucker coor的QR分解来计算orthonormal representation\[(\mathrm{U}, \mathrm{W}) \in S O(3) \times S O(2)\]
\[
[\mathbf{n} | \mathbf{v}]=U\left[\begin{array}{cc}{\omega_{1}} & {0} \\ {0} & {\omega_{2}} \\ {0} & {0}\end{array}\right]
\]
然后把\(W\)设定为如下式子:
\[
W=\left[\begin{array}{cc}{\omega_{1}} & {-\omega_{2}} \\ {\omega_{2}} & {\omega_{1}}\end{array}\right]
\]

如上图所示, \(U\)和\(W\)是三维和二维的旋转矩阵, 所以最小化的4参数表示方式是: \[\mathbf{p}^{\top}=\left(\boldsymbol{\theta}^{\top}, \theta\right)\]

这儿\(\boldsymbol{\theta}\)标识3-vector, \(\theta\)是标量.

这样的话\((U, W)\)的更新形式变成: \[\mathrm{U} \leftarrow \mathrm{U}R(\boldsymbol{\theta})\] 和 \[W \leftarrow W R(\theta)\].

从orthonormal representation到plucker coordinate的转换如下:
\[
\mathcal{L}=\left(\omega_{1} \mathbf{u}_{1}^{\top}, \omega_{2} \mathbf{u}_{2}^{\top}\right)^{\top}
\]
这里\(u_i\)表示U的第i列.

• [ ] 感觉这儿的\(\omega\)的序号有点问题.

B. Observation Model

如上图所示, 平面\(\pi_L\)由\(\mathcal{L}\)和\(C_L\)决定, 并和平面\(\mathcal{I}_L\)相较于\(I_L\).

所有在\(\pi_L\)上的线投在平面上都是这个\(I_L\). 这意味着\(\mathcal{L}\)的投影只由\(n\)决定, 与方向\(v\)无关.

3D端点\(C, D\)投影成为\(c, d\).

在左侧的相机坐标系中, \[\underline{\mathbf{c}}=\mathrm{KC}, \underline{\mathrm{d}}=\mathrm{KD}, \mathbf{n}=\mathrm{C} \times \mathrm{D}, \text { and } \underline{1}=\underline{\mathbf{c}} \times \underline{\mathrm{d}}\]

这样我们也可以知道, 线的投影方程:
\[
1=\mathcal{K} \mathbf{n} \tag{16}
\]

\[
\mathcal{K}=\left[\begin{array}{ccc}{f_{y}} & {0} & {0} \\ {0} & {f_{x}} & {0} \\ {-f_{y} x_{0}} & {-f_{x} y_{0}} & {f_{x} f_{y}}\end{array}\right]
\]

公式(16)表明, 在投影的时候只有normal vector会出现在投影方程里. 相反的, 在看图像观测的时候, 我们也只能得到plucker coordinate的 \(n\).

plucker coordinate的重投影方程和之前介绍的特征都不一样, 因为它不能直接将两个图像线相减, 我们把重投影误差定义为端点到预测线的距离.

这样的话, 对于端点\(d\)和线\(l_L\), cost的定义如下:
\[
z_{L}^{1}=\frac{\mathbf{d}^{\top} \cdot \mathbf{l}_{L}}{d_{3} \sqrt{l_{1}^{2}+l_{2}^{2}}} \in \mathbb{R}^{1}
\]
这里\[\mathbf{d}=\left(d_{1}, d_{2}, d_{3}\right)^{\top} \text { and } \underline{l}_{L}=\left(l_{1}, l_{2}, l_{3}\right)^{\top}\]

因为针孔问题, 连续帧中线段有噪声的位置, 只有观测中和期望的线投影正交的部分才有有意义的信息.

事实上, 我们也测试过其他重投影误差的方法, 比如线段间的旋转和平移的误差, 然而它降低了重建的精度.

这样, 在stereo image上的重投影误差如下:
\[
\epsilon_{\mathrm{z}}=\left(z_{L}^{1}, z_{L}^{2}, z_{R}^{1}, z_{R}^{2}\right)^{\top} \in \mathbb{R}^{4}
\]

C. Initialization

在左相机的观测中, 用左相机的光心和图像线可以计算一个平面:
\[
\underline{\boldsymbol{\pi}}_{L}=\mathrm{P}^{\top} \underline{1}_{L} \in \mathbb{R}^{4}
\]
我们回顾dual plucker matrix, 结果如下:
\[
\mathrm{L}^{*}=\boldsymbol{\pi}_{L} \boldsymbol{\pi}_{R}^{\top}-\underline{\boldsymbol{\pi}}_{R} \underline{\boldsymbol{\pi}}_{L}^{\top} \in \mathbb{R}^{4 \times 4}
\]

\[
\mathrm{L}^{*}=\left[\begin{array}{cc}{[\mathbf{v}]_{ \times}} & {\mathbf{n}} \\ {-\mathbf{n}^{\top}} & {0}\end{array}\right]
\]

这样我们就可以直接提取plucker line coordinates.

D. Endpoints Trimming

所有的几何计算都把3D线看作是有限的直线, 特定的端点位置不会影响3D线计算的结果.

3D line-based map比之3D point map的优势在于, 它可以更高效的收敛.

在实际情况中, 光线从相机中心反投影很难在3D线上停驻. 我们提了一种方案.

图像e的生成方式是e-d垂直与直线\(l_L\), 这样3D平面\(\pi\)就可以如下计算:
\[
\begin{array}{l}{\underline{1}_{e}=\underline{e} \times \underline{d}} \\ {\underline{\pi}=\mathrm{P}^{\top} \underline{l}_e}\end{array}
\]
给定plucker line coordinates, \[\mathcal{L}=\left(\mathbf{n}^{\top}, \mathbf{v}^{\top}\right)^{\top}\], plucker matrix的公式如下:
\[
\mathrm{L}=\left[\begin{array}{cc}{[\mathbf{n}]_{ \times}} & {\mathbf{v}} \\ {-\mathbf{v}^{\top}} & {0}\end{array}\right]
\]
这样的话, 3D点\(D\)可以如下计算:
\[
\underline{\mathbf{D}}=\mathrm{L} \boldsymbol{\pi}
\]

E. Straight Line Detection and Tracking

...

F. Loop Closure Detection

...

5. Graph Optimization

...

Appendix

Plucker Matrix

给定两个点\(A, B\). Plucker Matrix如下:
\[
[L]_{\times} \propto AB^T-BA^T
\]
是一个反对称矩阵.

它有一些特性:

• 平面\(E\)包含了直线:\(0=[L]_{\times}E\)
• 平面\(E\)和直线相交: \(X=[L]_{\times}E\)
• 点在线上: \(0=[\tilde{L}]_\times X\)
• \(E=[\tilde{L}]_\times X\)表示平面\(E\)包含点\(X\)和线\(L\).

Dual Plucker Matrix

点和平面都可以被标识为4-vector, 并且它们的几何关系的代数描述都是对称的. 通过在理论中交换两者(点和平面), 我们可以得到一个dual theorem.

在Plucker matrix中, 存在一种dual representation是两个平面相交表示一条线

存在两个平面 \(E, F\). Plucker Matrix是:
\[
[\tilde{L}]_{\times} = EF^T-FE^T
\]
然后
\[
G=[\tilde{L}]_{\times}X
\]
这个式子描述\(G\)平面包含点\(X\)和线\(L\).

也就是dual Plucker Matrix操作的是一条线和一个点生成一个平面.

Relationship between Primal and Dual Plucker Matrix

\(X=[L]_\times E\) 的结果是一个线上的点或者是一个zero-vector.

两者相乘是0矩阵

---

在projective plane中, 两个点的连线也是一个直线:
\[
l \propto a \times b = [l_0 \ l_1 \ l_2]^T\\
[l]_\times=ab^T-ba^T
\]
Geometric Interpretation
\[
[L]_\times=\left( \begin{matrix} [m]_\times & d \\ -d & 0 \end{matrix} \tag{1} \right)
\]

\[
[\tilde{L}]_\times = \left( \begin{matrix} [-d]_\times & m\\ -m & 0 \end{matrix} \tag{2} \right)
\]