CN115272460A - 一种基于rgbd相机的自适应里程计与优化关键帧选择方法 - Google Patents

Abstract

本发明涉及计算机视觉技术领域，具体的说是一种基于RGBD相机的自适应里程计与优化关键帧选择方法，包括以下步骤，通过深度相机获取彩色图M和深度图D，计算彩色图的模糊程度与深度图不确定度，采用点到平面ICP与直接法通过自适应加权计算当前帧到关键帧相对位置，判断当前帧是否满足创建关键帧条件，满足则转置步骤4)否则返回步骤1)，采用图形学方法补全当前帧深度图D，得到补全后的深度图，利用步骤5)补全的深度图作为参考将上一关键帧和当前帧之间通过RGB‑D相机获取的深度图与当前帧深度图进行自适应加权融合；本发明能够选择高质量的关键帧，充分利用普通帧信息并且再跟踪过程中根据图片质量自适应选取可靠的数据进行优化。

Description

一种基于RGBD相机的自适应里程计与优化关键帧选择方法

技术领域

本发明涉及一种自适应里程计与优化关键帧选择方法，具体为一种基于RGBD相机的自适应里程计与优化关键帧选择方法，属于计算机视觉技术领域。

背景技术

近年来随着计算机、机器人技术、人工智能技术的不断发展，各种高智能的机器设备孕育而生，为了使各种机器设备实现自主运动环境感知，机器设备需要实时获取周围环境以及自身位置。为解决上述问题同步定位和地图重构(SLAM)受到广大研究学者的关注，SLAM通过激光雷达、视觉、IMU等传感器构建增量地图并且定位传感器位置，广泛应用于自动驾驶、虚拟现实、增强现实、无人机等领域。

视觉SLAM前端是系统能否成功对相机进行跟踪的关键，一个健壮的前端系统能够极大程度较小系统累计误差并使后端能够收敛到最优结果，构造高精度的场景地图。传统SLAM通常采用激光雷达作为传感器获取目标点云信息，但是激光雷达价格高昂交互性差并且舍弃了大量纹理信息，随着微软推出的首款消费级深度相机Kinect，将深度相机作为传感器的SLAM研究逐渐成为主流。

传统的基于RGB-D相机的SLAM定位通常是基于关键帧，利用点到平面ICP(Iterative Closest Point)联合优化图像强度信息计算帧间相对运动，但是深度误差项和强度误差项往往采用固定权重并未考虑图片质量，关键帧的选取也是通过经验或者是随机选取，或者是采用尽量多选取的原则，在实际应用中由于相机抖动、快速运动、光照、物体材质、拍摄角度等都将影响彩色图和深度图质量，采用固定权重则会引入大量错误信息，并且传统的关键帧选择策略可能会选取的低质量的关键帧，并且只通过关键帧作为参考建图与定位忽略了大量普通帧信息同时会由于关键帧深度图的缺失导致建图不完整甚至定位失败。

有鉴于此特提出本发明。

发明内容

本发明的目的就在于为了解决上述问题而提供一种基于RGBD相机的自适应里程计与优化关键帧选择方法，。

本发明通过以下技术方案来实现上述目的，一种基于RGBD相机的自适应里程计与优化关键帧选择方法，包括以下步骤：

1)通过深度相机获取彩色图M和深度图D；

2)计算彩色图的模糊程度与深度图不确定度；

3)采用点到平面ICP与直接法通过自适应加权计算当前帧到关键帧相对位置；

4)判断当前帧是否满足创建关键帧条件，满足则转置步骤4)否则返回步骤1)；

5)采用图形学方法补全当前帧深度图D，得到补全后的深度图；

6)利用步骤5)补全的深度图作为参考将上一关键帧和当前帧之间通过RGB-D相机获取的深度图与当前帧深度图进行自适应加权融合，创建并更新关键帧；

7)输出融合后的深度图和彩色图作为关键帧。

进一步的，在步骤2)中，计算彩色图的模糊程度采用The Blur Effect:Perception and Estimation with a New No-Reference Perceptual Blur Metric中提出的方法计算彩色图M的模糊程度，流程如下，分别利用水平卷积核和垂直卷积核,对M进行水平和垂直模糊，水平模糊B_Ver＝h_v*M，垂直模糊B_Hor＝h_h*M，分别计算原图像和水平和竖直模糊图片的绝对差分图D_M_Ver，D_M_Hor,D_B_Ver,D_B_Hor

其中:

D_M_Ver(i,j)＝|M(i,j)-M(i-1,j)|

D_M_Hor(i,j)＝|M(i,j)-M(i,j-1)|

D_B_Ver(i,j)＝|B_Ver(i,j)-B_Ver(i-1,j)|

D_B_Hor(i,j)＝|B_Hor(i,j)-B_Hor(i,j-1)|；

获取原图与模糊图片的差分图片中水平与垂直方向上对应像素点差值分别构造水平与垂直方向差分图V_Ver,V_Hor.

V_Ver＝Max(0,D_M_Ver(i,j)-D_B_Ver(i,j))

V_Hor＝Max(0,D_M_Ver(i,j)-D_B_Hor(i,j))；

分别计算原始差分图像和模糊后差分图像像素差值之和，

计算彩色图M的模糊程度，

进一步的，在步骤2)中，计算深度图不确定度的方式为：

对于深度传感器获取的深度图样本，使用传感器模型和经验模型分别计算深度图的纵向不确定度，横向不确定度和表面不确定度，

计算纵向不确定度

式中d表示该点的深度，f为以像素为单位的焦距，b表示以米为单位的基线长度，m表示立体匹配误差设置为0.1，

计算表面不确定度

式中k表示对应点的k,d和d_i∈k分别表示该点以及邻域内的深度值，n表示邻域内深度值有效点的个数，

计算横向不确定度

式中θ表示该点和关键帧对应点偏转角，∈为设定阈值平衡不确定度和偏转角之间关系，

计算最终获取的深度图对应点不确定，度为δ_f＝δ_zδ_sδ_m。

进一步的，在步骤3)中，具体方法为：定义点到平面ICP能源函数E_geo，通过RGB-D相机获取的最新深度图数据与关键帧深度数据构造约束，寻找最优位姿矩阵T_f最小化点到平面的误差成本项，

式中

表示关键帧顶点坐标，T_f为位姿代求位姿更新量，T_pre为上一迭代过程估计的位姿初始化为根据匀速运动模型求得ρ_Tukey表示Tukey损失函数，n^k为对应点法向量，不确定模型δ_f＝δ_zδ_sδ_m，δ_z,δ_s,δ_m分别表示距离不确定度，表面不确定度以及测量不确定度；

构造光度项能源函数，利用RGB-D相机获取的彩色图与关键帧的彩色图光度项梯度构造约束，目的是寻找位姿矩阵T_f最小化当前帧和关键帧之间像素梯度误差，成本函数构建为

式中K表示相机内参，π为投影矩阵将点由相机坐标系投影到图像坐标系，

分别表示当前帧和关键帧匹配点图像强度梯度，ρ_Huber表示Huber损失函数；

构造自适应权重能源函数，将彩色图和深度图降采样构造4层图像金字塔，根据每一层深度图的平均不确定度以及彩色图片质量自适应构造成本函数E

E＝(1-ω_rgb)E_geo+ω_rgbE_rgb

式中

式中δ_rgb表示彩色图的模糊程度Blur，η表示设置的彩色图模糊的最大值，δ_depth表示深度图平均不确定度，a用于平衡彩色图片质量和深度不确定度设。

进一步的，在步骤4)中，具体的工作方式为通过步骤3)中计算出的最新数据相对于关键帧位姿T得到当前帧相对于关键帧的偏航角α，俯仰角β，滚转角γ，与平移矩阵t＝[t_x,t_y,t_z]^T并且计算整体偏移距离

如果α>α_max或β>β_max或γ>γ_max或

或

或

或者d>d_max符合关键帧创建条件转到5，

如果α>α_min或β>β_min或γ>γ_min或

或

或

并且彩色图模糊程度Blur<Blur_min转到步骤四否则转至步骤1。

进一步的，在步骤5)中，采用3*3的菱形卷积核使用膨胀算法填充最接近存在有效深度的点，

采用5*5的全卷积核使用图形学闭运算对闭合小孔洞，

采用7*7的全卷积核填充中型空洞，通过计算深度图掩码获取空像素点，再通过卷积核填充掩码图，

采用5*5的卷积核对补全后的深度图进行中值去噪，最后通过高斯滤波对表面进行平滑。

进一步的，在步骤6)中，对于中间深度图[D₁,D₂,……,D_n],通过步骤2)介绍的方法计算每一帧深度图的不确定度度量图[w₁,w₂,……,w_n]

对于中间帧D_i一像素点d_i(i,j)通过计算的位置矩阵T_i将其投影到当前帧获取该点在当前帧D与补全帧D_full的投影点d(i,j)＝π^-1(T^-1T_iπ(d_i(x,y))),其中π表示将像素点投影到相机坐标系。

判断投影点d(i,j)是否满足融合要求

如果d(i,j)>0&d_i(i,j)>0&|d(i,j)-d_i(i,j)|<0.01

d(i,j)＝w_id_i(i,j)+wd(i,j)

如果d_i(i,j)>0&d(i,j)≤0&d_full(i,j)>0&|d_full(i,j)-d_i(x,y)|<0.0，则d(i,j)＝d_i(i,j)

更新当前帧深度不确定图中点w(i,j)。

进一步的，在步骤7)中，输出融合后的深度图和彩色图作为关键帧，更新关键帧，返回步骤1)。

本发明的技术效果和优点：本发明方法能够选择高质量的关键帧，充分利用普通帧信息并且再跟踪过程中根据图片质量自适应选取可靠的数据进行优化，通过关键帧作为参考建图与定位的同时，对大量普通帧信息进行参考，同时由于增设了关键帧深度图，使得本方法建图更完整，定位效果更好。

附图说明

图1为本发明的实现流程图。

图2为本发明仿真实验结果可视化图；

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1-2所示，一种基于RGBD相机的自适应里程计与优化关键帧选择方法，包括以下步骤：

步骤1：通过深度相机获取彩色图M和深度图D，并将深度图存储用于关键帧创建时与关键帧深度图补全融合。

步骤2：计算彩色图的模糊程度与深度图不确定度。

步骤2.1:计算彩色图的模糊程度。

采用The Blur Effect:Perception and Estimation with a New No-ReferencePerceptual Blur Metric中提出的方法计算彩色图M的模糊程度，流程如下：

步骤2.1a:分别利用卷积核

h_h＝h_v ^T,对M进行水平和垂直模糊。

水平模糊B_Ver＝h_v*M

垂直模糊B_Hor＝h_h*M

步骤2.1b:分别计算原图像和水平和竖直模糊图片的绝对差分图D_M_Ver，D_M_Hor,D_B_Ver,D_B_Hor。

其中:

D_M_Ver(i,j)＝|M(i,j)-M(i-1,j)|

D_M_Hor(i,j)＝|M(i,j)-M(i,j-1)|

D_B_Ver(i,j)＝|B_Ver(i,j)-B_Ver(i-1,j)|

D_B_Hor(i,j)＝|B_Hor(i,j)-B_Hor(i,j-1)|

步骤2.1c:获取原图与模糊图片的差分图片中水平与垂直方向上对应像素点差值分别构造水平与垂直方向差分图V_Ver,V_Hor.

V_Ver＝Max(0,D_M_Ver(i,j)-D_B_Ver(i,j))

V_Hor＝Max(0,D_M_Ver(i,j)-D_B_Hor(i,j))

步骤2.1d:分别计算原始差分图像和模糊后差分图像像素差值之和

步骤2.1e:计算彩色图M的模糊程度。

步骤2.2:计算深度图不确定度。

对于深度传感器获取的深度图样本，使用传感器模型和经验模型分别计算深度图的纵向不确定度，横向不确定度和表面不确定度。

步骤2.2a:计算纵向不确定度

式中d表示该点的深度，f为以像素为单位的焦距，b表示以米为单位的基线长度，m表示立体匹配误差设置为0.1。

步骤2.2b:计算表面不确定度

式中k表示对应点的k邻域设置为4,d和d_i∈k分别表示该点以及邻域内的深度值，n表示邻域内深度值有效点的个数。

步骤2.2c:计算横向不确定度

式中θ表示该点和关键帧对应点偏转角，∈为设定阈值平衡不确定度和偏转角之间关系。

步骤2.2d:计算最终获取的深度图对应点不确定度为δ_f＝δ_zδ_sδ_m。

步骤3:采用点到平面ICP与直接法通过自适应加权计算当前帧到关键帧相对位置。

步骤3.1：定义点到平面ICP能源函数E_geo，通过RGB-D相机获取的最新深度图数据与关键帧深度数据构造约束，寻找最优位姿矩阵T_f最小化点到平面的误差成本项。

式中

表示关键帧顶点坐标，T_f为位姿代求位姿更新量，T_pre为上一迭代过程估计的位姿初始化为根据匀速运动模型求得ρ_Tukey表示Tukey损失函数，n^k为对应点法向量，不确定模型δ_f＝δ_zδ_sδ_m，δ_z,δ_s,δ_m分别表示距离不确定度，表面不确定度以及测量不确定度。

步骤3.2：构造光度项能源函数，利用RGB-D相机获取的彩色图与关键帧的彩色图光度项梯度构造约束，目的是寻找位姿矩阵T_f最小化当前帧和关键帧之间像素梯度误差，成本函数构建为：

式中K表示相机内参，π为投影矩阵将点由相机坐标系投影到图像坐标系，

分别表示当前帧和关键帧匹配点图像强度梯度，ρ_Huber表示Huber损失函数。

步骤3.3：构造自适应权重能源函数，将彩色图和深度图降采样构造4层图像金字塔，根据每一层深度图的平均不确定度以及彩色图片质量自适应构造成本函数E

E＝(1-ω_rgb)E_geo+ω_rgbE_rgb

式中

式中δ_rgb表示彩色图的模糊程度Blur，η表示设置的彩色图模糊的最大值，δ_depth表示深度图平均不确定度，a用于平衡彩色图片质量和深度不确定度设置为0.125。

步骤4：判断当前帧是否满足创建关键帧条件，满足则转置(4)否则返回(1)

步骤4.1：通过3中计算出的最新数据相对于关键帧位姿T得到当前帧相对于关键帧的偏航角α，俯仰角β，滚转角γ，与平移矩阵t＝[t_x,t_y,t_z]^T并且计算整体偏移距离

(4.1)如果α>α_max或β>β_max或γ>γ_max或

或

或

或者d>d_max符合关键帧创建条件转到5.

(4.2)如果α>α_min或β>β_min或γ>γ_min或

或

或

并且彩色图模糊程度Blur<Blur_min转到步骤四否则转至步骤1。

步骤5：采用图形学方法补全当前帧深度图D，得到补全后的深度图D_full。

步骤5.1：采用3*3的菱形卷积核使用膨胀算法填充最接近存在有效深度的点。

步骤5.2：采用5*5的全卷积核使用图形学闭运算对闭合小孔洞。

步骤5.3：采用7*7的全卷积核填充中型空洞，通过计算深度图掩码获取空像素点，再通过卷积核填充掩码图。

步骤5.5：采用5*5的卷积核对补全后的深度图进行中值去噪，最后通过高斯滤波对表面进行平滑。

步骤6：利用5补全的深度图D_full作为参考将上一关键帧和当前帧之间通过RGB-D相机获取的深度图与当前帧深度图进行自适应加权融合，创建并更新关键帧。

步骤6.1：对于中间深度图[D₁,D₂,……,D_n],通过(2)介绍的方法计算每一帧深度图的不确定度度量图[w₁,w₂,……,w_n]

步骤6.2：对于中间帧D_i一像素点d_i(i,j)通过计算的位置矩阵T_i将其投影到当前帧获取该点在当前帧D与补全帧D_full的投影点d(i,j＝π^-1(T^-1T_iπ(d_i(x,y))),其中π表示将像素点投影到相机坐标系。

步骤6.3：判断投影点d(i,j)是否满足融合要求

如果d(i,j)>0&d_i(i,j)>0&|d(i,j)-d_i(i,j|<0.01

d(i,j)＝w_id_i(i,j)+wd(i,j)

如果d_i(i,j)>0&d(i,j)≤0&d_full(i,j)>0&|d_full(i,j)-d_i(x,y)|<0.0。

d(i,j)＝d_i(i,j)

步骤6.4：更新当前帧深度不确定图中点w(i,j)

步骤7：输出融合后的深度图和彩色图作为关键帧，更新关键帧，返回步骤一。

2.仿真内容及仿真结果分析：

本发明仿真实验中使用的是在公开数据集ETH3D数据集，该数据集是一个具有全局快门的RGB-D数据深度图和彩色图具有一致快门，实验平台为Intel(R)Core(TM)i5-7300U CPU@2.90GHz,系统为ubuntu 20.04GPU 1660s。

为了对本发明仿真结果的效果进行评估，本发明采用采用绝对轨迹均方根误差((Root mean square error,RMSE)作为性能评价指标与现有的五种技术进行对比。

从表中可以看出本发明在ETH3D数据集上cables，einstein1，mannequin-face，plant均取得了最好的效果，在einstein_change，planar，mannequin-face，上也取得了平均第二的效果，证明本方法可以有效提高里程计精度。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (8)

1.一种基于RGBD相机的自适应里程计与优化关键帧选择方法，其特征在于，包括以下步骤：

1)通过深度相机获取彩色图M和深度图D；

2)计算彩色图的模糊程度与深度图不确定度；

3)采用点到平面ICP与直接法通过自适应加权计算当前帧到关键帧相对位置；

4)判断当前帧是否满足创建关键帧条件，满足则转置步骤4)否则返回步骤1)；

5)采用图形学方法补全当前帧深度图D，得到补全后的深度图；

6)利用步骤5)补全的深度图作为参考将上一关键帧和当前帧之间通过RGB-D相机获取的深度图与当前帧深度图进行自适应加权融合，创建并更新关键帧；

7)输出融合后的深度图和彩色图作为关键帧。

2.根据权利要求1所述的一种基于RGBD相机的自适应里程计与优化关键帧选择方法，其特征在于，在步骤2)中，计算彩色图的模糊程度采用The Blur Effect：Perception andEstimation with a New No-Reference Perceptual Blur Metric中提出的方法计算彩色图M的模糊程度，流程如下，分别利用水平卷积核和垂直卷积核，对M进行水平和垂直模糊，水平模糊B_Ver＝h_v*M，垂直模糊B_Hor＝h_h*M，分别计算原图像和水平和竖直模糊图片的绝对差分图D_M_Ver，D_M_Hor，D_B_Ver，D_B_Hor

其中：

D_M_Ver(i，j)＝|M(i，j)-M(i-1，j)|

D_M_Hor(i，j)＝|M(i，j)-M(i，j-1)|

D_B_Ver(i，j)＝|B_Ver(i，j)-B_Ver(i-1，j)|

D-B_Hor(i，j)＝|B_Hor(i，j)-B_Hor(i，j-1)|；

获取原图与模糊图片的差分图片中水平与垂直方向上对应像素点差值分别构造水平与垂直方向差分图V_Ver，V_Hor.

V_Ver＝Max(0，D_M_Ver(i，j)-D_B_Ver(i，j))

V_Hor＝Max(0，D_M_Ver(i，j)-D_B_Hor(i，j))；

分别计算原始差分图像和模糊后差分图像像素差值之和，

计算彩色图M的模糊程度，

3.根据权利要求2所述的一种基于RGBD相机的自适应里程计与优化关键帧选择方法，其特征在于，在步骤2)中，计算深度图不确定度的方式为：

对于深度传感器获取的深度图样本，使用传感器模型和经验模型分别计算深度图的纵向不确定度，横向不确定度和表面不确定度，

计算纵向不确定度

式中d表示该点的深度，f为以像素为单位的焦距，b表示以米为单位的基线长度，m表示立体匹配误差设置为0.1，

计算表面不确定度

式中k表示对应点的k，d和d_i∈k分别表示该点以及邻域内的深度值，n表示邻域内深度值有效点的个数，

计算横向不确定度

式中θ表示该点和关键帧对应点偏转角，∈为设定阈值平衡不确定度和偏转角之间关系，

计算最终获取的深度图对应点不确定，度为δ_f＝δ_zδ_sδ_m。

4.根据权利要求1所述的一种基于RGBD相机的自适应里程计与优化关键帧选择方法，其特征在于，在步骤3)中，具体方法为：定义点到平面ICP能源函数E_geo，通过RGB-D相机获取的最新深度图数据与关键帧深度数据构造约束，寻找最优位姿矩阵T_f最小化点到平面的误差成本项，

式中

表示关键帧顶点坐标，T_f为位姿代求位姿更新量，T_pre为上一迭代过程估计的位姿初始化为根据匀速运动模型求得ρ_Tukey表示Tukey损失函数，n^k为对应点法向量，不确定模型δ_f＝δ_zδ_sδ_m，δ_z，δ_s，δ_m分别表示距离不确定度，表面不确定度以及测量不确定度；

构造光度项能源函数，利用RGB-D相机获取的彩色图与关键帧的彩色图光度项梯度构造约束，目的是寻找位姿矩阵T_f最小化当前帧和关键帧之间像素梯度误差，成本函数构建为

式中K表示相机内参，π为投影矩阵将点由相机坐标系投影到图像坐标系，

分别表示当前帧和关键帧匹配点图像强度梯度，ρ_Huber表示Huber损失函数；

构造自适应权重能源函数，将彩色图和深度图降采样构造4层图像金字塔，根据每一层深度图的平均不确定度以及彩色图片质量自适应构造成本函数E

E＝(1-ω_rgb)E_geo+ω_rgbE_rgb

式中

式中δ_rgb表示彩色图的模糊程度Blur，η表示设置的彩色图模糊的最大值，δ_depth表示深度图平均不确定度，a用于平衡彩色图片质量和深度不确定度设。

5.根据权利要求1所述的一种基于RGBD相机的自适应里程计与优化关键帧选择方法，其特征在于：在步骤4)中，具体的工作方式为通过步骤3)中计算出的最新数据相对于关键帧位姿T得到当前帧相对于关键帧的偏航角α，俯仰角β，滚转角γ，与平移矩阵t＝[t_x，t_y，t_z]^T并且计算整体偏移距离

如果α＞α_max或β＞β_max或γ＞γ_max或

或

或

或者d＞d_max符合关键帧创建条件转到5，

如果α＞α_min或β＞β_min或γ＞γ_min或

或

或

并且彩色图模糊程度Blur＜Blur_min转到步骤四否则转至步骤1。

6.根据权利要求1所述的一种基于RGBD相机的自适应里程计与优化关键帧选择方法，其特征在于，在步骤5)中，采用3*3的菱形卷积核使用膨胀算法填充最接近存在有效深度的点，

采用5*5的全卷积核使用图形学闭运算对闭合小孔洞，

采用7*7的全卷积核填充中型空洞，通过计算深度图掩码获取空像素点，再通过卷积核填充掩码图，

采用5*5的卷积核对补全后的深度图进行中值去噪，最后通过高斯滤波对表面进行平滑。

7.根据权利要求1所述的一种基于RGBD相机的自适应里程计与优化关键帧选择方法，其特征在于，在步骤6)中，对于中间深度图[D₁，D₂，......，D_n]，通过步骤2)介绍的方法计算每一帧深度图的不确定度度量图[w₁，w₂，......，w_n]

对于中间帧D_i一像素点d_i(i，j)通过计算的位置矩阵T_i将其投影到当前帧获取该点在当前帧D与补全帧D_full的投影点d(i，j)＝π^-1(T^-1T_iπ(d_i(x，y)))，其中π表示将像素点投影到相机坐标系；

判断投影点d(i，j)是否满足融合要求

如果d(i，j)＞0&d_i(i，j)＞0&|d(i，j)-d_i(i，j)|＜0.01

d(i，j)＝w_id_i(i，j)+wd(i，j)

如果d_i(i，j)＞0&d(i，j)≤0&d_full(i，j)＞0&|d_full(i，j)-d_i(x，y)|＜0.0，则d(i，j)＝d_i(i，j)

更新当前帧深度不确定图中点w(i，j)。

8.根据权利要求1所述的一种基于RGBD相机的自适应里程计与优化关键帧选择方法，其特征在于：在步骤7)中，输出融合后的深度图和彩色图作为关键帧，更新关键帧，返回步骤1)。

Images