Eigen::局部坐标系和base坐标系转换

"越有故事的人越沉静简单，越肤浅单薄的人越浮躁不安！"

 

 

无论是做SLAM还是传感器标定，离不开的是坐标之间的转换关系

Eigen 一个开源的C++矩阵运算库

perfect~

 

言归正传：

使用Eigen库进行空间坐标的转换。
 

 

 

１.base坐标系－>局部坐标系

       已知p1 和p2求 t12

       假设传感器在base坐标系下p1处的位姿为（x，y，z，θ）=（1，1，0，0）

                           在base坐标系下p2处的位姿为（x，y，z，θ）=（2，2，0，45）
       求p1 和p2之间的转换关系 t12 简单表示为(1,1,0,45)。

/*
 * 2019年01月04日
 */
#include 
#include 
#include 
#include 
#include
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<２.局部坐标系－>base坐标系 
  
        已知p1和t12求p2
         假设传感器在base坐标系下p1处的位姿为（x，y，z，θ）=（1，1，0，0），
 
  
                                   p1 和p2之间的转换（局部坐标系）为t12表示为(1, 1, 0, 45)，
        求p2处世界坐标系下的位姿（x，y，z，θ）？
        结果为（2，2，0，45）  
 
  
#include 
#include 
#include 
#include 
#include
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<3.附一个Lidar标定转换的小程序，可以参考 
  
#include 
#include 


using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {

    Eigen::Vector3f e,p;
    p.x() = 4.6;
    p.y() = 1.25;
    p.z() = 0;
    e.x() = 41.9 * M_PI / 180;   //yaw 偏航角
    e.y() = -1 * M_PI / 180;     //pitch 俯仰角 
    e.z() = -0.6 * M_PI / 180;   //roll 水平角

    Eigen::Matrix3f rot;
    rot = Eigen::AngleAxisf(e(0), Eigen::Vector3d::UnitZ())//使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化角轴:AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以（0,0,1）为旋转轴，旋转45度
        * Eigen::AngleAxisf(e(1), Eigen::Vector3d::UnitY())
        * Eigen::AngleAxisf(e(2), Eigen::Vector3d::UnitX());
    
    Eigen::Matrix4f m;  //1.使用旋转矩阵的函数来初始化旋转矩阵:Matrix3d R1=Matrix3d::Identity();
    m.setIdentity();
    m.block<3, 3>(0, 0) = rot;
    m.block<3, 1>(0, 3) = p;

    Eigen::Affine3f vlp;
    vlp.matrix() = m;



    PointCloud pointcloud;
    pcl::fromROSMsg(*msg, pointcloud);

    PointCloudPtr base(new PointCloud);
    pcl::transformPointCloud(pointcloud, *base, vlp);
}
 
  

  
 
  
最后，附（特别好）
 
  
1.一个旋转矩阵（R），旋转向量（V）和四元数（Q）在Eigen中转换关系的总结:
 
  
https://blog.csdn.net/u011092188/article/details/77430988
 
  
2.使用eigen库进行空间变换:https://blog.csdn.net/yangziluomu/article/details/82631783