窥探PTAM之Mapping线程

5. 构建地图（Mapping）线程

地图构建系统主要是建立3D点云图的过程。地图的创建发生在两个不同的阶段:第一个阶段，绘制出初始的地图;第二个阶段，通过等待关键帧，加入新的特征点，不断的对初始地图进行扩增和更新。与跟踪线程不同，构建地图线程不要求实时性。
用可视化工具来看地图，也就是第二章提到的跟踪前准备数据，地图是这样的:

在地图的初始绘制阶段，可以使用5点算法或单应性分解生成初始化地图，但是需要使用者的配合。使用者首先将网络相机放到需要跟踪的场景中，然后按下一个键，通知系统第一个关键帧已经获取，接下来，使用者需要慢而平稳的将相机平移一段距离，然后再次按下一个键，这时第二个关键帧就被记录下来。在使用者移动网络相机的过程中，第一帧中的特征点会被跟踪，以获得其在第二帧上的对应位置。这样做，系统就获得了一组二维点的对应关系，然后估计两视图相对[R|T]矩阵和三角化特征点，最后通过边界调整生成最终的地图.

有两种方法可以估计当前[R|T]矩阵。一种方法是先求单应性矩阵和极点得到基础矩阵。然后对基础矩阵做SVD分解得到它的RT矩阵[14]。
另一种方法是直接 Homography矩阵分解得到 [R|T]矩阵[15]。实际PTAM中后一版本的代码里用的都是这种方法。

5.1 对极几何、基础矩阵与本征矩阵

在本节中，主要介绍两个视点��像间的几何关系[1]，��即所谓��的两图像��像间的极几何��。极几何是两��图像的点线关联��关系。对应点在对应����线上，这种关联关系������可用所谓��的基本矩阵����代数描述。基本矩阵有 7 个自由度��,从 8 对��像点对应可以线性唯一����������确定。在射影等价��意义下������,基本矩阵��确定��两����像所对应的摄像机矩阵。以下对极几何中的一些概念:

• 基线：指左右两摄像机光心和的连线。
• 极平面：指任一空间点和两摄像机光心三点确定的平面，如平面π就是一个极平面，所有的极平面相交于基线。
• 极点：指基线与左右两图像平面的交点，用e和e’表示。
• 极线：指极平面与左右两图像平面的交线，左图像或右图像中所有极线相交于极点。
  

本征矩阵(Essential Matrix) E：它包含了物理空间中两个摄像机相关的旋转(R)和平移信息(T)。T和R描述了一台摄像机相对于另外一台摄像机在全局坐标系中的相对位置。
基础矩阵(Fundamental Matrix)F：除了包含E的信息外，还包含了两个摄像机的内参数。由于F包含了这些内参数，因此它可以在像素坐标系将两个摄像机关联起来。
本征矩阵和基础矩阵具有如下性质:

(Pr)TEPl=0pTrFpl=0

PrPl 为摄像机坐标系下的点m位置，pr,pl为图像坐标系下的齐次坐标。
我们观察摄像机内参数矩阵M，如果图像没有畸变，即cx,cy为0，并且焦距进行了归一化处理，那么M就成了单位阵，此时本征矩阵F就等于基础矩阵E，即F=E。

5.2 基础矩阵估计的研究

目前已经有了很多利用图像点对来估算基础矩阵的方法。基础矩阵有7个自由度，从8对图像点对应可以线性唯一确定。基础矩阵估计方法可以分为8点算法和最小点对应算法。8点算法主要有Longuet-Higgins的8点算法和Hartley的改进8点算法。前者计算简单，易于实现，但对噪声异常敏感; 后者则通过在计算前对二维数据进行规范化处理，减小了噪声对实验结果的影响。OpenCV中主要使用这类方法。
然而实际情况中，不可能得到精确的图像点对应。DavidNister提出的5点算法，基本思想是两幅图像之间的运动为纯平移运动时，给定5对图像对应点，则可以线性确定本质矩阵，通过与基础矩阵的转化关系，便可以确定基础矩阵。相比“8点算法”和“6点算法”，“5点算法”所需求的点数更少，且对于平面图像不退化，实现精度更高。

5.3 地图初始化

估计基础矩阵可以还原[R|T]矩阵。另一种方法是对单应性矩阵做分解。基于分解单应性矩阵的方法，PTAM初始化地图过程主要有以下几个步骤:
(but: 如果不是用基础矩阵的方法，为什么要求是纯平移运动?看单应性论文，单应性也要求纯平移)
1. 从匹配到的点集中，随机选择至少4对点，并用它们计算这两幅图像的单应性。迭代几次选择最好的单应性矩阵。评分方法:遍历所有匹配点，用当前单应性矩阵得到第二帧的点位置，和观测值取方差和结果即为当前单应性得分。得分反映了当前射影变换和当前单应性矩阵的耦合度。
2. 上一步的得分计算实质也是个最小二乘的最优解问题。采用姿态估计时一样的策略，再次应用加权最小二乘剔除离群点集，可以进一步优化当前单应性矩阵。
3. 分解单应性矩阵。
4. 在可视条件内选择最好的分解。得到当前摄像头的视图矩阵。

5.4 单应性矩阵分解

如图所示，设π是不通过两摄像机任一光心的空间平面，令X是平面π的任一点，它在摄像机下的像为m，记为齐次坐标[x,y,1]，右相机的像为m’ 。用’区分左视图和右视图。图中m和m’所在的平面为π在两摄像机下的图像。

平面π到两个像平面之间存在两个单应性矩阵H1,H2。使得m=H1X,m′=H2X。由于π不过任一光心，所以m,m’之间存在一个二维射影变换 H=H2H−11使得

m′=Hm

假设两摄像机内参矩阵为K,K′，第二个摄像机相对第一个的位置为(R,t)，尝试求解H和RT矩阵的关系。

设X在摄像机坐标系下 坐标为Px(Xc,Yc,Zc),则有：

Xc/x=Yc/y=Zc

n为M所在平面法向量，d为光心到该平面距离，X所在平面方程为:

nTPx=d

用R,T表示两摄像机之间的旋转平移矩阵，则PX,P′X满足:

P′x=RPx+t

联立以上式子即可得单应性矩阵:

m′=(dR+tnT)m→H=R+tnTd

计算单应性矩阵
以上的图像坐标是未经校正的，假设相机内参矩阵为K，标定后的坐标记为n，则有：

n=Kmn′=Mn=KHK−1n

为了求解上式的，可以代入4个点直接线性求解，直接给出公式:

//TODO直接求解就删了吧。。

⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢u′1v′1u′2v′2u′3v′3u′4v′4⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢⎢⎢⎢u10............v10100u10v101−u1u′1−u1v′1−u′1v′1−v1v′1⎤⎦⎥⎥⎥⎥⎥⎥⎥⋅⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢m11m12m13m21m22m23m31m32⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥

再由内参关系式得H=K−1MK。由前面的推导可知，三维空间的图像单应性矩阵包含旋转平移信息，我们希望从它得到两摄像机的相对旋转平移矩阵。Faugeras提出了一种方法[15],对矩阵H做SVD分解。根据奇异值不同取值情况和可视区域条件可以找出最优[R|T]矩阵。

5.5 计算主平面和PCA法线估计

计算得到[R|T]矩阵后，即可对初始帧进行三角搜索和捆绑调整等操作，这部分内容在后面更新帧的张杰中介绍。问题是，做完这些处理后，当前视图是相对第一个关键帧的。我们需要将地图旋转平移到合适的位置。好让世界坐标系原点在摄像机视野内。这一步通过RANSAC完成:
随机3个点集构成一个平面，假设它是我们想要的主平面，然后用其它剩下的点做一致性测试，测试的方法:

• 计算3点平面法向量，归一化三点构面的法向量。
• 计算剩余点到平面距离d=np。剔除离群点。计算剔除后的距离之和作为一次迭代得分结果。
• 反复随机选择100组，取得分最低的一组进行下一步。
• 使用内点作为PCA数据集输入。对这团点云做法线估计[16]，最终确定主平面。

使用PCA做法线估计时，此数据集的的协方差矩阵为:

C=1k∑i=1k(pi−p¯)⋅(pi−p¯)T

其中，p为世界坐标系中的位置向量[x,y,z],k是点云的点个数，p¯为点云坐标平均值。
协方差矩阵的每个元素代表着各个随机变量之间的相关性。每个元素可表示为，当除对角元素外的其它元素为0时，各个随机变量互相独立。因此协方差的特征向量可以表示数据间的相关性，对协方差矩阵特征分解得:

C⋅vj=λj⋅vj,jϵ0,1,2

ref:CodingLabs - PCA的数学原理

λj为协方差矩阵的第j个特征向量，v为第j个特征向量。C为实对称矩阵，故分解的特征向量两两正交。而PCA分解中，特征值越大，对应点集在特征向量上的正交投影与中心点差值越大。也就是最大限度地降维保留了点信息。于是最大的两个特征值对应的特征向量构成主平面。特征值最小的，点云投影的能量最小，对应的特征向量即是目标法向量N。如图所示:

估计得法向量后，以它为世界坐标系的Z轴，把点集坐标位置和摄像机姿态变换到合适的位置。当前点的世界坐标系位置在第一帧位置。要把坐标系旋转平移到图中所示坐标系，变换方程为:

p′w=Rpw+T

而目标位置变换到原点则为:

p′w=R−1pw−T

在第三章中基变换可以拓展到三维，每个旋转矩阵可以看做3个基向量的列线性组合。R即是新坐标系的坐标轴向量在原坐标系中的值的线性组合。以N为z轴，构建三个相互正交的向量。(需要注意这三个向量是原基下的值，只有原基是单位正交基时才能直接计算新的基)。由于R是正交矩阵，R−1=RT,直接将3个向量填入转置矩阵行向量即得R−1:

⎧⎩⎨⎪⎪RT[1]=[1,0,0]−N⋅([1,0,0]⋅N)RT[2]=N⋅R[1]RT[3]=N

(博客，第一个向量与N垂直，和[1,0,0]一起构成直角三角形,每个公式都在博客贴一段代码。)
该坐标系的位置即当前点云的位置均值:

T=[x¯,y¯,z¯]

除了变换所有点，每一帧的视图矩阵也要变化，使视图区域内的点在摄像机坐标系中的位置不变，令M′cw为新的视图矩阵则:

Mcwpw=M′cwp′w

将[4-3式]代入得:

M′cw=Mcw[RT|−T]−1

遍历所有关键帧（当前只有起始帧两帧），按上式更新它们对应的视图矩阵即完成地图初始化工作。

5.6 添加关键帧

最初的地图仅包含两个关键帧，并且描述的是一个较小空间。由于照相机的移动而远离其初始位置，新的关键帧和地图特征被添加到系统中，进而扩增地图。可以被添加的关键侦需要满足以下条件:跟踪质量必须好，与被添加的上一个关键侦时间间隔必须超过20帧;相机的位置必须以最小距离远离巳加入地图的特征点。最小距离的要求避免了照相机恒定不动而破坏地图的问题，并且保证了相邻关键帧的间空间距离很接近。
同时在插入新帧时，当出现第一次观察到的点，也就是在前面的关键帧集中都无法找到对应的点，我们需要新观察到的点的信息：首先判断新观测到的点符合候选点定义，然后计算这个点在世界坐标系的位置，最后把它归类到当前关键帧。计算世界坐标系的过程叫三角搜索。在这之前需要一对点对应。找点对应的过程和跟踪线程相似。只是现在对所有的特征点做筛选，然后再做斑点搜索。

5.7 对极约束

出于性能和精度要求，先使用对极约束缩小搜索点集范围。首先：
1. 每帧图像的4个金字塔层分别保存有所有特征点位置。
2. 使用Shi-Tomasi得分筛选跟踪质量高的候选点。
3. 排除10*10像素内重复的点。
立体匹配中，为了降低可能匹配点对的数量，提出了极线约束，即左图像上的任一点，在右图像上的可能匹配点只可能位于极线上。以图5-1演示，对极约束的定义是：已知视图A的点m，视图B对应的点m’必定坐落在对应的极线l’上。极线约束使得立体匹配搜索范围从二维平面降到一维直线，大大减少了搜索时间，在立体匹配中占据着重要的地位。
由于测量粗差关系，实际m’不可能准确落在l’上。但是求出l’对缩小搜索范围还是有意义的。首先用下标s，t区分源帧和目标帧。把源帧的候选点m经摄像机反校准(CamUnproj)反投影(Unproj)得到源帧摄像机坐标系下z=1平面上的值：

pu=Unprojs(CamUnproj(m))

转为目标帧的摄像机坐标系，M为视图矩阵，O为原点：

p′u=MtM−1spu;Os=MtM−1s[Ts]

获得一个合适范围内的向量：

r=(p′u−Os)⋅len

投影到目标帧的视图上，即得到极线l：

l=projt(r)

后续把在l附近的点加入搜索点集，使用斑点搜索生成m的搜索模板，在点集中找到ZSSD得分最低的点即为一对匹配点。

5.8 奇异值分解解决三角搜索问题

本节介绍用上节得到的匹配点对三维重构它们的三维位置。

在双视图几何中，候选点符合公式x=PX和x′=P′X.P是视图矩阵，[x,y]为图像中的坐标，X即为所求的三维坐标值。则X满足

PX−[x,y,1]T−=[amin,amin,1]T

amin表示绝对值最小，以[i]表示P矩阵的第i行，写成方程组：

xP[3]X−P[1]X=minyP[3]X−P[2]X=min

同理，另一个视图中的x’也有类似的公式。把它们组合成矩阵表示：

||AX||=amin,A=⎡⎣⎢⎢⎢⎢xP[3]−P[1]yP[3]−P[2]x′P′[3]−P′[1]y′P′[3]−P′[2]⎤⎦⎥⎥⎥⎥

，
为了确保非零解，加入约束条件||X||=1，还有另一个约束条件是X是齐次坐标，X第4个元素应该等于1。对目标函数变形：

||AX||2=XTATAX=min

等式右边ATA是个二次型矩阵。加入约束条件，使用拉格朗日乘数法[17]令

L(x,λ)=XTATAX−λ(XTX−1)

取极值则有：

⎧⎩⎨⎪⎪∂L∂X∂L∂λ=2ATAX−2λX=0=1−XTX=0

知L取极值时，X为ATA特征值λ的特征向量，此时||AX||2=λXTX=λ。对A做SVD分解，得到V矩阵即为ATA的特征向量集。取V的最后一列即令||AX||最小的X向量。最后令X满足齐次坐标的约束条件得到三维坐标。

A=UDVTX=VT[4]p=XX[4]

遍历当前帧每个可用候选点，计算它们的三维坐标，即可得到一个当前帧的稀疏的点云。

5.9 捆绑调整（Bundle Adjustment）简介

捆绑调整是系统优化地图的方法。通过捆绑调整迭代来降低误差，以提高鲁棒性。这种方法不仅可以对照相机的位姿进行优化，同时还可以对场景中的所有特征点进行优化。基于全局优化的思路，PTAM有几种捆绑调整策略：
全局捆绑调整是对所有的关键帧进行的；而局部调整仅是对于某个新关键帧，以及其周围相邻的四个关键帧用以优化调整；以及对离群点做调整或者抛弃。如果关键帧被添加到地图时，捆綁调整正在进行，那么，立即中断捆绑调整，并将新关键帧添加到地图中后继续进行捆绑调整操作。事实上构建地图线程大部分编码和耗时都在这一步骤，但对帧捆绑调整的过程和姿态估计内容类似，本节不再赘述。