坐标系的变换(Eigen库四元数表示的坐标)

本文详述了如何利用Eigen库中的四元数进行空间坐标转换,包括从一个坐标系到另一个坐标系的旋转和平移计算,以及逆向求解的过程。通过具体示例,演示了四元数在机器人定位和姿态估计中的应用。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

本文主要介绍使用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<iostream>
#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/Geometry>
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()"<<q1.vec()<<endl;
    cout<<"p1 eular angle in world axis"<<(180/M_PI)*q1.matrix().eulerAngles(0,1,2)<<endl;
    cout<<"p1 position in world axis"<<t1<<endl;
   //2.p2 world position
    double p2yaw=45;
    double p2x=2;
    double p2y=2;
    Eigen::AngleAxisd rotzp2(p2yaw*M_PI/180, Eigen::Vector3d::UnitZ());
    Eigen::Vector3d  t2= Eigen::Vector3d(p2x,p2y, 0);
    Eigen::Quaterniond q2=Eigen::Quaterniond(rotzp2);
    //cout<<"q1.vec()"<<q1.vec()<<endl;
    cout<<"p2 eular angle in world axis"<<(180/M_PI)*q2.matrix().eulerAngles(0,1,2)<<endl;
    cout<<"p2 position in world axis"<<t2<<endl;


    // 3.calculate T12
    //q1*q12=q2
    Eigen::Quaterniond q12=q1.inverse()*q2;
    cout<<"rotaion from p1  to p2 "<<(180/M_PI)*q12.matrix().eulerAngles(0,1,2)<<endl;

   // Eigen::Vector3d t2=t1+q1.toRotationMatrix()*t12;

    Eigen::Vector3d  t12=q1.toRotationMatrix().inverse()*(t2-t1);
    cout<<"translation base on p1 aixs t12="<<t12<<endl;
}

2已知p1和t12求p2

假设机器在p1处在世界坐标系下的位姿为(x,y,z,roll,pitch,yaw)=(1,1,0,0,0,0,),p1 和p2之间的转换(p1经过旋转和平移)为t12表示为(1,1,0,0,0,45),
求p2处世界坐标系下的位姿(x,y,z,roll,pitch,yaw)?
结果为(2,2,0,0,0,45)
程序如下:

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

using namespace std;
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()"<<q1.vec()<<endl;
    cout<<"p1 eular angle"<<(180/M_PI)*q1.matrix().eulerAngles(0,1,2)<<endl;
    
    //2. t12 value
    double t12yaw=45;
    double t12x=1;
    double t12y=1;
    Eigen::AngleAxisd rott12(t12yaw*M_PI/180, Eigen::Vector3d::UnitZ());
    Eigen::Vector3d  t12= Eigen::Vector3d(t12x,t12y, 0);
    Eigen::Quaterniond q12=Eigen::Quaterniond(rott12);
     cout<<"t12 rotation eular angle "<<(180/M_PI)*q12.matrix().eulerAngles(0,1,2)<<endl;
  //  cout<<"q12.vec()"<<q12.vec()<<endl;
     
//    3.calculate p2 world positon
    Eigen::Quaterniond q2=q1*q12;
   // cout<<" q2 vector " <<q2.vec()<<endl;
    Eigen::Vector3d t2=t1+q1.toRotationMatrix()*t12;
    cout<<"t2="<<t2<<endl;
    cout<<"t2 eular angle "<<(180/M_PI)*q2.matrix().eulerAngles(0,1,2)<<endl;
}

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

评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值