视觉SLAM学习笔记：六
第七章、第八章
视觉里程计(VO-Visual Odometry)
特征点法
特征点：由关键点和描述子组成
关键点选角点（像素灰度变化明显的地方），
描述子：描述了周围像素的信息，按照“外观相似的特征应该有相似的描述子”设计的
ORB(Oriented FAST and Rotated BRIEF)特征
ORB的关键点是Oriented FAST
描述子是Rotated BRIEF
FAST特征点仅仅比较像素之间亮度的差异
ORB添加了尺度和旋转的描述，尺度不变性依靠构建图像金字塔，并在金字塔的每一层上检测角点实现的，特征的旋转是通过**灰度质心法(Intensity Centroid)**实现的
图像金字塔
金字塔是指通过对图像进行不同层次的降采样，以获得不同分辨率的图像
灰度质心法(Intensity Centroid)
在一小块图像B中，定义图像块的矩为：$$m_{pq}=\sum_{x,y \in B} x^p y^q I(x,y),\;\;\;\;\;\;\;p,q=\{0,1\}$$
通过矩可以找到图像块的质心：$$C=\left(\frac{m_{{10}}}{m_{00}}, \frac{m_{01}}{m_{00}}\right)$$
连接图像块的几何中心$O(x_{0},y_{0})$与质心$C$，得到一个方向向量$\overrightarrow{OC}$，定义特征点的方向为：$$\theta=\arctan\left(\frac{\frac{m_{01}}{m_{00}}-y_{0}}{\frac{m_{10}}{m_{00}}-x_{0}}\right)$$
特别地，对于$O$在原点的情况，有：$$\theta=\arctan\left(\frac{m_{01}}{m_{10}}\right)$$
BRIEF描述子
BRIEF描述子是一种二进制描述子，描述向量由多个0和1组成，这里的0和1编码了关键点附近的两个随机像素（比如p和q）的大小关系：如果p比q大，则取1，反之则取0。
原始的BRIEF描述子不具有旋转不变性，因此在图像发生旋转时容易丢失。而OEB在FAST特征点提取阶段计算了关键点的方向，所以可以利用方向信息，计算旋转后的”Steer BRIEF”特征使ORB的描述子具有较好的旋转不变性。
特征匹配
2D-2D：对极几何
基本模型
首选看基本的模型示意图：
在推导对极约束之前，先定义一些基本的量：
（虽然感觉这样做对下面的推导没有什么帮助，唯一就是看起来简洁了一点点，但是实际上给$x_{1},x_{2}$换个定义就解决了，或者直接带着写也不麻烦。但是，不理解，但尊重，所以这里还是这样写了）
还有就是第一个式子不带变换矩阵应该也是为了简洁，因为只有两张图像，所以干脆P的坐标就是在相机1下的空间坐标
上述两个投影关系可以写为：$$p_{1} \simeq KP \;\;\;\;\;\;\;\; p_{2} \simeq K(RP+t)$$
然后取：$$x_{1}=K^{-1}p_{1} \simeq P,\;\;\;\;\;\;\;\; x_{2}=K^{-1}p_{2} \simeq RP+t$$
故：$$x_{2} \simeq Rx_{1}+t$$
同时左乘$t^{\wedge}$：$$t^{\wedge} x_{2} \simeq t^{\wedge} Rx_{1}$$
再同时左乘$x_{2}^{T}$：$$x_{2}^{T} t^{\wedge} x_{2} \simeq x_{2}^{T} t^{\wedge} Rx_{1}$$
观察左侧，$t^{\wedge} x_{2}$与$t$和$x_{2}$都垂直，故左侧等于0，那右侧也等于0，即：$$x_{2}^{T} t^{\wedge} Rx_{1}=0$$
再把$p_{1} ,p_{2}$带入可得：$$p_{2}^{T} K^{-T} t^{\wedge} R K^{-1} p_{1} = 0$$
把这两个式子都称为对极约束，几何意义是$O_{1},P,O_{2}$三者共面。
对极约束中同时包含了平移和旋转。把中间部分记作两个矩阵：基础矩阵(Fundamental Matrix)F和本质矩阵(Essential Matrix)E，即：$$E=t^{\wedge} R \;\;\;\;\;\;\;\;F=K^{-T}EK^{-1}$$
进一步简化对极约束得到：$$x_{2}^{T} E x_{1}=p_{2}^{T}Fp_{1}=0$$
“对极约束简洁地给出了两个匹配点的空间位置关系。于是，相机位姿估计问题变为以下两步”：
- 根据配对点的像素位置求出$E$或者$F$
- 根据$E$或者$F$求出$R,t$
本质矩阵
设两个坐标：$$x_{1}=[u_{1},v_{1},1]^{T},x_{2}=[u_{2},v_{2},1]^{T}$$
根据对极约束，有：$$\left(\begin{matrix}
u_{2} & v_{2} & 1
\end{matrix}\right) \left(\begin{matrix}
e_{1} & e_{2} & e_{3} \\ e_{4} & e_{5} & e_{6} \\ e_{7} & e_{8} & e_{9}
\end{matrix}\right) \left(\begin{matrix}
u_{1} \\ v_{1} \\ 1
\end{matrix} \right)=0$$
我们希望变为$Ax=0$的形式，那样会好写很多，所以有下面的变换：$$\mathbf{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}u_{1},v_{2},u_{1},v_{1},1]\cdot \mathbf{e}=0$$
带入8个点的结果就是：
这样就把本质矩阵$E$求出来了，现在要恢复出相机的运动$R,t$，使用奇异值分解(SVD)得到，设$E$的$S V D$为：$$E=U \Sigma V^{T}$$
其中$U,V$为正交阵，$\Sigma$为奇异值矩阵。根据E的内在性质，知道$\Sigma = diag(\sigma,\sigma,0)$
在$SVD$分解中，对于任意一个$E$，存在两个可能的$t,R$与它对应：$$t_{1}^{\wedge}=U R_{Z} \left( \frac{\pi}{2} \right) \Sigma U^{T},\;\;\;\;\;R_{1}=UR_{Z}^{T} \left( \frac{\pi}{2} \right) V^{T}$$
其中，$R_{Z}\left( \frac{\pi}{2} \right)$表示沿Z轴旋转$90^{\circ}$得到的矩阵。
由于$-E$与$E$等价，所以对任意一个$t$取负号，也会得到同样的结果。
因此从$E$分解到$t,R$时，一共存在4个可能的解。
这里对于奇异值的处理不理解，不过先接受。
单应矩阵
另一种常见的矩阵是单应矩阵(Homography)$\mathbf{H}$
它描述了两个平面之间的映射关系，若场景种的特征点都落在同一平面上（比如墙、地面等），则可以通过单应性进行运动估计。
单应矩阵通常描述处于共同平面上的一些点在两张图像之间的变换关系。设图像$I_{1}$和$I_{2}$有一对匹配好的特征点$p_{1}$和$p_{2}$。这个特征点落在平面$P$上，设这个平面满足方程：$$n^{T}P+d=0$$
其中$n$是平面的法向量，$d$是一个常数，整理得到：$$-\frac{n^{T}P}{d}=1$$
根据$p_{2} \simeq K(RP+t)$，得到：
记作：$$p_{2} \simeq \mathbf{H} p_{1}$$
展开得到：$$\left(\begin{matrix}
u_{2} \\ v_{2} \\ 1
\end{matrix}\right) \simeq \left(\begin{matrix}
h_{1} & h_{2} & h_{3} \\ h_{4} & h_{5} & h_{6} \\ h_{7} & h_{8} & h_{9}
\end{matrix}\right) \left(\begin{matrix}
u_{1} \\ v_{1} \\ 1
\end{matrix} \right)$$
我们显式写为：$$s \left(\begin{matrix}
u_{2} \\ v_{2} \\ 1
\end{matrix}\right) = \left(\begin{matrix}
h_{1} & h_{2} & h_{3} \\ h_{4} & h_{5} & h_{6} \\ h_{7} & h_{8} & h_{9}
\end{matrix}\right) \left(\begin{matrix}
u_{1} \\ v_{1} \\ 1
\end{matrix} \right)$$
得到三个式子：
- $su_{2}=h_{1}u_{1}+h_{2}v_{1}+h_{3}$
- $sv_{2}=h_{4}u_{1}+h_{5}v_{1}+h_{6}$
- $s=h_{7}u_{1}+h_{8}v_{1}+h_{9}$ 用第三个式子消去第一个式子，得到：
- $u_{2}=\frac{h_{4}u_{1}+h_{5}v_{1}+h_{6}}{h_{7}u_{1}+h_{8}v_{1}+h_{9}}$
- $v_{2}=\frac{h_{4}u_{1}+h_{5}v_{1}+h_{6}}{h_{7}u_{1}+h_{8}v_{1}+h_{9}}$ 实际中，若$h_{9} \neq 0$，则令$h_{9}=1$，整理得到：
- $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}$
这个$\simeq$，或者说这个系数$s$可以任意变化（非零），所以这个$3 \times 3$的矩阵是$9-1=8$个自由度。也可以通过刚才令$h_{9}=1$得出，只有8个未知量了。
一组匹配点可以构成两个约束，所以只要4对匹配点，就能把$\mathbf{H}$算出来了
同样把$\mathbf{H}$的求解写为向量形式，这个类似$Ax=b$
对极几何的三种讨论
尺度不确定性：
简言之，在单目SLAM中，对轨迹和地图同时缩放任意倍数，得到的图像依然是一样的。
在单目视觉中，对两张图像的$t$的归一化相当于固定了尺度，虽然不知道实际的长度是多少，但我们以这时的$t$为单位1，计算相机运动和特征点的3D位置，这被称为单目SLAM的初始化。
在初始化后，就可以用3D-2D计算相机运动了，初始化后的轨迹和地图的单位，就是初始化时固定的尺度，因此，单目SLAM有一步不可避免的初始化。初始化的两张图像必须有一定程度的平移，而后的轨迹和地图都将以此不的平移为单位。
初始化的两种方法：
- 对$t$进行归一化
- 令初始化时所以的特征点的平均深度为1，
相比于令$t$长度为1的做法，特征点深度归一化可以控制场景的规模大小，使计算在数值上更稳定。
- 令初始化时所以的特征点的平均深度为1，
初始化的纯旋转问题
结论：单目初始化不能只有纯旋转，必须要有一定程度的平移。如果没有平移，单目将无法初始化。
多余八对点的情况
八点法中，解的是适定方程，如果多余八对点，就是一个超定方程。
方法是最小二乘误差。
刚才的八点法式子写为：$$A\mathbf{e}=0$$
最小化一个二次型来求解：$$\underset{e}{min} \|A\mathbf{e}\|^{2}_{2}=\underset{e}{min} \;\mathbf{e}^{T}A^{T} A \mathbf{e}$$
于是就得到了在最小二乘意义下的$E$矩阵。
不过，当可能存在误匹配的情况时，我们会更倾向于使用随机采样一致性(Random Sample Concensus, RANSAC) 来求，而不是最小二乘。
RANSAC是一种通用的做法，适用于很多带错误数据的情况，可以处理带有错误匹配的数据。
三角测量
刚刚使用对极几何约束估计了相机运动，在得到运动之后，需要用相机的运动估计特征点的空间位置。
在单目SLAM中，仅通过单张图像无法获得像素的深度信息，我们需要通过三角测量（Triangulation） 的方法估计地图点的深度。
按照对极几何中的定义，设$x_{1},x_{2}$为两个特征点的归一化坐标，满足：$$s_{2} x_{2} = s_{1} R x_{1} + t$$
现在已知$R,t$，想求解两个特征点的深度$s_{1},s_{2}$。从几何上看，可以在射线$O_{1} p_{1}$上寻找3D点，使其投影位置接近$p_{2}$。或者在$O_{2}p_{2}$上找，或者在两条线的中间找，都可以。
例如，希望计算$s_{1}$，那么先对上式两侧左乘一个$x^{\wedge}_{2}$，得：$$s_{2} x_{2}^{\wedge} x_{2} =0= s_{1} x_{2}^{\wedge} R x_{1} + x_{2}^{\wedge} t$$
右侧就是关于$s_{1}$的方程，求得$s_{1}$后，$s_{2}$也可以直接求得。
于是就得到了两帧下的点的深度，确定了它们的空间坐标。
当然，由于噪声的存在，所估得的$R,t$不一定精确使上式为0，所以更常见的做法是求最小而成解而不是直接的解。
三角测量是通过平移得到的，有平移才有对极几何中的三角形.
当平移很小的时候，像素上的不确定性将导致较大的深度不确定性。
如果特征点运动一个像素$\delta x$，使得视线角变化了一个角度$\delta \theta$，那么将测量到深度值有$\delta d$的变化。当$t$较大时，$\delta d$将明显变小，说明平移较大时，在同样的相机分辨率下，三角化测量将更精确。
如下图所示：
提高三角化的精度有两种方法，一种是提高特征点的精度，也就是提高图像的分辨率，但是导致图像变大，增加计算成本。另一种方式是使平移量增大，但是，这会导致图像的外观发生明显的变化，但是外观变化会使得特征提取于匹配变得困难。
简言之，就是增大平移可能导致匹配失效；而平移太小，则三角化精度不够——这就是三角化的矛盾，把这个问题称为“视差”（parallax）
3D-2D：PnP(Perspective-n-Point 透视n点)
PnP是求解3D点对2D点运动的方法
PnP问题有很多求解方法：例如，用3对点估计位姿的P3P、直接线性变换(DLT)、EPnP(Efficient PnP)、UPnP，等等。还可以使用非线性优化的方式，构建最小二乘问题并迭代求解，也就是BA，光束法平差（Bundle Adjustment）
直接线性变换(DLT)
考虑某个空间点P，其次坐标为$P=(X,Y,Z,1)^{T}$，投影到特征点$x_{1}=(u_{1},v_{1},1)^{T}$（以归一化平面齐次坐标表示）。此时，相机的位姿是未知的。
与单应矩阵的求解类似，定义增广矩阵$[R|t]$为一个$3 \times 4$矩阵，包含了旋转与平移信息（与变换矩阵$T$不同），展开如下：$$s \left(\begin{matrix}
u_{1} \\ v_{1} \\ 1
\end{matrix}\right) = \left(\begin{matrix}
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{matrix} \right) \left( \begin{matrix}
X \\ Y \\ Z \\ 1
\end{matrix} \right)$$
和之前的处理类似，用最后一行把$s$消去，得到两个约束：$$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$的行向量：$$t_{1}=(t_{1},t_{2},t_{3},t_{4})^{T},t_{2}=(t_{5},t_{6},t_{7},t_{8})^{T},t_{3}=(t_{9},t_{10},t_{11},t_{12})^{T}$$
把上面的式子可以重新写为：$$t_{1}^{T}P-t_{3}^{T}Pu_{1}=0$$
一对匹配点提供两个关于$t$的线性约束，假设有$N$个特征点，可以得到：$$\left( \begin{matrix} P_{1}^{T} & 0 & -u_{1}P_{1}^{T} \\ 0 & P_{1}^{T} & -v_{1}P_{1}^{T} \\ \vdots & \vdots & \vdots \\ P_{N}^{T} & 0 & -u_{N}P_{N}^{T} \\ 0 & P_{N}^{T} & -v_{N}P_{N}^{T} \end{matrix} \right) \left( \begin{matrix} t_{1} \\ t_{2} \\ t_{3} \end{matrix}\right) =0$$
$t$一共有12维，因此最少通过6对匹配点即可实现矩阵$T$的线性求解，这种方法称为$DLT$，当匹配点大于6对时，也可以使用SVD等方法对超定方程求最小二乘解。这里求解出了12个参数，假设是认为左边是$R$，右边是$t$，右边好说，就是一个三维向量，但是左边的$R$，要求是正交的，但是求出来的不一定是正交的，所以对于$R$，需要对左边$3 \times 3$的矩阵块进行一个近似，寻找一个最好的旋转矩阵对它进行近似，可以由QR分解完成，也可以像这样来计算：$$R \leftarrow (R R^{T})^{-\frac{1}{2}}R$$
这相当于把结果从矩阵空间重新投影到SE(3)流形上，转换成旋转和平移两部分。
补充：流形是一个“局部看起来像欧几里得空间，但整体可能弯曲或有复杂拓扑结构”的几何对象。
或者使用SVD分解：$$R=U \Sigma V^{T}$$
然后令$R_{new}=UV^{T}$即可
P3P
P3P仅使用3对匹配点，对数据要求较少
示意图如下：
由于已知$a,b,c$三点在归一化平面的坐标，所以$\cos
同除$OC^2$，记$x=\frac{OA}{OC},y=\frac{OB}{OC}$，得到：
记$v=\frac{AB^2}{OC^2},uv=\frac{BC^2}{OC^2},wv=\frac{AC^2}{OC^2}$，有：
把第一个式子的$v$带入到后两个式子，整理为：$$(1-u)y^2-ux^2-\cos
式子中$x,y$是未知的，求解这个方程需要用到吴消元法。
类似于分解$E$的情况，该方程最多可能得到4个解，可以用验证点（记为$D-d$）计算最可能的解，得到$A,B,C$在相机坐标系下的3D坐标。然后根据3D-3D的点对，计算相机的运动$R,t$
P3P的问题：
- P3P只利用3个点的信息，当给定的配对点多于3组，难以利用更多的信息
- 如果3D点或2D点受噪声影响，或者存在误匹配，则算法失效
最小化重投影误差
把相机和三维点放在一起进行最小化的问题，统称为Bundle Adjustment
考虑$n$个三维空间点$P$及其投影$p$，我们希望计算相机的位姿$R,t$，李群表示为
$T$。假设某空间点坐标为$P_{i}=[X_{i},Y_{i},Z_{i}]^{T}$，其投影的像素坐标为$u_{i}=[u_{i},v_{i}]^{T}$，可以写为：$$s_{i}u_{i}=KTP_{i}$$现在定义误差：$$e_{i}=u_{i}-\frac{1}{s_{i}}KTP_{i}$$
构建最小二乘问题，目的是求解最好的相机位姿，使得误差平方和最小化：$$T=arg \, \underset{T}{min} \frac{1}{2} \sum_{i=1}^{n} \| e_{i} \|^{2}_{2}$$
该问题的误差项，是将3D点的投影位置与观测位置作差，所以称为重投影误差
使用非齐次坐标$u_{i}=[u_{i},v_{i}]^{T}$，误差就只有两维了。
现在已经成为最小二乘问题了，使用高斯牛顿法或者列文伯格——马夸尔特方法，现在只需要知道每个误差项关于优化变量的导数$J^{T}$：$$e(x+\Delta x)\approx e(x)+J^{T} \Delta x$$
记变换到相机坐标系下的空间坐标为$P'$，并将其前3维取出来：$$P'=(TP)_{1:3}=[X',Y',Z']^{T}$$
得到：$$su=KP'$$
展开：$$\left[ \begin{matrix}
su \\ sv \\s
\end{matrix} \right]=\left[ \begin{matrix}
f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1
\end{matrix} \right] \left[ \begin{matrix}
X' \\ Y' \\ Z'
\end{matrix} \right] $$
同样用第三行消去前两行的$s$，得到：$$u=f_{x} \frac{X'}{Z'}+c_{x}, \;\;\;\; v=f_{y} \frac{Y'}{Z'}+c_{y} $$
故误差就可以写成：$$e_{i}=u_{i}-u$$
写为矩阵就是：$$e_{i}=\left[ \begin{matrix}
u_{i}-u \\ v_{i}-v
\end{matrix} \right]$$
对$T$左乘扰动量$\delta \xi$，考虑$e$的变化关于扰动量的导数，利用链式法则，可以写为下面这个式子：$$\frac{\partial e}{\partial \delta \xi}= {\lim_{ \delta \xi \to 0 } } \frac{e(\delta \xi \oplus \xi) -e(\xi)}{\delta \xi}=\frac{\partial e}{\partial P'} \frac{\partial P'}{\partial \delta \xi}$$
利用矩阵求导规则得到：$$\frac{\partial e}{\partial P'}=-\left[ \begin{matrix}
\frac{\partial u}{\partial X'} & \frac{\partial u}{\partial Y'} & \frac{\partial u}{\partial Z'} \\ \frac{\partial u}{\partial X'} & \frac{\partial u}{\partial Y'} & \frac{\partial u}{\partial Z'}
\end{matrix} \right]=-\left[ \begin{matrix}
\frac{f_{x}}{Z'} & 0 & -\frac{f_{x}X'}{Z^{2}} \\ 0 & \frac{f_{y}}{Z'} & -\frac{f_{y}Y'}{Z'^{2}}
\end{matrix} \right]$$
根据之前关于李代数导数的推导，知道：$$\frac{\partial (TP)}{\partial \delta \xi}=(TP)^{\odot}=\left[ \begin{matrix}
I & -P'^{\wedge} \\ 0^{T} & 0^{T}
\end{matrix} \right]$$
在$P'$的定义中，取出了前三维，于是：$$\frac{\partial P'}{\partial \delta \xi}=\left[\begin{matrix}
I,-P'^{\wedge}
\end{matrix}\right]$$
根据链式法则，把两项相乘，得到：$$\frac{\partial e}{\partial \delta \xi}=-\left[ \begin{matrix}
\frac{f_{x}}{Z'} & 0 & -\frac{f_{x}X'}{Z'^{2}} & -\frac{f_{x}X'Y'}{Z'^{2}} & f_{x}+\frac{f_{x}X'^{2}}{Z'^{2}} & -\frac{f_{x}Y'}{Z'} \\0 & \frac{f_{y}}{Z'} & -\frac{f_{y}Y'}{Z'^{2}} & -f_{y}-\frac{f_{y}Y'^{2}}{Z'^{2}} & \frac{f_{y}X'Y'}{Z'^{2}} & \frac{f_{y}X'}{Z'}
\end{matrix} \right]$$
除了优化位姿，还希望优化特征点的空间位置，所以，需要讨论$e$关于空间点$P$的导数，仍然使用链式法则：$$\frac{\partial e}{\partial P}=\frac{\partial e}{\partial P'} \frac{\partial P'}{\partial P}$$
第一项刚才已经知道了，后一项：$$P'=(TP)_{1:3}=RP+t$$
故：$$\frac{\partial P'}{\partial P}=R$$
所以：$$\frac{\partial e}{\partial P}= -\left[ \begin{matrix}
\frac{\partial u}{\partial X'} & \frac{\partial u}{\partial Y'} & \frac{\partial u}{\partial Z'} \\ \frac{\partial u}{\partial X'} & \frac{\partial u}{\partial Y'} & \frac{\partial u}{\partial Z'}
\end{matrix} \right] R$$
至此，推导出了观测方程关于相机位姿与特征点的两个导数矩阵，它们能够在优化过程中提供重要的梯度方向，指导优化的迭代
使用g2o进行BA优化
这里尊重原版，直接放上来
3D-3D：ICP(Iterative Cloest Point 迭代最近点)
假设有一组配对好的3D点（例如对两幅RGB-D图像进行了匹配）：$$P=\{ p_{1}, \cdots ,p_{n} \},\;\;\;\;P'=\{ p_{1}^{'}, \cdots, p_{n}^{'} \}$$
现在，想要找一个欧式变换$R,t$，使得：$$\forall i ,p_{i}=Rp_{i}^{'}+t$$
考虑3D-3D之间的变换时，和相机没有关系。
下面是两种求解方式：
- 线性代数求解（SVD为主）
- 非线性优化求解（类似于BA）
SVD方法
定义误差项：$$e_{i}=p_{i}-(Rp_{i}^{'}+t)$$
构建最小二乘问题：$$\underset{R,t}{min} \frac{1}{2} \sum^{n}_{i=1} \| e_{i} \|^{2}_{2}$$
下面是求解方法的推导。
首先定义两组点的质心：$$p=\frac{1}{n} \sum^{n}_{i=1}(p_{i}), \;\;\;\;p'=\frac{1}{n} \sum^{n}_{i=1}(p_{i}')$$
然后对误差项进行一加一减：
其中$p_{i}-p$和$p_{i}'-p'$在求和后就是0，所以最后一项直接消失，目标函数可以简化为：$$\underset{R,t}{min} \, J= \frac{1}{2} \sum^{n}_{i=1} \| p_{i}-p-R(p_{i}' -p') \|^{2} + \| p-Rp'-t \|^{2}$$
发现这个式子左侧只和$R$相关，右侧的和$R,t$都有关，而且右侧只和质心相关。
所以只要让式子左侧为最小，获得了$R$后，再把$R$带入第二项，令第二项为0就能得到$t$。
所以，ICP可以分为以下三个步骤求解：
- 计算两组点的质心位置$p,p'$，然后计算每个点的去质心坐标：$$q_{i}=p_{i}-p,\;\;\; q_{i}'=p_{i}'-p'$$
- 构建以下优化问题计算旋转矩阵：$$R^{*}=arg\,\, \underset{R}{min} \frac{1}{2} \sum^{n}_{i=1} \| q_{i}-Rq_{i}' \|^{2}$$
- 根据第二步的$R$计算$t$：$$t^{*}=p-Rp'$$
步骤解释完了，发现只要算出了$R$，$t$是非常好求的，所以下面把精力放到计算$R$上面来，把关于$R$的误差项进行展开，得到：$$\frac{1}{2} \sum^{n}_{i=1} \| q_{i}-Rq_{i}' \|^{2}=\frac{1}{2} \sum^{n}_{i=1}(q_{i}^{T}q_{i}+q_{i}'^{T}R^{T}Rq_{i}'-2q_{i}^{T}Rq_{i}')$$
发先第一项与$R$无关，且$R^{T}R=I$，所以第二项也与$R$无关。因此，优化目标函数变为：$$\sum^{n}_{i=1}-q_{i}^{T}Rq_{i}'= \sum^{n}_{i=1}- tr(q_{i}^{T}Rq_{i}')= -tr\left( R \sum^{n}_{i=1} q_{i}' q_{i}^{T} \right)$$
至于证明通过SVD解出的$R$是最优的，这里略过，这里直接应用方法，先定义矩阵：$$W=\sum^{n}_{i=1}q_{i}q_{i}^{T}$$
- 根据第二步的$R$计算$t$：$$t^{*}=p-Rp'$$
其中，$\Sigma$是奇异值组成的对角矩阵，对角线元素从大到小排列，而$U$和$V$为对角矩阵，当$W$满秩时，$R$为：$$R=UV^{T}$$
如果此时$R$的行列式为负，则取$-R$作为最优值。这样就得到了$R$的最优解，然后带入刚才得到的第二项就可以得到$t$了
类似我了，猜这里不会有人看到，不，应该不会有人看，要是有人看到这里，直接v你50，疯狂星期四
其实也可能会被发现，因为在特征点中，这个位置应该叫做边缘，而不是最难匹配的区块
非线性优化方法
到了非线性优化，一般就是要使用李代数了，现在使用李代数表达位姿，刚才的最小化的式子是：$$\underset{R,t}{min} \frac{1}{2} \sum^{n}_{i=1} \| e_{i} \|^{2}_{2}=\underset{R,t}{min} \frac{1}{2} \sum^{n}_{i=1} \| p_{i}-(Rp_{i}^{'}+t) \|^{2}_{2}= \underset{R,t}{min} \frac{1}{2} \sum^{n}_{i=1} \| p_{i}-Tp_{i}' \|^{2}_{2}$$
把变换矩阵变改为李代数的表示，从李代数到李群是指数映射，式子就变为了：$$\underset{R,t}{min} \frac{1}{2} \sum^{n}_{i=1} \| p_{i}-\exp(\xi^{\wedge}) p_{i}' \|^{2}_{2}$$
下面的问题就是求导，这里使用左扰动模型，之前的结论是：$$\frac{\partial TP}{\partial \delta \xi}=(TP)^{\odot}=\left[ \begin{matrix}
I & -P^{' \wedge} \\ 0^{T} & 0^{T}
\end{matrix} \right]$$
这里的$e$是$p_{i}-Tp_{i}'$，所以求导后有一个负号，结果就是：$$\frac{\partial e}{\partial \delta \xi }=-(\exp(\xi ^{\wedge})p_{i}')^{\odot}$$
这里得到的导数就是高斯牛顿里面的雅可比矩阵$J$：$$J=\frac{\partial e}{\partial \delta \xi }$$
于是，在非线性优化中只需要不断迭代，就能找到极小值。而且，可以证明，ICP问题存在唯一解或无穷多解的情况。在唯一解的情况下，只要能找到极小值解，这个极小值就是全局最优值——因此不会遇到局部最小极小而非全局最小的情况。这也意味着ICP求解可以任意选定初始值，这是已匹配点时求解ICP的一大好处。
这里的ICP是指已由图像特征给定了匹配的情况下进行位姿估计的问题。在匹配已知的情况下，这个最小二成问题实际上具有解析解，所以没有必要进行迭代优化。
在某些场合下，例如在RGB-D SLAM中，一个像素的深度数据可能有，也可能测量不到，所以可以混合着使用PnP和ICP优化：对于深度已知的特征点，建模它们的3D-3D误差；对于深度未知的特征点，则建模3D-2D的重投影误差。于是，可以将所有的误差放在同一个问题中考虑，使得求解更加方便。
直接法
2D光流
光流法还不是那么的直接，它保留特征点，但是只保留关键点，不计算描述子
光流是一种描述像素随时间在图像之间运动的方法，随着时间流逝，同一个像素会在图像中运动，而我们希望追踪它的运动过程。其中，计算部分像素运动的称为稀疏光流，计算所有像素的称为稠密光流。稀疏光流以Lucas-Kanade光流为代表，并可以在SLAM中用于追踪特征点的位置。稠密光流以Hom-Schunck光流为代表。
下面详细介绍稀疏光流Lucas-Kanade，也称LK光流
Lucas-Kanade光流
在LK光流中，认为来自相机的图像是随时间变化的。图像可以看作时间的导数：$I(t)$
那么，在一个$t$时刻，位于$(x,y)$处的像素，它的灰度可以写成：$$I(x,y,t)$$
这种方式把图像看成了关于位置与时间的函数，值域就是图像中像素的灰度。
首先引入光流法的基本假设：
- 灰度不变假设：同一个空间点的灰度像素值，在各个图像中是固定不变的
对于$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$轴上的运动速度，而$\frac{dy}{dt}$是像素在$y$轴上的运动速度，把它们记为$u,v$。
同时，$\frac{\partial I}{\partial x}$为图像在该点处$x$方向的梯度，$\frac{\partial I}{\partial y}$为图像在该点处$y$方向的梯度，记为$I_{x},I_{y}$。把图像灰度对时间的变化量记为$I_{t}$，写成矩阵形式，有：$$\left[ \begin{matrix}
I_{x} & I_{y}
\end{matrix} \right] \left[ \begin{matrix}
u \\ v
\end{matrix} \right]=-I_{t}$$
我们的目标是计算像素的运动$u,v$，仅凭这一个方程是不够的，所以需要引入额外的约束来计算$u,v$。在LK光流中，假设某一个窗口内的像素具有相同的运动。
考虑一个大小为$w \times w$的窗口，它含有$w^{2}$数量的像素。该窗口内像素具有同样的运动，一次得到$w^2$个方程：$$\left[ \begin{matrix}
I_{x} & I_{y}
\end{matrix} \right]_{k} \left[ \begin{matrix}
u \\ v
\end{matrix} \right]=-I_{tk}, \;\;\;\;\;k=1,\cdots,w^2$$
取：$$A=\left[ \begin{matrix}
[I_{x},I_{y}]_{1} \\ \vdots \\ [I_{x}, I_{y}]_{k}
\end{matrix} \right],\;\; b=\left[ \begin{matrix}
I_{t1} \\ \vdots \\ I_{tk}
\end{matrix} \right].$$
整个方程变为：$$A \left[ \begin{matrix}
u \\ v
\end{matrix} \right]=-b$$
这是一个关于$u,v$的超定线性方程，传统解法是求最小二乘解，书上这里直接给了结论，考虑到我没有学过研究生阶段的矩阵论，所以这里简单推导一下这个解到底是怎么来的。
先把要求的写为$x$，整个式子就是：$$Ax=-b$$
想要找到最小二乘解，其实就是：$$\underset{x}{min} \; \|Ax+b\|^{2}_{2}$$
方法其实就是求导，记：$$E=(Ax+b)^{T}(Ax+b)$$
展开：$$E=x^TA^TAx+x^TA^Tb+b^TAx+b^Tb$$
然后对$x^T$求导（根据在非线性优化那章补充的矩阵求导公式）：$$\frac{\partial E}{\partial x^T}=2(x^TA^TA+b^TA)$$
令导数等于0，找到极值即为答案，这是因为如果对$E$求二阶导（这里先对列向量$x$求导了）：$$H=\frac{\partial ^2 E}{\partial x \partial x^T}=2A^TA$$
这个矩阵$A^TA$的性质是：
- 是对称半正定
- 如果A是列满秩，那么$A^TA$是正定的
在光流法中， A 是由图像梯度$I_{x},I_{y}$构成的矩阵。只要图像区域有足够纹理（即$I_{X},I_{y}$ 不全为零，且不共线），$A^TA$就是正定的。
如果$H$正定，那么这个函数是严格的凸函数，只有一个极小值点，所以求导得0就是要找的$x$
再回到一阶导数：$$2(X^TA^TA+b^TA)=0$$
得到：$$x^{*}=\left[ \begin{matrix}
u \\ v
\end{matrix} \right]^{*}=-(A^TA)^{-1}A^{T}b$$
这样就得到了像素在图像间的运动速度$u,v$。
当$t$取离散的时刻而不是连续时间时，我们可以估计某块像素在若干个图像中出现的位置。由于像素梯度仅在局部有效，所以如果一次迭代不够好，我们会多迭代几次这个方程。在SLAM中，LK光流常被用来跟踪角点的运动。
多层光流
刚才提到，光流需要相机的运动比较小，两张图像的差异不能太大，但是如果就是很大，单层图像光流容易达到一个局部极小值，但是还有补救的措施，就是构建图像金字塔，之前已经提过，就是对图像进行缩放，原始图像作为底层，每往上一层，就对下层图像进行一定倍率的缩放。
这个过程也可以叫做由粗至精(Coarse-to-fine)，更加形象
效果就是，当原始图像的像素运动较大时，在金字塔顶端的图像看来，运动依然在这一个很小的范围内。
直接法
直接开始推导直接法。
首先看初始条件：
这里的内容比较简单，直接贴在这里
在直接法中，没有特征匹配这个东西，无从知道哪一个$p_{2}$与$p_{1}$对应着同一个点。
直接法的思路是根据当前相机的位姿估计值寻找$p_{2}$的位置。但若相机的位姿不够好，$p_{2}$的外观和$p_{1}$有明显差别。为了减小这个差别，我们优化相机的位姿(暂且不管初始值是怎么来的)，来寻找与$p_{1}$更相似的$p_{2}$。
这同样可以通过解一个优化问题完成，但是此时优化的不是重投影误差，而是光度误差，也就是$P$的两个像素的亮度误差：$$e=I_{1}(p_{1})-I_{2}(p_{2})$$
这里的$e$是一个标量。同样优化目标为误差二范数平方，暂时取不加权的形式，为：$$\underset{T}{min}\, J(T)=\| e \|^{2}$$
目前依然是依据灰度不变假设，假设一个空间点在各个视角下成像的灰度是不变的。
同样假设有$N$个空间点$P_{i}$，得到：$$\underset{T}{min}\, J(T)= \sum^{N}_{i=1} e_{i}^{T} e_{i} , \;\;\;\;\;\; e_{i}=I_{1}(p_{1,i})-I_{2}(p_{2,i}) $$
然后定义两个中间变量：$$q=TP,\;\;\;\;u=\frac{1}{Z_{2}}Kq$$
根据链式法则：$$\frac{\partial e}{\partial T}=\frac{\partial I_{2}}{\partial u} \frac{\partial u}{\partial q} \frac{\partial q}{\partial \delta \xi} \delta \xi$$
下面的推导之前类似的写过很多次了，下面不想写了，直接贴上来了。
相应的直接法也有多层直接法，和光流一样，也是构建一个金字塔。
为了内容的完整性，下面关于直接法的讨论和优缺点分析也贴到这里。
