本发明涉及一种slam初始化方法。
背景技术：
slam(simultaneouslocalizationandmapping，即时定位与地图构建)指的是机器人在自身位置不确定的条件下，在完全未知环境中创建地图，同时利用地图进行自主定位和导航。基于视觉传感器定位方式，是近些年国内外研究的一个热点，它又分为单目、双目和多目定位。由于单目slam一次观测不能获得相对于环境特征的全部信息，只能获得方向信息，距离信息无法提取，是一种仅有方向信息的方法，所以建立特征地图存在特征初始化方法，特征初始化就是对特征的初始位置进行估计，获得估计的深度信息。目前的单目特征slam初始化方法有ptam(paralleltrackingandmapping，同步追踪与构图)、orb-slam(areal-timeaccuratemonocularslamsystembasedonorbfeatures，基于orb特征点的slam系统)等。
ptam标准代码中，地图初始化是手动的。操作方法是手持摄像头运动一定的角度之后按一个键，得到两个关键帧，一个是初始帧，即最初的关键帧；另一个是结束帧，即运动一定角度后的关键帧。从初始帧到结束帧之间，会使用esm(efficientsecond-orderminimization，高效的二阶最小化)预测，ssd(sumofsquareddifference，差的平方和相关法)模板跟踪的方式在帧与帧之间进行关键点的匹配，最后ptam使用这两个关键帧进行地图的初始化。
orb-slam地图初始化常见的方法有三种：1、追踪一个已知物体。单帧图像的每一个点都对应于空间的一条射线。通过不同角度不同位置扫描同一个物体，期望能够将三维点的不确定性缩小到可接受的范围；2、基于假设空间存在一个平面物体，选取两帧不同位置的图像，通过计算homography来估计位姿。这类方法在视差较小或者平面上的点靠近某个主点时效果不好；3、根据两帧之间的特征点匹配计算fundamentalmatrix，进一步估计位姿。这种方法要求存在不共面的特征点。
然而上述方法都需要人为或算法来选取两帧进行初始化，估计摄像机姿态并重建出初始的三维点云。初始化的计算量非常大，导致初始化过程很慢，并且因为要重建三维点云，由于距离的问题，会受到景深的限制。
技术实现要素：
为了克服现有ptam、orb-slam等单目特征slam方法的初始化速度慢、计算量大、受景深限制的不足，本发明提供一种速度较快、计算量较小、不受景深限制的基于特征的单目slam快速初始化方法，通过提取第一帧图片的orb特征点，来进行初始化，使得单目slam初始化速度大大提高，远景特征提取更为精确。
本发明解决其技术问题所采用的技术方案是：
一种基于特征的单目slam快速初始化方法，所述方法包括以下步骤：
1)开始slam并获取第一帧图片，对图片提取orb特征点p(x,y)；
2)对p(x,y)进行图像去畸；
3)对步骤2)中的去畸特征点的图像坐标进行归一化，得到(pw_x,pw_y)；
4)为步骤3)中的每个特征点构建随机深度pw_z；
5)将步骤3)、步骤4)的结果组合，构建每个特征点对应的地图点(pw_x,pw_y,pw_z)，获取初始地图；
6)将后续相邻帧的匹配结果与现有地图进行优化，为接下去新增的每一帧执行正常的特征slam的流程，不断调整扩充地图，实现slam的连续跟踪。
进一步，所述步骤6)中，slam的流程如下：
6.1)初始化关键帧序列；
6.2)对于新来的一帧i，计算关键帧序列中最后一帧与i的运动，并估计该运动的大小e，有以下几种可能性：
6.2.1)若e>eerror，说明运动太大，可能是计算错误，丢弃该帧；
6.2.2)若没有匹配上，说明该帧图像质量不高，丢弃；
6.2.3)若e<ekey，说明离前一个关键帧很近，同样丢弃；
6.2.4)剩下的情况，只有是特征匹配成功，运动估计正确，同时又离上一个关键帧有一定距离，则把i作为新的关键帧，进入回环检测程序；
6.3)近距离回环：匹配i与关键帧序列末尾m个关键帧，匹配成功时，在图里增加一条边；
6.4)随机回环：随机在关键帧序列里取n个帧，与i进行匹配。若匹配上，在图里增加一条边；
6.5)将i放入关键帧序列末尾，若有新的数据，则返回6.2)；若无，则进行优化与地图拼接。
再进一步，所述步骤2)中，去畸过程为：
2.1)径向畸变校正建模如下：
其中，(x,y)为畸变点在成像仪上的原始位置，(x',y')为矫正后的新位置，标定中，k1,k2,k3为畸变内参；
2.2)切向畸变校正模型如下：
标定中，k1,k2为畸变内参。
更进一步，所述步骤3)中，归一化过程如下：
首先利用图像中对仿射变换具有不变性的矩来确定变换函数的参数，然后利用此参数确定的变换函数把原始图像变换为一个标准形式的图像；坐标归一化公式如下：
其中，(cx,cy)为主点，(fx,fy)为焦距。
所述步骤4)中，所述随机深度满足0.5-1.5的正态分布normal(1,0.125)，正态分布公式如下：
其中，μ为平均数，σ为标准差。
本发明的有益效果主要表现在：只需选取单帧进行初始化，使得整个初始化过程不必重建三维点云，即不再需要几何计算，故而大大减少了计算量，加快了初始化的速度，且不再受到景深的影响。
具体实施方式
下面对本发明作进一步描述。
一种基于特征的单目slam快速初始化方法，包括以下步骤：
1)、开始slam并获取第一帧图片，对图片提取orb特征点p(x,y)。
orb(orientedfastandrotatedbrief，面向快速旋转不变性算法)是一种快速特征点提取和描述的算法，具有旋转不变性的同时具有较好的抗噪能力，特征点的主方向是通过矩(moment)计算得来，也就是说通过矩来计算特征点以r为半径范围内的质心，特征点坐标到质心形成一个向量作为该特征点的方向。
2)对p(x,y)进行图像去畸
摄像机成像的过程实质上是几个坐标系的转换，首先空间中的一点由世界坐标系转换到摄像机坐标系，然后再将其投影到成像平面(图像物理坐标系)，最后再将成像平面上的数据转换到图像平面(图像像素坐标系)。
图像像素坐标系(xoy坐标系)下的无畸变坐标(x,y)，经过径向畸变和切向畸变后落在了xoy坐标系的(x",y")上。即就是说，真实图像imgp与畸变图像imgr之间的关系为:imgp(x,y)＝imgr(x",y")。
2.1)径向畸变校正建模如下：
(x,y)为畸变点在成像仪上的原始位置，(x',y')为矫正后的新位置，标定中，k1,k2,k3为畸变内参。
2.2)切向畸变校正模型如下：
标定中，k1,k2为畸变内参。
3)对步骤2)中的去畸特征点的图像坐标进行归一化
归一化就是通过一系列变换(即利用图像的不变矩寻找一组参数使其能够消除其他变换函数对图像变换的影响)，将待处理的原始图像转换成相应的唯一标准形式(该标准形式图像对平移、旋转、缩放等仿射变换具有不变特性)。
基于矩的图像归一化技术基本工作原理为：首先利用图像中对仿射变换具有不变性的矩来确定变换函数的参数，然后利用此参数确定的变换函数把原始图像变换为一个标准形式的图像。其坐标归一化公式如下：
其中(cx,cy)为主点，(fx,fy)为焦距；
4)对步骤3)中的每个特征点构建随机深度pw_z，该随机深度满足0.5-1.5的正态分布normal(1,0.125)。正态分布公式如下：
其中μ为平均数，σ为标准差。
5)将步骤3)、步骤4)的结果组合，构建每个特征点对应的地图点(pw_x,pw_y,pw_z)，获取初始地图；
6)在初始地图的基础上，将后续相邻帧的匹配结果与现有地图进行优化，剔除错误点，增加新的点，不断调整扩充地图，实现slam的连续跟踪。
slam的流程如下：
6.1)初始化关键帧序列。
6.2)对于新来的一帧i，计算关键帧序列中最后一帧与i的运动，并估计该运动的大小e，有以下几种可能性：
6.2.1)若e>eerror，说明运动太大，可能是计算错误，丢弃该帧；
6.2.2)若没有匹配上(match太少)，说明该帧图像质量不高，丢弃；
6.2.3)若e<ekey，说明离前一个关键帧很近，同样丢弃；
6.2.4)剩下的情况，只有是特征匹配成功，运动估计正确，同时又离上一个关键帧有一定距离，则把i作为新的关键帧，进入回环检测程序。
6.3)近距离回环：匹配i与关键帧序列末尾m个关键帧。匹配成功时，在图里增加一条边。
6.4)随机回环：随机在关键帧序列里取n个帧，与i进行匹配。若匹配上，在图里增加一条边。
6.5)将i放入关键帧序列末尾。若有新的数据，则返回6.2)；若无，则进行优化与地图拼接。