[发明专利]一种用于深度相机的位姿估计方法

权利要求书

1.一种用于深度相机的位姿估计方法，其特征在于包括以下步骤：

步骤一、将深度相机设置在移动机构上进行拍摄，获得深度图和RBG彩色图，从拍摄的每相邻两帧图像中提取Oriented FAST and Rotated BRIEF ORB特征点对；

步骤二、利用N个深度信息缺失的ORB特征点对，计算相机位姿变化的估计值ξ_P，利用M个深度信息完整的ORB特征点对，计算相机位姿变化的估计值ξ_Q，进而得到总估计值ξ₀；

步骤三、构建融合了深度信息缺失的ORB特征点对和深度信息完整的ORB特征点对对应的误差信息的最小重投影误差模型，进而得到对应的雅可比矩阵J；

步骤四、根据所述总估计值ξ₀、最小重投影误差模型和雅可比矩阵J，利用非线性优化方法，计算优化后的总估计值ξ_k，从而完成相邻两帧图像拍摄过程中的相机位姿变化的估计；

对于N个深度信息缺失的ORB特征点对，利用RANSAC算法计算估计值ξ_P；对于M个深度信息完整的ORB特征点对，利用直接线性变换法计算估计值ξ_Q，再利用如下方程式，计算总估计值ξ₀；

对于N个深度信息缺失的ORB特征点对，利用重投影方法，结合PnP算法，构建其对应的误差函数e_P；对于M个深度信息完整的ORB特征点对，利用重投影方法，结合ICP算法，构建其对应的误差函数e_Q；然后构建融合误差函数e_P和误差函数e_Q的最小重投影误差模型e，进而得到对应的雅可比矩阵J；

所述雅可比矩阵J的计算方法包括以下步骤：

步骤Ⅰ、对于N个深度信息缺失的ORB特征点对，记相邻两帧图像上匹配的ORB特征点对对应的空间点齐次坐标分别为P_i＝(x_i,y_i,z_i,1)^T，其相邻帧上的ORB特征点经过世界坐标系和像素坐标系的转换关系得到对应的像素坐标为

利用重投影方法，计算初始帧的ORB特征点P_i＝(x_i,y_i,z_i,1)^T在相邻帧上投影的像素齐次坐标p_i＝(u_i,v_i,1)^T，其李代数关系如下，

其中，si表示初始帧的ORB特征点在相邻帧上的投影对应的深度信息，K表示相机的内参矩阵，∧表示反对称符号，

结合PnP算法，构建深度信息缺失的ORB特征点对的误差函数e_P；

步骤Ⅱ、对于M个深度信息完整的ORB特征点对，记相邻两帧图像上匹配的ORB特征点对对应的空间点坐标分别为Q_i＝(X_i,Y_i,Z_i)^T，利用重投影方法，计算初始帧的ORB特征点Q_i＝(X_i,Y_i,Z_i)^T在相邻帧上投影的空间点坐标其李代数关系如下，

其中，∧表示反对称符号，

结合ICP算法，构建深度信息完整的ORB特征点对的误差函数e_Q；

步骤Ⅲ、构建最小重投影误差模型e，如下所示；

其中，δξ表示扰动量，f_x表示相机的焦距在像素坐标系的x轴分量，f_y表示相机的焦距在像素坐标系的y轴分量，点P′_i＝(x′_i,y′_i,z′_i,1)^T表示初始帧中深度信息缺失的ORB特征点P_i＝(x_i,y_i,z_i,1)^T乘以相机位姿变化的总估计值ξ₀后对应的点，点Q_i′＝(X′_i,Y′_i,Z′_i)^T表示初始帧中深度信息完整的ORB特征点Q_i＝(X_i,Y_i,Z_i)^T乘以相机位姿变化的总估计值ξ₀后对应的点。