CN111862316A - 一种基于优化的imu紧耦合稠密直接rgbd的三维重建方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明涉及三维重建领域，具体涉及一种基于优化的IMU紧耦合稠密直接RGBD的三维重建方法。

背景技术

惯性测量单元是测量物体三轴角速度以及比力的装置，一般的，一个IMU包含了一个三轴的加速度计和一个三轴的陀螺仪，加速度计检测物体在载体坐标系的比力信号，而陀螺仪检测载体相对于惯性坐标系的角速度信号。

利用深度详细获得的RGBD数据基于视觉稠密直接法来实时三维重建已经是较为成熟的应用，如申请公布号为CN110211223A公布的一种增量式多视图三维重建方法，和申请公布号为CN111009007A公布的一种指部多特征全面三维重建方法。

但是单纯的视觉稠密直接法在视觉传感器位姿跟踪估计上存在着鲁棒性的缺陷：假如位姿估计的非线性优化的初始值偏离真实值太远，则非线性优化的估计结果将变得不可靠，甚至非线性优化结果无法收敛。在视觉传感器运动过快或者视觉传感器经过几何结构无起伏和纹理缺失的地段时，单纯的视觉稠密直接法就会产生上述非线性优化中初值偏离真值过远的情况。

发明内容

本发明的目的是提供一种基于优化的IMU紧耦合稠密直接RGBD的三维重建方法，在利用稠密直接法对RGBD进行三维重建时，以IMU采集的角速度信息和比力信息作为约束，以提高三维重建的准确性和效率。

为实现上述发明目的，本发明提供的技术方案为：

一种基于优化的IMU紧耦合稠密直接RGBD的三维重建方法，包括以下步骤：

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

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

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

与现有技术相比，本发明具有的有益效果至少包括：

本发明实施例提供的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法中，在获取稠密RGBD数据、IMU数据的基础上，联合惯导误差、稠密光度误差、ICP误差联合优化RGBD传感器位姿，然后在RGBD传感器的优化位姿下对三维重建模型进行融合和调整，通过引入IMU观测，提升系统能观性，加强三维重建方法鲁棒性，增强三维重建的全局一致性，保证了三维重建模型的精度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动前提下，还可以根据这些附图获得其他附图。

图1是本发明提供的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法的流程图，其中，实线箭头表示该流程步骤执行成功得到该步骤的最终结果或条件判断为“是”时进行指向的下一流程步骤，虚线箭头表示该流程步骤执行未得到该步骤的最终结果或条件判断“否”时指向的下一流程步骤。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例对本发明进行进一步的详细说明。应当理解，此处所描述的具体实施方式仅仅用以解释本发明，并不限定本发明的保护范围。

本实施例提供了一种基于优化的IMU紧耦合稠密直接RGBD的三维重建方法。其主要思想是，在上一时刻t_k和当前时刻t_k+1之间的时间段内，根据上一时刻已经构建的三维重建模型，利用当前时刻采集的RGBD传感器数据对已经构建的三维重建模型进行实时更新的方法。在更新时为了解决单纯的视觉稠密直接法在视觉传感器位姿跟踪估计上存在着鲁棒性的缺陷问题，还引入了IMU传感器数据，利用IMU传感器数据的加入提升了对RGBD传感器位姿的估计的准确性和鲁棒性，在此优化位姿下利用RGBD传感器数据在已经构建的三维重建模型基础上进行模型的更新，大大提升了三维重建模型的准确性和效率。

图1是本发明提供的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法的流程图。如图1所示，实施例提供的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法包括以下步骤：

步骤1，原始的RGBD传感器数据的采集、IMU传感器数据的采集和优化的RGBD数据的提取。

根据本发明的主要思想，步骤1中主要采集两部分传感器数据，第一部分为RGBD传感器在当前时刻t_k+1采集的原始RGB数据E_k+1和原始深度数据D_k+1，组成RGBD数据，其中，RGBD传感器是指能够实时地提供稠密深度及RGB数据的传感器，包括但不限于双目相机、双目相机与深度摄像头的组合、双目相机与面阵型LiDAR的组合等。

第二部分为利用IMU传感器采集上一时刻t_k和当前时刻t_k+1时间区间内的比力序列{a_t,t∈[t_k,t_k+1]}和角速度序列{ω_t,t∈[t_k,t_k+1]}，组成IMU数据，由于IMU传感器的采集频率一般远大于RGBD传感器的采集频率，因此，在不包括两端的时间的开区间(t_k,t_k+1)内，总会存在若干组IMU数据。本实施例中，IMU传感器可以是陀螺仪和加速度计组成的IMU，IMU集成到RGBD传感器上，用于为构建RGBD传感器的位姿的观测量提供原始数据。

根据本发明的主要思想，步骤1中还需获取一种优化数据。这部分数据是从时间段[t_k-1,t_k]结束时更新的三维重建模型中提取的优化RGB数据

和优化深度数据

该提取过程具体是指：在上一时刻t_k的RGBD传感器的通过非线性优化得到的估计位姿下，对时间段[t_k-1,t_k]结束时经过更新的三维重建模型使用喷溅渲染(splatted rendering)法，提取出一部分surfel，这部分surfel中的那些自身的更新时间与时刻t_k之间的间隔小于一定值(该值一般设置为t_k-1与t_k的时间间隔相近)的那些对象就构成了相应的Active局域模型，该局域模型中的surfel的位置和颜色信息投影到上一时刻t_k的RGBD传感器估计位姿下得到优化RGB数据

和优化深度数据

步骤2，在判断当前时刻采集的RGBD数据和IMU数据为全局第一帧时，直接根据RGBD数据构建三维重建模型，之后获取RGBD传感器的初始位姿下的Active局域模型。

该三维重建方法中，将全局第一帧对应的RGBD传感器坐标系c₀作为世界坐标系，全局第一帧RGBD数据作为全局三维重建模型的最初数据。三维重建模型中的三维点，均采用带有方向的表面元surfel的方式来表示，每个surfel包含空间位置p、权重w、法向量n、颜色c、半径r、初始化时间t₀和最近更新时间t。

针对权重，采用RGBD传感器噪声阵对应的信息矩阵来构建权重，其中，RGBD传感器噪声阵对应的信息矩阵记作Λ_ICP，具体为：

针对原始RGBD数据中索引为i的数据点，其信息矩阵为：

若该数据点第一次被观测到，则其对应的surfel的权重在初始化时应为

分别为采集点i时RGBD传感器在x、y、z方向上的不确定度，该三个不确定度的值取决于RGBD传感器噪声特性以及该采集点所对应的真实空间内该处的被采集物体的实际情况。

在其他实时方式中，surfel的权重w也可以初始化为经验公式得到的值，具体表示为

其中γ为RGBD传感器相素平面上该surfel像素位置与相平面中心的归一化距离，σ为经验值，取0.6。

在三维重建模型的基础上，在RGBD传感器的初始位姿下获取Active局域模型，即在RGBD传感器的初始位姿(与世界坐标系间无相对平移和相对旋转的位姿)下，采用splatted rendering(喷溅渲染)方法根据选出的surfel构建Active局域模型，该局域模型内的surfel的位置和颜色信息投影到RGBD传感器的初始位姿下，作为下一个时间段内该三维重建方法采用的优化RGB数据

和优化深度数据

步骤3，在判断当前时刻采集的RGBD数据和IMU数据不为全局第一帧时(此时k>＝0)，根据原始RGBD数据、优化RGBD数据和IMU数据对RGBD传感器位姿进行估计，获得RGBD传感器的优化位姿。

具体地，根据时间段[t_k,t_k+1]内的IMU数据构建的误差、根据当前时刻t_k+1的原始RGB数据E_k+1和上一时刻t_k的优化RGB数据

构建的稠密光度误差以及根据当前时刻t_k+1的原始深度数据D_k+1和上一时刻t_k的优化深度数据

构建的ICP误差联合构建非线性代价函数，以该非线性代价函数的最小化为目标，对该非线性代价函数对应的增量方程求解，获得包括RGBD传感器位姿在内的系统待估计状态的增量，依据该增量对系统待估计状态进行估计，获得优化的系统状态，其中包括RGBD传感器的优化位姿。具体过程包括：

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

其中，Λ_IMU、Λ_RGB和Λ_ICP分别为根据IMU数据构建的惯导误差、根据RGB数据构建的稠密光度误差和根据深度数据构建的ICP误差对应的信息矩阵，e_IMU、e_RGB和e_ICP分别为惯导残差、光度残差和ICP残差，

和

为边缘化的先验Hessian矩阵和梯度，u为RGB数据的索引，i为深度数据的索引。

本实施例中，为了更好地利用深度数据的信息，在构建非线性优化问题中深度相关的残差时，不采用深度的重投影残差，而是根据深度数据基于点到面(point to plane)的ICP方法来构建残差，简称ICP残差，由ICP残差构建的那部分代价函数我们简称为ICP误差；

为鲁棒核函数，当鲁棒核函数采用Huber势时，

其中，ξ为残差阈值，该鲁棒核函数主要用于在错误匹配时降低错误匹配的影响，具体通过引入ξ为残差阈值对鲁棒核函数值进行分段，当ICP残差的值小于残差阈值时，鲁棒核函数值与ICP残差呈二次关系，当ICP残差的值大于残差阈值时，鲁棒核函数值与ICP残差呈线性关系，以此降低较可能来自误匹配的ICP残差对总代价函数的影响，进而提升匹配准确性。

(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)的微扰；

本实施例中，时间段[t_k,t_k+1]内系统状态数据χ(k,k+1)是由RGBD传感器位姿、IMU传感器速度、IMU传感器零偏、IMU传感器与RGBD传感器外参(即IMU传感器和RGBD传感器之间相对位姿)、世界坐标系与I坐标系间相对旋转这些待估计状态所组成的，以符号表述为：

其中，

分别为在世界坐标系下观察的t_k时刻RGBD传感器坐标系c_k和t_k+1时刻RGBD传感器坐标系c_k+1相对于世界坐标系c₀的平移向量，

分别为t_k时刻RGBD传感器坐标系c_k和t_k+1时刻RGBD传感器坐标系c_k+1到世界坐标系c₀的旋转所对应的四元数，

分别为t_k时刻和t_k+1时刻在I坐标系下观察的IMU传感器坐标系b_k和b_k+1的速度，

分别为t_k时刻和t_k+1时刻在IMU传感器坐标系b_k和b_k+1下加速度计的零偏，

分别为t_k时刻和t_k+1时刻在IMU传感器坐标系b_k和b_k+1下陀螺仪的零偏，

为世界坐标系c₀到I坐标系的旋转所对应的四元数，将z轴与重力加速度方向重合，并与世界坐标系间偏航角为零的坐标系记作I坐标系。特别地，

和

都是代表RGBD传感器坐标系与IMU传感器坐标系之间的相对位置和姿态的量，这些量中表示RGBD传感器坐标系和IMU传感器坐标系的字母b和c都没有使用帧编号作为下标。这是因为在算法开始前RGBD传感器和IMU传感器已经固连好，RGBD传感器坐标系与IMU传感器坐标系之间的相对位置和姿态不随时间改变，于是，

为RGBD传感器坐标系c相对于IMU传感器坐标系b的平移向量，

为RGBD传感器坐标系c到IMU传感器坐标系b的旋转对应的四元数；

其中各待估计状态的微扰设置采用混合法，位置、速度、加速度计和陀螺仪零偏的微扰直接在向量空间，旋转的微扰在李代数空间，且为提高全局一致性，旋转使用全局微扰，具体如下：

对于其中所有旋转相关的微扰，需要表示在李代数空间：待估计量中的旋转相关的待估计量为

对它们的四元数形式的微扰我们可以暂且记为

但是，四元数对旋转微扰的表示是过参数化的(over-parameterized),四元数形式的微扰并不符合最小坐标表示(minimal coordinaterepresentation)，因此，需要改用李代数空间的微扰来表达这些旋转的待估计量的微扰，于是，用

和

分别表示对

和

在李代数空间上的微扰，用

表示对

在李代数空间上的微扰，用

表示对

在李代数空间上的微扰；

其余的待估计量的微扰则较直观，

和

分别是对

和

在向量空间上的微扰，

和

分别是对

和

在向量空间上的微扰，

和

分别是对

和

在向量空间上的微扰，

和

分别是对

和

在向量空间上的微扰，

是对

在向量空间上的微扰；

具体地，用

代表四元数左乘运算的符号，则可以将全部待估计量及其相应的微扰之间的关系表示为：

下述残差关于待估计状态的雅克比即基于这样的微扰规则。

时间段[t_k,t_k+1]内惯导误差的Hessian矩阵和梯度的计算过程为：

首先，通过对时间段[t_k,t_k+1]内的IMU数据进行积分构建对位姿和速度变化的来自IMU的观测量，具体为：

其中，对每一帧采集的RGBD数据进行编号，全局最开始的一帧RGBD数据编号为0，以此类推，t_k为采集编号为k那一帧RGBD数据的时刻，t_k+1为采集编号为k+1的那一帧RGBD数据的时刻，t时刻为t_k到t_k+1时间范围中的IMU传感器采集数据的某时刻，

为t_k时刻IMU传感器坐标系到t_k+1时刻IMU传感器坐标系的位置变化，

为t时刻IMU传感器坐标系到t_k时刻IMU传感器坐标系的旋转矩阵，a_t为t时刻的比力，b_a为加速度计的零偏，b_g为陀螺仪的零偏，

为t_k时刻IMU传感器坐标系到t_k+1时刻IMU传感器坐标系的速度变化，

为t_k+1时刻IMU传感器坐标系到t_k时刻IMU传感器坐标系的姿态变化，

为t时刻IMU传感器坐标系到t_k时刻IMU传感器坐标系的姿态变化，ω_t为t时刻角速度，

代表四元数左乘符号，具体积分方式可以选择中值积分或四阶龙格-库塔积分；

然后，计算时间段[t_k,t_k+1]内惯导误差的Hessian矩阵和梯度，具体为：

惯导误差的残差记为

其中，位置残差e_P、姿态残差e_R、速度残差e_v、加速度计的零偏残差e_ba、陀螺仪的零偏残差分别表达为：

其中，特别地，

和

都是代表RGBD传感器坐标系与IMU传感器坐标系之间的相对位置和姿态的量，这些量中表示RGBD传感器坐标系和IMU传感器坐标系的字母b和c都没有使用帧编号作为下标，这是因为在算法开始前RGBD传感器和IMU传感器已经固连好，RGBD传感器坐标系与IMU传感器坐标系之间的相对位置和姿态不随时间改变。于是，

为RGBD传感器坐标系相对于IMU传感器坐标系的旋转矩阵，

为RGBD传感器坐标系c相对于IMU传感器坐标系b的平移向量，

为RGBD传感器坐标系c到IMU传感器坐标系b的旋转所对应的四元数，

为t_k时刻RGBD传感器坐标系到世界坐标系c₀的旋转矩阵，

为I坐标系到世界坐标系c₀的旋转矩阵，g^I为在I坐标系下的重力加速度矢量，

为t_k+1时刻RGBD传感器坐标系c_k+1到世界坐标系c₀的旋转矩阵，[]_XYZ代表取四元数虚部3×1的向量，e_R的四元数

用汉密尔顿记法，

为t_k时刻RGBD传感器坐标系c_k到世界坐标系c₀的旋转所对应的四元数，

为t_k+1时刻RGBD传感器坐标系c_k+1到世界坐标系c₀的旋转所对应的四元数，Δt为t_k时刻和t_k+1时刻之间的时间间隔；

根据惯导误差的残差表达式计算当系统待估计变量取线性化点的值时惯导误差的残差关于系统待估计变量的雅克比J_IMU为：

则根据雅克比J_IMU计算惯导误差的Hessian矩阵H_IMU和梯度b_IMU分别为：

时间段[t_k,t_k+1]内稠密光度误差的Hessian矩阵和梯度的计算过程为：

首先，对于当前时刻t_k+1的原始RGB数据E_k+1和上一时刻t_k的优化RGB数据

根据同一个空间点在这两个RGB图像中对应像素坐标位置的光度的差异计算稠密光度误差的残差表达为：

e_RGB,u＝I_ck+1(u')-I_ck(u)

其中，u为某一个空间点在RGB图像

中像素坐标，π是将空间点的齐次坐标写为三维坐标并投影到对应的二维像素坐标的操作，π^-1是其逆操作，根据点的二维像素坐标和其深度反投影到三维坐标并写为齐次坐标，于是u'表示u所对应的空间点在RGB图像E_k+1中的像素坐标，I_ck(u)表示在RGB数据

中位于像素索引u处的RGB数据所转换的光度，I_ck+1(u')表示在RGB数据E_k+1中位于像素索引u'处的RGB数据所转换的光度，T_c0,ck、T_c0,ck+1分别表示由t_k时刻RGBD传感器坐标系c_k和t_k+1时刻RGBD传感器坐标系c_k+1到世界坐标系c₀的欧氏变换矩阵，

为欧氏变换矩阵T_c0,ck+1的逆矩阵；

根据稠密光度误差的残差表达式计算当系统待估计变量取线性化点的值时稠密光度误差的残差关于系统待估计变量的雅克比J_RGB,u为：

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

各对稠密光度误差对应关系之间计算互相没有依赖，因此稠密光度误差的Hessian和梯度计算采用并行计算的方式进行。

时间段[t_k,t_k+1]内ICP误差的Hessian矩阵和梯度的计算过程为：

首先，称当前时刻t_k+1的原始深度数据D_k+1中索引为i处的点为深度数据点i，深度数据点i的ICP误差的残差表达为：

其中，

为深度数据点i对应的空间点在t_k+1时刻RGBD传感器坐标系c_k+1中的齐次坐标，

为深度数据点i投影到t_k时刻优化深度数据

得到的深度数据点所对应的空间点在世界坐标系c₀下的法向量，

为深度数据点i投影到t_k时刻优化深度数据

得到的深度数据点所对应的空间点在世界坐标系c₀下的齐次坐标，[]_0:2表示取四维齐次坐标前三维；

根据ICP误差的残差的表达式计算当系统待估计变量取线性化点的值时ICP误差的残差关于系统待估计变量的雅克比J_ICP,i为：

则当||e_ICP,i||≤ξ时，ICP误差的Hessian矩阵H_ICP和梯度b_ICP分别为：

则当||e_ICP,i||＞ξ时，ICP误差的Hessian矩阵H_ICP和梯度b_ICP分别为：

各深度数据点的point-to-plane的ICP误差对应关系之间的计算互相没有依赖，因此ICP误差的Hessian和梯度计算采用并行计算的方式进行。

时间段[t_k,t_k+1]内采用的边缘化先验的Hessian矩阵

等于时间段[t_k-1,t_k]的非线性优化迭代收敛后，对时刻t_k之前的状态进行边缘化操作得到的先验Hessian矩阵H_prior，时间段[t_k,t_k+1]内采用的边缘化先验的梯度

表达为：

其中b_prior为时间段[t_k-1,t_k]的非线性优化迭代收敛后，对时刻t_k之前的状态进行边缘化操作得到的先验梯度，Δχ_k的定义为：

其中

代表时间段[t_k,t_k+1]内状态数据χ(k,k+1)中的时刻t_k的状态数据的值；而

代表在对[t_k-1,t_k]时间段内状态数据χ(k-1,k)中的时刻t_k之前的状态进行边缘化操作之后，时刻t_k的状态数据的值，deviation()代表对两个状态的值求取差异，具体做法为，对两个状态中代表旋转的量，求取它们之间的相对旋转在李代数空间上的对应值；其余的量直接用向量减法求取它们在向量空间的差值。

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

实施例采用高斯-牛顿法，以该非线性代价函数COST的最小化为目标，对增量方程进行迭代求解。具体地，由增量方程解出增量，以增量更新待估计状态χ(k,k+1)，然后再根据更新后的待估计状态χ(k,k+1)来更新增量方程中的Hessian矩阵H和梯度b，再对增量方程求解，如此反复直到增量方程的解收敛，则最后一次更新的待估计状态χ(k,k+1)中的RGBD传感器位姿即作为RGBD传感器的优化位姿。

在迭代求解时，用于计算稠密光度误差、ICP误差的Hessian矩阵和梯度的RGBD数据需要遵循粗到细(coarse to fine)原则，即先使用较低分辨率的RGBD数据，随着迭代的进行逐渐变为较高分辨率的RGBD数据。

在迭代求解时，Hessian矩阵H_IMU、H_RGB、H_ICP、

梯度b_IMU、b_RGB、b_ICP、

的迭代计算过程中均遵循FEJ(First Estimate Jacobian)原则，与被边缘化的状态直接相关的状态的雅克比和残差必须固定在该边缘化操作发生时其所在的线性化点上进行计算。

为了尽量减小不可观状态的漂移对重建三维重建模型的影响，在步骤(c)的基础上，还需要依零空间正交化的原则对非线性优化估计得到的RGBD传感器的优化位姿进行调整。

在时间段[t_k,t_k+1]内求解获得包括RGBD传感器的优化位姿在内的系统优化状态后，还需要计算对时刻t_k+1之前的状态进行边缘化操作得到的先验Hessian矩阵和梯度，首先，当完成了时间段[t_k,t_k+1]内的非线性优化迭代后，将此时的总Hessian矩阵H和梯度b进行如下划分：

其中，H_k,k代表Hessian矩阵H中仅与t_k时刻的状态相关的观测所对应的约束，H_k,k+1代表Hessian矩阵H中同时与t_k时刻的系统状态和t_k+1时刻的系统状态相关的观测所对应的约束，H_k+1,k为H_k,k+1的转置，同样代表Hessian矩阵H中同时与t_k时刻的系统状态和t_k+1时刻的系统状态相关的观测所对应的约束，H_k+1,k+1代表Hessian矩阵H中仅与t_k时刻的状态相关的观测所对应的约束，b_k代表梯度b中与残差关于t_k时刻的系统状态的雅克比相关联的部分，b_k+1代表梯度b中与残差关于t_k+1时刻的系统状态的雅克比相关联的部分；然后，当对时刻t_k+1之前的状态进行边缘化操作，t_k时刻的系统状态是要被边缘化移出优化窗口的状态，t_k+1时刻的系统状态则是与这个即将被边缘化移出的状态直接相关联的状态，依据舒尔补操作(Schur complement operation)的原则，对时刻t_k+1之前的状态进行边缘化操作得到的先验Hessian矩阵H_prior和梯度b_prior将如下得到：

其中

代表若矩阵H_k,k可逆，则对其求逆，若矩阵H_k,k不可逆，则对其求伪逆。

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

本实施例中，步骤4三维重建的具体过程包括：

(a)获取在RGBD传感器的优化位姿下的Active局域模型，为Active局域模型寻找全局回环候选帧，将全局回环候选帧与Active局域模型进行RGBD稠密直接法配准。

具体过程为：首先，针对三维重建模型中各surfel，选出最后更新时间t满足t_k+1-t_ths≤t≤t_k+1的surfel，t_ths为可设置阈值，然后在时刻t_k+1的RGBD传感器的优化位姿

下，采用splatted rendering(喷溅渲染)方法根据选出的surfel构建局域模型，即为Active局域模型；

然后，对Active局域模型包含的RGBD数据进行降采样，再对降采样结果进行随机蕨编码后，在全局的随机蕨编码库中寻找与该随机蕨编码相似度最高的随机蕨编码所对应的数据帧作为当前Active局域模型的全局回环候选帧；

最后，将全局回环候选帧与Active局域模型进行RGBD稠密直接法配准，在配准过程中，采用的非线性代价函数仅包含ICP误差部分，即非线性代价函数

非线性代价函数COST_global中变量的含义与求解过程与非线性代价函数COST相同，在此不再赘述。

非线性优化后，结束迭代时增量方程中的Hessian矩阵的特征值用于衡量该非线性优化得到的估计状态的估计误差，检验这个估计误差，当该估计误差小于预设误差阈值时，认为配准结果满足要求，该全局回环候选帧为真正的全局回环帧，其与Active局域模型构成一个真正的全局回环；当该估计误差大于预设误差阈值时，认为配准结果不满足要求，该全局回环候选帧不为真正的全局回环帧。

(b)全局回环候选帧与Active局域模型的配准结果满足要求时，该全局回环候选帧是Active局域模型的真正的全局回环帧，即其与Active局域模型构成真正的全局回环；利用全局回环帧与Active局域模型的关联所构成的全局回环约束对三维重建模型的节点进行非线性优化，获得各节点的优化属性，所述优化属性包括各节点的优化后位置、仿射变换矩阵和平移向量，然后利用各节点的优化属性对全局三维重建模型进行变形；

具体过程为：首先，节点是通过特定手段构造的，用于控制三维重建模型进行变形调整的一种数据结构，节点维护着一系列服务于变形三维重建模型的属性，包括位置s、时间t、3×3仿射变换矩阵M和3×1平移向量d，节点具体的构造方法为，对当前三维重建模型中的surfel进行均匀采样得到一系列surfel，利用这些surfel构造相同数量的节点，节点的位置和时间初始化为被采样的surfel的位置和更新时间，节点的仿射变换矩阵M和平移向量d分别初始化为单位矩阵I₃和零向量[0,0,0]^T；若把某个节点记为G，则节点G的最近邻节点集合记为

对于surfel，在此surfel更新时间前后一定范围内的节点中找出空间上最近邻的m个节点组成集合Θ，m的典型值取4；根据集合Θ中节点对于集合Θ对应的surfel的位置和法向量的变形函数就分别表示为：

其中，

为变形后的位置，

为变形后的法向量，l为集合Θ中节点的索引，γ_l＝(1-||p-s_l||₂/dis_max)²，dis_max即在集合Θ中节点与surfel距离之中最大距离；

然后，构建用于优化节点的位置、仿射变换和平移向量的总代价函数，采用高斯-牛顿法对该代价函数构建相应增量方程并迭代进行求解，当迭代在预设步数内成功收敛时，得到各节点的优化后的位置、仿射变换矩阵和平移向量。

用于优化节点的优化位置、仿射变换矩阵和平移向量的总代价函数为COST_deform＝λ₁Cost_deform1+λ₂Cost_deform2+λ₃Cost_deform3+λ₄Cost_deform4；λ₁、λ₂、λ₃、λ₄为四个权重系数，依经验分别应设置为1,10,100，100。

其中，变形需要具有刚性，根据刚性约束建立的节点的位置、仿射变换矩阵和平移向量的误差代价函数Cost_deform1，具体为：

其中，|| ||_F代表取Frobenius范数；

变形需要具有平滑性，根据平滑性约束建立的节点的位置、仿射变换和平移向量的误差代价函数Cost_deform2，具体为：

其中，n为集合

中节点的索引；

回环的约束实际上对应变形的起始点和目标点的一系列三维点对，这些三维点对之间的距离应当尽量小，由此约束构建关于节点的位置、仿射变换矩阵和平移向量的误差代价函数Cost_deform3，具体为：

其中，

和

代表当前回环约束对应的起始点和目标点，

和

代表在当前变形中应予以保留的、来自更早时间段内的回环约束对应的起始点和目标点，j为包含起始点和目标点的三维点对的索引；

重力加速度矢量的方向在变形后仍应该沿着重力加速度方向，由此约束构建关于节点的位置、仿射变换矩阵和平移向量的误差代价函数Cost_deform4，具体为：

其中，g^c0表示在世界坐标系c₀下的重力加速度矢量。

最后，利用各节点的优化后的位置、仿射变换矩阵和平移向量对三维重建模型进行变形。具体过程为：

i:对任一个surfel，找到对surfel变形直接影响的节点组成集合Θ；

ii:根据集合Θ中节点和surfel的关系，各节点的优化后的位置、仿射变换矩阵和平移向量通过上述变形函数作用到相应surfel上，使surfel的位置和法向量得到更新。

(c)当找不到Active局域模型的全局回环候选帧，或全局回环候选帧与Active局域模型的配准结果不满足要求，或利用全局回环候选帧对三维重建模型的节点进行非线性优化不收敛，得不到各节点的优化属性时，获取在RGBD传感器的优化位姿下的Inactive局域模型，通过将Active局域模型与Inactive局域模型进行RGBD稠密直接法配准判断Active局域模型与Inactive局域模型是否构成局域回环；

具体过程为：首先，针对三维重建模型中各surfel中，选出最后更新时间t满足t＜t_k+1-t_ths的surfel，t_ths为可设置阈值，然后在RGBD传感器的优化位姿

下，采用splatted rendering(喷溅渲染)方法根据选出的surfel构建局域模型，即为Inactive局域模型，将Active局域模型与Inactive局域模型视为构成一对局域回环候选；

然后，对Active局域模型与Inactive局域模型构成的这一对局域回环候选进行RGBD稠密直接法配准，在配准过程中，采用的非线性代价函数仅包含稠密光度误差部分和ICP误差部分，即非线性代价函数

非线性代价函数COST_local中变量的含义与求解过程与非线性代价函数COST相同，在此不再赘述。配准结束后，增量方程中的Hessian矩阵的特征值用于衡量该次配准得到的估计状态的估计误差，检验该估计误差，当该估计误差其小于预设阈值时，认为配准结果满足要求，该局域回环候选为真正的局域回环，当该估计误差大于预设阈值时，认为局域回环候选不为真正的局域回环。

(d)Active局域模型与Inactive局域模型构成的这一对局域回环候选的配准结果满足要求时，利用该局域回环中的Active局域模型与Inactive局域模型的关联所构成的局域回环约束对三维重建模型的节点进行非线性优化，获得各节点的优化属性，然后利用各节点的优化属性对全局三维重建模型进行变形。

步骤(d)中，利用局域回环约束对三维重建模型的节点进行非线性优化，获得各节点的优化属性的过程与步骤(b)中利用全局回环约束对三维重建模型的节点进行非线性优化，获得各节点的优化属性的过程基本一致，唯一区别在于，局域回环约束的非线性优化的代价函数中的Cost_deform3中不包含最后一项来自更早时间段内的回环约束的项，即对于局域回环约束的非线性优化，其代价函数中的Cost_deform3变为

而其余的代价函数表达式都与全局回环非线性优化问题的代价函数保持一致。

对局域回环约束建立的非线性优化问题进行求解后，获得各节点的优化后位置、仿射变换矩阵和平移向量，然后按照步骤(b)中步骤利用各节点的优化后位置、仿射变换矩阵和平移向量对三维重建模型进行变形。

(e)根据当前时刻的RGB数据对三维重建模型的surfel进行更新，以获得更新三维重建模型。

具体根据当前RGBD传感器在世界坐标系下的位姿，可以将当前RGBD数据对应为对三维重建模型中一系列surfel的新观测值。对于每个surfel，将对surfel的新观测值进行融合，更新surfel包含的各项数据，假设符号s表示三维重建模型中surfel的某项数据，则符号s′表示对该项数据s的新观测值，符号

表示该项数据融合更新后结果，则融合具体为：

其中，w为权重，w′为权重的新观测值，w′为通过RGBD传感器噪声阵对应的信息矩阵计算得到，具体计算与步骤2中相同，此处不再赘述，

为融合后的权重，p为surfel的空间位置，p′为p的新观测值，

为融合后的空间位置，n为surfel的法向量，n′为n的新观测值，

为融合后的法向量，r为surfel的半径，r′为r的新观测值，

为融合后的半径。

步骤5，在步骤4获得更新三维重建模型的基础上，在RGBD传感器的优化位姿下获取Active局域模型，即在时刻t_k+1的RGBD传感器的优化位姿

下，采用splattedrendering(喷溅渲染)方法根据选出的surfel构建Active局域模型，作为下一个时间段内该三维重建方法采用的优化RGB数据

和优化深度数据

步骤6，在步骤5的基础上，对步骤5获得的Active局域模型中RGBD数据进行随机蕨编码，加入到全局随机蕨编码库中。

上述基于优化的IMU紧耦合稠密直接RGBD的三维重建方法中，通过引入IMU加速度和角速度的观测信息，可以改善仅RGBD数据基于视觉稠密直接法来实时三维重建带来的不足。一方面在视觉传感器快速运动或经过几何结构无起伏和纹理缺失的地段时，IMU的观测信息将仍然提供一个相对可靠的约束；另一方面IMU的信息的加入使得系统的能观性获得了提升，重力方向中的俯仰角和横滚角由纯视觉情况下的不可观变为了可观，为位姿估计和稠密建图都提供了额外的约束。因此，通过将惯性导航与纯视觉稠密直接法紧耦合跟踪传感器位姿并进行三维重建，提升了系统能观性，加强三维重建方法鲁棒性，增强三维重建的全局一致性，保证了三维重建模型的精度。

以上所述的具体实施方式对本发明的技术方案和有益效果进行了详细说明，应理解的是以上所述仅为本发明的最优选实施例，并不用于限制本发明，凡在本发明的原则范围内所做的任何修改、补充和等同替换等，均应包含在本发明的保护范围之内。

Claims (10)

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传感器的优化位姿和优化的其它系统状态。

3.如权利要求2所述的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法，其特征在于，步骤(b)中，时间段[t_k,t_k+1]内惯导误差的Hessian矩阵和梯度的计算过程为：

首先，通过对时间段[t_k,t_k+1]内的IMU数据进行积分构建对位姿和速度变化的来自IMU的观测量，具体为：

其中，t_k为采集编号为k那一帧RGBD数据的时刻，t_k+1为采集编号为k+1的那一帧RGBD数据的时刻，t时刻为t_k到t_k+1时间范围中的IMU传感器采集数据的某时刻，

为t_k时刻IMU传感器坐标系到t_k+1时刻IMU传感器坐标系的位置变化，

为t时刻IMU传感器坐标系到t_k时刻IMU传感器坐标系的旋转矩阵，a_t为t时刻的比力，b_a为加速度计的零偏，b_g为陀螺仪的零偏，

为t_k时刻IMU传感器坐标系到t_k+1时刻IMU传感器坐标系的速度变化，

为t_k+1时刻IMU传感器坐标系到t_k时刻IMU传感器坐标系的姿态变化，

为t时刻IMU传感器坐标系到t_k时刻IMU传感器坐标系的姿态变化，ω_t为t时刻角速度，

代表四元数左乘符号，具体积分方式可以选择中值积分或四阶龙格-库塔积分；

然后，计算时间段[t_k,t_k+1]内惯导误差的Hessian矩阵和梯度，具体为：

惯导误差的残差记为

其中，位置残差e_P、姿态残差e_R、速度残差e_v、加速度计的零偏残差e_ba、陀螺仪的零偏残差e_bg分别表达为：

其中，

为RGBD传感器坐标系相对于IMU传感器坐标系的旋转矩阵，

为RGBD传感器坐标系c相对于IMU传感器坐标系b的平移向量，

为RGBD传感器坐标系c到IMU传感器坐标系b的旋转所对应的四元数；

为t_k时刻RGBD传感器坐标系c_k到世界坐标系c₀的旋转矩阵，

为I坐标系到世界坐标系c₀的旋转矩阵，g^I为在I坐标系下的重力加速度矢量，

为t_k+1时刻RGBD传感器坐标系c_k+1到世界坐标系c₀的旋转矩阵，[]_XYZ代表取四元数虚部3×1的向量，e_R的四元数

用汉密尔顿记法，

为t_k时刻RGBD传感器坐标系c_k到世界坐标系c₀的旋转所对应的四元数，

为t_k+1时刻RGBD传感器坐标系c_k+1到世界坐标系c₀的旋转所对应的四元数，Δt为t_k时刻和t_k+1时刻之间的时间间隔；

根据惯导误差的残差的表达式计算系统待估计变量取线性化点的值时惯导误差关于系统待估计变量的雅克比J_IMU为：

则根据雅克比J_IMU计算惯导误差的Hessian矩阵H_IMU和梯度b_IMU分别为：

时间段[t_k,t_k+1]内稠密光度误差的Hessian矩阵和梯度的计算过程为：

首先，对于当前时刻t_k+1的原始RGB数据E_k+1和上一时刻t_k的优化RGB数据

根据同一个空间点在这两个RGB图像中对应像素坐标位置的光度的差异计算稠密光度误差的残差表达为：

e_RGB,u＝I_ck+1(u')-I_ck(u)

其中，u为某一个空间点在RGB图像

中像素坐标，π是将空间点的齐次坐标写为三维坐标并投影到对应的二维像素坐标的操作，π^-1是其逆操作，根据点的二维像素坐标和其深度反投影到三维坐标并写为齐次坐标，于是u'表示u所对应的空间点投影到RGB图像E_k+1中的像素坐标，I_ck(u)表示在RGB数据

中位于像素索引u处的RGB数据所转换的光度，I_ck+1(u')表示在RGB数据E_k+1中位于像素索引u'处的RGB数据所转换的光度，T_c0,ck、T_c0,ck+1分别表示由t_k时刻RGBD传感器坐标系c_k和t_k+1时刻RGBD传感器坐标系c_k+1到世界坐标系c₀的欧氏变换矩阵，

为欧氏变换矩阵T_c0,ck+1的逆矩阵；

根据稠密光度误差的残差的表达式计算在系统待估计状态取线性化点的值时稠密光度误差的残差关于待估计状态的雅克比J_RGB,u为：

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

时间段[t_k,t_k+1]内ICP误差的Hessian矩阵和梯度的计算过程为：

首先，称当前时刻t_k+1的原始深度数据D_k+1中索引为i处的点为深度数据点i，深度数据点i的ICP误差的残差表达为：

其中，

为深度数据点i对应的空间点在t_k+1时刻RGBD传感器坐标系c_k+1中的齐次坐标，

为深度数据点i投影到t_k时刻优化深度数据

得到的深度数据点所对应的空间点在世界坐标系c₀下的法向量，

为深度数据点i投影到t_k时刻优化深度数据

得到的深度数据点所对应的空间点在世界坐标系c₀下的齐次坐标，[]_0:2表示取四维齐次坐标前三维；

根据ICP误差的残差表达式计算当系统待估计变量取线性化点的值时ICP误差的残差关于系统待估计变量的雅克比J_ICP,i为：

则当||e_ICP,i||≤ξ时，ICP误差的Hessian矩阵H_ICP和梯度b_ICP分别为：

则当||e_ICP,i||＞ξ时，ICP误差的Hessian矩阵H_ICP和梯度b_ICP分别为：

时间段[t_k,t_k+1]内采用的边缘化先验的Hessian矩阵

等于时间段[t_k-1,t_k]的非线性优化迭代收敛后，对时刻t_k之前的状态进行边缘化操作得到的先验Hessian矩阵H_prior，时间段[t_k,t_k+1]内采用的边缘化先验的梯度

表达为：

其中b_prior为时间段[t_k-1,t_k]的非线性优化迭代收敛后，对时刻t_k之前的状态进行边缘化操作得到的先验梯度，Δχ_k的定义为：

其中

代表时间段[t_k,t_k+1]内状态数据χ(k,k+1)中的时刻t_k的状态数据的值；而

代表在对[t_k-1,t_k]时间段内状态数据χ(k-1,k)中的时刻t_k之前的状态进行边缘化操作之后，时刻t_k的状态数据的值，deviation()代表对两个状态的值求取差异。

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

在迭代求解时，Hessian矩阵H_IMU、H_RGB、H_ICP、

梯度b_IMU、b_RGB、b_ICP、

的迭代计算过程中均遵循FEJ原则；

在步骤(c)的基础上，还需要依零空间正交化的原则对非线性优化估计得到的RGBD传感器的优化位姿进行调整。

5.如权利要求2所述的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法，在时间段[t_k,t_k+1]内求解获得包括RGBD传感器的优化位姿在内的系统优化状态后，还需要计算对时刻t_k+1之前的状态进行边缘化操作得到的先验Hessian矩阵和梯度，首先，当完成了时间段[t_k,t_k+1]内的非线性优化迭代后，将此时的总Hessian矩阵H和梯度b进行如下划分：

其中，H_k,k代表Hessian矩阵H中仅与t_k时刻的系统状态相关的观测所对应的约束，H_k,k+1代表Hessian矩阵H中同时与t_k时刻的系统状态和t_k+1时刻的系统状态相关的观测所对应的约束，H_k+1,k为H_k,k+1的转置，同样代表Hessian矩阵H中同时与t_k时刻的系统状态和t_k+1时刻的系统状态相关的观测所对应的约束，H_k+1,k+1代表Hessian矩阵H中仅与t_k+1时刻的状态相关的观测所对应的约束，b_k代表梯度b中与残差关于t_k时刻的系统状态的雅克比相关联的部分，b_k+1代表梯度b中与残差关于t_k+1时刻的系统状态的雅克比相关联的部分；然后，当对时刻t_k+1之前的状态进行边缘化操作时，t_k时刻的系统状态是要被边缘化移出优化窗口的状态，t_k+1时刻的系统状态则是与这个即将被边缘化移出的状态直接相关联的状态，依据舒尔补操作的原则，对时刻t_k+1之前的状态进行边缘化操作得到的先验Hessian矩阵H_prior和梯度b_prior将如下得到：

其中

代表若矩阵H_k,k可逆，则对其求逆，若矩阵H_k,k不可逆，则对其求伪逆。

6.如权利要求1所述的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法，所述三维重建模型构建步骤包括：

(a)获取在RGBD传感器的优化位姿下的Active局域模型，为Active局域模型寻找全局回环候选帧，将全局回环候选帧与Active局域模型进行RGBD稠密直接法配准；

(b)全局回环候选帧与Active局域模型的配准结果满足要求时，该全局回环候选帧是Active局域模型的真正的全局回环帧，即其与Active局域模型构成真正的全局回环；利用全局回环帧与Active局域模型的关联所构成的全局回环约束对三维重建模型的节点进行非线性优化，获得各节点的优化属性，所述优化属性包括各节点的优化后位置、仿射变换矩阵和平移向量，然后利用各节点的优化属性对全局三维重建模型进行变形；

(c)当找不到Active局域模型的全局回环候选帧，或全局回环候选帧与Active局域模型的配准结果不满足要求，或利用全局回环候选帧对三维重建模型的节点进行非线性优化不收敛，得不到各节点的优化属性时，获取在RGBD传感器的优化位姿下的Inactive局域模型，通过将Active局域模型与Inactive局域模型进行RGBD稠密直接法配准判断Active局域模型与Inactive局域模型是否构成局域回环；

(d)Active局域模型与Inactive局域模型构成的这一对局域回环候选的配准结果满足要求时，利用该局域回环中的Active局域模型与Inactive局域模型的关联所构成的局域回环约束对三维重建模型的节点进行非线性优化，获得各节点的优化属性，然后利用各节点的优化属性对全局三维重建模型进行变形；

(e)根据当前时刻的RGBD数据对三维重建模型的surfel进行更新，以获得更新三维重建模型。

7.如权利要求6所述的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法，步骤(a)具体过程为：

首先，针对三维重建模型中各surfel，选出最后更新时间t满足t_k+1-t_ths≤t≤t_k+1的surfel，t_ths为可设置阈值，然后在RGBD传感器的优化位姿

下，采用喷溅渲染方法根据选出的surfel构建局域模型，即为Active局域模型；

然后，对Active局域模型包含的RGBD数据进行降采样，再对降采样结果进行随机蕨编码后，在全局的随机蕨编码库中寻找与该随机蕨编码相似度最高的随机蕨编码所对应的数据帧作为当前Active局域模型的全局回环候选帧；

最后，将全局回环候选帧与Active局域模型进行RGBD稠密直接法配准，在配准过程中，采用的非线性代价函数仅包含ICP误差部分。

8.如权利要求6所述的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法，步骤(b)的具体过程为：

首先，节点是用于控制三维重建模型进行变形调整的一种数据结构；节点维护着一系列服务于变形三维重建模型的属性，包括位置s、时间t、3×3仿射变换矩阵M和3×1平移向量d，节点的位置和时间初始化为被采样的surfel的位置和更新时间，节点的仿射变换矩阵M和平移向量d分别初始化为单位矩阵I₃和零向量[0,0,0]^T；若把某个节点记为G，则节点G的最近邻节点集合记为

对于surfel，在此surfel更新时间前后一定范围内的节点中找出空间上最近邻的m个节点组成集合Θ；根据集合Θ中节点对于集合Θ对应的surfel的位置和法向量的变形函数就分别表示为：

其中，

为变形后的位置，

为变形后的法向量，l为集合Θ中节点的索引，γ_l＝(1-||p-s_l||₂/dis_max)²，dis_max即在集合Θ中节点与surfel距离之中最大距离；

然后，构建用于优化节点的位置、仿射变换和平移向量的总代价函数，采用高斯-牛顿法对该代价函数建立相应的增量方程并迭代进行求解，当迭代在预设步数内成功收敛时，得到各节点的优化后的位置、仿射变换矩阵和平移向量；

用于优化节点的优化位置、仿射变换矩阵和平移向量的总代价函数为COST_deform＝λ₁Cost_deform1+λ₂Cost_deform2+λ₃Cost_deform3+λ₄Cost_deform4；λ₁、λ₂、λ₃、λ₄为四个权重系数；

其中，变形需要具有刚性，根据刚性约束建立的节点的位置、仿射变换矩阵和平移向量的误差代价函数Cost_deform1，具体为：

其中，|| ||_F代表取Frobenius范数；

变形需要具有平滑性，根据平滑性约束建立的节点的位置、仿射变换和平移向量的误差代价函数Cost_deform2，具体为：

其中，n为集合

中节点的索引；

回环的约束实际上对应变形的起始点和目标点的一系列三维点对，这些三维点对之间的距离应当尽量小，由此约束构建关于节点的位置、仿射变换矩阵和平移向量的误差代价函数Cost_deform3，具体为：

其中，

和

代表当前回环约束对应的起始点和目标点，

和

代表在当前变形中应予以保留的、来自更早时间段内的回环约束对应的起始点和目标点，j为包含起始点和目标点的三维点对的索引；

重力加速度矢量的方向在变形后仍应该沿着重力加速度方向，由此约束构建关于节点的位置、仿射变换矩阵和平移向量的误差代价函数Cost_deform4，具体为：

其中，g^c0表示在世界坐标系c₀下的重力加速度矢量。

最后，利用各节点的优化后的位置、仿射变换矩阵和平移向量对三维重建模型进行变形，具体过程为：

i:对任一个surfel，找到对surfel变形直接影响的节点组成集合Θ；

ii:根据集合Θ中节点和surfel的关系，各节点的优化后的位置、仿射变换矩阵和平移向量通过上述变形函数作用到相应surfel上，使surfel的位置和法向量得到更新。

9.如权利要求6所述的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法，步骤(c)的具体过程为：

首先，针对三维重建模型中各surfel中，选出最后更新时间t满足t＜t_k+1-t_ths的surfel，t_ths为可设置阈值，然后在RGBD传感器的优化位姿

下，采用喷溅渲染方法根据选出的surfel构建局域模型，即为Inctive局域模型；

然后，对Active局域模型与Inactive局域模型构成的这一对局域回环候选进行RGBD稠密直接法配准，在配准过程中，采用的非线性代价函数仅包含稠密光度误差部分和ICP误差部分；

步骤(d)中，局域回环约束的非线性优化的代价函数中的Cost_deform3为：

10.如权利要求1所述的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法，在三维重建模型构建步骤的基础上，在RGBD传感器的优化位姿下获取Active局域模型，即在时刻t_k+1的RGBD传感器的优化位姿

下，采用喷溅渲染方法根据选出的surfel构建Active局域模型，作为下一个时间段内该三维重建方法采用的优化RGB数据

和优化深度数据

同时对Active局域模型中RGBD数据进行随机蕨编码，加入到全局随机蕨编码库中。

Images