ROS中使用Eigen库

转载：https://blog.csdn.net/wxflamy/article/details/79315352

刚体旋转https://blog.csdn.net/zzyczzyc/article/details/94457475

重要 https://blog.csdn.net/u011092188/article/details/77430988 

Eigen::Quaterniond q(1,0,0,0);
Eigen::Vector3d t(1,1,6);

Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(q);
T.pretranslate(t);

cout << T.matrix() << endl;
cout << T.inverse().matrix() << endl;


//向量
Eigen::Vector3d p1 = Eigen::Vector3d(0.5, 0, 0.2);

//欧拉矩阵定义 4×4
Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
T1.rotate(q1.toRotationMatrix());
T1.pretranslate(t1);
T2.rotate(q2.toRotationMatrix());
T2.pretranslate(t2);
// cout << T1.matrix() << endl;
// cout << T2.matrix() << endl;

//计算向量的旋转
p2 = T2 * T1.inverse() * p1;
cout << p2.transpose() << endl;

 

——参考书《A Systematic Approach to Learning Robot Programming with ROS》 
 

 

ROS中的数据操作需要线性代数，Eigen库是C++中的线性代数计算库。它独立于ROS，但是在ROS中可以使用。在CMakeLists.txt文件中要做以下配置
 

#uncomment the following 4 lines to use the Eigen library
find_package(cmake_modules REQUIRED)
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
add_definitions(${EIGEN_DEFINITIONS})

在源代码中要包含以下头文件

#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

如果需要使用其他的功能，也需要用到其他的头文件。可以参考这个网址http://eigen.tuxfamily.org/dox/group__QuickRefPage.html

定义一个向量：

Eigen::Vector3d normal_vec(1,2,3); // here is an arbitrary normal vector, initialized to (1,2,3) upon instantiation

将向量变成单位向量:

normal_vec/=normal_vec.norm(); // make this vector unit length

定义一个矩阵的形式是：

Eigen::Matrix3d Rot_z;
Rot_z.row(0)<<0,-1,0;  // populate the first row--shorthand method
Rot_z.row(1)<<1, 0,0;  //second row
Rot_z.row(2)<<0, 0,1;  // yada, yada
cout<<"Rot_z: "<<endl;  

可以按行赋值。

矩阵与向量的乘法

v1 = Rot_z*normal_vec; 
//here is how to multiply a matrix times a vector
//although Rot_z and normal_vec are both objects (with associated data and member methods), multiplication is defined,
//resulting in the data members being altered or generated as expected for matrix-vector multiplies

 

 

Eigen中四元数可以用Eigen::Quaterniond(double类型)或者Eigen::Quaternionf(float类型)表示. 在Eigen中的四元数可以通过如下方式构造:

Quaternion (const Scalar &w, const Scalar &x, const Scalar &y, const Scalar &z)

w是四元数的实部, x,y,z 分别是四元数的虚部(PS: 构造的时候w在前面). 但是,这里有一个坑--------虽然构造的时候w在前面, 但是在实际的内存中, 四元数四个元素的顺序却是x, y, z, w . 可以通过下面的代码去提取四元数的元素验证:

Eigen::Vector4d q = q_AB.coeffs();