坐标变换

前言

昨天收到了高翔博士编写的《视觉SLAM十四讲》第二版，总体上变化并不大，主要有以下改动：

• 更多的实例
• 更深入的内容
• 更完善的工程项目
• 更通俗、简洁的表达

其实，如果学习过高博在深蓝学院开设的《视觉SLAM理论与实践》课程，就对新增的内容非常熟悉了。新增的内容基本来自课程的作业，之前一直因为课程还在开设，就没在平台上发布相关的内容。

这里，我准备将新增的内容记录一下，或者搬运一下，首先是坐标变换。

坐标变换

设有小萝卜1号和小萝卜2号位于世界坐标系中。记世界坐标系为W，小萝卜们的坐标系为$R_1$和$R_2$。小萝卜1号的位姿为$q_1=[0.35,0.2,0.3,0.1{]}^T$（$q$的第一项为实部），$t_1=[0.3,0.1,0.1{]}^T$。小萝卜2号的位姿为$q_2=[-0.5,0.4,-0.1,0.2{]}^T$，$t_2=[-0.1,0.5,0.3{]}^T$。这里的$q$和$t$表达的是$T_{R_k,W},k=1,2$，也就是世界坐标系到相机坐标系的变换关系。现在，小萝卜1号看到某个点在自身的坐标系下的坐标为$p_{R_1}=[0.5,0,0.2{]}^T$，求该向量在小萝卜2号坐标系下的坐标。

坐标变换关系：

$$p_{R_2}=T_{R_2,W}{T}_{W,R_1}{p}_{R_1}$$

提示：

• 四元数在使用前需要归一化
• 注意Eigen在使用四元数时的虚部和实部顺序

代码：

#include <iostream>
#include <vector>
#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;
}

CMakeLists.txt：

cmake_minimum_required(VERSION 3.5)
project(coordinateTransform)

include_directories("/usr/include/eigen3")
add_executable(coordinateTransform coordinateTransform.cpp)

参考

[1]《视觉SLAM十四讲》第2版
[2] https://github.com/gaoxiang12/slambook2