Eigen实现坐标转换

本文通过《视觉SLAM十四讲》第三讲习题7,详细介绍了如何利用Eigen库进行坐标转换,涉及四元数和欧拉矩阵的方法。通过计算两个不同坐标系下同一个点的坐标,展示坐标转换的步骤,并提供了相应的Eigen代码实现。
摘要由CSDN通过智能技术生成

(《视觉SLAM十四讲》第三讲习题7)设有小萝卜一号和二号在世界坐标系中。一号位姿q1 = [0.35, 0.2, 0.3, 0.1],t1=[0.3, 0.1, 0.1]。二号位姿q2=[-0.5, 0.4, -0.1, 0.2], t2=[-0.1, 0.5, 0.3].某点在一号坐标系下坐标为p=[0.5, 0, 0.2].求p在二号坐标系下的坐标

假设在世界坐标系中p点的坐标为P。

用四元数做旋转则有(在Eigen中四元数旋转为q×v,数学中则为q×v×q^-1):

  • q1 × P + t1 = p1
  • q2 × P + t2 = p2

由上两式分别解算出:

  • P = q1^-1 × (p1 - t1)
  • P = q2^-1 × (p2 - t2)

两式联立求解则得到:

p2 = q2 × q1^-1 × (p1 - t1) + t2

如果用欧拉矩阵(设一号欧拉矩阵为T1,二号欧拉矩阵为T2)则有:

  • p1 = T1 × P
  • p2 = T2 × P

求解P:

  • P = T1^-1 × p1
  • P = T2^-1 × p2

联立求解则有:

p2 = T2 × T1^-1 × p1

以下则是用Eigen实现的代码:

  • 2
    点赞
  • 13
    收藏
    觉得还不错? 一键收藏
  • 2
    评论
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值