SLAM中旋转向量（旋转轴/旋转角）、旋转矩阵、四元数、李代数的相互转化（附C++ Eigen库代码实例）

最新推荐文章于 2024-03-27 14:05:10 发布

河北一帆

最新推荐文章于 2024-03-27 14:05:10 发布

阅读量2.6k

点赞数 4

文章标签：矩阵线性代数

本文链接：https://blog.csdn.net/qq_41816368/article/details/127763879

版权

旋转轴/旋转角、旋转矩阵、四元数、李代数都可以表示旋转，那么这几者的转换是如何实现的呢？

绕一个轴 $n$ ，旋转角度 $\theta$ 的旋转，例如，在三维空间中，以 $(0,0.7071,0.7071)$ 为轴，旋转45°，表示为 $n=(0,0.7071,0.7071), \theta=\pi/4$ ，注意，旋转轴模长要化为1，旋转角度乘旋转轴即为旋转向量

假如有一个点P(1,2,3), 那么点P绕轴（0，0.7071，0.7071）转 $\pi/4$ ，后的位置 ${P}'=RP$ ，R为旋转矩阵，旋转向量到旋转矩阵的转化通过罗德里格斯公式实现

n右上面一个小帽子表示将向量 n(n1, n2, n3) 转化为反对称矩阵（skew-symmetirc）

$n^{\wedge}=\left [ \begin{matrix} 0& -n_{3} &n_{2} \\ n_{3}& 0& -n_{1} \\ -n_{2}& n_{1}& 0 \end{matrix} \right ]$

从旋转矩阵转化回旋转轴和旋转角的方法是对旋转矩阵求迹tr(R)

转轴n是旋转矩阵R特征值1所对应的特征向量。通过特征向量的求解方法求解旋转轴（Eigen库中有现成的轮子）

旋转轴/旋转角到四元数的方法

四元数到旋转矩阵的转换方法

下面说明旋转矩阵到李代数的转化方法，介绍李代数之前，先说明李群的概念，一种集合加上一种运算，就构成了群，比如：旋转矩阵和矩阵乘法构成一个群，如果一个群，具有连续（光滑）性质，则称为李群，旋转矩阵和矩阵乘法构成一个李群。每个李群，都有一个与之对应的李代数，因此，每个旋转矩阵，都有一个与之对应的李代数。

旋转矩阵R所属的李群用SO(3)表示，它对应的李代数用 $so(3)$ 表示(应该用哥特体写，但是我不知道csdn怎么打哥特体)，李代数体现为一个三维向量 $\phi$ 。

$\phi=\theta n$ ，即：旋转角乘旋转轴得到的旋转向量，就是旋转矩阵R对应的李代数。

已知李代数，转为旋转矩阵，首先将 $\phi$ 转成模长为1的旋转轴和旋转角，在根据旋转轴和旋转角求旋转矩阵。

上面说明了旋转，如果一个旋转加上平移该如何求解？（比如沿向量 $t(t_{1},t_{2},t_{3})$ 进行平移）

将旋转和平移结合，将三维坐标末尾加1，变成四维向量，成为齐次坐标。

对于一个点 $a$ ，经过平移和旋转，得到点 ${a}'$

变换矩阵T同样属于李群，用SE(3)表示，同理，每一个T都有对应的李代数 $se(3)$ ，体现为一个六维向量 $\xi=\begin{bmatrix} \rho\\ \phi \end{bmatrix}\subseteq R^{6}$ ，式中， $\phi$ 是旋转矩阵的李代数， $\rho$ 通过平移向量t求解。

（注意，求J的公式里的 $a$ 是旋转轴，这里把字母 $n$ 换成了 $a$ ）

c++ Eigen库代码示例如下：注，Eigen库中没有李代数，再引用Sophus库

#include <iostream>
#include <cmath>
#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>
#include <Eigen/Eigen>

// Sophus库中李群和李代数运算
#include "so3.cpp"
#include "se3.cpp"

#define M_PI 3.1415926
using namespace std;

int main(int argc, char** argv){
	double theta = M_PI / 4;	// 旋转角
	Eigen::Vector3d a = Eigen::Vector3d(0, 0.7071, 0.7071);	// 旋转轴
	Eigen::AngleAxisd rotation_vector(theta, a);	// 旋转向量
	// 旋转向量到旋转矩阵
	Eigen::Matrix3d R = rotation_vector.toRotationMatrix();
	// 旋转矩阵到旋转向量
	cout << "theta=" << acos((R.trace() - 1) / 2) << endl;
	Eigen::EigenSolver<Eigen::MatrixXd> es(R);
	cout << "第一种：" << endl;
	cout << "特征值为：" << endl;
	cout << es.eigenvalues() << endl;
	cout << "特征向量为：" << endl;
	cout << es.eigenvectors() << endl << endl;
	// se(3)中J的求解
	Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
	Eigen::Matrix3d J = sin(theta) / theta * I + (1 - sin(theta) / theta) * a * a.transpose() + (1 - cos(theta)) / theta * Sophus::SO3::hat(a);
	Eigen::Vector3d t = Eigen::Vector3d(1, 3, 4)
	Eigen::Vector3d rho = J.inverse() * t;
	cout << "rho:\n" << endl;
	cout << rho << endl;

	// 直接用Sophus库求李代数
	Sophus::SO3 SO3_R(R);
	Eigen::Vector3d so3 = SO3_R.log();

	// 四元数
	// 可以直接把AngleAxis赋值给四元数，反之亦然
	Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0.7071, 0.7071));
	Eigen::Matrix3d rotation_matrix = rotation_vector.toRotationMatrix();

	Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
	cout << "quaternion = \n" << q.coeffs() << endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
	// 也可以把旋转矩阵赋给它
	q = Eigen::Quaterniond(rotation_matrix);
	cout << "quaternion = \n" << q.coeffs() << endl;
	// 使用四元数旋转一个向量，使用重载的乘法即可
	v_rotated = q * v; // 注意数学上是qvq^{-1}
	cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;

	return 0;
}

参考文献：《视觉SLAM十四讲》——高翔，张涛