视觉SLAM十四讲 罗德里格斯公式推导 全解

最近在学习高翔的视觉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\cdot 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\cdot c)b-(a\cdot b)c$，所以可以推出：
$$\begin{array}{rl}{v}_{\perp }=v-v_{||}& =v-(v\cdot k)k=(k\cdot k)v-(v\cdot 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}^{\top }}{k^{\top }k}v\\ & =k{k}^{\top }v\end{array}\text{\hspace{1em}(15)}$$

最终，联立公式14，15，即可得旋转向量与旋转矩阵之间的转换关系：
$$\begin{array}{r}R=cos\theta I+(1-cos\theta )k{k}^{\top }+sin\theta \hat{k}\end{array}\text{\hspace{1em}(16)}$$

2. 相关知识

2.1 点乘和叉乘

dot点乘：点乘又称內积。对于$$a\cdot b$$，其几何意义有：
① 点乘的结果是a在b上的投影的模乘以b的模
② 描述了向量之间的夹角
$$a\cdot b=a^{\top }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