[发明专利]一种基于多鱼眼摄像机和双针孔投影模型的SLAM方法

权利要求书

1.一种基于多鱼眼摄像机和双针孔投影模型的SLAM方法，其特征在于，包括以下步骤：

步骤(1)标定摄像机内部和外部参数，其中摄像机内部参数是指标定所使用的鱼眼相机模型中涉及的所有参数，外部参数是指从摄像机坐标系到多相机系统模型中主体坐标系的刚性变换所涉及到的参数；

步骤(2)输入若干个鱼眼图像，如果系统尚未进行初始化，首先利用双针孔投影模型和双目测距原理完成多目SLAM初始化；

步骤(3)如果已经完成初始化，通过双针孔投影模型将输入图像校正为合适的双针孔透视图像，并且在每个视图的校正图像上提取特征；

步骤(4)利用运动模型或重定位估计初始的主体位姿，结合摄像机外部参数将地图点转换到每个视图进行特征跟踪和匹配；

步骤(5)固定局部地图的地图点进行多视图姿态优化，更新估计的主体姿态；

步骤(6)完成多关键帧插入、新地图点生成和局部地图更新后，联合局部地图的所有位姿和地图点进行多视图捆绑调整(bundle adjustment,BA)优化；

步骤(7)针对多视图的特点，采用双向对称的查询方式，保证正、反向经过同一场景时都能正确检测到回环，回环检测成功后进行回环校正；

在步骤(5)和步骤(6)中，对于优化任务，采用在g2o求解器中添加新的优化边，并提供解析形式的雅可比，通过整合多相机系统模型和双针孔投影模型，得到从世界点X_w到像素点x的最终方程为：

x＝KR_pcT_cbT_bwX_w；

其中，x为投影平面上的像素点，X_w为世界坐标系w下的3D点，K为针孔相机的内参矩阵，R_pc为从相机坐标系c到投影平面坐标系p的3×3旋转矩阵，T_cb为从主体坐标系b到相机坐标系c的刚性变换，T_bw为从世界坐标系w到主体坐标系b的刚性变换；

使用李代数表示刚性变换T∈SE(3)，其中为特殊欧式群SE(3)对应的李代数，φ为特殊正交群SO(3)对应的李代数，ρ为三维平移向量，上标T表示转置，并令观测误差e＝x-KR_pcT_cbT_bwX_w，则解析形式的雅可比为：

其中，x＝KX_p，X_p＝R_pcX_c，X_c＝T_cwX_w，T_cw＝T_cbT_bw，T_cw为从世界坐标系w到相机坐标系c的刚性变换，T_bw为从世界坐标系w到主体坐标系b的刚性变换，∈为T_cw对应的李代数，ξ为T_bw对应的李代数，J_ξ为观测误差e对ξ的导数，为观测误差e对X_w的导数，X_w为世界坐标系w下的3D点，X_p为投影平面坐标系p下的3D点，X_c为相机坐标系c下的3D点，为X_c的斜对称矩阵，I_3×3为3×3的单位阵，为刚性变换T_cb∈SE(3)的伴随表示，R为刚性变换T的旋转部分，τ为刚性变换T的平移部分，R_cb为刚性变换T_cb的旋转部分，R_bw为刚性变换T_bw的旋转部分。