[发明专利]一种结合深度图推断的线特征视觉里程计方法

权利要求书

1.一种结合深度图推断的线特征视觉里程计方法，其特征是，包括如下步骤：

步骤一、利用深度相机采集某一帧的彩色图像及深度图像；

步骤二、使用线段检测算法提取彩色图像中的2D线特征，而后确定包含线特征的各矩形区域；

步骤三、将各矩形区域映射到深度图像中，针对深度缺失问题，对各矩形区域使用像素滤波方法进行深度推断；

步骤四、采样2D线特征中的点，结合深度推断后的深度图像反投影为3D点，利用随机采样一致性方法找到过直线的最佳两点拟合3D线段，根据3D线段的匹配关系估计相机位姿；

所述步骤四具体为：

首先在线段l上均匀采样2D点，对第j个2D点记为

g_j＝[u_j v_j]^T

其中，u_j,v_j为该点在像素系中的横纵坐标，T代表转置，根据针孔相机模型，将该2D点反投影为相机坐标系下3D坐标，记为

其中，x_j,y_j,z_j为相机坐标系下3D坐标，c_u,c_v为相机光心坐标，f_u,f_v为焦距，s为尺度因子，d_j为g_j所对应的深度值；

对于l上其他2D点也执行以上操作，形成一系列3D点集合{G_j},j＝1,2,...,N，N为l上2D点个数，定义拟合阈值T_3D，对{G_j}中的3D点每次选取其中两个，形成3D线段，计算其余点到该3D线段的欧氏距离，若小于T_3D则记为内点，统计内点数量，选取内点数最多的一组两点(P,Q)∈{G_j}作为该3D线段的最优过点；

通过两个向量来表示3D线段，记三维线段为L，向量是L的单位方向向量，向量的模为相机原点到该3D线段L的距离，的方向平行于由相机原点与该3D线段L所构成的平面的法向量，假设m是线段的中点，则与的关系为：

其中^运算为叉乘，m是线段上的任意一点，该步骤完成2D线段反投影为3D线段；

步骤五、构建包含距离误差及角度误差的雅克比矩阵，利用高斯-牛顿法优化位姿，根据优化后的位姿结果判断是否将该帧的线特征加入局部地图，完成对某一帧的相机位姿估计；

步骤六、重复步骤一至五估计连续帧的位姿变换，完成视觉里程计功能。

2.根据权利要求1所述的结合深度图推断的线特征视觉里程计方法，其特征是，所述彩色图像中的某一点像素与深度图像中该点的像素是来自于同一处。