CN111862316A - 一种基于优化的imu紧耦合稠密直接rgbd的三维重建方法 - Google Patents
一种基于优化的imu紧耦合稠密直接rgbd的三维重建方法 Download PDFInfo
- Publication number
- CN111862316A CN111862316A CN202010737363.9A CN202010737363A CN111862316A CN 111862316 A CN111862316 A CN 111862316A CN 202010737363 A CN202010737363 A CN 202010737363A CN 111862316 A CN111862316 A CN 111862316A
- Authority
- CN
- China
- Prior art keywords
- rgbd
- imu
- time
- data
- error
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 103
- 238000005457 optimization Methods 0.000 title claims abstract description 66
- 238000010168 coupling process Methods 0.000 title claims abstract description 13
- 238000005859 coupling reaction Methods 0.000 title claims abstract description 13
- 230000008878 coupling Effects 0.000 title abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 97
- 239000013598 vector Substances 0.000 claims description 60
- 238000013519 translation Methods 0.000 claims description 31
- 230000009466 transformation Effects 0.000 claims description 30
- PXFBZOLANLWPMH-UHFFFAOYSA-N 16-Epiaffinine Natural products C1C(C2=CC=CC=C2N2)=C2C(=O)CC2C(=CC)CN(C)C1C2CO PXFBZOLANLWPMH-UHFFFAOYSA-N 0.000 claims description 26
- 230000008569 process Effects 0.000 claims description 26
- 230000001133 acceleration Effects 0.000 claims description 14
- 238000004364 calculation method Methods 0.000 claims description 14
- 230000014509 gene expression Effects 0.000 claims description 10
- 239000000126 substance Substances 0.000 claims description 10
- 238000009877 rendering Methods 0.000 claims description 8
- 230000010354 integration Effects 0.000 claims description 6
- 238000010276 construction Methods 0.000 claims description 5
- 230000008859 change Effects 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 4
- 238000013480 data collection Methods 0.000 claims description 2
- 238000013075 data extraction Methods 0.000 claims description 2
- 230000002441 reversible effect Effects 0.000 claims description 2
- 230000000295 complement effect Effects 0.000 claims 1
- 125000004122 cyclic group Chemical group 0.000 claims 1
- 230000004069 differentiation Effects 0.000 claims 1
- 230000000007 visual effect Effects 0.000 description 6
- 230000004927 fusion Effects 0.000 description 3
- 238000005259 measurement Methods 0.000 description 3
- 238000007792 addition Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 239000012530 fluid Substances 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
- G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
- G06T7/344—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods involving models
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2200/00—Indexing scheme for image data processing or generation, in general
- G06T2200/08—Indexing scheme for image data processing or generation, in general involving all processing steps from image acquisition to 3D model generation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Automation & Control Theory (AREA)
- Computer Graphics (AREA)
- Geometry (AREA)
- Software Systems (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于优化的IMU紧耦合稠密直接RGBD的三维重建方法,包括:(1)从上一时刻重建的局域模型中采集优化RGB数据和优化深度数据,获取原始RGB数据、原始深度数据和IMU数据;(2)根据惯导误差、稠密光度误差以及ICP误差联合构建非线性代价函数,以该非线性代价函数的最小化为目标构成非线性优化问题,对该非线性优化问题以高斯‑牛顿法求解,从而估计RGBD传感器优化位姿;(3)在RGBD传感器的优化位姿下利用当前时刻的原始RGB数据和原始深度数据进行实时三维重建,并在找到回环约束的情况下使用节点对三维重建模型进行变形以保证全局一致性。该方法提高了三维重建的准确性和效率。
Description
技术领域
本发明涉及三维重建领域,具体涉及一种基于优化的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的三维重建方法。其主要思想是,在上一时刻tk和当前时刻tk+1之间的时间段内,根据上一时刻已经构建的三维重建模型,利用当前时刻采集的RGBD传感器数据对已经构建的三维重建模型进行实时更新的方法。在更新时为了解决单纯的视觉稠密直接法在视觉传感器位姿跟踪估计上存在着鲁棒性的缺陷问题,还引入了IMU传感器数据,利用IMU传感器数据的加入提升了对RGBD传感器位姿的估计的准确性和鲁棒性,在此优化位姿下利用RGBD传感器数据在已经构建的三维重建模型基础上进行模型的更新,大大提升了三维重建模型的准确性和效率。
图1是本发明提供的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法的流程图。如图1所示,实施例提供的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法包括以下步骤:
步骤1,原始的RGBD传感器数据的采集、IMU传感器数据的采集和优化的RGBD数据的提取。
根据本发明的主要思想,步骤1中主要采集两部分传感器数据,第一部分为RGBD传感器在当前时刻tk+1采集的原始RGB数据Ek+1和原始深度数据Dk+1,组成RGBD数据,其中,RGBD传感器是指能够实时地提供稠密深度及RGB数据的传感器,包括但不限于双目相机、双目相机与深度摄像头的组合、双目相机与面阵型LiDAR的组合等。
第二部分为利用IMU传感器采集上一时刻tk和当前时刻tk+1时间区间内的比力序列{at,t∈[tk,tk+1]}和角速度序列{ωt,t∈[tk,tk+1]},组成IMU数据,由于IMU传感器的采集频率一般远大于RGBD传感器的采集频率,因此,在不包括两端的时间的开区间(tk,tk+1)内,总会存在若干组IMU数据。本实施例中,IMU传感器可以是陀螺仪和加速度计组成的IMU,IMU集成到RGBD传感器上,用于为构建RGBD传感器的位姿的观测量提供原始数据。
根据本发明的主要思想,步骤1中还需获取一种优化数据。这部分数据是从时间段[tk-1,tk]结束时更新的三维重建模型中提取的优化RGB数据和优化深度数据该提取过程具体是指:在上一时刻tk的RGBD传感器的通过非线性优化得到的估计位姿下,对时间段[tk-1,tk]结束时经过更新的三维重建模型使用喷溅渲染(splatted rendering)法,提取出一部分surfel,这部分surfel中的那些自身的更新时间与时刻tk之间的间隔小于一定值(该值一般设置为tk-1与tk的时间间隔相近)的那些对象就构成了相应的Active局域模型,该局域模型中的surfel的位置和颜色信息投影到上一时刻tk的RGBD传感器估计位姿下得到优化RGB数据和优化深度数据
步骤2,在判断当前时刻采集的RGBD数据和IMU数据为全局第一帧时,直接根据RGBD数据构建三维重建模型,之后获取RGBD传感器的初始位姿下的Active局域模型。
该三维重建方法中,将全局第一帧对应的RGBD传感器坐标系c0作为世界坐标系,全局第一帧RGBD数据作为全局三维重建模型的最初数据。三维重建模型中的三维点,均采用带有方向的表面元surfel的方式来表示,每个surfel包含空间位置p、权重w、法向量n、颜色c、半径r、初始化时间t0和最近更新时间t。
针对权重,采用RGBD传感器噪声阵对应的信息矩阵来构建权重,其中,RGBD传感器噪声阵对应的信息矩阵记作ΛICP,具体为:
针对原始RGBD数据中索引为i的数据点,其信息矩阵为:
若该数据点第一次被观测到,则其对应的surfel的权重在初始化时应为 分别为采集点i时RGBD传感器在x、y、z方向上的不确定度,该三个不确定度的值取决于RGBD传感器噪声特性以及该采集点所对应的真实空间内该处的被采集物体的实际情况。
在三维重建模型的基础上,在RGBD传感器的初始位姿下获取Active局域模型,即在RGBD传感器的初始位姿(与世界坐标系间无相对平移和相对旋转的位姿)下,采用splatted rendering(喷溅渲染)方法根据选出的surfel构建Active局域模型,该局域模型内的surfel的位置和颜色信息投影到RGBD传感器的初始位姿下,作为下一个时间段内该三维重建方法采用的优化RGB数据和优化深度数据
步骤3,在判断当前时刻采集的RGBD数据和IMU数据不为全局第一帧时(此时k>=0),根据原始RGBD数据、优化RGBD数据和IMU数据对RGBD传感器位姿进行估计,获得RGBD传感器的优化位姿。
具体地,根据时间段[tk,tk+1]内的IMU数据构建的误差、根据当前时刻tk+1的原始RGB数据Ek+1和上一时刻tk的优化RGB数据构建的稠密光度误差以及根据当前时刻tk+1的原始深度数据Dk+1和上一时刻tk的优化深度数据构建的ICP误差联合构建非线性代价函数,以该非线性代价函数的最小化为目标,对该非线性代价函数对应的增量方程求解,获得包括RGBD传感器位姿在内的***待估计状态的增量,依据该增量对***待估计状态进行估计,获得优化的***状态,其中包括RGBD传感器的优化位姿。具体过程包括:
(a)根据IMU数据构建的误差、RGB数据构建的稠密光度误差以及根据深度数据构建的ICP误差联合构建非线性代价函数COST为:
其中,ΛIMU、ΛRGB和ΛICP分别为根据IMU数据构建的惯导误差、根据RGB数据构建的稠密光度误差和根据深度数据构建的ICP误差对应的信息矩阵,eIMU、eRGB和eICP分别为惯导残差、光度残差和ICP残差,和为边缘化的先验Hessian矩阵和梯度,u为RGB数据的索引,i为深度数据的索引。
本实施例中,为了更好地利用深度数据的信息,在构建非线性优化问题中深度相关的残差时,不采用深度的重投影残差,而是根据深度数据基于点到面(point to plane)的ICP方法来构建残差,简称ICP残差,由ICP残差构建的那部分代价函数我们简称为ICP误差;
其中,ξ为残差阈值,该鲁棒核函数主要用于在错误匹配时降低错误匹配的影响,具体通过引入ξ为残差阈值对鲁棒核函数值进行分段,当ICP残差的值小于残差阈值时,鲁棒核函数值与ICP残差呈二次关系,当ICP残差的值大于残差阈值时,鲁棒核函数值与ICP残差呈线性关系,以此降低较可能来自误匹配的ICP残差对总代价函数的影响,进而提升匹配准确性。
(b)以高斯-牛顿法原则构建非线性代价函数对应的增量方程为:
Hδχ(k,k+1)=b
其中,HIMU、bIMU分别为惯导误差的Hessian矩阵和梯度,HRGB、bRGB分别为稠密光度误差的Hessian矩阵和梯度,HICP、bICP分别为ICP误差的Hessian矩阵和梯度,分别为边缘化先验的Hessian矩阵和梯度,δχ(k,k+1)为***状态χ(k,k+1)的微扰;
本实施例中,时间段[tk,tk+1]内***状态数据χ(k,k+1)是由RGBD传感器位姿、IMU传感器速度、IMU传感器零偏、IMU传感器与RGBD传感器外参(即IMU传感器和RGBD传感器之间相对位姿)、世界坐标系与I坐标系间相对旋转这些待估计状态所组成的,以符号表述为:
其中,分别为在世界坐标系下观察的tk时刻RGBD传感器坐标系ck和tk+1时刻RGBD传感器坐标系ck+1相对于世界坐标系c0的平移向量,分别为tk时刻RGBD传感器坐标系ck和tk+1时刻RGBD传感器坐标系ck+1到世界坐标系c0的旋转所对应的四元数,分别为tk时刻和tk+1时刻在I坐标系下观察的IMU传感器坐标系bk和bk+1的速度,分别为tk时刻和tk+1时刻在IMU传感器坐标系bk和bk+1下加速度计的零偏,分别为tk时刻和tk+1时刻在IMU传感器坐标系bk和bk+1下陀螺仪的零偏,为世界坐标系c0到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),因此,需要改用李代数空间的微扰来表达这些旋转的待估计量的微扰,于是,用和分别表示对和在李代数空间上的微扰,用表示对在李代数空间上的微扰,用表示对在李代数空间上的微扰;
下述残差关于待估计状态的雅克比即基于这样的微扰规则。
时间段[tk,tk+1]内惯导误差的Hessian矩阵和梯度的计算过程为:
首先,通过对时间段[tk,tk+1]内的IMU数据进行积分构建对位姿和速度变化的来自IMU的观测量,具体为:
其中,对每一帧采集的RGBD数据进行编号,全局最开始的一帧RGBD数据编号为0,以此类推,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矩阵和梯度,具体为:
其中,特别地,和都是代表RGBD传感器坐标系与IMU传感器坐标系之间的相对位置和姿态的量,这些量中表示RGBD传感器坐标系和IMU传感器坐标系的字母b和c都没有使用帧编号作为下标,这是因为在算法开始前RGBD传感器和IMU传感器已经固连好,RGBD传感器坐标系与IMU传感器坐标系之间的相对位置和姿态不随时间改变。于是,为RGBD传感器坐标系相对于IMU传感器坐标系的旋转矩阵,为RGBD传感器坐标系c相对于IMU传感器坐标系b的平移向量,为RGBD传感器坐标系c到IMU传感器坐标系b的旋转所对应的四元数,为tk时刻RGBD传感器坐标系到世界坐标系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矩阵和梯度的计算过程为:
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为:
各对稠密光度误差对应关系之间计算互相没有依赖,因此稠密光度误差的Hessian和梯度计算采用并行计算的方式进行。
时间段[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分别为:
各深度数据点的point-to-plane的ICP误差对应关系之间的计算互相没有依赖,因此ICP误差的Hessian和梯度计算采用并行计算的方式进行。
时间段[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()代表对两个状态的值求取差异,具体做法为,对两个状态中代表旋转的量,求取它们之间的相对旋转在李代数空间上的对应值;其余的量直接用向量减法求取它们在向量空间的差值。
(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矩阵HIMU、HRGB、HICP、梯度bIMU、bRGB、bICP、的迭代计算过程中均遵循FEJ(First Estimate Jacobian)原则,与被边缘化的状态直接相关的状态的雅克比和残差必须固定在该边缘化操作发生时其所在的线性化点上进行计算。
为了尽量减小不可观状态的漂移对重建三维重建模型的影响,在步骤(c)的基础上,还需要依零空间正交化的原则对非线性优化估计得到的RGBD传感器的优化位姿进行调整。
在时间段[tk,tk+1]内求解获得包括RGBD传感器的优化位姿在内的***优化状态后,还需要计算对时刻tk+1之前的状态进行边缘化操作得到的先验Hessian矩阵和梯度,首先,当完成了时间段[tk,tk+1]内的非线性优化迭代后,将此时的总Hessian矩阵H和梯度b进行如下划分:
其中,Hk,k代表Hessian矩阵H中仅与tk时刻的状态相关的观测所对应的约束,Hk,k+1代表Hessian矩阵H中同时与tk时刻的***状态和tk+1时刻的***状态相关的观测所对应的约束,Hk+1,k为Hk,k+1的转置,同样代表Hessian矩阵H中同时与tk时刻的***状态和tk+1时刻的***状态相关的观测所对应的约束,Hk+1,k+1代表Hessian矩阵H中仅与tk时刻的状态相关的观测所对应的约束,bk代表梯度b中与残差关于tk时刻的***状态的雅克比相关联的部分,bk+1代表梯度b中与残差关于tk+1时刻的***状态的雅克比相关联的部分;然后,当对时刻tk+1之前的状态进行边缘化操作,tk时刻的***状态是要被边缘化移出优化窗口的状态,tk+1时刻的***状态则是与这个即将被边缘化移出的状态直接相关联的状态,依据舒尔补操作(Schur complement operation)的原则,对时刻tk+1之前的状态进行边缘化操作得到的先验Hessian矩阵Hprior和梯度bprior将如下得到:
步骤4,在RGBD传感器的优化位姿下利用当前时刻的原始RGB数据和原始深度数据在局域模型上进行实时三维重建,并在找到回环约束的情况下使用节点对三维重建模型进行变形以保证全局一致性。
本实施例中,步骤4三维重建的具体过程包括:
(a)获取在RGBD传感器的优化位姿下的Active局域模型,为Active局域模型寻找全局回环候选帧,将全局回环候选帧与Active局域模型进行RGBD稠密直接法配准。
具体过程为:首先,针对三维重建模型中各surfel,选出最后更新时间t满足tk+1-tths≤t≤tk+1的surfel,tths为可设置阈值,然后在时刻tk+1的RGBD传感器的优化位姿下,采用splatted rendering(喷溅渲染)方法根据选出的surfel构建局域模型,即为Active局域模型;
然后,对Active局域模型包含的RGBD数据进行降采样,再对降采样结果进行随机蕨编码后,在全局的随机蕨编码库中寻找与该随机蕨编码相似度最高的随机蕨编码所对应的数据帧作为当前Active局域模型的全局回环候选帧;
最后,将全局回环候选帧与Active局域模型进行RGBD稠密直接法配准,在配准过程中,采用的非线性代价函数仅包含ICP误差部分,即非线性代价函数非线性代价函数COSTglobal中变量的含义与求解过程与非线性代价函数COST相同,在此不再赘述。
非线性优化后,结束迭代时增量方程中的Hessian矩阵的特征值用于衡量该非线性优化得到的估计状态的估计误差,检验这个估计误差,当该估计误差小于预设误差阈值时,认为配准结果满足要求,该全局回环候选帧为真正的全局回环帧,其与Active局域模型构成一个真正的全局回环;当该估计误差大于预设误差阈值时,认为配准结果不满足要求,该全局回环候选帧不为真正的全局回环帧。
(b)全局回环候选帧与Active局域模型的配准结果满足要求时,该全局回环候选帧是Active局域模型的真正的全局回环帧,即其与Active局域模型构成真正的全局回环;利用全局回环帧与Active局域模型的关联所构成的全局回环约束对三维重建模型的节点进行非线性优化,获得各节点的优化属性,所述优化属性包括各节点的优化后位置、仿射变换矩阵和平移向量,然后利用各节点的优化属性对全局三维重建模型进行变形;
具体过程为:首先,节点是通过特定手段构造的,用于控制三维重建模型进行变形调整的一种数据结构,节点维护着一系列服务于变形三维重建模型的属性,包括位置s、时间t、3×3仿射变换矩阵M和3×1平移向量d,节点具体的构造方法为,对当前三维重建模型中的surfel进行均匀采样得到一系列surfel,利用这些surfel构造相同数量的节点,节点的位置和时间初始化为被采样的surfel的位置和更新时间,节点的仿射变换矩阵M和平移向量d分别初始化为单位矩阵I3和零向量[0,0,0]T;若把某个节点记为G,则节点G的最近邻节点集合记为对于surfel,在此surfel更新时间前后一定范围内的节点中找出空间上最近邻的m个节点组成集合Θ,m的典型值取4;根据集合Θ中节点对于集合Θ对应的surfel的位置和法向量的变形函数就分别表示为:
然后,构建用于优化节点的位置、仿射变换和平移向量的总代价函数,采用高斯-牛顿法对该代价函数构建相应增量方程并迭代进行求解,当迭代在预设步数内成功收敛时,得到各节点的优化后的位置、仿射变换矩阵和平移向量。
用于优化节点的优化位置、仿射变换矩阵和平移向量的总代价函数为COSTdeform=λ1Costdeform1+λ2Costdeform2+λ3Costdeform3+λ4Costdeform4;λ1、λ2、λ3、λ4为四个权重系数,依经验分别应设置为1,10,100,100。
其中,变形需要具有刚性,根据刚性约束建立的节点的位置、仿射变换矩阵和平移向量的误差代价函数Costdeform1,具体为:
其中,|| ||F代表取Frobenius范数;
变形需要具有平滑性,根据平滑性约束建立的节点的位置、仿射变换和平移向量的误差代价函数Costdeform2,具体为:
回环的约束实际上对应变形的起始点和目标点的一系列三维点对,这些三维点对之间的距离应当尽量小,由此约束构建关于节点的位置、仿射变换矩阵和平移向量的误差代价函数Costdeform3,具体为:
重力加速度矢量的方向在变形后仍应该沿着重力加速度方向,由此约束构建关于节点的位置、仿射变换矩阵和平移向量的误差代价函数Costdeform4,具体为:
其中,gc0表示在世界坐标系c0下的重力加速度矢量。
最后,利用各节点的优化后的位置、仿射变换矩阵和平移向量对三维重建模型进行变形。具体过程为:
i:对任一个surfel,找到对surfel变形直接影响的节点组成集合Θ;
ii:根据集合Θ中节点和surfel的关系,各节点的优化后的位置、仿射变换矩阵和平移向量通过上述变形函数作用到相应surfel上,使surfel的位置和法向量得到更新。
(c)当找不到Active局域模型的全局回环候选帧,或全局回环候选帧与Active局域模型的配准结果不满足要求,或利用全局回环候选帧对三维重建模型的节点进行非线性优化不收敛,得不到各节点的优化属性时,获取在RGBD传感器的优化位姿下的Inactive局域模型,通过将Active局域模型与Inactive局域模型进行RGBD稠密直接法配准判断Active局域模型与Inactive局域模型是否构成局域回环;
具体过程为:首先,针对三维重建模型中各surfel中,选出最后更新时间t满足t<tk+1-tths的surfel,tths为可设置阈值,然后在RGBD传感器的优化位姿下,采用splatted rendering(喷溅渲染)方法根据选出的surfel构建局域模型,即为Inactive局域模型,将Active局域模型与Inactive局域模型视为构成一对局域回环候选;
然后,对Active局域模型与Inactive局域模型构成的这一对局域回环候选进行RGBD稠密直接法配准,在配准过程中,采用的非线性代价函数仅包含稠密光度误差部分和ICP误差部分,即非线性代价函数非线性代价函数COSTlocal中变量的含义与求解过程与非线性代价函数COST相同,在此不再赘述。配准结束后,增量方程中的Hessian矩阵的特征值用于衡量该次配准得到的估计状态的估计误差,检验该估计误差,当该估计误差其小于预设阈值时,认为配准结果满足要求,该局域回环候选为真正的局域回环,当该估计误差大于预设阈值时,认为局域回环候选不为真正的局域回环。
(d)Active局域模型与Inactive局域模型构成的这一对局域回环候选的配准结果满足要求时,利用该局域回环中的Active局域模型与Inactive局域模型的关联所构成的局域回环约束对三维重建模型的节点进行非线性优化,获得各节点的优化属性,然后利用各节点的优化属性对全局三维重建模型进行变形。
步骤(d)中,利用局域回环约束对三维重建模型的节点进行非线性优化,获得各节点的优化属性的过程与步骤(b)中利用全局回环约束对三维重建模型的节点进行非线性优化,获得各节点的优化属性的过程基本一致,唯一区别在于,局域回环约束的非线性优化的代价函数中的Costdeform3中不包含最后一项来自更早时间段内的回环约束的项,即对于局域回环约束的非线性优化,其代价函数中的Costdeform3变为而其余的代价函数表达式都与全局回环非线性优化问题的代价函数保持一致。
对局域回环约束建立的非线性优化问题进行求解后,获得各节点的优化后位置、仿射变换矩阵和平移向量,然后按照步骤(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局域模型,即在时刻tk+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误差对应的信息矩阵,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矩阵和梯度,具体为:
其中,为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矩阵和梯度的计算过程为:
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()代表对两个状态的值求取差异。
5.如权利要求2所述的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法,在时间段[tk,tk+1]内求解获得包括RGBD传感器的优化位姿在内的***优化状态后,还需要计算对时刻tk+1之前的状态进行边缘化操作得到的先验Hessian矩阵和梯度,首先,当完成了时间段[tk,tk+1]内的非线性优化迭代后,将此时的总Hessian矩阵H和梯度b进行如下划分:
其中,Hk,k代表Hessian矩阵H中仅与tk时刻的***状态相关的观测所对应的约束,Hk,k+1代表Hessian矩阵H中同时与tk时刻的***状态和tk+1时刻的***状态相关的观测所对应的约束,Hk+1,k为Hk,k+1的转置,同样代表Hessian矩阵H中同时与tk时刻的***状态和tk+1时刻的***状态相关的观测所对应的约束,Hk+1,k+1代表Hessian矩阵H中仅与tk+1时刻的状态相关的观测所对应的约束,bk代表梯度b中与残差关于tk时刻的***状态的雅克比相关联的部分,bk+1代表梯度b中与残差关于tk+1时刻的***状态的雅克比相关联的部分;然后,当对时刻tk+1之前的状态进行边缘化操作时,tk时刻的***状态是要被边缘化移出优化窗口的状态,tk+1时刻的***状态则是与这个即将被边缘化移出的状态直接相关联的状态,依据舒尔补操作的原则,对时刻tk+1之前的状态进行边缘化操作得到的先验Hessian矩阵Hprior和梯度bprior将如下得到:
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满足tk+1-tths≤t≤tk+1的surfel,tths为可设置阈值,然后在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分别初始化为单位矩阵I3和零向量[0,0,0]T;若把某个节点记为G,则节点G的最近邻节点集合记为对于surfel,在此surfel更新时间前后一定范围内的节点中找出空间上最近邻的m个节点组成集合Θ;根据集合Θ中节点对于集合Θ对应的surfel的位置和法向量的变形函数就分别表示为:
然后,构建用于优化节点的位置、仿射变换和平移向量的总代价函数,采用高斯-牛顿法对该代价函数建立相应的增量方程并迭代进行求解,当迭代在预设步数内成功收敛时,得到各节点的优化后的位置、仿射变换矩阵和平移向量;
用于优化节点的优化位置、仿射变换矩阵和平移向量的总代价函数为COSTdeform=λ1Costdeform1+λ2Costdeform2+λ3Costdeform3+λ4Costdeform4;λ1、λ2、λ3、λ4为四个权重系数;
其中,变形需要具有刚性,根据刚性约束建立的节点的位置、仿射变换矩阵和平移向量的误差代价函数Costdeform1,具体为:
其中,|| ||F代表取Frobenius范数;
变形需要具有平滑性,根据平滑性约束建立的节点的位置、仿射变换和平移向量的误差代价函数Costdeform2,具体为:
回环的约束实际上对应变形的起始点和目标点的一系列三维点对,这些三维点对之间的距离应当尽量小,由此约束构建关于节点的位置、仿射变换矩阵和平移向量的误差代价函数Costdeform3,具体为:
重力加速度矢量的方向在变形后仍应该沿着重力加速度方向,由此约束构建关于节点的位置、仿射变换矩阵和平移向量的误差代价函数Costdeform4,具体为:
其中,gc0表示在世界坐标系c0下的重力加速度矢量。
最后,利用各节点的优化后的位置、仿射变换矩阵和平移向量对三维重建模型进行变形,具体过程为:
i:对任一个surfel,找到对surfel变形直接影响的节点组成集合Θ;
ii:根据集合Θ中节点和surfel的关系,各节点的优化后的位置、仿射变换矩阵和平移向量通过上述变形函数作用到相应surfel上,使surfel的位置和法向量得到更新。
9.如权利要求6所述的基于优化的IMU紧耦合稠密直接RGBD的三维重建方法,步骤(c)的具体过程为:
首先,针对三维重建模型中各surfel中,选出最后更新时间t满足t<tk+1-tths的surfel,tths为可设置阈值,然后在RGBD传感器的优化位姿 下,采用喷溅渲染方法根据选出的surfel构建局域模型,即为Inctive局域模型;
然后,对Active局域模型与Inactive局域模型构成的这一对局域回环候选进行RGBD稠密直接法配准,在配准过程中,采用的非线性代价函数仅包含稠密光度误差部分和ICP误差部分;
步骤(d)中,局域回环约束的非线性优化的代价函数中的Costdeform3为:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010737363.9A CN111862316B (zh) | 2020-07-28 | 2020-07-28 | 一种基于优化的imu紧耦合稠密直接rgbd的三维重建方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010737363.9A CN111862316B (zh) | 2020-07-28 | 2020-07-28 | 一种基于优化的imu紧耦合稠密直接rgbd的三维重建方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111862316A true CN111862316A (zh) | 2020-10-30 |
CN111862316B CN111862316B (zh) | 2024-01-05 |
Family
ID=72948614
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010737363.9A Active CN111862316B (zh) | 2020-07-28 | 2020-07-28 | 一种基于优化的imu紧耦合稠密直接rgbd的三维重建方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111862316B (zh) |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112179373A (zh) * | 2020-08-21 | 2021-01-05 | 同济大学 | 一种视觉里程计的测量方法及视觉里程计 |
CN112612029A (zh) * | 2020-12-24 | 2021-04-06 | 哈尔滨工业大学芜湖机器人产业技术研究院 | 融合ndt和icp的栅格地图定位方法 |
CN112945240A (zh) * | 2021-03-16 | 2021-06-11 | 北京三快在线科技有限公司 | 特征点位置的确定方法、装置、设备及可读存储介质 |
CN112991515A (zh) * | 2021-02-26 | 2021-06-18 | 山东英信计算机技术有限公司 | 一种三维重建方法、装置及相关设备 |
WO2023197785A1 (zh) * | 2022-04-12 | 2023-10-19 | 清华大学 | 局域轨道函数的三维重构方法及装置 |
CN117710469A (zh) * | 2024-02-06 | 2024-03-15 | 四川大学 | 一种基于rgb-d传感器的在线稠密重建方法及*** |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20140139639A1 (en) * | 2013-01-30 | 2014-05-22 | Qualcomm Incorporated | Real-time 3d reconstruction with power efficient depth sensor usage |
CN106780576A (zh) * | 2016-11-23 | 2017-05-31 | 北京航空航天大学 | 一种面向rgbd数据流的相机位姿估计方法 |
US20180018787A1 (en) * | 2016-07-18 | 2018-01-18 | King Abdullah University Of Science And Technology | System and method for three-dimensional image reconstruction using an absolute orientation sensor |
CN109087394A (zh) * | 2018-08-02 | 2018-12-25 | 福州大学 | 一种基于低成本rgb-d传感器的实时室内三维重建方法 |
CN110986968A (zh) * | 2019-10-12 | 2020-04-10 | 清华大学 | 三维重建中实时全局优化和错误回环判断的方法及装置 |
-
2020
- 2020-07-28 CN CN202010737363.9A patent/CN111862316B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20140139639A1 (en) * | 2013-01-30 | 2014-05-22 | Qualcomm Incorporated | Real-time 3d reconstruction with power efficient depth sensor usage |
US20180018787A1 (en) * | 2016-07-18 | 2018-01-18 | King Abdullah University Of Science And Technology | System and method for three-dimensional image reconstruction using an absolute orientation sensor |
CN106780576A (zh) * | 2016-11-23 | 2017-05-31 | 北京航空航天大学 | 一种面向rgbd数据流的相机位姿估计方法 |
CN109087394A (zh) * | 2018-08-02 | 2018-12-25 | 福州大学 | 一种基于低成本rgb-d传感器的实时室内三维重建方法 |
CN110986968A (zh) * | 2019-10-12 | 2020-04-10 | 清华大学 | 三维重建中实时全局优化和错误回环判断的方法及装置 |
Non-Patent Citations (3)
Title |
---|
吴珂: "基于GPU的空间数据索引与查询技术研究", 中国优秀硕士学位论文全文数据库, vol. 2019, no. 02 * |
徐浩楠;余雷;费树岷;: "基于半直接法SLAM的大场景稠密三维重建***", 模式识别与人工智能, no. 05 * |
李健;杨苏;刘富强;何斌;: "RGBD融合明暗恢复形状的全视角三维重建技术研究", 数据采集与处理, no. 01 * |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112179373A (zh) * | 2020-08-21 | 2021-01-05 | 同济大学 | 一种视觉里程计的测量方法及视觉里程计 |
CN112612029A (zh) * | 2020-12-24 | 2021-04-06 | 哈尔滨工业大学芜湖机器人产业技术研究院 | 融合ndt和icp的栅格地图定位方法 |
CN112991515A (zh) * | 2021-02-26 | 2021-06-18 | 山东英信计算机技术有限公司 | 一种三维重建方法、装置及相关设备 |
CN112945240A (zh) * | 2021-03-16 | 2021-06-11 | 北京三快在线科技有限公司 | 特征点位置的确定方法、装置、设备及可读存储介质 |
WO2023197785A1 (zh) * | 2022-04-12 | 2023-10-19 | 清华大学 | 局域轨道函数的三维重构方法及装置 |
CN117710469A (zh) * | 2024-02-06 | 2024-03-15 | 四川大学 | 一种基于rgb-d传感器的在线稠密重建方法及*** |
CN117710469B (zh) * | 2024-02-06 | 2024-04-12 | 四川大学 | 一种基于rgb-d传感器的在线稠密重建方法及*** |
Also Published As
Publication number | Publication date |
---|---|
CN111862316B (zh) | 2024-01-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109307508B (zh) | 一种基于多关键帧的全景惯导slam方法 | |
CN111862316A (zh) | 一种基于优化的imu紧耦合稠密直接rgbd的三维重建方法 | |
CN109993113B (zh) | 一种基于rgb-d和imu信息融合的位姿估计方法 | |
CN111811506B (zh) | 视觉/惯性里程计组合导航方法、电子设备及存储介质 | |
CN109676604B (zh) | 机器人曲面运动定位方法及其运动定位*** | |
CN109242873B (zh) | 一种基于消费级彩色深度相机对物体进行360度实时三维重建的方法 | |
CN112304307A (zh) | 一种基于多传感器融合的定位方法、装置和存储介质 | |
CN110702107A (zh) | 一种单目视觉惯性组合的定位导航方法 | |
CN111024066A (zh) | 一种无人机视觉-惯性融合室内定位方法 | |
CN109540126A (zh) | 一种基于光流法的惯性视觉组合导航方法 | |
CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
CN111161337B (zh) | 一种动态环境下的陪护机器人同步定位与构图方法 | |
CN106595659A (zh) | 城市复杂环境下多无人机视觉slam的地图融合方法 | |
CN111156997B (zh) | 一种基于相机内参在线标定的视觉/惯性组合导航方法 | |
CN112577493B (zh) | 一种基于遥感地图辅助的无人机自主定位方法及*** | |
CN114001733B (zh) | 一种基于地图的一致性高效视觉惯性定位算法 | |
CN106289250A (zh) | 一种航向信息采集*** | |
CN115371665B (zh) | 一种基于深度相机和惯性融合的移动机器人定位方法 | |
CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
CN110929402A (zh) | 一种基于不确定分析的概率地形估计方法 | |
CN111932614B (zh) | 一种基于聚类中心特征的激光雷达即时定位与建图方法 | |
CN114485640A (zh) | 基于点线特征的单目视觉惯性同步定位与建图方法及*** | |
CN109871024A (zh) | 一种基于轻量级视觉里程计的无人机位姿估计方法 | |
CN117367427A (zh) | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 | |
CN109785428A (zh) | 一种基于多态约束卡尔曼滤波的手持式三维重建方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |