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

旋转向量 rotation vector

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

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

特别注意:

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

旋转矩阵 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 : 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 的函数计算结果一致, 验证了代码的正确性.

收集整理和创作不易, 若有帮助