四元數矩陣與 so(3) 左右雅可比

04-01

我們知道，單位四元數 q 和 so(3) 向量 $oldsymbolphi$ （即 rotation vector）的對應關係為：

${f q}({m phi}) = [ sinfrac{phi}{2} {f a}, , cos frac{phi}{2}]$ , in which $phi = | m phi|, quad {f a} = frac{m phi}{phi}$

當 $phi$ 很小時，可以近似表達為：

${f q}({m phi}) = [ frac{1}{2} {f m phi}, , 1]$

四元數小量和李代數小量有很簡單的對應關係，所以在使用四元數的優化問題中，往往也取 $deltam phi$ 小量作為更新量，例如 OKVIS [1]。

具體到優化過程中，使用李群李代數（如預積分論文 [2]）和使用四元數之間還是有區別的。使用李群李代數，在 residual 推導時會用到 so3 的左雅可比或右雅可比；使用四元數，會用到四元數矩陣（或共軛四元數矩陣）。不過既然兩者使用的小量一致，那四元數矩陣和 so3 的左右雅可比的關係如何呢？

之所以有這個疑問，是因為 Xingyin 兄在博客里指出 [3]，OKVIS 代碼中使用了預積分，代碼和預計分論文里的思路一致。不過實際上，雖然預積分流程一致，兩者在雅可比推導上有所不同：預積分使用 so3 的右雅可比；OKVIS 使用四元數矩陣。所以，我們有必要把這兩者的關係釐清。

四元數矩陣

兩個旋轉疊加，可以表達為兩個四元數相乘，或者一個 4*4 矩陣與一個四元數相乘：

${f q otimes p = Q}_l({f q)p} = {f Q}_r{f (p)q}$

這裡的 ${f Q}_l$ 為四元數矩陣， ${f Q}_r$ 為共軛四元數矩陣：

${f Q}_l({f q}) = egin{bmatrix} q_w {f I}_3 + {f q}_{1:3}^wedge & {f q}_{1:3}\ - {f q}_{1:3} & q_w end{bmatrix}$ , ${f Q}_r ({f q}) = egin{bmatrix} q_w {f I}_3 - {f q}_{1:3}^wedge & {f q}_{1:3}\ - {f q}_{1:3} & q_w end{bmatrix}$

兩種矩陣對於求導非常方便：

$frac{partial{f q otimes p}}{partialf p} = {f Q}_l({f q}), ,frac{partial{f q otimes p}}{partialf q} = {f Q}_r ({f p})$

現在，假設有個一個待估計的旋轉量 q ，其更新方式為 $f q leftarrow q otimes q(malpha)$ ， $m alpha$ 為 so3 小量。假定有 q 的測量量 $f ilde q$ ，定義誤差函數 ${f e}_{ m qt}$ ：

${f e}_{ m qt} = 2( ilde {f q}^{-1}otimes {f q})_{1:3}$

則其相對於更新量的雅可比為

$egin{aligned} frac{partial {f e}_{ m qt}}{partial m alpha} &= 2frac{partial ( ilde{f q}^{-1}otimes{f q}otimes {f q}({m alpha}))}{partial m alpha}Bigg | _{(1:3, :)} \ &=2frac{partial ( ilde{f q}^{-1}otimes{f q}otimes {f q}({m alpha}))}{partial {f q}({m alpha})} frac{partial {f q}({m alpha})}{partial m alpha}Bigg | _{(1:3, :)} \ &={f Q}_l( ilde{f q}^{-1}otimes{f q}) egin{bmatrix} {f I}_3 \ {f 0_{1 imes 3}} end{bmatrix}Bigg | _{(1:3, :)} \ &={f Q}_l( ilde{f q}^{-1}otimes{f q}) _{(1:3, 1:3)} end{aligned}$

即 $frac{partial {f e}_{ m qt}}{partial m alpha} ={f Q}_l[{f q}({f e}_{ m qt})] _{(1:3, 1:3)}$

so(3) 雅可比

據 Barfoot [4]，若 $mphi$ 的左雅可比為 ${f J}_l({m phi})$ ，右雅可比為 ${f J}_r({m phi})$ ：

於是，假設有待估計量 C，更新方式為 ${f C} leftarrow {f C}, { m Exp}({m alpha})$ ， $m alpha$ 為 so3 小量。假定有 C 的測量量 $f ilde C$ ，定義誤差函數 ${f e}_{ m lie}$ ：

${f e}_{ m lie} = { m Log}( ilde {f C} {f C})$

則其相對於更新量的雅可比為：

$egin{aligned} frac{partial {f e}_{ m lie}}{partial m alpha} &= frac{partial{ m Log}( ilde{f C}{f C} ,{ m Exp} ({m alpha}))}{partial m alpha} \ & = { m J}_r({ m Log( ilde{f C}{f C} )})^{-1}\ & = { m J}_r({f e}_{ m lie})^{-1} end{aligned}$

討論

我們來比較一下 $frac{partial {f e}_{ m qt}}{partial m alpha}$ 和 $frac{partial {f e}_{ m lie}}{partial m alpha}$ 。

首先，

$egin{aligned} frac{partial {f e}_{ m qt}}{partial m alpha} &={f Q}_l[{f q}({f e}_{ m qt})] _{(1:3, 1:3)}\ &=q_w({f e}_{ m qt}) {f I}_3 + {f q}_{1:3}^wedge({f e}_{ m qt}) end{aligned}$

如果可以認為誤差函數 ${f e}_{ m qt}$ 很小，則

$frac{partial {f e}_{ m qt}}{partial m alpha} approx {f I}_3 + frac{1}{2}{f e}_{ m qt}^wedge$

另一方面，據 Barfoot [4]，有

$frac{partial {f e}_{ m lie}}{partial m alpha} = { m J}_r({f e}_{ m lie})^{-1} = frac{phi_{e}}{2}cotfrac{phi_{e}}{2} {f I}+(1-frac{phi_{e}}{2}cotfrac{phi_{e}}{2}){f a_{it e}a_{it e}} + frac{phi_{e}}{2}{f a}_{e}^wedge$

如果可以認為誤差函數很小，即 $phi_{e} ightarrow 0$ ，則 $frac{phi_{e}}{2}cotfrac{phi_{e}}{2} ightarrow 1$ ，於是

$frac{partial {f e}_{ m lie}}{partial m alpha} approx {f I}+frac{phi_{e}}{2}{f a}_{e}^wedge={f I} + frac{1}{2}{f e}_{ m lie}^wedge$ 。

可以看到，當可以認為誤差函數很小時， $frac{partial {f e}_{ m qt}}{partial m alpha}$ 和 $frac{partial {f e}_{ m lie}}{partial m alpha}$ 是相等的。這其實很好理解，因為當 ${f e}_{ m qt} = 2( ilde {f q}^{-1}otimes {f q})_{1:3}$ 很小時， ${f q}({m phi}) = [ frac{1}{2} {f m phi}, , 1]$ 估計成立，所以 ${f e}_{ m qt}, {f e}_{ m lie}$ 實際上就表示同一個量，都是誤差旋轉量的李代數，推導出來的雅可比自然就一樣了，我們只是通過不同的途徑去算同一個量而已。這個例子的價值在於，展示了四元數矩陣和 so3 雅可比之間的具體關係，即：

如果有可以認為很小的 so3 量，如某個誤差函數 ${f e}_{phi}$ ，則

${f J}_r({f e}_{phi})^{-1} = {f Q}_l({f q}({f e}_{phi}))_{(1:3,1:3)}$

上面用的是右擾動模型；如果使用左擾動，可類似地得到 ${f J}_l({f e}_{phi})^{-1} = {f Q}_r({f q}({f e}_{phi}))_{(1:3,1:3)}$ 。

尾聲

如果 $phi$ 不能認為很小， ${f J}_r(m phi)^{-1}$ 與 ${f Q}_l({f q}( {mphi} ))$ 的關係是怎樣？並不複雜。區別僅在於此時 ${f q}({m phi}) = [ frac{1}{2} {f m phi}, , 1]$ 的估計不能成立， $2{f q}_{1:3}$ 和 $m phi$ 不能當作一個量而已；但它們之間的相互關係是已知的： ${f q}_{1:3}= sinfrac{|mphi|}{2} cdot frac{m phi}{|mphi|}$ 。在上面的演算中，我們實際上得到的結果是

$frac{partialm phi}{partial{m alpha}}={f J}_r({mphi})^{-1},quad frac{partial{f q}_{1:3}}{partial{m alpha}}={f Q}_l({f q}({mphi}))_{(1:3,1:3)}$

可以證明：

${f J}_r({mphi})^{-1} = frac{partialm phi}{partial{f q}_{1:3}} ,{f Q}_l({f q}({mphi}))_{(1:3,1:3)}$ 。

代入過程就不貼了。

參考文獻

[1] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, P. Furgale, 「Keyframe-based visual-inertial odometry using nonlinear optimization」, Int. Journal of Robotics Research (IJRR), 2014.

[2] C. Forster, L. Carlone, F. Dellaert and D. Scaramuzza, "On-Manifold Preintegration for Real-Time Visual--Inertial Odometry," in IEEE Transactions on Robotics, vol. 33, no. 1, pp. 1-21, Feb. 2017.

[3] OKVIS IMU 誤差公式代碼版本

[4] T. Barfoot, State Estimation for Robotics.