一、罗德里格斯公式（Rodrigues’ rotation formula）是一个用于计算绕任意轴旋转向量的数学公式。它是由欧仁·罗德里格斯（Olinde Rodrigues）在1840年提出的。这个公式在三维空间中描述了一个向量绕着单位向量旋转θ角度后的结果。
设v是一个待旋转的三维向量，k是单位旋转轴（必须是单位向量），θ是旋转角度（通常以弧度为单位），则v绕k旋转θ角度后的向量v’可以通过罗德里格斯公式计算得到：
v’ = v * cos(θ) + (k × v) * sin(θ) + k * (k ⋅ v) * (1 - cos(θ))
其中：
-
v’ 是旋转后的向量。
-
cos 和 sin 分别表示余弦和正弦函数。
-
× 表示向量叉乘。
-
⋅ 表示向量点乘。
罗德里格斯公式是由以下几个部分组成的：
-
v * cos(θ)：v 在旋转轴平行方向的分量，在旋转中不会改变。
-
(k × v) * sin(θ)：v 在垂直于旋转轴的平面上的分量，绕轴旋转后的分量。
-
k * (k ⋅ v) * (1 - cos(θ))：v 在旋转轴方向上的分量，这部分分量在旋转中增强了旋转轴方向的分量。
在图形学、机器人技术、航空航天和其他需要旋转向量的领域中，罗德里格斯公式是一种简单而有效的计算工具。此外，罗德里格斯公式也常被用来构造旋转矩阵或四元数，这在实现三维旋转时非常有用。
二、从轴角变换到旋转矩阵可以使用罗德里格斯公式进行转换
罗德里格斯公式可以用来从轴角表示（axis-angle representation）转换到旋转矩阵。轴角表示是一种描述旋转的方式，它使用一个单位旋转轴 k 和旋转角度 θ 来表示三维空间中的旋转。
给定单位旋转轴 k = (kx, ky, kz).T和旋转角度 θ，可以通过罗德里格斯旋转公式构造对应的旋转矩阵 R：
R = I + sin(θ) * K + (1 - cos(θ)) * K^2；
有的教材也写成：
R = cos(θ) * I + sin(θ) * K + (1 - cos(θ)) * (k * k.T)，其中k = (kx, ky, kz).T为列向量
这两个公式其实是一样的。
这里：
-
I 是单位矩阵。
-
K 是根据旋转轴 k 定义的反对称矩阵。其形式如下：
-
K^2 表示矩阵 K 与自身的矩阵乘积。
将 K 和 K^2 计算出来并代入上述公式，就可以得到完整的旋转矩阵 R。这个矩阵可以用来作用于任意向量 v，即通过矩阵-向量乘法 Rv 来实现向量 v 绕轴 k 旋转 θ 角度的效果。
python案例
R = I + sin(θ) * K + (1 - cos(θ)) * K^2；
import numpy as np
# 这边没有转变成列向量，因为不需要，之后都是大K的计算
k = np.array([1, 0, 0])
# x轴旋转90度
theta = np.pi / 2
K = np.array([[0, -k[2], k[1]],[k[2], 0, -k[0]],[-k[1], k[0], 0]])
K_squared = np.dot(K, K)
# 通过罗德里格斯公式计算旋转矩阵
R = np.eye(3) + (1 - np.cos(theta)) * K_squared + np.sin(theta) * K
print(np.round(R, 5))R = cos(θ) * I + sin(θ) * K + (1 - cos(θ)) * (k * k.T)
import numpy as np
# 这边特意把k转变成列向量
k = np.array([1, 0, 0]).reshape((-1, 1))
theta = np.pi / 2
K = np.array([[0, -k[2][0], k[1][0]],[k[2][0], 0, -k[0][0]],[-k[1][0], k[0][0], 0]])
# K_squared = np.dot(K, K)
print(k, k.T)
K_squared = np.dot(k, k.T)
print(K_squared)
R = np.cos(theta) * np.eye(3) + (1 - np.cos(theta)) * K_squared + np.sin(theta) * K
print(np.round(R, 5))结果都是一样的，最多就是精度有所差别
