[发明专利]一种基于优化的IMU紧耦合稠密直接RGBD的三维重建方法

权利要求书

1.一种基于优化的IMU紧耦合稠密直接RGBD的三维重建方法，其特征在于，所述三维重建方法包括以下步骤：

数据提取和采集步骤：从上一时刻重建的局域模型中提取优化RGB数据和优化深度数据，从当前时刻的RGBD传感器采集原始RGB数据和原始深度数据，利用IMU传感器采集上一时刻和当前时刻时间区间内的比力序列和角速度序列，组成IMU数据，所述比力为载体的全加速度去掉重力加速度之后的值；

RGBD传感器系统状态优化步骤：根据IMU数据构建的惯导误差、RGB数据构建的稠密光度误差以及根据深度数据构建的ICP误差联合构建非线性代价函数，以该非线性代价函数的最小化为目标，通过循环迭代的形式，对该非线性代价函数对应的增量方程求解，获得包括RGBD传感器位姿在内的系统状态的增量，依据该增量对包括RGBD传感器位姿在内的系统状态进行迭代更新优化，迭代结束时，获得系统状态的优化估计，其中包括RGBD传感器的优化位姿；

三维重建模型构建步骤：在RGBD传感器的优化位姿下利用当前时刻的原始RGB数据和原始深度数据在三维重建模型上进行实时更新，并在找到回环约束的情况下使用节点对三维重建模型进行变形以保证全局一致性。

2.如权利要求1所述的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法，其特征在于，所述RGBD传感器系统状态优化步骤具体包括：

(a)根据IMU数据构建的误差、RGB数据构建的稠密光度误差以及根据深度数据构建的ICP误差联合构建非线性代价函数COST为：

其中，Λ_IMU、Λ_RGB和Λ_ICP分别为根据IMU数据构建的惯导误差、根据RGB数据构建的稠密光度误差和根据深度数据构建的ICP误差对应的信息矩阵，e_IMU、e_RGB和e_ICP分别为惯导残差、光度残差和ICP残差，和为边缘化先验Hessian矩阵和梯度，u为RGB数据的索引，i为深度数据的索引，为鲁棒核函数，当鲁棒核函数采用Huber势时，

其中，ξ为残差阈值；

(b)以高斯-牛顿法的原则构建非线性代价函数对应的增量方程为：

Hδχ(k,k+1)＝b

其中，H_IMU、b_IMU分别为惯导误差的Hessian矩阵和梯度，H_RGB、b_RGB分别为稠密光度误差的Hessian矩阵和梯度，H_ICP、b_ICP分别为ICP误差的Hessian矩阵和梯度，分别为边缘化先验的Hessian矩阵和梯度，δχ(k,k+1)为系统状态χ(k,k+1)的微扰；

(c)以该非线性代价函数COST的最小化为目标，对增量方程进行迭代求解，获得包括RGBD传感器位姿在内的系统状态的增量，依据该增量对包括RGBD传感器位姿在内的系统状态进行迭代更新估计，获得RGBD传感器的优化位姿和优化的其它系统状态。