SLAM中直線特徵如何優化

這篇文章主要介紹SLAM中基於直線正交表達的優化問題。下面將從頭慢慢道來

1直線的普呂克參數化方法

根據多視圖幾何裡面介紹，直線可以用6個參數的普呂克坐標表示，然而普呂克坐標有6個參數 $（n^{T},d^{T}）$ 其中d是直線的方向向量，n是直線與光心所成平面的法向量，

其中存在一個垂直約束 $n^{T}d=0$ ，仍然有五個自由度，對於三維直線來講，三維直線只有四個自由度。這種過參數表達不能在無約束的優化函數中使用。

2直線的正交表示法

四自由度的正交表達 $(U,W)_{Usubseteq so3，Wsubseteq so2}$ 更適合參與直線優化。

在直線上定義一個坐標系：單位方向向量和單位法向量作為坐標系的兩個軸，另一個軸叉乘得到。該坐標系和相機坐標系之間的旋轉變換關係可以用一個旋轉矩陣U表示

$U=R(psi)=[frac{n}{||n||},frac{d}{||d||},frac{n imes d}{||n imes d||}]$

其中 $psi=[psi_{1},psi_{2},psi_{3}]$ 表示直線坐標系繞x-，y-，z-軸的旋轉夾角。

包含一個自由度，可以用三角函數矩陣參數化，如下

$W = left( egin{array}{l} cos (phi );;; - sin (phi )\ sin (phi );;;;cos (phi ) end{array} ight){ m{ = }}frac{1}{{sqrt {(||n|{|^2} + ||d|{|^2})} }}left( egin{array}{l} ||n||;;; - ||d||\ ||d||;;;||n|| end{array} ight)$

隱含了坐標原點到直線的距離，定義 $O=[psi, phi]^{T}$ 為三維直線進行參數優化時的的最小參數化表達。

${L^w} = Uleft[ egin{array}{l} left| n ight|;;;;;;0\ 0;;;;;;;left| d ight|\ 0;;;;;;;;0 end{array} ight] = [frac{n}{{||n||}};;;;frac{d}{{||d||}};;;;frac{{n imes d}}{{||n imes d||}}]left[ egin{array}{l} left| n ight|;;;;;;0\ 0;;;;;;;left| d ight|\ 0;;;;;;;;0 end{array} ight]$

上式中兩個實數（||n||,||d||）也包含一個自由度

因此可以用參數化

令

$egin{array}{l} {w_1} = cos phi = frac{{||n||}}{{sqrt {(||n|{|^2} + ||d|{|^2})} }}\ {w_2} = sin phi = frac{{||d||}}{{sqrt {(||n|{|^2} + ||d|{|^2})} }} end{array}$

3直線的重投影誤差測量模型

相機坐標系中三維空間直線可以通過投影變換投影到圖像平面，c表示在相機坐標系下。

$I = left[ egin{array}{l} {l_1}\ {l_2}\ {l_3} end{array} ight] = Kn = left[ egin{array}{l} {f_y};;;;;;;;;;;0;;;;;;;;;0\ 0;;;;;;;;;;;;{f_x};;;;;;;;0\ - {f_y}{c_x};; - {f_x}{c_y};;;;;{f_x}{f_y} end{array} ight]n$

n為普呂克表達中的法向量，自然也在相機坐標系下。

$l_{1}，l_{2}，l_{3}$ 為直線的係數。K為投影矩陣。 ${f_x}, {f_y}, {c_x}, {c_y}$ 為相機內參數裡面的焦距和圖像中心。

重投影誤差定義為投影的直線與檢測的端點s和e之間的距離誤差

在相機坐標系 $c_{i}$ 下，第條直線的重投影誤差為

${r_l} = left[ egin{array}{l} d(s_l^{{c_i}},I_l^{{c_i}})\ d(e_l^{{c_i}},I_l^{{c_i}}) end{array} ight]$

d(s,l)是檢測到的線段端點s到投影直線之間的距離

$d(s,l) = frac{{{s^T}I}}{{sqrt {l_1^2 + l_2^2} }}$

通過最小化重投影誤差可以優化第i幀相機坐標系的位姿 $T_{c_{i}w}$ 和第條空間直線的參數 $O_{l}$

4直線優化求導

優化過程涉及的雅克比矩陣可以用鏈式法則進行求導得到：

${J_l} = frac{{partial {r_l}}}{{partial {I^{{c_i}}}}}frac{{partial {I^{{c_i}}}}}{{partial {L^{{c_i}}}}}frac{{partial {L^{{c_i}}}}}{{partial {L^w}}}frac{{partial {L^w}}}{{partial delta O}}$