本文主要介绍使用Eigen库进行空间坐标的转换。
坐标系转换如图

假设机器在p1处在世界坐标系下的位姿为（x，y，z，θ）=（1，1，0，0），p2处世界坐标系下的位姿（x，y，z，θ）=（2，2，0，45）
求p1 和p2之间的转换t12。
此例中t12简单表示为(1,1,0,45)

１.世界坐标系－>局部坐标系

已知p1 和p2求t12
程序如下

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include<iostream>
using namespace std;
int main()
{
    //1.p1 world position
    double p1a=0;
    double p1x=1;
    double p1y=1;
    Eigen::AngleAxisd rotzp1(p1a*M_PI/180, Eigen::Vector3d::UnitZ());
    Eigen::Matrix3d rotp1 = rotzp1.toRotationMatrix();
    Eigen::Isometry3d p1;
    p1 = rotp1;
    p1.translation() = Eigen::Vector3d(p1x,p1y, 0);

    //2.p2 world positon
    double p2a=45;
    double p2x=2;
    double p2y=2;
    Eigen::AngleAxisd rotzp2(p2a*M_PI/180, Eigen::Vector3d::UnitZ());
    Eigen::Matrix3d rotp2 = rotzp2.toRotationMatrix();
    Eigen::Isometry3d p2;
    p2 = rotp2;
    p2.translation() = Eigen::Vector3d(p2x,p2y, 0);

    //3.calculate t12
    Eigen::Isometry3d t12=p1.inverse() *p2;
    Eigen::Matrix3d r12m=t12.rotation();
    Eigen::Vector3d vt12=t12.translation();
    Eigen::Vector3d eulerAngle=r12m.eulerAngles(0,1,2);
    cout<<"eulerAngle roll pitch yawn"<<180*eulerAngle/M_PI<<endl;
    cout<<"t12="<<endl<<vt12<<endl;
}

２.局部坐标系－>世界坐标系

已知p1和t12求p2
假设机器在p1处在世界坐标系下的位姿为（x，y，z，θ）=（1，1，0，0），p1 和p2之间的转换（局部坐标系）为t12表示为(1,1,0,45)，
求p2处世界坐标系下的位姿（x，y，z，θ）？
结果为（2，2，0，45）
程序如下：

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include<iostream>
using namespace std;
int main()
{
    //1.p1 world position
    double p1a=0;
    double p1x=1;
    double p1y=1;
    Eigen::AngleAxisd rotzp1(p1a*M_PI/180, Eigen::Vector3d::UnitZ());
    Eigen::Isometry3d p1;
    p1 = rotzp1.toRotationMatrix();
    p1.translation() = Eigen::Vector3d(p1x,p1y, 0);

     //2. t12 value
    double t12a=45;
    double t12x=1;
    double t12y=1;
    Eigen::AngleAxisd rott12(t12a*M_PI/180, Eigen::Vector3d::UnitZ());
    Eigen::Isometry3d t12;
    t12= rott12.toRotationMatrix();
    t12.translation()=Eigen::Vector3d(t12x, t12y, 0);

    //3.calculate p2 world positon
    Eigen::Isometry3d p2=p1*t12;
    Eigen::Matrix3d p2rm=p2.rotation();
    Eigen::Vector3d p2t=p2.translation();
    Eigen::Vector3d eulerAngle=p2rm.eulerAngles(0,1,2);
    cout<<"eulerAngle roll pitch yawn"<<180*eulerAngle/M_PI<<endl;
    cout<<"t12="<<endl<<p2t<<endl;
}

最后

以上就是斯文帆布鞋为你收集整理的Eigen局部坐标系和世界坐标系转换１.世界坐标系－>局部坐标系２.局部坐标系－>世界坐标系的全部内容，希望文章能够帮你解决Eigen局部坐标系和世界坐标系转换１.世界坐标系－>局部坐标系２.局部坐标系－>世界坐标系所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。