[发明专利]一种同时定位与稠密三维重建方法

权利要求书

1.一种同时定位与稠密三维重建方法，其特征在于包括如下步骤：

1)通过RGB-D对准估计相机初始姿态，使用基于单应性的特征匹配优化相机姿态；

2)运行“增量集束调整”算法，自适应地更新、优化关键帧的相机姿态；

所述的步骤2)具体为：

当有新的关键帧加入或者检测到闭合回路的时候，运行如下增量集束调整：

将当前相机运动参数定义为C_i∈SE(3)，n_c是相机个数，将当前三维点位置定义为X_j∈R³，n_p为三维点的个数，将相机i可见的三维点的集合定义为V_i^c，将三维点j可见的相机集合定义为用高斯-牛顿算法迭代优化：

每次迭代，将当前相机运动参数定义为将当前三维点位置定义为将相机运动参数的增量定义为将三维点位置的增量定义为将e(C_i，X_j)在处线性展开：

其中，表示在上叠加一个对应的se(3)增量，定义法向方程：

U、V分别是n_c×n_c、n_p×n_p的对角块矩阵

对每个子矩阵U_i∈R^6×6，V_j∈R^3×3，W为n_c×n_p的块矩阵：

每个子矩阵W_ij∈R^6×3，法向方程右侧向量：

每个子向量u_i∈R⁶，v_j∈R³，定义舒尔补方程

其中，

S＝U-WV^-1W^T

g＝u-WV^-1v

S为n_c×n_c的块矩阵：

每个子矩阵S_ij∈R^6×6，舒尔补方程右侧向量

对每个子向量g_i∈R⁶，定义辅助向量其中i＝1，...，n_c，j∈V_i^c，定义辅助变量其中j＝1，...，n_p，初始时刻，令U＝0，V＝0，W＝0，u＝0，v＝0，S＝0，g＝0，每次迭代，运行以下步骤：

a)对每个三维点j及其可见的相机如果或上次迭代被更新，则计算处的并由下式更新各变量：

b)对每个三维点j，如果V_j在本次迭代中被更新，则对每个三维点j可见的相机由下式更新各变量：

Y_ij＝W_ijV_j^-1

并对三维点j可见的相机对由下式更新各变量：

c)使用预处理共轭梯度算法求解上述的舒尔补方程，得到相机增量对每个相机i，如果则用更新

d)对每个三维点j，如果V_j本次迭代被更新，或相机中存在本次迭代被更新，则计算如果则用更新

e)如果本次迭代没有任何相机或三维点被更新，或达到最大迭代次数，则结束循环，否则，重复步骤a)；

3)将非关键深度图融合到关键帧，基于关键帧的深度图重建三维模型，通过重融合在相机姿态改变时更新三维模型。