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

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

绕一个轴，旋转角度的旋转，例如，在三维空间中，以为轴，旋转45°，表示为，注意，旋转轴模长要化为1，旋转角度乘旋转轴即为旋转向量

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

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

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

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

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

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

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

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

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

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

上面说明了旋转，如果一个旋转加上平移该如何求解？（比如沿向量进行平移）

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

对于一个点，经过平移和旋转，得到点

变换矩阵T同样属于李群，用SE(3)表示，同理，每一个T都有对应的李代数，体现为一个六维向量，式中，是旋转矩阵的李代数，通过平移向量t求解。

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

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

#include 
#include 
#include 
// Eigen 几何模块
#include 
#include 
 
// 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 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十四讲》——高翔，张涛