视觉SLAM学习打卡【2】-坐标变换

本讲主要介绍了坐标欧式变换（旋转+平移）。其中，旋转变换不同表示方式之间转换的推导占大篇幅，多是些线性代数的知识。若对欧氏空间、反对称矩阵、基、正交矩阵等基础概念尚存有❓，不妨先回头温故相关的概念，再学习本讲内容将会事半功倍。

一、为什么要进行坐标变换

假设我们在景区，目前我们手上有两个坐标系：世界坐标系（基底1）和以眼睛为原点的“相机”坐标系（基底2）（眼睛类似于机器人的相机，二者都为传感器）。眼睛看到的物体会基于我们自身的坐标系有一个坐标x1，当我们需要判断自己的位置（定位），并去到下一个景点（导航）时，仅仅依靠眼睛收到的信息还不够，更需要知道这些物体基于世界坐标系的一个坐标x2，即需要进行坐标转换x1—>x2。

二、数学基础

1.向量坐标表示

向量a在线性空间的基[e1,e2,e3]下的坐标[a1,a2,a3]：
$$\mathrm{a}=[{\mathrm{e}}_1,{\mathrm{e}}_2,{\mathrm{e}}_3][\begin{array}{c}{\mathrm{a}}_1\\ {\mathrm{a}}_2\\ {\mathrm{a}}_3\end{array}]={\mathrm{a}}_1{\mathrm{e}}_1+{\mathrm{a}}_2{\mathrm{e}}_2+{\mathrm{a}}_3{\mathrm{e}}_3$$

2.内积/数量积/点积 & 外积/向量积/叉积

$$\mathrm{a}\cdot \mathrm{b}={\mathrm{a}}^{\mathrm{T}}\mathrm{b}=\sum _{\mathrm{i}=1}^3{\mathrm{a}}_{\mathrm{i}}{\mathrm{b}}_{\mathrm{i}}=|\mathrm{a}||\mathrm{b}|\cos ⟨\mathrm{a},\mathrm{b}⟩$$
$$\mathrm{a}\times \mathrm{b}=[\begin{array}{ccc}\mathrm{i}& \mathrm{j}& \mathrm{k}\\ {\mathrm{a}}_1& {\mathrm{a}}_2& {\mathrm{a}}_3\\ {\mathrm{b}}_1& {\mathrm{b}}_2& {\mathrm{b}}_3\end{array}]=\left[\begin{array}{c}{\mathrm{a}}_2{\mathrm{b}}_3-{\mathrm{a}}_3{\mathrm{b}}_2\\ {\mathrm{a}}_3{\mathrm{b}}_1-{\mathrm{a}}_1{\mathrm{b}}_3\\ {\mathrm{a}}_1{\mathrm{b}}_2-{\mathrm{a}}_2{\mathrm{b}}_1\end{array}\right]$$

3.反对称矩阵

$${\mathrm{a}}^{\wedge }=[\begin{array}{ccc}0& -{\mathrm{a}}_3& {\mathrm{a}}_2\\ {\mathrm{a}}_3& 0& -{\mathrm{a}}_1\\ -{\mathrm{a}}_2& {\mathrm{a}}_1& 0\end{array}]$$
附：其余线性代数基础请点击链接【速通线性代数】

4.四元数

• 四元数定义：类似于复数。1个实部+3个虚部
  $$\mathrm{q}={\mathrm{q}}_0+{\mathrm{q}}_1\mathrm{i}+{\mathrm{q}}_2\mathrm{j}+{\mathrm{q}}_3\mathrm{k}$$
• 向量表示形式：
  $$\mathrm{q}=[\mathrm{s},\mathrm{v}],\mathrm{s}={\mathrm{q}}_0\in \mathbb{R},\mathrm{v}=[{\mathrm{q}}_1,{\mathrm{q}}_2,{\mathrm{q}}_3{]}^{\mathrm{T}}\in {\mathbb{R}}^3$$
• 运算法则：
  $$\{\begin{array}{r}{\mathrm{i}}^2={\mathrm{j}}^2={\mathrm{k}}^2=-1\\ \mathrm{i}\mathrm{j}=\mathrm{k},\mathrm{j}\mathrm{i}=-\mathrm{k}\\ \mathrm{j}\mathrm{k}=\mathrm{i},\mathrm{k}\mathrm{j}=-\mathrm{i}\\ \mathrm{k}\mathrm{i}=\mathrm{j},\mathrm{i}\mathrm{k}=-\mathrm{j}\end{array}$$
  附：其余四元数运算请点击链接【四元数的运算】
• 四元数表示旋转
  类似于复变(复变量乘以$e^{i\theta }$,将原向量逆时针旋转角度θ），可以用单位四元数（$e^{i\theta }$模长为1）表示旋转。

旋转度数	效果
乘以 i / j / k	旋转180°
乘以i方 i^2=-1	旋转360°得到一个相反向量
旋转两周	与原先的样子相等

三、坐标的欧式变换

1.何为欧式变换

欧式变换：前后的两个坐标系下，同一个向量的模长和方向不发生改变。欧式变换=旋转+平移。

2.旋转矩阵

同一个向量a在空间中是不变的，但在不同的坐标系基底下对应的坐标值。
$$[{\mathrm{e}}_1,{\mathrm{e}}_2,{\mathrm{e}}_3][\begin{array}{c}{\mathrm{a}}_1\\ {\mathrm{a}}_2\\ {\mathrm{a}}_3\end{array}]=[{\mathrm{e}}_1^{\prime },{\mathrm{e}}_2^{\prime },{\mathrm{e}}_3^{\prime }]\left[\begin{array}{c}{\mathrm{a}}_1^{\prime }\\ {\mathrm{a}}_2^{\prime }\\ {\mathrm{a}}_3^{\prime }\end{array}\right]$$
等式左右两边同时左乘$[{\mathrm{e}}_1^{\mathrm{T}},{\mathrm{e}}_2^{\mathrm{T}},{\mathrm{e}}_3^{\mathrm{T}}{]}^{\mathrm{T}}$，可得：$$[\begin{array}{c}{\mathrm{a}}_1\\ {\mathrm{a}}_2\\ {\mathrm{a}}_3\end{array}]=\left[\begin{array}{ccc}{\mathrm{e}}_1^{\mathrm{T}}{\mathrm{e}}_1^{\prime }& {\mathrm{e}}_1^{\mathrm{T}}{\mathrm{e}}_2^{\prime }& {\mathrm{e}}_1^{\mathrm{T}}{\mathrm{e}}_3^{\prime }\\ {\mathrm{e}}_2^{\mathrm{T}}{\mathrm{e}}_1^{\prime }& {\mathrm{e}}_2^{\mathrm{T}}{\mathrm{e}}_2^{\prime }& {\mathrm{e}}_2^{\mathrm{T}}{\mathrm{e}}_3^{\prime }\\ {\mathrm{e}}_3^{\mathrm{T}}{\mathrm{e}}_1^{\prime }& {\mathrm{e}}_3^{\mathrm{T}}{\mathrm{e}}_2^{\prime }& {\mathrm{e}}_3^{\mathrm{T}}{\mathrm{e}}_3^{\prime }\end{array}\right]\left[\begin{array}{c}{\mathrm{a}}_1^{\prime }\\ {\mathrm{a}}_2^{\prime }\\ {\mathrm{a}}_3^{\prime }\end{array}\right]\triangleq {\mathrm{R}\mathrm{a}}^{\prime }$$

• 其中R为旋转矩阵.（旋转矩阵为正交矩阵，行向量/列向量是两两正交的单位向量，det|R|=1)
• 所有旋转矩阵构成特殊正交群SO(n): S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ⁡ ( R ) = 1 } \mathrm{SO(n)=\{R\in\mathbb{R}^{n\times n}|RR^T=I,\det(R)=1\}} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}

3.欧式变换的向量表示

$${\mathrm{a}}^{\prime }=\mathrm{R}\mathrm{a}+\mathrm{t}$$

4.变换矩阵与齐次坐标

由${\mathrm{a}}^{\prime }=\mathrm{R}\mathrm{a}+\mathrm{t}$进行多次变换，变换后的形式将会显得啰嗦，特此引入齐次坐标和变换矩阵。在三维向量的末尾添加1,构成的四维向量称为齐次坐标，将旋转和平移写入一个矩阵中，得到变换矩阵T。
$$[\begin{array}{c}{\mathrm{a}}^{\prime }\\ 1\end{array}]=\left[\begin{array}{cc}\mathrm{R}& \mathrm{t}\\ 0& 1\end{array}\right]\left[\begin{array}{c}\mathrm{a}\\ 1\end{array}\right]\triangleq \mathrm{T}\left[\begin{array}{c}\mathrm{a}\\ 1\end{array}\right]$$

• 特殊欧式群SE(3):$\mathrm{S}\mathrm{E}(3)=\{\mathrm{T}=[\begin{array}{cc}\mathrm{R}& \mathrm{t}\\ 0& 1\end{array}]\in {\mathbb{R}}^{4\times 4}|\mathrm{R}\in \mathrm{S}\mathrm{O}(3),\mathrm{t}\in {\mathbb{R}}^3\}$

四、旋转的不同表现形式

表示形式	数学表达式
旋转矩阵	$[\begin{array}{ccc}{\mathrm{e}}_1^{\mathrm{T}}{\mathrm{e}}_1^{\prime }& {\mathrm{e}}_1^{\mathrm{T}}{\mathrm{e}}_2^{\prime }& {\mathrm{e}}_1^{\mathrm{T}}{\mathrm{e}}_3^{\prime }\\ {\mathrm{e}}_2^{\mathrm{T}}{\mathrm{e}}_1^{\prime }& {\mathrm{e}}_2^{\mathrm{T}}{\mathrm{e}}_2^{\prime }& {\mathrm{e}}_2^{\mathrm{T}}{\mathrm{e}}_3^{\prime }\\ {\mathrm{e}}_3^{\mathrm{T}}{\mathrm{e}}_1^{\prime }& {\mathrm{e}}_3^{\mathrm{T}}{\mathrm{e}}_2^{\prime }& {\mathrm{e}}_3^{\mathrm{T}}{\mathrm{e}}_3^{\prime }\end{array}]$
旋转向量/轴角Axis-Angle(存在奇异性)	θn（θ-旋转角度 n-旋转轴向量）
欧拉角（直观但存在万向锁问题）	[r,p,y] ^T (rpy角的旋转顺序为ZYX)
四元数（无奇异性）	$\mathrm{q}=[\mathrm{s},\mathrm{v}],\mathrm{s}={\mathrm{q}}_0\in \mathbb{R}，v=[{\mathrm{q}}_1,{\mathrm{q}}_2,{\mathrm{q}}_3{]}^{\mathrm{T}}\in {\mathbb{R}}^3$

五、旋转不同表现形式之间的转换

转换	公式
旋转向量到旋转矩阵	$\mathrm{R}=\cos \theta \mathrm{I}+(1-\cos \theta )\mathrm{n}{\mathrm{n}}^{\mathrm{T}}+\sin \theta {\mathrm{n}}^{\wedge }$
旋转向量到单位四元数	$\mathrm{q}={\left[\cos (\frac{\theta }{2}),\mathrm{n}\sin (\frac{\theta }{2})\right]}^{\mathrm{T}}={\left[\cos (\frac{\theta }{2}),{\mathrm{n}}_{\mathrm{x}}\sin (\frac{\theta }{2}),{\mathrm{n}}_{\mathrm{y}}\sin (\frac{\theta }{2}),{\mathrm{n}}_z\sin (\frac{\theta }{2})\right]}^{\mathrm{T}}$
旋转矩阵到旋转向量	$\begin{array}{rl}& \text{旋转角}\theta =\arccos \left(\frac{\mathrm{t}\mathrm{r}(\mathrm{R})-1}{2}\right)\\ & \text{旋转轴n是矩阵R特征值}1\text{对应的特征向量}\end{array}$
单位四元数到旋转向量*	$\{\begin{array}{l}\theta =2\arccos {\mathrm{q}}_0\\ [{\mathrm{n}}_{\mathrm{x}},{\mathrm{n}}_{\mathrm{y}},{\mathrm{n}}_{\mathrm{z}}]=[{\mathrm{q}}_1,{\mathrm{q}}_2,{\mathrm{q}}_3{]}^{\mathrm{T}}/\sin \frac{\theta }{2}\end{array}$
单位四元数到旋转矩阵	$R=v{v}^T+s^{2}I+2s{v}^{\wedge }+{\left(v^{\wedge }\right)}^2$

六、常见变换的性质比较

笔者对仿射变换和射影变换的了解较少，仅列出有一印象。

变换名称	不变性质
欧式变换	长度、夹角、体积
相似变换	体积比
仿射变换	平行性、体积比
射影变换	接触平面的相交和相切

七、坐标变换的例子

设有小萝卜一号和小萝卜二号位于世界坐标系中。小萝卜一号的位姿为: 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] (q 的第一项为实部)。这里的 q 和 t 表达的是世界到相机的变换关系。现在，小萝卜一号看到某个点在自身的坐标系下的坐标为 p1 = [0.5, 0, 0.2] ，求该向量在小萝卜二号坐标系下的坐标。

#include <iostream>
#include <vector>
#include <algorithm>
#include <Eigen/Core>        //Eigen的核心部分
#include <Eigen/Geometry>   //Eigen的几何模块

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);     //点在小萝卜1号坐标系下的坐标

  Isometry3d T1w(q1), T2w(q2);  //旋转矩阵（自动把四元数转化为旋转矩阵）和平移向量构成变换矩阵
  T1w.pretranslate(t1);
  T2w.pretranslate(t2);

  Vector3d p2 = T2w * T1w.inverse() * p1; //T1w.inverse() * p1把点在小萝卜1号下的坐标转换到世界坐标系；T2w *把点在世界坐标系下的坐标转换到小萝卜2号坐标系中
  cout << endl << p2.transpose() << endl;  //转换后的坐标转置输出
  return 0;
}