《增强现实：原理、算法与应用》读书笔记（6）运动恢复结构（下）自定标、代表性SfM方法

《增强现实：原理、算法与应用》读书笔记（6）运动恢复结构（下）初始化、相机位姿估计、集束调整

自定标

自定标（Self-calibration）是SfM里一个非常重要的技术，通过利用图像上的二维信息自动求解出相机的内部参数，而不需要标定物。

一般带自定标的SfM求解流程是先通过两帧或三帧求解来初始化射影空间上的三维结构和相机运动参数，然后通过增量式的求解方式扩大求解的帧数和重建的三维点云，再选择合适的时机通过自定标技术将重建的结果转换到度量空间上。

绝对二次曲线是自定标理论中一个非常重要的概念，一般用对偶绝对二次曲面来表达，记为${\Omega }^{\ast }$。${\Omega }^{\ast }$在射影空间坐标下可以表达成一个$4\times 4$秩为3的对称半正定矩阵，在度量坐标框架下可以进一步简化成对角矩阵$diag(1,1,1,0)$，且对于相似变换具有不变性。通过自定标技术，可以求解出一个提升变换矩阵$U$将${\Omega }^{\ast }$变换成$diag(1,1,1,0)$，即$U{\Omega }^{\ast }{U}^{\top }=diag(1,1,1,0)$，从而将三维重建从射影空间提升到度量空间（即$P_M=P{U}^{-1}{P}^{\top },X_M=UX$）。

对偶绝对二次曲面在图像上的投影可以用公式$w^{\ast }=P{\Omega }^{\ast }{P}^{\top }$来描述，因为在度量坐标框架下，$w^{\ast }$只与相机内参有关，而与相机运动无关（即$w^{\ast }=K{K}^{\top }$），所以我们可以进一步得到如下公式：
$$w^{\ast }\sim K_k{K}_k^{\top }\sim P_k{\Omega }^{\ast }{P}_k^{\top }$$

$K_k$和$P_k$分别是第$k$帧的内参矩阵和投影矩阵，为了求解稳定，投影矩阵$P_k$一般要先经过如下归一化处理（假设图像的长和宽分别为$w$和$h$）：
$$P_k^N=(K^N{)}^{-1}{P}_k,K^N=\left[\begin{array}{ccc}w+h& 0& w/2\\ 0& w+h& h/2\\ 0& 0& 1\end{array}\right]$$

根据这个约束方程，在多帧情况下，可以求解出${\Omega }^{\ast }$和各帧对应的内参矩阵$K$，从而实现相机的自定标，并进一步求出提升变换矩阵$U$，完成射影重建到度量重建的提升。

内参矩阵$K$有五个变量，为了求解的稳定性，一般需要对其做一些合理的约束限制，比如限定相机主点在图像中心，纵横比为1，倾斜率为0等。如果主点和纵横比已知且倾斜角为0，根据Pollefeys等提出的线性自定标方法，等式$w^{\ast }\sim K_k{K}_k^{\top }\sim P_k{\Omega }^{\ast }{P}_k^{\top }$可改写成如下形式：
$${\lambda }_k\left[\begin{array}{ccc}{f}_k^2& 0& 0\\ 0& f_k^2& 0\\ 0& 0& 1\end{array}\right]=P_k\left[\begin{array}{cccc}{f}_1^2& 0& 0& a_1\\ 0& f_1^2& 0& a_2\\ 0& 0& 1& a_3\\ a_1& a_2& a_3& {\Vert a\Vert }^2\end{array}\right]P_k^{\top }$$

根据上述归一化后，焦距一般会比较接近1,。根据文献（Pollefeys et al., 2004），通过对焦距等参数的波动范围做一定合理的假设，可以得到如下等式：
$$\frac{1}{9\nu }(P_k[1]{\Omega }^{\ast }{P}_k[1{]}^{\top }-P_k[3]{\Omega }^{\ast }{P}_k[3{]}^{\top })=0$$$$\frac{1}{9\nu }(P_k[2]{\Omega }^{\ast }{P}_k[2{]}^{\top }-P_k[3]{\Omega }^{\ast }{P}_k[3{]}^{\top })=0$$$$\frac{1}{0.2\nu }(P_k[1]{\Omega }^{\ast }{P}_k[1{]}^{\top }-P_k[2]{\Omega }^{\ast }{P}_k[2{]}^{\top })=0$$$$\frac{1}{0.1\nu }(P_k[1]{\Omega }^{\ast }{P}_k[3{]}^{\top })=0$$$$\frac{1}{0.1\nu }(P_k[2]{\Omega }^{\ast }{P}_k[3{]}^{\top })=0$$$$\frac{1}{0.01\nu }(P_k[1]{\Omega }^{\ast }{P}_k[2{]}^{\top })=0$$

这里$P_k[i]$表示$P_k$的第$i$行向量，$\nu$是一个缩放因子，初始值设为1，迭代的时候设为$P_k[3]{\tilde{\Omega }}^{\ast }{P}_k[3{]}^{\top }$（${\tilde{\Omega }}^{\ast }$表示上一次迭代获得的${\Omega }^{\ast }$的值）。每次迭代，可以采用最小二乘法求解上述方程组，一般只需要迭代两次。

但是，实验中发现上述线性自定标方法并不稳定，容易出现求解的f_k^2很小，甚至是负的情况。其原因在于方程组前三个式子对$f_k^2$的约束不对称，导致$P_k[1]{\tilde{\Omega }}^{\ast }{P}_k[1{]}^{\top }/P_k[3]{\tilde{\Omega }}^{\ast }{P}_k[3{]}^{\top }$和$P_k[2]{\tilde{\Omega }}^{\ast }{P}_k[2{]}^{\top }/P_k[3]{\tilde{\Omega }}^{\ast }{P}_k[3{]}^{\top }$容易从1.0偏向零，甚至是负的。

为此，我们对上述线性方程组进行改进，构建了如下非线性目标函数（Zhang et al., 2007）：
$$E_{calib}=\frac{1}{N-1}\sum _{k=2}^N{E}_k$$$$E_k=E_{k_1}+E_{k_2}$$$$E_{k_1}={\left(\frac{1}{9}\right)}^2\left({\left(\frac{P_k[1]{\tilde{\Omega }}^{\ast }{P}_k[1{]}^{\top }}{P_k[3]{\tilde{\Omega }}^{\ast }{P}_k[3{]}^{\top }}-1\right)}^2+{\left(\frac{P_k[3]{\tilde{\Omega }}^{\ast }{P}_k[3{]}^{\top }}{P_k[1]{\tilde{\Omega }}^{\ast }{P}_k[1{]}^{\top }}-1\right)}^2\right)+{\left(\frac{1}{9}\right)}^2\left({\left(\frac{P_k[2]{\tilde{\Omega }}^{\ast }{P}_k[2{]}^{\top }}{P_k[3]{\tilde{\Omega }}^{\ast }{P}_k[3{]}^{\top }}-1\right)}^2+{\left(\frac{P_k[3]{\tilde{\Omega }}^{\ast }{P}_k[3{]}^{\top }}{P_k[2]{\tilde{\Omega }}^{\ast }{P}_k[2{]}^{\top }}-1\right)}^2\right)+{\left(\frac{1}{0.2}\right)}^2\left({\left(\frac{P_k[1]{\tilde{\Omega }}^{\ast }{P}_k[1{]}^{\top }}{P_k[2]{\tilde{\Omega }}^{\ast }{P}_k[2{]}^{\top }}-1\right)}^2+{\left(\frac{P_k[2]{\tilde{\Omega }}^{\ast }{P}_k[2{]}^{\top }}{P_k[1]{\tilde{\Omega }}^{\ast }{P}_k[1{]}^{\top }}-1\right)}^2\right)$$$$E_{k_2}={\left(\frac{1}{0.1}\right)}^2{\left(\frac{P_k[1]{\tilde{\Omega }}^{\ast }{P}_k[3{]}^{\top }}{P_k[3]{\tilde{\Omega }}^{\ast }{P}_k[3{]}^{\top }}\right)}^2+{\left(\frac{1}{0.1}\right)}^2{\left(\frac{P_k[2]{\tilde{\Omega }}^{\ast }{P}_k[3{]}^{\top }}{P_k[3]{\tilde{\Omega }}^{\ast }{P}_k[3{]}^{\top }}\right)}^2+{\left(\frac{1}{0.01}\right)}^2{\left(\frac{P_k[1]{\tilde{\Omega }}^{\ast }{P}_k[2{]}^{\top }}{P_k[3]{\tilde{\Omega }}^{\ast }{P}_k[3{]}^{\top }}\right)}^2$$

求解上述非线性目标函数$E_{calib}$需要有初值，可以用先行算法先求解出$f_k$作为初值，然后代入到目标函数$E_{calib}$中进行求解。该方法能有效提高自定标的稳定性（尤其对于焦距变换情况）。

初始帧选择

准确的自定标要求足够多的特征点匹配，而且结构和运动不能濒临退化，通常可以采用Pollefeys等（2004）提出的基于图像的距离（image-based distance，IBD）来判断场景的结构是否退化成一个平面：
$$b=median(d(\pi (H\hat{x}),x^{\prime }))$$
这里，$H$是一个$3\times 3$的满秩单应性矩阵。$d(\pi (H\hat{x}),x^{\prime })$表示$x$经过$H$变换之后与$x^{\prime }$在图像上的欧氏距离。$H$可以通过最小化$b$求解得到。

在焦距已知或固定不变的情况下，只根据特征点的匹配数目和基于图像的距离来选择初始两帧一般已经足够了，这也是多数SfM系统采用的。但是在焦距变化的情况下，仅采用这两个标准选出的初始帧进行自定标还不是很可靠，需要考虑更多的因素。

（1）焦距变化度：

相机焦距变化较大的情况下进行求解很容易遇到稳定性问题。主要原因如下：
【1】镜头缩放很容易跟相机的前后运动发生混淆，尤其是当场景三维结构退化成一个平面时；
【2】镜头缩放很容易带来其他问题，比如匹配噪声偏大、运动模糊等，使得特征跟踪更加困难。

实际求解中，很容易遇到重投影误差较小，但是恢复的运动和结构误差很大，焦距变化小的情况求解稳定。

（2）自定标质量：

因为是先进性射影重建，再进行自定标技术的度量重建，所以射影重建的质量很重要。实际过程中很容出现，尽管重投影误差很小，但是求解的投影矩阵依然是病态的，从而导致自定标失败。

可以根据以下几个标准来判断某段子序列是否适合度量重建：
【1】特征点匹配足够多；
【2】结构和运动非退化；
【3】焦距变化度比较小；
【4】自定标质量比较好。

一般来说，特征点匹配数目越多，求解越稳定。但是这个数目阈值比较难设定。根据自定标理论，三帧求解比两帧求解稳定，因此可以采用三帧作为SfM求解的基本单元。

完成特征匹配后，一般需要抽取关键帧进行求解，提高求解的稳定性和效率性。很短的特征轨迹对求解帮助不大，只选取跟踪时间较长的轨迹参与求解，例如特征轨迹长度长于$N$，称为“黄金跟踪轨迹”。

关键帧间隔越大，一般基线越长，对求解越有利，但间隔太远可能没有足够数目的对应点。一个简单的策略是在以$(N-1)/2$间隔在视频序列上选取关键帧，这样可以保证任意一个黄金跟踪轨迹，至少会存在于两个关键帧上，即黄金跟踪轨迹总可以参与求解。

另外为了鲁棒地求解，任意三个连续帧的关键帧之间必须有足够的公共黄金跟踪轨迹（例如设置为不少于30个）。如果黄金跟踪轨迹数目不足，那么需要把间隔的阈值临时调低来选择关键帧。

---

基于自定标的单序列增量式SfM求解框架

1、自动抽取特征点并匹配；
2、抽取关键帧组成关键帧序列；
3、初始化度量空间下的三维空间：
——3.1、选择合适的三帧组进行射影重建的初始化；
——3.2、采用增量式求解，并选择合适时机进行自定标，将射影重建转换到度量重建；
4、对于每一个新加入求解的关键帧：
——4.1、初始化新求解帧的相机参数和相关的三维点；
——4.2、用局部集束调整算法对局部已经求解的结构和运动进行求精；
5、求解所有非关键帧的相机参数；
6、对整个序列恢复的结构和运动用集束调整进行最后优化。

---

自定标的合适时机

对于自定标来说，在投影矩阵比较准确的情况下，方程数越多，一般求解越稳定。但是估计的投影矩阵难免会有误差累积，特别是对于长序列来说，投影矩阵很容易沿着序列逐渐偏离真实值，导致自定标失败。

因此，需要对误差累积进行监控，以选择一个合适的时机（在误差累积过大之前）进行自定标，及时将射影重建提升到度量重建。

我们将参考三帧组中的特征轨迹称为参考轨迹，其对应的三维点称为参考三维点。因为参考三帧组比较适合初始化，所以其射影重建的结果一般也比较准确。

每新加入一帧进行求解时，都需要检查一下是否至少含有$n_p$个参考三维点在该帧上有对应的二维特征点，并且这些参考三维点在该帧上的平均投影误差小于$e_p$。一般可以取$n_p=15，e_p=3.0$。

如果没有新的关键帧满足这个条件，便停止射影重建，然后用自定标方法进行射影重建到度量重建的初始化。

完成度量重建的初始化后，开始进入算法“基于自定标的单序列增量式SfM求解框架”的步骤4，即后续的求解都在度量空间上进行。每当新增加一帧（假设第$i$帧）进行求解时，先求解出该帧的投影矩阵$P_i$。然后对$P_i=K_i[R_i|t_i]$进行RQ分解，并立即对分解的$K_i,R_i,t_i$最小化投影误差来进一步优化：
$$\underset{argmin}{K_i,R_i,t_i}\sum _j{\Vert \pi (K_i{R}_i{X}_j+K_i{t}_i)-x_{ij}\Vert }^2$$

然后，使用集束调整来优化所有已经求解的相机参数和三维点。所有关键帧通过这种增量式的方式求解完毕后，再用恢复的这些三维点求解其他非关键帧的相机参数。最后，可以将整个序列的相机参数和三维点用集束调整进行最后的优化。

代表性SfM方法

增量式SfM

增量式SfM采用逐张图片加入处理的方法，也是目前最广泛使用的方法。

Bundler（Snavely et al., 2006）是增量式SfM方法的一个代表，它先使用特征匹配和RANSAC方法计算图像对之间的基本矩阵。根据图像的匹配结果，将多幅图像中的对应点合并成轨迹，然后将图像逐一加入SfM的集束调整中进行优化。

良好的初始化对增量式SfM非常重要，为了能使初始三维点提供最够的冗余，Bundler选择初始化图像对时需要同时满足有足够的特征匹配以及相机基线足够大这两个条件。由于Bundler每增加一帧或若干帧就进行集束调整。但是这种算法处理大数据集时速度很慢。

ACTS（Zhang et al., 2007）是我们开发的基于视频序列的相机自动跟踪系统，也是采用类似的增量式求解策略，但初始化方法不一样，而且能够在焦距未知的情况下进行相机的自定标。

VisualSFM（Wu, 2013）在Bundler架构的基础上加入了局部集束调整的处理，对于新加的图像，只在其临近的图像子集上进行集束调整。通过这种方法，显著提高了SfM系统的效率。但是，由于不断在已有结果上加入新的图像进行求解优化，这使得增量式SfM系统的积累误差不断增大。此外，由于相机的位姿估计不够精确，使得一些原本正确的特征匹配无法进行三角化，这些匹配的丢失同样带来了误差的累积。

因此，VisualSFM提出了在集束调整之后重建三角化的方法，以增加更多的三维点，从而减少误差累积。

Bundler和VisualSFM等工作中，直接采用图像间鲁邦估计基础矩阵的方法来获得特征匹配和相对位姿。但基础矩阵约束的几何意义不够强，对于一些退化场景和运动不能很好地处理。

COLMAP提出了将基础矩阵估计和单应性矩阵估计结合起来，区分相机纯旋转或者纯平面场景的方法，可以有效地解决纯旋转情形不足以提供有效基线的问题。此外，COLMAP中对于特征点匹配在新图像中的分布也进行了计算，并优先选择在图像中特征点匹配覆盖更均匀的图像加入SfM。

层次式SfM

增量式SfM有实现简单、求解鲁棒等优点，但因为频繁地调用集束调整，效率不是很高，尤其图像数目很多的时候。

层次式SfM首先对部分图像匹配形成的子问题进行求解，然后进行补充或合并来得到完整的重建，可以显著提高求解的效率。

Snavely等（2008b）提出从全体图像中寻找一个骨架子集以充分覆盖所有图像所拍摄的场景，然后先对这个子集进行重建，再恢复剩余图像的位姿。这种方法跟针对视频序列上的基于关键帧的SfM方法类似，可以显著提高增量重建的效率。

Agarwal等（2011）在这个工作的基础上进一步采用了分布式匹配，利用集群可以在一天内快速完成城市规模场景的重建。

Farenzena等（2009）提出利用层次式聚类的方法，从图像匹配中生成树谱图。图上所有的叶子节点对应了一幅图像，而节点的汇聚对应了图像间的2D-2D立体匹配、图像和已重建点云的2D-3D匹配以及两个重建好点云的3D-3D匹配。

全局式SfM和混合式SfM

增量或层次式SfM都需要不断增长地进行重建，在增长中不断重复进行集束调整，因此带来很大的计算开销。一些全局方法尝试一次性求解全体图像的外参并通过极少的集束调整完成优化。

由于三维旋转的非线性特点，全局式SfM方法通常将旋转、平移等参数分开处理。首先通过旋转平均方法，全局的求解所有图像的旋转，再注册全局平移，最后整体进行集束调整的方法进行SfM求解。其中，全局旋转和全局平移则是采用了先局部再合并的方法——先求解有匹配点的图像对之间的相对旋转或平移，然后通过相应的方法进行全局对齐。

在Cui等（2017）的方法中，作者利用全局式方法估计所有图像的旋转，然后回归增量式方法求解图像的位置。通过这种全局和增量方法的结合，混合式方法大幅减少了SfM重建需要的时间，同时错误的匹配关系可以及时得到修正，从而保证了重建的完整性。

语义SfM系统

相比基于底层特征匹配（SIFT、ORB等特征）的SfM系统，语义SfM系统将场景和物体识别得到的语义标签信息融合到系统的优化体系中，一方面提高了SfM系统对三维点云和相机位姿的估计准确度，另一方面也提高了物体的识别准确度。例如，Bao等（2011）提出了一种新的优化框架，将物体的语义标签添加到系统的约束中去，通过对物体三维位置以及方位的建模，以最大似然估计的方式，联合优化相机位姿、三维点云以及三维物体的属性。