在博文<三维空间中刚体运动>中介绍了三维空间刚体运动和坐标转换的概念,包括坐标变换的各中表示:旋转矩阵、四元数、旋转角、旋转向量、变换矩阵等,初步尝试了使用Eigen进行坐标变换。本文在此基础上,进一步总结Eigen中各种数据类型及其基本操作,便于查阅。
Eigen要点
重载的赋值运算符无法在声明的同时拷贝构造赋值,即Matrix A=B是错误的,原因在于Eigen内部屏蔽了拷贝构造函数。
所有的旋转都是逆时针为正。
Eigen的旋转矩阵表示的是从旋转后的坐标系到旋转前坐标系的转换。即如果载体当前坐标系为A,初始参考坐标系为O,则Eigen的旋转矩阵R表示的是R_AO,即从A坐标系到O坐标系的转换关系,如果在A坐标系中有一个向量r_A,则它在O坐标系下的表示为r_O = R_AO * r_A。
四元数和旋转向量表示的也是从旋转后的坐标系到旋转前坐标系的转换,因此在进行姿态更新的时候,所有的姿态增量都是右乘,例如当前旋转矩阵为R_AO,旋转到B坐标系以后,R_BO = R_AO * R_BA。四元数下的运算为Q_BO = Q_AO * Q_BA,旋转向量同理。
矩阵的基本操作
1.转置
m.transpose()
2.矩阵各元素求和
m.sum()
3.矩阵各元素均值
m.mean()
4.矩阵的迹
m.trace()
5.矩阵各元素求积
m.prod()
6.矩阵各元素求最小值
m.minCoeff()
7.矩阵各元素求最大值
m.maxCoeff()
向量的基本操作
注意:向量也是矩阵的一种形式,因此上面的函数也适用。
1.点积(内积)
Vector3d v(1,2,3);
Vector3d w(0,1,2);
v.dot(w);
2.叉积(外积)
v.cross(w)
3.模
v.norm()
几何变换
Eigen中各种变换之间的转换关系如下:
旋转矩阵
构造函数
单位矩阵构造
Eigen::Matrix3d rotation_m=Eigen::Matrix3d::Identity();
由旋转向量构造
rotation_m=rotation_v.toRotationMatrix();
常用函数
进行旋转变换
Eigen::Vector3d p(1,0,0);//P点 Eigen::Vector3d p_r=m*p; cout<<p<<endl<<"after p rotated:"<<p_r<<endl<<endl;
旋转向量
构造函数
旋转轴和旋转角构造. 比如旋转轴向量为[0,0,1],旋转角为M_PI/4时,初始化如下:
Eigen::AngleAxisd rotation_v(M_PI/4,Eigen::Vector3d(0,0,1));
由旋转矩阵构造
常用函数
转换为Matrix格式
cout<<"rotation_v.matrix:\n"<<rotation_v.matrix()<<endl<<endl;
使用进行旋转
//使用旋转向量进行旋转变换 Eigen::Vector3d p(1,0,0);//P点 Eigen::Vector3d p_r=rotation_v*p; cout<<p<<endl<<"after p rotated:"<<p_r<<endl<<endl;
欧拉角
构造函数
由旋转矩阵获得
Eigen::Vector3d euler_a=rotation_m.eulerAngles(2,1,0); cout<<"yaw pitch roll="<<euler_a.transpose()<<endl<<endl;
四元数
构造函数
直接赋值,eg:
Quaterniond q(1.0,0.0,0.0,0.0);
赋值的顺序是先实数,再虚数:w,x,y,z,而内部存储则是先虚数,再实数:x,y,z,w从旋转矩阵构造,eg:
Matrix3d mat; Quaterniond q(mat);
从旋转向量构造
VectorXd vq(4); vq<<1.0, 0, 0, 0; Quaterniond qv(vq);
使用旋转的角度和旋转轴向量(此向量为单位向量)来初始化四元数,
即使用当旋转轴向量为N=[n_x,n_y,n_z],旋转角度为A时,四元数初始化为:
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度
从数组构造
数组的顺序应该是[w x y z]
常用函数
输出系数
q.coeffs(); //[x y z w]
输出虚部
q.vec(); //[x y z]
以上两种输出都是以Eigen中的向量Vector形式输出输出旋转矩阵
要注意的是,只有单位四元数才表示旋转矩阵,所以要先对四元数做单位化
q.normalized(); //important Matrix3d R=q.toRotationMatrix();
输出单个元素
cout<<q.w()<<" "<<q.x()<<" "<<q.y()<<" "<<q.z()<<endl;
共轭/即反向旋转
一般不用inverse,在表示旋转的时候(范数是1),共轭即可表示相反的的旋转。
//q.inverse(); q.conjugate();
平移向量
构造
直接构造
Eigen::Vector3d t1 = Eigen::Vector3d(0.3, 0.1, 0.1);
变换矩阵
初始化
一般先定义为单位变换矩阵,然后再赋值旋转和平移:
Eigen::Matrix3d m; m<<0.999555,-0.00658702,0.0290829,0.0030476,0.992756,0.120107,-0.0296634,-0.119964,0.992335; Eigen::Vector3d v; v<<0.0506837,-0.0582575,0.140693; //构造变换矩阵 Eigen::Isometry3d Tcw=Eigen::Isometry3d::Identity();; Tcw.rotate(m); Tcw.pretranslate(v);
其它类型矩阵的相互转换
//转换为普通的4维矩阵 Tcw.matrix(); //提取出旋转矩阵 Tcw.rotation(); //提取平移向量 Tcw.translation(); //转换为仿射矩阵 Eigen::Affine3d Taf; Taf=Tcw.affine();
会持续更新。
参考资料
[0]小人物1990.使用Eigen进行位姿计算.2019-04.https://blog.csdn.net/RQren/article/details/89287124