多视图几何总结——三角形法

在《视觉SLAM十四讲》中三角测量那一节中简单介绍了下如何通过两帧中匹配的点获得空间点深度，这对单目相机的成像是非常重要的，其证明如下，设x1x_1x1,x2x_2x2分别为两帧中匹配好的特征点的归一化坐标，然后满足：s1x1=s2Rx2+ts_{1} \boldsymbol{x}_{1}=s_{2} \boldsymbol{R} \boldsymbol{x}_{2}+\boldsymbol{t} s1x1=s2Rx2+t我们已经知道变换矩阵RRR和ttt，然后上面方程左乘一个x1∧x_{1}^{\wedge}x1∧就可以求得s2s_2s2，如下：s1x1∧x1=0=s2x1∧Rx2+x1∧ts_{1} \boldsymbol{x}_{1}^{\wedge} \boldsymbol{x}_{1}=0=s_{2} \boldsymbol{x}_{1}^{\wedge} \boldsymbol{R} \boldsymbol{x}_{2}+\boldsymbol{x}_{1}^{\wedge} \boldsymbol{t} s1x1∧x1=0=s2x1∧Rx2+x1∧t很简单的，文中也提到由于存在噪声，上式不一定为零，需要用最小二乘法进一步求解，怎么求呢？如下

线性三角形法

在多视图几何中对问题的描述稍稍有点不一样，文中采用摄像机矩阵描述问题，摄像机矩阵指的是内参和外参合成的矩阵P=KR[I∣−C]\mathrm{P}=\mathrm{KR}[\mathrm{I} |-{\mathrm{C}}] P=KR[I∣−C]对于图像中的点通过摄像机矩阵应该满足：x=PXx′=P′X{\mathbf{x}}=\mathrm{P}{\mathbf{X}} \quad {\mathbf{x}}^{\prime}=\mathrm{P'} {\mathbf{X}} x=PXx′=P′X而实际上应为噪声的存在，他们并不满足基本的对极几何约束，如下图所示
我们通过叉乘构造基本方程，对第一幅图像有x×(PX)=0\mathbf{x} \times(\mathrm{PX})=0x×(PX)=0，展开得x(p3⊤X)−(p1⊤X)=0y(p3⊤X)−(p2⊤X)=0x(p2⊤X)−y(p1⊤X)=0\begin{aligned} x\left(\mathbf{p}^{3 \top} \mathbf{X}\right)-\left(\mathbf{p}^{1 \top} \mathbf{X}\right) &=0 \\ y\left(\mathbf{p}^{3 \top} \mathbf{X}\right)-\left(\mathbf{p}^{2 \top} \mathbf{X}\right) &=0 \\ x\left(\mathbf{p}^{2 \top} \mathbf{X}\right)-y\left(\mathbf{p}^{1 \top} \mathbf{X}\right) &=0\end{aligned} x(p3⊤X)−(p1⊤X)y(p3⊤X)−(p2⊤X)x(p2⊤X)−y(p1⊤X)=0=0=0其中p1⊤\mathbf{p}^{1 \top}p1⊤为摄像机矩阵的第一行，为4维向量，X\mathbf{X}X是空间点齐次坐标，为4维向量，是我们的未知量，上述三个方程中有两个是线性独立的，因此我们将其中两维拿出来与另外一幅图像组成一个AX=0\mathbf{A} \mathbf{X}=\mathbf{0}AX=0的方程组如下：A=[xp3⊤−p1⊤yp3⊤−p2⊤x′p′3⊤−p1⊤y′p′3⊤−p′2⊤]\mathrm{A}=\left[ \begin{array}{c}{x \mathbf{p}^{3 \top}-\mathbf{p}^{1 \top}} \\ {y \mathbf{p}^{3 \top}-\mathbf{p}^{2 \top}} \\ {x^{\prime} \mathbf{p}^{\prime 3 \top}-\mathbf{p}^{1 \top}} \\ {y^{\prime} \mathbf{p}^{\prime 3 \top}-\mathbf{p}^{\prime 2 \top}}\end{array}\right] A=⎣⎢⎢⎡xp3⊤−p1⊤yp3⊤−p2⊤x′p′3⊤−p1⊤y′p′3⊤−p′2⊤⎦⎥⎥⎤在有噪声的情况下（没噪声就没什么好讲的了）求解的方法和我们在多视图几何总结——基础矩阵、本质矩阵和单应矩阵的求解过程吗、介绍的方法一致了，ORB SLAM2里面三角化的过程如下

void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
{cv::Mat A(4,4,CV_32F);A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);cv::Mat u,w,vt;cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);x3D = vt.row(3).t();x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}

方法和代码是对应一致的

（1）齐次方法

上述方程组未知量是四维向量但自由度一共只有三个（齐次坐标），而系数矩阵是四维的矩阵，因此是一个冗余方程组，如果将四维向量的约束条件设为∥X∥=1\|\mathbf{X}\|=1∥X∥=1，这可以将这个视为AX=0\mathbf{A X}=\mathbf{0}AX=0的齐次方程进行求解，解法是求是在约束∣∣X∣∣=1||\mathbf{X}||=1∣∣X∣∣=1的最小化范数∣∣AX∣∣||\mathbf{A}\mathbf{X}||∣∣AX∣∣，即求∣∣AX∣∣/∣∣X∣∣||\mathbf{A}\mathbf{X}||/||\mathbf{X}||∣∣AX∣∣/∣∣X∣∣的最小值问题，该问题解为ATA\mathbf{A}^T\mathbf{A}ATA的最小特征值的特征矢量，也就是A\mathbf{A}A的最小奇异值的奇异矢量

（2）非齐次方法

和上面不同的是，如果将四维向量的约束条件设为最后一个齐次值为1的话，可以将上述方程构造成AX=b\mathbf{A X}=\mathbf{b}AX=b的非齐次方程，那这个求解的方法就是最小二乘法了，没什么好说的

上面两种解法中，第一种方法是更好的，结论和求解单应矩阵是相同的，因为第二种方法最后一维实际很接近零的话（点在无穷远处），那么求解的结果就会出现问题

几何法

这里介绍的所谓几何法其实类似于重投影误差，如下图所示：
即寻求满足对极几何约束的点x^\hat{\mathbf{x}}x^和点x^′\hat{\mathbf{x}}^{\prime}x^′使得重投影误差最小：
C(x,x′)=d(x,x^)2+d(x′,x^′)2\mathcal{C}\left(\mathbf{x}, \mathbf{x}^{\prime}\right)=d(\mathbf{x}, \hat{\mathbf{x}})^{2}+d\left(\mathbf{x}^{\prime}, \hat{\mathbf{x}}^{\prime}\right)^{2}C(x,x′)=d(x,x^)2+d(x′,x^′)2其解法也有如下两种：

（1）非线性优化法

诸如高斯牛顿法、列温伯格法之列的，常规的非线性优化操作，不在此赘述

（2）最优解法

如下图所示：

我们可以将点到点的距离转化为点到直线的距离：d(x,l)2+d(x′,l′)2d(\mathbf{x}, \mathbf{l})^{2}+d\left(\mathbf{x}^{\prime}, \mathbf{l}^{\prime}\right)^{2} d(x,l)2+d(x′,l′)2具体步骤如下：
（1）用参数ttt在第一幅图像中初始化一个对极线l(t)\mathbf{l}(t)l(t)
（2）利用基本矩阵FFF（已知），计算第二幅图像上对应的对极线l′(t)\mathbf{l}^{\prime}(t)l′(t)
（3）把距离函数d(x,l(t))2+d(x′,l′(t))2d(\mathbf{x}, \mathbf{l}(t))^{2}+d\left(\mathbf{x}^{\prime}, \mathbf{l}^{\prime}(t)\right)^{2}d(x,l(t))2+d(x′,l′(t))2表示为ttt的函数
（4）求最小化这个函数的ttt值
其中，第二步的操作如下：
书中结论8.5：假设l\mathbf{l}l和l′\mathbf{l}^{\prime}l′是对应的对极线，且kkk是不过对极点eee的任何直线，则l\mathbf{l}l和l′\mathbf{l}^{\prime}l′间的关系是l′=F[k]×l\mathbf{l}^{\prime}=F[k]_×\mathbf{l}l′=F[k]×l
这个方法和第一种方法不同的是，第一种方法是对点求导，涉及到三个变量，这种方法只有一个变量ttt，最后构造的一个6次多项式，通过求导的方式可以获得其最小值。求得ttt之后再求得极线上对应的两个图像坐标点，再通过三角法恢复空间点坐标就完成了。

误差分析

线性三角形法和几何法比较的话，几何法获得误差会相对更小，但是在ORB SLAM2里面作者是直接三用线性三角法的，几何法的计算量摆在这儿呢，另外，如下图所示：

纯旋转情况下是无法进行三角化的（因为不满足对极约束的要求），平移量越大误差会越小，但是平移量过大的话，物体匹配会变困难，这个叫三角测量矛盾

补充：深度滤波器

和三角形法相关的一个比较有意思的东西叫深度滤波器，SVO的深度估计就是通过深度滤波器实现的，《视觉SLAM十四讲》中也有总结，这里也顺带总结一下：
深度滤波器使用的背景是，在单目中如果想实现稠密或者半稠密的SLAM的话如果对每个点都进行三角法估计深度的话是不现实的，因为不可能对图像中每个点都进行匹配，于是就诞生了基于极线搜索和块匹配技术的深度滤波器，如下图：
解释起来很简单，就是当p1p_1p1的深度不确定时，p2p_2p2就成了一个极线段而不是一个点，我们在p1p_1p1和p2p_2p2周围取一些像素小块进行匹配，这就是极线搜索和块匹配技术，块匹配的话一般是拿灰度值（SAD、SSD、NCC等，具体的可以查书，反正就是相似性的一种计算）进行匹配的，匹配完成后就可以进行深度滤波，这里假设p1p_1p1的深度ddd是满足高斯分布的P(d)=N(μ,σ2)P(d)=N\left(\mu, \sigma^{2}\right) P(d)=N(μ,σ2)假设我们匹配好的像素块的深度同样满足高斯分布P(dobs)=N(μobs,σobs2)P\left(d_{o b s}\right)=N\left(\mu_{o b s}, \sigma_{o b s}^{2}\right) P(dobs)=N(μobs,σobs2)这里的滤波就是通过高斯相乘，即μfuse=σobs2μ+σ2μobsσ2+σobs2,σfuse2=σ2σobs2σ2+σobs2\mu_{f u s e}=\frac{\sigma_{o b s}^{2} \mu+\sigma^{2} \mu_{o b s}}{\sigma^{2}+\sigma_{o b s}^{2}}, \quad \sigma_{f u s e}^{2}=\frac{\sigma^{2} \sigma_{o b s}^{2}}{\sigma^{2}+\sigma_{o b s}^{2}} μfuse=σ2+σobs2σobs2μ+σ2μobs,σfuse2=σ2+σobs2σ2σobs2规则知道了，那么现在的问题是我们匹配好的像素块的深度分布怎么计算呢？均值μobs\mu_{obs}μobs就是像素块中心确定的深度，方差σobs\sigma_{obs}σobs计算方法是计算相差一个像素距离的变化值，如下
这里的求解就是高中数学知识了，上图中这几个变量的关系是a=p−tα=arccos⁡⟨p,t⟩β=arccos⁡⟨a,−t⟩\begin{aligned} \boldsymbol{a} &=\boldsymbol{p}-\boldsymbol{t} \\ \alpha &=\arccos \langle\boldsymbol{p}, \boldsymbol{t}\rangle \\ \beta &=\arccos \langle\boldsymbol{a},-\boldsymbol{t}\rangle \end{aligned} aαβ=p−t=arccos⟨p,t⟩=arccos⟨a,−t⟩对p2p_2p2进行一个像素的扰动有δβ=arctan⁡1f\delta \beta=\arctan \frac{1}{f} δβ=arctanf1因此有β′=β+δβγ=π−α−β′\begin{array}{l}{\beta^{\prime}=\beta+\delta \beta} \\ {\gamma=\pi-\alpha-\beta^{\prime}}\end{array} β′=β+δβγ=π−α−β′由正弦定理可以求得p’\boldsymbol{p’}p’的大小有∥p′∥=∥t∥sin⁡β′sin⁡γ\left\|\boldsymbol{p}^{\prime}\right\|=\|\boldsymbol{t}\| \frac{\sin \beta^{\prime}}{\sin \gamma} ∥p′∥=∥t∥sinγsinβ′所以有σobs=∥p∥−∥p′∥\sigma_{o b s}=\|\boldsymbol{p}\|-\left\|\boldsymbol{p}^{\prime}\right\| σobs=∥p∥−∥p′∥上面过程通顺之后，我们就不断进行迭代，最后直到深度收敛即可，总结步骤如下

假设所有像素的深度满足某个初始的高斯分布；
当新数据产生时，通过极线搜索和块匹配确定投影点位置；
根据几何关系计算三角化后的深度以及不确定性；
将当前观测融合进上一次的估计中。若收敛则停止计算，否则返回 2

这里注意的是，稠密SLAM的话还是要对每一个像素值都进行深度滤波的，只是说通过深度滤波不需要对每个像素进行匹配，其计算量还是很大的。

这篇总结就到这里啦，欢迎交流～

此外，对SLAM算法感兴趣的同学可以看考我的博客SLAM算法总结——经典SLAM算法框架总结