视觉里程计 第一部分：前30年的发展和理论基础

视觉里程计 第一部分：前30年的发展和理论基础

VisualOdometry -Part I The First 30 Years and Fundamentals
TaylorGuo, 2017年1月27日星期五



视觉里程计是通过单个相机或多个相机作为输入估计自身运动的过程。应用领域涵盖机器人、可穿戴计算、增强现实和自动驾驶。Visual
Odometry (VO)视觉里程计由Nister在2004年路标论文中创造。这个名词和车轮测距非常类似，增量式地通过对车轮转过的圈数积分估计车辆的运动。同样的，视觉里程计使用板载相机检测图像运动变化增量式地估计车辆位姿。为了使视觉里程更有效，必须有足够的环境图像，具有足够纹理的静态图像可以提取运动特征。另外，需要捕捉连续的图像使场景重叠。
相比车辆测距，视觉里程计的优势是不受车轮在不平整地面或其他不良状况下滑动的影响。视觉里程计还提供了更为精确的轨迹估计，相对位置误差范围为0.1%到2%。这使得视觉里程成为车轮测距有益补充，还有其他导航系统，比如全球卫星定位系统GPS，惯性测量单元IMU，和镭射测距（与视觉里程类似，镭射测距通过连续的镭射扫描匹配车辆估计自身运动）。在GPS失效的环境下，比如水下或太空中，视觉里程计非常重要。
这个指南和综述分为两部分，详细介绍视觉里程计，包含从1980年到2011年间的研究。前20年大多是离线执行，在视觉里程计第一次用于火星探索时，第3个10年实时系统开始大量出现。第一部分指南，回顾了头30年该领域的研究及基础。简要地讨论了相机模型和标定，介绍了单目和双目的主要运动估计方法，分析了每种方法的优势和劣势。第二部分主要讲述特征匹配，鲁棒性和应用。它主要回顾了视觉里程计中的点特征检测和不同的无效数据（离群点）去除方案。重点讲述了RANSAC随机抽样一致性算法，讨论采用独特设计方法提高计算速度。其他的主题涵盖误差模型构建，位置识别（或闭环回路检测），捆集调整。
这个手册给没有经验和非专业用户提供了算法指南和参考用于构建完整的视觉里程系统。既然理想的或独一无二的适用各种工作环境的视觉里程计方案并不存在，那么最佳的方案应该要根据特定的导航环境和给定的运算资源仔细地选择。

视觉里程的历史

计算机视觉领域中，从一组相机图像（校准的或没有校准的）中重构相关相机位姿和3维结构的问题，众所周知，采用运动恢复结构structure
from motion (SfM)方法。它源自于论文2和论文3的工作。视觉里程计是运动恢复结构SfM方法的特例。运动恢复结构SfM方法更通用，处理从连续图像序列中的结构和相机位姿进行3D重建问题。最终结构和相机位姿通过一个离线优化（比如，捆集调整）来精炼，它的计算时间随图像的数量增长，如论文4。相反地，视觉里程计主要估计相机序列的3D运动，以新的图像帧进入的方式，它是实时的。捆集调整用于精炼轨迹的局部估计。
单独利用视觉输入估计车辆自身运动的问题始于1980年代早期，论文5中有描述。早期的视觉里程计研究，如论文5到论文9，用于行星探路车，受到NASA火星探索计划的激励，致力于提供可以在各种地形下探路车可以测量6自由度的运动的能力，即使是在不平整路面和粗糙地形下出现轮子打滑。
Moravec论文5的方法出众，不仅仅因为第一次提供了运动估计方法，它们的主要功能模块至今仍在使用，还提供了一种最早期的角点检测方法（第一次由Hannah论文10在1974年提出）如今大家熟知的Moravec角点检测，如论文11，是Forstner论文12和Harris和Stephens论文3和论文82的先驱。
Moravec在一个行星探路车上测试了他的工作，这个车子配备了称为“sliderstereo”的一个单目相机在轨道上滑动。机器人以停止-运动的方式前进，在每个位置获取和分析图像。每停一次，相机水平滑动、在相同的间隔下拍9张照片。图像上角点检测采用他的操作方法，沿极对线用归一化互相关系数方法匹配其他8帧图像。如果有大尺度的场景变化采用coarse-to-fine策略（金字塔）通过相关系数查找机器人下一个位置的潜在匹配。无效数据（离群点）通过检测8个立体图像的深度值的不一致性去除。最后，从两个连续的机器人位置对比三角化的3D点云计算运动状况作为粗略的移动变换。系统可以通过一个加权的最小二乘法处理，权值与3D点云的距离呈相反关系。
尽管Moravec采用了单目滑动相机，但他的工作仍然属于立体视觉里程计算法的一种。相关的3D位置特征通过每个位置的三角化方法直接测得，用于获取相对运动测量。三目的方法也属于同一类算法问题。相对立体视觉，单目相机也是一种方法。在这种情况下，只有轮子滚动的信息可以采用。不足之处是运动信息只能在一个维度上获取。绝对尺度信息可以通过直接测量的方法（比如，测量场景中的某一个元素的尺寸大小），运动约束，或集成其它传感器，如IMU，气压传感器，和距离传感器，来获得。当相机到场景的距离过大、大于立体相机的基线（两个相机之间的距离），stereoVO的效果就退化成单目。在这种情况下，立体视觉无效，必须采用单目视觉方法。经过多年发展，单目和立体视觉里程计分别发展成两个独立的研究路线。接下来，我们将了解相关领域的工作。

立体视觉里程计

大部分视觉里程计研究采用的立体相机。基于Moravec的工作，Matthies和Shafer论文6和论文7采用双目系统和Moravec的方法检测和追踪角点。与Moravec不确定概率的数量表示不同的是，他们采用三角化特征的误差协方差矩阵，并将其整合进运动估计的步骤中去。他们还演示了探路车运动轨迹的效果，5.5米的路上2%的相关误差。Olson的论文9和论文13，后来扩展了这些工作，引入了绝对方向传感器（比如，电子罗盘或全景相机），采用Forstner角点检测，明显加快了计算速度。他们发现单独采用相机自身运动估计会产生累积误差，该误差随着移动距离的增加呈超线性增加，最后导致航向误差。如果加入一个绝对方向传感器，误差的增长会随着移动距离的增加呈线性增长。他们测出在20米的道路上相对位置的误差为1.2%。
Lacroix论文8在探路车上采用了和前面类似的立体视觉里程方法。差别是关键点的选择。没有用Forstner角点检测，他们采用稠密立体视觉，通过分析峰值处的相关函数选取候选关键点，方法如论文14，论文15所示。这种选择方法基于观测值，在相关曲线的形状和特征深度值的标准偏移之间有非常强的相关性。这种观测方法后来被Cheng论文16和论文17应用在火星探路车上的视觉里程计上。他们在两个方面改进了早期Olson论文9和论文13里的实现方法。第一，采用Harris角点检测方法，使用特征点相关函数的曲率，如Lacroix提到的，去定义图像点云的误差协方差矩阵。第二，如Nister论文1，他们采用RANSAC随机抽样一致性算法在最小二乘法运动估计步骤中去除离群点。
另外一种用于运动估计和离群点去除的方法由Milella和Siegwart论文14中提出。他们采用论文19中Shi-Tomasi方法进行角点检测，与Lacroix，他们保留了那些在立体差别地图中高质量的云点。运动估计第一次用最小二乘法解决，后来采用迭代最近点算法IterativeClosestPoints（ICP）论文20，该算法广泛应用于激光镭射扫描的3D融合，用来做位姿优化。考虑到鲁棒性，离群点的移除步骤也整合进ICP算法。
上面提到的工作，都是3D点三角化用于立体图像对，相对运动问题通过3D到3D点的融合（对比）问题进行处理。一种完全不同的方法是2004年Nister在论文1提到的。这篇论文不仅创造性地提出了“视觉里程计”的概念，还第一次提出并实现了实时长距离鲁棒性的离群点去除的算法流程。Nister在几个方面进行了改进。第一，与之前所有的工作相比，他们没有在所有图像帧上跟踪特征，而是单独使用Harris角点检测特征，在特征之间进行匹配。在基于交叉相关的跟踪中的优势是可以避免特征偏移。第二，他们没有计算3D到3D云点融合问题的相关运动模型，但计算的是3D到2D相机位姿估计问题（这个问题称为“运动估计”）。最后，他们将RANSAC离群点去除算法整合进运动估计的步骤中。
Comport在论文21中介绍了一种不同运动估计算法。并没有采用3D到3D点云融合或3D到2D相机位姿估计技术，他们采用四视点张量几何关系，可以通过2D到2D图像匹配，而不需要三角化3D点云。直接使用原始2D点云替代三角化的3D点云可以更精确地计算运动信息。
关键帧的选择是非常重要的一步，一般应该在更新运动之前完成。

单目视觉里程计

双目和单目视觉里程计不同的是，相关运动和3D结构必须从2D数据中计算出来。既然绝对尺度不知道，最开始的两个相机位姿的距离设置为1。当一个新的图像帧进来的时候，相对于前两个图像帧的相对尺度和相机位姿采用3D结构或三焦点张量方法，如《多视图几何》里面所讲。
长距离（大概几公里）单目相机采用透视和全景相机在过去10年中获得了巨大的成功。相关的工作分为3类：基于特征的方法，基于表面外观的方法，和混合方法。特征方法，跟踪图像帧间显著可重复的特征；表面外观方法，使用图像或图像子区域的所有像素稠密信息；混合方法，是将两种方法合并使用。
第一类方法，有论文1，24，25，27和论文30-32。第一次使用单目相机的实时大场景视觉里程计是Nister的论文1。他们采用RANSAC去除离群点，用3D到2D相机位姿估计计算新的相机位姿。采用了5点法的最小解决方法计算RANSAC中的运动假设，如论文33。之后，5点RANSAC在视觉里程计里变得非常流行，并应用于论文23，25，27中。Corke在论文24中提出了一种使用反射相机和光流基于全景拍摄的单目视觉里程计。Lhuillier论文25和Mouragnon论文30提出了一种基于局部窗口的捆集调整方法重构运动和3D地图（这意味着捆集调整可以在前m帧图像子窗口中执行）。同样的，他们也采用了5点RANSAC方法去除离群点。Tardif论文27提供了在2.5公里的视觉里程计方法，他没有采用捆集调整。与之前的工作不同的是，他们解耦了旋转和平移估计。旋转通过无穷远点计算，平移通过重构3D地图计算。通过5点RANSAC方法去除错误的对应。
基于表面外观或混合的方法，如论文26，28，29。Goecke论文26采用Fourier-Mellin变换融合汽车上的基准透视图像。Milford和Wyeth论文28采用RatSLAM算法从汽车上的单透视相机提取粗略的旋转和平移速度信息。采用模板跟踪场景中心位置。基于场景外观的方法在遮挡的鲁棒性上不好。因此，Scaramuzza和Siegwart论文29使用场景外观估计汽车的旋转，从基准估计旋转和绝对尺度中估计特征。基于特征的方法也用于检测基于场景外观方法是否失效。
上面提到的这些方法的设计都没有6自由度的运动约束。然而，有几种车用视觉里程计采用了运动约束。它的优势是减少了计算时间，增强了运动估计精度。例如，Liang和Pears论文35，Ke和Kanade论文36，Wang论文37，Guerrero论文38采用了单应射在一个基准上估计自身运动。Scaramuzza在论文31，39介绍了单点RANSAC离群点去除方法，它基于汽车非完整的约束加速自身运动估计到400Hz。在后续的工作中，他们用非完整约束从单目相机中恢复汽车转弯的绝对尺度。根据他的工作，Pretto论文32也采用了非完整约束增强了特征跟踪，Fraundorfer论文41采用了子窗口捆集调整。

减少偏移

视觉里程计增量式地计算相机轨迹（一个位姿一个位姿地），每一个新的帧到帧运动计算随着时间会累积误差。这样在实际路径和估计的轨迹之间就会产生偏移。对有些应用，尽可能地减少偏移是非常重要的，可以通过前m个相机位姿做局部优化。这种方法称为滑动窗口捆集调整或子窗口捆集调整，它应用于论文41-44。例如，在一个10公里的视觉里程计的实验中，Konolige论文43演示了子窗口捆集调整通过因子2-5减少最终位置误差。显然，视觉里程计可以通过融合其他传感器，如GPS和镭射激光，甚至是IMU，减少误差，如论文43,45,46。

V-SLAM

尽管本文主要讲视觉里程计，但视觉SLAM同样值得在这里提一下。具体更深入的SLAM问题的研究，可以参考Durrant-Whyte和Bailey的论文47和48。V-SLAM中两种方法占主导地位：1）基于滤波器的图像信息融合方法，采用概率分布，论文49；2）非滤波器方法（也称为关键帧方法）对挑选的关键帧使用全局优化，如论文50。论文51评估和总结了它们的主要优点。
过去的几年中，单目和立体相机都取得了成功，如论文49,52-62。大部分的工作都局限于小的室内场景，最近有些工作是大场景的，如论文54,60,62。早期的V-SLAM采用了全协方差卡尔曼方法，如论文52,53,49。Davison论文49的优势是在任何时间上可重复定位。Handa论文59基于概率论框架采用了主动匹配技术优化了这个工作。Civera论文60采用了RANSAC模型假设阶段的先验信息的卡尔曼滤波和单点RANSAC融合。最后，Strasdat论文61提供了一个大场景V-SLAM框架采用关键帧优化方法，如论文50，这成为SLAM的一个特色。

视觉里程计只关注局部轨迹的一致性，SLAM关注全局轨迹的一致性。

VO与V-SLAM

这里会详细分析视觉里程计和V-SLAM。SLAM（特指V-SLAM）的目的是要对机器路径有全局一致性的估计。这意味着要追踪环境地图（有些情况下地图可能不需要）因为当机器人走过之前去过的地方它需要能够识别。（这称作闭环控制。当回环检测到后，这个信息用于减少地图和相机路径的漂移。理解什么时候生成回环和有效地将这个新的约束集成到当前地图中，是SLAM两个主要问题。）而视觉里程计主要是增量式地重构路径，一个位姿接一个位姿地，优化前n个路径位姿（也称为子窗口捆集调整）。这个滑动的窗口优化相当于在SLAM中建立一个局部地图；然而，其思想方法是不同的：在视觉里程计中我们只关心轨迹的局部一致性，局部地图用来更精确地估计局部轨迹（比如捆集调整），但SLAM关心的是全局地图的一致性。
视觉里程计用于构建SLAM算法的一个模块，用于重构相机的增量运动；然而，为了设计完整的SLAM方法，需要添加回环检测，全局优化以获得精确一致的地图（没有这一步，地图在逻辑上是一致的）。
如果用户只对相机路径感兴趣，但不需要环境地图，也可以用完整的V-SLAM方法，而不是视觉里程计技术。V-SLAM方法可以更精确，因为它增加了很多路径上约束，但不一定更具鲁棒性（比如，离群点可能严重影响地图一致性）。另外，它更复杂，计算代价也大。
最后，关于视觉里程计和V-SLAM的选择取决于性能，一致性和简单性的选择。尽管，需要相机路径全局一致性，视觉里程计可以为了实时性而牺牲一致性，不需要追踪之前相机的位姿。

视觉里程计问题的公式推导

假设在时间k，通过相机移动，追踪环境图像。如果是单目相机，k时间上拍摄的一组图像表示为I0:n={I0，…，In}。如果是立体相机，每个时刻有左右两幅图像，记为Il,0:n={Il,0，…，Il,n}和Ir,0:n={Ir,0，…，Ir,n}。图1表示了这个具体问题。



图1，演示了视觉里程计问题。

相邻的相机位置的相对位姿Tk,K-1（或相机系统位置），通过视觉特征进行计算，将位姿连起来获得关于初始坐标帧k=0时的绝对位姿Ck。

假设相机的图像帧坐标为机器人的坐标。立体相机，为不失一般性，左眼的坐标为原点。

在连续时刻k-1和k，相机位置关系通过变换Tk，k-1∈R4x4记为：



其中Rk，k-1∈SO(3)是旋转矩阵，tk，k-1∈R3x1是平移向量。这组向量T1:n={T1,0，…，Tn,n-1}包括连续的相机运动。为了简便，我们将Tk，k-1记为Tk。最后，相机位姿C0:n={C0,…
,Cn}包含了相机相对初始坐标k=0的变换。当前位姿Cn可以通过联立变换Tk（k=1…n）计算，因此，Cn=Cn-1Tn，其中C0是相机k=0时的位姿，用户可以任意指定。
视觉里程计的主要任务就是计算图像Ik到Ik-1的相对变换，然后联立变换重建相机的C0:n的轨迹。这意味着视觉里程计一个位姿一个位姿的增量式地重建轨迹。这个步骤之后，前m个位姿迭代优化可以获得局部轨迹更精确的估计。这个迭代优化的过程，是计算重构前m个图像3D点云重投影误差开方的和的最小化值（称为窗口化捆集调整，因为它在m帧图像上的子窗口执行。捆集调整将在第二部分详细介绍）。三角化图像上的点获得3D点云（参考“三角化和关键帧的选择”）。
单目视觉里程计有两种方法计算相对运动Tk：基于场景外观的（或全局的）方法，采用两个输入图像的所有像素的亮度信息；基于特征的方法，只采用从图像中提取（追踪）的显著的、可重复的特征。全局方法的精度比基于特征的方法低，计算复杂度更高一些。（在之前“视觉里程计的历史”章节中，大部分基于场景外观的方法应用于单目视觉里程计中。主要是由于相比立体相机更容易执行。）基于特征的方法要求对图像帧的特征提取（追踪）具有鲁棒性，它比全局方法更快、更精确。因此，大部分视觉里程计的执行都采用基于特征的方法。
视觉里程计的主要流程方法如图2所示。对每个新的图像Ik（如果是立体相机，就是图像对），前两步包括检测和匹配2D特征，是通过与之前的图像帧做对比而进行的。从不同的图像帧上提取的3D特征是2维特征的重投影，这是图像的对应关系。（我们将在第二部分详细讲解特征匹配和特征跟踪的区别。第一个特征匹配是在图像中单独检测特征，然后基于相似的特点再将它们进行匹配；第二个特征跟踪是先查找一幅图像中的特征，然后用局部搜索方法，比如关联程度，在下一幅图像中跟踪它们。）第三步是计算两个时刻间k-1和k的相对运动Tk。根据对应关系是3维或2维的不同，有3种不同的方法处理这个问题（如“运动估计”章节所示）。相机位姿Ck就可以通过联立位姿Tk和之前的位姿计算获得。最后，通过对前m帧图像进行迭代优化（捆集调整）可以更精确地估计局部轨迹。



图2：视觉里程计的主要流程

本文对运动估计也有详细说明（参考“运动估计”章节）。特征检测和匹配，捆集调整将在第二部分详细说明。同样的，一个精确地运动计算，特征对应不应该包含离群点（比如错误的数据关联）。即使出现离群点也要确保精确的运动估计，是鲁棒估计的主要任务，也将在第二部分详细说明。大部分视觉里程计的执行都假设相机已经校准过。我们将浏览一下透视相机或全景相机的标准模型和校准过程。

相机模型和校准

视觉里程计可以采用透视相机和全景相机。我们将在本章主要讨论一下相机的模型。

透视相机模型

透视相机常用模型是针孔射影系统：物体反射的光线通过透镜中心（投影中心）在焦点平面形成图像，如图3a所示。令X=[x,y,z]T为相机参考帧上场景中的一个点，p=[u,v]T为图像屏幕上的一个像素的投影。从3D世界到2D图像之间的对应关系可以通过一下透视投影等式表示：



其中，λ是深度因子，αu和αv是焦距，u0和v0是图像投影中心的坐标。这些参数称为内参。当相机的视角大于45o时，就会产生光线扭曲，可以通过二次或更高阶多项式构建模型。完整的模型推到可以参考计算机视觉的教科书，如论文22和论文63所示。令p=[u,v,1]T=K-1[u,v,1]T为类似的图像坐标。归一化坐标将在后面的章节中大量使用。

全景相机模型

全景相机的视角非常宽（甚至超过180度），可以用鱼眼镜头或通过镜像合并标准相机来制造[也称为反折射相机，如图3b所示]。反折射相机中典型的镜头形状是旋转的二次曲面（比如抛物面或双曲线体），因为它们产生一个投影中心，使得采用运动估计成为可能，这将在“运动估计”中详细讲解。



图3（a）透视投影，（b）反射投影，和（c）透视相机和全景相机地球形模型。图像点朝向观察点，在单位圆球中标准化。

目前，有两种全景相机的模型。第一个是论文64中的反折射相机（抛物面或双曲线的），第二个是论文65中的针对鱼眼和反折射相机统一的模型。论文66和论文67对着两种模型进行了研究。统一模型的投影等式为：



其中，和a0,a1,…,an是相机内参，取决于它是镜面镜头还是鱼眼镜头。如论文65所示，n=4对镜面或鱼眼镜头来说是一个比较恰当的选择。最后，这个模型假设图像平面满足理想特性，相机感光平面的轴心和镜面轴心是对齐的。尽管这个假设只是针对鱼眼镜头和反折射相机的，但可以通过引入理想平面和图像平面的透视投影构建偏移模型，如论文66所示。

圆球模型

我们希望相机有一个有效单视点(singleeffective
viewpoint)，即在视觉系统中只存在一个投影中心。其主要原因是单视点可从全方位摄像头采集的图像中产生正确的几何透视图像，同时还适用于成熟的外极线几何理论。反折射相机中，当光线通过镜面相交于一个点C时，才会形成投影中心。如果存在这个点，可以使我们能够从一个单视点到圆球通过对应关系构建全景投影模型。为了方便，通常会采用单位球体。
值得注意的是球体模型不仅可以应用于全景相机中，还可以应用于透视相机。如果相机校准过，透视图像或全景图像上的任何点就可以映射到单位球体上的一个向量。如图3（C）所示，单位向量指向观察到的图像场景中的点。这些向量是单位球体上归一化图像点。

相机校准

相机校准的目的是为了获得相机系统更精确的内部参数和外部参数。在多相机系统中（比如立体或三相机），外部参数描述了每个相机对的彼此位置关系和方向信息。最常用的模型是采用平面棋盘模型。棋盘上方块的位置已知。为了精确地计算校准参数，用户需要在不同的位置和方向拍几张棋盘的图片，拍的时候要确保相机视角尽可能地覆盖整个场景。相机的内部参数和外部参数可以通过最小二乘法计算。输入数据是棋盘方格的四角的2D位置和它们对应的每幅图像的像素坐标。
许多相机校准工具都有MATLAB和C版本。最新的可以在网页68中找到。在这些工具中，最常用的是论文69中的Matlab和论文70到72的透视和全景相机。透视相机的C语言校准版本OpenCV，网页73中有提供一个开源计算机视觉库OpenCV。

运动估计

运动估计对每幅图像来说在视觉里程计中是一个核心步骤。精确的说，是计算运动估计中当前图像和之前的图像间的相机运动。将这些单次运动链接起来，就可以重构相机的运动轨迹。本章详细解释如何从时刻K-1和K的两组对应的特征fk-1和fk中计算两幅图像Ik-1和Ik的变换Tk。根据特征对应关系是2维还是3维，有3中不同的方法：

2D到2D：fk-1和fk都是2维图像坐标；

3D到3D：fk-1和fk都是3维图像坐标。需要三角化每个时刻的3维云点，比如使用立体相机；

3D到2D：fk-1是3维，fk是它们对应的图像上的2D重投影。单目相机情况下，3D结构需要从2个相邻的相机视图（比如Ik-2和Ik-1）进行三角化，然后和第3幅视图中的2D图像特征进行匹配（比如，Ik）。在单目算法中，需要匹配至少3幅视图。

注意，特征可能是点或者线。通常，在无结构的场景中由于缺乏直线，就在视觉里程计中使用点特征。一个深入分析这三种点和线特征方法的论文，在参考论文74中可见。本文只讲解点特征的推导。

2D到2D：图像特征匹配中的运动

估计本征矩阵

校准过的相机图像Ik和Ik-1之间的几何关系可以通过本征矩阵E表示。E中包含了相机运动参数具有一个未知的平移变换因子：





图4.对极约束示意图



用对极约束计算2D到2D特征匹配的本征矩阵。最小化方案是采用5个2D到2D的对应关系，如论文75，Nister的论文76提供了一种实现方法。Nister的5点算法成为标准的2D到2D的运动估计方法，包括对离群点的处理。对于n>=8的非共面点的一种简单直接的计算方法是Longuet-Higgins的8点算法，如论文2中总结。每个特征匹配都给出一个约束：



8点法得到的这些约束进行排列可以得到一个线性方程组AE=0，通过解方程，就可以计算出E的值。这个齐次方程组可以很容易通过奇异值分解（SVD）计算，如论文2。如果超过8个点会产生超定方程组（方程个数大于未知量个数的方程组），如果超定方程组给定的条件（限制）过于严格，导致解不存在，就用最小二乘法进行拟合，求出最小二乘解，该方法对噪声具有鲁棒性。矩阵A的奇异值分解（SVD）形式为A=USVT，E的最小二乘估计为||E||=1可以用V的最后一行查找。然而，E的线性估计并不满足本征矩阵的特征约束，它是旋转矩阵R和反对称平移t的乘积。在本征矩阵的奇异值里面可以看到这些约束。经过奇异值分解（SVD）分解后的本征矩阵是E=USVT，提取主对角线上的值diag(S)={s,s,0}，第一个和第二个奇异值相等，第三个是0。为了得到满足约束的有效的本征矩阵E，需要将它投影到具有有效的本征矩阵的空间中去。投影本征矩阵为

。
当3维空间中点共面时，8点算法方案退化。相应的，5点算法则可以应用于计算共面点。最后，8点算法对校准过的（透视或全景）相机和没有校准的相机都可适用，5点算法只适用于校准过的（透视或全景）相机。

从本征矩阵E提取旋转矩阵R和平移矩阵t

从计算估计出来的中，可以提取旋转和平移矩阵。通常，对同一本征矩阵有4种不同的方法求解R，t；其实，只要三角化一个点，就可以求出一对R和t。这4种方法是：



一种将E有效分解成R和t的方法在论文76中有详细介绍。

通过三角化点云选择正确的方案，选择点的时候这个点要在两个相机的正前方，用估计的R，t作为初始值，可以采用非线性优化方法对旋转和平移参数进行优化。最小化重投影误差的方程用公式10定义。

捆集调整可用于优化相机轨迹的局部估计。

计算相关尺度

为了重构图像序列的轨迹，需要将不同的变换T0:n进行链接。为了达到这个目标，两幅图像的平移绝对尺度无法计算，需要计算它们的相对尺度。图像子集变换的相对尺度是可以计算出来的。其中一种方法是三角化两个图像子集的一对图像集Xk-1和Xk的3维点。从对应的3维点坐标可以计算两个3维点之间的距离。计算图像对Xk-1和Xk之间的距离的比值r可以得出相应的尺度。



考虑到鲁棒性，计算了很多的比例因子，采用平均值，如果出现离群值就取中值。平移向量t也可以通过这个距离比值计算。相对尺度的计算要求在多个图像帧上的特征已经匹配好（或被追踪），至少是三幅图像帧。与采用外部三角化3维点不同的是，比例因子还可以通过寻找2维特征的三视图匹配之间的三焦点约束来计算，如《多视图几何》所示。

2D到2D对应关系的视觉里程计算法如算法1所示。



捆集调整可以精炼轨迹的局部估计。

3D到3D：3D结构对应中的运动

对于3D到3D的特征对应，可以通过两组3D特征的一致变换计算相机的运动Tk。3D到3D的特征对应只适用于立体视觉中。



论文77中，最小化方案采用3组3D到3D非共线对应关系，可以用于出现离群点的鲁棒估计，将在第2部分讲解。当n>=3的对应点，可能的方法，论文78，是分别用3D特征组的不同质心计算平移部分，和用奇异值分解（SVD）计算旋转部分。平移部分通过下式计算：

其中-表示算术平均值。



如果3D点的测量不确定性已知，可以通过论文17的方法添加权重。变换的计算具有绝对尺度，因此，图像序列的轨迹可以直接连接变换得到。

3D到3D对应关系的视觉里程计算法如算法2所示。



为了计算变换，也可以不用立体相机3D点的三角化，而采用4焦点约束。论文21讲解了这个方法。4焦点张量也可以直接从2D到2D的立体对应关系中计算变换。

3D到2D：3D结构和图像特征对应中的运动

Nister的论文1指出，3D到2D对应的运动估计比3D到3D对应的更精确，因为它的图像重投影误差更小（如论文10），而3D到3D的特征位置误差估计不是最小，如论文9。通过3D到2D的对应关系Xk-1和pk计算变换Tk。Xk-1可以用立体数据估计，或在单目视觉中，可以三角化图像量测pk-1和pk-2获得。然后，再使用三视图的图像对应关系。



众所周知，这个问题是n点透视问题，有很多不同的方法可以用，如论文79。论文18中，最小化方法采用3个3D到2D的对应。称之为3点透视（P3P）方法，有4种方案用1个或更多点来消除歧义。（一种优化加速P3P算法如论文80所示，C代码可以在作者的网站上下载。）在3D到2D的状况下，P3P是一种用于在离群点出现的情况下鲁棒运动估计的标准方法，如论文18所示。鲁棒估计将在第2部分详细介绍。

一种简单直接的解决n>=6点的PnP问题的方法是直接线性变换算法，如论文22所示。一个3D到2D点的对应提供了Pk=[R|t]的2个约束，如下所示。



其中，每个PjT都是一个4维向量（j表示PK的第j行），x,y,z是3D点Xk-1的三个坐标值。

讲6个点的对应关系的约束排列产生一个线性方程组为AP=0。A中包含0向量，采用SVD奇异值分解，可以计算出P。旋转和平移部分通过Pk=[R|t]提取。生成的旋转矩阵R不需要正则化。然而，R和t都可以通过论文10中重投影误差非线性优化来处理，所以问题不大。

3D到2D的运动估计采用的2D图像点来自于单目相机。这意味着对立体相机，2D图像点要么是左边的相机要么是右边的相机生成。显然，我们希望同时使用两个相机的图像点。Nister在论文81中提出了一种对于外参校准（相机间的相对位置和方向已知）的相机的一般化的3D到2D运动估计算法，可应用于非共点的射线（比如，多相机的2D图像点）。
对于单目相机，需要三角化3D图像点，用一种独特的方法从3D到2D的匹配中估计位姿。这种独特的算法常常被称为SfM。从2个视图开始，通过2D到2D特征匹配计算3D点的初始点集和第一个变换。接下来的变换用3D到2D的特征匹配计算。为了达成这种做法，特征的匹配或跟踪，需要在多个图像帧中处理（至少是3个图像帧）。当新变换计算好后，将被加入到3D特征点集中，新的3D特征会被三角化。这种方法的挑战是需要维护一个一致的、精确的三角化的3D特征，在至少3个连续图像帧上创建3D到2D的特征匹配。
3D到2D对应的视觉里程计算法总结如算法3所示。



2D到2D和3D到2D运动估计比3D到3D方法更精确。

三角化和关键帧的旋转

之前有些运动估计方法要求从2D图像对应中三角化3D点（结构）。捆集调整BundleAdjustment也需要计算运动结构，从而计算出局部轨迹更精确的估计。
从至少2幅图像帧的2D图像对应的射线反向交汇，进行3D点三角化。理想情况下，这些射线将会在一个3D点处交汇。然而，由于图像噪声，相机模型和标定误差，还有特征匹配的不确定性，它们可能不会交汇。因此，在最小二乘情况下，最小距离的点，其所有射线交汇，可以作为3D点的位置的估计。所有射线交汇的三角化的3D点的距离的标准偏差是一种理想的3D点。具有较大不确定的3维点将会被丢弃。这种情况在图像帧非常靠近被拍摄的场景时会发生。这种情况发生时，3D点表现出非常大的不确定性。避免这种问题的方法是忽略这些图像帧，直到这些3D点的不确定性减小到一个设定的阈值。这些挑选的图像帧被称为关键帧。关键帧的选择是视觉里程计中非常重要的一步，它是在更新运动之前必须完成。

视觉里程计在一致性和实时性能方面需要做取舍。

==========不忘初心 方得始终 ==========taylorguo@126.com===2017年4月20日编辑===EJU
in Shanghai===

讨论

根据Nister论文1，对于运动的计算，与3D到3D方法相比2D到2D和3D到2D的方法具有优势。对于立体相机，Nister对比了视觉里程计的性能，3D到3D的和3D到2D的情况，发现3D到2D要比3D到3D好很多。主要原因是三角化的3D点在深度方面具有更多的不确定性。3D到3D的特征对应用于运动计算，不确定性会减少运动估计的效果。事实上，3D到3D的运动估计，3D位置误差，论文9，可以在3D到2D图像重投影误差下最小化。

在单目算法中，相对3D到2D，更推荐使用2D到2D方法，因为它避免了点云的三角化。但实际应用中，3D到2D方法使用的比2D到2D方法多。主要原因是它快速的数据关联。由于需要更精确的运动估计计算，对于输入数据排除离群点非常重要。去除离群点是非常微秒的一步，它的计算时间与估计运动的最少点数密切相关。如前所述，2D到2D的计算要求最少5点对应关系（参考5点算法）；然而，在3D到2D运动估计中只有3个对应关系（参考P3P）。点数越少，运动估计计算越快。

立体相机对比单目相机的优势，除了3D特征可以在绝对尺度下直接计算之外，特征匹配可以直接从2幅视图中计算而不是单目相机算法中的3幅视图。另外，3D结构从一对立体图像对中计算得出而不是单目相机中的联立多幅图像，在小运动中，立体算法比单目算法的漂移更小。单目方法更有意思，因为立体方法可能会退化成单目，当相机到场景点的距离远远大于立体相机间的基线（两个相机间的距离）时就会出现这种情况。这种情况下，立体相机方法无效，只能使用单目方法。

无论采用哪种运动估计算法，都需要执行局部捆集调整（前m个图像上）估计更精确的轨迹。捆集调整之后，运动估计的效果就得到非常大的缓解。

结论

本文讨论了视觉里程计的历史，问题的推导，不同的运动估计方法。视觉里程计是机器人上必要的一部分，比较容易理解。第二部分将会总结视觉里程计的其他部分：如何检测和匹配图像间显著的可重复的特征，出现无效数据后的鲁棒估计，和捆集调整。另外，还有误差迭代，应用，和开源代码。

参考文献
[1]D. Nister, O. Naroditsky, and J. Bergen, “Visual odometry,” inProc.Int.Conf.
Computer Vision and Pattern Recognition,2004, pp. 652–659.
[2]H. Longuet-Higgins, “Acomputer
algorithm for reconstructing ascene from twoprojections,”Nature,vol.
293, no. 10, pp. 133–135,1981.
[3]C. Harris and J. Pike, “3d positional integration from imagesequences,” inProc.Alvey
Vision Conf.,1988, pp. 87–90.
[4]J.-M. Frahm, P. Georgel, D. Gallup, T. Johnson, R. Raguram, C.Wu,Y.-H. Jen, E. Dunn, B. Clipp,
S. Lazebnik, and M. Pollefeys,“Buildingrome on a cloudless day,” in Proc.European Conf. Computer Vision,2010,pp.
368–381.
[5]H. Moravec, “Obstacle avoidance and navigation in the real world byaseeing robot rover,” Ph.D.
dissertation, Stanford Univ., Stanford,CA, 1980.
[6]L. Matthies and S. Shafer, “Error modeling in stereonavigation,”IEEEJ.Robot.
Automat.,vol. 3, no. 3, pp. 239–248,1987.
[7]L. Matthies, “Dynamic stereo vision,” Ph.D. dissertation,CarnegieMellonUniv., Pittsburgh, PA, 1989.
[8]S. Lacroix, A. Mallet, R. Chatila, and L. Gallo, “Rover selflocalizationin planetary-like environments,”
in Proc.Int. Symp. Articial Intelligence,Robotics, and Automation for Space(i-SAIRAS),1999,
pp. 433–440.
[9]C. Olson, L. Matthies, M. Schoppers, and M. W. Maimone, “Robuststereoego-motion for long distance
navigation,” in Proc.IEEE Conf.Computer Vision and Pattern Recognition,2000,
pp. 453–458.
[10]M. Hannah, “Computer matching of areas in stereo images,”Ph.D.dissertation, Stanford Univ., Stanford,
CA, 1974.
[11]H. Moravec, “Towards automatic visual obstacle avoidance,” inProc.5thInt.
Joint Conf. Artificial Intelligence,Aug. 1977, p. 584.
[12]W. Forstner, “A feature based correspondence algorithm forimagematching,”Int.Arch.
Photogrammetry,vol. 26, no. 3, pp. 150–166,1986.
[13]C. Olson, L. Matthies, M. Schoppers, and M. Maimone, “Rovernavigationusing stereo ego-motion,”Robot.Autonom.
Syst.,vol. 43, no. 4,pp. 215–229,2003.
[14]A. Milella and R. Siegwart, “Stereo-based ego-motionestimationusing pixel tracking and iterative
closest point,” inProc.IEEE Int. Conf.Vision Systems,pp.
21–24,2006.
[15]A. Howard, “Real-time stereo visual odometry for autonomousgroundvehicles,” inProc.IEEE/RSJ
Int. Conf. Intelligent Robots and Systems,2008,pp. 3946–3952.
[16]Y. Cheng, M. W. Maimone, and L. Matthies, “Visual odometry onthemars exploration rovers,”IEEERobot.
Automat. Mag.,vol. 13, no. 2,pp. 54–62,2006.
[17]M. Maimone, Y. Cheng, and L. Matthies, “Two years of visualodometryon the mars exploration rovers:
Field reports,”J.Field Robot.,vol.
24,no. 3, pp. 169–186,2007.
[18]M. A. Fischler and R. C. Bolles, “Random sample consensus: Aparadigmfor model fitting with applications
to image analysis andautomatedcartography,”Commun.ACM,vol.
24, no. 6, pp. 381–395,1981.
[19]C. Tomasi and J. Shi, “Good features to track,” inProc.ComputerVision
and Pattern Recognition (CVPR ’94),1994, pp. 593–600.
[20]P. Besl and N.McKay, “Amethod for registration of 3-dshapes,”IEEETrans.Pattern
Anal. Machine Intell.,vol. 14, no. 2, pp. 239–256,1992.
[21]A. Comport, E. Malis, and P. Rives, “Accuratequadrifocal
trackingfor robust 3d visual odometry,”inProc.IEEE
Int. Conf. Robotics andAutomation,2007, pp. 40–45.
[22]R. Hartley and A. Zisserman,MultipleView
Geometry in ComputerVision,2nd ed. Cambridge U.K.: Cambridge Univ. Press, 2004.
[23]D. Nister, O. Naroditsky, and J. Bergen, “Visual odometry forgroundvehicle applications,”J.Field
Robot.,vol. 23, no. 1, pp. 3–20,2006.
[24]P. I. Corke, D. Strelow, and S. Singh, “Omnidirectional visualodometryfor a planetary rover,”
in Proc.IEEE/RSJ Int. Conf. Intelligent Robotsand Systems,2005,
pp. 4007–4012.
[25]M. Lhuillier, “Automatic structure and motion using acatadioptriccamera,” inProc.IEEE
Workshop Omnidirectional Vision,2005, pp. 1–8.
[26]R. Goecke, A. Asthana, N. Pettersson, and L. Petersson, “Visualvehicleegomotion estimation using
the Fourier-Mellin transform,” inProc.IEEEIntelligent Vehicles Symp.,2007,
pp. 450–455.
[27]J. Tardif, Y. Pavlidis, and K. Daniilidis, “Monocular visualodometryin urban environments using
an omnidirectional camera,” inProc.IEEE/RSJ Int. Conf. Intelligent Robots and Systems,2008,
pp. 2531–2538.
[28]M. J. Milford and G.Wyeth, “Single camera vision-only SLAM onasuburban road network,” inProc.IEEE
Int. Conf. Robotics and Automation(ICRA ’08),2008, pp. 3684–3689.
[29]D. Scaramuzza and R. Siegwart, “Appearance-guidedmonocularomnidirectional visual odometry for
outdoor groundvehicles,”IEEETrans.Robot. (Special Issue on Visual SLAM),vol.
24, no. 5, pp. 1015–1026,Oct.
2008.
[30]E. Mouragnon, M. Lhuillier, M. Dhome, F. Dekeyser, and P. Sayd,“Realtime localization and 3d reconstruction,”
in Proc.Int. Conf.Computer Vision and Pattern Recognition,2006,
pp. 363–370.
[31]D. Scaramuzza, F. Fraundorfer, and R. Siegwart, “Real-timemonocularvisual odometry for on-road
vehicles with 1-point RANSAC,”inProc.IEEE Int. Conf. Robotics and Automation (ICRA ’09),2009,pp.
4293–4299.
[32]A. Pretto, E. Menegatti, and E. Pagello, “Omnidirectionaldenselarge-scale mapping and navigation
based on meaningfultriangulation,”in Proc.IEEE Int. Conf. Robotics and Automation,2011,
pp. 3289–3296.
[33]D. Nister, “An efficient solution to the five-point relative poseproblem,”inProc.Int.
Conf. Computer Vision and Pattern Recognition,2003,pp. 195–202.
[34]M. Milford, G. Wyeth, and D. Prasser, “RatSLAM: A hippocampalmodelfor simultaneous localization
and mapping,” in Proc.IEEE Int.Conf. Robotics and Automation (ICRA ’04),2004,
pp. 403–408.
[35]B. Liang and N. Pears, “Visual navigation using planarhomographies,”inProc.IEEE
Int. Conf. Robotics and Automation (ICRA ’02),2002,pp. 205–210.
[36]Q. Ke and T. Kanade, “Transforming camera geometry to avirtualdownward-looking camera: Robust
ego-motion estimation andgroundlayerdetection,” in Proc.Computer Vision and Pattern Recognition(CVPR),June
2003, pp. 390–397.
[37]H. Wang, K. Yuan, W. Zou, and Q. Zhou, “Visual odometry basedonlocally planar ground assumption,”
in Proc.IEEE Int. Conf. InformationAcquisition,2005,
pp. 59–64.
[38]J. Guerrero, R. Martinez-Cantin, and C. Sagues, “Visualmap-lessnavigation based on homographies,”J.Robot.
Syst.,vol. 22, no. 10,pp. 569–581,2005.
[39]D. Scaramuzza, “1-point-RANSAC structure from motion forvehicle-mounted cameras by exploiting
non-holonomic constraints,”Int.J.Comput. Vis.,vol.
95, no. 1, pp. 74–85,2011.
[40]D. Scaramuzza, F. Fraundorfer, M. Pollefeys, and R.Siegwart,“Absolute scale in structure from
motion from a singlevehicle mountedcamera by exploiting nonholonomic constraints,” inProc.IEEE Int. Conf.Computer Vision (ICCV),Kyoto,
Oct. 2009, pp. 1413–1419.
[41]F. Fraundorfer, D. Scaramuzza, and M. Pollefeys, “Aconstrictedbundle adjustment parameterization
for relative scaleestimation in visualodometry,” in Proc.IEEE Int. Conf. Robotics and Automation,2010,pp.
1899–1904.
[42]N. Sunderhauf, K. Konolige, S. Lacroix, and P. Protzel,“Visualodometry using sparse bundle adjustment
on an autonomousoutdoorvehicle,” in TagungsbandAutonome Mobile Systeme, Reihe Informatikaktuell.Levi,
Schanz, Lafrenz, and Avrutin, Eds. Berlin,Springer-Verlag,2005, pp. 157–163.
[43]K. Konolige, M. Agrawal, and J. Sol, “Large scale visual odometryforrough terrain,” inProc.Int.
Symp. Robotics Research,2007.
[44]J. Tardif, M. G. M. Laverne, A. Kelly, and M. Laverne, “Anewapproach to vision-aided inertial
navigation,” in Proc.IEEE/RSJ Int. Conf.Intelligent Robots and Systems,2010,
pp. 4161–4168.
[45]A. I. Mourikis and S. Roumeliotis, “A multi-state constraintkalmanfilter for vision-aided inertial
navigation,” in Proc.IEEE Int. Conf. Roboticsand Automation,2007,
pp. 3565–3572.
[46]E. Jones and S. Soatto, “Visual-inertial navigation, mappingandlocalization: A scalable real-time
causal approach,”Int.J. Robot. Res.,vol.30,
no. 4, pp. 407–430,2010.
[47]H. Durrant-Whyte and T. Bailey, “Simultaneouslocalization
andmapping (SLAM): Part I. The essentialalgorithms,”Robot.Automat.Mag.,vol.
13, no. 2, pp. 99–110,2006.
[48]T. Bailey and H. Durrant-Whyte, “Simultaneouslocalisation
andmapping (SLAM): Part II. State of the art,”Robot.Automat.
Mag.,vol. 13,no. 3, pp. 108–117,2006.
[49]A. Davison, “Real-timesimultaneous
localisation and mapping witha single camera,”inProc.Int.
Conf. Computer Vision,2003, pp. 1403–1410.
[50]G. Klein and D. Murray, “Paralleltracking
and mapping for small arworkspaces,”inProc.Int.
Symp. Mixed and Augmented Reality,2007,pp. 225–234.
[51]H. Strasdat, J. Montiel, and A. Davison, “Realtime
monocularSLAM: Why filter?”inProc.IEEE
Int. Conf. Robotics and Automation,2010,pp. 2657–2664.
[52]A. Chiuso, P. Favaro, H. Jin, and S. Soatto, “3-D motion andstructurefrom 2-D motion causally
integrated over time:Implementation,” inProc.European Conf. Computer Vision,2000,
pp. 734–750.
[53]M. C. Deans, “Bearing-only localization and mapping,” Ph.D.dissertation,Carnegie Mellon Univ.,
Pittsburgh, 2002.
[54]L. A. Clemente, A. J. Davison, I. Reid, J. Neira, and J. D.Tardos,“Mapping large loops with a
single hand-held camera,” inProc.RoboticsScience and Systems,2007.
[55]T. Lemaire and S. Lacroix, “Vision-based SLAM: Stereo andmonocularapproaches,”Int.J.
Computer Vision,vol. 74, no. 3, pp. 343–364,2006.
[56]E. Eade and T. Drummond, “Monocular SLAM as a graph ofcoalescedobservations,” inProc.IEEE
Int. Conf. Computer Vision,2007,pp. 1–8.
[57]G. Klein and D. Murray, “Improving the agility ofkeyframe-basedSLAM,” inProc.European
Conf. Computer Vision,2008, pp. 802–815.
[58]K. Konolige and M. Agrawal, “FrameSLAM:From
bundle adjustmentto real-time visual mappping,”IEEETrans.
Robot.,vol. 24, no. 5,pp. 1066–1077,2008.
[59]A. Handa, M. Chli, H. Strasdat, and A. J. Davison, “Scalableactivematching,” inProc.Int.
Conf. Computer Vision and Pattern Recognition,2010,pp. 1546–1553.
[60]J. Civera, O. Grasa, A. Davison, and J. Montiel, “1-point RANSACforekf filtering: Application
to real-time structure from motionandvisual odometry,”J.Field Robot.,vol.
27, no. 5, pp. 609–631,2010.
[61]H. Strasdat, J. Montiel, and A. J. Davison, “Scale drift-awarelargescale monocular SLAM,” inProc.Robotics
Science and Systems,2010.
[62]C. Mei, G. Sibley, M. Cummins, P. Newman, and I. Reid, “RSLAM:Asystem for large-scale mapping
in constant-time using stereo,”Int.J.Computer Vision,vol.
94, no. 2, pp. 198–214,2010.
[63]Y. Ma, S. Soatto, J. Kosecka, and S. Sastry,AnInvitation
to 3DVision, from Images to Models.Berlin: Springer-Verlag, 2003.
[64]C. Geyer and K. Daniilidis, “A unifying theory for centralpanoramicsystems and practical applications,”
in Proc.European Conf. ComputerVision,2000,
pp. 445–461.
[65]D. Scaramuzza, A. Martinelli, and R. Siegwart, “A flexibletechniquefor accurate omnidirectional
camera calibration andstructure frommotion,” in Proc.IEEE Int. Conf. Computer Vision Systems (ICVS) 2006,Jan.2006,
pp. 45–53.
[66]D. Scaramuzza, “Omnidirectional vision: From calibration torobotmotion estimation” Ph.D. dissertation,
ETH Zurich, 2008.
[67]D. Scaramuzza, “Omnidirectional camera,” inEncyclopediaof
ComputerVision,K. Ikeuchi, Ed. Berlin: Springer-Verlag, 2012.
[68]J. Bouguet (2011). List of camera calibration toolboxes.[Online].Available:http://www.vision.caltech.edu/bouguetj/calib.doc/htmls/links.html
[69]J. Bouguet. Camera calibration toolbox for Matlab.[Online].Available:http://www.vision.caltech.edu/bouguetj/calib.doc/index.html
[70]D. Scaramuzza. (2006). Ocamcalib toolbox: Omnidirectionalcameracalibration toolbox for Matlab
(uses planar grids) google for“ocamcalib.”[Online].Available: https://sites.google.com/site/scarabotix/ocamcalibtoolbox [71]C. Mei. (2006). Omnidirectional camera calibration toolbox forMatlab(uses planar grids). [Online].
Available:http://homepages.laas.fr/~cmei/index.php/Toolbox
[72]J. Barreto. Omnidirectional camera calibration toolbox forMatlab(uses lines). [Online]. Available:http://www.isr.uc.pt/~jpbar/CatPack/pag1.htm
[73]Opencv: Open-source computer vision library. [Online].Available:http://opencv.willowgarage.com/wiki/
[74]T. Huang and A. Netravalli, “Motionand
structure from feature correspondences: A review,”Proc.IEEE,vol.
82, no. 2, pp. 252–268,1994.
[75]E. Kruppa, “Zur ermittlung eines objektes aus zwei perspektivenmitinnerer orientierung,”Sitzungsberichteder
Akademie der Wissenschaften,Wien,Mathematisch-Naturwissenschaftlichen Klasse, Abteilung IIa,vol.122, pp. 1939–1948,1913.
[76]D. Nister, “Anefficient
solution to the five-point relative pose problem,”inProc.Computer
Vision and Pattern Recognition (CVPR ’03),2003,pp. II: 195–202.
[77]H. Goldstein,ClassicalMechanics.Reading,
MA:Addison-Wesley, 1981.
[78]K. S. Arun, T. S. Huang, and S. D. Blostein, “Least-squares fittingoftwo 3-d point sets,”IEEETrans.
Pattern Anal. Machine Intell.,vol. 9, no. 5,pp. 698–700,1987.
[79]F. Moreno-Noguer, V. Lepetit, and P. Fua, “Accuratenon-iterativeO(n)
solution to the PnP problem,”inProc.IEEE
Int. Conf. ComputerVision,2007, pp. 1–8.
[80]L. Kneip, D. Scaramuzza, and R. Siegwart, “A novelparameterizationof the perspective-three-point
problem for a directcomputation of absolutecamera position and orientation,” in
Proc.IEEE Conf. ComputerVision and Pattern Recognition,2011,
pp. 2969–2976.
[81]D. Nister, “A minimal solution to the generalised 3-point poseproblem,”inProc.IEEE
Conf. Computer Vision and Pattern Recognition,2004,pp. 560–567.
[82]C. Harris and M. Stephens, “Acombined
corner and edge detector,”inProc.Alvey
Vision Conf.,1988, pp. 147–151.