《自主移动机器人 》课件 第6章 里程估计 3 视觉里程估计

6.4视觉里程计6.4.1视觉特征提取与匹配视觉里程计VisualOdometry(VO)指智能体（机器人、无人车等）利用所携带的一个或者多个摄像机得到的图像信息估计其自身的运动问题定义于上个世纪80年代早期，起源于火星机器人在不平整地面上行走的里程估计需要，2004年被D.Nister命名为VO，并首次应用于火星机器人上VO问题描述检测由运动所导致的图像变化来估计两帧数据之间的位姿变化量VO求解基于图像的VO利用两幅图像中所有像素的亮度信息计算量大、精确度低基于特征的VO从图像中提取醒目的可重复的特征要求两帧之间鲁棒匹配或者跟踪特征基于特征的VO求解框架基于特征的VO求解框架STEP1：特征检测图像特征：图像中有代表性的点特征点描述：关键点(Key-point)：描述特征在图像中的位置等信息描述子(Descriptor)：描述该关键点与邻域在亮度、颜色、纹理等方面的图像模式特征检测面向VO，特征应具备的特性可重复性repeatability：即大量特征应在下一幅图像中出现定位准确性localizationaccuracy（包括位置和尺度）不变性invariance:光照不变性、几何不变性（旋转、尺度、视点）独特性distinctiveness鲁棒robustness：适应噪声、压缩、模糊计算效率高computationalefficiency常用图像特征角点特征(corner)：ORB,Haris,Shi-Tomasi,FAST圆块特征(blob):SIFT,SURF,CENSURE主要特征特性比较角点特征圆块特征旋转不变性尺度不变性仿射不变性可重复性定位准确性鲁棒性计算效率ORBxxx+++++++++++++Harisxx++++++++++Shi-Tomasixx+++++++++FASTxxx++++++++++SIFTxxxx+++++++++SUFTxxxx+++++++++CENSURExxxx+++++++++++ORB特征关键点:OrientedFAST局部像素灰度变化明显的像素以像素点为中心一定半径的圆上，有连续N个点与像素灰度值差大于阈值通过构建图像金字塔和灰度质心法使特征具有尺度不变性和旋转不变性ORB特征关键点:OrientedFAST描述子:BRIEF(BinaryRobustIndependentElementaryFeature)通过0和1编码关键点附近两个像素的大小关系，对于像素p和q,如果p比q大，则取1，反之取0按概率分布生成随机取128个这样两个像素的模版，得到128维由0和1组成的向量基于特征的VO求解框架特征匹配最简单的方法：暴力搜索将第一帧图像中的所有特征与第二帧图像中的所有特征进行比较，相似度在阈值范围内的建立对应关系由于第一帧图像中的特征可能在第二帧图像中找到多个对应，因此需要进行交叉一致性验证，即第二帧图像中的所有特征与第一帧图像中的所有特征进行比较，建立对应关系，当彼此相互存在对应关系时才认为匹配存在问题：耗时，是特征数的二次方特征匹配——增加匹配约束条件区域约束：根据运动预测（特征跟踪）特征需要具备三维空间位置适用于运动偏移量和图像变化较小的情况如果要对一段长的图像序列采用跟踪策略，要对每一个特征建立仿射变形模型特征匹配——增加匹配约束条件利用立体视觉极线约束：在第二帧图像的极线附近搜索适用于运动模型一致，但特征非三维ee’基线极点特征匹配——光流法使用利用特征点周围像素块的一致性对特征进行追踪光流法利用图像序列中像素在两帧之间的相关性特征点的对应关系光度不变性假设保证像素块短时间内的一致性常用的光流使用梯度计算图像信息，如Lucas-Kanade(LK)算法基于特征的VO求解框架6.4视觉里程计6.4.2运动估计基于特征匹配的运动估计单目相机仅有2D像素坐标，根据两组2D点估计运动2D-to-2D采用对极几何方法双目/RGBD相机有距离信息，根据两组3D点估计运动3D-to-3D常用ICP方法有三维点及其在相机的投影位置3D-to-2DPnP方法2D-to-2D:对极几何

针孔相机模型对极约束

两个向量叉乘得到同时垂直于这两个向量的向量相互垂直的向量点乘为0

可得

对极约束公式

对极约束运动估计求解方法根据配对点的像素位置求出基础矩阵F或者本质矩阵E根据E或者F求出R,tE和F只相差了相机内参，内参已知，因此一般使用形式更简单的EE在不同尺度下是等价的，因此可以只需要8个匹配点对来估计，称为八点法(Eight-pointalgorithm)八点法记一对匹配点的归一化坐标为：根据对极约束有：写成向量形式为：

八点法八点方程为：由上述方程求解得到E的各元素

采用SVD方法从E分解到R,t会存在四种可能性需要把一个点代入到四种解中，检测该点在两个相机下的深度，来确定哪个是正确解关于错误匹配当匹配点对多于8时，考虑匹配错误，采用RANSAC方法进行优化点到极线的距离在阈值

范围内/图像特征射线与

极平面之间的夹角在阈

值范围内的为内点关于特殊情况

关于特殊情况

2D-to-2D:对极几何的局限性尺度不确定问题：2D特征不带有深度信息，归一化仅仅是固定尺度初始化需平移运动：为归一化深度信息需初始化，纯旋转将导致E是0，无法求解R3D-to-2D：PnP求解3D到2D点对运动的方法给出n个3D空间点及其投影位置时求解相机的位姿变化不需要使用对极约束，可以在很少的匹配点中获得较好的运动估计主要方法有：采用6个点对的直接线性变换DLT方法采用3个点对的P3P优化后的EPnP、PnP、UPnPBundleAdjustment（构建成最小二乘问题迭代求解）P3P利用三角形相似性质，求解投影点a,b,c在相机坐标系下的三维坐标,把问题转化为一个3D到3D的位姿估计问题基于特征的VO求解框架6.4视觉里程计6.4.3局部优化局部优化针对问题：特征点对位姿估计存在误差特征点对存在错误匹配帧与帧之间的误差不断累积解决方法：光束平移法（BundleAdjustment）光束平移法（BundleAdjustment）对场景中任意三维点X，由从每个视图所对应的摄像机的光心发射出来并经过图像中X对应的像素后的光线，都将交于X这一点，即对于三维点形成多个光束(bundle)光束平移法（BundleAdjustment）实际应用中由于噪声等存在，多条光线几乎不可能汇聚与一点，因此在求解过程中，需要不断对待求信息进行调整(adjustment)融合多帧信息，建立匹配特征点光束约束，通过非线性最优化求解光束平移法（BundleAdjustment）

相机位姿相机运动轨迹三维特征点相机观测

6.4视觉里程计6.4.4直接法使用像素光度的直接法直接法将位姿估计问题中的匹配与求解步骤耦合在一起图像数据关联基于光照一致性假设，认为同一场景的两次观测，图像上像素的光度是不变的直接法算法流程输入两幅图片I1，I2，设置初始的R0，t0当没有达到非线性优化的收敛条件时：定义当前R，t下光度误差函数F(R,t)遍历I1中每一个像素点pi，通过R，t投影至I2图片上的像素位置pj在F中加上这一对点的光度误差项F(R,t)+=||I2(pj)-I1(pi)||2求解以R，t为自变量的光度误差函数F的最小二乘估计F(R*,t*)得R=R*，t=t*达到收敛条件后返回R*，t*Vo存在问题应用对环境有多方面要求环境光照足够场景包含足够纹理相邻帧之间有足够的重叠内容场景静态可以避免里程估计受地面不平整的影响，但噪声和错误匹配仍会影响里程估计的准确性，并会被不断叠加6.5多信息融合里程估计6.5.1视觉惯导里程计多信息融合里程估计单一传感器由于自身特征，在特定情景或使用方式下存在缺陷：轮式里程计：地面不平、轮子打滑等情况下位姿推算不准IMU：长期位姿估计存在累积误差激光：空旷环境、狭长走廊等环境中无法准确估计位姿相机：弱纹理环境、图像过曝或过暗时无法估计位姿，单目尺度问题等融合多传感器的信息以提高里程估计的性能主要方法：EKF、MSCKF等多信息融合里程估计：

视觉惯性里程计（Visual-InertialOdometry）IMU可以为短时间内的快速运动提供较好的估计，但是IMU容易受到干扰，使得积分结果存在明显的漂移。视觉图像基本不存在漂移，但是存在尺度不确定性。视觉定位信息可以估计IMU的零偏以减少IMU由零偏导致的发散和累积误差，而IMU可以在视觉失效（快速运动、场景限制等）时提供短期的精确估计。由于IMU和视觉在特性上的互补性，以视觉与IMU融合实现的视觉惯性里程计（Visual-InertialOdometry，VIO）受到了越来越多的关注。在融合方式上，根据是否将图像特征信息加入状态向量可以将VIO框架分为松耦合和紧耦合两大类。视觉惯性里程计：VIO系统示意图

视觉惯性里程计：松耦合&紧耦合松耦合框架紧耦合框架视觉惯性里程计：VIO系统流程视觉惯性里程计：视觉惯导联合初始化

视觉惯性里程计：VIO前端数据处理VIO前端数据处理分为图像处理和IMU预积分两部分，分别求解视觉重投影误差和IMU残差。图像处理：VIO前端的图像处理是指利用相机采集到的图像信息进行初始化，然后对相机位姿进行正常追踪和丢失处理。其主要流程包括：特征点提取与匹配；利用对极几何约束计算两图像之间的位姿；根据已知的相机位姿和特征点二维坐标通过三角化得到三维坐标；利用3D点和2D特征点通过PnP求取新的相机位姿。基于以上四步，我们就可以得到尺度统一的相机位姿估计结果了。相机的位姿估计结果能够为后续和IMU融合提供视觉重投影误差。IMU预积分

IMU预积分

IMU预积分

位置、速度、姿态和偏置的预积分公式预积分量

IMU预积分

视觉惯导融合估计

6.5多信息融合里程估计6.5.2视觉激光里程估计多信息融合估计：视觉激光里程估计将视觉与激光传感器结合进行里程估计具有很好的互补性。激光传感器不受环境中光照变化、纹理缺失等因素的影响，能够直接获得环境的深度信息，而视觉传感器感知数据相对连续稠密，且不受几何约束退化的影响。常见的视觉激光里程计流程图如下：多信息融合估计：激光里程帧间估计激光里程计往往采用匀速线性模型对激光点云进行畸变修正。由于视觉传感器的帧率高于激光雷达，因此在激光两帧点云数据采集间