Eigne 坐标变换（二）

最新推荐文章于 2024-01-16 11:14:29 发布

侃侃_天下

最新推荐文章于 2024-01-16 11:14:29 发布

阅读量875

点赞数

分类专栏： Eigen C++ 文章标签： c++

本文链接：https://blog.csdn.net/douzi949389/article/details/121806636

版权

C++ 同时被 2 个专栏收录

76 篇文章 0 订阅

订阅专栏

Eigen

11 篇文章 5 订阅

订阅专栏

坐标系变换是右乘

坐标变换是左乘

Eigen::Matrix4d 表示平移+ 旋转变换

#include <iostream>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

int main()
{
    //四元数
    Eigen::Quaterniond q1 = Eigen::Quaterniond(0.35, 0.2, 0.3, 0.1).normalized();
    Eigen::Quaterniond q2 = Eigen::Quaterniond(-0.5, 0.4, -0.1, 0.2).normalized();
    //平移向量
    Eigen::Vector3d p1 = Eigen::Vector3d(1, 2, 3);
    Eigen::Vector3d p2 = Eigen::Vector3d(4, 5, 6);

    Eigen::Vector4d P1 = Eigen::Vector4d(1, 2, 3, 1);
    Eigen::Vector4d P2 = Eigen::Vector4d(4, 5, 6, 1);

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

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

    Eigen::Vector4d pO = Eigen::Vector4d(0, 0, 0, 1);
//    std::cout<<"t1 * pO\n"<<t1 * pO;//左乘 表示 相同坐标系 O点=》P1点的
//    std::cout<<"t2 * pO\n"<<t2 * pO;//左乘 表示 相同坐标系 O点=》P2点的 转换

    //四元数求解
    //q1*q12=q2
    Eigen::Quaterniond q12 = q1.inverse() * q2;

     Eigen::Vector3d  p12= q1.toRotationMatrix().inverse()*(p2-p1);

     auto t12 = Eigen::Matrix4d();
     t12.setIdentity();
     t12.block<3,3>(0,0) = q12.toRotationMatrix();
     t12.block<3,1>(0,3) = (p2-p1);
     std::cout<<"t12 \n"<<t12;

     std::cout<<"t12 * P1 \n"<<(t12 * P1);

//     std::cout<<"p2-p1 \n"<<(p2-p1);//右乘 表示 不同坐标系变换 O点=》P2点的 转换
     std::cout<<std::endl;
//    cout<<"translation base on p1 aixs t12="<<t12<<endl;
}

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

int main()
{
    //Eigen::Matrix3d    旋转矩阵(3X3):
    //Eigen::AngleAxisd  旋转向量(3X1):
    //Eigen::Quaterniond 四元数(4X1):
    //Eigen::Vector3d    平移向量(3X1):
    //Eigen::Isometry3d  变换矩阵(4X4):

    auto quat = Eigen::Quaterniond(0.25, 0.1, 0.2, 0.3).normalized();
    std::cout << "quat("<<quat.w() << ","<<quat.x()<<","<<quat.y()<<", "<<quat.z()<<")" << std::endl;

    auto pos = Eigen::Vector3d(0.7, 0.8, 0.9);
    std::cout << "pos("<<pos.x()<<","<<pos.y()<<","<<pos.z()<<")" << std::endl<< std::endl;

    auto matrix3d = quat.toRotationMatrix();
    std::cout << matrix3d << std::endl<< std::endl;

    auto matrix4d = Eigen::Matrix4d();
    matrix4d.setIdentity();
    matrix4d.block<3,3>(0,0) = matrix3d;
    matrix4d.block<3,1>(0,3) = pos;

    std::cout << matrix4d << std::endl<< std::endl;
    return 0;
}

关于向量和坐标系基元是左乘还是右乘，其实举个栗子就可以分清

已知坐标系基元为(i, j, k)，基元表示为一维行向量，因而基元的变换只能是右乘

举个栗子[1x3]=[1x3]*[3x3]。

而坐标表示为一维列向量，因而坐标变换只能是左乘

举个栗子[3x1]=[3x3]*[3x1]

因而本质是写出来的数学关系，让其能够成立，只是一种表述方法。