由于书上讲的比较清楚，此处仅记录重点知识，梳理相关代码。

1.环境配置

1.1 cmake

1.1.1 基本使用

最基础的cmake 相关指令:project() 与 add_executable

# 声明要求的 cmake 最低版本
 cmake_minimum_required( VERSION 2.8 )
 # 声明一个 cmake 工程
 project( HelloSLAM )
 # 添加一个可执行程序
 # 语法：add_executable( 程序名 源代码文件 ）
 add_executable( helloSLAM helloSLAM.cpp )

相关使用
1
2
3
4
mkdir build cd build cmake .. make
1.1.2 生成库
将add_executable替换成add_library(.a后缀)
1
add_library( hello libHelloSLAM.cpp )
如果生成共享库(.so后缀)
1
add_library( hello_shared SHARED libHelloSLAM.cpp )
除了库文件,还需要提供一个头文件.头文件和库文件需要同时提供给用户
1.1.3 使用库

如果库与源代码在同一个位置下

1 2	`add_executable( useHello useHello.cpp ) target_link_libraries( useHello hello_shared )`

如果库与源代码不在同一个位置下

1.2 IDE Kdevelop

见34

2.知识部分

2.1 基础数学知识

2.1.1 外积的表示

p42

对于向量引入运算^,该运算表示
$$
\left[\begin{array}{ccc}
0 & -a_{3} & a_{2} \
a_{3} & 0 & -a_{1} \
a_{2} & a_{1} & 0
\end{array}\right]
$$
从而 $a\times b$=$a$^$b$
坐标系之间的欧式变换
$$
\left[e_{1}, e_{2}, e_{3}\right]\left[\begin{array}{l}
a_{1} \
a_{2} \
a_{3}
\end{array}\right]=\left[e_{1}^{\prime}, e_{2}^{\prime}, e_{3}^{\prime}\right]\left[\begin{array}{c}
a_{1}^{\prime} \
a_{2}^{\prime} \
a_{3}^{\prime}
\end{array}\right]\Rightarrow\
\left[\begin{array}{l}
a_{1} \
a_{2} \
a_{3}
\end{array}\right]=\left[\begin{array}{lll}
e_{1}^{T} e_{1}^{\prime} & e_{1}^{T} e_{2}^{\prime} & e_{1}^{T} e_{3}^{\prime} \
e_{2}^{T} e_{1}^{\prime} & e_{2}^{T} e_{2}^{\prime} & e_{2}^{T} e_{3}^{\prime} \
e_{3}^{T} e_{1}^{\prime} & e_{3}^{T} e_{2}^{\prime} & e_{3}^{T} e_{3}^{\prime}
\end{array}\right]\left[\begin{array}{l}
a_{1}^{\prime} \
a_{2}^{\prime} \
a_{3}^{\prime}
\end{array}\right] \triangleq \boldsymbol{R} \boldsymbol{a}^{\prime}
$$
中间的矩阵记为R,其中$R^{-1}=R^T$

2.1.2 齐次坐标与特殊欧式群

p46

齐次坐标下的变化可以表示成特殊欧式群中的元素
$$
S E(3)=\left{T=\left[\begin{array}{cc}
R & t \
0^{T} & 1
\end{array}\right] \in \mathbb{R}^{4 \times 4} \mid R \in S O(3), t \in \mathbb{R}^{3}\right}
$$
2.1.3 旋转向量与欧拉角
p50
2.1.3.1 旋转向量
起因:SO(3)的旋转矩阵有九个量,但是一次旋转只有三个自由度;同时旋转矩阵自身带有约束,不利于求解
旋转向量:方向与旋转轴一致,而长度等于旋转角.事实上,旋转向量即使李代数.

2.1.3.2 罗德里格斯公式

-旋转向量到旋转矩阵的转换,公式为
$$
\boldsymbol{R}=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{n} \boldsymbol{n}^{T}+\sin \theta \boldsymbol{n}^{\wedge}
$$

旋转矩阵到旋转向量的转换,公式为
$$
\begin{aligned}
\operatorname{tr}(\boldsymbol{R}) &=\cos \theta \operatorname{tr}(\boldsymbol{I})+(1-\cos \theta) \operatorname{tr}\left(\boldsymbol{n} \boldsymbol{n}^{T}\right)+\sin \theta \operatorname{tr}\left(\boldsymbol{n}^{\wedge}\right) \
&=3 \cos \theta+(1-\cos \theta) \
&=1+2 \cos \theta
\end{aligned}
$$
因此:
$$
\theta=\arccos \left(\frac{\operatorname{tr}(\boldsymbol{R})-1}{2}\right)
$$
关于转轴 $n$, 由于旋转轴上的向量在旋转后不发生改变, 说明
$$
R n=n
$$
因此，转轴 $n$ 是矩阵 $R$ 特征值 1 对应的特征向量。求解此方程, 再归一化, 就得到旋转轴

2.1.3.3 欧拉角

将旋转分解为三次绕不同轴的旋转,顺序为ZYX轴的旋转,三个角度为“偏航-俯仰-滚转”（yaw-pitch-roll）
用$[r,p,y]^T$表示
万向锁问题:在俯仰角为±90◦ 时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度.
(理论上来首,三个实数表示三维旋转都有该问题,所以欧拉角不适合插值和迭代,只能用于人机交互)
2.1.4 四元数
2.1.4.1 四元数定义
找不到不带奇异性的三维向量的描述方式(三维旋转是一个三维流形,想要无奇异性地表达它,三个量是不够的)
四元数是紧凑的,也没有奇异性
$$
q=q_{0}+q_{1} i+q_{2} j+q_{3} k
$$
其中 $i, j, k$ 为四元数的三个虚部
$$
\left{\begin{array}{l}
i^{2}=j^{2}=k^{2}=-1 \
i j=k, j i=-k \
j k=i, k j=-i \
k i=j, i k=-j
\end{array}\right.
$$
也可以表示成
$$
\boldsymbol{q}=[s, \boldsymbol{v}], \quad s=q_{0} \in \mathbb{R}, \boldsymbol{v}=\left[q_{1}, q_{2}, q_{3}\right]^{T} \in \mathbb{R}^{3}
$$
这里, $s$ 称为四元数的实部, 而 $v$ 称为它的虚部。
旋转向量转换为
2.1.4.2 四元数的运算
乘法
共轭
模长
逆
数乘与点乘
2.1.4.3 四元数与旋转
单元四元数表示空间中的一个旋转,乘以i表示绕着i轴旋转了180度(虽然i^2=-1)
2.1.4.3.1 四元数表示旋转
首先, 把三维空间点用一个虚四元数来描述:
$$
\boldsymbol{p}=[0, x, y, z]=[0, \boldsymbol{v}]
$$
这相当于我们把四元数的三个虚部与空间中的三个轴相对应。然后, 参照式 (3.19), 用四元数 $q$ 表示这个旋转:
$$
\boldsymbol{q}=\left[\cos \frac{\theta}{2}, \boldsymbol{n} \sin \frac{\theta}{2}\right]
$$
那么, 旋转后的点 $p^{\prime}$ 即可表示为这样的乘积:
$$
p^{\prime}=q p q^{-1}
$$
计算结果的实部为 0 , 故为纯虚四元数。其虚部的三个分量表示旋转后 $3 \mathrm{D}$ 点的坐标。
2.1.4.3.2 四元数与旋转向量的转换
从旋转向量到四元数
$$
\boldsymbol{q}=\left[\cos \frac{\theta}{2}, n_{x} \sin \frac{\theta}{2}, n_{y} \sin \frac{\theta}{2}, n_{z} \sin \frac{\theta}{2}\right]^{T}
$$
从四元数到旋转响铃
$$
\left{\begin{array}{l}
\theta=2 \arccos q_{0} \
{\left[n_{x}, n_{y}, n_{z}\right]^{T}=\left[q_{1}, q_{2}, q_{3}\right]^{T} / \sin \frac{\theta}{2}}
\end{array}\right.
$$
(旋转$2\pi$得到的是相反的四元数,虽然旋转到了原处)
2.1.4.3.3 四元数与旋转矩阵的转换
通过将四元数转换为旋转向量,再转换为矩阵需要计算arccos,太麻烦,通过下面的工作计算.
设四元数 $q=q_{0}+q_{1} i+q_{2} j+q_{3} k$, 对应的旋转矩阵 $R$ 为:
$$
\boldsymbol{R}=\left[\begin{array}{ccc}
1-2 q_{2}^{2}-2 q_{3}^{2} & 2 q_{1} q_{2}+2 q_{0} q_{3} & 2 q_{1} q_{3}-2 q_{0} q_{2} \
2 q_{1} q_{2}-2 q_{0} q_{3} & 1-2 q_{1}^{2}-2 q_{3}^{2} & 2 q_{2} q_{3}+2 q_{0} q_{1} \
2 q_{1} q_{3}+2 q_{0} q_{2} & 2 q_{2} q_{3}-2 q_{0} q_{1} & 1-2 q_{1}^{2}-2 q_{2}^{2}
\end{array}\right]
$$
反之，由旋转矩阵到四元数的转换如下。假设矩阵为 $\boldsymbol{R}=\left{m_{i j}\right}, i, j \in[1,2,3]$, 其对应的四元数 $q$ 由下式给出:
$$
q_{0}=\frac{\sqrt{\operatorname{tr}(R)+1}}{2}, q_{1}=\frac{m_{23}-m_{32}}{4 q_{0}}, q_{2}=\frac{m_{31}-m_{13}}{4 q_{0}}, q_{3}=\frac{m_{12}-m_{21}}{4 q_{0}}
$$
值得一提的是，由于 $\boldsymbol{q}$ 和 $-\boldsymbol{q}$ 表示同一个旋转, 事实上一个 $\boldsymbol{R}$ 对应的四元数表示并不是惟一的。
实际编程中,当 $q_{0}$ 接近 0 时, 其余三个分量会非常大，导致解不稳定，此时我们再考虑使用其他的方式进行转换。

2.1.4.3.4 四元数的运算

乘法:
$$
\begin{aligned}
\boldsymbol{q}{a} \boldsymbol{q}{b}=& s_{a} s_{b}-x_{a} x_{b}-y_{a} y_{b}-z_{a} z_{b} \
&+\left(s_{a} x_{b}+x_{a} s_{b}+y_{a} z_{b}-z_{a} y_{b}\right) i \
&+\left(s_{a} y_{b}-x_{a} z_{b}+y_{a} s_{b}+z_{a} x_{b}\right) j \
&+\left(s_{a} z_{b}+x_{a} y_{b}-y_{b} x_{a}+z_{a} s_{b}\right) k
\end{aligned}\
\boldsymbol{q}{a} \boldsymbol{q}{b}=[s_as_b-\boldsymbol{v}{a} ^T\boldsymbol{v}{b},s_a\boldsymbol{v}{b}+s_b\boldsymbol{v}{a}+\boldsymbol{v}{a}\times \boldsymbol{v}{b}]
$$
共轭
四元数的共轭是把虚部取成相反数:
$$
\boldsymbol{q}{a}^{*}=s{a}-x_{a} i-y_{a} j-z_{a} k=\left[s_{a},-\boldsymbol{v}{a}\right]
$$
四元数共轭与自己本身相乘，会得到一个实四元数, 其实部为模长的平方:
$$
q^{} \boldsymbol{q}=\boldsymbol{q} \boldsymbol{q}^{}=\left[s{a}^{2}+\boldsymbol{v}^{T} \boldsymbol{v}, \mathbf{0}\right]
$$
逆
$$
q^{-1}=q^*/||q||^2,(q_aq_b)^{-1}=(q_b)^{-1}(q_a)^{-1}
$$
数乘与点乘类似于向量的数乘和点乘/
2.1.5 相似/仿射/射影变换
相似变换
$$
T_{S}=\left[\begin{array}{ll}
s \boldsymbol{R} & \boldsymbol{t} \
\mathbf{0}^{T} & 1
\end{array}\right]
$$
仿射变换
$$
T_{A}=\left[\begin{array}{ll}
A & \boldsymbol{t} \
\mathbf{0}^{T} & 1
\end{array}\right]
$$
其中只要求A是一个可逆矩阵,而不是正交矩阵
射影变换
$$
T_{P}=\left[\begin{array}{ll}
A & \boldsymbol{t} \
\mathbf{a}^{T} & 1
\end{array}\right]
$$
可以对真个矩阵除以v得到一个右下角为1的矩阵,此时有15个自由度
从真实世界到相机照片的变换可以看成一个射影变化,如果相机的i焦距为无穷远,那么这个变换则为仿射变换
3.代码部分

#slam

(2021-10-01)slam 14 courses-review

http://example.com/2023/01/10/2021-10-01-slam-14-courses-review/

Author

Zhiqi Li

Posted on

January 10, 2023

Licensed under

(2021-10-05)Games 102 Geometry-Review Previous

(2021-09-28)3D-Reconstruction Review(1) Next

(2021-10-01)slam 14 courses-review

1.环境配置

1.1 cmake

1.1.1 基本使用

1.1.2 生成库

1.1.3 使用库

1.2 IDE Kdevelop

2.知识部分

2.1 基础数学知识

2.1.1 外积的表示

2.1.2 齐次坐标与特殊欧式群

2.1.3 旋转向量与欧拉角

2.1.3.1 旋转向量

2.1.3.2 罗德里格斯公式

2.1.3.3 欧拉角

2.1.4 四元数

2.1.4.1 四元数定义

2.1.4.2 四元数的运算

2.1.4.3 四元数与旋转

2.1.4.3.1 四元数表示旋转

2.1.4.3.2 四元数与旋转向量的转换

2.1.4.3.3 四元数与旋转矩阵的转换

2.1.4.3.4 四元数的运算

2.1.5 相似/仿射/射影变换

3.代码部分