SLAM知识点

原创文章点击这里

1 SLAM实验环境

操作系统：	Ubuntu16 / Ubuntu18
编译工具：	cmake
依赖库：	Eigen、Sophus、

2 三维空间刚体运动

2.1 旋转矩阵（R: 3x3）

假设坐标系从 $e$ 经过了欧式变换后，其正交基向量从 $\left[\begin{array}{c}{e}_1,e_2,e_3\end{array}\right]$ 变成了 $\left[\begin{array}{c}{e}_1^{\prime },e_2^{\prime },e_3^{\prime }\end{array}\right]$，在变换中存在一个向量 $a$，其坐标从 $\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]$ 变成了 $\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]$，所以满足关系：
$$\left[\begin{array}{c}{e}_1,e_2,e_3\end{array}\right]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{c}{e}_1^{\prime },e_2^{\prime },e_3^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]$$
可以转换成：
$$a=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{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}_3^{\prime }& e_1^T{e}_3^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]=R{a}^{\prime }$$
这里的 $R$ 称为旋转矩阵，它是一个行列式为1的正交矩阵（即逆为自身转置的矩阵），则反向旋转有：
$$a^{\prime }=R^{-1}a=R^{T}a$$
如果要描述向量在空间中的欧式变换，还需要一个平移向量 $t$，因为 $R$ 只描述了旋转，所以向量 $a_1$ 到向量 $a_2$ 的欧式变换：
$$a_2=R{a}_1+t$$

2.2 变换矩阵（T: 4x4）

上式的欧式变换用到了 $R$ 和 $t$，有两次变换就不是线性关系，但可以使用齐次坐标和变换矩阵来将它线性化，设变换矩阵 $T$：
$$T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]$$
对应的欧式变化如下：
$$\left[\begin{array}{c}{a}^2\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 0\end{array}\right]\left[\begin{array}{c}{a}^1\\ 1\end{array}\right]=T\left[\begin{array}{c}{a}^1\\ 1\end{array}\right]$$

2.3 旋转向量和欧拉角

• 旋转向量

  旋转状态也可以用一个旋转轴$n$和旋转角$\theta$来描述，组合在一起为旋转向量 ($\theta n$)，它和旋转矩阵可以相互转化：

  旋转向量 --> 旋转矩阵：

  $$R=cos\theta I+(1-cos\theta )n{n}^T+sin\theta \hat{n}$$

  旋转矩阵 --> 旋转向量：

  $$\begin{array}{rl}tr(R)& =cos\theta I+(1-cos\theta )tr(n{n}^T)+sin\theta tr(\hat{n})\\ & =3cos\theta +(1-cos\theta )\\ & =1+2cos\theta \end{array}$$

  $$\theta =arccos\frac{tr(R)-1}{2}$$
  $$Rn=n$$
  上面的 $\hat{n}$ 表示 $n$ 的反对称矩阵:
  $$\hat{a}=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$$

• 欧拉角

  使用 ${\left[\begin{array}{c}r,p,y\end{array}\right]}^T$ 来描述旋转，表示分别绕三个坐标轴旋转，不同的旋转顺序会导致结果不一样，如 $rpy$ 角的旋转顺序是 $ZYX$，欧拉角的缺点是存在万象锁问题；

2.4 四元数

$$q={\left[\begin{array}{c}s,v\end{array}\right]}^T,s=q_0\in \mathbb{R},v={\left[\begin{array}{c}{q}_1,q_2,q_3\end{array}\right]}^T\in {\mathbb{R}}^3$$

四元数为复数，上式中，$s$ 为实部，$v$ 为虚部；

注：c++中的矩阵运算、旋转向量、欧拉角、四元数的转化可以使用Eigen库，参考这里；

3 李群与李代数

3.1 李群

旋转矩阵构成特殊正交群 SO(3)，变换矩阵构成了特殊欧式群 SE(3):
S O ( 3 ) = { R ∈ R 3 ∗ 3 ∣ R R T = I , d e t ( R ) = 1 } SO(3) = \{R\in \mathbb{R}^{3*3} \mid RR^T = I, det(R) = 1\} SO(3)={R∈R3∗3∣RRT=I,det(R)=1}
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 ∗ 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3) = \{T=\begin{bmatrix}R & t \\ 0^T & 1 \end{bmatrix}\in \mathbb{R}^{4*4} \mid R \in SO(3), t \in \mathbb{R}^3\} SE(3)={T=[R0T​t1​]∈R4∗4∣R∈SO(3),t∈R3}
这类矩阵之所以称之为‘群’，是因为它们具有一些属性，比如它们相乘后还是属于一类：
$$R_1{R}_2\in SO(3),T_1{T}_2\in SE(3)$$

3.1 李代数

推导李代数可以由 $R(t)R(t{)}^T=I$ 式子引出，求导后在使用反对称矩阵、泰勒展开、微分方程的方法可以得到式子：
$$R(t)=exp(\widehat{{\varphi }_0}t)$$
即 $R=exp(\hat{\varphi })$ ，这里的 $\varphi$ 就是李代数；

李代数 $\mathtt{S}\mathtt{O}(3)$:
S O ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ^ ∈ R 3 ∗ 3 } \mathtt{SO(3)} = \{\phi \in \mathbb{R}^3, \Phi = \hat{\phi} \in \mathbb{R}^{3*3} \} SO(3)={ϕ∈R3,Φ=ϕ^​∈R3∗3}

李代数 $\mathtt{S}\mathtt{E}(3)$:
S E ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ S O ( 3 ) , ξ ^ = [ ϕ ^ ρ 0 T 0 ] ∈ R 4 ∗ 4 } \mathtt{SE(3)} = \{ \xi = \begin{bmatrix}\rho \\ \phi \end{bmatrix}\in \mathbb{R}^6, \rho \in \mathbb{R}^3, \phi \in \mathcal{\mathtt{SO}(3), \hat{\xi} = \begin{bmatrix} \hat{\phi} & \rho \\ 0^T & 0 \end{bmatrix} \in \mathbb{R}^{4*4}} \} SE(3)={ξ=[ρϕ​]∈R6,ρ∈R3,ϕ∈SO(3),ξ^​=[ϕ^​0T​ρ0​]∈R4∗4}

注：c++中的李群、李代数之间的转化可以使用Sophus库，参考这里；

4 相机与图像

参数	说明
$1/Z$	$Z$是物平面的深度，$X,Y$乘上$1/Z$后深度变成了1,即归一化
$f$	相机焦距
$\alpha ,\beta$	缩放（把物理单位转换成像素单位）
$C_x,C_y$	平移
$K$	集成了$C_x,C_y,\alpha ,\beta$

注:
$$K=\left[\begin{array}{ccc}{f}_x& 0& C_x\\ 0& f_y& C_y\\ 0& 0& 1\end{array}\right],f_x=\alpha f,f_y=\beta f$$

坐标系转换公式：

$$Z{P}_{uv}=Z\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=K(R{P}_w+t)=KT{P}_w$$
其中，$R$、$t$ 为位姿，$P_w$为世界坐标，$(R{P}_w+t)$是相机坐标，$K$ 为相机内参，$P_{uv}$ 为像素坐标，$(R{P}_w+t)/Z$是归一化坐标；

5 非线性优化

参考这里，使用 g2o 模块实现高斯牛顿法；

6 视觉里程计（特征点法）

视觉里程计的算法主要分为两大类：特征点法 和 直接法。

:::tip

• 特征点法：是视觉里程计的主流方法，它具有对光照、动态物体不敏感的优势，是目前比较成熟的解决方案。
• 直接法：
  :::

特征点法首先是提取、匹配图像特征点，然后根据对应的点来估计两帧之间的相机运动和场景结构，从而实现一个两帧间视觉里程计。

估计运动有多种方法，其中，由于相机的原理不同，会采取不同的方法：

相机类型	点类型	方法
单目	2D + 2D	对极几何
双目、RGB-D	3D + 3D	ICP
其他	3D + 2D	PnP

6.1 特征点

特征点指图像中一些特别的地方，比如：角点、区块、边缘，这些特征用于标记相邻图片的对应位置。所以第一步就是通过算法来计算出图像中的特征点，著名算法有：SIFT、SURF、ORB等，找出特征点后，更重要的是把对应的点匹配起来，匹配方法有暴力匹配、快速近似最邻近，相关的算法已经集成在 Opencv 中。

6.2 2D-2D: 对极几何

• 对极约束

  

  图中：

  • $O_1$、$O_2$：相机中心
  • $I_1$、$I_2$：像平面
  • $p_1$、$p_2$：对应的特征点
  • $e_1$、$e_2$：对应的极点
  • $l_1$、$l_2$：对应的极线

  我们知道两个像素点 $p_1$、$p_2$ 的像素位置：
  $$s_1{p}_1=KP,s_2{p}_2=K(RP+t)$$
  其中，$K$为相机内参矩阵，$R$、$t$ 为两个坐标系的相机运动。因为 $s_1{p}_1$ 和 $p_1$ 成投影关系，它们在齐次坐标下的意义是相等的，称这种相等关系为尺度意义下相等，记作：$sp\backsimeq p$，那么上面的投影关系可以写为：
  $$p_1\backsimeq KP,p_2\backsimeq K(RP+t)$$
  现在取：
  $$x_1=K^{-1}{p}_1,x_2=K^{-1}{p}_2$$
  经过推倒可以得到一个式子：
  $$x_2^T\hat{t}R{x}_1=0$$
  带入 $x_1$、$x_2$ 得：
  $$p_2^T{K}^{-T}\hat{t}R{K}^{-1}{p}_1=0$$
  上面的两个式子都称为对极约束，它的几何意义是 $O_1$、$P$、$O_2$ 共面。我们从上式子中提取两个矩阵：基础矩阵$F$、本质矩阵$E$，于是可以进一步简化对极矩阵约束：
  $$E=\hat{t}R,F=K^{-T}E{K}^{-1},x_2^{T}E{x}_1=p_2^{T}F{p}_1=0$$
  对极约束简洁的给出了两个匹配点的空间位置关系，于是，相机位姿估计问题变成以下两步：

  1. 根据匹配点的像素位置求出 $E$ 或 $F$;
  2. 根据 $E$ 或 $F$ 求出 $R$，$t$。

• 本质矩阵
  本质矩阵 $E=\hat{t}R$，它是一个 3x3 的矩阵，有 9 个未知数，我们可以利用 $E$ 的线性性质来使用八点法求解。
  我们设一对匹配点，它们的归一化坐标为 $x_1={\left[\begin{array}{c}{u}_1,v_1,1\end{array}\right]}^T$，$x_2={\left[\begin{array}{c}{u}_2,v_2,1\end{array}\right]}^T$，根据对极约束，有：
  $$\left[\begin{array}{c}{u}_2,v_2,1\end{array}\right]{\left[\begin{array}{ccc}{e}_1& e_2& e_3\\ e_4& e_5& e_6\\ e_7& e_8& e_9\end{array}\right]}^T\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=0$$
  如果我们把矩阵 $E$ 展开，写成向量形式：
  $$e={\left[\begin{array}{c}{e}_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9\end{array}\right]}^T$$
  那么，对极约束改写成与$e$有关的形式：
  $$\left[\begin{array}{c}{u}_2{u}_1,u_2{v}_1,u_2,v_2{u}_1,v_2{v}_1,v_2,u_1,v_1,1\end{array}\right]e=0$$
  上面的式子是关于一对点的约束，如果我们使用8对点，变成了线性方程组：
  $$\left[\begin{array}{ccccccccc}{u}_2^1{u}_1^1& u_2^1{v}_1^1& u_2^1& v_2^1{u}_1^1& v_2^1{v}_1^1& v_2^1& u_1^1& v_1^1& 1\\ u_2^2{u}_1^2& u_2^2{v}_1^2& u_2^2& v_2^2{u}_1^2& v_2^2{v}_1^2& v_2^2& u_1^2& v_1^2& 1\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ u_8^2{u}_1^8& u_2^8{v}_1^8& u_2^8& v_2^8{u}_1^8& v_2^8{v}_1^8& v_2^8& u_1^8& v_1^8& 1\end{array}\right]\left[\begin{array}{c}{e}_1\\ e_2\\ e_3\\ e_4\\ e_5\\ e_6\\ e_7\\ e_8\\ e_9\end{array}\right]=0$$
  如果8对匹配点组成的矩阵满足秩为8的条件，那么$E$的各元素就可以由上诉方程得到。
  :::tip
  到这里，已经得到了本质矩阵$E$，根据它再分解出相机的运动 $R$、$t$需要采用奇异分解（SVD），分解后会得到4组解，不过分别把这4组解拿来运算就可以排除三项不合常规的。
  :::

• 单应矩阵

6.3 三角测量

6.4 3D-2D: PnP

如果两张图像中的特征点的3D位置已知，那么最少需要3对点就可以估计相机运动。在双目、RGN-D的视觉里程计中，可以直接使用PnP估计喜相机运动；而在单目视觉里程计中，必须先要进行初始化才能使用PnP。PnP问题有多种求解方法：P3P、直接线性变换（DLT）、EPnP、UPnP等，还可以使用非线性优化的方式。

• 直接线性变换
  :::tip 针对问题
  假如在世界坐标下存在一批3D点，它们的3D坐标已知，然后相机相对世界坐标系的原点做了运动，并获取到了这些点在相机上的投影，我们需要根据这批3D点坐标和相机的2D投影坐标来求相机的位姿R、t。
  （当然这个问题可以引申，比如把这里的世界坐标换成上一个时刻的相机坐标，那么求解的就是相对位姿变换了。）
  :::
  在这个问题中，我们设这个3D点为$P$，它的齐次坐标为 $P=[X,Y,Z,1{]}^T$，对应的2D投影点 归一化坐标 为$x_1=[u_1,v_1,1{]}^T$，过程中的 $R,T$ 是未知的，所以设定一个增广矩阵$[R|t]$，其展开形式为：
  $$s\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=\left[\begin{array}{cccc}{t}_1& t_2& t_3& t_4\\ t_5& t_6& t_7& t_8\\ t_9& t_{10}& t_{11}& t_{12}\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$$
  :::warning
  上式中的${\left[\begin{array}{c}{u}_1,v_1,1\end{array}\right]}^T$是归一化后的坐标，需要用像素坐标转换的得到。后文的P3P、最小重投影的例子中才是像素坐标。
  :::
  当然，上面式子中的等量关系是3D点在相机坐标下的坐标，等式左边直接用深度 x 归一化，等式右边用[R|t] x 世界坐标。通过上式，可以得到约束：
  $$u_1=\frac{t_{1}X+t_{2}Y+t_{3}Z+t_4}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}},v_1=\frac{t_{5}X+t_{6}Y+t_{7}Z+t_8}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}},$$

为了更简化，设：
$$t_1={\left[\begin{array}{c}{t}_1,t_2,t_3,t_4\end{array}\right]}^T,t_2={\left[\begin{array}{c}{t}_5,t_6,t_7,t_8\end{array}\right]}^T,t_3={\left[\begin{array}{c}{t}_9,t_{10},t_{11},t_{12}\end{array}\right]}^T$$

于是有：
$$t_1^{T}P-t_3^{T}P{u}_1=0$$
$$t_2^{T}P-t_3^{T}P{v}_1=0$$

所以每一个特征点提供了两个关于$t$的线性约束，假设有$N$的特征点，则有以下方程组：
$$\left[\begin{array}{ccc}{P}_1^T& 0& -u_1{P}_1^T\\ 0& P_1^T& -v_1{P}_1^T\\ .& .& .\\ .& .& .\\ .& .& .\\ P_N^T& 0& -u_N{P}_N^T\\ 0& P_N^T& -v_N{P}_N^T\end{array}\right]\left[\begin{array}{c}{t}_1\\ t_2\\ t_3\end{array}\right]=0$$

$t$ 一共有12维，因此最少通过 6对 匹配点（每个点有两个约束）即可求解，这种方法就是直接线性变换（DLT）。当匹配点大于6对时，也可以使用SVD等方法对超定方程求最小二乘解。

• P3P

P3P 是另一种解 PnP 的方法，不过它仅仅使用 3对 匹配点，对数据要求少。
:::tip 针对问题
我们要解决的问题和前面一样，已经知道3D点的世界坐标和相机的2D成像坐标，求相机的位姿R，t。
:::
首先建立这样的相机模型：

图中，$A,B,C$是世界坐标下（不是相机坐标）的点，坐标已知，$a,b,c$是相机上的投影点。

首先，通过余弦定理得到：

$$O{A}^2+O{B}^2-2OA\cdot OB\cdot cos<a,b>=A{B}^2$$
$$O{B}^2+O{C}^2-2OB\cdot OC\cdot cos<b,c>=B{C}^2$$
$$O{A}^2+O{C}^2-2OA\cdot OC\cdot cos<a,c>=A{C}^2$$

以上 3 式整体处以 $O{C}^2$，并且记 $x=OA/OC,y=OB/OC$，得：

$$x^2+y^2-2xycos<a,b>-v=0$$
$$y^2+1-2ycos<b,c>-uv=0$$
$$x^2+1-2xcos<a,c>-wv=0$$

把第一个式子代入到后面两个式子中，得到：

$$(1-u)y^2-u{x}^2-\cos <b,c>y+2uxy\cos <a,b>+1=0$$
$$(1-w)x^2-w{y}^2-\cos <a,c>x+2wxy\cos <a,b>+1=0$$

在这两个式子中，$\cos <a,b>,\cos <b,c>,\cos <a,c>,u=B{C}^2/A{B}^2,w=A{C}^2/A{B}^2$为已知量，那么就剩下未知量$x,y$，所以问题变成了求解关于$x,y$的二元二次方程。求解是一个复杂的过程，需要用到吴消元法，解的的结果最多有4个，可以使用验证点来计算最有可能的解，得到$A,B,C$在相机坐标系下的3D坐标，然后，根据3D-3D的对应点，计算相机的运动$R,t$。

:::danger 这种方法存在的问题是

1. 当匹配的点多余3组时，难以利用更多的信息；
2. 如果3D点或2D点受噪声影响，或者存在误匹配，则算法失效；
   :::

• 最小化重投影误差求解PnP

这种方法比较通用，是一类把相机、三维点放在一起进行最小化的问题。

:::tip 针对问题
要解决的问题和前面一样，已经知道3D点的世界坐标和相机的2D成像坐标，求相机的位姿R，t。
:::
我们设3D点的坐标为$P_i={\left[\begin{array}{c}{X}_i,Y_i,Z_i\end{array}\right]}^T$，2D成像坐标（像素坐标）为$u_i={\left[\begin{array}{c}{u}_i,v_i\end{array}\right]}^T$，用李群$T$来表示$R,t$，那么有以下关系：
$$s_i=\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=KT\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\\ 1\end{array}\right]$$
即：
$$s_i{u}_i=KT{P}_i$$

由于相机的位姿是未知的，并且观测点存在噪声，所以该等式存在一个误差，我们就针对这个误差来构建一个最小二乘问题，然后找到最好的相机位姿，使它最小化：
$$T^{\ast }=\arg \min \frac{1}{2}\sum _{i=1}^n\Vert u_i-\frac{1}{s_1}KT{P}_i{\Vert }_2^2$$

该问题的误差项是将3D点的投影位置与预测位置做差，所以称为重投影误差。如下图，$P_2^{\prime }$为预测位置，$P_2$为投影位置，$e$为投影误差：

最小二乘问题参考这里，其中最关键的要求出误差项关于优化变量的导数，这里不做推导，只列出对应的两个雅可比，一个是误差关于相机位姿的导数，一个是误差关于特征点的导数。

误差关于相机位姿 $T$ 的导数:
$$\frac{\partial e}{\partial \delta \xi }=-\left[\begin{array}{cccccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{{Z^{\prime }}^2}& -\frac{f_x{X}^{\prime }{Y}^{\prime }}{{Z^{\prime }}^2}& f_x+\frac{f_x{X^{\prime }}^2}{{Z^{\prime }}^2}& -\frac{f_x{Y}^{\prime }}{Z^{\prime }}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{{Z^{\prime }}^2}& -f_y-\frac{f_y{Y^{\prime }}^2}{{Z^{\prime }}^2}& \frac{f_y{X}^{\prime }{Y}^{\prime }}{{Z^{\prime }}^2}& \frac{f_y{X}^{\prime }}{Z^{\prime }}\end{array}\right]$$

误差关于特征点 $P$ 的导数:
$$\frac{\partial e}{\partial P}=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{{Z^{\prime }}^2}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{{Z^{\prime }}^2}\end{array}\right]R$$
其中，$[X^{\prime },Y^{\prime },Z^{\prime }{]}^T=(TP{)}_{1:3}$，所以$T$、$P$每一次迭代后，$X^{\prime }$、$Y^{\prime }$、$Z^{\prime }$的值就会发生改变，从而再一次去更新上面的导数值，知道最后达到最优化。

重投影误差的g2o实现参考文章案例2。

6.5 3D-3D ICP

:::tip 针对问题
已经知道两个坐标系下的两组3D点，$P=p_1,...,P_n,P^{\prime }=P_1^{\prime },...,P_n^{\prime }$，求他们之间的位姿变换$R,t$。
:::
同样的，有两种方法可以解决这类问题：线性求解（SVD）、非线性优化求解。

• SVD 方法
  首先定义第 $i$ 对点的误差项：
  $$e_i=p_i-(R{p}_i^{\prime }+t)$$
  然后构建最小二乘问题：
  $$\underset{R,t}{\min }\frac{1}{2}\sum _{i=1}^n\Vert (p_i-(R{p}_i^{\prime }+t)){\Vert }_2^2$$
  如果定义两组点的质心：
  $$p=\frac{1}{n}\sum _{i=1}^n(p_i),p^{\prime }=\frac{1}{n}\sum _{i=1}^n(p_i^{\prime })$$

经过推导，最后的优化目标函数可以简化为：
$$\underset{R,t}{\min }J=\frac{1}{2}\sum _{i=1}^n\Vert p_i-p-R(p_i^{\prime }-p^{\prime }){\Vert }^2+\Vert p-R{p}^{\prime }-t{\Vert }^2$$

仔细观察左右两项，我们发现左边只和旋转矩阵 $R$ 相关，而右边既有 $R$ 也有 $t$，但只和质心相关。我们获得了 $R$，令第二项为零就能得到 $t$。
:::tip ICP分为以下三个步骤求解：

1. 计算两组点的质心位置$p,p^{\prime }$，然后计算每个点的去质心坐标：
   $$q_i=p_i-p,q_i^{\prime }=p_i^{\prime }-p^{\prime }$$
2. 根据以下优化问题计算旋转矩阵：
   $$R^{\ast }=\arg \underset{R}{\min }\frac{1}{2}\sum _{i=1}^n\Vert q_i-R{q}_i^{\prime }{\Vert }^2$$
3. 根据第2步的 $R$ 计算 $t$：
   $$t^{\ast }=p-R{p}^{\prime }$$
   :::

• 非线性优化方法

非线性优化方法和 PnP 的优化一样，不过在优化过程中，仅在一次迭代之后就已经收敛。

7 视觉里程计（直接法）

7.1 2D 光流

:::tip
直接法是由光流演变而来的。光流描述了像素在图像中的运动，而直接法则附带着一个相机运动模型。
:::

对于图像中的运动，假设同一个物体的灰度像素不变，那么它会随着时间的流逝出现在不同的位置，我们要做的就是判断它在图像上的运动方向。其中，计算部分像素运动的称为稀疏光流，计算所有像素的称为稠密光流。稀疏光流方法： Lucas-Kanade、高斯牛顿法，稠密光流：Horn-Schunck。

7.1.1 Lucas-Kanade 光流

光流问题的前提假设是 灰度不变 ：同一个空间点的像素灰度值，在各个图像中是固定不变的。

对于 $t$ 时刻位于 $(x,y)$ 处的像素，我们设 $t+dt$ 时刻它运动到 $(x+dx,y+dy)$ 处，由于灰度不变，可以得到：
$$I(x+dx,y+dy,t+dt)=I(x,y,t)$$
对等式左边泰勒展开：
$$I(x+dx,y+dy,t+dt)\approx I(x,y,t)+\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt$$
由于假设了灰度不变，通过上式可以得到：
$$\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt=0$$
等式两边处以 $dt$，得到：
$$\frac{\partial I}{\partial x}\frac{dx}{dt}+\frac{\partial I}{\partial y}\frac{dy}{dt}=-\frac{\partial I}{\partial t}$$

设，$\frac{dx}{dt}$ 是在 $x$ 轴上的运动速度 $u$，$\frac{dy}{dt}$ 是在 $y$ 轴上的运动速度 $v$，同时，$\frac{\partial I}{\partial x}$ 是该点在 $x$ 方向的梯度 $I_x$，$\frac{\partial I}{\partial x}$ 是在 $y$ 方向的梯度 $I_y$。把图像灰度对时间的变化量记为 $I_t$：
$$\left[\begin{array}{cc}{I}_x& I_y\end{array}\right]\left[\begin{array}{c}u\\ v\end{array}\right]=-I_t$$
我们想计算的是像素的运动 $u,v$，但是该式是带有两个变量的一次方程，仅凭它无法计算出 $u,v$。因此需要其他的约束条件，由于在 LK 光流中假设了某以窗口内 $w\times w$ 的像素具有相同的运动（窗口指追踪像素附近的一块区域），那我们可以把窗口内的像素一起加入运算，形成超定线性方程：
$${\left[\begin{array}{cc}{I}_x& I_y\end{array}\right]}_k\left[\begin{array}{c}u\\ v\end{array}\right]=-I_{tk},k=1,...,w^2$$
传统的解法是求最小二乘解。opencv 有自带的 LK 算法模块：

cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error);

:::details

#include <opencv2/opencv.hpp>
#include <iostream>

int main()
{
    cv::Mat img1, img2;
    img1 = cv::imread("./cancer1-1.jpg", 1);
    img2 = cv::imread("./cancer1-2.jpg", 1);

    std::vector<cv::KeyPoint> kp1;
    cv::Ptr<cv::GFTTDetector> detector = cv::GFTTDetector::create(500, 0.01, 20); // maximum 500 keypoints
    detector->detect(img1, kp1); // 从img1中提取特征点

    std::vector<cv::Point2f> pt1, pt2;
    for (auto &kp: kp1) pt1.push_back(kp.pt); // 改变特征点格式，从 KeyPoint 到 Point2f

    std::vector<uchar> status;
    std::vector<float> error;
    cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error); // 计算光流

    for (int i = 0; i < pt2.size(); i++) {
        if (status[i]) { // 如果存在对应光流点，就绘制下面的点、线
            cv::circle(img1, pt1[i], 2, cv::Scalar(0, 255, 0), 2);
            cv::line(img1, pt1[i], pt2[i], cv::Scalar(0, 255, 0));
        }
    }

    cv::imshow("1", img1);
    cv::waitKey(0);
    return 0;
}

:::

7.1.2 高斯牛顿法

• 单层光流

  我们可以将其定义为一个最小二乘问题：
  $$\underset{\Delta x,\Delta y}{\min }\Vert I_1(x,y)-I_2(x+\Delta x,y+\Delta y){\Vert }_2^2$$
  其中，误差 $e$ 为：
  $$e=I_1(x,y)-I_2(x+\Delta x,y+\Delta y)$$
  各项偏导：
  $$\frac{\partial e_i}{\partial \Delta x}=\frac{I_2(x+\Delta x+1,y+\Delta y)-I_2(x+\Delta x-1,y+\Delta y)}{2}$$
  $$\frac{\partial e_i}{\partial \Delta y}=\frac{I_2(x+\Delta x,y+\Delta y+1)-I_2(x+\Delta x,y+\Delta y-1)}{2}$$
  雅可比为 $J={\left[\begin{array}{cc}\frac{\partial e_i}{\partial \Delta x}& \frac{\partial e_i}{\partial \Delta y}\end{array}\right]}^T$，然后通过下面的方程求出更新量 $\Delta a$：
  $$(\sum _{i=1}^N{J}_i{J}_i^T)\Delta a=\sum _{i=1}^N-J_i{e}_i$$

7.2 直接法

8 后端（BA）

BA 是指从视觉图像中提炼出最优的 3D 模型和相机参数（内参和外参）。

9 后端（位姿）

优化位姿。

10 回环检测

如果像视觉里程计那样只估计相邻帧之间的位姿，那么随着时间的推移，误差会慢慢的累积，那么这样就会对构建全局图会有很大的影响。由于在相机运动的过程中，存在相机回到原位置的情况，那么我们就可以根据这两个时刻的位置来优化整个过程中的位姿变换。

所以我们要解决的问题就是如何判断相机回到了原位置，这里采用 SLAM 的主流方法，即通过特征向量来计算图像的相似性。该方法和前端、后端的估计没有关系，只需要找出图像中特征点，然后根据类型统计这些特征点从而生成一个特征向量，计算向量之间的距离即可表示图像的相似性。

由于特征点有多种类型，所以采用 字典 的方法来存储这些特征，相关的使用参考这里。

11 建图