Eigen::关于旋转(旋转向量\旋转矩阵\四元数\欧拉角)的初始化和转换以及应用
code:
#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
using namespace std;
using namespace Eigen;
int main(int argc, char **argv) {
//下面三个变量作为下面演示的中间变量
AngleAxisd t_V(M_PI / 4, Vector3d(0, 0, 1)); //旋转向量:绕z轴旋转45度
Matrix3d t_R = t_V.matrix(); //旋转向量--->旋转矩阵
Quaterniond t_Q(t_V); //旋转向量--->四元数
/***************************旋转向量的初始化***************************/
/*******************************
* 对旋转向量(轴角)赋值的三大种方法
* 1.使用角度和矢量
* 2.使用旋转矩阵
* 3.使用四元数
*******************************/
///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;
/***************************四元数的初始化***************************/
/*******************************
* 对四元数初始化的三大种方法
* 1.使用四元数的值Quaterniond(Vector4d(x,y,z,w)) 或者 Quaterniond(w,x,y,z,w)
* 2.使用旋转矩阵
* 3.使用旋转向量
*******************************/
///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;
/***************************旋转矩阵的初始化***************************/
/*******************************
* 对旋转矩阵初始化的三大种方法
* 1.直接矩阵赋值
* 2.使用旋转向量
* 3.使用四元数
*******************************/
///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;
/***************************旋转向量\旋转矩阵\四元数====>欧拉角***************************/
///1.旋转向量===>欧拉角
Vector3d eulerFromAngleAxisd=t_V.matrix().eulerAngles(2,1,0); //(z,y,x)--(2,1,0) (z,x,y)--(2,0,1)
cout << "euler from 旋转向量:\n"<<(180)/(M_PI)*eulerFromAngleAxisd.transpose()<<endl;
///2.旋转矩阵===>欧拉角
Vector3d eulerFromRotationMatrix=t_R.eulerAngles(2,1,0);
cout << "euler from 旋转矩阵:\n"<<(180)/(M_PI)*eulerFromRotationMatrix.transpose()<<endl;
///3.四元数===>欧拉角
Vector3d eulerFromQuaternion = t_Q.matrix().eulerAngles(2,1,0);
cout << "euler from 四元数:\n"<<(180)/(M_PI)*eulerFromQuaternion.transpose()<<endl;
/***************************应用举例***************************/
/*******************************
* 利用3次旋转构造一个姿态R
* 1.绕z轴旋转45度
* 2.绕y轴旋转70度
* 3.绕x轴旋转30度
*******************************/
Vector3d rollpitchyaw((30.0/180.0)*M_PI,(70.0/180.0)*M_PI,(45.0/180.0)*M_PI);
AngleAxisd v_z((45.0/180.0)*M_PI , Vector3d(0, 0, 1));
AngleAxisd v_y((70.0/180.0)*M_PI , Vector3d(0, 1, 0));
AngleAxisd v_x((30.0/180.0)*M_PI , Vector3d(1, 0, 0));
//欧拉角为ZYX-321顺序的调用框架---------------------------------------
/*******************************
* 利用3次旋转构造一个姿态R
* 1.绕z轴旋转45度
* 2.绕y轴旋转70度
* 3.绕x轴旋转30度
*******************************/
Matrix3d R_n2b_zyx=v_x.matrix()*v_y.matrix()*v_z.matrix();
cout << "R_ZYX :\n"<<R_n2b_zyx<<endl;
AngleAxisd total;
total=v_x*v_y*v_z;
Vector3d zyx_Euler_fromR=R_n2b_zyx.eulerAngles(0,1,2);//Eigen中使用右乘的顺序,因此ZYX对应的是012,实际上这个编号跟乘法的顺序一致就可以了(从左向又右看的顺序)
cout<< "zyx euler from Rotation [输出顺序为:x,y,z]:\n"<<(180)/(M_PI)*zyx_Euler_fromR.transpose()<<endl;
//------------------------------------------------------------
//欧拉角为ZXY-312顺序的调用框架---------------------------------------
/*******************************
* 利用3次旋转构造一个姿态R
* 1.绕z轴旋转45度
* 2.绕x轴旋转30度
* 3.绕y轴旋转70度
*******************************/
Matrix3d R_n2b_zxy=v_y.matrix()*v_x.matrix()*v_z.matrix();
cout << "R_ZXY :\n"<<R_n2b_zxy<<endl;
Vector3d zxy_Euler_fromR=R_n2b_zxy.eulerAngles(1,0,2);//Eigen中使用右乘的顺序,因此ZYX对应的是012
cout<< "zxy euler from Rotation [输出顺序为:y,x,z]:\n"<<(180)/(M_PI)*zxy_Euler_fromR.transpose()<<endl;
//------------------------------------------------------------
///关于输出顺序: 就是做旋转矩阵的乘法的顺序
//(ZYX-321):v_x.matrix()*v_y.matrix()*v_z.matrix() 对应 pitch,roll,yaw的输出顺序
//(ZXY-312):v_y.matrix()*v_x.matrix()*v_z.matrix() 对应 roll,pitch,yaw的输出顺序
return 0;
}
参考内容:
https://blog.csdn.net/weixin_38213410/article/details/89892336
https://blog.csdn.net/yang__jing/article/details/82316093
Eigen:关于转换成欧拉角的函数的说明:
https://eigen.tuxfamily.org/dox/group__Geometry__Module.html#ga17994d2e81b723295f5bc3b1f862ed3b
#include "tf2/utils.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include "tf/tf.h"
#include "tf/transform_datatypes.h"
#include "geometry_msgs/TransformStamped.h"
#include "eigen_conversions/eigen_msg.h"
int main(int argc, char **argv)
{
ros::init(argc,argv,"test");
tf2_ros::TransformBroadcaster tfb;
geometry_msgs::TransformStamped tfs;
ros::Rate r(100);
while(ros::ok()){
tfs.header.stamp = ros::Time::now();
tfs.transform.translation.x=1;
tfs.transform.translation.y=0;
tfs.transform.translation.z=0;
tfs.header.frame_id="map";
tfs.child_frame_id="camera";
// 1. map 坐标系沿 原map坐标系x轴顺时针旋转90度 (roll)
// 2. map 坐标系沿 原map坐标系y轴顺时针旋转90度 (roll)
// 3. map 坐标系沿 原map坐标系z轴顺时针旋转90度 (yaw)
//=================================================
tfs.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(-M_PI/2,M_PI/2,-M_PI/2);
// 另外一种思路是: 以map坐标系为参考,camera姿态为 -M_PI/2 , 即 yaw角为 -90度
//tfs.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(0,0,-M_PI/2);
std::cout<<tfs.transform.rotation.w<<" "<<tfs.transform.rotation.x<<" "<< tfs.transform.rotation.y<<" "
<<tfs.transform.rotation.z<<std::endl;
Eigen::Isometry3d transform;
tf::transformMsgToEigen(tfs.transform,transform);
///
/// tf::createQuaternionMsgFromRollPitchYaw(-M_PI/2,M_PI/2,-M_PI/2);
/// 上面的操作,跟下面的等价
/// R_C_W = roll * yaw
/// 即相当于将世界坐标系:
/// 1. 沿 原map坐标系绕Z轴顺时针旋转90度 (yaw)
/// 2. 得到的坐标系 沿 新的X轴顺时针旋转90度 (roll)
/// 3. 得到camera坐标系
/// 4. 将camera坐标系的点转换到世界坐标系:
/// p_w = R_C_W.inv()*p_c + t_W_C
///
/// R_C_W.inv() 等价于 tfs.transform.rotation 表示从相机坐标系到map坐标系的旋转变换
///
Eigen::Matrix3d R_C_W=Eigen::Matrix3d::Identity();
// 顺时针90度
Eigen::Matrix3d yaw=Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d(0, 0, 1)).matrix();
// 逆时针90度
Eigen::Matrix3d pitch=Eigen::AngleAxisd(-M_PI/2, Eigen::Vector3d(0, 1, 0)).matrix();
// 顺时针90度
Eigen::Matrix3d roll=Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d(1, 0, 0)).matrix();
R_C_W = (roll) * (pitch) * (yaw);
Eigen::Quaterniond q(R_C_W.transpose());
std::cout<<"q: "<<q.w()<<" "<<q.x()<<" "<<q.y()<<" "<<q.z()<<std::endl;
Eigen::Vector3d point_in_camera(1,0,0);
// Eigen::Vector3d point_in_map= transform* point_in_camera;
Eigen::Vector3d point_in_map= R_C_W.transpose()* point_in_camera + Eigen::Vector3d(1,0,0);
// std::cout<<point_in_map.transpose()<<std::endl;
tfb.sendTransform(tfs);
r.sleep();
}
// google::ShutdownGoogleLogging();
}