"越有故事的人越沉靜簡單，越膚淺單薄的人越浮躁不安！"

無論是做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 <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;
}
 

２.區域性座標系－>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 <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;
}

3.附一個Lidar標定轉換的小程式，可以參考

#include <iostream>
#include <Eigen/Dense>


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