SLAM公式引出、推导和理解 2-2

3 视觉里程计

根据相邻的图像信息估计出粗略的相机运动，给后端（非线性优化）提供良好的初始值。算法主要分为特征点法和光流法。

3.1 特征点法

特征点法由于是基于两帧之间（相邻图像）的运动所提出的算法，也成为两视图几何（Two-view geometry）。定位与构图都是基于图像上的信息，特征点法是通过选取图像上比较有代表性的点。在经典SLAM中，我们称之为路标，对于视觉SLAM，称之为图像特征（Feature）。特征是图像信息的另一种数学表达式。
特征点由关键点（Key-point）和描述子（Descriptor）组成。关键点是指该特征点在图像里的位置，有些特征点还具有朝向、大小等信息；描述子通常是一个向量，按照某种认为设计的方式，描述了该关键点周围像素的信息。描述子的设计原则为外观相似的特征该有相似的描述子，换句话说就是两个特征点的描述子在向量空间上的距离相近，就可以认为它们为相同的特征点。典型算法有 ORB 算法。

ORB特征
ORB（Oriented FAST and Rotated BRIEF)特征点，是由带方向的FAST关键点和BRIEF描述子组成的。
FAST是一种角点，主要检测局部像素灰度变化明显的地方，思想是如果一个像素与邻域的像素差别较大（过明或过暗），更可能是角点。检测过程如下：

• 在图像中选取像素 $p$，假设它的亮度为 $I_p$。
• 设置一个阈值 $T$（比如，$I_p$ 的 20%)。
• 以像素 $p$ 为中心，选取半径为 3 的圆上的 16 个像素点。
• 若选取的圆上有连续的 $N$ 个点的亮度在 $I_p\pm T$ 范围之外，那么像素 $p$ 可以被认为是特征点（$N$ 通常取值为 9、11、12，即被称为 FAST-9 、FAST-11 、 FAST-12）。
• 循环以上四步，对每个像素执行相同的操作。
  

在 FAST-12 算法中，为了提高运算速率，可以对像素进行预处理，以排除绝大多数非角点的像素：对于每个像素，直接检测邻域圆上的第 1、5、9、13 个像素的亮度，仅当以上四个点中的三个点同时在 $I_p\pm T$ 范围之外时，当前像素可能为角点。但也存在着重复性不强、分布不均匀的缺点。同时，FAST 也不具备方向信息，并存在着尺度问题：远看是角点，近处却不一定是。
对于 ORB 来说，为关键点带有尺度不变性而提出的构建图像金字塔（对图像进行不同层次的降采样，以获得不同分辨率的图像），并在每一层上检测角点来实现。
金字塔塔底是原始图像。每往上一层，就对图像进行一个固定倍率的缩放，从而生成了不同分辨率的图像。层数越高，图像分辨率越低。上层的图像可以看做是远处过来的场景。通过匹配不同层上的图像，从而实现尺度不变性。如相机在后退时，可以在前一帧的图像金字塔上层和当前帧的图像金字塔下层中找到匹配。

旋转不变性通过提出的灰度质心法（Intensity Centroid）实现，计算特征点附近的图像灰度质心（图像块灰度值作为权重的中心），有

• 定义图像块 $B$ 的矩（一阶矩）$$m_{pq}=\sum _{x,y\in B}{x}^p{y}^{q}I(x,y)p,q=0,1$$
• 计算图像块的质心 $C$（一阶矩/零阶矩）$$C=\left(\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}}\right)$$
• 向量 $\overrightarrow{OC}$ 的角度 $\theta$ ($O$为图像的几何中心)，特征点的方向$$\theta =\arctan (m_{01}/m_{10})$$

BRIEF描述子
提取关键点之后，计算每个关键点的描述子。BRIEF描述子是一种二进制描述子，其描述向量由许多个 0 和 1 组成，代表着关键点附近两个随机像素 $p$ 和 $q$ 的大小关系：$p>q$，则取 1 ，反之取 0。若选取了 $N$ 对点比较，则会产生 $N$ 维由 0 和 1 组成的向量。原始的BRIEF描述子不具有方向，而对于ORB来说，由于关键点具有方向，因此描述子也具有旋转不变性。

---

3.2 相机运动

通过特征点法匹配了相邻图像中好的点对，根据点对估计相机运动。由于相机的不同，分为下列情况：单目相机——只有 2D 像素坐标，根据两组坐标估计运动，用对极几何解决；双目相机等（已知深度信息），已知 3D 坐标估计运动，用ICP解决；已知一组为 3D，一组为 2D坐标（3D 点和在相机中投影的位置）估计相机运动，通过PnP求解。

---

对极几何

取两帧图像 $I_1$ 和 $I_2$ 的运动，设第一帧到第二帧的运动为 $R$ 和 $t$。两个相机中心分别为 $O_1$ 和 $O_2$。其中，$I_1$ 中有特征点 $p_1$，$I_2$ 中有特征点 $p_2$。假设匹配成功，则该两点为同一空间在相邻平面上的投影。
首先，连线 $\overrightarrow{O_1{p}_1}$ 和 $\overrightarrow{O_2{p}_2}$ 在三维空间交于点 $P$。点$O_1，O_2，P$ 共面，称为极平面（Epipolar plane）。$O_1{O}_2$ 连线与像平面 $I_1,I_2$ 交于点 $e_1,e_2$，称为极点（Epipoles），$O_1{O}_2$ 被称为基线。称极平面与像平面之间的相交线 $l_1,l_2$ 为极线（Epipolar line）。
设 $P$ 在 $O_1$ 坐标系下的空间坐标为 $P=[X,Y,Z{]}^T$，像素点 $p_1,p_2$ 的像素位置为 $$s_1{p}_1=KP，s_2{P}_2=K(RP+t)\text{\hspace{1em}(3.2.1)}$$有时，会使用齐次坐标形式表达像素点。一个向量将等于它自身乘上任意的非零常数，这常用来表达一个投影关系，尺度意义下的相等，记作$$sp\simeq p$$则上述两个投影关系可以写成$$p_1\simeq KP，p_2\simeq K(RP+t)\text{\hspace{1em}(3.2.2)}$$ $P$ 为中间变量，令$$x_1=K^{-1}{p}_1，x_2=K^{-1}{p}_2\text{\hspace{1em}(3.2.3)}$$
因为像素点可以视为齐次坐标，所以 $x_1，x_2$ 为归一化坐标（归一化平面为 $Z=1$），则有$$\begin{array}{rl}{x}_2& \simeq R{x}_1+t\\ t^{\wedge }{x}_2& \simeq t^{\wedge }R{x}_1\\ x_2^T{t}^{\wedge }{x}_2& \simeq x_2^T{t}^{\wedge }R{x}_1\end{array}$$

因为 $t^{\wedge }{x}_2$ 表示为 $t$ 和 $x_2$ 所构成平面，因此式子左侧为零，故有$$x_2^T{t}^{\wedge }R{x}_1=0\text{\hspace{1em}(3.2.4)}$$代入式子(3.2.3)有$$p_2^T{K}^{-T}{t}^{\wedge }R{K}^{-1}{p}_1=0\text{\hspace{1em}(3.2.5)}$$式子(3.2.4)(3.2.5)称为对极约束。
几何意义为 $O_{1}P{O}_2$ 共面（$x_2,t,R{x}_1$ 共面，其中$x_1$代表 $O_1{p}_1$，$x_2$代表 $O_2{p}_2$，$R$ $t$代表从 $O_1$ 到 $O_2$，$p_1$ $p_2$为点 $P$ 的投影）
取 $E=t^{\wedge }R$ 为本质矩阵(Essential Matrix)和 $F=K^{-T}E{K}^{-1}$ 为基础矩阵(Fundamental Matrix) $F$，简化对极约束为$$x_2^{T}E{x}_1=p_2^{T}F{p}_1=0\text{\hspace{1em}(3.2.6)}$$于是，相机位姿估计变成以下两步

• 根据匹配点的像素位置求出 $E$ 或 $F$
• 根据 $E$ 或 $F$ 求出 $R,t$

由于 $E$ 和 $F$ 只差了一个已知的相机内参矩阵 $K$，因此往往使用更为简单的 $E$。

$F$ 的自由度是7。首先是 $3\times 3$矩阵，9个自由度，然后有尺度等价性和 $det(F)=0$ 两个约束条件。9-2=7。

本质矩阵
由于本质矩阵是根据对极约束定义的，对极约束为等式为零的约束，乘以任意一个非零常数项约束条件仍满足，因此称为 $E$ 在不同尺度上是等价的；同时，由于 $E=t^{\wedge }R$ 可知，本质矩阵为 $3\times 3$ 的矩阵，可以证明其奇异值必定为 $diag[\sigma ,\sigma ,0]$ 的形式¹（本质矩阵的内在性质）；$R$ 旋转有三个自由度，$t$ 平移有两个自由度（尺度等价），故本质矩阵有五个自由度。有五个自由度表示需要至少五对特征点来求解 $E$，但是对于内在性质（非线性性质）来说，只用五对点来求解时，会在估计中带来麻烦，因此，可以只考虑本质矩阵的尺度等价性，即用 $3\times 3-1$ 对点来计算 $E$—八点法（Eight-point-algorithm）。
一对特征点的归一化坐标为 $x_1=[u_1,v_1,1{]}^T,x_2=[u_2,v_2,1{]}^T$ 根据对极约束则有$$\left[\begin{array}{ccc}{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]\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=0$$将 $E$ 写成向量的形式$$e=[e_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9{]}^T$$则对极约束可以写为如下线性形式$$[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]\cdot e=0\text{\hspace{1em}(3.2.7)}$$用 $u^i,v^i$ 表示第 $i$ 个特征点，选取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_2^8{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\times 9$，若此矩阵满秩，则可以计算出 $E$。由奇异值分解（SVD）可得 ¹，可以将矩阵 $E$ 分解为 $t^{\wedge }R$ 的形式有如下四种$$\begin{array}{rl}& \\ \pm t_1^{\wedge }& =U{R}_Z(\frac{\pi }{2})\Sigma U^T,R_1=U{R}_Z^T(\frac{\pi }{2})V^T\\ \pm t_2^{\wedge }& =U{R}_Z(\frac{\pi }{2})\Sigma U^T,R_2=U{R}_Z^T(\frac{\pi }{2})V^T\end{array}\text{\hspace{1em}(3.2.8)}$$其中 $R_Z(\frac{\pi }{2})$ 表示沿 $Z$ 轴旋转 90° 得到的旋转矩阵。

只有（1）是正确的深度。通常，根据线性方程组解出的 $E$ ，可能不满足内在性质 $\Sigma =diag[{\sigma }_1,{\sigma }_2,{\sigma }_3]，{\sigma }_1>{\sigma }_2>{\sigma }_3$ ，因此会可以调整为$$E=U\Sigma V^T=Udiag[\frac{{\sigma }_1+{\sigma }_2}{2},\frac{{\sigma }_1+{\sigma }_2}{2},0]V^T$$

单应矩阵
单应矩阵 $H$（Homography），描述了两个平面之间的映射关系。若场景的特征落在同一平面上，则可以通过单应性估计运动。
单应矩阵描述的就是同一个平面的点在不同图像之间的映射关系。如扫描银行卡时，在相机中的银行卡不是正对着的，扫描后的卡是规则的矩形。银行卡四个点都是同一平面，不同的图像就是指相机中的和扫描之后的。又如将什么信息P到高速路两边的广告牌上。还有重要的应用，相机标定，张正友相机标定法。
设图像 $I_1$ 和图像 $I_2$ 有一对匹配好的特征点 $p_1$ 和 $p_2$，这两个特征点落在同一平面上，此平面的平面方程（点法式）为$$\begin{array}{r}{n}^{T}P+d=0\\ -\frac{n^{T}P}{d}=1\end{array}$$ 将上式带入式子(3.2.2)中，有（整理点 $P$）$$\begin{array}{rl}{p}_2& \simeq K(RP+t)\\ & \simeq K\left(RP+t\cdot (-\frac{n^{T}P}{d})\right)\\ & \simeq K\left(R-\frac{t{n}^T}{d}\right)P\\ & \simeq K\left(R-\frac{t{n}^T}{d}\right)K^{-1}{p}_1\\ & \simeq H{p}_1\end{array}\text{\hspace{1em}(3.2.9)}$$其中 $H$ 的定于与旋转、平移和平面的参数有关。求解思路与 $E$ 类似。将上式展开$$\left[\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right]\simeq \left[\begin{array}{ccc}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right]\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]$$由于上式为投影关系，因此有$$\begin{array}{r}{u}_2=\frac{u_2}{1}=\frac{h_1{u}_1+h_2{v}_1+h_3}{h_7{u}_1+h_8{v}_1+h_9}\\ v_2=\frac{v_2}{1}=\frac{h_4{u}_1+h_5{v}_1+h_6}{h_7{u}_1+h_8{v}_1+h_9}\end{array}$$因为尺度的不确定性，可以令 $h_9=1$，则有$$\begin{array}{r}{h}_1{u}_1+h_2{v}_1+h_3-h_7{u}_1{u}_2-h_8{v}_1{u}_2=u_2\\ h_4{u}_1+h_5{v}_1+h_6-h_7{u}_1{v}_2-h_8{v}_1{v}_2=v_2\end{array}$$一组特征点有两个约束条件（实际上有三个，但是第三行线性相关）则求解 $H$ 矩阵可以通过四组特征点（八个线性方程）$$\left[\begin{array}{cccccccc}{u}_1^1& v_1^1& 1& 0& 0& 0& -u_1^1{u}_2^1& -v_1^1{u}_2^1\\ 0& 0& 0& u_1^1& v_1^1& 1& -u_1^1{v}_2^1& -v_1^1{v}_2^1\\ u_1^2& v_1^2& 1& 0& 0& 0& -u_1^2{u}_2^2& -v_1^2{u}_2^2\\ 0& 0& 0& u_1^2& v_1^2& 1& -u_1^2{v}_2^2& -v_1^2{v}_2^2\\ u_1^3& v_1^3& 1& 0& 0& 0& -u_1^3{u}_2^3& -v_1^3{u}_2^3\\ 0& 0& 0& u_1^3& v_1^3& 1& -u_1^3{v}_2^3& -v_1^3{v}_2^3\\ u_1^4& v_1^4& 1& 0& 0& 0& -u_1^4{u}_2^4& -v_1^4{u}_2^4\\ 0& 0& 0& u_1^4& v_1^4& 1& -u_1^4{v}_2^4& -v_1^4{v}_2^4\end{array}\right]\left[\begin{array}{c}{h}_1\\ h_2\\ h_3\\ h_4\\ h_5\\ h_6\\ h_7\\ h_8\end{array}\right]=\left[\begin{array}{c}{u}_2^1\\ v_2^1\\ u_2^2\\ v_2^2\\ u_2^3\\ v_2^3\\ u_2^4\\ v_2^4\end{array}\right]$$此做法将 $H$ 矩阵视为向量，通过解该向量的线性方程来恢复 $H$，又称直接线性变换法（Direct Linear Transform，DLT）。也可以讲等式同时乘以等号左式的叉乘，使等号左边为0。分解 $H$ 矩阵有数值法、解析法等。
单应性在现实生活中具有重要意义，当特征点共面或者相机发生纯旋转时，基础矩阵的自由度就会下降，出现了所谓的退化（degenerate）。现实中的数据带有噪声，如果此时继续用八点法求解基础矩阵，基础矩阵多出来的自由度将会有噪声决定，为了避免退化现象造成的影响，通常我们会同时估计基础矩阵 $F$ 和单应矩阵 $H$，选择重投影误差小的那个作为最终的运动估计矩阵。

尺度不确定性：对 $t$ 长度的归一化，直接导致了单目视觉的尺度不确定性。在单目SLAM中，对轨迹和地图同时缩放任意倍数，得到的图像都是一样的。单目视觉中，对两个图的 $t$ 归一化，相当于固定了尺度。虽然我们不知道它的实际长度是多少，但我们以这时的 t 为单位 1，计算相机运动和特征点的 3D 位置。这被称为单目 SLAM 的初始化。在初始化之后，就可以用 3D−2D 计算相机运动了。因此，单目 SLAM 有一步不可避免的初始化。初始化的两张图像必须有一定程度的平移，而后的轨迹和地图都将以此步的平移为单位。

初始化的纯旋转问题：单目初始化不能只有纯旋转，必须要有一定程度的平移。
多于 8 对点的情况，可以计算最小二乘解。多于八个点，$Ae=0$为超定方程。可以通过最小化二次型求得。
$$\underset{e}{\min }||Ae|{|}_2^2=\underset{e}{\min }{e}^T{A}^{T}Ae$$
于是就求出了在最小二乘意义下的 $E$ 矩阵。不过，当可能存在误匹配的情况时，我们会更倾向于使用随机采样一致性（ Random Sample Concensus， RANSAC） 来求，而不是最小二乘。RANSAC 是一种通用的做法，适用于很多带错误数据的情况，可以处理带有错误匹配的数据。

---

三角测量
对极约束估计了相机运动，下一步需要用相机运动来估计特征点的空间位置，需要用到三角测量（Triangulation）（或三角化）。
考虑图像 $I_1$ 和 $I_2$，以左图为参考，右图的变换矩阵为 $T$。相机光心为 $O_1$ 和 $O_2$。在 $I_1$ 上有特征点 $p_1$，对应 $I_2$ 上有特征点 $p_2$。理论上，直线 $O_1{p}_1$ 与 $O_2{p}_2$ 会相交于一点 $P$，但实际中存在噪声，直线无法相交，可以通过最小二乘法求解。
按照对极几何中的定义，$x_1$ 和 $x_2$ 为两个特征点的归一化坐标，则满足$$s_2{x}_2=s_{1}R{x}_1+t$$已知 $R$ 和 $t$，求解特征点的深度 $s_1$ 和 $s_2$。从几何关系上看，可以从射线 $O_1{p}_1$ 上寻找点 $P$，使其投影位置靠近 $p_2$；同理 $O_2{p}_2$；也可以在两条线的中间找。例如，对于计算 $s_1$ 来说，上式左乘 $x_2^{\wedge }$，得$$s_2{x}_2^{\wedge }{x}_2=0=s_1{x}_2^{\wedge }R{x}_1+x_2^{\wedge }t$$上式左侧为零，右侧只有 $s_1$ 一个未知数，则可计算。当然，由于噪声的存在，通常求解为最小二乘解。

---

PnP

PnP(Perspective-n-Point)描述了知道 $n$ 个3D空间点及其投影位置，如何估计相机的位姿。如果两张图像中的一张特征点的3D位置已知，那么最少只需3个点对（以及至少一个额外点验证结果）就可以估计相机运动。对于双目相机或者可测量深度的相机（RGB-D），可以直接使用PnP估计相机运动，而对于单目相机来说，需要先三角化已知3D点的深度，才能使用PnP。PnP无需对极约束，又可以在很少的匹配点中获得较好的运动估计，使最重要的姿态估计方法。主要介绍PnP方法中的直接线性变换（DLT）和光束法平差（Bundle Adjustment，BA）—非线性优化的方式，构建最小二乘问题并迭代求解。

直接线性变换
有如下问题：已知一组3D点的位置，以及它们在某个相机中的投影位置，求该相机的位姿。对于空间点 $P$，齐次坐标为 $P=(X,Y,Z,1{)}^T$。在图像 $I_1$ 中，投影到特征点 $x_1=(u_1,v_1,1{)}^T$ （用齐次坐标在归一化平面上表述）。此时，相机位姿 $R$ 和 $t$ 是未知的。与单应矩阵的求法类似，我们定义增广矩阵 $[R|t]$ 为一个 $3\times 4$ 的矩阵，将其展开为$$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)$$解法同单应矩阵$$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$ 的行向量$${\mathbf{t}}_1=(t_1,t_2,t_3,t_4{)}^T,{\mathbf{t}}_2=(t_5,t_6,t_7,t_8{)}^T,{\mathbf{t}}_3=(t_9,t_{10},t_{11},t_{12}{)}^T$$则有$${{\mathbf{t}}_1}^{T}P-{{\mathbf{t}}_3}^{T}P{u}_1=0,{{\mathbf{t}}_2}^{T}P-{{\mathbf{t}}_3}^{T}P{v}_1=0$$可以看到每个特征点提供了两个关于 $\mathbf{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}{\mathbf{t}}_1\\ {\mathbf{t}}_2\\ {\mathbf{t}}_3\end{array}\right)=0\text{\hspace{1em}(3.1.10)}$$其中 $\mathbf{t}$ 一共有 12 维，因此至少通过 6 对匹配点可以实现 $[R|t]$ 的求解。当匹配点大于6对的时候，也可以使用SVD等方法对超定方程²求最小二乘解。但求解解出的 $R$ 不满足旋转矩阵的约束，需要对其进行投影到流形，通过$$R\leftarrow (R{R}^T{)}^{-\frac{1}{2}}R$$求解近似问题。

P3P
需要利用给定的3个点的几何关系。输入数据为3对3D-2D匹配点。记3
点为 $A$ $B$ $C$，2D点为$a$ $b$ $c$，其中小写字母代表的点为对应大写字母代表的点在相机成像平面上的投影。P3P 还需要使用一对验证点，以从可能的解中选出正确的那一个（类似于对极几何情形）。记验证点对为 $D-d$，相机光心为 $O$。$A$, $B$, $C$ 在世界坐标系中的坐标，而不是在相机坐标系中的坐标。
由余弦定理可得
$$\begin{array}{rl}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\end{array}\text{\hspace{1em}(3.1.11)}$$
对上式全体除以 $O{C}^2$，并记 $x=OA/OC,y=OB/OC$，得
$$\begin{array}{rl}& x^2+y^2-2xy\cos ⟨a,b⟩=A{B}^2/O{C}^2\\ & y^2+1-2y\cos ⟨b,c⟩=B{C}^2/O{C}^2\\ & x^2+1-2x\cos ⟨a,c⟩=A{C}^2/O{C}^2\end{array}\text{\hspace{1em}(3.1.12)}$$
记 $v=A{B}^2/O{C}^2,uv=B{C}^2/O{C}^2,wv=A{C}^2/O{C}^2$，有
$$\begin{array}{rl}& x^2+y^2-2xy\cos ⟨a,b⟩-v=0\\ & y^2+1-2y\cos ⟨b,c⟩-uv=0\\ & x^2+1-2x\cos ⟨a,c⟩-wv=0\end{array}\text{\hspace{1em}(3.1.13)}$$
上式 $v$ 代入后两式整理，得
$$\begin{array}{rl}& (1-u)y^2-u{x}^2-\cos ⟨b,c⟩2y+2uxy\cos ⟨a,b⟩+1=0\\ & (1-w)x^2-w{y}^2-\cos ⟨a,c⟩2x+2wxy\cos ⟨a,b⟩+1=0\end{array}\text{\hspace{1em}(3.1.14)}$$
由于我们知道 2D 点的图像位置，3 个余弦角是已知的。$u=B{C}^2/A{B}^2,w=A{C}^2/A{B}^2$ 可以通过 $A$, $B$, $C$ 在世界坐标系下的坐标算出，变换到相机坐标系下之后，这个比值并不改变。该式中的 $x,y$ 是未知的，随着相机移动会发生变化。因此，该方程组是关于 $x,y$ 的一个二元二次方程（多项式方程）。方程组的解析解是一个复杂的过程，需要用吴消元法。类似于分解 E 的情况，该方程最多可能得到 4 个解，但我们可以用验证点来计算最可能的解，得到 $A,B,C$ 在相机坐标系下的 3D 坐标。然后，根据 3D−3D 的点对，计算相机的运动 $R,t$。
从 P3P 的原理可以看出，为了求解 PnP，我们利用了三角形相似性质，求解投影点 $a,b,c$ 在相机坐标系下的 3D 坐标，最后把问题转换成一个 3D 到 3D 的位姿估计问题。

最小化重投影误差
非线性优化是把相机位姿和空间点一起作为优化变量，进行最小化误差的办法，称为Bundle Adjustment。考虑 $n$ 个空间点 $P$ 及其投影 $p$，希望计算相机位姿 $R$ 和 $t$，用李群 $T$ 来表示。假设空间点的坐标为 $P_i=[X_i,Y_i,Z_i{]}^T$ 及其投影点的像素坐标 ${\mathbf{u}}_{\mathbf{i}}=[u_i,v_i{]}^T$，有$$\begin{array}{rl}{s}_i{\mathbf{u}}_{\mathbf{i}}& =KT{P}_i\\ 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]\end{array}\text{\hspace{1em}(3.1.15)}$$由于相机位姿未知及观测点的噪声，该等式存在一个误差。现将相机位姿和空间点的误差求和，构建一个最小二乘问题，然后寻找最好的相机位姿，使它最小化：$$T^{\ast }=\arg \underset{T}{\min }\frac{1}{2}\sum _{i=1}^n{\Vert {\mathbf{u}}_{\mathbf{i}}-\frac{1}{s_i}KT{P}_i\Vert }_2^2\text{\hspace{1em}(3.1.16)}$$该问题的误差项是将 3D投影点位置与观测位置作差，所以称为重投影误差。使用齐次坐标，误差最后一维为0，因此更多时候使用非其次坐标。我们通过特征匹配知道了 $p1$ 和 $p2$ 是同一个空间点 $P$ 的投影，但是不知道相机的位姿。在初始值中，$P$ 的投影 ${\hat{p}}_2$ 与实际的 $p_2$ 之间有一定的距离。于是我们调整相机的位姿，使得这个距离变小。不过，由于这个调整需要考虑很多个点，所以最后的效果是整体误差的缩小，而每个点的误差通常都不会精确为零。
使用高斯牛顿法和列文伯格—马夸尔特方法之前，我们需要知道每个误差项关于优化变量的导数，对上述误差函数线性化
$$e(x+\Delta x)\approx e(x)+J^T\Delta x\text{\hspace{1em}(3.1.17)}$$

当 $e$ 为像素坐标误差（ 2 维），$x$ 为相机位姿（ 6 维）时， $J^T$ 将是一个 $2\times 6$ 的矩阵。我们来推导 $J^T$ 的形式。

首先，记变换到相机坐标系下的空间点坐标为 $P^{\prime }$ ，并且将其前 3 维取出来：
$$P^{\prime }=(TP{)}_{1:3}=[X^{\prime },Y^{\prime },Z^{\prime }{]}^T\text{\hspace{1em}(3.1.18)}$$
那么，相机投影模型相对于$P^{\prime }$为
$$su=K{P}^{\prime }\text{\hspace{1em}(3.1.19)}$$
展开
$$\left[\begin{array}{c}su\\ sv\\ s\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}^{\prime }\\ Y^{\prime }\\ Z^{\prime }\end{array}\right]\text{\hspace{1em}(3.1.20)}$$
整理得
$$u=f_x\frac{X^{\prime }}{Z^{\prime }}+c_x,v=f_y\frac{Y^{\prime }}{Z^{\prime }}+c_y\text{\hspace{1em}(3.1.21)}$$
求误差时，可以把这里的 $u$ $v$与实际的测量值求差。
在定义了中间变量（相机坐标系点$P^{\prime }$ ）后，我们对 $T$ 左乘扰动量 $\delta \xi$，然后考虑误差 $e$ 的变化关于扰动量的导数。
$$\frac{\partial x}{\partial \delta \xi }=\underset{\delta \xi \to 0}{\lim }\frac{e(\delta \xi \oplus \xi )-e(\xi )}{\delta \xi }=\frac{\partial e}{\partial P^{\prime }}\frac{\partial P^{\prime }}{\partial \delta \xi }\text{\hspace{1em}(3.1.22)}$$
这里的 $\oplus$ 指李代数上的左乘扰动。第一项是误差关于投影点的导数，容易得
$$\frac{\partial e}{\partial P^{\prime }}=-\left[\begin{array}{ccc}\frac{\partial u}{\partial X^{\prime }}& \frac{\partial u}{\partial Y^{\prime }}& \frac{\partial u}{\partial Z^{\prime }}\\ \frac{\partial v}{\partial X^{\prime }}& \frac{\partial v}{\partial Y^{\prime }}& \frac{\partial v}{\partial Z^{\prime }}\end{array}\right]=-\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]\text{\hspace{1em}(3.1.23)}$$
第二项为变换后的点关于李代数的导数
$$\frac{\partial TP}{\partial \delta \xi }=(TP{)}^{\odot }=\left[\begin{array}{cc}I& -P^{\prime \wedge }\\ 0^T& 0^T\end{array}\right]\text{\hspace{1em}(3.1.24)}$$
而在 $P^{\prime }$ ，我们取了前三维，
$$\frac{\partial P^{\prime }}{\partial \delta \xi }=[I,-P^{\prime \wedge }]\text{\hspace{1em}(3.1.25)}$$
于是得
$$\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]\text{\hspace{1em}(3.1.26)}$$
这个雅可比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系。我们保留了前面的负号，这是因为误差是由观测值减预测值定义的。当然也可以反过来，将它定义成“预测值减观测值”的形式。

除了优化位姿，我们还希望优化特征点的空间位置。因此，需要讨论 e 关于空间点 $P$ 的导数。
$$\frac{\partial e}{\partial P}=\frac{\partial e}{\partial P^{\prime }}\frac{\partial P^{\prime }}{\partial 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$$

推导出了观测相机方程关于相机位姿与特征点的两个导数矩阵。它们十分重要，能够在优化过程中提供重要的梯度方向，指导优化的迭代。

---

ICP

迭代最近点 Iterative Closest Point ：对激光来说，无法知道两个点集之间的匹配关系，只能认为距离最近的两个点为同一个，所以称为迭代最近点。而在视觉中，特征点提供了更好的匹配关系。

SVD求解
两组对应的匹配点
P = { p 1 , ⋯   , p n } , P ′ = { p 1 ′ , ⋯   , p n ′ } P=\{p_1,\cdots,p_n\},P^\prime=\{p^\prime_1,\cdots,p^\prime_n\} P={p1​,⋯,pn​},P′={p1′​,⋯,pn′​}
定义第 $i$ 对点的误差为
$$e_i=p_i-(R{p}_i^{\prime }+t)$$
构建最小二乘问题，使误差平方和达到极小的 $R$，$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 })$$
处理误差函数
$$\begin{array}{rl}\frac{1}{2}\sum _{i=1}^n\Vert p_i-(R{p}_i^{\prime }+t){\Vert }^2& =\frac{1}{2}\sum _{i=1}^n\Vert p_i-R{p}_i^{\prime }-t-p+R{p}^{\prime }+p-R{p}^{\prime }{\Vert }^2\\ & =\frac{1}{2}\sum _{i=1}^n\Vert (p_i-p-R(p_i^{\prime }-p^{\prime }))+(p-R{p}^{\prime }-t){\Vert }^2\\ & =\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\\ & +2(p_i-p-R(p_i^{\prime }-p^{\prime }){)}^T(p-R{p}^{\prime }-t))\end{array}$$
对
$$\begin{gathered} \sum _{i=1}^n(p_i-p)=\sum _{i=1}^n(p_i)-np=0 \\ \sum _{i=1}^n(p_i^{\prime }-p^{\prime })=\sum _{i=1}^n(p_i^{\prime })-n{p}^{\prime }=0 \end{gathered}$$
上式简化为
$$\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$。

ICP分解

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

展开关于 $R$ 的误差项
$$\frac{1}{2}\sum _{i=1}^n\Vert q_i-R{q}_i^{\prime }{\Vert }^2=\frac{1}{2}\sum _{i=1}^n(q_i^T{q}_i+q_i^{\prime }{R}^{T}R{q}_i^{\prime }-2q_i^{T}R{q}_i^{\prime })$$
因为第一项和第二项与 $R$ 无关。因此，优化目标函数变为
$$\sum _{i=1}^n-q_i^{T}R{q}_i^{\prime }=\sum _{i=1}^n-tr(R{q}_i^{\prime }{q}_i^T)=tr\left(R\sum _{i=1}^n{q}_i^{\prime }{q}_i^T\right)$$

通过 SVD 解出上述问题中最优的 $R$：
定义矩阵
$$W=\sum _{i=1}^n{q}_i{q}_i^{\prime T}$$
对 $W$ 进行SVD分解，得
$$W=U\Sigma V^T$$
当 $W$ 满秩时，有
$$R=U{V}^T$$
此时 $R$ 的行列式为负，则取 $-R$ 为最优值。

---

非线性优化
以迭代的方式去找最优值。
以李代数表示位姿时，目标函数可以写为
$$\underset{\xi }{\min }=\frac{1}{2}\sum _{i=1}^n\Vert p_i-\exp ({\xi }^{\wedge })p_i^{\prime }{\Vert }_2^2$$
单个误差项关于位姿的导数为
$$\frac{\partial e}{\partial \delta \xi }=-(\exp ({\xi }^{\wedge })p_i^{\prime }{)}^{\odot }$$
于是，只需不断迭代，就能找到极小值。而且，可以证明ICP问题存在唯一解和无穷解的情况。当为唯一解时，只要找到极小值，这个极小值就位全局最优值。这意味着可以选定任意的初始值。

---

1. 出自Multiple View Geometry in Computer Vision(Second Edition) ↩︎ ↩︎

2. 超定方程组是指方程个数大于未知量个数的方程组。超定方程一般是不存在解的矛盾方程。例如，如果给定的三点不在一条直线上， 我们将无法得到这样一条直线，使得这条直线同时经过给定这三个点。 也就是说给定的条件（限制）过于严格， 导致解不存在。在实验数据处理和曲线拟合问题中，求解超定方程组非常普遍。比较常用的方法是最小二乘法。形象的说，就是在无法完全满足给定的这些条件的情况下，求一个最接近的解。 ↩︎