文章目录
- 旋转向量 rotation vector
- 旋转矩阵 rotation matrix
- 罗德里格斯公式 Rodrigues’ formula
- 基于 Python 和 NumPy 实现 Rodrigues 公式
旋转向量 rotation vector
任何一个旋转都可以通过一个 旋转轴 加一个 旋转角 进行描述, 即围绕 旋转轴 旋转一个 旋转角. 此时可以通过一个 旋转向量 来描述这组 旋转轴 和 旋转角 .
旋转向量 的方向与 旋转轴 一致, 旋转向量 的长度等于 旋转角 的大小.
特别注意:
- 此处
旋转角的单位为 弧度. - 旋转向量不等于欧拉角, 旋转向量可以看做是绕着旋转轴一次性完成旋转, 而欧拉角则是绕着当前坐标系的坐标轴依次旋转.
旋转矩阵 rotation matrix
任何一个旋转运动都可以通过一个
3
×
3
3times3
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
3times3
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=[xyz]Tn∧=
0z−y−z0xy−x0
基于 Python 和 NumPy 实现 Rodrigues 公式
注意: 此处rot_vector使用的是列向量.
#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@File : rodrigues.py
@Time : 2023/06/08 14:21:32
@Author : KRISNAT
@Version : 0.0
@Contact : krisnat.ton@gmail.com
@License : (C)Copyright 2023, KRISNAT.
@Desc : None
'''
import cv2
import numpy as np
def rodrigues(rot_vector):
""""""
theta = np.linalg.norm(rot_vector)
rot_vector = np.array(rot_vector).reshape(3, 1) / theta
K = 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 的函数计算结果一致, 验证了代码的正确性.
收集整理和创作不易, 若有帮助🉑, 请帮忙点赞👍➕收藏❤️, 谢谢!✨✨🚀🚀
服务器托管，北京服务器托管，服务器租用 http://www.fwqtg.net
源创会，线下重启！2023年7月1日深圳站—基础软件技术面面谈！早鸟票限时抢购！ 夏天来了，没想到连 ibdata1 文件也要开始“减肥”了~~~ 作者：杨彩琳 爱可生华东交付部 DBA，主要负责 MySQL 日常问题处理及 DMP 产品支持。爱好跳舞，追剧。…
