視覺SLAM十四講 羅德里格斯公式推導（沒有省略任何步驟）

• 1. 羅德里格斯公式推導
• 2. 相關知識
• • 2.1 點乘和叉乘
  • 2.2 向量投影和向量投影矩陣
  • 2.3 雙重向量積

最近在學習高翔的視覺SLAM十四講，在第三章提到了羅德里格斯公式（Rodrigue’s Formula），但書中并沒有給出相應的推導公式。在網上搜了不上相關的博客，看了好幾篇才看明白，一些基礎知識并沒有在公式推導過程中給出，導致一開始看得云里霧里。

所以這篇博客就是總結羅德里格斯公式推導過程以及過程中所對應的知識點，包括一些基礎知識。我在這篇博客里的公式推導比較詳細，把一些其他博客省略的步驟加了進去。

1. 羅德里格斯公式推導

整個推導過程都是圍繞這上面這張圖展開的，其中$V$是要旋轉的向量，$V_{rot}$是旋轉后的向量，$k$是旋轉軸方向上的單位向量，$V_{||}$是$V$關于旋轉軸上的平行分量，$V_{\perp }$是$V$關于旋轉軸的垂直分量。

搞清楚定義之后，開始進行推導。

根據向量的加法運算：
$$V=V_{||}+V_{\perp }\text{\hspace{1em}(1)}$$

現假設$k$為旋轉軸上的單位向量，則$k$可以表示為：
$$k=\frac{a\times b}{|a\times b|}\text{\hspace{1em}(2)}$$

其中a和b為與k垂直的平面上的兩個向量，可以不用在意，只是表述k和平面的垂直關系，后續只用到k。
?

那么對于$V$的平行分量和垂直分量，利用向量點乘和叉乘的性質，有：
$$V_{||}=(v?k)k\text{\hspace{1em}(3)}$$
$$v_{\perp }=v?v_{||}=?k\times (k\times v)\text{\hspace{1em}(4)}$$
$$v_{||}=v?v_{\perp }=v+k\times (k\times v)\text{\hspace{1em}(5)}$$

公式3即點乘的性質，比較好理解。在公式4中，我們知道向量之間叉乘得垂直向量，而k是單位向量，則叉乘$k\times v$相當于$v_{\perp }$繞著k旋轉了90°，再叉乘一個k就相當于旋轉了180°，方向與$v_{\perp }$相反，所以再加個負號就為垂直分量。至于為什么要這么做，是因為利用叉乘將$v,v_{||},v_{\perp }$三個量關聯起來，后面的推導會用到。
?

對于公式4，還有一種推導方法，根據雙重向量積的性質，對于給定空間的三個向量a,b,c，有$a\times (b\times c)=(a?c)b?(a?b)c$，所以可以推出：
$$\begin{array}{rl}{v}_{\perp }=v?v_{||}& =v?(v?k)k=(k?k)v?(v?k)k\\ & =k\times (v\times k)=?k\times (k\times v)\end{array}\text{\hspace{1em}(6)}$$

在有了$v,v_{||},v_{\perp }$三個量之間的關系式后，再求旋轉后的向量分量$v_{rot||},v_{rot\perp }$：
$$v_{rot||}=v_{||}\text{\hspace{1em}(7)}$$
$$\begin{array}{rl}{v}_{rot\perp }=cos\theta v_{\perp }+sin\theta k\times v_{\perp }& =cos\theta v_{\perp }+sin\theta k\times (v?v_{||})\\ & =cos\theta v_{\perp }+sin\theta k\times v?0\\ & =cos\theta v_{\perp }+sin\theta k\times v\end{array}\text{\hspace{1em}(8)}$$

那么聯立公式4，5，7，8可得：
$$\begin{array}{rl}{v}_{rot}& =v_{rot||}+v_{rot\perp }\\ & =v_{||}+cos\theta v_{\perp }+sin\theta k\times v\\ & =v+k\times (k\times v)+cos\theta (?k\times (k\times v))+sin\theta k\times v\end{array}\text{\hspace{1em}(9)}$$
?

這是向量叉乘形式，現在轉化成矩陣相乘形式。對于向量叉乘$a\times b$，可以轉化為矩陣相乘$\hat{a}b$，其中$\hat{a}$表示a的反對成矩陣，那么將k轉化為反對稱矩陣K，就有：
$$k\times v=Kv\text{\hspace{1em}(10)}$$
$$k\times (k\times v)=K^{2}v\text{\hspace{1em}(11)}$$

聯立公式9，10，11，最后得$v_{rot}$的表達式：
$$v_{rot}=v+(1?cos\theta )K^{2}v+sin\theta Kv\text{\hspace{1em}(12)}$$

令$v_{rot}=Rv$，則可得羅德里格斯公式：
$$R=I+(1?cos\theta )K^2+sin\theta K\text{\hspace{1em}(13)}$$
?

上面的公式是單位向量k的反對稱矩陣形式，將其轉成視覺SLAM第三講的公式形式，可以利用向量投影矩陣。先將公式9轉換一下：
$$\begin{array}{rl}{v}_{rot}& =v_{rot||}+v_{rot\perp }\\ & =v_{||}+cos\theta v_{\perp }+sin\theta k\times v\\ & =cos\theta v+(1?cos\theta )v_{||}+sin\theta k\times v\end{array}\text{\hspace{1em}(14)}$$
因為$v_{||}$是v在k上的投影，并且k是單位向量，所以由向量投影矩陣，可得：
$$\begin{array}{rl}{v}_{||}& =\frac{k{k}^{?}}{k^{?}k}v\\ & =k{k}^{?}v\end{array}\text{\hspace{1em}(15)}$$

最終，聯立公式14，15，即可得旋轉向量與旋轉矩陣之間的轉換關系：
$$\begin{array}{r}R=cos\theta I+(1?cos\theta )k{k}^{?}+sin\theta \hat{k}\end{array}\text{\hspace{1em}(16)}$$

2. 相關知識

2.1 點乘和叉乘

dot點乘：點乘又稱內積。對于$$a?b$$，其幾何意義有：
① 點乘的結果是a在b上的投影的模乘以b的模
② 描述了向量之間的夾角
$$a?b=a^{?}b=|a||b|cos(a,b)$$
矩陣沒有點乘一說
?

cross product叉乘：叉乘又稱外積、向量積。對于$$a\times b$$，其幾何意義有：
① 代表了aOb平面的法向量
② 該法向量長度為$$|a||b|sin(a,b)$$
③ 上式也代表a和b組成的平行四邊形的面積
叉乘形成的法向量成右手法則，所以叉乘的順序會影響結果。
?

2.2 向量投影和向量投影矩陣

https://blog.csdn.net/wlk1229/article/details/84779370
?

2.3 雙重向量積

定義：給定空間的三個向量a,b,c,如果先做其中兩個向量a,b的向量積a×b,再做所得向量與第三向量的向量 積，那么最后的結果仍然是一個向量，叫做所給三向量的雙重向量積，記做：(a×b)×c。
?
性質：

• (a×b)×c=(a·c)b-(b·c)a
• a×(b×c)=-(b×c)×a=(a·c)b-(a·b)c

總結

以上是生活随笔為你收集整理的视觉SLAM十四讲 罗德里格斯公式推导 全解的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。