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)


    std::cout<< P1<<std::endl;
    std::cout<< P2<<std::endl;

    std::cout<< "======={P0 = }==================="<<std::endl;
    
    auto P0 = t1.inverse() * P1;//  t1.inverse() * P1 ==t2.inverse() * P2
    
    std::cout<< t1.inverse() * P1<<std::endl;//验证 P0 = t1.inverse() * P1  输出 [0,0,0,1]
    std::cout<< t2.inverse() * P2<<std::endl;//验证 P0 = t2.inverse() * P2  输出 [0,0,0,1]

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

    std::cout<< t12.inverse() <<std::endl;

    std::cout<< (t12 * P1) <<std::endl; //P2 = t12 *P1
    return 0;
}

最后

以上就是内向钢笔为你收集整理的Eigne 坐标变换 （四）的全部内容，希望文章能够帮你解决Eigne 坐标变换 （四）所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。