c++ Eigen使用

Eigen 可以用来运动学解算。

参考链接：https://blog.csdn.net/yang__jing/article/details/82316093

Eigen3官网-API-http://eigen.tuxfamily.org/dox/

转换实例：https://www.cnblogs.com/cc111/p/9354924.html

Eigen 几何模块的使用方法:

• 旋转向量 R
• 利用Eigen::AngleAxisd(参数：角度，轴)得到

//沿 Z 轴旋转 45 度
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); 
//由角轴得到旋转变量
Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix()
           

• 旋转矩阵 Eigen::Matrix3d

//在这里插入代码片`旋转向量转换到旋转矩阵
 rotation_vector.toRotationMatrix(); 
           

• 欧拉角

//( 2,1,0 )表示ZYX顺序，即roll pitch yaw顺序,将旋转矩阵转换到欧拉角
Eigen::Vector3d rotation_matrix.eulerAngles ( 2,1,0 )
           

• 四元数

//定义四元数
Eigen::Quaterniond q = Eigen::Quaterniond (rotation_vector);
//旋转矩阵定义四元素
q = Eigen::Quaterniond (rotation_matrix);
           

• 欧拉角与四元数相互转换

// 欧拉角转四元数 R-P-Y
Eigen::Vector3d eulerAngle(yaw,pitch,roll);
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
 
Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;

//四元数转欧拉角 R-P-Y
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);
           

四元数、欧拉角转换实例。

以下代码在欧拉角出现pi/2时，以及大于pi时会出现问题。

#include <iostream>
#include <cmath>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

using namespace std;
int main()
{
    Eigen::Vector3d eulerAngle(M_PI/3,M_PI/5,M_PI/4);
    cout<<"roll(X) pitch(Y) yaw(Z)=\n"<< eulerAngle.transpose()<<endl<<endl;

    Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(0),Eigen::Vector3d::UnitX()));
    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1),Eigen::Vector3d::UnitY()));
    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(2),Eigen::Vector3d::UnitZ()));

    Eigen::Quaterniond quaternion;
    quaternion=rollAngle*pitchAngle*yawAngle;
    Eigen::Vector3d eulerAngle_1=quaternion.matrix().eulerAngles(0,1,2); 
    cout<<"roll(X) pitch(Y) yaw(Z)=\n"<< eulerAngle_1.transpose()<<endl<<endl; 

	//Eigen::Vector3d eulerAngle(roll,pitch,yaw);
	//Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
	//Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
	//Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
	//Eigen::Quaterniond quaternion;
	//quaternion=yawAngle*pitchAngle*rollAngle;
	//Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);
	
    Eigen::Quaterniond quaternion2(-0.5,0,0,1); 
    Eigen::Vector3d eulerAngle_2=quaternion2.matrix().eulerAngles(2,1,0);
    cout<<"yaw(Z) pitch(Y) roll(Z)=\n"<< eulerAngle_2.transpose()<<endl<<endl;                                                                                                                   

    return 0;
}
           

g++ test.cpp

tf 实现四元数到欧拉角转换接口使用及测试

#include <iostream>
#include <tf/transform_datatypes.h>//转换函数头文件
#include <nav_msgs/Odometry.h>//里程计信息格式

using namespace std;
int main ()
{
    tf::Quaternion quat(0,0,1,-0.5);
    double roll, pitch, yaw;//定义存储r\p\y的容器

    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);//进行转换
    cout << "roll:" << roll << "\npitch:" << pitch << "\nyaw:" << yaw << endl;
    return 0;
}
           

直接编译命令：

g++ `pkg-config --libs --cflags tf` -ldl tf_test.cpp
           

• 欧式变换矩阵(其次坐标包含R，t)

//虽然称为3d(指3维空间的欧式坐标)，实质上是4＊4的矩阵， 旋转R + 平移T
Eigen::Isometry3d T=Eigen::Isometry3d::Identity();
//按照rotation_vector进行旋转
T.rotate (rotation_vector);
//按四元数表示的旋转
Eigen::Isometry3d T(q);
//把平移向量设成(1,3,4)
T.pretranslate (Eigen::Vector3d ( 1,3,4 )); 
           

• Eigen::Map 数组、向量及矩阵的引用。

double parameters[7] = {0, 0, 0, 1, 0, 0, 0};
//quaterd为parameters的引用
Eigen::Map<const Quaterniond> quaterd(parameters[0]);
//同Eigen::Map<const Quaterniond> quaterd(parameters);
//quaterd为parameters[4]的引用，由于元素地址连续
Eigen::Map<const Eigen::Vector3d> trans(parameters[0] + 4);
//同Eigen::Map<const Eigen::Vector3d> trans(parameters + 4);
           

引用：类似于变量的别名，一旦指定，不可改变，与变量值绑定

指针：指针存放变量的地址。可更改

参考链接：https://blog.csdn.net/GMeng_ROBOT/article/details/84326798