坐标系的变换（Eigen库四元数表示的坐标）

本文主要介绍使用Eigen库中的四元数进行空间坐标的转换。
Eigen库的结构体对应的名称如下：
旋转矩阵（3×3） Eigen::Matrix3d
旋转向量（3×1）Eigen::AngleAxisd
欧式变换矩阵（4×4） Eigen::Isometry3d
四元数（4×1）Eigen::Quaterniond

坐标系转换如图

假设机器在p1处在世界坐标系下的位姿为（x，y，z，roll,pitch,yaw）=（1，1，0，0,0,0,），p2处世界坐标系下的位姿（x，y，z，roll,pitch,yaw）=（2，2，0，0 ，0，45）

1.已知p1,p2求p1 和p2之间的转换T12

此例中T12结果简单表示为(1,1,0,0,0,45)
程序如下：

#include
#include
#include
int main()
{
    //1.p1 world position
    double p1yaw=0;
    double p1x=1;
    double p1y=1;
    Eigen::AngleAxisd rotzp1(p1yaw*M_PI/180, Eigen::Vector3d::UnitZ());
    Eigen::Vector3d  t1= Eigen::Vector3d(p1x,p1y, 0);
    Eigen::Quaterniond q1=Eigen::Quaterniond(rotzp1);
    //cout<<"q1.vec()"<

参考文献
Eigen中四元数、欧拉角、旋转矩阵、旋转向量之间的转换
https://blog.csdn.net/yang__jing/article/details/82316093