[发明专利]基于多地图融合的位姿优化方法及位姿优化系统

权利要求书

1.一种基于多地图融合的位姿优化方法，其特征在于，所述方法包括：

分别对多个相机的广角镜头进行标定，获得所述多个相机的广角镜头的标定参数；

根据所述标定参数，对与该标定参数相对应地广角镜头的广角图像进行矫正，以获得正常视角图像；

将所述多个相机放置于世界坐标系中的相同坐标位置处，并初始化；

跟踪并获取每个所述相机运动时的正常视角图像，计算得到每个所述相机的关键帧位姿，生成关键帧位姿集合，其中，每个所述相机自所述相同坐标位置处独立运动；

对所述关键帧位姿集合中的每一个所述关键帧位姿进行优化处理，生成优化关键帧位姿集合，并根据所述优化关键帧位姿集合，生成融合地图，步骤如下：

建立下述优化模型：

其中，T_i为第i个SLAM系统的世界坐标系到标准世界坐标系的变换矩阵，R_j,t_j为每个SLAM系统中的第j个相机的关键帧位姿，P_jk表示每个SLAM系统中的第j个相机的第k个三维空间点，p′_jk表示每个SLAM系统中的第j个相机的广角图像中的第k个特征点，f(x)表示广角图像的坐标到矫正后的坐标，d(x,y)表示x与y之间的向量距离；

基于所述优化模型，利用非线性优化算法，对全部所述关键帧位姿进行优化处理，生成所述优化关键帧位姿集合，并根据所述优化关键帧位姿集合，生成所述融合地图。

2.根据权利要求1所述的基于多地图融合的位姿优化方法，其特征在于，所述根据所述标定参数，对与该标定参数相对应地广角镜头的广角图像进行矫正，以获得正常视角图像的步骤包括：

设定广角图像中的点的坐标为(x，y)，以及设定矫正后的正常视角图像中的该点的坐标为(X，Y)；

获取预设尺度z，得到向量(x，y，z)；

根据所述向量(x，y，z)以及下述公式进行矫正：

ρ＝arctan(z/norm)；

f(ρ)＝a₀+a₁ρ+a₂ρ²+a₃ρ³+......+a_nρⁿ；

u＝x*f(ρ)/norm；

v＝y*f(ρ)/norm；

其中，norm为图像坐标点L2范数，ρ为z方向角，f(ρ)为畸变矫正模型函数，n为标定的项数，a₀、a₁、a₂、a₃至a_n为标定得到的多项式f(ρ)的系数参数，u为第一计算中间值，v为第二计算中间值，c、d和e均是标定得到的仿射参数，x_c为标定的行偏移量参数，y_c为标定的列偏移量参数。

3.根据权利要求1所述的基于多地图融合的位姿优化方法，其特征在于，所述跟踪并获取每个所述相机运动时的正常视角图像，计算得到每个所述相机的关键帧位姿，生成关键帧位姿集合的步骤包括：

基于SLAM算法，对每个所述相机的正常视角图像进行处理，得到所述正常视角图像中每帧的位姿；

将全部所述正常视角图像中的全部帧的位姿与预设关键帧条件进行匹配，并获取全部与所述预设关键帧条件相匹配的帧的位姿，生成所述关键帧位姿集合。