CN104851094A - 一种基于rgb-d的slam算法的改进方法 - Google Patents

Abstract

本发明公开了一种基于RGB-D的SLAM算法的改进方法，分为前端和后端两部分，前端的任务分为特征检测与描述符提取、特征匹配、运动变换估计和运动变换优化；后端的任务利用前端得到的6D运动变换关系初始化位姿图进行闭环检测添加闭环约束条件，利用非线性误差函数优化方法进行位姿图优化，得到全局最优相机位姿和相机运动轨迹，并进行三维环境重建。本发明使用ORB方法进行特征检测与描述符提取，并对深度信息不合法的特征点进行过滤；使用基于FLANN的KNN方法进行双向特征匹配，使用单应性矩阵变换对匹配结果进行优化；使用改进RANSAC的运动变换估计方法得到更精确的inliers匹配点对；使用基于GICP的运动变换优化方法提高点云配准的速度和精度。

Description

一种基于RGB-D的SLAM算法的改进方法

技术领域

本发明涉及移动机器人研究领域，具体是一种基于RGB-D的SLAM算法的改进方法。

背景技术

为了在未知环境中进行导航，移动机器人需要构建环境地图并且同时定位自身在地图中的位置，像这样同时解决这两个问题的过程就称为同步定位与地图构建(Simultaneously Localization And Mapping，SLAM)。当机器人处于室外环境的时候，这个问题可以通过高精度的GPS来解决。但是，当机器人处于室内环境时，或当GPS不够精确无法满足高精度的需求时，或者当机器人所处环境涉密时，人们就必须使用其他方法来精确估计机器人的位置并且同时构建环境地图。SLAM问题就是在这样的需求背景下提出的，是当前移动机器人研究领域的一个重要课题。

移动机器人可以通过不同类型的传感器获取所处环境的信息，传感器的类型不同，得到的环境信息的内容也不相同。如声呐测距仪获取的信息为传感器与障碍的距离信息，激光扫描测距仪获取的信息为传感器与障碍的距离和角度信息，视觉传感器获取的信息为环境的图像信息等等。移动机器人可以同时配置多类传感器，通过融合不同传感器所获取的信息，可以有效地提高地图环境的精度。

把视觉传感器和红外传感器集成到一起并实现两种传感器信息之间的良好同步，这样就构成了RGB-D传感器。基于RGB-D传感器的VSLAM则被称为RGB-D SLAM。

因此，本发明提出了一种针对RGB-D SLAM实现算法的各部分改进方法。

发明内容

本发明的目的在于提供一种增强特征匹配、匹配点精确的基于RGB-D的SLAM算法的改进方法，针对现有RGB-D SLAM算法中存在的效率低与误差大的问题做出改进。

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

一种基于RGB-D的SLAM算法的改进方法，分为前端和后端两部分，前端的任务为提取不同观察结果之间的空间关系，分为特征检测与描述符提取、特征匹配、运动变换估计和运动变换优化；后端的任务为使用非线性误差函数优化位姿图中相机的位姿信息；使用基于图的位姿优化方法，利用前端得到的6D运动变换关系初始化位姿图，然后进行闭环检测添加闭环约束条件，再利用非线性误差函数优化方法进行位姿图优化，最终得到全局最优相机位姿和相机运动轨迹，并进行三维环境重建；其中，特征检测与描述符提取、特征匹配、运动变换估计和运动变换优化的具体方法如下所述：

1)基于ORB的特征检测与描述符提取方法

ORB算法是一种快速特征检测与描述符提取算法，包含oFAST特征点检测和rBRIEF描述符提取两个步骤；该算法的具体内容如下：

(1)在oFAST特征点检测步骤中，首先对原图像构建尺度空间金字塔，然后使用FAST特征点检测方法检测出足够多的特征点并使用Harris角点评估方法对检测到的特征点进行筛选，再根据强度矩心求出特征点的方向；

(2)在rBRIEF描述符提取步骤中，首先对oFAST特征点生成BRIEF特征描述符，然后利用oFAST特征点的方向矢量对BRIEF特征描述符进行转向生成steered BRIEF特征描述符，再使用贪心学习算法从训练集中筛选出一组具有较大方差值、高度不相关且位均值位于0.5附近的二元点集，然后使用这组二元点集生成BRIEF特征描述符并进行转向，得到具有旋转不变性的rBRIEF特征描述子；

2)基于FLANN的增强特征匹配方法

基于FLANN的增强特征匹配方法是对查找帧和训练帧特征描述符进行双向的基于FLANN的KNN匹配，然后使用单应性矩阵变换对匹配结果进行优化；

3)改进RANSAC的运动变换估计方法

使用RANSAC的方法进行运动变换估计总体上是一个迭代的过程；在每一次迭代过程中从三维坐标匹配点对集合中随机选择20个三维坐标匹配点对，根据这些三维坐标匹配点对求出一个运动变换模型；对于三维坐标匹配点对集合中的每一个元素，使用所求出的运动变换模型将元素中的原坐标进行投影以得到投影坐标，计算目标坐标与投影坐标之间的误差，如果误差小于阈值，则该三维坐标匹配点对是局内点，将其加入局内点集合；否则认为它是局外点，不作处理；统计局内点的数目，并计算该模型下的平均误差；如果局内点数目高于阈值并且平均误差低于阈值，则利用所有的局内点重新计算新的运动变换模型；然后再次对新的运动变换模型进行评估；

(4)基于GICP的运动变换优化方法

GICP算法在对标准ICP算法和点到面ICP算法进行归纳与综合的基础上，更进一步使用了面到面的思想进行改进；GICP算法的抽象程度更高，而且在一定条件下退化成标准ICP算法或者点到面ICP算法；GICP算法点云配准所需的时间高于标准ICP算法，但是GICP的精度远远高于标准ICP，使用GICP算法替换标准ICP算法进行RGB-D SLAM算法中的运动变换优化，以使得求解出的运动变换误差更小。

作为本发明进一步的方案：oFAST特征点检测的具体步骤如下所述，FAST是一种非常快速的特征点检测算法，根据候选点与其圆形邻域内相邻像素点的灰度值差异来判断候选点是否为特征点，具体判断方法如式(1-1)所示：

$N={\mathrm{\Σ}}_{x\∀\mathrm{circle}\left(p\right)}|I\left(x\right)-I\left(p\right)|>{\mathrm{\ϵ}}_d---(1-1)$

式中，p为候选点，circle(p)为p的圆形邻域，x为候选点圆形邻域内任意一像素点，I(p)为候选点的图像灰度值，I(x)为x处的图像灰度值；若N超出阈值条件ε_d，则该候选点为特征点；

在FAST特征点检测结束后，使用Harris角点评估方法对特征点进行筛选；如果想要得到的目标特征点数目为N，则首先设置一个足够低的阈值以获得超过N个特征点，然后使用Harris评估方法对这些特征点进行排序，然后选取前N个作为所得到的目标特征点；

为了使检测到的特征点满足旋转不变形，ORB算法中使用强度矩心的方法为检测到的特征点分配方向；该方法假设角点的强度为它与中心的偏移量，并且这个矢量能用于求解角点方向；局部图像的矩如式(1-2)所示：

m_pq＝Σ_x，yx^py^qI(x，y)              (1-2)

然后通过式(1-2)求出矩m₀₀、m₁₀和m₀₁后，根据式(1-3)求出矩心C：

$C=(\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}})---(1-3)$

然后构建一个从角点中心O到矩心C的向量的方向正是局部图像角点的方向，计算公式如式(1-4)所示：

θ＝atan2(m₀₁，m₁₀)            (1-4)

此外，为增强方法的旋转不变性，求取矩的时候，x和y应当始终在角点的r半径圆形邻域内，所以在ORB算法中显示地令r为局部图像的大小，即x和y的范围为[-r，r]；得到了带有方向的FAST特征点，它被称为oFAST特征点。

作为本发明进一步的方案：rBRIEF描述符提取的具体步骤如下所述：其中BRIEF特征描述符是一种可快速计算的二进制编码形式描述符；它首先对图像进行高斯平滑滤波，然后在oFAST特征点的某邻域内随机选取点对(x，y)并对这些点的灰度值进行二值化测试，并将测试结果按照一定顺序生成一个二进制串，这个二进制串即为BRIEF特征描述符；二值化测试τ的定义如式(1-5)所示，其中，p(x)为p在点x处的强度；

$\mathrm{\&tau;}(p;x,y):=\left\{\begin{array}{cc}1& :p\left(x\right)<p\left(y\right)\\ 0& :p\left(x\right)\&GreaterEqual;p\left(y\right)\end{array}\right.---(1-5)$

BRIEF特征描述符即为n个二值化测试所构成的向量，如式(1-6)所示：

f_n(p)：＝Σ_1≤i≤n2^i-1τ(p，x_i，y_i)           (1-6)

在ORB算法中，点对(x，y)的选取符合高斯分布，其中S为特征点的邻域大小；

由于BRIEF不具有旋转不变性，所以ORB算法中利用特征点的方向矢量对BRIEF特征描述符进行转向，生成steered BRIEF特征描述符；

设原始的BRIEF选取的点集如式(1-7)所示：

$S=\left(\begin{array}{c}{x}_1,...,x_n\\ y_1,...,y_n\end{array}\right)---(1-7)$

使用特征点方向θ和相应的旋转矩阵R_θ构建旋转的点集S_θ，如式(1-8)所示：

S_θ＝R_θS            (1-8)

则生成的steered BRIEF特征描述符即如式(1-9)所示：

g_n(p，θ)：＝f_n(p)|(x_i，y_i)∈S_θ      (1-9)

贪心学习算法具体过程如下：首先对全部的训练局部图像执行所有可能的二值化测试，然后根据测试结果与均值0.5的距离对所有测试进行排序，并生成测试向量T，然后开始贪心搜索过程：首先从T中取出第一个测试放入结果向量R中，然后从T中取出下一个测试，并将它和R中的所有测试进行比较，并估算它们之间的相关性，如果相关性高于某个阈值，则丢弃它；否则将它加入R；重复上述步骤，直到R中的测试数目达到256个，然后结束；否则如果T中的所有测试都已经取出，而R中的测试数目仍然少于256，那么降低阈值，重新开始贪心搜索过程；通过上述步骤即可得到具有旋转不变形和强抗噪声能力rBRIEF特征描述符。

作为本发明进一步的方案：所述基于FLANN的增强特征匹配方法，具体内容如下：

FLANN是在高维空间执行快速近似最近邻搜索的一个库，它包含一组优秀的最近邻搜索算法和一个自动选择最优算法并且根据数据集对算法参数进行优化的系统；对于高维空间中的最近邻搜索问题，FLANN采用基于分层k-均值树的优先级搜索算法或采用多重随机k-d树时具有最好的性能；相对于其他传统最近邻搜索算法，基于FLANN的近似快速最近邻搜索的速度提高了一个数量级；

其次，为了解决BruteForce匹配法为查找帧中所有特征点查找其在训练帧中距离最近的匹配特征点的问题，采用基于FLANN的KNN搜索方法进行查找，并对查找结果进行过滤；当给定向量空间M中的一个点集P＝{p₁，p₂，...，p_n}和一个待查询点q∈M，则q的最近邻元素NN(q，P)∈P定义为NN(q，P)＝qrgmin_x∈pd(q，x)，其中d(q，x)表示q与x的距离；KNN搜索就是同时查找K最近邻的搜索方法，K最近邻的定义如式(1-10)所示：

KNN(q，P，K)＝Q               (1-10)

其中A是满足如式(1-11)所示约束的集合：

$\left|A\right|=K,A\&SubsetEqual;P,\&ForAll;x\&Element;A,y\&Element;P-A,d(q,x)\&le;d(q,y)---(1-11)$

在使用KNN搜索方法时令K＝2，即对查找帧中的每一个特征点s_i，都在训练帧中使用KNN二近邻搜索方法查找它的最近邻点t_i1和次近邻点t_i2，s_i和它们的距离分别为d_i1和d_i2；比较d_i1和d_i2之间的距离，只有当d_i1远远小于d_i2时，则s_i和t_i是正确匹配点对；否则当d_i1和d_i2比较接近时，则s_i和t_i1、s_i和t_i2都不是正确匹配点对，应该将其剔除；或者认为s_i和t_i1、s_i和t_i2都能够正确匹配，但是为了减少这种匹配关系对后续运动变换的影响，也应该将其剔除；令在最终实现的系统中，当ratio_i小于0.6时保留对应的匹配点对，否则将其剔除；

为了消除单方向匹配造成的错误匹配点对，使用双向的基于FLANN的KNN搜索来查找正确的匹配点对；具体做法为，首先对于查找帧QueryFrame中的每一个特征点，使用基于FLANN的二近邻搜索在训练帧TrainFrame中查找匹配点对，并对最近邻和次近邻距离相近的匹配点对过滤，记录下最终得到的好的匹配点对集合S₁；然后反方向执行上述步骤，即为对于训练帧TrainFrame中的每一个特征点，使用基于FLANN的二近邻搜索在查找帧QueryFrame中查找匹配点对，并对最近邻和次近邻距离相近的匹配点对过滤，记录下最终得到的好的匹配点对集合S₂；对于任意pair(i，j)∈S₁，查找相应的pqir(j，k)∈S₂，如果i＝k则认为查找帧QueryFrame中的特征点i与训练帧TrainFrame中的特征点j是双向匹配的，将该匹配点对加入集合S；否则认为查找帧QueryFrame中的特征点i与训练帧TrainFrame中的特征点j不匹配，将其丢弃；遍历集合S₁中的各个匹配点对，对其执行上述操作，得到的集合S即为满足双向匹配关系的匹配点对集合；

为了解决相同或者相似物体在环境中重复出现导致匹配结果错误的问题，使用单应性矩阵变换来进一步优化匹配结果；具体做法是，对于每一个pair(i，j)∈S，提取出i在查找帧QueryFrame中的坐标(x_i，y_i)并加入查找帧坐标集合QureyPosSets，提取出j在训练帧TrainFrame中的坐标(x_j，y_j)并加入训练帧坐标集合TrainPosSets，然后对这两个坐标集合使用RANSAC方法进行单应性矩阵变换求出最优单应性变换矩阵，然后剔除集合S中不满足单应性变换矩阵约束的匹配点对，得到的集合S即为具有高可信度的匹配点对。

作为本发明进一步的方案：基于GICP的运动变换优化方法：标准ICP算法首先计算两个点云中的匹配点，然后计算使这些匹配点距离最小的运动变换；

点到面ICP是标准ICP的一种改进，通过使用表面法线信息来提高点云配准的精度；该算法使用 $T\&LeftArrow;\stackrel{\arg \min }{T}\left\{{\mathrm{\&Sigma;}}_i{w}_i{\left|\right|{\mathrm{\&eta;}}_i\&CenterDot;(T\&CenterDot;b_i-m_i)\left|\right|}^2\right\}$ 来代替标准ICP算法的运动变换求解约束，其中η_i为点m_i的表面法线；

GICP在对标准ICP和点到面ICP进行综合的基础上，还在运动变换求解步骤中使用了概率模型进行最小化运算；假设在标准ICP算法中for循环部分计算两个点云中的匹配点的步骤完成后得到了两个点云A＝{a_i}_{i＝1，...，N}和B＝{b_i}_{i＝1，...，N}，且下标相同的A和B的元素相互对应，同时假设A和B集合中满足||m_i-T·b_i||＞d_max对应关系已经被剔除，则GICP算法的改进方法如下：

在概率模型中假设存在基础点集和且它们根据和生成集合A和B，其中和是待测量点的协方差矩阵；假设和完全对应，则存在运动变换T^*满足式(1-12)：

${\hat{b}}_i=T*{\hat{a}}_i---(1-12)$

对任意刚体变换T，定义d_i ^(T)＝b_i-Ta_i，则有式(1-13)成立：

$T=\underset{T}{\arg \min }{\mathrm{\&Sigma;}}_i{d_i}^{{\left(T\right)}^T}{(C_i^B+T{C}_i^A{T}^T)}^{-1}{d_i}^{\left(T\right)}---(1-13)$

而式(1-13)正是GICP对标准ICP运动变换求解步骤的具体改进方法；当且时，式(1-13)退化为标准ICP的运动变换求解约束；当且时，式(1-13)退化为而该式为点到面的ICP的运动变换求解约束的另一种表示形式，即GICP退化为点到面ICP；因此，上述改进方法被命名为GICP，即广义ICP。

与现有技术相比，本发明的有益效果是：

1、本发明提供在特征检测与描述符提取阶段使用基于ORB的特征检测和描述符提取方法，即使用ORB方法进行特征检测与描述符提取，并对深度信息不合法的特征点进行过滤。

2、本发明提出在特征匹配阶段使用基于FLANN的增强特征匹配方法，即使用基于FLANN的KNN方法进行双向特征匹配，并且使用单应性矩阵变换对匹配结果进行优化。

3、本发明中在运动变换估计阶段使用改进RANSAC的运动变换估计方法，以得到更精确的inliers匹配点对。

4、本发明中在运动变换优化阶段使用基于GICP的运动变换优化方法，该方法基于高精度的GICP算法进行点云配准，使用inliers生成的点云进行GICP点云配准并对失败的情况做了退化处理，并且对RANSAC失败的情况做了退化处理，以提高点云配准的速度和精度。

附图说明

图1为本发明中改进的RGB-D SLAM算法流程图

图2为本发明中GICP算法与其他方法误差对比；

图3为本发明中基于FLANN的增强特征匹配方法流程图；

图4为本发明中基于GICP的运动变换优化方法整体流程图；

图5为本发明中GICP算法与其他方法误差对比图；

图6是RGB-D SLAM算法整体过程示意图。

具体实施方式

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

请参阅图6，一种基于RGB-D的SLAM算法的改进方法，包括以下内容：

1)基于ORB的特征检测与描述符提取方法。

ORB(Oriented FAST and Rotated BRIEF，具有方向的FAST和旋转的BRIEF)算法是一种快速特征检测与描述符提取算法，包含oFAST特征点检测和rBRIEF描述符提取两个步骤。该算法的具体内容如下：

(1)在oFAST特征点检测步骤中，首先对原图像构建尺度空间金字塔，然后使用FAST(Features from Accelerated Segment Test，加速分割测试特征)特征点检测方法检测出足够多的特征点并使用Harris角点评估方法对检测到的特征点进行筛选，最后根据强度矩心(Intensity Centroid)求出特征点的方向。

其中，FAST是一种非常快速的特征点检测算法，它根据候选点与其圆形邻域内相邻像素点的灰度值差异来判断候选点是否为特征点，具体判断方法如式(1-1)所示：

$N={\mathrm{\&Sigma;}}_{x\&ForAll;\mathrm{circle}\left(p\right)}|I\left(x\right)-I\left(p\right)|>{\mathrm{\&epsiv;}}_d---(1-1)$

式中，p为候选点，circle(p)为p的圆形邻域，x为候选点圆形邻域内任意一像素点，I(p)为候选点的图像灰度值，I(x)为x处的图像灰度值。若N超出阈值条件ε_d，则该候选点为特征点。在ORB中采用的是FAST-9，即圆形邻域半径为9。

由于FAST特征点检测算法会产生非常大的边缘响应，所以在FAST特征点检测结束后，还需要使用Harris角点评估方法对特征点进行筛选。如果想要得到的目标特征点数目为N，则首先设置一个足够低的阈值以获得超过N个特征点，然后使用Harris评估方法对这些特征点进行排序，最终选取前N个作为所得到的目标特征点。

为了使检测到的特征点满足旋转不变形，ORB算法中使用强度矩心的方法为检测到的特征点分配方向。强度矩心的方法是由Rosin提出的，该方法假设角点的强度为它与中心的偏移量，并且这个矢量可以用于求解角点方向。Rosin定义局部图像的矩如式(1-2)所示：

m_pq＝Σ_x，yx^py^qI(x，y)                     (1-2)

然后通过式(1-2)求出矩m₀₀、m₁₀和m₀₁后，根据式(1-3)求出矩心C：

$C=(\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}})---(1-3)$

然后构建一个从角点中心O到矩心C的向量的方向正是局部图像角点的方向，计算公式如式(1-4)所示：

θ＝atan2(m₀₁，m₁₀)                     (1-4)

此外，为增强方法的旋转不变性，求取矩的时候，x和y应当始终在角点的r半径圆形邻域内，所以在ORB算法中显示地令r为局部图像的大小，即x和y的范围为[-r，r]。

至此，算法得到了带有方向的FAST特征点，它被称为oFAST特征点。

(2)在rBRIEF描述符提取步骤中，首先对oFAST特征点生成BRIEF特征描述符，然后利用特征点的方向矢量对BRIEF特征描述符进行转向生成steered BRIEF特征描述符，最后使用贪心学习算法从训练集中筛选出一组具有较大方差值、高度不相关且位均值位于0.5附近的二元点集，然后使用这组二元点集生成BRIEF特征描述符并进行转向，最终得到具有旋转不变性的rBRIEF特征描述子。

其中BRIEF(Binary Robust Independent Element Feature，二值化鲁棒独立元特征)特征描述符是由Michael Calonder和Vincent Lepetit等人于2010年提出的一种可快速计算的二进制编码形式描述符。它首先对图像进行高斯平滑滤波，然后在特征点的某邻域内随机选取点对(x，y)并对这些点的灰度值进行二值化测试，并将测试结果按照一定顺序生成一个二进制串，这个二进制串即为BRIEF特征描述符。二值化测试τ的定义如式(1-5)所示，其中，p(x)为p在点x处的强度。

$\mathrm{\&tau;}(p;x,y):=\left\{\begin{array}{cc}1& :p\left(x\right)<p\left(y\right)\\ 0& :p\left(x\right)\&GreaterEqual;p\left(y\right)\end{array}\right.---(1-5)$

BRIEF特征描述符即为n个二值化测试所构成的向量，如式(1-6)所示：

f_n(p)：＝Σ_1≤i≤n2^i-1τ(p，x_i，y_i)         (1-6)

在ORB算法中，点对(x，y)的选取符合高斯分布，其中S为特征点的邻域大小。

由于BRIEF不具有旋转不变性，所以ORB算法中利用特征点的方向矢量对BRIEF特征描述符进行转向，生成steered BRIEF特征描述符。

设原始的BRIEF选取的点集如式(1-7)所示：

$S=\left(\begin{array}{c}{x}_1,...,x_n\\ y_1,...,y_n\end{array}\right)---(1-7)$

使用特征点方向θ和相应的旋转矩阵R_θ构建旋转的点集S_θ，如式(1-8)所示：

S_θ＝R_θS       (1-8)

则生成的steered BRIEF特征描述符即如式(1-9)所示：

g_n(p，θ)：＝f_n(p)|(x_i，y_i)∈S_θ       (1-9)

由于steeredBRIEF通过添加方向矢量增强了描述子的旋转不变性，但其可区分性相对BRIEF有所减弱，为了解决这一问题，ORB提出使用贪心学习算法从训练集中筛选出一组具有较大方差值、高度不相关且位均值位于0.5附近的二元点集，然后使用这组二元点集生成BRIEF特征描述符并进行转向，得到具有旋转不变性的rBRIEF特征描述子。

贪心学习算法具体过程如下：首先对全部的训练局部图像执行所有可能的二值化测试，然后根据测试结果与均值0.5的距离对所有测试进行排序，并生成测试向量T，然后开始贪心搜索过程：首先从T中取出第一个测试放入结果向量R中，然后从T中取出下一个测试，并将它和R中的所有测试进行比较，并估算它们之间的相关性，如果相关性高于某个阈值，则丢弃它；否则将它加入R。重复上述步骤，直到R中的测试数目达到256个，然后结束；否则如果T中的所有测试都已经取出，而R中的测试数目仍然少于256，那么降低阈值，重新开始贪心搜索过程。通过上述步骤即可得到具有旋转不变形和强抗噪声能力rBRIEF特征描述符。

对一帧640×480的图像执行一次ORB算法所需平均时间如表1所示：

表1 单帧图像ORB算法执行时间分析表

ORB算法	构建尺度金字塔	oFAST	rBRIEF
				花费时间(ms)	4.43	8.68	2.12

从表1可以看出，无论是构建尺度金字塔、使用oFAST进行特征点检测还是使用rBRIEF进行描述符提取阶段，ORB算法所花费的时间都是非常少的，因此ORB特征检测与描述符提取方法的速度非常快，比SURF快一个数量级，比SIFT快两个数量级，可以很好地满足实时性要求较高的应用，三种算法平均每帧图像花费时间如表2所示：

表2 ORB与SIFT、SURF方法时间对比结果表

特征检测与描述符提取方法	ORB	SURF	SIFT
				平均每帧图像所需时间(ms)	15.3	217.3	5228.7

2)基于FLANN的增强特征匹配方法。

使用基于FLANN的增强特征匹配方法对原始算法进行改进，具体内容如下：

首先，为了解决BruteForce匹配方法速度慢的问题，本发明使用基于FLANN(FastLibrary for Approximate Nearest Neighbors，快速近似最近邻搜索库)的特征匹配方法进行特征匹配。FLANN是在高维空间执行快速近似最近邻搜索的一个库，它包含一组优秀的最近邻搜索算法和一个自动选择最优算法并且根据数据集对算法参数进行优化的系统。对于高维空间中的最近邻搜索问题，FLANN采用基于分层k-均值树的优先级搜索算法或采用多重随机k-d树时具有最好的性能。相对于其他传统最近邻搜索算法，基于FLANN的近似快速最近邻搜索的速度提高了一个数量级。

其次，为了解决BruteForce匹配法为查找帧中所有特征点查找其在训练帧中距离最近的匹配特征点的问题，本发明采用基于FLANN的KNN搜索(KNearest Neighbor Search)方法进行查找，并对查找结果进行过滤。当给定向量空间M中的一个点集P＝{p₁，p₂，...，p_n}和一个待查询点q∈M，则q的最近邻元素NN(q，P)∈P定义为NN(q，P)＝argmin_x∈pd(q，x)，其中d(q，x)表示q与x的距离。KNN搜索就是同时查找K最近邻的搜索方法，K最近邻的定义如式(1-10)所示：

KNN(q，P，K)＝A            (1-10)

其中A是满足如式(1-11)所示约束的集合：

$\left|A\right|=K,A\&SubsetEqual;P,\&ForAll;x\&Element;A,y\&Element;P-A,d(q,x)\&le;d(q,y)---(1-11)$

本发明在使用KNN搜索方法时令K＝2，即对查找帧中的每一个特征点s_i，都在训练帧中使用KNN二近邻搜索方法查找它的最近邻点t_i1和次近邻点t_i2，s_i和它们的距离分别为d_i1和d_i2。比较d_i1和d_i2之间的距离，只有当d_i1远远小于d_i2时，则s_i和t_i是正确匹配点对；否则当d_i1和d_i2比较接近时，则s_i和t_i1、s_i和t_i2都不是正确匹配点对，应该将其剔除；或者认为s_i和t_i1、s_i和t_i2都能够正确匹配(比如在场景中出现若干相同物体)，但是为了减少这种匹配关系对后续运动变换的影响，也应该将其剔除。令在最终实现的系统中，当ratio_i小于0.6时保留对应的匹配点对，否则将其剔除。

再次，为了消除单方向匹配造成的错误匹配点对，本发明使用双向的基于FLANN的KNN搜索来查找正确的匹配点对。具体做法为，首先对于查找帧QueryFrame中的每一个特征点，使用基于FLANN的二近邻搜索在训练帧TrainFrame中查找匹配点对，并对最近邻和次近邻距离相近的匹配点对过滤，记录下最终得到的好的匹配点对集合S₁；然后反方向执行上述步骤，即为对于训练帧TrainFrame中的每一个特征点，使用基于FLANN的二近邻搜索在查找帧QueryFrame中查找匹配点对，并对最近邻和次近邻距离相近的匹配点对过滤，记录下最终得到的好的匹配点对集合S₂。对于任意pair(i，j)∈S₁，查找相应的pair(j，k)∈S₂，如果i＝k则认为查找帧QueryFrame中的特征点i与训练帧TrainFrame中的特征点j是双向匹配的，将该匹配点对加入集合S；否则认为查找帧QueryFrame中的特征点i与训练帧TrainFrame中的特征点j不匹配，将其丢弃。遍历集合S₁中的各个匹配点对，对其执行上述操作，最终得到的集合S即为满足双向匹配关系的匹配点对集合。

最后，为了解决若干相同或者相似物体在环境中重复出现导致匹配结果错误的问题，本发明使用单应性矩阵变换来进一步优化匹配结果。单应性变换是计算机视觉领域中一个射影平面到另一个射影平面的可逆变换，空间中同一平面的任意两幅图像可以通过单应性变换关联在一起，从而得到一个单应性变换矩阵，通过该单应性变换矩阵可以求出两幅图像之间的匹配点对关系。本发明使用单应性矩阵变换来剔除错误匹配点对的依据是如果某物体在场景中重复出现，但是其所在的背景场景相似度较低，那么可以利用背景的不匹配来剔除这些重复出现物体的错误匹配关系。具体做法是，对于每一个pair(i，j)∈S，提取出i在查找帧QueryFrame中的坐标(x_i，y_i)并加入查找帧坐标集合QureyPosSets，提取出j在训练帧TrainFrame中的坐标(x_j，y_j)并加入训练帧坐标集合TrainPosSets，然后对这两个坐标集合使用RANSAC方法进行单应性矩阵变换求出最优单应性变换矩阵，然后剔除集合S中不满足单应性变换矩阵约束的匹配点对，最终得到的集合S即为具有高可信度的匹配点对。

综上所述，基于FLANN的增强特征匹配方法可以综合描述为对查找帧和训练帧特征描述符进行双向的基于FLANN的KNN匹配，然后使用单应性矩阵变换对匹配结果进行优化，

3)改进RANSAC的运动变换估计方法。

运动变换估计方法的改进方案如下：

使用三维RANSAC的方法进行运动变换估计总体上是一个迭代的过程。在每一次迭代过程中从三维坐标匹配点对集合中随机选择20个三维坐标匹配点对，根据这些三维坐标匹配点对求出一个运动变换模型。对于三维坐标匹配点对集合中的每一个元素，使用所求出的运动变换模型将元素中的原坐标进行投影以得到投影坐标，计算目标坐标与投影坐标之间的误差，如果误差小于阈值，该三维坐标匹配点对是局内点(inlier)，将其加入局内点集合；否则认为它是局外点，不作处理。统计局内点的数目，并计算该模型下的平均误差。如果局内点数目高于阈值并且平均误差低于阈值，则利用所有的局内点重新计算新的运动变换模型。然后再次对新的运动变换模型进行评估，具体方法为，对于三维坐标匹配点对集合中的每一个元素，使用最新求出的运动变换模型将元素中的原坐标进行投影以得到投影坐标，计算目标坐标与投影坐标之间的误差，如果误差小于阈值，该三维坐标匹配点对是局内点，将其加入局内点集合；否则认为它是局外点，不作处理。统计局内点的数目，并计算最新求得的模型的平均误差。如果得到的新的运动变换模型的局内点数目高于当前最优运动变换模型并且平均误差低于当前最优运动变换模型，则认为该新运动变换模型比当前最优变换模型更好，则将当前最优变换模型更新为新运动变换模型。上述过程迭代进行若干次，在每一次迭代过程中都随机选择新的三维坐标匹配点，并且在每一次迭代的末尾都保存当前最优的运动变换模型参数。迭代结束后即可得到最优运动变换模型和最优inliers集合及误差后。如果最优inliers集合中局内点的数目高于阈值并且误差低于阈值，则利用最优inliers集合再次计算新的运动变换模型并记为最终运动变换模型，然后再次对最终运动变换模型进行评估，对于三维坐标匹配点对集合中的每一个元素，使用最终运动变换模型对元素中的原坐标进行投影以得到投影坐标，计算目标坐标与投影坐标之间的误差，如果误差小于阈值，该三维坐标匹配点对是局内点，将其加入最终inliers集合；否则认为它是局外点，不作处理。最终得到的最终运动变换模型和最终inliers集合即为所求结果。

(4)基于GICP的运动变换优化方法

由于原始RGB-D SLAM算法中使用标准ICP算法进行运动变换优化，而标准ICP算法进行点云配准时的误差较大。Stanford大学的Aleksandr V.Segal、Dirk Haehnel和Sebastian Thrun最新提出了一种用于进行高精度点云配准的算法，即GICP(Generalized-ICP，广义ICP)算法，该算法的误差比标准ICP算法误差小一个数量级，所以本发明提出使用GICP算法对RGB-D SLAM算法的运动变换优化阶段进行改进。

GICP算法在对标准ICP算法和点到面ICP(point-to-plane ICP)算法进行归纳与综合的基础上，更进一步使用了面到面(plane-to-plane)的思想进行改进。GICP算法的抽象程度更高，而且在一定条件下可以退化成标准ICP算法或者点到面ICP算法。

标准ICP算法首先计算两个点云中的匹配点，然后计算使这些匹配点距离最小的运动变换。

点到面ICP是标准ICP的一种改进，通过使用表面法线信息来提高点云配准的精度。该算法使用 $T\&LeftArrow;\underset{T}{\arg \min }\left\{{\mathrm{\&Sigma;}}_i{w}_i{\left|\right|{\mathrm{\&eta;}}_i\&CenterDot;(T\&CenterDot;b_i-m_i)\left|\right|}^2\right\}$ 来代替标准ICP算法的运动变换求解约束，其中η_i为点m_i的表面法线。

GICP在对标准ICP和点到面ICP进行综合的基础上，还在运动变换求解步骤中使用了概率模型进行最小化运算。为了简化描述，假设在标准ICP算法中for循环部分计算两个点云中的匹配点的步骤完成后得到了两个点云A＝{a_i}_{i＝1，...，N}和B＝{b_i}_{i＝1，...，N}，且下标相同的A和B的元素相互对应(例如，a_i和b_i相对应)，同时假设A和B集合中满足||m_i-T·b_i||＞d_max对应关系已经被剔除，则GICP算法的改进方法如下：

在概率模型中假设存在基础点集和且它们根据和生成集合A和B，其中和是待测量点的协方差矩阵。假设和完全对应，则存在运动变换T^*满足式(1-12)：

对任意刚体变换T，定义d_i ^(T)＝b_i-Ta_i，则有式(1-13)成立：

$T=\underset{T}{\arg \min }{\mathrm{\&Sigma;}}_i{d_i}^{{\left(T\right)}^T}{(C_i^B+T{C}_i^A{T}^T)}^{-1}{d_i}^{\left(T\right)}---(1-13)$

而式(1-13)正是GICP对标准ICP运动变换求解步骤的具体改进方法。当且时，式(1-13)退化为标准ICP的运动变换求解约束；当且时，式(1-13)退化为而该式为点到面的ICP的运动变换求解约束的另一种表示形式，即GICP退化为点到面ICP。因此，上述改进方法被命名为GICP，即广义ICP。

GICP算法点云配准所需的时间高于标准ICP算法，但是GICP的精度远远高于标准ICP，所以本发明提出使用GICP算法替换标准ICP算法进行RGB-DSLAM算法中的运动变换优化，以使得求解出的运动变换误差更小。

实施例1

本发明实施例中，所运行的硬件环境为联想笔记本电脑Lenovo ideapad Y471A，InterCore i5-2410M CPU2.30GHz双核四线程2.3GHz处理器，三级缓存为3M和6M，4G内存。

算法所运行的系统为Ubuntu 12.04，内核版本为3.5.0-54-generic，本发明所有算法均通过gcc 4.6.3编译，优化级别为三级(-O3)。

在对RGB-D SLAM算法进行评估时，可以分别从效率和精度两方面进行考虑。如果优化后的算法比原始算法所需时间更短或者精度更高，误差更小，则认为优化是正确的。为了对所提出的改进方法进行验证，本发明分别从效率和精度两个方面对改进前和改进后的算法进行了测试与对比，并根据对比结果对算法进行评估。

同时，为了增强实验结果的说服力，本发明还使用了德国Munich理工大学计算机科学系的Jurgen Sturm和Daniel Cremers和Freiburg大学计算机科学系的NikolasEngelhard和Felix Endres提供的标准测试程序和标准测试数据集rgbd_dataset_freiburg1_360对改进前和改进后的算法进行评估。

rgbd_dataset_freiburg1_360数据集中包含754帧RGB和Depth图像和相应的groundtruth数据，其中图像所记录的内容为德国Freiburg大学某个实验室中的场景，包含书桌、椅子、风扇、电脑、黑板、门窗、海报、地面、天花板、墙面、摄像机、书柜、书和人等，groundtruth数据是使用包含8个高速追踪相机(100hz)的高精度运动捕捉系统所记录Kinect相机的真实位姿信息。数据集作者通过手持Kinect相机的方式录制数据，在录制过程中缓慢运动，并且在录制数据的时候，作者不断对相机进行各种平移和旋转，相机位姿不断发生变化，录制的RGB-D图像的为360°全方位图像，所以使用该数据集测试得到的实验结果非常具有说服力。

图1所示为改进的RGB-D SLAM算法流程图。其中提出使用基于ORB的特征检测和描述符提取提取方法、基于FLANN的增强特征匹配方法、改进的RANSAC运动变换估计方法和基于GICP的运动变换优化方法对原始RGB-D SLAM算法进行改进，以使得改进后的算法满足高效率和高精度的要求。

图2所示为基于ORB的特征检测与描述符提取方法流程图。在基于ORB的特征检测与描述符提取方法中，首先创建ORB特征检测算子进行特征检测，然后过滤深度信息不合法的特征点，然后再创建ORB描述符提取算子进行描述符提取。之所以要添加过滤深度信息不合法特征点这个步骤，是因为深度信息不合法特征点将导致运算结果误差变大，将其滤除能够有效地降低误差；而将过滤深度信息不合法特征点步骤加到描述符提取步骤之前，则可以有效地减少描述符提取的数量，从而减少运算量，提高算法速度。

图3所示为基于FLANN的增强特征匹配方法流程图。基于FLANN的增强特征匹配方法可以综合描述为对查找帧和训练帧特征描述符进行双向的基于FLANN的KNN匹配，然后使用单应性矩阵变换对匹配结果进行优化。

图4所示为基于GICP的运动变换优化方法整体流程图。为了接下来描述方便，本发明将由原始三维坐标匹配点对集合生成的点云称为MatchesPointCloud，将最终inliers匹配点对集合生成的点云称为InliersPointCloud，将对原始图像对应的整个点云称为TotalPointCloud；将对MatchesPointCloud进行ICP点云配准从而进行运动变换优化称为MatchesPointCloudICP，将对InliersPointCloud进行ICP点云配准从而进行运动变换优化称为InliersPointCloudICP，将对TotalPointCloud进行ICP点云配准从而进行运动变换优化称为TotalPointCloudICP；将对MatchesPointCloud进行GICP点云配准从而进行运动变换优化称为MatchesPointCloudGICP，将对InliersPointCloud进行GICP点云配准从而进行运动变换优化称为InliersPointCloudGICP，将对TotalPointCloud进行GICP点云配准从而进行运动变换优化称为TotalPointCloudGICP。

如图4所示，本发明所提出的基于GICP的运动变换估计方法具体表示为：当RANSAC成功时使用可退化的InliersPointCloudGICP子流程进行运动变换优化，而当特征匹配结果数目过小或者RANSAC失败的情况下进行无初始运动变换估计的TotalPointCloudGICP来求解运动变换矩阵，并直接返回结果。

图5所示为GICP算法与其他方法误差对比图。由于原始RGB-D SLAM算法中使用标准ICP算法进行运动变换优化，而标准ICP算法进行点云配准时的误差较大。Stanford大学的Aleksandr V.Segal、Dirk Haehnel和Sebastian Thrun最新提出了一种用于进行高精度点云配准的算法，即GICP(Generalized-ICP，广义ICP)算法，该算法的误差比标准ICP算法误差小一个数量级。

本发明在测试时选取了使用SIFT、SURF_default、SURF_0、ORB_default和ORB_10000分别对改进前后的算法进行了测试、对比与评估，其中SURF_0和ORB_10000用于极端情况下的对比。

特征检测与描述符提取测试主要是通过运用测试集的单幅图像测试结果对比、测试数据集所有图像测试结果对比和过滤深度信息不合法的特征点。

特征匹配测试主要是通过运用测试集的相邻两帧测试结果对比、测试数据集所有图像测试结果对比和特征检测、描述符提取与特征匹配综合比较。

运动变换估计测试主要是通过运用测试集的相邻两帧运动变换估计方法实验结果对比和测试数据集所有图像运动变换估计实验结果对比。

运动变化优化测试主要是通过运用测试集的点云降采样实验结果对比、对相邻帧运动变换优化方法实验结果对比和测试数据集所有图像运动变换优化实验结果对比，以及前端算法综合时间对比。

整个前端算法结果对比测试主要是通过运用测试集的前端综合优化结果与测试数据集groundtruth对比、前端结果相机位姿对比和前端地图拼接结果展示。

实际环境测试主要是通过小型场景地图拼接结果展示、中型场景地图拼接结果展示和前端地图拼接结果展示。

原始SIFT_RGB-D_SLAM算法和原始SURF_RGB-D_SLAM算法均为低速度低精度算法，其中原始SIFT_RGB-D_SLAM算法的速度和精度均略高于原始SURF_RGB-D_SLAM算法，而原始ORB_RGB-D_SLAM算法为高速度低精度算法。改进SIFT_RGB-D_SLAM算法和改进SURF_RGB-D_SLAM算法为低速度高精度算法，其中改进SIFT_RGB-D_SLAM算法的速度和精度均略高于改进SURF_RGB-D_SLAM算法；改进SIFT_RGB-D_SLAM算法与原始SIFT_RGB-D_SLAM算法相比精度大大提高，但是速度略有下降；改进SURF_RGB-D_SLAM算法和原始SURF_RGB-D_SLAM算法相比精度大大提高，且速度也略有提高。改进的ORB_RGB-D_SLAM算法为高速度高精度算法；相比于原始的ORB_RGB-D_SLAM算法，改进的ORB_RGB-D_SLAM算法的精度有了非常大的提高，而速度稍有降低；另一方面，改进的ORB_RGB-D_SLAM算法的精度与改进SIFT_RGB-D_SLAM算法基本相同，而速度远远高于改进的改进SIFT_RGB-D_SLAM算法。

综上所述，提出了RGB-D SLAM算法改进方法能够解决原始算法低速度和低精度的问题，使得改进后的RGB-D SLAM算法能够同时满足高速度和高精度的要求。因此可以得出以下结论：本发明提出的RGB-D SLAM算法改进方法是正确的。

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

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

Claims (5)

1.一种基于RGB-D的SLAM算法的改进方法，分为前端和后端两部分，前端的任务为提取不同观察结果之间的空间关系，分为特征检测与描述符提取、特征匹配、运动变换估计和运动变换优化；后端的任务为使用非线性误差函数优化位姿图中相机的位姿信息；使用基于图的位姿优化方法，利用前端得到的6D运动变换关系初始化位姿图，然后进行闭环检测添加闭环约束条件，再利用非线性误差函数优化方法进行位姿图优化，最终得到全局最优相机位姿和相机运动轨迹，并进行三维环境重建；其特征在于，其中，特征检测与描述符提取、特征匹配、运动变换估计和运动变换优化的具体方法如下所述：

1)基于ORB的特征检测与描述符提取方法

ORB算法是一种快速特征检测与描述符提取算法，包含oFAST特征点检测和rBRIEF描述符提取两个步骤；该算法的具体内容如下：

(1)在oFAST特征点检测步骤中，首先对原图像构建尺度空间金字塔，然后使用FAST特征点检测方法检测出足够多的特征点并使用Harris角点评估方法对检测到的特征点进行筛选，再根据强度矩心求出特征点的方向；

(2)在rBRIEF描述符提取步骤中，首先对oFAST特征点生成BRIEF特征描述符，然后利用oFAST特征点的方向矢量对BRIEF特征描述符进行转向生成steered BRIEF特征描述符，再使用贪心学习算法从训练集中筛选出一组具有较大方差值、高度不相关且位均值位于0.5附近的二元点集，然后使用这组二元点集生成BRIEF特征描述符并进行转向，得到具有旋转不变性的rBRIEF特征描述子；

2)基于FLANN的增强特征匹配方法

基于FLANN的增强特征匹配方法是对查找帧和训练帧特征描述符进行双向的基于FLANN的KNN匹配，然后使用单应性矩阵变换对匹配结果进行优化；

3)改进RANSAC的运动变换估计方法

使用RANSAC的方法进行运动变换估计总体上是一个迭代的过程；在每一次迭代过程中从三维坐标匹配点对集合中随机选择20个三维坐标匹配点对，根据这些三维坐标匹配点对求出一个运动变换模型；对于三维坐标匹配点对集合中的每一个元素，使用所求出的运动变换模型将元素中的原坐标进行投影以得到投影坐标，计算目标坐标与投影坐标之间的误差，如果误差小于阈值，则该三维坐标匹配点对是局内点，将其加入局内点集合；否则认为它是局外点，不作处理；统计局内点的数目，并计算该模型下的平均误差；如果局内点数目高于阈值并且平均误差低于阈值，则利用所有的局内点重新计算新的运动变换模型；然后再次对新的运动变换模型进行评估；

(4)基于GICP的运动变换优化方法

GICP算法在对标准ICP算法和点到面ICP算法进行归纳与综合的基础上，更进一步使用了面到面的思想进行改进；GICP算法的抽象程度更高，而且在一定条件下退化成标准ICP算法或者点到面ICP算法；GICP算法点云配准所需的时间高于标准ICP算法，但是GICP的精度远远高于标准ICP，使用GICP算法替换标准ICP算法进行RGB-D SLAM算法中的运动变换优化，以使得求解出的运动变换误差更小。

2.根据权利要求1所述的基于RGB-D的SLAM算法的改进方法，其特征在于，所述oFAST特征点检测的具体步骤如下所述，FAST是一种非常快速的特征点检测算法，根据候选点与其圆形邻域内相邻像素点的灰度值差异来判断候选点是否为特征点，具体判断方法如式(1-1)所示：

$N={\mathrm{\&Sigma;}}_{x\&ForAll;\mathrm{circle}\left(p\right)}|I\left(x\right)-I\left(p\right)|>{\mathrm{\&epsiv;}}_d---(1-1)$

式中，p为候选点，circle(p)为p的圆形邻域，x为候选点圆形邻域内任意一像素点，I(p)为候选点的图像灰度值，I(x)为x处的图像灰度值；若N超出阈值条件ε_d，则该候选点为特征点；

在FAST特征点检测结束后，使用Harris角点评估方法对特征点进行筛选；如果想要得到的目标特征点数目为N，则首先设置一个足够低的阈值以获得超过N个特征点，然后使用Harris评估方法对这些特征点进行排序，然后选取前N个作为所得到的目标特征点；

为了使检测到的特征点满足旋转不变形，ORB算法中使用强度矩心的方法为检测到的特征点分配方向；该方法假设角点的强度为它与中心的偏移量，并且这个矢量能用于求解角点方向；局部图像的矩如式(1-2)所示：

m_pq＝Σ_x，yx^py^qI(x，y)         (1-2)

然后通过式(1-2)求出矩m₀₀、m₁₀和m₀₁后，根据式(1-3)求出矩心C：

$C=(\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}})---(1-3)$

然后构建一个从角点中心O到矩心C的向量的方向正是局部图像角点的方向，计算公式如式(1-4)所示：

θ＝atan2(m₀₁，m₁₀)          (1-4)

此外，为增强方法的旋转不变性，求取矩的时候，x和y应当始终在角点的r半径圆形邻域内，所以在ORB算法中显示地令r为局部图像的大小，即x和y的范围为[-r，r]；得到了带有方向的FAST特征点，它被称为oFAST特征点。

3.根据权利要求1所述的基于RGB-D的SLAM算法的改进方法，其特征在于，所述rBRIEF描述符提取的具体步骤如下所述：其中BRIEF特征描述符是一种快速计算的二进制编码形式描述符；它首先对图像进行高斯平滑滤波，然后在oFAST特征点的某邻域内随机选取点对(x，y)并对这些点的灰度值进行二值化测试，并将测试结果按照一定顺序生成一个二进制串，这个二进制串即为BRIEF特征描述符；二值化测试τ的定义如式(1-5)所示，其中，p(x)为p在点x处的强度；

$\mathrm{\&tau;}(p;x,y):=\left\{\begin{array}{cc}1& :p\left(x\right)<p\left(y\right)\\ 0& :p\left(x\right)\&GreaterEqual;p\left(y\right)\end{array}\right.---(1-5)$

BRIEF特征描述符即为n个二值化测试所构成的向量，如式(1-6)所示：

f_n(p)：＝Σ_1≤i≤n2^i-1τ(p，x_i，y_i)      (1-6)

在ORB算法中，点对(x，y)的选取符合高斯分布，其中S为特征点的邻域大小；

由于BRIEF不具有旋转不变性，所以ORB算法中利用特征点的方向矢量对BRIEF特征描述符进行转向，生成steered BRIEF特征描述符；

设原始的BRIEF选取的点集如式(1-7)所示：

$S=\left(\begin{array}{c}{x}_1,...,x_n\\ y_1,...,y_n\end{array}\right)---(1-7)$

使用特征点方向θ和相应的旋转矩阵R_θ构建旋转的点集S_θ，如式(1-8)所示：

S_θ＝R_θS             (1-8)

则生成的steered BRIEF特征描述符即如式(1-9)所示：

g_n(p，θ)：＝f_n(p)|(x_i，y_i)∈S_θ    (1-9)

贪心学习算法具体过程如下：首先对全部的训练局部图像执行所有可能的二值化测试，然后根据测试结果与均值0.5的距离对所有测试进行排序，并生成测试向量T，然后开始贪心搜索过程：首先从T中取出第一个测试放入结果向量R中，然后从T中取出下一个测试，并将它和R中的所有测试进行比较，并估算它们之间的相关性，如果相关性高于某个阈值，则丢弃它；否则将它加入R；重复上述步骤，直到R中的测试数目达到256个，然后结束；否则如果T中的所有测试都已经取出，而R中的测试数目仍然少于256，那么降低阈值，重新开始贪心搜索过程；通过上述步骤即得到具有旋转不变形和强抗噪声能力rBRIEF特征描述符。

4.根据权利要求1所述的基于RGB-D的SLAM算法的改进方法，其特征在于，所述基于FLANN的增强特征匹配方法，具体内容如下：FLANN是在高维空间执行快速近似最近邻搜索的一个库，它包含一组优秀的最近邻搜索算法和一个自动选择最优算法并且根据数据集对算法参数进行优化的系统；对于高维空间中的最近邻搜索问题，FLANN采用基于分层k-均值树的优先级搜索算法或采用多重随机k-d树时具有最好的性能；相对于其他传统最近邻搜索算法，基于FLANN的近似快速最近邻搜索的速度提高了一个数量级；其次，为了解决BruteForce匹配法为查找帧中所有特征点查找其在训练帧中距离最近的匹配特征点的问题，采用基于FLANN的KNN搜索方法进行查找，并对查找结果进行过滤；当给定向量空间M中的一个点集P＝{p₁，p₂，...，p_n}和一个待查询点q∈M，则q的最近邻元素NN(q，P)∈P定义为NN(q，P)＝argmin_x∈pd(q，x)，其中d(q，x)表示q与x的距离；KNN搜索就是同时查找K最近邻的搜索方法，K最近邻的定义如式(1-10)所示：

KNN(q，P，K)＝A          (1-10)

其中A是满足如式(1-11)所示约束的集合：

$\left|A\right|=K,A\&SubsetEqual;P,\&ForAll;x\&Element;A,y\&Element;P-A,(q,x)\&le;d(q,y)---(1-11)$

在使用KNN搜索方法时令k＝2，即对查找帧中的每一个特征点s_i，都在训练帧中使用KNN二近邻搜索方法查找它的最近邻点t_i1和次近邻点t_i2，s_i和它们的距离分别为d_i1和d_i2；比较d_i1和d_i2之间的距离，只有当d_i1远远小于d_i2时，则s_i和t_i是正确匹配点对；否则当d_i1和d_i2比较接近时，则s_i和t_i1、s_i和t_i2都不是正确匹配点对，应该将其剔除；或者认为s_i和t_i1、s_i和t_i2都能够正确匹配，但是为了减少这种匹配关系对后续运动变换的影响，也应该将其剔除；令在最终实现的系统中，当ratio_i小于0.6时保留对应的匹配点对，否则将其剔除；

为了消除单方向匹配造成的错误匹配点对，使用双向的基于FLANN的KNN搜索来查找正确的匹配点对；具体做法为，首先对于查找帧QueryFrame中的每一个特征点，使用基于FLANN的二近邻搜索在训练帧TrainFrame中查找匹配点对，并对最近邻和次近邻距离相近的匹配点对过滤，记录下最终得到的好的匹配点对集合S₁；然后反方向执行上述步骤，即为对于训练帧TrainFrame中的每一个特征点，使用基于FLANN的二近邻搜索在查找帧QueryFrame中查找匹配点对，并对最近邻和次近邻距离相近的匹配点对过滤，记录下最终得到的好的匹配点对集合S₂；对于任意pair(i，j)∈S₁，查找相应的pair(j，k)∈S₂，如果i＝k则认为查找帧QueryFrame中的特征点i与训练帧TrainFrame中的特征点j是双向匹配的，将该匹配点对加入集合S；否则认为查找帧QueryFrame中的特征点i与训练帧TrainFrame中的特征点j不匹配，将其丢弃；遍历集合S₁中的各个匹配点对，对其执行上述操作，得到的集合S即为满足双向匹配关系的匹配点对集合；

为了解决相同或者相似物体在环境中重复出现导致匹配结果错误的问题，使用单应性矩阵变换来进一步优化匹配结果；具体做法是，对于每一个pair(i，j)∈S，提取出i在查找帧QueryFrame中的坐标(x_i，y_i)并加入查找帧坐标集合QureyPosSets，提取出j在训练帧TrainFrame中的坐标(x_j，y_j)并加入训练帧坐标集合TrainPosSets，然后对这两个坐标集合使用RANSAC方法进行单应性矩阵变换求出最优单应性变换矩阵，然后剔除集合S中不满足单应性变换矩阵约束的匹配点对，得到的集合S即为具有高可信度的匹配点对。

5.根据权利要求1所述的基于RGB-D的SLAM算法的改进方法，其特征在于，基于GICP的运动变换优化方法：标准ICP算法首先计算两个点云中的匹配点，然后计算使这些匹配点距离最小的运动变换；

点到面ICP是标准ICP的一种改进，通过使用表面法线信息来提高点云配准的精度；该算法使用 $T\&LeftArrow;\underset{T}{\arg \min }\left\{{\mathrm{\&Sigma;}}_i{w}_i{\left|\right|{\mathrm{\&eta;}}_i\&CenterDot;(T\&CenterDot;b_i-m_i)\left|\right|}^2\right\}$ 来代替标准ICP算法的运动变换求解约束，其中η_i为点m_i的表面法线；

GICP在对标准ICP和点到面ICP进行综合的基础上，还在运动变换求解步骤中使用了概率模型进行最小化运算；假设在标准ICP算法中for循环部分计算两个点云中的匹配点的步骤完成后得到了两个点云A＝{a_i}_{i＝1，...，N}和B＝{b_i}_{i＝1，...，N}，且下标相同的A和B的元素相互对应，同时假设A和B集合中满足||m_i-T·b_i||＞d_max对应关系已经被剔除，则GICP算法的改进方法如下：

在概率模型中假设存在基础点集和且它们根据和生成集合A和B，其中和是待测量点的协方差矩阵；假设和完全对应，则存在运动变换T^*满足式(1-12)：

$\widehat{b_{\mathrm{\&iota;}}}=T^{*}\widehat{a_{\mathrm{\&iota;}}}$

对任意刚体变换T，定义d_i ^(T)＝b_i-Ta_i，则有式(1-13)成立：

$T=\underset{T}{\arg \min }{\mathrm{\&Sigma;}}_i{{d_i}^{\left(T\right)}}^T{(C_i^B+T{C}_i^A{T}^T)}^{-1}{d_i}^{\left(T\right)}$

而式(1-13)正是GICP对标准ICP运动变换求解步骤的具体改进方法；当且时，式(1-13)退化为标准ICP的运动变换求解约束；当且时，式(1-13)退化为而该式为点到面的ICP的运动变换求解约束的另一种表示形式，即GICP退化为点到面ICP；因此，上述改进方法被命名为GICP，即广义ICP。