文章目录

旋转向量 rotation vector旋转矩阵 rotation matrix罗德里格斯公式 Rodrigues' formula基于 Python 和 NumPy 实现 Rodrigues 公式

旋转向量 rotation vector

任何一个旋转都可以通过一个旋转轴加一个旋转角进行描述, 即围绕旋转轴旋转一个旋转角. 此时可以通过一个旋转向量来描述这组旋转轴和旋转角.

旋转向量的方向与旋转轴一致,旋转向量的长度等于旋转角的大小.

特别注意:

此处旋转角的单位为弧度.旋转向量不等于欧拉角, 旋转向量可以看做是绕着旋转轴一次性完成旋转, 而欧拉角则是绕着当前坐标系的坐标轴依次旋转.

旋转矩阵 rotation matrix

任何一个旋转运动都可以通过一个 3 × 3 3\times3 3×3的旋转矩阵R R R 进行描述. 旋转矩阵是一个单位正交矩阵(unit orthogonal matrix), 具有以下性质:

d e t ( R ) = 1 R T = R − 1 det(R)=1 \\ R^{T}=R^{-1} det(R)=1RT=R−1

罗德里格斯公式 Rodrigues’ formula

R = cos ⁡ ( θ ) I + ( 1 − cos ⁡ ( θ ) ) n n T + sin ⁡ ( θ ) n ∧ R=\cos (\theta )I + (1 - \cos(\theta))nn^{T}+\sin(\theta)n^{\wedge} R=cos(θ)I+(1−cos(θ))nnT+sin(θ)n∧

式中, R R R 表示 3 × 3 3\times3 3×3的旋转矩阵, θ \theta θ 为旋转角(单位: 弧度rad), n n n 为旋转向量除以 θ \theta θ 后得到的向量(有些文献也直接叫做旋转向量), n ∧ n^{\wedge} n∧表示 n n n对应的反对称矩阵, 如下所示:

n = [ x y z ] T n ∧ = [ 0 − z y z 0 − x − y x 0 ] n= \begin{bmatrix}x & y & z \end{bmatrix}^{T} \\ n^{\wedge}=\begin{bmatrix} 0 & -z & y \\ z & 0 & -x \\ -y & x & 0 \end{bmatrix} n=[x​y​z​]Tn∧= ​0z−y​−z0x​y−x0​ ​

基于 Python 和 NumPy 实现 Rodrigues 公式

注意: 此处rot_vector使用的是列向量.

#!/usr/bin/env python# -*- encoding: utf-8 -*-'''@File : rodrigues.py@Time : /06/08 14:21:32@Author : KRISNAT@Version : 0.0@Contact : krisnat.ton@@License : (C)Copyright , KRISNAT.@Desc : None'''import cv2import numpy as npdef rodrigues(rot_vector):""""""theta = np.linalg.norm(rot_vector)rot_vector = np.array(rot_vector).reshape(3, 1) / thetaK = np.asanyarray([[0, -rot_vector[2, 0], rot_vector[1, 0]],[rot_vector[2, 0], 0, -rot_vector[0, 0]],[-rot_vector[1, 0], rot_vector[0, 0], 0]])return np.asanyarray(np.cos(theta)*np.eye(3) + (1 - np.cos(theta))*rot_vector*rot_vector.T + np.sin(theta) * K )if __name__ == '__main__':print(f"Rodrigues by NumPy: ")print(rodrigues(rot_vector=np.asanyarray([0.0955415182750808, 0.0475064330746053,0.0485482885665371])))print(f"Rodrigues by OpenCV funtion: ")print(cv2.Rodrigues(np.asanyarray([0.0955415182750808, 0.0475064330746053,0.0485482885665371]))[0])

运行结果: 使用NumPy编写的 Rodrigues 公式计算旋转矩阵的结果与调用 OpenCV 的函数计算结果一致, 验证了代码的正确性.

收集整理和创作不易, 若有帮助🉑, 请帮忙点赞👍➕收藏❤️, 谢谢!✨✨🚀🚀