高翔视觉slam十四讲书籍笔记(第三讲04-罗德里格斯公式推导)

罗德里格斯（Rodrigues）旋转方程

此公式有2种形式，故而也有2种推导方法。这里只推导书籍里面的这个公式。
书籍里的条件如下：假设有一个旋转轴为n, 角度为 θ 旋转，显然，它对应的旋转向量为θn。

罗德里格斯公式推导

假设V在三维空间中，绕单位旋转轴 k ，旋转 θ ，到V_rot。

从上图可以看出，对于向量 V 可以分出两个分向量:

V = V_‖ + V_⊥

其中 V_‖ 平行于旋转轴 k ,由点乘的投影几何意义可得(V•k为标量，所以再乘k得到一个矢量):

V_‖ = (V · k)k

**V_⊥**垂直于旋转轴 k，根据向量减法可得:

V_⊥ = V - V _‖ = V - ( k · V) k = - k × ( k × V)

从V - (k · V)k = -k × (k × V)不知道是什么怎么实现变换的，但是从图上可以看出来，二者相等

在这里我们将两个分向量带进去得到：

V = ( k ⋅ V) k − k × ( k × V)

向量k×V可以看作 V_⊥ 绕 k 反向旋转90°。
看一下向量叉乘的几何意义：

叉乘公式和点乘公式转换：

a × ( b × c) = ( a ⋅ c) b − ( a ⋅ b) c

旋转之后的 V_rot 为分向量为 V_‖rot ：

V _‖rot = V _‖
| V _⊥rot| = | V_⊥|
V~⊥rot ~= b + a = cos θV_⊥ + sin θk x V_⊥

a和b是由V_rot⊥正交分解得到的矢量，既有大小又有方向，所以在求解时，我们要对其大小和方向分别求解。 一、b的求解
1.大小 由图得：|b| = cosθ’ |V_⊥rot| ( θ’为b和V⊥rot的夹角 ） 又因为|V_⊥rot| = |V_⊥| ，θ’= π - θ 所以：|b| = cos(π -
θ) |V_⊥| = (- cosθ)|V_⊥|

2.方向 由 b 的方向与 V_⊥ 方向相反可得
b的单位方向向量为：-V_⊥ / |V_⊥| 综上可得：b= (-V_⊥ / |V_⊥| ) |b| = cosθV_⊥

二、a的求解
1.大小 |a| = sinθ’ |V_⊥rot| = sinθ’ |V_⊥| = sin(π - θ) |V_⊥| = sinθ|V_⊥|
= sinθsinθ’’|V| (θ’‘为V和k的夹角）
= sinθsinθ’’|V| |K| (因为 |K|=1，所以可直接加上） 其中sinθ’’|V| |K| = |K x V| 所以：|a| = sinθ
|K x V|

2.方向 由叉乘方向可得 a 的单位方向向量为
K x V / |K x V| 综上可得：a = (K x V / |K x V|) sinθ |K x V| = sinθ K x V

其中：

k × V_⊥ = k × (V − V_∥) = k × V − k × V_∥ = k × V

因此:

V_⊥rot = cosθV_⊥ + sinθk × V

旋转后的向量 Vrot为：

V_rot = V_∥rot + V_⊥rot
V_rot = V∥ + cosθv_⊥ + sinθk × V
=V∥ + cosθ(V−V_∥) + sinθk × V
=cosθV + (1−cosθ)V_∥ + sinθk × V
=cosθV + (1−cosθ)(k⋅V)k + sinθk × V

其中：

(k⋅V)k = k(k⋅V) = k(k^TV) = (kk^T)V

所以(I 为大写的 i)：

V_rot = (cosθI + (1 - cosθ)kk^T + sinθk^)V

注意，如果在

V_rot = V_∥ + cosθV_⊥ + sinθk x V

中，将V_∥替换为 V - V_⊥,再将V_⊥替换为 -k x (k x V) 则：