Eigne 坐标变换(二)

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]

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

  • 0
    点赞
  • 1
    收藏
    觉得还不错? 一键收藏
  • 2
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值