【十四讲】旋转向量、旋转矩阵、欧拉角

学习slam,依托计算机视觉life平台
（如有侵权，删除）
hw2
1、旋转向量与旋转矩阵
https://blog.csdn.net/qq_31806429/article/details/87920597
2、旋转矩阵与欧拉角
https://www.jianshu.com/p/5e130c04a602
3、四元数与矩阵
https://blog.csdn.net/loongkingwhat/article/details/88427828
4、代码实现
https://www.cnblogs.com/cc111/p/9354924.html
https://blog.csdn.net/dieju8330/article/details/104226386`
还有自己写的一部分代码，不太规范
注意四元数初始化为W,X,Y,Z，输出为X,Y,Z,W

#include <cmath>
#include<iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
using namespace std;
#define M_PI 3.14159265358979323846
int main(int argc, char** argv)
{
	// ----------  初始化 -----------//

	// 旋转向量（轴角）：沿Z轴旋转45°
	Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0, 1));
	cout << "rotation_vector axis = \n" << rotation_vector.axis() << "\n rotation_vector angle = " << rotation_vector.angle() << endl;

	//旋转矩阵：沿Z轴旋转45°
	 Eigen::Matrix3d rotation_matrix =Eigen::Matrix3d::Identity();
	//Eigen::Matrix<double, 3, 3> rotation_matrix;
	rotation_matrix << 0.707, -0.707, 0,
		0.707, 0.707, 0,
		0, 0, 1;
	cout << "rotation matrix =\n" << rotation_matrix << endl;

	// 四元数：沿Z轴旋转45°
	Eigen::Quaterniond quat = Eigen::Quaterniond(0, 0, 0.383, 0.924);// Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, w
	cout << "四元数输出方法1：quaternion = \n" << quat.coeffs() << endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部,相反w=0.924
	cout << "四元数输出方法2：\n x = " << quat.x() << "\n y = " << quat.y() << "\n z = " << quat.z() << "\n w = " << quat.w() << endl;

	// 欧拉角: ：沿Z轴旋转45°
	Eigen::Vector3d euler_angles = Eigen::Vector3d(M_PI / 4, 0, 0);// ZYX顺序，即roll pitch yaw顺序
	cout << "Euler: yaw pitch roll = " << euler_angles.transpose() << endl;



	//  相互转化关系 
	// ----------  请在下面补充对应的代码 ----------
	//旋转向量转矩阵
	double theta = rotation_vector.angle();
	Eigen::Vector3d n = rotation_vector.axis();

	cout << "theta is " << theta << endl;
	cout << " n is " << n << endl;

	Eigen::Matrix<double, 3, 3> n_hat;
	n_hat << 0, -n[2], n[1], n[2], 0, -n[0], -n[1], n[0], 0;

	Eigen::Matrix<double, 3, 3> R;//公式
	R = cos(theta)*Eigen::Matrix3d::Identity() + (1 - cos(theta))*n*n.transpose() + sin(theta)*n_hat;

	// 旋转向量转化为其他形式
	cout << "旋转向量转化为旋转矩阵方法1：rotation matrix =\n" << R << endl;
	cout << "旋转向量转化为旋转矩阵方法2：rotation matrix =\n"<< rotation_vector.toRotationMatrix() <<endl;//或者用.matrix()

	quat = Eigen::Quaterniond(cos(theta*0.5), n[0]*sin(theta*0.5), n[1] * sin(theta*0.5), n[2] * sin(theta*0.5));//或者  quat = Eigen::Quaterniond ( rotation_vector ); 
	cout << "旋转向量转化为四元数：quaternion =\n"<< quat.coeffs() <<endl;  //coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部


	// 旋转矩阵转化为其他形式
		//记旋转向量的单位向量为 r(rx, ry, rz) ,通过下图公式便可求解得出 r 向量的反对成矩阵,即可得出 r 向量
	cout << "根据旋转矩阵求解旋转向量..." << endl;


	//注意：fromRotationMatrix 参数只适用于旋转向量，不适用于四元数
	rotation_vector.fromRotationMatrix(rotation_matrix);
	cout << "旋转矩阵直接给旋转向量赋值初始化：rotation_vector axis = \n" << rotation_vector.axis() <<"\n rotation_vector angle = "<< rotation_vector.angle() <<endl;

	//martix--ol

	cout << "旋转矩阵转化为欧拉角：yaw pitch roll= " << rotation_matrix.eulerAngles(2, 1, 0)<<endl;//zyx

	//旋转矩阵转四元数
	quat.w = sqrt(rotation_matrix.trace() + 1)*0.5;
	quat.x = rotation_matrix(2, 1) - rotation_matrix(1, 2)*0.25 / quat.w;
	quat.y = rotation_matrix(0, 2) - rotation_matrix(2, 0)*0.25 / quat.w;
	quat.z = rotation_matrix(1, 0) - rotation_matrix(0, 2)*0.25 / quat.w;*/
	
	cout << "旋转矩阵转化为四元数2：quaternion =\n" << Eigen::Quaterniond(rotation_matrix).coeffs()<<endl;
	// 四元数转化为其他形式
	rotation_vector = Eigen::AngleAxisd(quat);
		cout << "四元数转化为旋转向量：rotation_vector axis = \n" << rotation_vector.axis() <<"\n rotation_vector angle = " << rotation_vector.angle() << endl;

	
	rotation_matrix = Eigen::Matrix3d(quat);
		cout << "四元数转化为旋转矩阵方法2：rotation matrix =\n"<<rotation_matrix <<endl;


	//  欧氏变换矩阵

	 // 欧氏变换矩阵使用 Eigen::Isometry，仿射变换用 Eigen::Affine3d，射影变换用 Eigen::Projective3d
	Eigen::Isometry3d T = Eigen::Isometry3d::Identity();              // 名称为3d，实质上是4 x 4的矩阵
	T.rotate(rotation_vector);
	T.pretranslate(Eigen::Vector3d(1, 2, 3));
    cout << "Transform matrix = \n" << T.matrix() <<endl;             // 变换矩阵需要用成员函数转一下输出

    cout << "欧氏变化矩阵提取旋转矩阵：rotation_matrix = \n" << T.rotation() << endl;
    cout << "欧氏变化矩阵提取平移向量：translation = \n" << T.translation() << endl;

   // ----------  结束 ----------
	return 0;
}

结果为：

rotation_vector axis = 
0
0
1
 rotation_vector angle = 0.785398
rotation matrix =
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
四元数输出方法1：quaternion = 
    0
0.383
0.924
    0
四元数输出方法2：
 x = 0
 y = 0.383
 z = 0.924
 w = 0
Euler: yaw pitch roll = 0.785398        0        0
旋转向量转化为旋转矩阵方法1：rotation matrix =
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
旋转向量转化为旋转矩阵方法2：rotation matrix =
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
旋转向量转化为四元数：quaternion =
       0
       0
0.382683
 0.92388
旋转矩阵转化为旋转向量：rotation_vector axis = 
0
0
1
 rotation_vector angle = 0.785336
旋转矩阵直接给旋转向量赋值初始化：rotation_vector axis = 
0
0
1
 rotation_vector angle = 0.785336
旋转矩阵转化为欧拉角：yaw pitch roll =        0 0.785398        0
旋转矩阵转化为四元数：quaternion =
       0
       0
0.382638
0.923851
四元数转化为旋转向量：rotation_vector axis = 
0
0
1
 rotation_vector angle = 0.785336
四元数转化为旋转矩阵方法1：rotation matrix =
0.707177   -0.707        0
   0.707 0.707177        0
       0        0        1
四元数转化为旋转矩阵方法2：rotation matrix =
0.707177   -0.707        0
   0.707 0.707177        0
       0        0        1
Transform matrix = 
 0.707151 -0.707063         0         1
 0.707063  0.707151         0         2
        0         0         1         3
        0         0         0         1
欧氏变化矩阵提取旋转矩阵：rotation_matrix = 
 0.707151 -0.707063         0
 0.707063  0.707151         0
        0         0         1
欧氏变化矩阵提取平移向量：translation = 
1
2
3