CN108010081A - 一种基于Census变换和局部图优化的RGB-D视觉里程计方法 - Google Patents

Abstract

本发明涉及一种基于Census变换和局部图优化的RGB‑D视觉里程计方法，包括以下步骤：通过RGB‑D传感器获取环境的彩色和深度图像信息，使用彩色图像计算Census描述图；基于Census描述图，使用直接法对当前帧进行运动估计，计算当前帧与局部图中最新关键帧的相对位姿；对于局部图中梯度信息显著但缺少深度信息的点，在当前帧中进行深度跟踪估计；根据当前帧的位姿估计结果，生成新的关键帧，并插入到局部图中，对局部图进行图优化和关键帧管理操作。本发明使用Census描述图进行直接法运动估计，并结合局部图的优化和管理，提高了视觉里程计在彩色图像亮度发生变化和深度图像深度信息不足时的实时鲁棒性。

Description

一种基于Census变换和局部图优化的RGB-D视觉里程计方法

技术领域

本发明涉及计算机视觉技术领域，更具体地，涉及一种基于Census变换和 局部图优化的RGB-D视觉里程计方法。

背景技术

视觉里程计是通过输入图像序列增量地估计与视觉传感器固定连接的人或 物的运动轨迹的方法。相比于传统的惯导和轮式里程计，视觉里程计克服了惯 导漂移和轮胎打滑的测量误差问题，无需卫星导航配合，且视觉传感器有功耗 小，采集的信息丰富等优点，因而视觉里程计在移动机器人定位和导航领域得 到了广泛关注和逐步应用。

目前，视觉里程计的方法主要有特征点法和直接法。特征点法的主要流 程是特征点提取，特征点匹配和基于最小化重投影误差(Reprojection error) 的帧间位姿估计。特征点法是传统的视觉里程计方法，并已经有了一些成功的 应用实例，但是也存在一些问题。特征点法的特征点提取和匹配步骤存在比较 耗时和有误匹配的问题，且获取的特征点较为稀疏，当图像有运动模糊、低光 照、重复纹理或缺少纹理等情况时，特征点法的准确性会受到较大影响。鉴于 特征点法的问题，研究者提出了直接对齐两帧图片的像素亮度值的直接法，直 接法通过最小化光度误差(Photometric error)来估计两帧间的位姿。相比于特征点法，直接法不再需要提取和匹配特征点，而是基于两帧图片对应像素点 的亮度值不变的假设，通过相机模型直接使用像素点的值构造光度误差，并最 小化光度误差估计位姿参数。稠密的直接法会使用图片上所有像素点来计算光 度误差，计算量是巨大的，一般视觉里程计中的直接法是半稠密的，也就是只 使用有一定梯度信息的像素点来计算光度误差，这样的处理在保持位姿估计的 相对准确性的同时让直接法具备了的实时性。直接法在相机运动较小时能得到 鲁棒而准确的位姿估计结果，且因较为充分地利用了图像信息，运动模糊，重 复纹理和缺少纹理的图像对直接法准确性的影响没有特征点法严重。直接法的主要问题是，进行对齐的两帧图片需要符合亮度不变的假设，图片亮度差异程 度决定了直接法的估计结果的准确性。较小的亮度差异下直接法仍能工作，较 大的亮度差异下直接法会得到错误的估计结果。

视觉里程计所使用的视觉传感器一般是单目相机，双目相机或RGB-D相 机，无论使用这三种相机中的哪一种，都可以基于特征点法或直接法实现视觉 里程计。基于纯单目相机的视觉里程计方法较为复杂，在位姿估计的同时还需 要重建三维的地图点，且估计得到的位姿和三维点没有绝对尺度，容易出现尺 度漂移，需要较好的初始化。双目相机可通过双目立体匹配得到图像场景中像 素点的深度值，图像处理工作量大，在昏暗或纹理特征不明显的环境不适用。 RGB-D相机能同时获取场景的彩色图像和深度图像，深度图像通过红外结构光或 飞行时间法的硬件结构获取，因易受日光影响只能在室内使用，且深度测量的 范围受限。基于已有深度的视觉里程计能估计得到含有绝对尺度的运动轨迹， 一般没有尺度漂移，位姿估计结果较纯单目更准确。

发明内容

本发明的目的在于克服上述现有技术所述的至少一种缺陷(不足)，提供一 种基于Census变换和局部图优化的RGB-D视觉里程计方法，通过使用Census 变换提取具有亮度变化不变性的描述图，在此基础上进行鲁棒的直接法位姿估 计，并设计局部图的优化管理，解决位姿优化和RGB-D有时采集的深度信息不 足的问题。

为了实现上述目的，本发明采用的技术方案为：一种基于Census变换和局 部图优化的RGB-D视觉里程计方法，其中，包括以下步骤：

S1.通过RGB-D传感器获取环境的彩色和深度图像信息，使用彩色图像计算Census描述图；

S2.基于Census描述图，使用直接法对当前帧进行运动估计，计算当前帧 与局部图中最新关键帧的相对位姿；

S3.对于局部图中梯度信息显著但缺少深度信息的点，在当前帧中进行深度 跟踪估计；

S4.根据当前帧的位姿估计结果，生成新的关键帧，并插入到局部图中，对 局部图进行图优化和关键帧管理操作。

具体地，所述的步骤S1中，Census描述图的计算，首先将彩色图转化为灰 度图，然后基于Census变换计算一幅Census描述图。Census变换的计算方式 为，对于灰度图中的每个像素x，将其像素值与局部邻域像素Ne(x)内的所有像 素值依次进行大小比较，得到描述向量d(x,I)：

其中局部邻域像素Ne(x)选用8邻域像素，所以Census变换后得到的是一 幅8通道的描述图D(I)，并有D(x,I)＝d(x,I)。对得到的Census描述图进行 高斯平滑处理。

优选地，选用窗口大小为5，标准差为σ＝1.5的高斯平滑。

具体地，所述步骤S2中，对当前帧使用直接法进行运动估计，选出局部图 中的最新关键帧，基于Census描述图使用直接法估计最新关键帧到当前帧的位 姿变换，迭代优化方法为基于金字塔的LK反向相乘算法。记当前帧为I，局部 图中的最新关键帧为K，直接法优化目标函数为：

T^*＝argmin_T∑_x∈Ωw_x*||D(W(x，T)，l)-D(x，K)||²

其中T为关键帧K到当前帧I的位姿变换；W(x,T)为将从K中选出的像素 点投影到当前帧的变换：W(x，T)-π(T^π^-1(x，d_x))，其中π(·)为相机投影函数，将像 素点从相机坐标系投影到图像平面中；π^-1(·，·)为相机反投影函数，将给定深度值 的像素点从图像平面反投影到相机坐标系。w_x为通过迭代重赋权最小二乘(IRLS) 方法计算的权重值，Ω为从关键帧K中选出的满足梯度信息阈值t_g且有有效深度 值的像素点。

直接法迭代估计结束后，如果像素点x的权重w_x大于权重阈值t_w，则认为x 在估计中属于良好点，良好点个数占运动估计所用的总像素个数的比例是生成 关键帧的判据之一。

优选地，设梯度信息阈值t_g＝3；迭代重赋权最小二乘方法得到的权值w_x经过 归一化，值域在0到1之间，设权重阈值t_w-0.7。

具体地，所述步骤S3中，局部图由最新的前N个关键帧和帧上的梯度信息 显著点组成，其中在深度图中缺少深度信息的梯度信息显著点会在已估计位姿 的当前帧上进行深度跟踪估计。梯度信息显著点为满足梯度信息大于阈值t_g的 点，其中用于深度跟踪估计的缺少深度信息的梯度信息显著点会用非最大值抑 制进行过滤，从而提取有效点并减少处理量。深度跟踪估计的方法是，沿着极 线在当前帧中寻找一个最佳的NCC匹配，找到最佳匹配后通过三角化可以得到 对应的深度估计值。

具体地，所述步骤S3中，缺少深度信息的梯度信息显著点的深度跟踪估计， 一次合理的深度跟踪匹配需要满足最佳NCC匹配的分值大于阈值t_NCC。另外，在 第一次跟踪估计时会沿着极线在整个图像的范围内寻找最佳NCC匹配，而在后 续帧的跟踪估计时，则会根据之前的估计结果，在限定的范围内搜索最佳NCC 匹配。得到多次合理的深度跟踪估计后，深度跟踪估计收敛的判定条件为：

(i)得到的合理估计次数要大于等于5；(ii)多次估计的结果要有兼容性。

满足所有收敛条件后，取平均值作为最终的逆深度收敛结果，并不再在 后续帧中进行跟踪估计，这个收敛结果将作为局部图优化步骤的有效初始值。

优选地，设阈值t_NCC＝0.05。

具体地，所述步骤S4中，根据当前帧的运动估计结果，关键帧生成的判 定标准如下：

(1)良好点个数占运动估计所用的总像素个数的比例小于阈值t_gwp；

(2)位姿估计结果中位移量模值大于阈值t_tran；

(3)位姿估计结果中旋转量模值大于阈值t_rot。

优选地，设阈值t_gwp＝0.7，t_cran-0.1，t_rot-2。

具体地，所述步骤S4中，新关键帧插入局部图进行图优化和关键帧管理的 操作包括下面步骤：

(1)构造局部图优化目标函数，进行非线性最小二乘优化；

(2)优化完后，将局部图关键帧中的梯度信息显著点都投影到最新关键帧 中，更新最新关键帧的深度图信息；

(3)保持局部图中的关键帧数量不超过N_k，超过N_k时，将最旧的关键帧从局 部图中移除。

优选地，设局部图最大关键帧数量N_k＝6。

具体地，上述步骤(1)中，构造局部图优化目标函数，首先从局部图的每个 关键帧中选出一定数量的梯度信息显著点：考虑优化的实时性能，每个关键帧 使用非最大值抑制和梯度信息值排序，选出300个有有效深度的梯度信息显著 点，并附加在步骤S3中进行了深度跟踪估计且收敛的点。然后将选出的点分别 投影到局部图的其他关键帧中，构成残差项。

将每个残差项相加构成总的局部图优化残差函数，其中待优化的变量包括 局部图中世界坐标系到每个关键帧的位姿变换T_i，以及那些深度跟踪估计后已收 敛的点的逆深度，初始值为收敛后得到的逆深度值。作为变量经过局部图优化 后的逆深度值，将作为有效深度值填充到对应关键帧的深度图中。

具体地，上述步骤(2)中，投影梯度信息显著点更新最新关键帧深度图信息 的操作，这些投影的梯度信息显著点需要在对应深度图中含有有效深度值，在 最新关键帧的投影位置处填充深度值：投影位置取整处理，如果投影位置在深 度图上原来已有有效深度值，则不填充，如果原来没有有效深度值，则进行填 充。

本发明与现有技术相比，有益的效果是：采用了Census变换生成的描述图 进行直接法位姿估计，提高了在图像亮度发生变化时直接法估计的鲁棒性和准 确性，同时结合LK反向相乘迭代算法，保持了位姿估计的实时性。另外，局部 图优化的优化和管理进一步的优化了视觉里程计的估计精度，并解决了RGB-D 相机采集的深度信息不足的问题。

附图说明

图1为本发明方法的整体流程图；

图2为本发明方法的运动跟踪操作流程图；

图3为本发明方法的局部图插入关键帧、优化和管理操作流程图。

具体实施方式

下面结合附图和实施例对本发明作进一步的描述，但本发明的实施方式不 限于此。

一种基于Census变换和局部图优化的RGB-D视觉里程计方法，如图1及图 2所示，包括以下步骤：

S1.通过RGB-D传感器获取环境的彩色和深度图像信息，使用彩色图像计算Census描述图；

S2.基于Census描述图，使用直接法对当前帧进行运动估计，计算当前帧 与局部图中最新关键帧的相对位姿；

S3.对于局部图中梯度信息显著但缺少深度信息的点，在当前帧中进行深度 跟踪估计；

S4.根据当前帧的位姿估计结果，生成新的关键帧，并插入到局部图中，对 局部图进行图优化和关键帧管理操作。

所述步骤S1中，计算Census描述图，首先将彩色图转化为灰度图，然后 基于Census变换计算一幅Census描述图。Census变换的计算方式为，对于灰 度图中的每个像素x，将其像素值与局部邻域像素Ne(x)内的所有像素值依次进 行大小比较，得到描述向量d(x,I)：

其中局部邻域像素Ne(x)选用8邻域像素，所以Census变换后得到的是一 幅8通道的描述图D(I)，并有D(x,I)＝d(x,I)。最后对得到的Census描述图 进行窗口大小为5，标准差为σ＝1.5的高斯平滑处理。

步骤S2中，对当前帧使用直接法进行运动估计，并使用基于金字塔的LK 反向相乘算法作为迭代优化方法，其子步骤为：

步骤2.1，计算Census描述图金字塔，首先计算图像金字塔，再根据步骤 S1中的Census变换得到描述图金字塔。

步骤2.2，记当前帧为I，记局部图中最新插入的关键帧记为K，选取K的 Census描述图中满足梯度信息大于阈值t_g＝3且具有有效深度值的像素点集合 Ω，构造基于Census描述图的直接法优化目标函数：

T^*＝argmin_T∑_x∈Ωw_x*||D(W(x，T)，l)-D(x，K)||² (2)

式子(2)中，T为关键帧K到当前帧I的位姿变换，W(x,T)为将从K中选 出的像素点投影到当前帧的变换：W(x，T)＝π(T*π^-1(x，d_x))，其中π(·)为相机投影函 数，将像素点从相机坐标系投影到图像平面中；π^-1(.，.)为相机反投影函数，将给 定深度值的像素点从图像平面反投影到相机坐标系。w_x为通过迭代重赋权最小二 乘(IRLS)方法计算的权重值。

步骤2.3，迭代优化的更新方式使用LK后向相乘算法，每次迭代估计的位 姿变换更新量可以表示为：

ΔT^*＝argmin_ΔT∑_x∈Ωw_x*||D(W(x，T)，l)-D(W(x，ΔT)，K)||² (3)

然后进行反向更新：T←T*ΔT^-1。

步骤2.4，在Census描述图金字塔中由粗到精地估计位姿变换。

上述子步骤2.2中，迭代重赋权最小二乘方法得到的权值w_x经过归一化，值 域在0到1之间，设定权重阈值t_w＝0.7，像素点x的权重w_x大于t_w时则认为x 在估计中属于良好点，良好点个数占运动估计所用的总像素个数的比例是生成 关键帧的判据之一。

步骤S3中，局部图由最新的前N个关键帧和帧上的梯度信息显著点组成， 其中在深度图中缺少深度信息的梯度信息显著点会在已估计位姿的当前帧上进 行深度跟踪估计。梯度信息显著点为满足梯度信息大于阈值t_g的点，其中用于深 度跟踪估计的缺少深度信息的梯度信息显著点会用非最大值抑制进行过滤，从 而提取有效点并减少处理量。深度跟踪估计的方法是，沿着极线在当前帧中寻 找一个最佳的NCC匹配，找到最佳匹配后通过三角化可以得到对应的深度估计 值d，这里用逆深度(深度的倒数)ρ＝d^-1进行记录和建模，并计算该逆深度估 计值的几何不确定度σ_p，即在最佳匹配位置沿着极线方向移动一个像素时的深度 估计值变化量。记关键帧上的一个需要进行深度跟踪估计的点为x，沿着极线在 当前帧中寻找到x的最佳NCC匹配的像素位置x′，那么对应的逆深度和几何不确 定度为：

ρ＝d(x，x′)^-1，σ_ρ＝|d(x，x′)^-1-d(x，x′+1)^-1| (4)

其中d(·，·)表示在已知位姿变换的前提下，给定对应匹配点，通过三角化得到点x的深度估计值；而x′+1表示的是在位置x′沿着极线方向移动一个像素。另外， 一次合理的深度跟踪匹配需要满足最佳NCC匹配的分值大于阈值t_NCC-0.85。

步骤S3中，在第一次跟踪估计时会沿着极线在整个图像的范围内寻找最佳 NCC匹配，而在后续帧的跟踪估计时，则会根据之前估计的逆深度和不确定度， 在限定的范围内搜索最佳NCC匹配。多次进行深度跟踪估计后，设得到的合理 估计值为ρ_i和σ_ρi，深度跟踪估计收敛的判定条件为：

(i)得到的合理估计次数要大于等于5；

(ii)多次估计的结果要有兼容性，计算估计结果中最大值与最小值的差 与全局不确定度的比值：

其中为全局不确定度。当满足Con<2时，满足兼容性条件。

满足所有收敛条件后，取平均值作为最终的逆深度收敛结果，并不再在后 续帧中进行跟踪估计，这个收敛结果将作为局部图优化步骤的有效初始值。

步骤S4中，根据当前帧的运动估计结果，关键帧生成的判定标准如下：

(1)良好点个数占运动估计所用的总像素个数的比例小于阈值t_gwp＝0.7

(2)位姿估计结果中位移量模值大于阈值t_tran＝0.1

(3)位姿估计结果中旋转量模值大于阈值t_rot＝2

当前帧的运动估计结果只要满足以上任意一个判定标准，就将当前帧作为 新的关键帧插入到局部图，并进行局部图优化和关键帧管理。

步骤S4中，如图3所示，新关键帧插入局部图进行图优化和关键帧管理的 操作包括下面步骤：

(1)构造局部图优化目标函数，使用ceres优化库进行非线性最小二乘优化。

首先从局部图的每个关键帧中选出一定数量的梯度信息显著点：考虑优化 的实时性能，每个关键帧使用非最大值抑制和梯度信息值排序，选出300个有 有效深度的梯度信息显著点，并附加在步骤S3中进行了深度跟踪估计且收敛的 点。然后将选出的点分别投影到局部图的其他关键帧中，构成残差项，每个残 差项可表示为：

e_ij(x)＝γ(||D(W(xT_j*T_i ^-1)K_j)-D(xK_i)||²)，x∈Ω_ij，i≠j (6)

其中Ω_ij表示局部图第i个关键帧中选出的梯度信息显著点能投影到第j个 关键帧上的集合；γ(·)为Huber范数；K_i，K_j表示第i和第j个关键帧；T_i，T_j表示 从世界坐标系到第i和第j个关键帧的位姿变换。

总的局部图优化残差函数为：

其中待优化的变量O包括局部图中世界坐标系到每个关键帧的位姿变换T_i， 以及那些深度跟踪估计后已收敛的点的逆深度，初始值为收敛后得到的逆深度 值。作为变量经过局部图优化后的逆深度值，将作为有效深度值填充到对应关 键帧的深度图中。

优化时要固定最旧一个关键帧的位姿和其他已有有效深度值的梯度信息显 著点。

(2)优化完后，将局部图关键帧中的梯度信息显著点都投影到最新关键帧 中，更新最新关键帧的深度图信息。这些投影的梯度信息显著点需要在对应深 度图中含有有效深度值，在最新关键帧的投影位置处填充深度值：投影位置取 整处理，如果投影位置在深度图上原来已有有效深度值，则不填充，如果原来 没有有效深度值，则进行填充。

(3)保持局部图中的关键帧数量不超过N_k＝6，超过N_k时，将最旧的关键帧 从局部图中移除。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细， 但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域 的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和 改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附 权利要求为准。

Claims (9)

1.一种基于Census变换和局部图优化的视觉里程计方法，其特征在于，包括以下步骤：

S1.通过RGB-D传感器获取环境的彩色和深度图像信息，使用彩色图像计算Census描述图；

S2.基于Census描述图，使用直接法对当前帧进行运动估计，计算当前帧与局部图中最新关键帧的相对位姿；

S3.对于局部图中梯度信息显著但缺少深度信息的点，在当前帧中进行深度跟踪估计；

S4.根据当前帧的位姿估计结果，生成新的关键帧，并插入到局部图中，对局部图进行图优化和关键帧管理操作。

2.根据权利要求1所述的基于Census变换和局部图优化的视觉里程计方法，其特征在于，所述的步骤S1中的Census描述图的计算，首先将彩色图转化为灰度图，然后基于Census变换计算一幅Census描述图；Census变换的计算方式为，对于灰度图中的每个像素x，将其像素值与局部邻域像素Ne(x)内的所有像素值依次进行大小比较，得到描述向量d(x,I)：

其中局部邻域像素Ne(x)选用8邻域像素，所以Census变换后得到的是一幅8通道的描述图D(I)，并有D(x,I)＝d(x,I)，对得到的Census描述图进行高斯平滑处理。

3.根据权利要求1所述的基于Census变换和局部图优化的视觉里程计方法，其特征在于，所述的步骤S2中对当前帧使用直接法进行运动估计，选出局部图中的最新关键帧，基于Census描述图使用直接法估计最新关键帧到当前帧的位姿变换，迭代优化方法为基于金字塔的LK反向相乘算法；记当前帧为I，局部图中的最新关键帧为K，直接法优化目标函数为：

T^*＝argmdn_TΣ_xeΩw_x*||D(W(x，T)，l)-D(x，k)||²

其中T为关键帧K到当前帧I的位姿变换，W(x,T)为将从K中选出的像素点投影到当前帧的变换：W(x，T)＝π(T*π^-1(x，dx))，其中π(·)为相机投影函数，将像素点从相机坐标系投影到图像平面中；π^-1(·，·)为相机反投影函数，将给定深度值的像素点从图像平面反投影到相机坐标系，w_x为通过迭代重赋权最小二乘(IRLS)方法计算的权重值，Ω为从关键帧K中选出的满足梯度信息阈值t_g且有有效深度值的像素点；直接法迭代估计结束后，如果像素点x的权重w_x大于权重阈值t_w，则认为x在估计中属于良好点，良好点个数占运动估计所用的总像素个数的比例是生成关键帧的判据之一。

4.根据权利要求1所述的基于Census变换和局部图优化的视觉里程计方法，其特征在于，所述的步骤S3中，局部图由最新的前N个关键帧和帧上的梯度信息显著点组成，其中在深度图中缺少深度信息的梯度信息显著点会在已估计位姿的当前帧上进行深度跟踪估计；梯度信息显著点为满足梯度信息大于阈值t_g的点，其中用于深度跟踪估计的缺少深度信息的梯度信息显著点会用非最大值抑制进行过滤，从而提取有效点并减少处理量；深度跟踪估计的方法是，沿着极线在当前帧中寻找一个最佳的NCC匹配，找到最佳匹配后通过三角化可以得到对应的深度估计值。

5.根据权利要求4所述的基于Census变换和局部图优化的视觉里程计方法，其特征在于，所述的缺少深度信息的梯度信息显著点的深度跟踪估计，一次合理的深度跟踪匹配需要满足最佳NCC匹配的分值大于阈值t_NCC；另外，在第一次跟踪估计时会沿着极线在整个图像的范围内寻找最佳NCC匹配，而在后续帧的跟踪估计时，则会根据之前的估计结果，在限定的范围内搜索最佳NCC匹配，得到多次合理的深度跟踪估计后，深度跟踪估计收敛的判定条件为：

(i)得到的合理估计次数要大于等于5；(ii)多次估计的结果要有兼容性；

满足所有收敛条件后，取平均值作为最终的逆深度收敛结果，并不再在后续帧中进行跟踪估计，这个收敛结果将作为局部图优化步骤的有效初始值。

6.根据权利要求1所述的基于Census变换和局部图优化的视觉里程计方法，其特征在于，所述的步骤S4中，根据当前帧的运动估计结果，关键帧生成的判定标准如下：

(1)良好点个数占运动估计所用的总像素个数的比例小于阈值t_gwp；

(2)位姿估计结果中位移量模值大于阈值t_tran；

(3)位姿估计结果中旋转量模值大于阈值t_rot。

7.根据权利要求1所述的基于Census变换和局部图优化的视觉里程计方法，其特征在于，所述的步骤S4中，新关键帧插入局部图进行图优化和关键帧管理的操作包括下面步骤：

(1)构造局部图优化目标函数，进行非线性最小二乘优化；

(2)优化完后，将局部图关键帧中的梯度信息显著点都投影到最新关键帧中，更新最新关键帧的深度图信息；

(3)保持局部图中的关键帧数量不超过N_k，超过N_k时，将最旧的关键帧从局部图中移除。

8.根据权利要求7所述的基于Census变换和局部图优化的视觉里程计方法，其特征在于，所述的构造局部图优化目标函数，首先从局部图的每个关键帧中选出一定数量的梯度信息显著点：考虑优化的实时性能，每个关键帧使用非最大值抑制和梯度信息值排序，选出300个有有效深度的梯度信息显著点，并附加在步骤S3中进行了深度跟踪估计且收敛的点，然后将选出的点分别投影到局部图的其他关键帧中，构成残差项；

将每个残差项相加构成总的局部图优化残差函数，其中待优化的变量包括局部图中世界坐标系到每个关键帧的位姿变换T_i，以及那些深度跟踪估计后已收敛的点的逆深度，初始值为收敛后得到的逆深度值，作为变量经过局部图优化后的逆深度值，将作为有效深度值填充到对应关键帧的深度图中。

9.根据权利要求7所述的基于Census变换和局部图优化的视觉里程计方法，其特征在于，所述的步骤(2)投影梯度信息显著点更新最新关键帧深度图信息的操作中，这些投影的梯度信息显著点需要在对应深度图中含有有效深度值，在最新关键帧的投影位置处填充深度值：投影位置取整处理，如果投影位置在深度图上原来已有有效深度值，则不填充，如果原来没有有效深度值，则进行填充。