[发明专利]一种同步定位与地图构建方法和系统

权利要求书

1.一种同步定位与地图构建方法，其特征在于，包括：

检测相邻帧图像的ORB特征，对相邻帧图像的ORB特征进行匹配；

根据相邻帧图像间特征点的对应关系，计算出本质矩阵；

根据所述本质矩阵计算出相邻帧图像的位姿变化关系；

获取相邻帧图像中第一帧图像上像素点的三维坐标，利用所述位姿变化关系，将第一帧图像对应图像平面上的像素点重投影到相邻帧图像中第二帧图像对应的图像平面上；优化位姿变化关系的光度误差；

根据每两帧相邻帧图像间的位姿变化关系，将每两帧相邻帧图像间的相对量进行累加，得到定位轨迹。

2.如权利要求1所述的同步定位与地图构建方法，其特征在于，所述根据相邻帧图像间特征点的对应关系，计算出本质矩阵，包括：

根据相邻帧图像间特征点的对应关系，计算若干组相邻帧图像间的本质矩阵，从若干组所述本质矩阵中确定出使用的本质矩阵。

3.如权利要求2所述的同步定位与地图构建方法，其特征在于，所述根据相邻帧图像间特征点的对应关系，计算若干组相邻帧图像间的本质矩阵采用五点算法计算。

4.如权利要求3所述的同步定位与地图构建方法，其特征在于，所述根据相邻帧图像间特征点的对应关系，计算若干组相邻帧图像间的本质矩阵，从若干组所述本质矩阵中确定出使用的本质矩阵，包括：

采用RANSAC算法进行预订次数的迭代，每次迭代中，从一组对应关系中随机采样五个点，计算出对应的本质矩阵，检查随机采样以外的像素点是否为内点；

选择各次迭代中，与检查出的最大内点数对应的本质矩阵作为使用的本质矩阵。

5.如权利要求1所述的同步定位与地图构建方法，其特征在于，还包括：在对相邻帧图像的ORB特征进行匹配过程中，若匹配的特征点总数或比例低于第一阈值，则重新对相邻帧图像的ORB特征进行检测。

6.如权利要求1或5所述的同步定位与地图构建方法，其特征在于，所述对相邻帧图像间的ORB特征进行匹配，包括：

计算图像函数f(x,y)在像素点(x,y)的梯度：

梯度方向为：

Φ(x,y)＝tan^-1(G_x/G_y)

梯度幅值为：

其中，Gx和Gy分别表示x方向和y方向的梯度；

在梯度方向和梯度幅值偏差第二阈值以内时，则判定对应的像素点匹配成功。

7.如权利要求1所述的同步定位与地图构建方法，其特征在于，在将第一帧图像对应图像平面上的像素点重投影到相邻帧图像中第二帧图像对应的图像平面上时，若ORB特征匹配失败，则将所述位姿变化关系初始化为单位矩阵。

8.如权利要求1所述的同步定位与地图构建方法，其特征在于，所述优化位姿变化关系的光度误差，包括：

其中，T_k,k-1为位姿变化关系，δI(T_k,k-1,u_i)为像素投影后的光度差，u为像素点坐标，I为灰度值。

9.如权利要求1所述的同步定位与地图构建方法，其特征在于，在得到定位轨迹之后，所述的同步定位与地图构建方法还包括：

利用词袋模型进行回环检测；

将每一组新输入的帧图像均加入到因子图模型中，每添加一组帧图像，均添加对应的位姿约束关系，并设定初始化值，构成位姿信息矩阵；

在当前已有的位姿信息矩阵中进行线性化，QR分解位姿信息矩阵，利用因子图的稀疏化处理方式，去掉因子节点之间的关系，得到更新后的位姿信息。

10.如权利要求1所述的同步定位与地图构建方法，其特征在于，在检测相邻帧图像的ORB特征前，先对相邻帧图像进行去畸变处理。