Eigne 坐标变换 （五）

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
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<<std::endl<<std::endl;
    std::cout<< t2<<std::endl<<std::endl;

    Eigen::Vector4d p0 = Eigen::Vector4d(0, 0, 0, 1);

    auto P1 =  t1 *p0;//坐标转换 P0(0, 0, 0) =>转换矩阵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  ["<<P1.transpose()<< "]"<<std::endl;
    std::cout<< "P2  ["<<P2.transpose()<< "]"<<std::endl;

    //P11 = t12 * P2
    std::cout<< "P11 ["<<(t12.inverse() *P2).transpose()<< "]"<<std::endl;
    // P0 -> P2 ->P1 从P0转到P2,再转到P1
    std::cout<< "P1 ["<<(t12.inverse() * t2 *P0).transpose()<< "]"<<std::endl;
    //P1在 P2下的坐标 = t12逆 *P(0,0,0) 此时P2相当于坐标原点
    return 0;
}