[发明专利]一种基于曼哈顿世界假设及因子图的相机位姿优化方法

权利要求书

1.一种基于曼哈顿世界假设及因子图的相机位姿优化方法，用于弱纹理大场景的SLAM稳健定位，其特征在于，包括如下步骤：

(1)提取点、线、面特征；

(2)计算点、线、面特征的重投影误差；

(3)根据曼哈顿世界假设计算线特征和面特征的结构性误差；

(4)利用重投影误差和结构性误差构建因子图模型并进行位姿优化。

2.根据权利要求1所述的一种基于曼哈顿世界假设及因子图的相机位姿优化方法，其特征在于，所述步骤(1)中，提取点、线、面特征，具体步骤为：

(1.1)根据灰度图提取图像的ORB特征点；

(1.2)根据深度图对点云进行分割后得到平面特征，提取平面边缘得到线特征。

3.根据权利要求1所述的一种基于曼哈顿世界假设及因子图的相机位姿优化方法，其特征在于，所述步骤(2)中，计算点、线、面特征的重投影误差，具体步骤为：

(2.1)根据匹配关系得到点特征的观测值，计算所有点特征的重投影误差；

(2.2)根据匹配关系得到平面特征的观测值，计算所有平面特征的重投影误差E_π1：

其中q(·)表示参数降维变换，π_ij为第j个平面在第i个时刻的观测值，为世界坐标系下的第j个平面，为第i个世界坐标系到相机坐标系的变换矩阵；

(2.3)根据匹配关系得到线特征的观测值计算所有线特征的重投影误差E_l1：

其中s(·)代表参数降维变换，l_ij为第j个平面在第i个时刻的观测值，为世界坐标系下的第j个平面，为第i个世界坐标系到相机坐标系的变换矩阵。

4.根据权利要求1所述的一种基于曼哈顿世界假设及因子图的相机位姿优化方法，其特征在于，所述步骤(3)中，根据曼哈顿世界假设计算线特征和面特征的结构性误差，具体步骤为：

(3.1)计算面特征的结构性误差E_π2，根据曼哈顿世界假设，面特征之间存在平行约束e_πp和垂直约束e_πv，计算所有相关面特征之间的结构性误差：

其中q_n(·)为q(·)的前两维，表示对平面法向量的参数降维变换，n_ij是第j个平面的法向量在i时刻的观测值，是第j个平面的法向量在世界坐标系的值，是i时刻世界坐标系到相机坐标系的旋转矩阵，R_tg是将法向量n_ij旋转到其垂直方向的附加旋转矩阵；

(3.2)计算线特征的结构性误差E_l2，根据曼哈顿世界假设，线特征之间存在平行约束e_lp和垂直约束e_lv，计算所有相关线特征之间的结构误差：

其中s_n(·)为s(·)的前两维，表示对线的方向向量的参数降维变换，n_ij是第j条线的方向向量在i时刻的观测值，是第j条线的方向向量在世界坐标系的值，是i时刻世界坐标系到相机坐标系的旋转矩阵，R_tg是将方向向量n_ij旋转到其垂直方向的附加旋转矩阵。

5.根据权利要求1所述的一种基于曼哈顿世界假设及因子图的相机位姿优化方法，其特征在于，所述步骤(4)中，利用重投影误差和结构性误差构建因子图模型并进行位姿优化，具体步骤为：

(4.1)利用重投影误差和结构性误差构建因子图模型，总误差为：

T_cw＝argmin(∑H_p(E_p)+∑H_π1(E_π1)+∑H_π2(E_π2)+∑H_l1(E_l1)+∑H₁₂(E_l2))

其中H(·)是Huber核函数，E_p、E_π1、E_l1、E_π2、E_l2是步骤(2)(3)中的误差项；

(4.2)在跟踪期间进行局部BA，通过减小重投影误差和结构性误差优化相机位姿；

(4.3)当检测到系统存在回环时，即到达以前经过的地方，停止局部BA转而执行全局BA，所有的相机位姿和点、线、平面路标都会根据当前的观测结果进行修正，消除累计误差，并更新关键帧位姿和全局地图。