官方地址传送
Space transformations
常用
- 1.旋转矩阵（3X3）:Eigen::Matrix3d
- 2.旋转向量（3X1）:Eigen::AngleAxisd
- 3.四元数（4X1）:Eigen::Quaterniond
- 4.平移向量（3X1）:Eigen::Vector3d
- 5.变换矩阵（4X4）:Eigen::Isometry3d
AngleAxis（angle, axis）：绕该轴逆时针旋转angle(rad)。
变换矩阵
Eigen::Isometry3d T;
T.matrix()才是变换矩阵，做运算时需加.matrix()后缀;
T.pretranslate()以及T.prerotate()可以给平移部分和旋转矩阵赋值，但是若循环中使用，末尾不重置变换矩阵的话，这个设置量会累加，而不是覆盖。
四元数赋值：Eigen::Quaterniond Q;
Q.x() = 3 「类似地 Q.y() = Q.z() = Q.w()」
1、 旋转矩阵（R），旋转向量（V）和四元数（Q）在Eigen中转换关系的总结：
2、旋转矩阵（R），旋转向量（V）和四元数（Q）分别通过自身初始化自己的方式：
R通过自身初始化的方法：
- 使用旋转矩阵的函数来初始化旋转矩阵
Matrix3d R1=Matrix3d::Identity();
cout << “Rotation_matrix1” << endl << R1 << endl;
V通过自身初始化的方法：
- 使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化角轴
AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以（0,0,1）为旋转轴，旋转45度
cout << “Rotation_vector1” << endl << V1.matrix() << endl;
Q通过自身初始化的方法：
- 使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化四元数,即使用
q=[cos(A/2),n_xsin(A/2),n_ysin(A/2),n_z*sin(A/2)]
Quaterniond Q1(cos((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 1 * sin((M_PI / 4) / 2));//以（0,0,1）为旋转轴，旋转45度
cout << “Quaternion1” << endl << Q1.coeffs() << endl;
演示代码：
#include <iostream>
#include <Eigen/Dense>
#define M_PI 3.1415926
using namespace std;
using namespace Eigen;
void run_eigen_geometry()
{
//下面三个变量作为下面演示的中间变量
AngleAxisd t_V(M_PI / 4, Vector3d(0, 0, 1)); // 角轴 旋转向量
Matrix3d t_R = t_V.matrix(); // 旋转矩阵
Quaterniond t_Q(t_V); // 四元数
//对旋转向量（轴角）赋值的三类方法
cout << "对旋转向量（轴角）赋值的三类方法" << endl;
//1.使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化角轴
AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以（0,0,1）为旋转轴，旋转45度
cout << "Rotation_vector1" << endl << V1.matrix() << endl;
//2.通过旋转矩阵的方式
//2.1 使用旋转向量的fromRotationMatrix()函数来对旋转向量赋值（注意此方法为旋转向量独有,四元数没有）
AngleAxisd V2;
V2.fromRotationMatrix(t_R);
cout << "Rotation_vector2" << endl << V2.matrix() << endl;
//2.2 直接使用旋转矩阵来对旋转向量赋值
AngleAxisd V3;
V3 = t_R;
cout << "Rotation_vector3" << endl << V3.matrix() << endl;
//2.3 使用旋转矩阵来对旋转向量进行初始化
AngleAxisd V4(t_R);
cout << "Rotation_vector4" << endl << V4.matrix() << endl;
//3. 使用四元数来对旋转向量进行赋值
//3.1 直接使用四元数来对旋转向量赋值
AngleAxisd V5;
V5 = t_Q;
cout << "Rotation_vector5" << endl << V5.matrix() << endl;
//3.2 使用四元数来对旋转向量进行初始化
AngleAxisd V6(t_Q);
cout << "Rotation_vector6" << endl << V6.matrix() << endl;
//------------------------------------------------------
//对四元数赋值的三类方法（注意Eigen库中的四元数前三维是虚部,最后一维是实部）
cout << "对四元数赋值的三类方法" << endl;
//1.使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化四元数,即使用q=[cos(A/2),n_x*sin(A/2),n_y*sin(A/2),n_z*sin(A/2)]
Quaterniond Q1(cos((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 1 * sin((M_PI / 4) / 2));//以（0,0,1）为旋转轴，旋转45度
//第一种输出四元数的方式
cout << "Quaternion1" << endl << Q1.coeffs() << endl;
//第二种输出四元数的方式
cout << Q1.x() << endl << endl;
cout << Q1.y() << endl << endl;
cout << Q1.z() << endl << endl;
cout << Q1.w() << endl << endl;
//2. 使用旋转矩阵转四元數的方式
//2.1 直接使用旋转矩阵来对四元数赋值
Quaterniond Q2;
Q2 = t_R;
cout << "Quaternion2" << endl << Q2.coeffs() << endl;
//2.2 使用旋转矩阵来对四元數进行初始化
Quaterniond Q3(t_R);
cout << "Quaternion3" << endl << Q3.coeffs() << endl;
//3. 使用旋转向量对四元数来进行赋值
//3.1 直接使用旋转向量对四元数来赋值
Quaterniond Q4;
Q4 = t_V;
cout << "Quaternion4" << endl << Q4.coeffs() << endl;
//3.2 使用旋转向量来对四元数进行初始化
Quaterniond Q5(t_V);
cout << "Quaternion5" << endl << Q5.coeffs() << endl;
//----------------------------------------------------
//对旋转矩阵赋值的三类方法
cout << "对旋转矩阵赋值的三类方法" << endl;
//1.使用旋转矩阵的函数来初始化旋转矩阵
Matrix3d R1 = Matrix3d::Identity();
cout << "Rotation_matrix1" << endl << R1 << endl;
//2. 使用旋转向量转旋转矩阵来对旋转矩阵赋值
//2.1 使用旋转向量的成员函数matrix()来对旋转矩阵赋值
Matrix3d R2;
R2 = t_V.matrix();
cout << "Rotation_matrix2" << endl << R2 << endl;
//2.2 使用旋转向量的成员函数toRotationMatrix()来对旋转矩阵赋值
Matrix3d R3;
R3 = t_V.toRotationMatrix();
cout << "Rotation_matrix3" << endl << R3 << endl;
//3. 使用四元数转旋转矩阵来对旋转矩阵赋值
//3.1 使用四元数的成员函数matrix()来对旋转矩阵赋值
Matrix3d R4;
R4 = t_Q.matrix();
cout << "Rotation_matrix4" << endl << R4 << endl;
//3.2 使用四元数的成员函数toRotationMatrix()来对旋转矩阵赋值
Matrix3d R5;
R5 = t_Q.toRotationMatrix();
cout << "Rotation_matrix5" << endl << R5 << endl;
}
演示代码2：eigen_geometry_2.cpp
#include <iostream>
#include <cmath>
#define M_PI 3.1415926
using namespace std;
#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>
/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/
void run_eigen_geometry_2()
{
//注意一下类型名的最后一个字符为d表示双精度类型,换成f表示单精度类型，两种类型不能混用，必须显示转换
// Eigen/Geometry 模块提供了各种旋转和平移的表示
// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
/****旋转向量****/
// 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
// 乘以该向量，表示进行一个坐标变换
//任意旋转可用一个旋转轴和一个旋转角度来表示。
//旋转向量，旋转向量的方向与旋转轴一致，长度为旋转角度。
/*********************************/
/*旋转向量 沿 Z 轴旋转 45 度 角度 轴 */
Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0, 1)); //沿 Z 轴旋转 45 度
cout.precision(3);
cout << "rotation matrix =\n" << rotation_vector.matrix() << endl; //用matrix()转换成矩阵,也可以直接赋值
/*********************************/
/*旋转矩阵*/
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();//单位阵
rotation_matrix = rotation_vector.toRotationMatrix();//转成旋转矩阵 由罗德里格公式进行转换
// 用 AngleAxis 可以进行坐标变换
Eigen::Vector3d v(1, 0, 0);
/*************旋转向量进行坐标变换********************/
Eigen::Vector3d v_rotated = rotation_vector * v;
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
// 或者用旋转矩阵
/*****************旋转矩阵进行坐标变换****************/
v_rotated = rotation_matrix * v;
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
/**欧拉角表示的旋转**/
// 欧拉角: 可以将旋转矩阵直接转换成欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即roll pitch yaw顺序
cout << "yaw pitch roll = " << euler_angles.transpose() << endl;
/***欧式变换矩阵表示旋转**/
// 欧氏变换矩阵使用 Eigen::Isometry
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();// 虽然称为3d，实质上是4＊4的矩阵 齐次坐标
T.rotate(rotation_vector); // 按照rotation_vector进行旋转
T.pretranslate(Eigen::Vector3d(1, 3, 4)); // 把平移向量设成(1,3,4)
cout << "Transform matrix = \n" << T.matrix() << endl;
// 用变换矩阵进行坐标变换
Eigen::Vector3d v_transformed = T*v; // 相当于R*v+t
cout << "(1,0,0) after Isometry3d tranformed = " << v_transformed.transpose() << endl;
// 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略
/*******四元数表示的旋转***********/
// 可以直接把AngleAxis赋值给四元数，反之亦然 Quaterniond 表示双精度 四元素 Quaternionf 表示单精度四元素
Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);// 表示沿Z 轴旋转 45 度 的四元素变换
cout << "quaternion from AngleAxis rotation_vector = \n" << q.coeffs() << endl; // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
// 也可以把旋转矩阵赋给它
q = Eigen::Quaterniond(rotation_matrix);
cout << "quaternion from rotation_matrix = \n" << q.coeffs() << endl;
// 使用四元数旋转一个向量，使用重载的乘法即可
/*注意程序表达形式和实际运算的不一样*/
v_rotated = q*v; // 注意数学上是q*v*q^{-1} 而程序为了简化表示 直接使用 q*v代替
cout << "(1,0,0) after Quaterniond rotation = " << v_rotated.transpose() << endl;
/*编程题目
小萝卜１号位姿q1=[0.35,0.2,0.3,0.1],t1=[0.3,0.1,0.1]' 世界坐标系到相机变换
小萝卜２号位姿q2=[-0.5,0.4,-0.1,0.2],t2=[-0.1,0.5,0.3]'
小萝卜１号看到位于自身坐标系下p=[0.5,0,0.2]'
求该向量在小萝卜２号下的坐标
*/
Eigen::Quaterniond q1(0.35, 0.2, 0.3, 0.1);//wxyz q1.coeffs() xyzw q1.vec() xyz
//q1 << 0.35,0.2,0.3,0.1;
Eigen::Matrix<double, 3, 1> t1;//float类型
t1 << 0.3, 0.1, 0.1;
Eigen::Quaterniond q2(-0.5, 0.4, -0.1, 0.2);
//q2 << -0.5,0.4,-0.1,0.2;
Eigen::Matrix<double, 3, 1> t2;//float类型
t2 << -0.1, 0.5, 0.3;
Eigen::Matrix<double, 3, 1> p1;//float类型
p1 << 0.5, 0, 0.2;
cout << "q1= \n" << q1.coeffs() << endl;
cout << "t1= \n" << t1 << endl;
cout << "q2= \n" << q2.coeffs() << endl;
cout << "t2= \n" << t2 << endl;
/*
q1.setIdentity();
cout<<"q1 after setIdentity \n"<<q1.coeffs() <<endl;
q2.setIdentity();
cout<<"q2 after setIdentity \n"<<q2.coeffs() <<endl;
*/
q1 = q1.normalized();//规范化 归一化 除以模长
cout << "q1 after normalized\n" << q1.coeffs() << endl;
q2 = q2.normalized();
cout << "q2 after normalized \n" << q2.coeffs() << endl;
Eigen::Matrix3d q1rotation_matrix = Eigen::Matrix3d::Identity();//单位阵
q1rotation_matrix = q1.toRotationMatrix();
Eigen::Isometry3d Tc1w = Eigen::Isometry3d::Identity();// 虽然称为3d，实质上是4＊4的矩阵 齐次坐标
Tc1w.rotate(q1rotation_matrix); // 按照q1rotation_matrix进行旋转
Tc1w.pretranslate(t1); // 把平移向量设成t1
//Eigen::Isometry3d Twc1=Tc1w.inverse();//由world 到c1的逆变换 成 c1到world
Eigen::Matrix<double, 3, 1> pw = Tc1w.inverse()*p1; //将c1坐标系下的点p1变换到world坐标系下
Eigen::Matrix3d q2rotation_matrix = Eigen::Matrix3d::Identity();//单位阵
q2rotation_matrix = q2.toRotationMatrix();
Eigen::Isometry3d Tc2w = Eigen::Isometry3d::Identity();// 虽然称为3d，实质上是4＊4的矩阵 齐次坐标
Tc2w.rotate(q2rotation_matrix); // 按照q2rotation_matrix进行旋转
Tc2w.pretranslate(t2); // 把平移向量设成t2
Eigen::Matrix<double, 3, 1> p2 = Tc2w*pw; //将world坐标系下的点pw变换到c2坐标系下
cout << "the loc of p1 in c1 = \n" << p1 << endl;
cout << "the loc of p1 in world = \n" << pw << endl;
cout << "the loc of p1 in c2 = \n" << p2 << endl;
}
来源：CSDN
作者：Brock-123
链接：https://blog.csdn.net/a943368093/article/details/103526399