窥探PTAM之模板搜索

3. 斑块查找

在跟踪线程中，使用者变换摄像头姿态或位置得到新关键帧，新关键帧需要重新查找特征点，找到与上一帧匹配的点以实现点跟踪。为了快速精确地找到邻帧对应特征点，PTAM使用一种基于斑块搜索的特征匹配方法。首先从照相机采集一帧灰度图像，由运动估计模型计算一个估计姿态。将地图中的特征点投影到图像上。然后进行斑块搜索。

其基本原理就是从源图像中生成一个搜索模板，该模板可以补偿摄像机运动造成的尺度和角度变换。用该模板在目标图像中搜索找到最佳的匹配位置。具体步骤如下：

1. 在当前帧查找SIFT特征点。SIFT特征查找方法有多种，本文不再赘述。
2. 计算源帧在当前姿态下的一个变形矩阵A。
3. 通过A的行列式决定搜索层。
4. 通过A生成搜索模板。
5. 使用该图像模板在当前帧一定范围内搜索匹配，找到斑点位置。
6. 使用inverse-compositoin计算该点所在的亚像素精确位置。

3.1仿射变换

本节介绍第2步，介绍前了解一种常见的2D变换：仿射变换。在目标跟踪应用中，图像目标通常会发生旋转和缩放等变化，而另一方面，目标与图像采集设备之间的距离通常较远，其三维变化常可以用二维变化来近似，所以，在兼顾算法效果和计算效率的考虑之下，经常可以选择使用了六参数的仿射变换运动模型。 在我们的模型中，摄像机运动变换导致的图像变换也可以用仿射变化近似模拟，参考图[3-1]。
仿射变换是射影几何中的一种基本变换。仿射变换有6个自由度，分别是两个平移,两个旋转旋转，一个错切,和一个缩放。
二维欧氏空间上的仿射变换可以表示为：

$$\binom{u'}{v'} = \begin{bmatrix} a_1 & a_2 \\ a_3 &a_4 \end {bmatrix} \begin{bmatrix}u \\ v \end {bmatrix}+ \begin{bmatrix}T_x \\ T_y \end {bmatrix}$$

当源帧图像生成模板时执行上面的这些投影可以确保扭曲矩阵能补偿透视投影、尺度变化以及镜头畸变造成的图像影响。A即为上式中$a_n$z组成的矩阵。它表示旋转，缩放，错切的合成变换。
仿射变换具有6个自由度，如果要对仿射矩阵线性求解，至少需要6对点对应。我们没有6组点对应，但是有每个点关于空间的投影关系。利用这些关系也能求得仿射矩阵。
在应用仿射变化之前，下节引入一些概念，以更好地理解仿射变换乃至射影变换的实质。

3.2基与坐标变换

假设向量与矩阵乘积Mx，我们可以把这个形式的矩阵M理解为对向量x的基——即坐标系的声明，向量x可看作空间的一点。乘积的结果向量，就是x在标准正交基I下的空间坐标，或者是坐标值不变，只是换了一个基。
一个变换T(x)，其M下的变换矩阵为A，则变换过程可以表示为MAx，其位置就是MAx，而变换后的坐标，从数值上来讲也可以认为是相对位置没有变而基变了：从x以M为基变换到了x以MA为基。变换是相对的。所以这个变换T的矩阵A，也可以理解为是对基M的一个变换。也就是运动的实质，就是基变换。仿射变换的实质也是基变换。
从这个角度理解，正交基下的变换有特殊的性质，以仿射变换为例，对点(1,0)做2D变换，可以表示为：

$$p = \begin{bmatrix} m1 & m2 \\ m3 & m4 \end{bmatrix} [1,0]^T = I[m1,m3]^T$$
上式x方向单位向量做了个基变换得到一个向量，这个向量正好是原基的第一列。该基的Y方向化为单位正交基的坐标为[m2,m4]。也就是正交基下，两个向量按列排序组合即可组成新的基。

3.3计算扭曲矩阵

本节介绍PTAM中计算仿射变换矩阵的过程，同时也能证明，在静态场景下，仅摄像机的移动造成的图像变换可以用仿射变换来代替。下面简称第一次找到该点的关键帧为源帧，该帧对应的视图矩阵为源视图矩阵，记为$E_{cws}$。当前帧的视图矩阵用$E_{cw}$表示。但是当前还没有估算姿态，所以这里使用的是上一帧的姿态。
我们执行仿射变化是为了用2D变换近似模拟3D变换，最终目的是为了在两幅不同摄像机姿态的图像中找到对应着同一三维点的一对像素。
每个mapPoint有一个从属关键帧的摄像机坐标系的位置。这个位置很可能是不精确的。我们把该位置看做一个平面中的一个点。假设这个平面为P。这个平面的法向量指向源帧摄像机。还假设一个平面z=1，垂直于摄像机坐标系的z轴，这个就是经摄像机内参矫正后的图像平面。平面P平行于Z=1平面。
如图所示：

取匹配点，也是斑点中心点m(u,v,1)，的正下方一个像素(u,v+1,1)，连接该像素和匹配点得到一个竖直向下的向量d。还已知m点在世界坐标系的位置，应用原始帧的针孔摄像机模型，可以计算出向量d在平面P的投影$Down_w$。如果取m右边一像素，同样可得“右向量”在P的投影$Down_r$。显然P离摄像机越远，上述向量的模越小。然后将$Down_w$左乘$E_{cw}$,得到该向量在当前视图的投影向量：

$$\begin{equation} \mathbf{Down_c} = [\Delta u_d,\Delta v_d ]^T = E_{cw} Down_w \end{equation}$$

由前一节矩阵与基的关系可知。$\{\Delta u_d,\Delta v_d \}$对应原图像(0,1)向量。同理，下标r表示$Right_w$方向的投影，$\{\Delta u_r,\Delta v_r \}$对应（1，0）。以斑点中心点为原点，把它们组成一对新的基，那么该基下的点在正交基下的值为：

$$\begin{equation}[u_c,v_c]^T = \begin{bmatrix} \Delta u_d & \Delta u_r \\ \Delta v_d & \Delta v_r \end {bmatrix} [u_s -u_0,v_s-v_0]^T \end{equation}$$

图[3-2] 的上半部分是上式的结果，显然变换的结果转为笛卡尔坐标系后是个平行四边形。为了后续SSD计算方便，我们需要的是一个8x8的方形模板。所以还要做一个对基做一个变换，以产生一个正交基下的坐标值，用A表示等式右边的2x2矩阵，它表示仿射变换的一部分，做如下基变化：

$$\begin{equation}I \binom{u_T}{v_T} = A \binom{u_S}{v_S} \rightarrow A^{-1}\binom{u_T}{v_T} = I \binom{u_S}{v_S} \end{equation}$$
最终得到 $A^{-1}$即是我们需要的扭曲矩阵。
(基变换，图)

同时这两个单位投影向量组成的平行的四边形面积即为矩阵A的行列式：$S = \Delta U_R *\Delta V_D - \Delta V_R * \Delta U_D$。
当当前摄像机比源帧摄像机位置更靠近斑点时，面积更大。此时画面尺度变大，根据这个面积值大小决定使用源帧图像金字塔的那一层，以更好地和当前图像匹配。
有了基，我们还需要坐标值才能得到变换后的结果。式子中，下标T表示变换后的坐标值，S表示源图像。现在使用$u_T,v_T,A^{-1}$计算得到源帧下对应的四个点。将它们围成的平行四边形采样，双线性插值到目标图像，即得到这个mapPoint在当前视图中的8x8像素的搜索模板。这个过程可以使用TooN库的transform函数完成。

CVD::transform(p.pPatchSourceKF->aLevels[p.nSourceLevel].im, mimTemplate, m2,vec(p.irCenter),vec(mirCenter));

参数分别为源图像，目标图像，扭曲矩阵，源图像采样中心点（对应图[3-2-3]的原点)，目标图像中心点中心点。采样方向和单位长度由扭曲矩阵决定，尺度由目标图像大小决定。
以上就是扭曲矩阵A补偿尺度和角度变化的原理。

3.4 模板搜索

模板匹配步骤可以应用上一步生成模板在目标图像中寻找匹配位置，找到的点和源点形成一对点对应，用于后续姿态估计算法中。上一节使用源点对应源帧的图像、变形矩阵、该点在图像上的投影位置，生成一个8x8像素的模板。然后计算图像金字塔层数，从目标图像的图像金字塔层中得到目标图层，以源位置为中心，在一个合适的半径内搜索，找到最匹配的位置。搜索过程如下：首先在该范围内找到所有角点，在此系统中角点是该图像的FAST特征点，然后在角点位置应用该模板计算SSD算法得分，迭代每个点选择差异值最小的角点作为最佳匹配位置。假如这个得分小余某个预设的预置，那么就认为在该范围找到了该斑块和点对应。

3.5 反向合成法计算该点亚像素位置简介

在上面所说的搜索中，当搜索层数大于0时，得到的模板最佳匹配位置是所在图层的像素位置。后续跟踪时还需要它的第0层的图像坐标，这个位置可以使用根据图像邻点关系，使用反向合成法做图像对齐，然后获得第0层坐标，得到的坐标称为亚像素位置。