Eigne 坐标变换 （五）

#include 
#include 
#include 
#include 
int main()
{

    Eigen::Quaterniond q1 = Eigen::Quaterniond(0, 0, 0, 90).normalized();
    Eigen::Quaterniond q2 = Eigen::Quaterniond(0, 90, 0, 0).normalized();
    Eigen::Vector3d p1 = Eigen::Vector3d(1, 2, 3);
    Eigen::Vector3d p2 = Eigen::Vector3d(4, 5, 6);

    Eigen::Matrix4d t1 = Eigen::Matrix4d();
    t1.setIdentity();
    t1.block<3,3>(0,0) = q1.toRotationMatrix();
    t1.block<3,1>(0,3) = p1;

    Eigen::Matrix4d t2 = Eigen::Matrix4d();
    t2.setIdentity();
    t2.block<3,3>(0,0) = q2.toRotationMatrix();
    t2.block<3,1>(0,3) = p2;

    std::cout<< t1<转换矩阵T1() =>P1(1, 2, 3)
    auto P2 =  t2 *p0;//坐标转换 P0(0, 0, 0) =>转换矩阵T2() =>P2(1, 2, 3)
    auto P0 = t1.inverse() * P1;//  t1.inverse() * P1 ==t2.inverse() * P2

    //
    auto t12 = t2 * t1.inverse(); //t12 = t2 * t1.inverse();

    std::cout<< "P1  ["< P2 ->P1 从P0转到P2,再转到P1
    std::cout<< "P1 ["<<(t12.inverse() * t2 *P0).transpose()<< "]"<