一种单目SLAM初始化方法与流程

本发明属于计算机视觉、图像处理技术领域，尤其涉及一种单目slam初始化方法。

背景技术：

同时定位与地图创建也被称为slam(simultaneouslocalizationandmapping)，简单来说就是机器人在自身位置不确定的情况下，探索静态的未知环境，并且同时建立地图和定位的技术。它的应用平台也十分广泛，从家用的扫地机器人到道路上行驶的无人车，都离不开slam技术的支持。由于单目相机廉价的特点，使得单目slam的开发非常火热。

单目相机无法获得图像中场景的深度信息，因此单目slam有一个初始化的过程来恢复场景，但是这样恢复出来的场景仍然存在一个尺度问题，即不知道场景深度的单位。现有单目slam的初始化方法主要都是通过多帧形成视差，然后用三角化的方法初始化，但是这种初始化方法耗时久，并且需要一段平移相机的操作，不适合新手用户使用。目前存在的一些快速初始化的方法大致可分为两类：一类使用先验信息，一类不使用先验信息。不使用先验信息的快速初始化方法不能恢复场景的尺度，建立的地图大多不够准确，并且初始化也不是很快速。相对来说使用先验信息的方法可以得到一个更加好的初始化效果。

现有技术中的单目slam初始化普遍存在初始化耗时久，建立地图精度不够高，无法计算场景真实尺度的问题，需要进一步改进。

技术实现要素：

本发明的目的是提供一种单目slam初始化方法，克服传统的单目slam初始化耗时久，建立地图精度不够高，无法计算场景真实尺度的问题。

为了实现上述目的，本发明技术方案如下：

一种单目slam初始化方法，所述单目slam初始化方法，包括：

加载模板图片，提取模板图片orb特征点；

将该模板图片打印出来作为模板，测量模板的实际大小，建立自定义坐标系，利用模板上定义的已知点计算相机与模板图片的位姿，保存模板图片的特征点、相机与模板图片的位姿到配置文件；

读取配置文件，采集含有模板的图片作为第一帧图片，提取第一帧图片的orb特征点并且与模板图片上的特征点进行匹配，根据匹配结果计算单应矩阵；

剔除特征点匹配中的误匹配点，利用保留下来的特征点再次计算单应矩阵；

将模板图片中定义的已知点利用单应矩阵投影到第一帧图片中，计算第一帧图片相对于模板图片的位姿；

根据建立的自定义坐标系，计算出模板图片上与第一帧图片匹配的特征点中保留下来的特征点的空间坐标，作为slam地图中对应的地图点，完成初始化。

进一步地，所述建立自定义坐标系，是以模板中心点为原点，水平向右为x轴，水平向下为y轴。

进一步地，所述根据建立的自定义坐标系，计算出模板图片上与第一帧图片匹配的特征点中保留下来的特征点的空间坐标，计算公式如下：

其中，x与y分别为特征点的像素坐标，realx、realy与realz为特征点的空间坐标，col列与row行为模板图片的像素大小，x与y为打印出来的模板图片实际尺寸的一半。

进一步地，所述剔除特征点匹配中的误匹配点，包括：

利用上一步骤计算出的单应矩阵将模板图片上的特征点投影到第一帧图片上，计算第一帧图片上的特征点与投影过来的特征点之间的误差，误差大于阈值就标记为误匹配点。

进一步地，所述加载模板图片，提取模板图片orb特征点，还包括：

使用已经标定好的相机内参去矫正特征点的位置；

所述使用已经标定好的相机内参去矫正特征点的位置，包括：

采用如下公式进行径向畸变纠正：

其中，x，y是原始的像素坐标，x'和y'是通过径向畸变参数k1、k2矫正之后的像素坐标，并且r²＝x²+y²；

采用如下公式进行切向畸变矫正：

其中，x"和y"代表了最终的矫正后的像素坐标，p1和p2是切向畸变矫正参数。

本发明提出的一种单目slam初始化方法，通过将模板图片的特征点、相机与模板图片的位姿到配置文件，然后采集含有模板的图片作为第一帧图片，提取第一帧图片的orb特征点并且与模板图片上的特征点进行匹配，从而计算出单应矩阵，然后将模板图片中定义的已知点利用单应矩阵投影到第一帧图片中，计算第一帧图片相对于模板图片的位姿，最后根据建立的自定义坐标系，计算出模板图片上与第一帧图片匹配的特征点中保留下来的特征点的空间坐标，作为slam地图中对应的地图点，完成初始化。本发明提出的快速初始化方法比现存的方法拥有更高的精度和更快的初始化速度，极大的减小初始化的计算代价。精度体现在初始化之后建立的地图点的准确性上，速度体现在耗时上。

附图说明

图1为本发明单目slam初始化方法流程图。

具体实施方式

下面结合附图和实施例对本发明技术方案做进一步详细说明，以下实施例不构成对本发明的限定。

图1示出了一种单目slam初始化方法的一种实施例，包括：

步骤s1、加载模板图片，提取模板图片orb特征点。

本实施例事先准备一张模板图片，该模板图片是一张有纹理的已知图片，将该模板图片打印出来作为模板备用。首先提取模板图片orb特征点，orb是一种计算速度比较快的特征点计算方法，它采用了fast角点检测和brief描述子，并且采用图像金字塔来保持尺度不变性，采用灰度质心法来保持旋转不变性，是一种可实时计算的鲁棒的特征。

提取orb特征时，首先确定需要提取的特征点的数量，并且将图像分为若干小块，每一小块提取等量的特征点的方式来避免图片某些区域特征点聚集而图片的另外一些区域几乎没有特征点的情况。又考虑到尺度带来的特征点匹配的问题，本实施例采用图像金字塔来解决这个问题，图像金字塔的底层比上层的分辨率高，因此提取的特征理应也多一些。本实施例采用等比数列的方式来分配每一层应该提取的特征点的数量。每一层的特征点数量由公式(1)计算得出：

其中，allocation代表图像金字塔每一层分配到的特征点数目，nfeatures代表事先确定的需要提取的特征点的数量，factor代表图像金字塔的缩放因子，n代表图像金字塔的第几层。

由于相机镜头上的透镜的存在，会使得光线投影到成像平面的过程中会产生畸变。径向畸变和切向畸变是产生误差的两个主要的来源，需要矫正他们。

本实施例所述加载模板图片，提取模板图片orb特征点，还包括：

使用已经标定好的相机内参去矫正特征点的位置。

本实施例采用如下的公式进行纠正，其中径向畸变纠正采用如下公式：

这里的x，y是原始的像素坐标，x'和y'是通过径向畸变参数k1、k2矫正之后的像素坐标，并且r²＝x²+y²。

切向畸变矫正使用如下公式：

这里的x"和y"代表了最终的矫正后的像素坐标，p1和p2是切向畸变矫正参数。

其中，径向畸变参数和切向畸变矫正参数是相机内参，这里不再赘述。

步骤s2、将该模板图片打印出来作为模板，测量模板的实际大小，建立自定义坐标系，利用模板上定义的已知点计算相机与模板图片的位姿，保存模板图片的特征点、相机与模板图片的位姿到配置文件。

首先使用测量工具测量打印出来的模板得到模板的实际尺寸，选取模板上的一个点作为原点，定义坐标系，然后定义模板上若干个已知点的3d坐标。例如以模板中心点为原点(0,0,0)，水平向右为x轴，水平向下为y轴。则根据模板的实际尺寸，可得模板矩形边框的四个顶点的3d坐标。又因为已知内存中模板图片的像素大小，因此可以将图像2d坐标(像素坐标)与其3d坐标相对应，使用p3p算法即可获得相机与模板图片的位姿。

以上两步都是初始化预处理过程，在slam运行之前提前计算好，因为模板图片的特征点、相机与模板图片的位姿都是固定的，因此将它保存到配置文件，减少每次初始化的计算代价。

步骤s3、读取配置文件，采集含有模板的图片作为第一帧图片，提取第一帧图片的orb特征点并且与模板图片上的特征点进行匹配，根据匹配结果计算单应矩阵。

读取配置文件，直接恢复模板图片上的关键信息。然后开始slam的初始化。采集含有模板的一帧图片，作为第一帧图片，对该图片进行orb特征的提取。然后将从配置文件中读取的特征点与从当前图片上提取的特征点进行特征匹配。

特征匹配是视觉slam中极为关键的一步，宽泛地说，特征匹配解决了slam中的数据关联问题，即确定当前看到的路标与之前看到的路标之间的对应关系。特征匹配即比较特征点之间描述子的距离，距离越小表示两个特征越相似，本实施例采用海明距离来确定两个特征之间的距离。

由于模板图片是一个平面，也就是说本实施例是在一个平面上进行初始化，单应矩阵描述了两个平面之间的映射关系，通常用来描述处于共同平面上的一些点在两张图像之间的变换关系。单应矩阵计算方式如下：

假设在图像i1和i2上有一对匹配的特征点p1，p2。设m0(x0,y0,z0)是平面上一个确定的点，p(x,y,z)是平面上任一点，向量n(a,b,c)是平面法向，与平面垂直。得特征点所在平面方程为:

a(x-x0)+b(y-y0)+c(z-z0)＝0(10)

化简得：

ax+by+cz+(-ax0-by0-cz0)＝0(11)

令(-ax0-by0-cz0)＝d得：

n^tp+d＝0(12)

稍加整理得：

又根据相机归一化平面上的点到平面图像的投影公式：

p＝k(rp+t)(14)

可得：

将记为h，得p2＝hp1。展开得：

可以看出来h矩阵是一个3*3的矩阵，令h9＝1，于是h矩阵存在8个未知数，只要知道4对匹配点即可求解出h矩阵。如果匹配点大于4对则使用svd分解来求解这个超定方程。

由于特征点的配对未必准确，不准确的匹配会导致计算出来的单应矩阵不准确，因此本实施例采用随机采样一致性(ransac)算法来迭代求解h矩阵，保留一个响应最好的解。

步骤s4、剔除特征点匹配中的误匹配点，利用保留下来的特征点再次计算单应矩阵。

由于计算出的单应矩阵越准确，算法的精度也就越高，因此这里对单应矩阵进行优化。首先使用第3步中计算出来的单应矩阵去剔除特征点匹配中的误匹配点。

具体方法为将模板图片上的特征点用第3步中计算出来的单应矩阵投影到第一帧图片上，计算第一帧图片上的特征点与投影过来的特征点之间的误差，误差大于阈值就标记为外点(即误匹配点)。剔除特征点匹配中的误匹配点，剩下的特征点也称为内点。

对保留下来的内点再次使用ransac算法计算单应矩阵，可得到一个高精度的单应矩阵。

步骤s5、将模板图片中定义的已知点利用单应矩阵投影到第一帧图片中，计算第一帧图片相对于模板图片的位姿。

将模板图片上的已知空间坐标的点利用单应矩阵投影到第一帧图片中，利用p3p算法计算得出slam初始化第一帧图片相对于模板图片的位姿。

步骤s6、根据建立的自定义坐标系，计算出模板图片上与第一帧图片匹配的特征点中保留下来的特征点的空间坐标，作为slam地图中对应的地图点，完成初始化。

本实施例根据事先已经定义的坐标系，并且初始化是在模板所在平面上进行的，因此可以计算出模板图片上与第一帧图片匹配的特征点中保留下来的特征点的空间坐标，作为slam地图中对应的地图点。本实施例在自定义坐标系为以模板图片的中心为原点，水平向右为x轴，水平向下为y轴，则计算公式如下：

其中x与y分别为特征点的像素坐标，realx、realy与realz为特征点的空间坐标，col列与row行为模板图片的像素大小，x与y为打印出来的模板图片实际尺寸的一半。

容易理解的是，如果建立的自定义坐标系不同，例如可以一个顶点为原点来建立坐标系，则公式17会不同，该公式可以根据数学几何原理进行推导，这里不再赘述。

至此，本技术方案获得了相机与模板图片的位姿、第一帧图片相对于模板图片的位姿以及地图点，完成了单目相机slam初始化。

以上实施例仅用以说明本发明的技术方案而非对其进行限制，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。