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$