Eigen局部坐标系和世界坐标系转换

最新推荐文章于 2025-07-08 14:32:37 发布

火星机器人life

最新推荐文章于 2025-07-08 14:32:37 发布

阅读量9.7k

点赞数 4

CC 4.0 BY-SA版权

分类专栏： SLAM

本文链接：https://blog.youkuaiyun.com/ktigerhero3/article/details/77155199

SLAM 专栏收录该内容

48 篇文章

订阅专栏

本文介绍如何使用Eigen库实现从世界坐标系到局部坐标系及从局部坐标系到世界坐标系的转换。通过具体实例展示了如何计算两个不同位置之间的转换矩阵，并逆向计算局部坐标系下位置的世界坐标。

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

本文主要介绍使用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 yaw\n"<<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 yaw\n"<<180*eulerAngle/M_PI<<endl;
    cout<<"t12="<<endl<<p2t<<endl;
}