實作 SO(3) 對數映射

上一篇文章簡介了 SO(3) 的指數映射，而這篇文章要介紹的是對數映射，也就是給定一個旋轉矩陣 R，如何算出對應的李代數向量 \(\phi\)。

上一篇文章中我們推導出以下指數映射的式子： \[ exp(\phi) = R = I + \frac{(1-cos\ \theta)}{\theta^2} \phi^{\wedge}\phi^{\wedge} + \frac{sin \theta}{\theta}\phi^{\wedge} \] 其中的 \(\phi^{\wedge}\phi^{\wedge}\) 是個對稱矩陣（參考前文）： \[ \phi^{\wedge}\phi^{\wedge} = \phi \phi^T - I \] 而 \(\phi^{\wedge}\) 是個反對稱矩陣： \[ (\phi^{\wedge})^T = -\phi^{\wedge} \] 我們如果將 \(R\) 轉置後可以得到： \[ R^T = I + \frac{(1-cos\ \theta)}{\theta^2} (\phi^{\wedge}\phi^{\wedge})^T + \frac{sin \theta}{\theta}(\phi^{\wedge})^T \\ = I + \frac{(1-cos\ \theta)}{\theta^2} \phi^{\wedge}\phi^{\wedge} - \frac{sin \theta}{\theta}\phi^{\wedge} \] 接著計算 \(R - R^T\)： \[ R - R^T = \frac{2 sin \theta}{\theta}\phi^{\wedge} \] 因此可以利用 vee operator 算出 \(\phi\)： \[ \phi = (\frac{\theta}{2 sin \theta}(R - R^T))^{\vee } \] 至於 \(\theta\) 怎麼算呢？我們重新回到 Rodrigues' Formula： \[ R = cos\ \theta I + (1-cos\ \theta)nn^T + sin\ \theta n^{\wedge} \] 我們取 \(R\) 的 trace： \[ tr(R) = cos\ \theta tr(I) + (1-cos\ \theta)\ tr(nn^T) + sin\ \theta \ tr(n^{\wedge}) \\ = 3cos\ \theta + (1 - cos\ \theta) \\ = 1 + 2cos\ \theta \] 因此： \[ \theta = arccos \frac{tr(R) - 1}{2} \] 

以上的式子為一般的形式，而實作上在考慮邊界條件時必須用特別的形式，可以參考這份投影片的第 23 頁。