SLAM实践:坐标变换

题目:假设两个物体位于世界坐标系中,物体1号位姿为q1(四元数表示旋转),t1(表示平移);物体2号位姿为q2(四元数表示旋转),t2(表示平移);现已知物体1号看到某个点在自身坐标系下的坐标为p1,求该点在物体2号坐标系下的坐标p2。

解析:记世界坐标系为W,物体坐标系分别为R1,R2。T表示坐标系之间的变换关系。则公式为 p 2 = T 2 W ∗ T W 1 ∗ p 1 p2=T_{2W} * T_{W1} * p1 p2=T2WTW1p1,且q1,t1是 T 1 W T_1W T1W,需要求逆。

注意:四元数使用之前需要归一化。

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

using namespace std;
using namespace Eigen;

int main(int argc, char** argv) {
  Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
  q1.normalize();
  q2.normalize();
  Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
  Vector3d p1(0.5, 0, 0.2);

  Isometry3d T1w(q1), T2w(q2);
  T1w.pretranslate(t1);
  T2w.pretranslate(t2);

  Vector3d p2 = T2w * T1w.inverse() * p1;
  cout << endl << p2.transpose() << endl;
  return 0;
}
  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值