Eigen——⼏何模块的使⽤⽅法与举例
Eigen——⼏何模块的使⽤⽅法
常⽤表⽰：
1、旋转矩阵（3X3）：Eigen::Matrix3d
2、旋转向量（3X1）:Eigen::AngleAxisd
旋转向量也就是所谓的轴⾓，定义如下：
3、四元数
（4X1）:Eigen::Quaterniond
4、平移向量（3X1）:Eigen::Vector3d
5、变换矩阵（4X4）:Eigen::Isometry3d
6、绕该轴逆时针旋转angle(rad)：AngleAxis（angle, axis）angle(rad)表⽰旋转⾓度默认为：弧度制
7、变换矩阵 ：
Eigen::Isometry3d T;
T.matrix()才是变换矩阵，做运算时需加.matrix()后缀;
T.pretranslate()以及T.rotate()可以给平移部分和旋转矩阵赋值，但是若循环中使⽤，末尾不重置变换矩阵的话，这个设置量会累加，⽽不是覆盖。
#include<iostream>
#include<cmath>
#include<Eigen/Core>
// Eigen ⼏何模块
#include<Eigen/Geometry>
constexpr auto M_PI =3.14159265358979323846;
using namespace std;
/****************************
* 本程序演⽰了 Eigen ⼏何模块的使⽤⽅法
****************************/
int main(int argc,char** argv)
{
// Eigen/Geometry 模块提供了各种旋转和平移的表⽰
/
/ 3D 旋转矩阵直接使⽤ Matrix3d 或 Matrix3f
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
// 旋转向量使⽤ AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
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()转换成矩阵
// 也可以直接赋值
rotation_matrix = RotationMatrix();
// ⽤ AngleAxis 可以进⾏坐标变换
Eigen::Vector3d v(1,0,0);
Eigen::Vector3d v_rotated = rotation_vector * v;
cout <<"(1,0,0) after rotation = "<< anspose()<< endl;
// 或者⽤旋转矩阵
v_rotated = rotation_matrix * v;
cout <<"(1,0,0) after rotation = "<< anspose()<< endl;
// 欧拉⾓: 可以将旋转矩阵直接转换成欧拉⾓
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);// ZYX顺序，即roll pitch yaw顺序
cout <<"yaw pitch roll = "<< anspose()<< endl;
// 欧⽒变换矩阵使⽤ Eigen::Isometry
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();// 虽然称为3d，实质上是4＊4的矩阵
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 <<"v tranformed = "<< anspose()<< endl;
// 对于仿射和射影变换，使⽤ Eigen::Affine3d 和 Eigen::Projective3d 即可，略
// 四元数
// 可以直接把AngleAxis赋值给四元数，反之亦然
Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
cout <<"quaternion = \n"<< q.coeffs()<< endl;// 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部// 也可以把旋转矩阵赋给它
q = Eigen::Quaterniond(rotation_matrix);
cout <<"quaternion = \n"<< q.coeffs()<< endl;
// 使⽤四元数旋转⼀个向量，使⽤重载的乘法即可
v_rotated = q * v;// 注意数学上是qvq^{-1}
cout <<"(1,0,0) after rotation = "<< anspose()<< endl;
return0;
identity matrix是什么意思}
例⼦：
Eigen::Vector3d  cal_delta_distance(Eigen::Vector3d odom_pose)
{
static Eigen::Vector3d now_pos,last_pos;
Eigen::Vector3d d_pos;//return value
now_pos = odom_pose;
//TODO:
// -------------------------------------------------------------------------------------------------// d_pos = last_pos.inverse() * now_pos;
// d_pos = now_pos - last_pos；是不对的
d_pos = now_pos - last_pos;
Eigen::AngleAxisd temp(last_pos(2),Eigen::Vector3d(0,0,1));
Eigen::Matrix3d trans=temp.matrix().inverse();
d_pos=trans*d_pos;
//end of TODO:
last_pos = now_pos;
return d_pos;
}

版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。