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)$，其中：

• $\hat{l}$表示直線的方向向量，模長爲1；
• $m=l\times p$, $p$爲直線上的一點，可以看到這部分也表示線與原點組成平面的法線；
• $\frac{\|m\|}{\|l\|}$表示原點到直線的距離；

所以，這個直線的表示形式基本上是方向向量和法向量的表示方法，如下圖：

另一點，$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}$
$\hat{l}\cdot p$表示把$p$向量映射到直線方向上的長度，也就是上圖看到的$p_{\perp}-p$的長度；所以$\boldsymbol{p}_{\perp}$的模長就是遠點到直線的最短距離，因爲$\hat{l}$與$m$垂直，因此：
$||p_{\perp}||=||\hat{l}|| \cdot ||m||sin(90)=||m|| \tag{2}$

兩種特殊的情況

自然而然的想到，如果直線過原點怎麼表示？如果直線是無窮遠點怎麼表示？

過原點的直線

因爲直線過原點，那麼就把p設爲原點，則$m=\overrightarrow{0}\times l=\overrightarrow{0}$，跟它的實際意義也相符；

過無窮遠點的直線

直線過無窮點時，那麼就把p設爲無窮遠點，有$p=t\overline{p}$，則座標可以寫作：
$L:=(l, t\overline{p}\times l):=(\frac{l}{t}, \overline{p}\times l)=(0, m) \tag{3}$

 

在空間中的表示方法

使用兩個點進行表示

給定兩個3D空間中的點，有：$M^T=(\overline{M}, m)^T, N^T=(\overline{N}, n)^T$，其中$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}$
這裏簡單分析一下自由度，由上可知，Plucker的表示下一條直線有6個參數表示，是5維齊次座標系下的齊次表示，同時，因爲Plucker座標中的$a, b$分別表示線的方向以及線和原點組成的法向量，因此有約束$a^Tb=0$的約束，所以最終Plucker表示下的直線就有4個自由度，和空間直線的自由度一致。

使用兩個平面的交表示

這部分屬於空間中點和線的對偶形式，給定3D空間中的兩個平面，有：$M^T=(\overline{M}, m)^T, N^T=(\overline{N}, n)^T$，這裏$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}$

簡單的說一下上個公式中的b是怎麼得來的，假設點$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}$
所以可以看到$b=(n\overline{M}-m\overline{N})$垂直於直線上一點，同時$b$也垂直於直線的方向向量$(\overline{M}\times \overline{N})$，因此$b$就是直線與原點構成平面的法向量。

 

---

投影模型

對於投影矩陣$P=[\overline{P}, p]_{3\times4}$，那麼經過投影之後的點而言，有：
$\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}$
對於由兩個面相交而得到的直線表示來說，把直線表示替換掉爲面表示就可以了（對偶賽高）。

根據上面的推導很容易擴展出世界座標系到相機座標系的轉移矩陣有：
$\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}$
簡單說一下上述公式：第一行由公式（6）得到，把其中的$P$用$R,t$替換就可以了；第二行也很好理解，就是簡單的把直線的方向進行了旋轉。後面之所以把整個公式變換爲camera->world主要是因爲這是位姿的表示方法。其中的一步推導用到了一個性質，該性質在李羣中比較常見，即：當$M\in\mathbf{SO(3)}$時，有$[Mu]_{\times}=M[u]_{\times}M^T$。

 

---

三角化

這裏主要涉及兩個方法：第一個方法是解耦的方法，即先獲取表達式的初值，再對初值進行正交補償；第二個方法是耦合的來解，即使用迭代的方法得到一個即滿足優化函數，又能滿足正交要求的解。下面簡單的說明一下這兩個方法。

獲取直線表示的初值

這部分比較簡單，優化的目標函數就是投影誤差。假設直線$\mathbf{L}$被相機$P_i,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}$
可以看到基本上就是在第 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}$
其中：

• 設$l_{3\times 1}=\widetilde{\mathbf{P}}^{i} \mathbf{L}$，那麼$l_1=l(1), l_2=l(2)$；

該方程有很多的解法，這裏就不贅述了。需要注意的是這個部分只是求解了方程，並沒有考慮Plucker約束，即$\mathbf{a}^T\mathbf{b}=0$。

 

對Plucker約束進行補償

這部分可以着重參考論文【2】，基本思路是把這個問題當做純數學問題進行求解，獲取與初值最接近且正交的向量，但是這個解就不一定滿足公式（9）的最優解了。

 

類線性算法（Quasi-linear algorithm）

這個算法主要是使用迭代的思路使得解既能滿足公式（9）的優化函數，又能滿足Plucker正交關係的解，主要的思路如下：

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

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$
   可以看到這個思路主要想使得迭代前後兩次的方向向量和法向量是正交的；

3. 爲了求解上述公式，顯然可以得到最優的$L_{k+1}$是在$L_{k}^TG$的零空間中，所以對$L_{k}^TG$進行了SVD分解：
   $L_{k}^TG=U_{1\times1}^T\Sigma_{1\times6}V_{6\times6}^T=U_{1\times1}^T\Sigma_{1\times6}(v_{6\times1}, \overline{V}_{6\times5})^T$
   上述公式中，$v_{6\times 1}$是唯一一個特解，而$V_{6 \times 5}$就是$L_{k+1}$所在的零空間，所以$L_{k+1}$可以被$V_{6\times 5}$的五個列向量組成的空間基底線性表示；

4. 那麼現在的問題變爲如何求解這個線性表示了，或者說變爲求解$\gamma_{5\times 1}$這個向量。其實也簡單，經過第三步其實已經得到了$V_{6\times 5}$，那麼設$L_{k+1}=\overline{V}_{6\times 5}\gamma_{5\times 1}$，代入公式（9）後得到：
   $\mathcal{B}(\mathbf{L_{k+1}}, \mathcal{M})=\|A_{2n\times 6}\overline{V}_{6\times 5}\gamma_{5\times 1}\|^2=0$
   這個時候就比較明瞭了，構建優化目標函數爲：
   $\begin{aligned} \mathrm{min}_{r} &\|A\overline{V}\gamma\|^2 \\ s.t. &\|\gamma\|^2=1 \end{aligned}$
   這裏其實沒有特別想明白爲什麼約束條件是$\|\gamma\|^2=1$而不是$\sum\gamma_i=1$，私以爲可能是爲了方便運算，因爲上式直接用拉格朗日乘子法解的話，用$\|\gamma\|^2=1$的約束顯然比$\sum\gamma_i=1$要容易的多；

5. 經過這個之後，還有一個步驟是要更新公式（9）中的A，因爲每個直線與觀測線之間的誤差都與直線表示${L_{k+1}}$有關，因此當獲得了新的直線表示之後記得更新這個地方；

6. 重複2、 3、 4、 5步驟直到得到的$L_{k+1}$收斂；

 

---

線特徵的正交表示（Orthonormal Representation）

正交表示主要的貢獻點在於可以使得整個表示的未知數與自由度是對應的，也就是可以用最小表示進行線特徵的表示，這樣的好處對於優化問題是不言而喻的，就和李羣和李代數的關係一樣。下面就簡單的介紹一下這個表示方法。

正交表示的推導

假設直線的表示爲：$\mathcal{L}=[n^T, d^T]^T$，寫作矩陣形式爲$\mathbf{C}=\left[\mathbf{a}, \mathbf{b}\right]_{3\times2}$，其中：

• $\mathbf{n}$表示直線的法向量，$\mathbf{d}$表示直線的方向向量；
• $\mathbf{a}=\mathbf{n}, \mathbf{b}=\mathbf{d}$；

將矩陣$\mathbf{C}$進行QR分解有：
$C \sim \underbrace{\left(\frac{a}{\|a\|} \frac{b}{\|\mathbf{b}\|} \frac{a \times b}{\|\mathbf{a} \times \mathbf{b}\|}\right)}_{S O(3)} \underbrace{\left(\begin{matrix}\|\mathbf{a}\| & 0 \\ 0 & \|\mathbf{b}\| \\ 0 & 0\end{matrix}\right)}_{(\|\mathbf{a}\|\|\mathbf{b}\|)^{\top} \in \mathbb{P}^{1}} \tag{10}$
整個分解之後由兩部分組成，一部分是旋轉矩陣，另一部分是一個一維的齊次座標的表示。

簡單的說一下這個部分：

1. 第二部分的右上角的元素爲0是必然的，因爲$\mathbf{a}\perp\mathbf{b}$；

2. 因爲第二個部分本質上只有兩個變量，都除以$\|\mathbf{b}\|$之後，其實可以得到兩個元素爲$(d=\|\mathbf{a}\|/\|\mathbf{b}\|, 1)$，其中第一個元素根據公式（1）可以知道這部分就是直線到原點的距離。

所以看到，經過QR分解之後的整個直線表示的狀態量剛好是4個（第一部分有3個，第二部分有1個），因此說正交表示可以最小表示直線。

到這裏其實還並沒有完成正交表示，因爲作者認爲第二部分並不容易進行優化和更新，所以做了一個變化，把這部分變作了一個$\mathbf{SO(2)}$的表示，畢竟相比於這個$3\times 2$的矩陣來說，$\mathbf{SO(2)}$的更新更加穩定且容易，最重要的是並沒有造成信息的缺失，有如下的過程：
$W=\begin{bmatrix} \frac{1}{\sqrt{1+d^2}} & \frac{d}{\sqrt{1+d^2}} \\ \frac{-d}{\sqrt{1+d^2}} & \frac{1}{\sqrt{1+d^2}} \end{bmatrix} = \begin{bmatrix} \frac{\|a\|}{\sqrt{\|a\|^2+\|b\|^2}} & \frac{\|b\|}{\sqrt{\|a\|^2+\|b\|^2}} \\ -\frac{\|b\|}{\sqrt{\|a\|^2+\|b\|^2}} & \frac{\|a\|}{\sqrt{\|a\|^2+\|b\|^2}} \end{bmatrix} = \begin{bmatrix} cos(\phi) & -sin(\phi) \\ sin(\phi) & cos(\phi)\end{bmatrix} = \begin{bmatrix}w1 & -w2 \\ w2 & w1\end{bmatrix} \in \mathbf{SO(2)} \tag{11}$
在公式（10）中，比較容易看到，當對$\theta$進行更新了之後，可以通過$cos(\phi)/sin(\phi)$的公式來得到直線到原點的距離。

綜上所述，直線的正交表示可以寫作如下形式：
$C \sim (U, W) \in \mathbf{SO(3)} \times \mathbf{SO(2)}$
這部分用下圖表示十分合適：

 

正交表示與Plucker座標系的轉換

正交表示可以最小的表示直線，但是該表示方法的投影方程並沒有線性的表示，即該方法並不能像Plucker座標系表示方法一樣，通過公式（7）得到一個線性的映射關係，但是我們又特別的看重該表示方法的最小表示的性質，好在兩者之間的互換關係還是十分簡單的，如下：
$\underbrace{\mathcal{L}_{6\times1}}_{Plucker表示}=\sqrt{\|a\|^2+\|b\|^2} \underbrace{\begin{bmatrix} w1\mathbf{u_1} \\ w2\mathbf{u_2} \end{bmatrix}}_{正交表示} \tag{12}$
這裏面差了一個歸一化參數，這部分其實可以不用管，因爲可以在算$(\mathbf{a}, \mathbf{b})$的時候稍微進行一下歸一化或者其他的方式限制這個值。

 

---

優化問題

這部分其實跟點的過程一樣，也主要是觀測方程和誤差函數，上面談到的公式（6）和公式（8）其實已經簡單的涉及到了這部分，下面的內容主要是從頭到尾的串一遍了。

這部分用下面的一張圖表示：

其中：

• $L^W$表示在世界座標系的表示，$L^{C}$表示在相機座標系下的表示；
• $L^n$表示歸一化平面上的線，$L^I$表示在圖像座標系下的線；
• 這裏故意把歸一化平面與圖像平面分開畫，兩個平面之間其實差了一個內參矩陣，不過這個矩陣與常見的K不同，這裏需要稍微進行推導。

所以這裏進行從世界座標系到圖像座標系的整個投影可以表示爲：
$l^{iamge} = \mathcal{K} \mathbf{l^{normal}}=\mathcal{K}\tilde{P}_{3\times 6}\mathbf{^wL}_{6\times1}=\mathcal{K}\mathbf{m}^{\prime} \tag{13}$
其中：

• $\mathcal{K}$表示直線的內參矩陣；
• $\tilde{P}_{3\times 6}\mathbf{^wL}_{6\times1}$參考公式（６）；
• 這裏的$\mathbf{m}^{\prime}$表示在相機的歸一化座標系下的法向量；

簡單的說一下$\mathcal{K}$的推導：

1. 在歸一化平面上，假設直線的點爲：$^nC, {^n}D$；
2. 在圖像平面上，經過內參映射之後的點爲：${^i}c, {^i}d$，其中兩者的關係爲${^i}c=K^nC, {^i}d=K{^n}D$，其中K就是熟悉的相機內參矩陣；

對於歸一化平面和相機平面，兩者其實都是2維的射影平面，所以在歸一化平面和歸一化平面上的直線的表示爲：
$\begin{aligned} {^i}c \times {^i}d &= (K{^n}C) \times (K{^n}D) \\ &=\begin{bmatrix}fxX_C+cx \\ fyY_C+cy \\ 1\end{bmatrix}_{\times}\begin{bmatrix}fxX_D+cx \\ fyY_D+cy \\ 1\end{bmatrix} \\ &=\begin{bmatrix}0 & -1 & (fyY_C+cy) \\ 1 & 0 & -(fxX_C+cx) \\ -(fyY_C+cy) & (fxX_C+cx) & 0 \end{bmatrix}\begin{bmatrix}fxX_D+cx \\ fyY_D+cy \\ 1\end{bmatrix} \\ &=\begin{bmatrix}fy(Y_C-Y_D) \\ fx(X_C-X_D) \\ fxfy(X_CY_D-Y_CX_D)+fxcy(X_D-X_C)+fycx(Y_D-Y_C) \end{bmatrix} \\ &=\begin{bmatrix}fy & 0 & 0 \\ 0 & fx & 0 \\ -fycx & -fxcy & fxfy \end{bmatrix}\begin{bmatrix}Y_D-Y_C \\ X_D-X_C \\ X_CY_D-Y_CX_D \end{bmatrix} \\ &=\begin{bmatrix}fy & 0 & 0 \\ 0 & fx & 0 \\ -fycx & -fxcy & fxfy \end{bmatrix} \begin{bmatrix}X_C \\ Y_C \\ 1 \end{bmatrix}_{\times} \begin{bmatrix}X_D \\ Y_D \\ 1 \end{bmatrix} \\ &=\mathcal{K}({^n}C \times {^n}D) \\ &=\mathcal{K}\mathbf{m}^{\prime} \end{aligned}\tag{14}$
上述的推導其實很容易看出，因爲兩個歸一化平面的點的叉乘就是在相機座標系下的Plucker表示中的法向量，因此可以看到在歸一化座標系到圖像座標系的轉換中，僅僅涉及到了直線表示中的法向量部分。

 

誤差函數

用下面這張圖表示誤差量，如下：

圖中的$I_L$表示直線$\mathcal{L}$在圖像平面的投影，所以定義誤差項爲（就是簡單的兩個點到直線的距離）：
$\mathbf{r_L}=\begin{bmatrix}\mathbf{r_1} \\ \mathbf{r_2} \end{bmatrix} = \begin{bmatrix}\frac{c^TI}{\sqrt{l_1^2+l_2^2}} \\ \frac{d^TI}{\sqrt{l_1^2+l_2^2}} \end{bmatrix} \tag{15}$
 

求解Jacobian

跟對3D點的優化問題一樣，就是從誤差不停的遞推到位姿以及直線表示上，用到最最最基本的求導的鏈式法則：

通用的公式如下：
$\frac{\partial \mathbf{r_L}}{\partial X}=\frac{\partial \mathbf{r_L}}{\partial L^{I}} \frac{\partial L^{I}}{\partial L^{n}} \frac{\partial L^{n}}{\partial L^{c}} \begin{aligned}\begin{cases} \frac{\partial L^{c}}{\partial \theta} &\text{ X=}\theta \\\frac{\partial L^{c}}{\partial t} &\text{ X=t} \\\frac{\partial L^{c}}{\partial L^{w}}\frac{\partial L^{w}}{\partial{(\theta,\phi)}} &\text{ X=}L^{w}\end{cases} \end{aligned}\tag{16}$
先對前面最通用的部分進行求解：

第一部分：
$\begin{aligned}\frac{\partial \mathbf{r_L}}{\partial L^{I}} &= \begin{bmatrix}\frac{\partial{\mathbf{r1}}}{\partial{l_1}} & \frac{\partial{\mathbf{r1}}}{\partial{l_2}} & \frac{\partial{\mathbf{r1}}}{\partial{l_3}} \\ \frac{\partial{\mathbf{r2}}}{\partial{l_1}} & \frac{\partial{\mathbf{r2}}}{\partial{l_2}} & \frac{\partial{\mathbf{r2}}}{\partial{l_3}}\end{bmatrix} \\&=\begin{bmatrix}\frac{-l_1 c^TL^{I}}{(l_1^2+l_2^2)^{\frac{3}{2}}}+\frac{u_c}{(l_1^2+l_2^2)^{\frac{1}{2}}} & \frac{-l_2 c^TL^{I}}{(l_1^2+l_2^2)^{\frac{3}{2}}}+\frac{v_c}{(l_1^2+l_2^2)^{\frac{1}{2}}} & \frac{1}{(l_1^2+l_2^2)^{\frac{1}{2}}} \\ \frac{-l_1 d^TL^{I}}{(l_1^2+l_2^2)^{\frac{3}{2}}}+\frac{u_d}{(l_1^2+l_2^2)^{\frac{1}{2}}} & \frac{-l_2 d^TL^{I}}{(l_1^2+l_2^2)^{\frac{3}{2}}}+\frac{v_d}{(l_1^2+l_2^2)^{\frac{1}{2}}} & \frac{1}{(l_1^2+l_2^2)^{\frac{1}{2}}} \end{bmatrix}_{2\times 3}\end{aligned} \tag{17}$
其中：

• $l1, l2, l3$表示圖像座標系下直線的三個參數；
• $u_c, v_c$表示點c的xy座標值，$u_d, v_d$同理；

第二部分：

根據公式（13）可知：
$\frac{\partial L^{I}}{\partial L^{n}}=\mathcal{K}_{3\times3} \tag{18}$
第三部分：

由公式（6）和（13）可知，直線的Plucker表示在歸一化平面上只用了其中的法向量部分，因此若有$\mathcal{{^c}L}=\left[\mathbf{{^c}n}, \mathbf{{^c}d}\right]^T$，那麼$\mathcal{{^n}L}=\mathbf{{^c}n}$，所以求導有：
$\frac{\partial L^{n}}{\partial L^{c}}=\begin{bmatrix}\mathbf{I}_{3\times3} & 0_{3\times3}\end{bmatrix}_{3\times6} \tag{19}$
第四部分就分這幾種情況進行討論：

對於位姿的姿態部分

根據公式（7）有：
$\frac{\partial {^c}L}{\partial \theta} = \begin{bmatrix}\frac{\partial n_c}{\partial \theta} \\ \frac{\partial d_c}{\partial \theta}\end{bmatrix} = \begin{bmatrix}\frac{\partial{(R_{wc}^T(n_w+[t_{wc}]_{\times}b_w))}}{\partial \theta} \\ \frac{\partial{R_{wc}^Tb_w}}{\partial \theta}\end{bmatrix}=\begin{bmatrix} [R_{wc}^T(n_w+[t_{wc}]_{\times}b_w)]_{\times} \\ [R_{wc}^Tb_w]_{\times} \end{bmatrix}_{6\times3} \tag{20}$

上述的推導使用了李羣的右擾動模型，即$(R_{wc}Exp(\theta))^T=Exp(-\theta)R_{wc}^T$。

對於位姿的位移部分

同樣根據公式（7）有：
$\frac{\partial {^c}L}{\partial t} = \begin{bmatrix}\frac{\partial n_c}{\partial t} \\ \frac{\partial d_c}{\partial t}\end{bmatrix} = \begin{bmatrix}\frac{\partial{(R_{wc}^T(n_w+[t_{wc}]_{\times}b_w))}}{\partial t} \\ \frac{\partial{R_{wc}^Tb_w}}{\partial t}\end{bmatrix}=\begin{bmatrix} -R_{wc}^T[b_{w}]_{\times} \\ \mathbf{0} \end{bmatrix}_{6\times3} \tag{21}$

對於世界座標系下直線表示部分

這部分按照公式（16）的步驟，依舊分兩個部分：

1. $\frac{\partial L^{c}}{\partial L^{w}}$部分：
   $\frac{\partial L^{c}}{\partial L^{w}}=\begin{bmatrix}\frac{\partial{\mathbf{n^C}}}{\partial{\mathbf{n^W}}} & \frac{\partial{\mathbf{n^C}}}{\partial{\mathbf{b^W}}} \\ \frac{\partial{\mathbf{d^C}}}{\partial{\mathbf{n^W}}} & \frac{\partial{\mathbf{d^C}}}{\partial{\mathbf{b^W}}}\end{bmatrix} =\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] \tag{22}$

2. $\frac{\partial L^{w}}{\partial(\theta, \phi)}$部分，這部分其實還可以繼續分，如下：
   $\frac{\partial L^{w}}{\partial(\theta, \phi)}=\left[\frac{\partial L^{w}}{\partial \theta}, \frac{\partial L^{w}}{\partial \phi}\right]=\left[\frac{\partial L^{w}}{\partial{U}}\frac{\partial{U}}{\partial \theta}, \frac{\partial L^{w}}{\partial{W}}\frac{\partial{W}}{\partial \phi}\right]$

   第一部分

   $\begin{aligned}\frac{\partial L^{w}}{\partial{U}}\frac{\partial{U}}{\partial \theta}&=\frac{\partial{\begin{bmatrix} w1\mathbf{u_1} \\ w2\mathbf{u_2} \end{bmatrix}}}{\partial{[\mathbf{u_1},\mathbf{u_2}, \mathbf{u_3}]}}\frac{\partial{[\mathbf{u_1},\mathbf{u_2}, \mathbf{u_3}]}}{\partial{\theta}} \\&=\begin{bmatrix}w1 & 0 & 0 \\ 0 & w2 & 0 \end{bmatrix}_{6\times9}\begin{bmatrix}0 & -\mathbf{u3} & \mathbf{u2} \\ \mathbf{u3} & 0 & -\mathbf{u1} \\ -\mathbf{u2} & \mathbf{u1} & 0 \end{bmatrix}_{9\times3} \\&= \begin{bmatrix} 0 & -w1\mathbf{u3} & w1\mathbf{u2} \\ -w2\mathbf{u3} & 0 & -w2\mathbf{u1} \end{bmatrix}_{6\times3}\end{aligned} \tag{23}$

   第二部分

   $\begin{aligned}\frac{\partial L^{w}}{\partial{W}}\frac{\partial{W}}{\partial \phi}&=\frac{\partial{\begin{bmatrix} w1\mathbf{u_1} \\ w2\mathbf{u_2} \end{bmatrix}}}{\partial{[w1, w2]^T}}\frac{\partial{[w1, w2]^T}}{\partial{\phi}} \\&=\begin{bmatrix}\mathbf{u1} & 0 \\ 0 & \mathbf{u2} \end{bmatrix}_{6\times2}\begin{bmatrix} -w2 \\ w1 \end{bmatrix}_{2\times1} \\&= \begin{bmatrix} -w2\mathbf{u1} \\ w1\mathbf{u2}\end{bmatrix}_{6\times1}\end{aligned} \tag{24}$

   其中$w1=cos(\phi), w2=sin(\phi)$。

3. 兩個部分合起來爲：
   $\frac{\partial L^{w}}{\partial(\theta, \phi)}=\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]_{6\times6}\begin{bmatrix} 0 & -w1\mathbf{u3} & w1\mathbf{u2} & -w2\mathbf{u1} \\ -w2\mathbf{u3} & 0 & -w2\mathbf{u1} & w1\mathbf{u2} \end{bmatrix}_{6\times4} \tag{25}$

 

---

總結

線特徵這部分確實比點特徵要複雜很多，表示方法就很不同（這部分也是筆者花了很久才接受的一部分），但是好在整個推導過程都可以藉助點的轉移方程逐步進行推導。