详解基于罗德里格斯（Rodrigues）公式由旋转向量到旋转矩阵的 Python 实现

旋转向量 rotation vector

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

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

特别注意:

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

旋转矩阵 rotation matrix

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

$$\begin{gathered} det(R)=1 \\ {R}^T=R^{-1} \end{gathered}$$

罗德里格斯公式 Rodrigues’ formula

$$R=\cos (\theta )I+(1-\cos (\theta ))n{n}^T+\sin (\theta )n^{\wedge }$$

式中, $R$ 表示$3\times 3$的旋转矩阵, $\theta$ 为旋转角(单位: 弧度rad), $n$ 为旋转向量除以 $\theta$ 后得到的向量(有些文献也直接叫做旋转向量), $n^{\wedge }$表示$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 : 2023/06/08 14:21:32
@Author : KRISNAT
@Version : 0.0
@Contact : [email protected]
@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 的函数计算结果一致, 验证了代码的正确性.

---

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