一种基于优化的IMU紧耦合稠密直接RGBD的三维重建方法技术

本发明专利技术公开了一种基于优化的IMU紧耦合稠密直接RGBD的三维重建方法，包括：(1)从上一时刻重建的局域模型中采集优化RGB数据和优化深度数据，获取原始RGB数据、原始深度数据和IMU数据；(2)根据惯导误差、稠密光度误差以及ICP误差联合构建非线性代价函数，以该非线性代价函数的最小化为目标构成非线性优化问题，对该非线性优化问题以高斯‑牛顿法求解，从而估计RGBD传感器优化位姿；(3)在RGBD传感器的优化位姿下利用当前时刻的原始RGB数据和原始深度数据进行实时三维重建，并在找到回环约束的情况下使用节点对三维重建模型进行变形以保证全局一致性。该方法提高了三维重建的准确性和效率。

A 3D reconstruction method based on optimized IMU tightly coupled dense direct rgbd

【技术实现步骤摘要】
一种基于优化的IMU紧耦合稠密直接RGBD的三维重建方法
本专利技术涉及三维重建领域，具体涉及一种基于优化的IMU紧耦合稠密直接RGBD的三维重建方法。
技术介绍
惯性测量单元是测量物体三轴角速度以及比力的装置，一般的，一个IMU包含了一个三轴的加速度计和一个三轴的陀螺仪，加速度计检测物体在载体坐标系的比力信号，而陀螺仪检测载体相对于惯性坐标系的角速度信号。利用深度详细获得的RGBD数据基于视觉稠密直接法来实时三维重建已经是较为成熟的应用，如申请公布号为CN110211223A公布的一种增量式多视图三维重建方法，和申请公布号为CN111009007A公布的一种指部多特征全面三维重建方法。但是单纯的视觉稠密直接法在视觉传感器位姿跟踪估计上存在着鲁棒性的缺陷：假如位姿估计的非线性优化的初始值偏离真实值太远，则非线性优化的估计结果将变得不可靠，甚至非线性优化结果无法收敛。在视觉传感器运动过快或者视觉传感器经过几何结构无起伏和纹理缺失的地段时，单纯的视觉稠密直接法就会产生上述非线性优化中初值偏离真值过远的情况。>专利技术内本文档来自技高网...

【技术保护点】
1.一种基于优化的IMU紧耦合稠密直接RGBD的三维重建方法，其特征在于，所述三维重建方法包括以下步骤：/n数据提取和采集步骤：从上一时刻重建的局域模型中提取优化RGB数据和优化深度数据，从当前时刻的RGBD传感器采集原始RGB数据和原始深度数据，利用IMU传感器采集上一时刻和当前时刻时间区间内的比力序列和角速度序列，组成IMU数据，所述比力为载体的全加速度去掉重力加速度之后的值；/nRGBD传感器系统状态优化步骤：根据IMU数据构建的惯导误差、RGB数据构建的稠密光度误差以及根据深度数据构建的ICP误差联合构建非线性代价函数，以该非线性代价函数的最小化为目标，通过循环迭代的形式，对该非线性代...

【技术特征摘要】
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误差对应的信息矩阵，eIMU、eRGB和eICP分别为惯导残差、光度残差和ICP残差，和为边缘化先验Hessian矩阵和梯度，u为RGB数据的索引，i为深度数据的索引，为鲁棒核函数，当鲁棒核函数采用Huber势时，



其中，ξ为残差阈值；
(b)以高斯-牛顿法的原则构建非线性代价函数对应的增量方程为：
Hδχ(k,k+1)＝b
其中，HIMU、bIMU分别为惯导误差的Hessian矩阵和梯度，HRGB、bRGB分别为稠密光度误差的Hessian矩阵和梯度，HICP、bICP分别为ICP误差的Hessian矩阵和梯度，分别为边缘化先验的Hessian矩阵和梯度，δχ(k,k+1)为系统状态χ(k,k+1)的微扰；
(c)以该非线性代价函数COST的最小化为目标，对增量方程进行迭代求解，获得包括RGBD传感器位姿在内的系统状态的增量，依据该增量对包括RGBD传感器位姿在内的系统状态进行迭代更新估计，获得RGBD传感器的优化位姿和优化的其它系统状态。


3.如权利要求2所述的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法，其特征在于，步骤(b)中，时间段[tk,tk+1]内惯导误差的Hessian矩阵和梯度的计算过程为：
首先，通过对时间段[tk,tk+1]内的IMU数据进行积分构建对位姿和速度变化的来自IMU的观测量，具体为：









其中，tk为采集编号为k那一帧RGBD数据的时刻，tk+1为采集编号为k+1的那一帧RGBD数据的时刻，t时刻为tk到tk+1时间范围中的IMU传感器采集数据的某时刻，为tk时刻IMU传感器坐标系到tk+1时刻IMU传感器坐标系的位置变化，为t时刻IMU传感器坐标系到tk时刻IMU传感器坐标系的旋转矩阵，at为t时刻的比力，ba为加速度计的零偏，bg为陀螺仪的零偏，为tk时刻IMU传感器坐标系到tk+1时刻IMU传感器坐标系的速度变化，为tk+1时刻IMU传感器坐标系到tk时刻IMU传感器坐标系的姿态变化，为t时刻IMU传感器坐标系到tk时刻IMU传感器坐标系的姿态变化，ωt为t时刻角速度，代表四元数左乘符号，具体积分方式可以选择中值积分或四阶龙格-库塔积分；
然后，计算时间段[tk,tk+1]内惯导误差的Hessian矩阵和梯度，具体为：
惯导误差的残差记为其中，位置残差eP、姿态残差eR、速度残差ev、加速度计的零偏残差eba、陀螺仪的零偏残差ebg分别表达为：















其中，为RGBD传感器坐标系相对于IMU传感器坐标系的旋转矩阵，为RGBD传感器坐标系c相对于IMU传感器坐标系b的平移向量，为RGBD传感器坐标系c到IMU传感器坐标系b的旋转所对应的四元数；为tk时刻RGBD传感器坐标系ck到世界坐标系c0的旋转矩阵，为I坐标系到世界坐标系c0的旋转矩阵，gI为在I坐标系下的重力加速度矢量，为tk+1时刻RGBD传感器坐标系ck+1到世界坐标系c0的旋转矩阵，[]XYZ代表取四元数虚部3×1的向量，eR的四元数用汉密尔顿记法，为tk时刻RGBD传感器坐标系ck到世界坐标系c0的旋转所对应的四元数，为tk+1时刻RGBD传感器坐标系ck+1到世界坐标系c0的旋转所对应的四元数，Δt为tk时刻和tk+1时刻之间的时间间隔；
根据惯导误差的残差的表达式计算系统待估计变量取线性化点的值时惯导误差关于系统待估计变量的雅克比JIMU为：



则根据雅克比JIMU计算惯导误差的Hessian矩阵HIMU和梯度bIMU分别为：






时间段[tk,tk+1]内稠密光度误差的Hessian矩阵和梯度的计算过程为：
首先，对于当前时刻tk+1的原始RGB数据Ek+1和上一时刻tk的优化RGB数据根据同一个空间点在这两个RGB图像中对应像素坐标位置的光度的差异计算稠密光度误差的残差表达为：
eRGB,u＝Ick+1(u')-Ick(u)



其中，u为某一个空间点在RGB图像中像素坐标，π是将空间点的齐次坐标写为三维坐标并投影到对应的二维像素坐标的操作，π-1是其逆操作，根据点的二维像素坐标和其深度反投影到三维坐标并写为齐次坐标，于是u'表示u所对应的空间点投影到RGB图像Ek+1中的像素坐标，Ick(u)表示在RGB数据中位于像素索引u处的RGB数据所转换的光度，Ick+1(u')表示在RGB数据Ek+1中位于像素索引u'处的RGB数据所转换的光度，Tc0,ck、Tc0,ck+1分别表示由tk时刻RGBD传感器坐标系ck和tk+1时刻RGBD传感器坐标系ck+1到世界坐标系c0的欧氏变换矩阵，为欧氏变换矩阵Tc0,ck+1的逆矩阵；
根据稠密光度误差的残差的表达式计算在系统待估计状态取线性化点的值时稠密光度误差的残差关于待估计状态的雅克比JRGB,u为：



则根据雅克比JRGB,u计算稠密光度误差的Hessian矩阵HRGB和梯度bRGB为：






时间段[tk,tk+1]内ICP误差的Hessian矩阵和梯度的计算过程为：
首先，称当前时刻tk+1的原始深度数据Dk+1中索引为i处的点为深度数据点i，深度数据点i的ICP误差的残差表达为：



其中，为深度数据点i对应的空间点在tk+1时刻RGBD传感器坐标系ck+1中的齐次坐标，为深度数据点i投影到tk时刻优化深度数据得到的深度数据点所对应的空间点在世界坐标系c0下的法向量，为深度数据点i投影到tk时刻优化深度数据得到的深度数据点所对应的空间点在世界坐标系c0下的齐次坐标，[]0:2表示取四维齐次坐标前三维；
根据ICP误差的残差表达式计算当系统待估计变量取线性化点的值时ICP误差的残差关于系统待估计变量的雅克比JICP,i为：



则当||eICP,i||≤ξ时，ICP误差的Hessian矩阵HICP和梯度bICP分别为：






则当||eICP,i||＞ξ时，ICP误差的Hessian矩阵HICP和梯度bICP分别为：






时间段[tk,tk+1]内采用的边缘化先验的Hessian矩阵等于时间段[tk-1,tk]的非线性优化迭代收敛后，对时刻tk之前的状态进行边缘化操作得到的先验Hessian矩阵Hprior，时间段[tk,tk+1]内采用的边缘化先验的梯度表达为：其中bprior为时间段[tk-1,tk]的非线性优化迭代收敛后，对时刻tk之前的状态进行边缘化操作得到的先验梯度，Δχk的定义为：其中代表时间段[tk,tk+1]内状态数据χ(k,k+1)中的时刻tk的状态数据的值；而代表在对[tk-1,tk]时间段内状态数据χ(k-1,k)中的时刻tk之前的状态进行边缘化操作之后，时刻tk的状态数据的值，deviation()代表对两个状态的值求取差异。


4.如权利要求2所述的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法，其特征在于，步骤(c)中，在迭代求解时，用于计算稠密光度误差、ICP误差的Hessian矩阵和梯度的R...

【专利技术属性】
技术研发人员：吴珂，方晓健，
申请(专利权)人：杭州深瞳科技有限公司，
类型：发明
国别省市：浙江;33