SLAM线特征学习（1）——基本的线特征表示与优化推导

Reference

1. Plucker Coordinates for Lines in the Space.
2. Structure-From-Motion Using Lines: Representation, Triangulation and Bundle Adjustment. 本文深度参考这篇论文
3. 多视图几何.
4. https://zhuanlan.zhihu.com/p/65674067 关于线特征很好的解读.

 

---

直线的自由度

3D空间中的直线有4个自由度，通常认为一条线由两个点或者面组成，所以自由度有2x3=6个，但是由于直线绕着自身的方向轴旋转和移动的话，直线还是这个直线，所以真实自由度为6-2=4个。

 

---

Plucker坐标系

Plucker坐标系是一种比较重要的表示方法，后面很多的表示方法也能和该方法进行互换

Plucker坐标系使用两个向量表示， $L:=(\hat{l},m)$L:=(l^,m)，其中：

• $\hat{l}$l^表示直线的方向向量，模长为1；
• $m=l\times p$m=l×p, $p$p为直线上的一点，可以看到这部分也表示线与原点组成平面的法线；
• $\frac{\|m\|}{\|l\|}$∥l∥∥m∥​表示原点到直线的距离；

所以，这个直线的表示形式基本上是方向向量和法向量的表示方法，如下图：

另一点， $m$m表示了原点到直线的距离，公式推导如下：
$\begin{aligned} \boldsymbol{p}_{\perp} &=\boldsymbol{p}-(\hat{\boldsymbol{l}} \cdot \boldsymbol{p}) \hat{\boldsymbol{l}} \\ &=(\hat{\boldsymbol{l}} \cdot \hat{\boldsymbol{l}}) \boldsymbol{p}-(\hat{\boldsymbol{l}} \cdot \boldsymbol{p}) \hat{l} \\ &=\hat{\boldsymbol{l}} \times(\boldsymbol{p} \times \hat{\boldsymbol{l}}) \\ &=\hat{\boldsymbol{l}} \times \boldsymbol{m} \end{aligned} \tag{1}$p⊥​​=p−(l^⋅p)l^=(l^⋅l^)p−(l^⋅p)l^=l^×(p×l^)=l^×m​(1)
$\hat{l}\cdot p$l^⋅p表示把 $p$p向量映射到直线方向上的长度，也就是上图看到的 $p_{\perp}-p$p⊥​−p的长度；所以 $\boldsymbol{p}_{\perp}$p⊥​的模长就是远点到直线的最短距离，因为 $\hat{l}$l^与 $m$m垂直，因此：
$||p_{\perp}||=||\hat{l}|| \cdot ||m||sin(90)=||m|| \tag{2}$∣∣p⊥​∣∣=∣∣l^∣∣⋅∣∣m∣∣sin(90)=∣∣m∣∣(2)

两种特殊的情况

自然而然的想到，如果直线过原点怎么表示？如果直线是无穷远点怎么表示？

过原点的直线

因为直线过原点，那么就把p设为原点，则 $m=\overrightarrow{0}\times l=\overrightarrow{0}$m=0 ×l=0 ，跟它的实际意义也相符；

过无穷远点的直线

直线过无穷点时，那么就把p设为无穷远点，有 $p=t\overline{p}$p=tp​，则坐标可以写作：
$L:=(l, t\overline{p}\times l):=(\frac{l}{t}, \overline{p}\times l)=(0, m) \tag{3}$L:=(l,tp​×l):=(tl​,p​×l)=(0,m)(3)

 

在空间中的表示方法

使用两个点进行表示

给定两个3D空间中的点，有： $M^T=(\overline{M}, m)^T, N^T=(\overline{N}, n)^T$MT=(M,m)T,NT=(N,n)T，其中 $m,n$m,n一方面使得直线变为其次坐标的表示，另一方面表示该点的逆深度，所以根据上面的Plucker坐标的表示，有：
$L=\left(\begin{array}{c}\overline{l} \\ m\end{array}\right)=\left(\begin{array}{c} \frac{\overline{M}}{m}-\frac{\overline{N}}{n} \\ \overline{M}\times \overline{N} \end{array} \right) = \left(\begin{array}{c} n\overline{M}-m\overline{N} \\ \overline{M}\times \overline{N} \end{array} \right) = \left(\begin{array}{c} a \\ b \end{array} \right) \tag{4}$L=(lm​)=(mM​−nN​M×N​)=(nM−mNM×N​)=(ab​)(4)
这里简单分析一下自由度，由上可知，Plucker的表示下一条直线有6个参数表示，是5维齐次坐标系下的齐次表示，同时，因为Plucker坐标中的 $a, b$a,b分别表示线的方向以及线和原点组成的法向量，因此有约束 $a^Tb=0$aTb=0的约束，所以最终Plucker表示下的直线就有4个自由度，和空间直线的自由度一致。

使用两个平面的交表示

这部分属于空间中点和线的对偶形式，给定3D空间中的两个平面，有： $M^T=(\overline{M}, m)^T, N^T=(\overline{N}, n)^T$MT=(M,m)T,NT=(N,n)T，这里 $m, n$m,n就不表示逆深度了，而是表示截距，同样按照Plucker坐标的表示，有：
$L=\left(\begin{array}{c}\overline{l} \\ m \end{array}\right)=\left(\begin{array}{c} \overline{M}\times \overline{N} \\ \frac{\overline{M}}{m}-\frac{\overline{N}}{n} \end{array} \right) = \left(\begin{array}{c} \overline{M}\times \overline{N} \\ n\overline{M}-m\overline{N} \end{array} \right) = \left(\begin{array}{c} a \\ b \end{array} \right) \tag{5}$L=(lm​)=(M×NmM​−nN​​)=(M×NnM−mN​)=(ab​)(5)

简单的说一下上个公式中的b是怎么得来的，假设点 $X$X在直线上，则有：
$\begin{aligned} \begin{cases} \overline{M}X+m=0 \\ \overline{N}X+n=0 \end{cases} \rightarrow (n\overline{M}-m\overline{N})X=0 \end{aligned}${MX+m=0NX+n=0​→(nM−mN)X=0​
所以可以看到 $b=(n\overline{M}-m\overline{N})$b=(nM−mN)垂直于直线上一点，同时 $b$b也垂直于直线的方向向量 $(\overline{M}\times \overline{N})$(M×N)，因此 $b$b就是直线与原点构成平面的法向量。

 

---

投影模型

对于投影矩阵 $P=[\overline{P}, p]_{3\times4}$P=[P,p]3×4​，那么经过投影之后的点而言，有：
$\begin{aligned} l_{image}&=m_{image}\times n_{image} \\ &=(PM^w) \times (PN^w) \\ &=(\overline{P}\overline{M}+pm) \times (\overline{P}\overline{N}+pn) \\ &=\overline{P}\overline{M} \times \overline{P}\overline{N}+pm \times \overline{P}\overline{N}+\overline{P}\overline{M} \times pn+ pm \times pn \\ &=det(\overline{P})\overline{P}^{-T}(\overline{M} \times \overline{N})+[p]_{\times}\overline{P}(m\overline{N}-n\overline{M}) \\ &=[det(\overline{P})\overline{P}^{-T}, [p]_{\times}\overline{P}] \begin{bmatrix} \overline{M} \times \overline{N} \\ m\overline{N}-n\overline{M} \end{bmatrix} \\ &=\tilde{P}_{3\times 6}\mathbf{L}_{6\times1} \end{aligned} \tag{6}$limage​​=mimage​×nimage​=(PMw)×(PNw)=(PM+pm)×(PN+pn)=PM×PN+pm×PN+PM×pn+pm×pn=det(P)P−T(M×N)+[p]×​P(mN−nM)=[det(P)P−T,[p]×​P][M×NmN−nM​]=P~3×6​L6×1​​(6)
对于由两个面相交而得到的直线表示来说，把直线表示替换掉为面表示就可以了（对偶赛高）。

根据上面的推导很容易扩展出世界坐标系到相机坐标系的转移矩阵有：
$\left[\begin{array}{c} \mathbf{n}_{c} \\ \mathbf{b}_{c} \end{array}\right]= \left[\begin{array}{cc} \mathrm{R}_{cw} & {\left[\mathbf{t}_{cw}\right]_{\times} \mathrm{R}_{cw}} \\ \mathbf{0} & \mathrm{R}_{cw} \end{array}\right]\left[\begin{array}{c} \mathbf{n}_{w} \\ \mathbf{b}_{w} \end{array}\right]= \left[\begin{array}{cc} \mathrm{R}_{wc}^{T} & {\left[-\mathrm{R}_{wc}^{T}\mathbf{t}_{wc}\right]_{\times} \mathrm{R}_{wc}^T} \\ \mathbf{0} & \mathrm{R}_{wc}^T \end{array}\right]\left[\begin{array}{c} \mathbf{n}_{w} \\ \mathbf{b}_{w} \end{array}\right]= \left[\begin{array}{cc} \mathrm{R}_{wc}^{T} & {\mathrm{R}_{wc}^{T}\left[\mathbf{t}_{wc}\right]_{\times} } \\ \mathbf{0} & \mathrm{R}_{wc}^T \end{array}\right]\left[\begin{array}{c} \mathbf{n}_{w} \\ \mathbf{b}_{w} \end{array}\right] \tag{7}$[nc​bc​​]=[Rcw​0​[tcw​]×​Rcw​Rcw​​][nw​bw​​]=[RwcT​0​[−RwcT​twc​]×​RwcT​RwcT​​][nw​bw​​]=[RwcT​0​RwcT​[twc​]×​RwcT​​][nw​bw​​](7)
简单说一下上述公式：第一行由公式（6）得到，把其中的 $P$P用 $R,t$R,t替换就可以了；第二行也很好理解，就是简单的把直线的方向进行了旋转。后面之所以把整个公式变换为camera->world主要是因为这是位姿的表示方法。其中的一步推导用到了一个性质，该性质在李群中比较常见，即：当 $M\in\mathbf{SO(3)}$M∈SO(3)时，有 $[Mu]_{\times}=M[u]_{\times}M^T$[Mu]×​=M[u]×​MT。

 

---

三角化

这里主要涉及两个方法：第一个方法是解耦的方法，即先获取表达式的初值，再对初值进行正交补偿；第二个方法是耦合的来解，即使用迭代的方法得到一个即满足优化函数，又能满足正交要求的解。下面简单的说明一下这两个方法。

获取直线表示的初值

这部分比较简单，优化的目标函数就是投影误差。假设直线 $\mathbf{L}$L被相机 $P_i,i=1...n$Pi​,i=1...n观测到，那么对于每一次的观测，可以定义误差：
$E=\left(\mathbf{x}^{i \top} \widetilde{\mathbf{P}}^{i} \mathbf{L}\right)^{2}+\left(\mathbf{y}^{i^{\top}} \widetilde{\mathbf{P}}^{i} \mathbf{L}\right)^{2} \tag{8}$E=(xi⊤P iL)2+(yi⊤P iL)2(8)
可以看到基本上就是在第 i 帧上的图像上的两个对应点到投影直线的距离。所以所有的观测都考虑之后有：
$\begin{aligned} \mathcal{B}(\mathbf{L}, \mathcal{M}) &=\sum_{i=1}^{n}\left(\left(\frac{\mathbf{x}^{i \top} \widetilde{\mathbf{P}}^{i} \mathbf{L}}{\sqrt{(l_1)^2+(l_2)^2}}\right)^{2}+\left(\frac{\mathbf{y}^{i^{\top}} \widetilde{\mathbf{P}}^{i} \mathbf{L}}{\sqrt{(l_1)^2+(l_2)^2}}\right)^{2}\right) \\ &=\left\|A_{(2 n \times 6)} \mathbf{L}\right\|^{2} \quad \text { with } \quad \mathrm{A}=\left(\begin{array}{c} \cdots \\ \mathbf{x}^{i^{\top}} \widetilde{\mathbf{P}}^{i} \\ \mathbf{y}^{i^{\top}} \widetilde{\mathbf{P}}^{i} \\ \ldots \end{array}\right) \end{aligned} \tag{9}$B(L,M)​=i=1∑n​⎝⎛​((l1​)2+(l2​)2 ​xi⊤P iL​)2+((l1​)2+(l2​)2 ​yi⊤P iL​)2⎠⎞​=∥∥​A(2n×6)​L∥∥​2 with A=⎝⎜⎜⎛​⋯xi⊤P iyi⊤P i…​⎠⎟⎟⎞​​(9)
其中：

• 设 $l_{3\times 1}=\widetilde{\mathbf{P}}^{i} \mathbf{L}$l3×1​=P iL，那么 $l_1=l(1), l_2=l(2)$l1​=l(1),l2​=l(2)；

该方程有很多的解法，这里就不赘述了。需要注意的是这个部分只是求解了方程，并没有考虑Plucker约束，即 $\mathbf{a}^T\mathbf{b}=0$aTb=0。

 

对Plucker约束进行补偿

这部分可以着重参考论文【2】，基本思路是把这个问题当做纯数学问题进行求解，获取与初值最接近且正交的向量，但是这个解就不一定满足公式（9）的最优解了。

 

类线性算法（Quasi-linear algorithm）

这个算法主要是使用迭代的思路使得解既能满足公式（9）的优化函数，又能满足Plucker正交关系的解，主要的思路如下：

1. 先按照公式（9）求得一个初始 $L_0$L0​；

2. 构建一个迭代公式：
   $C_k(L_{k+1})=L_k^T\underbrace{\begin{bmatrix}0 & I_{3\times3} \\ I_{3\times3} & 0\end{bmatrix}}_{G}L_{k+1}=b_k^Ta_{k+1}+a^T_kb_{k+1}=0$Ck​(Lk+1​)=LkT​G [0I3×3​​I3×3​0​]​​Lk+1​=bkT​ak+1​+akT​bk+1​=0
   可以看到这个思路主要想使得迭代前后两次的方向向量和法向量是正交的；

3. 为了求解上述公式，显然可以得到最优的 $L_{k+1}$Lk+1​是在 $L_{k}^TG$LkTkT​G [0I3×3​​I3×3​0​]​​Lk+1​=bkT​ak+1​+akT​bk+1​=0
   可以看到这个思路主要想使得迭代前后两次的方向向量和法向量是正交的；

4. 为了求解上述公式，显然可以得到最优的 $L_{k+1}$Lk+1​是在 $L_{k}^TG$LkT​G的零空间中，所以对 $L_{k}^TG$