slam特征点深度 svd_（七）ORBSLAM特征点的三角化
ORBSLAM2特征点三角化简介
插入关键帧以后，我们还需要插入新的地图点。为了确保新插入的地图点是足够鲁棒的，进行严格的检查是必要的。ORBSLAM2在插入地图点的时候也十分仔细，上一讲我们提到了地图的更新策略，唯独三角化没有细讲，倒不是因为它不重要而不提，而是因为三言两语说不清楚，所以才需要单独用一讲来说说这个方法。
接下来，笔者将会从一下两个方面来介绍本讲的内容：
1. 线性三角化方法；
2. 创建新地图点的方法提纲；
线性三角化方法
同之前一样，笔者会介绍创建新地图点所必要的技术细节，然后再讲方法提纲。
在ORBSLAM2源码中，创建新的地图点采用的是线性三角化方法，当然有些点并不是用这个方法创建的，但是这一小节我们不提，留到方法提纲那一节再介绍。
OK，我们就直接进入主题吧。
ORBSLAM2采用的是针孔模型的相机，而不是鱼眼或者事件相机之类的。因此这类相机的投影模型，大家应该都是比较熟悉的。笔者就不赘述了。
假设三维空间点为 $P_{w} = [ X, Y, Z, 1 ]^{T}$，参考帧的相机位姿为 $T_{rw} = [ R_{rw} | t_{rw} ]$，表示从世界坐标系变换到参考帧坐标系，同理，当前帧的相机位姿为 $T_{cw} = [ R_{cw} | t_{cw} ]$。点 $P_{w}$ 在参考帧归一化平面里的位置为 $p_{r} = [ x_{r}/z_{r}, y_{r}/z_{r}, 1 ]^{T}$，深度为 $z_{r}$，在当前帧归一化平面里的位置为 $p_{c} = [ x_{c}/z_{c}, y_{c}/z_{c}, 1 ]^{T}$，深度为 $z_{c}$。根据位置变换关系有：
\begin{equation}
z_{r}p_{r} = T_{rw}P_{w}
\end{equation}
\begin{equation}
z_{c}p_{c} = T_{cw}P_{w}
\end{equation}
对公式 $(1)$ 和公式 $(2)$ 分别叉乘对应的归一化平面的点，得到：
\begin{equation}
p_{r}\times z_{r}p_{r} = p_{r}\times T_{rw}P_{w} = 0
\end{equation}
\begin{equation}
p_{c}\times z_{c}p_{c} = p_{c}\times T_{cw}P_{w} = 0
\end{equation}
我们知道，一个三维向量 $t$，叉乘自己 $t\times t = 0$。于是整理上面的式子，可以得到：
\begin{equation}
\begin{bmatrix} p_{r}\times T_{rw} \\p_{c}\times T_{cw} \end{bmatrix} P_{w} = Ax = 0
\end{equation}
即通过构建一个齐次方程来求解空间点 $P_{w}$。其中，矩阵 $A \in R^{6\times 4}$， 因此它是一个超定方程，所以我们的目标函数是：
\begin{equation}
J(x) = \min\|Ax\|
\end{equation}
显然 $x = 0$ 是很自然的一个解，但我们想要的是非0解，因此我们假设 $x$ 满足 $\|x\| = 1$，于是我们用奇异值分解的方法来求解。
通过SVD分解，我们可以得到 $A = UDV^{T}$，其中，$U \in R^{6\times 6}$，$D \in R^{6\times 4}$，$V \in R^{4\times 4}$，则原方程可以写成：
\begin{equation}
J(x) = \min\|Ax\| = \min\|UDV^{T}x\| = \min\|DV^{T}x\|
\end{equation}
因为 U 和 V 都是正交矩阵，行列式为1。因此有 $\|V^{T}x\| = \|x\|$。
令 $y = V^{T}x$，则原问题进一步简化为：
\begin{equation}
J(y) = J(V^{T}x) = \min\|Dy\|
\end{equation}
为什么说进一步简化了呢？因为原始矩阵 A 是一个无序矩阵，到了公式 $(8)$ 的时候，矩阵 A 变成了对角阵 D。且由于对角阵 D 是矩阵 A 的奇异值从大到小降序排列而成，因此 $J(y)$ 的最小值在 D 矩阵奇异值最小的地方取到。由于 D 是 $6\times 4$ 维的矩阵， 则原代价函数最小值在 $y = [ 0, 0, 0, 1 ]^{T}$ 时取到。于是我们得到：
\begin{equation}
y = V^{T}x \Longrightarrow x = Vy
\end{equation}
于是，$x$ 的解就变成了正交矩阵 V 的最后一列的列向量。到此，我们就通过上面一系列方法，求出了空间点 $x = P_{w}$，但是由于它是一个模长为1的齐次向量，所以我们进行归一化，即：
\begin{equation}
\begin{bmatrix} x \\ y \\ z \\ w \end{bmatrix} \Longrightarrow \begin{bmatrix} x/w \\ y/w \\ z/w \\ w/w \end{bmatrix} = \begin{bmatrix} X \\ Y \\ Z \\1 \end{bmatrix}
\end{equation}
至此，我们的线性三角化方法就全部介绍完了。当然，如果感兴趣的话，大家也可以查阅相关的文档，另外，也可以看看奇异值分解是怎么用在非线性方程求解问题上的。
另外，值得注意的是，ORBSLAM2这里并不是利用两个点构建一个 $6\times 4$ 维的 A 矩阵，而是通过删除其中两行，来生成一个 $4\times 4$ 的 A 矩阵。笔者认为二者并无本质区别，不过可能在计算上更加方便。(笔者后面复现这方面代码时，会做一下对比，到时再回来补充。先立个Flag，Push自己去做！)
创建新地图点的方法提纲
笔者还是照往常一样，为大家罗列ORBSLAM2创建新地图点的方法提纲。
1. 创建新的地图点需要有足够视差的两个匹配点才能进行三角化，所以首先将当前帧共视图里所有关键帧全部包含进来；
2. 计算当前帧和共视图关键帧的基本矩阵；
3. 在共视图关键帧中，为当前帧所有特征点查找最好的匹配点，构建匹配关系；
Note：在匹配阶段，利用BOW向量进行筛选加速，计算描述子匹配分数，确定最优匹配，这里还要检查是否满足对极几何关系。
4. 计算匹配对的归一化坐标，并计算射线的夹角 $\theta_{n}$；
5. 计算当前帧和共视图关键帧左右两视图观测该地图点形成的射线的夹角 $\theta_{c}$ 和 $\theta_{g}$；
6. 比较三个夹角的大小，若步骤5中的任意一个夹角大于 $\theta_{n}$，则直接用该帧的深度信息创建新的地图点，否则使用上面介绍的线性三角化方法生成新的地图点；
7. 确认新的地图点位置是否在两个相机的前面，这里主要通过位姿变换确认深度值为正数即可；
8. 计算重投影误差，即将该地图点分别投影回两帧图像中，与投影点计算误差，确认其在阈值范围内；
9. 最后就是确认尺度信息，即确定匹配对的尺度误差不能太大；
10. 剩余的就是关联观测信息还有插入地图点，然后计算法向量，描述子这些东西了，前面已经介绍过的。
OK，提纲已经基本介绍完了。那么特征点的三角化这一讲就结束拉。
总结：
按照惯例，我们还是总结一下。
这一讲，我们主要介绍了ORBSLAM2中特征点的三角化，主要包括其线性三角化方法，其利用了反对称矩阵的性质以及SVD分解的方法来求解三维空间点。
此外，我们还罗列了ORBSLAM2中具体的创建新的地图点的主要提纲，把用到的步骤全部都精炼呈现给大家。有需要的小伙伴可以按需查代码。
下一讲，笔者将为大家介绍SLAM中一个非常重要的模块——回环检测。但是由于回环检测篇幅较大，所以分成两讲。下一讲，先为大家介绍回环检测里面的位置识别。
参考文献：
[1] 视觉SLAM十四讲
[5] 矩阵分析
PS：
如果您觉得我的博客对您有所帮助，欢迎关注我的博客。此外，欢迎转载我的文章，但请注明出处链接。