【备份自用】Eigen库中欧拉角、方向余弦矩阵、四元数、旋转矢量之间的转换

简单整理一下c++ Eigen库中姿态--欧拉角、方向余弦矩阵、四元数、旋转矢量之间的转换。

引用头文件<Eigen/Geometry>， 这个模块的官方文档：Eigen: Space transformations

#include<Eigen/Geometry>

using Eigen::Matrix3d;
using Eigen::Quaterniond;
using Eigen::Vector3d;

代码中的定义并没有完全按照Eigen自带的来：

Vector3d euler;                //欧拉角（row，pitch，yaw）
Matrix3d DCM;                  //方向余弦矩阵
Quaterniond q;                 //四元数
Vector3d rotvec;               //等效旋转矢量，Eigen自带了一个AngleAxis轴角，但不能直接与矩阵/向量运算

1、欧拉角与方向余弦矩阵

//方向余弦矩阵-->欧拉角
Vector3d DCM2Euler(Matrix3d DCM)
{
	double pitch = atan(-DCM(2, 0) / sqrt(DCM(2, 1) * DCM(2, 1) + DCM(2, 2) * DCM(2, 2)));	//[-pi/2, pi/2]
	double roll, yaw;
	if (DCM(2, 0) <= -0.999) //奇异值
	{
		roll = 0;
		yaw = atan2((DCM(1, 2) - DCM(0, 1)), (DCM(0, 2) + DCM(1, 1)));
	}
	else if (DCM(2, 0) >= 0.999) 
	{
		roll = 0;
		yaw = pi + atan2((DCM(1, 2) + DCM(0, 1)), (DCM(0, 2) - DCM(1, 1)));
	}
	else 
	{
		roll = atan2(DCM(2, 1), DCM(2, 2));
		pitch = atan2(DCM(1, 0), DCM(0, 0));
	}

	Vector3d euler;
	euler << roll, pitch, yaw;
	return euler;
}

//欧拉角-->方向余弦矩阵(公式)
Matrix3d Euler2DCM1(Vector3d euler)
{
	Matrix3d DCM;
	double roll = euler(0), pitch = euler(1), yaw = euler(2);
	DCM << cos(pitch) * cos(yaw), -cos(roll) * sin(yaw) + sin(roll) * sin(pitch) * cos(yaw), sin(roll)* sin(yaw) + cos(roll) * sin(pitch) * cos(yaw),
		cos(pitch)* sin(yaw), cos(roll)* cos(yaw) + sin(roll) * sin(pitch) * sin(yaw), -sin(roll) * cos(yaw) + cos(roll) * sin(pitch) * sin(yaw),
		-sin(pitch), sin(roll)* cos(pitch), cos(roll)* cos(pitch);
	return DCM;
}

//欧拉角-->方向余弦矩阵(Eigen1)
Matrix3d Euler2DCM2(Vector3d euler) 
{
    return Matrix3d(Eigen::AngleAxisd(euler[2], Vector3d::UnitZ()) *
                    Eigen::AngleAxisd(euler[1], Vector3d::UnitY()) *
                    Eigen::AngleAxisd(euler[0], Vector3d::UnitX()));
}

//欧拉角-->方向余弦矩阵(Eigen2)
Matrix3d Euler2DCM3(Vector3d euler) 
{
    Matrix3d DCM = (Eigen::AngleAxisd(euler[2], Vector3d::UnitZ()) *
                   Eigen::AngleAxisd(euler[1], Vector3d::UnitY()) *
                   Eigen::AngleAxisd(euler[0], Vector3d::UnitX())).toRotationMatrix();
    return DCM;
}

DCM2Euler() 中，当俯仰角接近 ±90 度时，即 DCM(2,0) = -sin(pitch) 趋近 ±1 时，出现万向锁问题，这里人为设横滚角为0，全部给航向角。

Euler2DCM() 提供三种方法，第一种套公式，第二、三种直接用Eigen自带的函数，都是先将欧拉角转换为Eigen自带的旋转矢量AngleAxis，再转为方向余弦矩阵，只是表示略有不同。

函数AngleAxisd(angle, axis)，angle是旋转的角度（以弧度为单位），axis是一个Vector3d类型的旋转轴向量，表示绕 axis 轴旋转 angle 角度。AngleAxisd的乘法表示从左往右依次绕各轴旋转一定角度，返回的仍然是一个AngleAxisd变量，也就是两次变换后的旋转矢量。

Vector3d::UnitZ()这些…嗯，私认为是惯性系下的单位XYZ向量，毕竟，任给一组欧拉角，都可以求出其对应的DCM，变换过程是不变的，与是哪两个坐标系间的旋转变换无关。

toRotationMatrix()函数可以作用于AngleAxis、Quaternion和Transform等类的对象上。

（AngleAxisd等末尾的d表示double）

2、四元数与欧拉角

//四元数-->欧拉角
Vector3d Quaternion2Euler(Quaterniond q) 
{
	return DCM2Euler(q.toRotationMatrix());
}

//欧拉角-->四元数
Quaterniond Euler2Quaternion(Vector3d euler)
{
	return Quaterniond(Eigen::AngleAxisd(euler[2], Vector3d::UnitZ()) * 
                       Eigen::AngleAxisd(euler[1], Vector3d::UnitY()) * 
                       Eigen::AngleAxisd(euler[0], Vector3d::UnitX()));
}

四元数转欧拉角需要以方向余弦矩阵作为中介；四元数转欧拉角也是先将欧拉角转为 AngleAxis 再转为四元数。

3、四元数与方向余弦矩阵

//方向余弦矩阵-->四元数
Quaterniond DCM2Quaternion(Matrix3d matrix) 
{
	return Quaterniond(matrix);
}

//四元数-->方向余弦矩阵
Matrix3d Quaternion2DCM(Quaterniond q)
{
	return q.toRotationMatrix();
}

4、四元数与等效旋转矢量

//等效旋转矢量-->四元数
Quaterniond Rotvec2Quaternion(Vector3d rotvec) 
{
	double angle = rotvec.norm();
	Vector3d vec = rotvec.normalized();    //归一化
	return Quaterniond(Eigen::AngleAxisd(angle, vec));
}

//四元数-->等效旋转矢量
Vector3d Quaternion2Rotvec(Quaterniond q) 
{
	Eigen::AngleAxisd axisd(q);
	return axisd.angle() * axisd.axis();
}

等效旋转矢量与其它的转换可先转为四元数再继续。

欢迎指正！