2021年1月21日 星期四

實作 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 頁。

沒有留言:

張貼留言