视觉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 × v k \times v