这是我在组会上讲的文章，当时跟着论文的行文思路来讲的，可惜没能讲得很清楚。现在打算用我自己的思路来介绍这篇文章，希望用尽可能易懂的方法来讲解清楚。也希望通过博客总结，来进一步提升自己的理解。

——致我的第一次组会报告

Image-Based Localization

首先介绍一下什么是Image-Based Localization。随着计算机视觉的发展，现在人们可以利用 SfM（structure from motion） 技术利用场景的图片集合来进行三维重建，得到场景的点云模型。这类软件有几种开源实现，比如 Bundler， VisualSfM。基于图像的定位系统就是利用已知的场景三维模型，通常是点云模型，来对新的图像进行定位，得到拍摄这副图像的相机在三维空间中的位置和朝向。

如上图所示，右边是场景的点云模型的一部分，左上角是查询图像，基于图像的定位就是要求得对应的相机的位置和姿态（点云中红色锥形表示了相机的位置和朝向）。

通常这种定位系统需要离线建立好场景的点云模型，定位的时候，对查询对象首先提取局部特征，通常是 SIFT， 然后需要建立图像的2D特征和点云模型中3D点的对应关系，在得到足够多的2D-3D Matches 之后，利用RANSAC和Pnp 算法，就可以求得相机六个自由度的位置和姿态。

这里的关键是如何寻找到合适的2D-3D匹配关系，通常点云模型中每一个3D还会包含有重建出这个3D点所对应的所有的2D图像特征，一般是通过3D点所包含的2D特征和特性的2D特征的匹配关系，来得到3D点和特性的2D特征的匹配关系，但是在具体寻找匹配关系时，会有很多种思路和策略。

Voting-Based Camera Pose Estimation

介绍了什么是image-based localization之后，下面就开始介绍这篇论文中提到的定位策略。

上图是本文中提出的定位流程图，对查询图像，首先得到每个特征一对多的匹配关系，然后把所有这些匹配关系通过一系列的几何滤波，滤除掉大部分错误的匹配关系，然后再构建文章中提出的 Voting Shape，如果有位置先验信息（比如GPS位置），可以进一步约束 Voting Shape。这里对每一个2D-3D匹配关系都可以构建出一个 Voting Shape，类似于Hough voting，对区域进行一个投票，最后票数最多的区域所表示的姿态就认为是真实的相机姿态，然后得到内点，最后对这些内点通过RANSAC和PnP算法，求得最终的姿态。

Find Reliable 2D-to-single-3D Matches VS Find Extended 2D-to-many-3D Matches

传统寻找2D-3D匹配的方法中，需要在数据库2D描述子中寻找查询图像的2D描述子的最近邻（Nearest Neighbor ），一般会找到最近邻和次近邻，然后通过 Iowe’s Ratio Test 来判断这样的匹配是否是一个正确的匹配。Ratio Test 在计算机视觉中特征匹配时用得很多，也很有效。

我把这种策略称为 Find Reliable 2D-to-single-3D Matches ，也就是对于一个查询2D特征，来找到一个可靠的3D点来得到一个2D-3D匹配关系。这是因为后面再计算姿态时是使用的RANSAC来随机采样，如果你的内点率不高的话，RANSAC采样方法很难得到正确的结果。

但是这种 ratio test 有其内在的缺陷性，当场景变得越来越大时，那么数据库中就会有越来越多的三维点和描述子，也就是说描述子空间中的描述子的密度会越来越大。

图3. Correct Match is Rejected when Descriptor Space is Dense

如上图所示，假设对于查询特征（红色五角星）在数据中真实匹配时红色球，但是数据库中还有很多特征也和查询特征很相似，那么这种情况下还使用 ratio test 的话，并不能得到一个理想的匹配结果（要么得不到一个匹配，要么得到错误的匹配关系）。
所以为了解决随着场景增大，使用 ratio test 得不到正确匹配的问题，论文提出要为查询图像的每一个2D特征来找 N 个匹配关系，也就是一对多的匹配关系，然后通过一些几何约束关系来滤除掉明显错误的匹配，通过投票的方式得到相机的位置和姿态。

Assumptions

首先，论文做出了一些必要的假设。

- Assumption 1: The 3D scene model is gravity aligned, and the ground plane is approximated.
- Assumption 2: Given the camera intrinsic calibration K and gravity direction.
- Assumption 3: The height of query camera is close to ground plane within certain interval.

假设2中要求知道相机的内参，同时能知道相机的重力方向，文章中重力方向的获取是通过计算图片的垂直消隐点来得到的。同样假设2中，也要知道重力方向，这样就可以把整个模型的坐标系和重力方向对齐，地平面是通过数据库中相机的位置插值得到的。

这篇论文中提出的这种模型之所以能够work，很大程度上就是因为对场景有很强的先验，对查询相机也有很强的约束在里面。

Reformulate the Pose Estimation Problem

有了上面的假设了之后，就可以把相机姿态估计这个问题进行简化了。

首先假设我们经过第一步已经得到了所有特征一对多匹配的结果了（后面会再说如何得到一对多匹配关系的具体方法）。

现在就有很多个2D-3D匹配关系了，同时3D点在空间中的位置是已经知道的。先考虑最简单的情况，假设已经相机的高度为 $h$ , 并且相机的 $Z$ 轴是已经和重力方向对齐了的，那么现在问题就简化了成了要在空间中为相机找到一个2D位置和一个绕重力方向的旋转角度，来使得2D-3D匹配关系成立。对于一个特定的2D-3D匹配关系，是可以算出这个3D点在当前相机中的深度$d$的，也就是3D点距离光心的距离。

图4. Camera Sees the 3D Point X

如图所示，假设2D特征点的图像像素坐标为$（x，y）$,根据摄像机的投影关系可以得到3D点的深度 $d = \frac{f(X_z - H_c)}{y}$：

图5. The Possible Place of the Camera (height is known) When it Sees the 3D Point X

那么如果这个2D-3D匹配关系是正确的话，相机观察到这样一个匹配关系，那么相机可能的位置在$xy$平面上就是一个圆，圆心就是3D点在$xy$平面上的投影 $X‘$，半径就是深度$d$。

那么，对每一个这样的2D-3D匹配，都可以在$xy$平面找到这样一个圆，表示相机在$xy$平面可能出现的位置。

图6. The Possible Place of the Camera (height is known) When it Sees the 3D Point X1, X2 , X3

如上图所示，假如有三个正确的2D-3D对应关系，那么可以得到三个圆，三个圆相交的位置就是相机的位置。
当然实际的匹配关系中会有很多错误的匹配，但是可以采用类似 Hough Tranform 中的方法，对区域进行划分，然后投票，得到票数最多的区域，就是得到的相机位置。

Accounting for uncertainty

上一章节是假设了已知准确的相机高度，是理想情况，实际上并不知道相机的准确高度，假设2 中只是假设了相机的高度是在地平面附近的一个区间中。同时2D-3D对应关系中，3D点反投影到像平面上面也会有一定的误差。所以需要把这些不确定性都考虑进来。

图7. 2D-3D Match is Correct when Reprojection Error is within r.

首先，对于一个2D-3D匹配关系，光心、2D像点、3D点并不能同时准确地在一条直线上，考虑到一定的误差，可以认为把3D点反投影到像平面上，会和2D像点的误差在 $r$个像素内。反过来看，通过连接光心和像平面上以像点为圆心、 $r$为半径的圆上的点，可以得到一个圆锥，如图中下部分所示，圆锥无限延伸下去，那么在三维空间中对应的3D点就应该会落入到这个圆锥体中，把这个圆锥体称为 error cone。

图8.Error Cone is Intersected at height Intervals $[h_{min} \space \space h_{max}]$

注意这里的高度是在全局坐标系下描述的。如果相机的高度准确知道，那么可以进一步，把error cone 和 相对光心的高度为 $X_z - H_c（X_z是3D点的高度，即Z轴坐标, H_c是相机光心的高度）$ 的水平面相交，会得到一个椭圆，3D点就会在这个椭圆平面上。因为相机的高度不确定，有一个变化的区间 $[h_{min} \space \space h_{max}]$，那么3D点相对于光心的高度变化范围$[h_l \space \space h_u]$就是$[X_z-h_{max} \space \space X_z-h_{min}]$，这时3D点就会落在相对光心的高度为$[h_l \space \space h_u]$的两个平面截取error cone 所得到的立体中。如果把此时的立体和3D点都投影到水平面上，会得到上图中蓝色梯形所示区域，3D点也应该在蓝色梯形中。把蓝色区域表示的四边形称为error shape。

图9. Convert the Error Shape from Point Position to Camera Position

因为在空间中，3D点的位置是已经确定了的，所以就把上面说的error shape转换到相机的位置上，这样的话相机可能的位置就不再是只在一个圆的边上了，而应该是在一个圆环内(Error Ring)，如下图10.中所示，蓝色的圆环表示相机所有可能的位置，注意和图5.相对比，图5.中高度是固定的，而这里高度是不确定的，只知道一个大致的区间。

图10. Error Ring - The Possible Place of the Camera when Uncertainty Considered

同样的，对于每一个2D-3D匹配对，都可以构建出这样一个圆环区域表示相机可能的位置，自然地，和上图6类似，如果把所有2D-3D匹配关系都考虑进来，就得到了如下图11.所示的结果，相交最多的位置就是相机最有可能的位置了。

图11. Voting Shapes are Intersecting in the Most Likely Camera Position

理解了几何上的原理之后，后面的事情就是要计算每个圆环的位置，然后通过对平面进行划分，对每一个小区域投票，得到投票计数最多的区域就是相机的位置了，最后可以通过此时的内点再进行一个RACSAC+PnP的操作，得到更精确的位置和姿态。

Efficient Voting Shape Computation

要计算上面说的圆环，需要分几步来操作。首先要在相机坐标系下得到图8.中蓝色区域表示的 Error Shape ，接着通过图9.中所示把这个Error Shape转换到相机的位置上，然后再转换到全局坐标系里，最后再考虑旋转角度的问题，得到图10.所示的 Error Ring。

Local Error Shape Computation

图12. Voting Shape Computation

首先来计算在相机坐标系下Error Shape的表达式。这里是用一个四边形来拟合Error Shape，需要得到Error Shape距离光心（相机坐标系原点）的最近距离 $d_n$和最远距离$d_f$，以及$xy$平面上的最左和最后的射线$r_l , r_r$。

图13. The Four Extreme Ray

现在我们来看和一个图像2D特征点对应的3D点在像平面上的反投影的四个极致位置，即垂直和水平方向上的极值点。连接光心和这四个极值点将得到四条射线，红蓝黄绿分别对应图12.中的 $r_n，r_f，r_l，r_r$。明显这四个极值点距离特征点的距离都是$r$，相对于特征点的坐标分别是偏置是$(0,r) ， (0, -r)，( r，0), (-r,0)$。

但是要考虑到相机坐标的$Z$轴不一定是和重力方向对齐的，就会有一个旋转矩阵$R_g$来把局部相机坐标系旋转到和重力方向对齐（$Z$轴正方向竖直向上）。所以四个极值点坐标就会相应变化了。下面介绍具体求解方法。

这里我们引入前面说 error cone 的表达式：

令上式中$v=1$，就得到了图13.中在像平面上的圆的轨迹，分别求坐标$y，z$取得极值时的$u$，得到四个极值点的偏置之后，就可以得到这四个极值点和光心的连线的射线表达式，即上面所说的四条射线$r_n，r_f，r_l，r_r$。求解时利用拉格朗日乘子法来求解。

对于射线$r_n，r_f$，让其分别和$z = X_z - h_{max}, z = X_z - h_{min}$的水平面相交，如图12.(a)中所示，就可以求得 Error Shape 到相机光心的距离$d_n, d_f$。

对于射线$r_l，r_r$，让其夹角为angular resolution(2°)的一半。

另外，需要说明的是还需要考虑重力方向的不确定度，因为重力方向是通过垂直消隐线计算出来的，会有一定的不确定性，论文中3.1节专门讨论了这个问题。这里就不再详述了，感兴趣的话参考原文把。

Convert Local Error Shape into Global Camera Error Shape

得到了相机坐标系下的 Error Shape 表达式，还需要将其转化的全局坐标系里面去。

图14. 把不确定性转化到相机的位置

考虑对于一个2D-3D匹配关系$m=(x,X)$，令 $M$表示其 Error Shape， $\bar{M}$表示$M$的中心。通过Minkowski difference 得到相机的位置的 Error Shape（Voting Shape）$M_C(m) = M_C+t‘，t‘ = X‘ -\bar{M}$，$X‘$是点$X$在$xy$平面的投影，$t‘$是相机坐标系到世界坐标系的平移。因此世界坐标中的Global Voting Shape 就是$V(m) = M_C+t‘$。

把旋转角度考虑进去，得到$V(m，\phi) = R_{\phi}M_C+t_{\phi}‘=R_{\phi}M_C+X‘-R_{\phi}\bar{M}=\{X‘-R_{\phi}p|p \in M\}$。也就是图10.中表示的的蓝色圆环。

同样，对于多个匹配对，都这样构建一个Voting Shape，所有的这些Voting Shape 重叠部分最多的就是相机最有可能的位置。由于对每个匹配对只需要操作一次，所以这个算法时线性时间复杂的的。

至此，应该说是把文中最重要的内容给介绍清楚了。

Filtering Based On Geometry Constraints

因为是对所有的特征进行一对多的匹配，那么必然就会产生很多对匹配对，而且这里面只有一小部分是内点。可以通过一些几何上的约束来剔除一部分外点。论文中主要提出了三种几何滤波器，下面一一介绍。

1. Relative feature orientation。提取局部特征描述子的时候，会有一个方向信息，既然已经知道了重力方向，那么就可以得到所有特征的方向相对于重力方向的夹角。对于一个3D点包含的所有2D特征，描述子相对于重力方向的夹角应该比较一致，可以利用此信息滤波一些明显错误的匹配对。
2. 3D Point Visibility from SfM Model。同样，对于一个3D点，数据库中观测到这个点的相机的位置是已知的，那么查询相机如果也能看到这个3D点，那么视角应该也保持一定的一致性。这就是图11.出现的invisible area的原因，因为可以判断这些视角是不可能观测到该3D点的。
3. Feature Scale。论文中提出了scale of 3D point 这么一个概念，其实就是用投影的三角形比例关系，由2D特征的尺度，和焦距，深度来得到一个3D点的尺度，$S_{3D}=S_I *d/f$，同一个3D点计算出来的$S_{3D}$的分布也应该是保持一致性。
4. Positional Prior。另外，如果有GPS这种先验位置的信息，可以直接限制投票区域。

论文中称这些滤波器可以滤除掉85%的初始匹配对。可见效果还是挺好的。

Experiments and Results

前面说对特征进行一对多的匹配，那么该怎么一对多匹配呢?
文章中提出了四种思路在近邻搜索时来找到N个匹配。

1. Ratio test. For 1:N matches the ratio test is performed wrt. the N+1th neighbor.
2. Constant number of nearest neighbors.
3. Absolute thresholding.
4. A variable radius search.

另外每一策略都可以加上 back-matching来反向验证，对此2D-3D匹配关系，验证3D在查询图像的特征空间中的最近邻就是该2D特征。

图15.实验数据

实验采用的数据主要是红色框中的两个。SF-0是一个大场景的城市街景数据集。下面开始来说实验结果吧。

图16. 滤波操作和匹配策略的实验

从上图中可以看到，滤波操作效果明显，在95%精度处，召回率从蓝色的Baseline（不到50%) 可以提升到65%，甚至70%(with GPS)。不同的匹配策略效果也不同，可以看到策略4（variable radius search），N=3 效果最好。后面实验都采用这个设置。

图17. SF-0数据集实验结果

红色框中为相机最后的6 DoF 姿态，相比较之前的方法，召回率大大提高了。并且可以看到，最后的P3P算法并没有提高定位的性能，但是可以得到更精确的相机的姿态。

图18.Dubrovnik数据集实验结果

在较小的数据集Dubrovnik上也达到了state-of-the-art的性能。尤其是在进行了Bundle Adjustment之后，相机位置的误差中值只有0.47m，这时迄今为止所有方法中取得的最好的结果。

Reference

Zeisl, Bernhard, Torsten Sattler, and Marc Pollefeys. Camera Pose Voting for Large-Scale Image-Based Localization. Proceedings of the IEEE International Conference on Computer Vision. 2015.