精华内容
下载资源
问答
  • ORB

    千次阅读 2018-06-28 20:54:50
    1、算法介绍ORB(Oriented FAST and Rotated BRIEF)是一种快速特征点提取和描述的算法。这个算法是由Ethan Rublee, Vincent Rabaud, Kurt Konolige以及Gary R.Bradski在2011年一篇名为“ORB:An Efficient ...

    1、算法介绍

    ORB(Oriented FAST and Rotated BRIEF)是一种快速特征点提取和描述的算法。这个算法是由Ethan Rublee, Vincent Rabaud, Kurt Konolige以及Gary R.Bradski在2011年一篇名为“ORB:An Efficient Alternative to SIFTor SURF”的文章中提出。ORB算法分为两部分,分别是特征点提取和特征点描述。特征提取是由FAST(Features from  Accelerated Segment Test)算法发展来的,特征点描述是根据BRIEF(Binary Robust IndependentElementary Features)特征描述算法改进的。ORB特征是将FAST特征点的检测方法与BRIEF特征描述子结合起来,并在它们原来的基础上做了改进与优化。据说,ORB算法的速度是sift的100倍,是surf的10倍。

    1.1 Fast特征提取

    ORB算法的特征提取是由FAST算法改进的,这里成为oFAST(FASTKeypoint Orientation)。也就是说,在使用FAST提取出特征点之后,给其定义一个特征点方向,以此来实现特征点的旋转不变形。FAST算法是公认的最快的特征点提取方法。FAST算法提取的特征点非常接近角点类型。oFAST算法如下:


    图1  FAST特征点判断示意图

    步骤一:粗提取。该步能够提取大量的特征点,但是有很大一部分的特征点的质量不高。下面介绍提取方法。从图像中选取一点P,如上图1。我们判断该点是不是特征点的方法是,以P为圆心画一个半径为3pixel的圆。圆周上如果有连续n个像素点的灰度值比P点的灰度值大或者小,则认为P为特征点。一般n设置为12。为了加快特征点的提取,快速排出非特征点,首先检测1、9、5、13位置上的灰度值,如果P是特征点,那么这四个位置上有3个或3个以上的的像素值都大于或者小于P点的灰度值。如果不满足,则直接排出此点。

    步骤二:机器学习的方法筛选最优特征点。简单来说就是使用ID3算法训练一个决策树,将特征点圆周上的16个像素输入决策树中,以此来筛选出最优的FAST特征点。

    步骤三:非极大值抑制去除局部较密集特征点。使用非极大值抑制算法去除临近位置多个特征点的问题。为每一个特征点计算出其响应大小。计算方式是特征点P和其周围16个特征点偏差的绝对值和。在比较临近的特征点中,保留响应值较大的特征点,删除其余的特征点。

    步骤四:特征点的尺度不变形。建立金字塔,来实现特征点的多尺度不变性。设置一个比例因子scaleFactor(opencv默认为1.2)和金字塔的层数nlevels(pencv默认为8)。将原图像按比例因子缩小成nlevels幅图像。缩放后的图像为:I’= I/scaleFactork(k=1,2,…, nlevels)。nlevels幅不同比例的图像提取特征点总和作为这幅图像的oFAST特征点。

    步骤五:特征点的旋转不变性。ORB算法提出使用矩(moment)法来确定FAST特征点的方向。也就是说通过矩来计算特征点以r为半径范围内的质心,特征点坐标到质心形成一个向量作为该特征点的方向。矩定义如下:


    其中,I(x,y)为图像灰度表达式。该矩的质心为:


    假设角点坐标为O,则向量的角度即为该特征点的方向。计算公式如下:


    1.2 rBRIEF特征描述

    rBRIEF特征描述是在BRIEF特征描述的基础上加入旋转因子改进的。下面先介绍BRIEF特征提取方法,然后说一说是怎么在此基础上修改的。

    BRIEF算法描述

    BRIEF算法计算出来的是一个二进制串的特征描述符。它是在一个特征点的邻域内,选择n对像素点pi、qi(i=1,2,…,n)。然后比较每个点对的灰度值的大小。如果I(pi)> I(qi),则生成二进制串中的1,否则为0。所有的点对都进行比较,则生成长度为n的二进制串。一般n取128、256或512,opencv默认为256。另外,值得注意的是为了增加特征描述符的抗噪性,算法首先需要对图像进行高斯平滑处理。在ORB算法中,在这个地方进行了改进,在使用高斯函数进行平滑后,又用了其他操作,使其更加的具有抗噪性。具体方法下面将会描述。

    关于在特征点SxS的区域内选取点对的方法,BRIEF论文(附件2)中测试了5种方法:

    1)在图像块内平均采样;

    2)p和q都符合(0,S2/25)的高斯分布;

    3)p符合(0,S2/25)的高斯分布,而q符合(0,S2/100)的高斯分布;

    4)在空间量化极坐标下的离散位置随机采样;

    5)把p固定为(0,0),q在周围平均采样。

    五种采样方法的示意图如下:


    论文指出,第二种方法可以取得较好的匹配结果。在旋转不是非常厉害的图像里,用BRIEF生成的描述子的匹配质量非常高,作者测试的大多数情况中都超越了SURF。但在旋转大于30°后,BRIEF的匹配率快速降到0左右。BRIEF的耗时非常短,在相同情形下计算512个特征点的描述子时,SURF耗时335ms,BRIEF仅8.18ms;匹配SURF描述子需28.3ms,BRIEF仅需2.19ms。在要求不太高的情形下,BRIEF描述子更容易做到实时。

    改进BRIEF算法—rBRIEF(Rotation-AwareBrief)

    (1)steered BRIEF(旋转不变性改进)

    在使用oFast算法计算出的特征点中包括了特征点的方向角度。假设原始的BRIEF算法在特征点SxS(一般S取31)邻域内选取n对点集。


    经过旋转角度θ旋转,得到新的点对


    在新的点集位置上比较点对的大小形成二进制串的描述符。这里需要注意的是,在使用oFast算法是在不同的尺度上提取的特征点。因此,在使用BRIEF特征描述时,要将图像转换到相应的尺度图像上,然后在尺度图像上的特征点处取SxS邻域,然后选择点对并旋转,得到二进制串描述符。

    (2)rBRIEF-改进特征点描述子的相关性

    使用steeredBRIEF方法得到的特征描述子具有旋转不变性,但是却在另外一个性质上不如原始的BRIEF算法。是什么性质呢,是描述符的可区分性,或者说是相关性。这个性质对特征匹配的好坏影响非常大。描述子是特征点性质的描述。描述子表达了特征点不同于其他特征点的区别。我们计算的描述子要尽量的表达特征点的独特性。如果不同特征点的描述子的可区分性比较差,匹配时不容易找到对应的匹配点,引起误匹配。ORB论文中,作者用不同的方法对100k个特征点计算二进制描述符,对这些描述符进行统计,如下表所示:


    图2 特征描述子的均值分布.X轴代表距离均值0.5的距离y轴是相应均值下的特征点数量统计

    我们先不看rBRIEF的分布。对BRIEF和steeredBRIEF两种算法的比较可知,BRIEF算法落在0上的特征点数较多,因此BRIEF算法计算的描述符的均值在0.5左右,每个描述符的方差较大,可区分性较强。而steeredBRIEF失去了这个特性。至于为什么均值在0.5左右,方差较大,可区分性较强的原因,这里大概分析一下。这里的描述子是二进制串,里面的数值不是0就是1,如果二进制串的均值在0.5左右的话,那么这个串有大约相同数目的0和1,那么方差就较大了。用统计的观点来分析二进制串的区分性,如果两个二进制串的均值都比0.5大很多,那么说明这两个二进制串中都有较多的1时,在这两个串的相同位置同时出现1的概率就会很高。那么这两个特征点的描述子就有很大的相似性。这就增大了描述符之间的相关性,减小之案件的可区分性。

    下面我们介绍解决上面这个问题的方法:rBRIEF。

    原始的BRIEF算法有5中去点对的方法,原文作者使用了方法2。为了解决描述子的可区分性和相关性的问题,ORB论文中没有使用5种方法中的任意一种,而是使用统计学习的方法来重新选择点对集合。

    首先建立300k个特征点测试集。对于测试集中的每个点,考虑其31x31邻域。这里不同于原始BRIEF算法的地方是,这里在对图像进行高斯平滑之后,使用邻域中的某个点的5x5邻域灰度平均值来代替某个点对的值,进而比较点对的大小。这样特征值更加具备抗噪性。另外可以使用积分图像加快求取5x5邻域灰度平均值的速度。

    从上面可知,在31x31的邻域内共有(31-5+1)x(31-5+1)=729个这样的子窗口,那么取点对的方法共有M=265356种,我们就要在这M种方法中选取256种取法,选择的原则是这256种取法之间的相关性最小。怎么选取呢?

    1)在300k特征点的每个31x31邻域内按M种方法取点对,比较点对大小,形成一个300kxM的二进制矩阵Q。矩阵的每一列代表300k个点按某种取法得到的二进制数。

    2)对Q矩阵的每一列求取平均值,按照平均值到0.5的距离大小重新对Q矩阵的列向量排序,形成矩阵T。

    3)将T的第一列向量放到R中。

    4)取T的下一列向量和R中的所有列向量计算相关性,如果相关系数小于设定的阈值,则将T中的该列向量移至R中。

    5)按照4)的方式不断进行操作,直到R中的向量数量为256。

    通过这种方法就选取了这256种取点对的方法。这就是rBRIEF算法。

     

    2、ORB特征提取实验

    实验代码用opencv中的

    2.1ORB特征提取和匹配实验


    (1)


    (2)


    (3)


    (4)

    2-1 ORB特征匹配

    从上图(1)(2)(3)可以看出,ORB算法的特征匹配效果比较理想,并且具有较稳定的旋转不变性。但是通过(4)看出,ORB算法在尺度方面效果较差,在增加算法的尺度变换的情况下仍然没有取得较好的结果。

    ORB是一种快速的特征提取和匹配的算法。它的速度非常快,但是相应的算法的质量较差。和sift相比,ORB使用二进制串作为特征描述,这就造成了高的误匹配率。

    展开全文
  • ORB特征提取详解

    万次阅读 多人点赞 2016-09-22 18:46:48
    ORB(Oriented FAST and Rotated BRIEF)是一种快速特征点提取和描述的算法。这个算法是由Ethan Rublee, Vincent Rabaud, Kurt Konolige以及Gary R.Bradski在2011年一篇名为“ORB:An Efficient Alternative to SIFT...

    网上虽然出现了很多讲解ORB特征提取和描述的方法,但都不够详尽。为了搞明白到底是怎么回事,只能结合别人的博客和原著对ORB的详细原理做一个研究和学习。哪里有不对的地方,请多多指教

     

    1、算法介绍

    ORB(Oriented FAST and Rotated BRIEF)是一种快速特征点提取和描述的算法。这个算法是由Ethan Rublee, Vincent Rabaud, Kurt Konolige以及Gary R.Bradski在2011年一篇名为“ORB:An Efficient Alternative to SIFTor SURF”的文章中提出。ORB算法分为两部分,分别是特征点提取和特征点描述。特征提取是由FAST(Features from  Accelerated Segment Test)算法发展来的,特征点描述是根据BRIEF(Binary Robust IndependentElementary Features)特征描述算法改进的。ORB特征是将FAST特征点的检测方法与BRIEF特征描述子结合起来,并在它们原来的基础上做了改进与优化。据说,ORB算法的速度是sift的100倍,是surf的10倍。

    1.1 Fast特征提取

    ORB算法的特征提取是由FAST算法改进的,这里成为oFAST(FASTKeypoint Orientation)。也就是说,在使用FAST提取出特征点之后,给其定义一个特征点方向,以此来实现特征点的旋转不变形。FAST算法是公认的最快的特征点提取方法。FAST算法提取的特征点非常接近角点类型。oFAST算法如下:

     

    图1  FAST特征点判断示意图

    步骤一:粗提取。该步能够提取大量的特征点,但是有很大一部分的特征点的质量不高。下面介绍提取方法。从图像中选取一点P,如上图1。我们判断该点是不是特征点的方法是,以P为圆心画一个半径为3pixel的圆。圆周上如果有连续n个像素点的灰度值比P点的灰度值大或者小,则认为P为特征点。一般n设置为12。为了加快特征点的提取,快速排出非特征点,首先检测1、9、5、13位置上的灰度值,如果P是特征点,那么这四个位置上有3个或3个以上的的像素值都大于或者小于P点的灰度值。如果不满足,则直接排出此点。

    步骤二:机器学习的方法筛选最优特征点。简单来说就是使用ID3算法训练一个决策树,将特征点圆周上的16个像素输入决策树中,以此来筛选出最优的FAST特征点。

    步骤三:非极大值抑制去除局部较密集特征点。使用非极大值抑制算法去除临近位置多个特征点的问题。为每一个特征点计算出其响应大小。计算方式是特征点P和其周围16个特征点偏差的绝对值和。在比较临近的特征点中,保留响应值较大的特征点,删除其余的特征点。

    步骤四:特征点的尺度不变形。建立金字塔,来实现特征点的多尺度不变性。设置一个比例因子scaleFactor(opencv默认为1.2)和金字塔的层数nlevels(pencv默认为8)。将原图像按比例因子缩小成nlevels幅图像。缩放后的图像为:I’= I/scaleFactork(k=1,2,…, nlevels)。nlevels幅不同比例的图像提取特征点总和作为这幅图像的oFAST特征点。

    步骤五:特征点的旋转不变性。ORB算法提出使用矩(moment)法来确定FAST特征点的方向。也就是说通过矩来计算特征点以r为半径范围内的质心,特征点坐标到质心形成一个向量作为该特征点的方向。矩定义如下:

     

    其中,I(x,y)为图像灰度表达式。该矩的质心为:

     

    假设角点坐标为O,则向量的角度即为该特征点的方向。计算公式如下:

     

    1.2 rBRIEF特征描述

    rBRIEF特征描述是在BRIEF特征描述的基础上加入旋转因子改进的。下面先介绍BRIEF特征提取方法,然后说一说是怎么在此基础上修改的。

    BRIEF算法描述

    BRIEF算法计算出来的是一个二进制串的特征描述符。它是在一个特征点的邻域内,选择n对像素点pi、qi(i=1,2,…,n)。然后比较每个点对的灰度值的大小。如果I(pi)> I(qi),则生成二进制串中的1,否则为0。所有的点对都进行比较,则生成长度为n的二进制串。一般n取128、256或512,opencv默认为256。另外,值得注意的是为了增加特征描述符的抗噪性,算法首先需要对图像进行高斯平滑处理。在ORB算法中,在这个地方进行了改进,在使用高斯函数进行平滑后,又用了其他操作,使其更加的具有抗噪性。具体方法下面将会描述。

    关于在特征点SxS的区域内选取点对的方法,BRIEF论文(附件2)中测试了5种方法:

    1)在图像块内平均采样;

    2)pq都符合(0,S2/25)的高斯分布;

    3)p符合(0,S2/25)的高斯分布,而q符合(0,S2/100)的高斯分布;

    4)在空间量化极坐标下的离散位置随机采样;

    5)把p固定为(0,0),q在周围平均采样。

    五种采样方法的示意图如下:

    论文指出,第二种方法可以取得较好的匹配结果。在旋转不是非常厉害的图像里,用BRIEF生成的描述子的匹配质量非常高,作者测试的大多数情况中都超越了SURF。但在旋转大于30°后,BRIEF的匹配率快速降到0左右。BRIEF的耗时非常短,在相同情形下计算512个特征点的描述子时,SURF耗时335ms,BRIEF仅8.18ms;匹配SURF描述子需28.3ms,BRIEF仅需2.19ms。在要求不太高的情形下,BRIEF描述子更容易做到实时。

    改进BRIEF算法—rBRIEF(Rotation-AwareBrief)

    (1)steered BRIEF(旋转不变性改进)

    在使用oFast算法计算出的特征点中包括了特征点的方向角度。假设原始的BRIEF算法在特征点SxS(一般S取31)邻域内选取n对点集。

     

    经过旋转角度θ旋转,得到新的点对

     

    在新的点集位置上比较点对的大小形成二进制串的描述符。这里需要注意的是,在使用oFast算法是在不同的尺度上提取的特征点。因此,在使用BRIEF特征描述时,要将图像转换到相应的尺度图像上,然后在尺度图像上的特征点处取SxS邻域,然后选择点对并旋转,得到二进制串描述符。

    (2)rBRIEF-改进特征点描述子的相关性

    使用steeredBRIEF方法得到的特征描述子具有旋转不变性,但是却在另外一个性质上不如原始的BRIEF算法。是什么性质呢,是描述符的可区分性,或者说是相关性。这个性质对特征匹配的好坏影响非常大。描述子是特征点性质的描述。描述子表达了特征点不同于其他特征点的区别。我们计算的描述子要尽量的表达特征点的独特性。如果不同特征点的描述子的可区分性比较差,匹配时不容易找到对应的匹配点,引起误匹配。ORB论文中,作者用不同的方法对100k个特征点计算二进制描述符,对这些描述符进行统计,如下表所示:

    图2 特征描述子的均值分布.X轴代表距离均值0.5的距离y轴是相应均值下的特征点数量统计

    我们先不看rBRIEF的分布。对BRIEF和steeredBRIEF两种算法的比较可知,BRIEF算法落在0上的特征点数较多,因此BRIEF算法计算的描述符的均值在0.5左右,每个描述符的方差较大,可区分性较强。而steeredBRIEF失去了这个特性。至于为什么均值在0.5左右,方差较大,可区分性较强的原因,这里大概分析一下。这里的描述子是二进制串,里面的数值不是0就是1,如果二进制串的均值在0.5左右的话,那么这个串有大约相同数目的0和1,那么方差就较大了。用统计的观点来分析二进制串的区分性,如果两个二进制串的均值都比0.5大很多,那么说明这两个二进制串中都有较多的1时,在这两个串的相同位置同时出现1的概率就会很高。那么这两个特征点的描述子就有很大的相似性。这就增大了描述符之间的相关性,减小之案件的可区分性。

    下面我们介绍解决上面这个问题的方法:rBRIEF。

    原始的BRIEF算法有5中去点对的方法,原文作者使用了方法2。为了解决描述子的可区分性和相关性的问题,ORB论文中没有使用5种方法中的任意一种,而是使用统计学习的方法来重新选择点对集合。

    首先建立300k个特征点测试集。对于测试集中的每个点,考虑其31x31邻域。这里不同于原始BRIEF算法的地方是,这里在对图像进行高斯平滑之后,使用邻域中的某个点的5x5邻域灰度平均值来代替某个点对的值,进而比较点对的大小。这样特征值更加具备抗噪性。另外可以使用积分图像加快求取5x5邻域灰度平均值的速度。

    从上面可知,在31x31的邻域内共有(31-5+1)x(31-5+1)=729个这样的子窗口,那么取点对的方法共有M=265356种,我们就要在这M种方法中选取256种取法,选择的原则是这256种取法之间的相关性最小。怎么选取呢?

    1)在300k特征点的每个31x31邻域内按M种方法取点对,比较点对大小,形成一个300kxM的二进制矩阵Q。矩阵的每一列代表300k个点按某种取法得到的二进制数。

    2)对Q矩阵的每一列求取平均值,按照平均值到0.5的距离大小重新对Q矩阵的列向量排序,形成矩阵T。

    3)将T的第一列向量放到R中。

    4)取T的下一列向量和R中的所有列向量计算相关性,如果相关系数小于设定的阈值,则将T中的该列向量移至R中。

    5)按照4)的方式不断进行操作,直到R中的向量数量为256。

    通过这种方法就选取了这256种取点对的方法。这就是rBRIEF算法。

     

    2、ORB特征提取实验

    实验代码用opencv中的

    2.1ORB特征提取和匹配实验

     

    (1)

     

    (2)

    (3)

     

    (4)

    2-1 ORB特征匹配

    从上图(1)(2)(3)可以看出,ORB算法的特征匹配效果比较理想,并且具有较稳定的旋转不变性。但是通过(4)看出,ORB算法在尺度方面效果较差,在增加算法的尺度变换的情况下仍然没有取得较好的结果。

    ORB是一种快速的特征提取和匹配的算法。它的速度非常快,但是相应的算法的质量较差。和sift相比,ORB使用二进制串作为特征描述,这就造成了高的误匹配率。

     

    展开全文
  • 适用于单目、双目、rgb-d摄像机视觉slam代码orb-slam2
  • ORB-SLAM:一种通用的(全能的)精确的单目SLAM系统 # 摘要 本文提出了ORB-SLAM,在大小场景、室内室外环境下都可以实时操作的一种基于特征的单目SLAM系统。系统对复杂的剧烈运动具有鲁棒性,允许宽基线的闭环和重...
    ORB-SLAM:一种通用的(全能的)精确的单目SLAM系统

    摘要

    本文提出了ORB-SLAM,在大小场景、室内室外环境下都可以实时操作的一种基于特征的单目SLAM系统。系统对复杂的剧烈运动具有鲁棒性,允许宽基线的闭环和重定位,且包含完整的自动初始化。基于最近几年的优秀算法之上,我们从头开始设计了一种新颖的系统,它对所有SLAM任务使用相同的特征:追踪、建图、重定位和闭环。合适策略的存在使得选择的重建点和关键帧具有很好的鲁棒性,并能够生成紧凑的可追踪的地图,只有当场景内容发生变化地图才改变,从而允许长时间操作。本文从最受欢迎的数据集中提供了27个序列的详尽评估。相对于其他最先进的单目SLAM方法,ORB-SLAM实现了前所未有的性能。为了社会的利益,我们将源代码公开。

    关键字:持续建图,定位,单目视觉,识别,同时定位和制图(SLAM)。

    引言

    BA提供相机定位的精确估计以及稀疏几何重建[1,2],并且提供了强烈的匹配网络和良好的初始猜测。一段长的时间,这种方法被认为不符合实时性的应用,如视觉VSLAM。VSLAM系统在构建环境的同时需要估计相机的轨迹。现在,我们为了不以过高的计算成本获得准确的结果,实时SLAM算法必须向BA提供以下信息。

    • 在候选图像帧子集中(关键帧)匹配观测的场景特征(地图云点)
    • 由于关键帧数量的增长,需要做筛选避免冗余
    • 关键帧和云点的网络配置可以产生精确的结果,也就是,分布良好的关键帧集合和有明显视差、大量回环匹配的观测云点
    • 关键帧和云点位置的初始估计,采用非线性优化的方法
    • 在构建局部地图的过程中,优化的关键是获得良好的稳定性
    • 本系统可以实时执行快速全局优化(比如位姿图)以实现闭环回路

    BA第一次实时应用是在Mouragon等人[13]提出的视觉里程计算法中,其次是在Klein和Murray的突破性工作PTAM[4]算法中。尽管受制于小场景的应用,PTAM算法对关键帧的选择,特征匹配,点的三角化,相机位姿估计,追踪失败后的重定位非常有效。然而,由于缺少闭环检测和对遮挡场景的处理,再加上其视图不变性差,在地图初始化时需要人工干预等多个因素,使得PTAM算法的应用收到了严重的限制。

    在本文中,我们基于PTAM算法的主要框架,采用Gálvez-López和Tardós提出的place recognition(场景/位置识别)算法,Strasdat等人提出的scale-aware loop closing(具备尺度感知的闭环检测)算法以及文献[7][8]中的大尺度操作中Covisibility信息的使用,重新设计了一种新的单目SLAM系统ORB-SLAM,本文的贡献主要包括:

    • 对所有的任务采用相同的特征:追踪、地图构建、重定位和闭环检测。这使得我们的系统更有效率、简单可靠。采用的ORB特征[9]在没有GPU的情况下也有很好的实时性,且具有旋转不变性和光照不变性。
    • 算法支持在宽阔环境中实时操作。由于covisibility graph的使用,特征点的跟踪与构图主要集中在局部共视区域,而与全局地图的大小无关。
    • 使用Essential Graph来优化位姿实现回环检测。构建生成树,并由系统、闭环检测链接和covisibility graph的强边缘进行维护。
    • 算法的实时相机重定位具有明显的旋转不变特性和光照不变性。这就使得点跟踪丢失后可以恢复,增强了地图的重用性。
    • 提出了一种合适的方法来选择地图点云和关键帧,通过严格删选关键帧和地图点,剔除冗余信息,使得特征点的跟踪具备了更好的稳定性,从而增强算法的可持续操作性。好的挑选方法可以增强追踪的鲁棒性,同时舍弃多余的关键帧加强系统长时间操作性

    我们在公共数据集上对算法的性能在室内和室外环境下进行了评估,包括手持设备、汽车和机器人。值得一提的是,与目前最新的直接SLAM算法[10]相比——直接SLAM方法通过直接对像素点的灰度进行优化而不是最小化特征重投影误差,我们的方法能够实现更精确的摄像头定位精度。我们在文章的第IX-B部分还讨论了基于特征的SLAM方法定位比直接法更准确的原因。

    闭环检测和重定位的方法是基于我们之前的论文[11]。系统最初的版本是论文[12]。本文中我们添加了初始化的方法,Essential graph ,并完善其他模块。我们详细了描述了系统的各个板块,并且开展了实验验证。
    据我们所知,这是目前最完整最可靠的单目SLAM系统,为了使更多人获益,我们将源代码开放。视频演示和源代码放在我们的项目网站上。

    相关工作

    A、位置识别

    Williams等人在综述[13]中比较了几种基于景象的位置识别方法,即图像到图像的匹配,这种方法在大环境下比地图到地图或图像到地图的方法更准确。在景象匹配方法中,bags of words(词袋)[14]的使用以其效率很高而脱颖而出,比如概率方法FAB-MAP[15]算法,。DBoW2方法[5]则首次使用了BRIEF描述子[16]生成的二进制词袋和非常高效的FAST特征检测算法[17]与SURF和SIFT相比,FAST算法的运时间减小了至少一个数量级。然而,尽管系统运行效率高、鲁棒性好,采用BRIEF描述子不具有旋转不变性和尺度不变性,系统只能运行在同一平面内(否则会造成尺度变化) ,闭环检测也只能从相似的视角中进行。在我们之前的工作[11]中,我们提出了一个使用ORB特征检测子的DBoW2位置识别器。ORB特征是具有旋转不变和尺度不变特性的二进制特征,因此,用它生成的快速识别器具有较好的视角不变性。我们在4组不同的数据集上演示了位置识别功能,从10K图像数据库中提取一个候选闭合回路的运算时间少于39毫秒。在本文的工作中,我们提出了一种改进版本的位置识别方法,采用covisibility信息,在检索数据库时返回几个假设情况而不是最好的匹配。

    B、地图初始化

    单目SLAM系统需要设计专门的策略来生成初始化地图,因为单幅图像不具备深度信息。解决这个问题的一种方法是一开始跟踪一个已知结构的对象,正如文献[20]。另一个方法是用一个具有高不确定度的逆深度参数[21]来初始化点的深度信息,理想情况下,该参数会在后期逐渐收敛到真值。最近Engel提出的半稠密方法[10]中就采用类似的方法将像素的深度信息初始化为一个随机数值。

    如果是从两个视角来初始化特征,就可以采用以下方法:一种是假设局部场景在同一平面内[4],[22],然后利用Faugeras等人论文[23]中的单应性来重构摄像头相对位姿。第二种是将场景建模为通用情况(不一定为平面),通过Nister提出的五点算法[26]来计算本征矩阵[24],[25],但该方法存在多解的问题。这两种摄像头位姿重构方法在低视差下都没有很好的约束,如果平面场景内的所有点都靠近摄像机的中心,则结果会出现双重歧义[27]。另一方面,非平面场景可以通过线性8点算法[2]来计算基础矩阵,相机的相对位姿就可以无歧义的重构出来。

    针对这一问题,我们在本文的第四部分提出了一个新的基于模型选择的自动初始化方法,对平面场景算法选择单应性矩阵,而对于非平面场景,算法选择基础矩阵。模型选择的综述方法可参见Torr等人的论文[28]。基于类似的理论,我们设计了一种启发式初始化算法,算法考虑到在接近退化情况(比如:平面,近平面,或是低视差)下选择基础矩阵进行位姿估计可能存在的问题,则选择单应性计算。在平面的情况下,为了保险起见,如果最终存在双重歧义,则算法避免进行初始化,因为可能会因为错误选择而导致算法崩溃。因此,我们会延迟初始化过程,直到所选的模型在明显的视差下产生唯一的解。

    C、单目SLAM

    单目SLAM最初采用滤波框架[20],[21],[29],[30]来建模。在该类方法中,每一帧都通过滤波器联合估计地图特征位置和相机位姿。这样做带来的问题是在处理连续帧图像上对计算资源的浪费和线性误差的累积。而另外一种SLAM框架是基于关键帧的,即采用少数筛选过的图像(关键帧)来构建地图,因为构图不再与帧率相关联,因此基于关键帧的SLAM方法不但节省了计算资源,还可以进行高精度的BA优化。Strasdar等人在论文[31]中证明了基于关键帧的单目SLAM方法比滤波器方法在相同的运算代价上定位结果更精确。

    基于关键帧的SLAM系统最具代表性可能是由Klein和Murray等人提出的PTAM算法[4]。它第一次将相机追踪和地图构建拆分成两个并行的线程运行,并成功用于小环境的实时增强现实中。后来文献[32]引入边缘特征对PTAM算法进行了改进,在跟踪过程中增加了旋转估计步骤,实现了更好的重定位效果。由于PTAM中的地图云点通过图像区块与FAST角点匹配,因此仅适合于特征跟踪并不适合用于后期的位置识别。而实际上,PTAM算法并没有进行大闭环检测,其重定位也仅是基于关键帧低分辨率缩略图的相关性进行的,因此视角不变性较差。

    Strasdat等人在文献[6]中提出了一个基于GPU实现的大尺度单目SLAM系统,该系统前端采用光流算法,其次用FAST特征匹配和运动BA;后端是基于滑动窗口的BA。闭环检测通过具有相似性约束(7自由度)的位姿图优化来进行,该方法可以矫正在单目SLAM系统中出现的尺度偏移问题。在本文中,我们也将采用这种7自由度的位姿图优化方法,并将其应用到我们的Essential Graph中,更多细节将在第三部分D节里面描述。

    Strasdat等人在文献[7]中采用了PTAM的前端,但其跟踪部分仅在一个从covisibility graph提取的局部图中进行。他们提出了一个双窗口优化后端,在内部窗口中连续进行BA,在有限大小的外部窗口中构建位姿图。然而, 只有当外部窗口尺寸足够大到可以包含整个闭环回路的情况下,闭环检测才能起作用。在我们的算法中,我们利用了Strasdat等人提出的基于covisibility的局部地图的优势,并且通过covisibility map来构建位姿图,同时重新设计前端和后端。另一个区别是,我们并没有用特别的特征提取方法做闭合回路检测(比如SURF方法),而是基于相同的追踪和建图的特征进行位置识别,获得具有鲁棒性的重定位和闭环检测。

    在Pirker等人的论文[33]中作者提出了CD-SLAM方法,一个非常复杂的系统,包括闭环检测,重定位,大尺度操作以及对算法在动态环境运行所做的改进。但文中并没有提及地图初始化。因此不利于后期读者对算法的复现,也致使我们没法对其进行精确性、鲁棒性和大场景下的测试对比。

    Song等人在论文[34]提出的视觉里程计方法中使用了ORB特征做追踪和处理BA后端滑动窗口。相比之下,我们的方法更加全面,因为他们的算法中没有涉及全局重定位,闭环回路检测,而且地图也不能重用。他们也使用了相机到地面的真实距离来限制单目SLAM算法的尺度漂移。Lim等人在我们提交本文最初的版本[12]之后发表了论文[25],他们也采用相同的特征进行跟踪,地图构建和闭环检测。但是,由于Lim等人的算法选择的BRIEF描述子不具备尺度不变性,因此其算法运行受限在平面轨迹上。他们的算法仅从上一帧关键帧开始跟踪特征点,因此访问过的地图不能重用,这样的处理方式与视觉里程计很像,存在系统无限增长的问题。我们在第三部分E小节里面与该算法进行了定性比较。

    Engel等人在最近的论文[10]里提出了LSD-SLAM算法,其可以构建大场景的半稠密地图。算法并没有采用特征提取和BA方法,而是选择直接法(优化也是直接通过图像像素灰度进行)。算法的结果让人印象深刻,其在没有GPU加速的情况下实时构建了一个半稠密地图,相比基于特征的稀疏地图SLAM系统而言,LSD-SLAM方法在机器人领域有更大的应用潜力。然而,该算法的运行仍然需要基于特征做闭环检测,且相机定位的精度也明显低于PTAM和我们的算法,相关实验结果我们将在第8部分的B小节中展示,对该结果的讨论在文章IX部分B小节进行。

    Forster等人在论文[22]中提出了介于直接方式和基于特征的方法之间的半直接视觉里程计算法SVO方法。该方法不需要对每帧图像都提取特征点,且可以以很高的帧率运行,在四轴飞行器上取得了令人惊叹的定位效果。然而,SVO算法没有进行闭环检测,且目前主要基于下视摄像头运行。

    最后,我们想讨论一下目前关键帧的选择方法。由于所有的视觉SLAM算法选择所有的云点和图像帧运行BA是不可行的。因此,在论文[31]中,Strasdat等人证明最合理的选择是保留尽可能多地点云和非冗余关键帧。PTAM方法非常谨慎插入关键帧避免运算量增长过大。然而,这种严格限制关键帧插入的策略在算法运行困难的情况下可能会导致追踪失败。在本文中,为了达到更好的稳定性,我们选择一种更为合适的关键帧插入策略,当算法运行困难的时候算法选择尽快的插入关键帧,然后在后期将冗余的关键帧删除以避免额外的计算成本。

    系统架构

    A、特征选择

    我们系统设计的中心思想是对SLAM系统的构图、跟踪、重定位以及闭环检测等模块都采用相同的特征,这将使得我们的系统更有效率,避免了像以往文章[6],[7]一样还需要额外插入一些额外的识别性强的特征以用于后期的闭环检测。我们每张图像的特征提取远少于33毫秒,远小于目前的SIFT算法(300ms),SURF算法(300ms),或最近提出的A-KAZE(~100ms)算法。为了使算法的位置识别能力能更加通用化,我们需要提取的特征具备旋转不变性,而BRIEF和LDB不具备这样的特性。

    为此,我们选择了ORB[9]特征,其是具有256位描述符的带方向的多尺度FAST角点。他们计算和匹配的速度非常快,同时对视角具有旋转不变的特性。这样可以在更宽的基准线上匹配他们,增强了BA的精度。我们已经在论文[11]中演示了基于ORB特征的位置识别性能。需要申明的是,虽然本文的方案中采用ORB算法,但所提出的技术并不仅限于该特征。

    B、三个线程:追踪、局部地图构建和闭环检测

    在这里插入图片描述>图1 ORB-SLAM系统框架,图中显示了算法的三个线程——跟踪、局部构图与闭环检测的所有步骤。另外还有场景识别和地图的主要组成部分。

    我们的系统框架如图1所示,包括三个并行的线程:跟踪、局部地图构建和闭环回路检测。跟踪线程负责对每帧图像的相机位置进行定位,并决定什么时候插入新的关键帧。我们首先通过与前一帧图像匹配得到初始特征点,然后采用运动BA优化摄像头位姿。如果特征跟丢(比如由于遮挡或是突然运动),则由位置识别模块进行全局重定位。一旦获得最初的相机位姿估计和特征匹配,则使用由系统维护的关键帧的covisibility graph提取一个局部可视化地图,如图2(a),图2(b)所示。然后通过重投影方法搜索当前帧与局部地图点对应的匹配点,并利用所有的匹配点优化当前相机位姿。最后,跟踪线程决定是否插入新的关键帧。所有的跟踪步骤将在第5部分详细阐述。创建初始化地图的新方法将在第4部分进行说明。

    局部地图构建模块负责处理新的关键帧,对周围的相机位姿进行局部BA以优化重构。在covisibility graph已连接的关键帧中搜索新的关键帧中ORB特征的匹配点,然后三角化新的地图点。有时尽管已经创建了新的点云,但基于跟踪线程过程中新收集的信息,为了保证点云的高质量,可能会根据点云筛选策略临时删除一些点。局部地图构建模块也负责删除冗余的关键帧。我们将在第6章详细说明局部地图构建的步骤。

    对每个新的关键帧都要进行闭环搜索,以确认是否形成闭环。如果闭环被侦测到,我们就计算相似变换来查看闭环的累积误差。这样闭环的两端就可以对齐,重复的云点就可以被融合。最后,为了确保全局地图的一致性,利用相似性约束[6]对位姿图进行优化。这里值得一提的是,本文主要对Essential Graph进行优化,它是一个covisibility graph中的一个更稀疏的子图,更多细节将在第三部分D小节描述。闭环检测和校验步骤将在第7部分详细描述。

    我们使用g2o[37]库中的Levenverg-Marquardt算法执行所有的优化。我们在附录中描述了每个优化的误差,计算成本和变量。

    C、地图点云、关键帧及其选择标准

    对每个地图点云pi保存以下信息:

    • 它在世界坐标系中的3D坐标X_w.i
    • 视图方向n_i,即所有视图方向的平均单位向量(该方向是指连接该点云和其对应观测关键帧光心的射线方向)
    • ORB特征描述子D_i,与其他所有能观测到该点云的关键帧中ORB描述子相比,该描述子的汉明距离最小
    • 根据ORB特征尺度不变性约束,可观测的点云的最大距离dmax和最小距离dmin

    对每个关键帧K_i保存以下信息:

    • 相机位姿T_(i,w),从世界坐标系转换到相机坐标系下的变换矩阵
    • 相机内参,包括主点和焦距
    • 从图像帧提取的所有ORB特征,不管其是否已经关联了地图云点, 这些ORB特征点都经过畸变模型矫正过

    地图点云和关键帧的创建条件较为宽松,但是之后则会通过一个非常严格苛刻的删选机制进行挑选,该机制会检测出冗余的关键帧和匹配错误的或不可跟踪的云点进行删除。这样做的好处在于地图在构建过程中具有一定的弹性,在外界条件比较困难的情况下(比如:旋转,相机快速运动),算法仍然可以实现鲁棒的跟踪,而与此同时,当相机对同一个环境重访问时,地图的尺度大小是可控的,这就利于该系统的长期工作。另外,与PTAM算法相比,我们构建的地图中基本不包含局外点,因为秉持的原则是很苛刻的,宁缺毋滥。地图云点和关键帧的筛选过程将在第6部分B节和E节分别解释。

    D、Covisibility Graph和Essential Graph

    关键帧之间的Covisibility信息在本文的SLAM系统中几个模块上都非常有用,像论文[7]一样,我们将其表示成一个间接的权重图。图中每个节点代表一个关键帧,如果两个关键帧都能同时观测到地图云点中至少15个点,则这两个关键帧之间用一条边线相连,我们用权重θ表示两个关键帧能共同观测到的云点数量

    为了矫正闭环回路,我们像论文[6]那样做位姿图优化,优化方法延着位姿图将闭环回路的误差进行分散。考虑到covisibility graph可能非常密集的边缘,我们提出构建一个(Essential Graph),该图中保留了covisibility graph的所有节点(关键帧),但是边缘更少,仍旧保持一个强大的网络以获得精确的结果。系统从初始关键帧开始增量式地构建一个生成树,它是一个边缘数量最少的covisibility graph的子图像。当插入新的关键帧时,则判断其与树上的关键帧能共同观测到多少云点,然后将其与共同观测点最多的关键帧相连反之,当一个关键帧通过筛选策略被删除时,系统会重新更新与其相关的连接。Essential Graph包含了一个生成树,一个高covisibility(θmin=100)的covisibility graph边缘子集,以及闭环回路的边缘,这样的组合共同构建了一个强大的相机网络。图2展示了一个covisibility graph,生成树和相关的essential graph的例子。在本文第8部分第E节的实验里,当算法运行位姿图优化时,结果可以达到非常高的精度以至于即便是全局BA优化都很难达到。。essential graph的效用和θmin对算法的影响将在第8部分E节的最后讨论。

    在这里插入图片描述>图2 对TUM RGB-D标准库[38]中fr3_long_office_household图像序列进行重构以及本文用到的各种姿态图的例子

    E、基于图像词袋模型的位置识别

    为了实现闭环检测与重定位,系统嵌入了基于DBoW2[5]算法来执行闭环检测和重定位。视觉词汇(Visual words)是一个离散化的特征描述子空间,被称为视觉词典。这部视觉词典是通过从大量图像中提取ORB描述子离线创建的。如果图像的通用性强,则同一部视觉词典在不同的环境下也能获得很好的性能,正如我们之前的论文[11]那样。SLAM系统增量式地构建一个数据库,该数据库中包含了一个反向指针,用于存储每个视觉词典里的视觉单词,关键帧可以通过这个数据库查询视觉词典,从而实现高效检索。当一个关键帧通过筛选程序删除时,数据库也会相应更新。

    由于关键帧之间可能会存在视图上的重叠,因此检索数据库时,可能返回的结果不止一个高分值的关键帧。原版的DBoW2认为是图像重叠的问题,就将时间上接近的图像的分值相加。但这并没有包括观测同一地点但在不同时间插入的关键帧。为了解决这一问题,我们将这些与covisibility graph相连的关键帧进行分类。另外,我们的数据库返回的是分值高于最好分值75%的所有关键帧。
    用词袋模型来表示特征匹配的另外一个优势在论文[5]里有详细介绍。如果我们想计算两个ORB特征的对应关系,我们可以强制匹配视觉字典树上某一层(我们在6层里面选第2层)的相同节点(关键帧)里的特征,这可以加快搜索速度。在本文中,我们就利用这个小技巧来搜索匹配的特征点,用于三角化新的点云,闭环检测和重定位。我们还引入一个方向一致性测试来改进匹配点,具体如论文[11],这可以去掉无效数据,保证所有对应匹配点的旋转方向一致。

    IV. 地图自动初始化

    地图初始化的目的是计算两帧图像之间的相对位姿来三角化一组初始的地图云点。这个方法应该与场景无关(平面的或一般的)而且不需要人工干预去选择良好的双视图配置,比如两幅图应具有明显的视差。本文算法提出并行计算两个几何模型,一个是面向平面视图的单映矩阵,另一个是面向非平面视图的基础矩阵。然后,采用启发式的方法选择模型,并使用所选的模型从两图像的相对位姿中对地图点云进行重构。本文算法只有当两个视图之间的视差达到安全阈值时,才进行地图初始化。如果检测到低视差的情况或已知两视图模糊的情况(如论文[27]所示),则为了避免生成一个有缺陷的地图而推迟初始化。算法的步骤是:

    • 1.查找初始的匹配点对:
      从当前帧中提取ORB特征 F c F_c Fc(只在最好的尺度上),与在参考帧 F r F_r r搜索匹配点对 x c ↔ x r x_c\leftrightarrow x_r xcxr。如果找不到足够的匹配点对,就重置参考帧。
    • 2.并行计算两个模型:
      在两个线程上并行计算单应矩阵 H c r H_{cr} Hcr和基础矩阵 F c r F_{cr} Fcr
      在这里插入图片描述
      在文献[2]中详细解释了基于RANSAC的归一化DLT算法和8点算法计算原理。为了使两个模型的计算流程尽量一样,将两个模型的迭代循环次数预先设置成一样,每次迭代的特征点数目也预先设置好,基础矩阵是8个特征点对,单映矩阵是4个特征点对。每次迭代中,我们给每个模型M(H表示单映射,F表示基本矩阵)计算一个分值SM:

    在这里插入图片描述

    其中, d c r 2 d_{cr}^2 dcr2 d r c 2 d_{rc}^2 drc2是帧到帧之间的对称传递误差,其计算方法参见文献[2]。 T M T_M TM是无效数据的排除阈值,它的依据是 χ 2 \chi ^2 χ2测试的95%( T H = 5.99 , T F = 3.84 T_H=5.99, T_F=3.84 TH=5.99,TF=3.84,假设在测量误差上有1个像素的标准偏差)。 等于 T H T_H TH,这样两个模型在有效数据上对于同一误差d的分值相同,同样使得运算流程保持一致。
    我们从单应矩阵和基本矩阵的计算中选择分值最高的,但如果两个模型分值都不高(没有足够的局内点),则算法流程重启,从step1开始重新计算。

    • 3.模型选择:

      如果场景是平面,近平面或存在低视差的情况,则可以通过单映矩阵来求解。同样地,我们也可以找到一个基础矩阵,但问题是基础矩阵不能够很好的约束该问题[2],而且从基础矩阵中计算得到的运动结果是错误的。在这种情况下,我们应该选择单映矩阵才能保证地图初始的正确性,或者如果检测到低视差的情况则不进行初始化工作。另一方面,对于非平面场景且有足够的视差的情况则可以通过基础矩阵来计算,而在这种情况下单映矩阵只有基于平面点或者低视差的匹配点才能找到。因此,在这种情况下我们应该选择基础矩阵。我们利用如下强大的启发式进行计算:
      在这里插入图片描述
      如果 R H > 0.45 R_H>0.45 RH>0.45 , 这表示二维平面和低视差的情况,我们将选择计算单应矩阵。其他的情况,我们选择基础矩阵。

    • 4.运动和从运动到结构的重构

    • 一旦选择好模型,我们就可以获得相应的运动状态。如果选择单映矩阵,我们按照Faugeras等人发表的论文[23]中提到的方法,提取8种运动假设,该方法提出用cheriality测试来选择有效解。然而,如果在低视差的情况下,这些测试就会失效,因为云点很容易在相机的前面或后面移动,会导致选解错误。我们提出的方法是直接按这8种解将二维点三角化,然后检查是否有一种解可以使得所有的云点都位于两个相机的前面,且重投影误差较小。如果没有一个明确的解胜出,我们就不执行初始化,重新从第一步开始。这种方法使初始化程序在低视差和两个交叉的视图情况下更具鲁棒性,这也是我们整个算法体现鲁棒性的关键所在。
      在基本矩阵的情况下,我们使用标定矩阵K用下式将其转换为本质矩阵:.
      在这里插入图片描述
      然后用文献[2]中的奇异值分解方法计算4个运动解,然后就像上文中叙述的一样,我们将四个解用于三角化特征点,以选择正解。

    • 5.Bundle adjustment
      最后我们执行一个全局BA,详细优化过程参见附录,以优化初始重构得到的点云地图。
      如图3所示是对论文[39]中的室外NewCollege机器人图像序列进行地图初始化的例子,室外环境下初始化工作具有很大挑战性。从图中可以看出,PTAM算法和LSD-SLAM算法对位于同一平面上的所有点都进行了初始化,而我们的方法是当两幅图像有足够视差之后才进行初始化,并基于基础矩阵得到了正确的结果。

      在这里插入图片描述
      图3 基于NewCollege图像序列[39]进行地图初始化,最上面一行:PTAM算法,中间一行:LSD-SLAM算法,底下一行:ORB-SLAM算法。其中,PTAM算法和LSD-SLAM算法初始化了一个错误的平面地图,而我们的方法自动选择在两帧图像存在足够视差的情况下再利用基础矩阵初始化。如果人工选择关键帧,则PTAM算法也能够初始化得很好。

    跟踪

    在这一部分,我们将详细介绍跟踪线程在相机每帧图像上执行的步骤。在几个步骤中都提到的相机位姿优化,包括运动BA,将在附录部分进行阐述。

    A、ORB特征提取

    我们在8层图像金字塔上提取FAST角点,金字塔图像尺度因子为1.2。如果图像分辨率从512384到752480,我们发现提取1000个角点比较合适,如果分辨率提高,如KITTI数据集[40],则提取2000个角点。为了确保特征点均匀分布,我们将每层图像分成网格,每格提取至少5个角点。然后检测每格角点,如果角点数量不够,就调整阈值。如果某些单元格内检测不出角点,则其对应提取的角点数量也相应减少。最后,根据保留的FAST的角点计算方向和ORB特征描述子。ORB特征描述子将用于算法后续所有的特征匹配,而不是像PTAM算法中那样根据图像区块的相关性进行搜索。

    B、通过前一图像帧估计相机的初始位姿

    如果上一帧图像跟踪成功,我们就用运动速率恒定模型来预测当前相机的位置(即认为摄像头处于匀速运动),然后搜索上一帧图像中的特征点在地图中的对应云点与当前帧图像的匹配点,最后利用搜索到的匹配点对当前相机的位姿进一步优化。但是,如果没有找到足够的匹配点(比如,运动模型失效,非匀速运动),我们就加大搜索范围,搜索地图云点附近的点在当前帧图像中是否有匹配点,然后通过寻找到的对应匹配点对来优化当前时刻的相机位姿。

    C、通过全局重定位来初始化位姿

    如果扩大了搜索范围还是跟踪不到特征点,(那么运动模型已经失效),则计算当前帧图像的词袋(BoW)向量,并利用BoW词典选取若干关键帧作为备选匹配帧(这样可以加快匹配速度);然后,在每个备选关键帧中计算与地图云点相对应的ORB特征,就如第三部分E节所描述的。接着,对每个备选关键帧轮流执行PnP算法[41]计算当前帧的位姿(RANSAC迭代求解)。如果我们找到一个姿态能涵盖足够多的有效点,则搜索该关键帧对应的更多匹配云点。最后,基于找到的所有匹配点对相机位置进一步优化,如果有效数据足够多,则跟踪程序将持续执行。

    D、跟踪局部地图

    一旦我们获得了初始相机位姿和一组初始特征匹配点,我们就可以将更多的地图云点投影到图像上以寻找更多的匹配点。为了降低大地图的复杂性,我们只映射局部地图。该局部地图包含一组关键帧K1,它们和当前关键帧有共同的地图云点,还包括与关键帧K1在covisibility graph中相邻的一组关键帧K2。这个局部地图中有一个参考关键帧 K r e f ∈ K 1 K_{ref}∈K1 KrefK1,它与当前帧具有最多共同的地图云点。现在对K1, K2中可见的每个地图云点,在当前帧中进行如下搜索:

    • 1.计算地图云点在当前帧图像中的投影点x。如果投影位置超出图像边缘,就将对应的地图云点删除
    • 2.计算当前视图射线v和地图云点平均视图方向n的夹角。如果n<cos(60o),就删除对应云点
    • 3.计算地图云点到相机中心的距离d。如果它不在地图云点的尺度不变区间内,即d∉[dmin,dmax],就删除该云点
    • 4.计算每帧图像的尺度比d/dmin
    • 5.对比地图云点的特征描述子D和当前帧中还未匹配的ORB特征,在预测的尺度层和靠近x的云点作最优匹配

    相机位姿最后通过当前帧中获得所有的地图云点进行优化。(这个环节的目的是在当前帧和局部地图之间找到更多的匹配点对,来优化当前帧的位姿)。

    新关键帧的判断

    最后一步是决定当前帧是否可以作为关键帧。由于局部地图构建的过程中有一个机制去筛选冗余的关键帧,所以我们需要尽快地插入新的关键帧以保证跟踪线程对相机的运动更具鲁棒性,尤其是对旋转运动。我们根据以下要求插入新的关键帧:

    • 1.距离上一次全局重定位后需要超过20帧图像。
    • 2.局部地图构建处于空闲状态,或距上一个关键帧插入后,已经有超过20帧图像。
    • 3.当前帧跟踪少于50个地图云点。
    • 4.当前帧跟踪少于参考关键帧K_ref云点的90%。

    与PTAM中用关键帧之间的距离作为判断标准不同,我们加入一个最小的视图变换,如条件4。条件1 确保一个好的重定位,条件3保证好的跟踪。如果局部地图构建处于忙状态(条件2的后半部分)的时候插入关键帧,就会发信号去暂停局部BA,这样就可以尽可能快地去处理新的关键帧。

    局部建图

    这章我们将描述根据每个新的关键帧 K i K_i Ki构建局部地图的步骤。

    A、关键帧插入

    首先更新covisibility graph,具体包括:添加一个关键帧节点 K i K_i Ki,检查与 K i K_i Ki有共同云点的其他关键帧,用边线连接。然后,更新生成树上与 K i K_i Ki有最多共享点的其他关键帧的链接。计算表示该关键帧的词袋,并利用三角法生成新的地图云点。

    地图点云筛选

    三角化的云点为了已知保留在地图中,必须在其创建后的头三个关键帧中通过一个严格的测试,该测试确保留下的云点都是能被跟踪的,不是由于错误的数据而被三角化的。一个云点必须满足如下条件:

    • 1.跟踪线程必须在超过25%的图像中找到该特征点。
    • 2.如果创建地图云点经过了多个关键帧,那么它必须至少是能够被其他3个关键帧观测到。

    一旦一个地图云点通过测试,它只能在被少于3个关键帧观测到的情况下移除。这样的情况在关键帧被删除以及局部BA排除异值点的情况下发生。这个策略使得我们的地图包含很少的无效数据。

    C、新地图点云创建

    新的地图云点的创建是通过对covisibility graph中连接的关键帧Kc中的ORB特征点进行三角化实现的。对Ki中每个未匹配的ORB特征,我们在其他关键帧的未匹配云点中进行查找,看是否有匹配上的特征点。这个匹配过程在第三部分第E节中有详细阐述,然后将那些不满足对级约束的匹配点删除。ORB特征点对三角化后,需要对其在摄像头坐标系中的深度信息,视差,重投影误差和尺度一致性进行审查,通过后则将其作为新点插入地图。起初,一个地图云点通过2个关键帧观测,但它在其他关键帧中也有对应匹配点,所以它可以映射到其他相连的关键帧中,搜索算法的细则在本文第5部分D节中有讲述。

    D、局部BA

    局部BA主要对当前处理的关键帧 K i K_i Ki,以及在covisibility graph中与 K i K_i Ki连接的其他关键帧 K c K_c Kc,以及这些关键帧观测到的地图云点进行优化所有其他能够观测到这些云点的关键帧但没有连接 K i K_i Ki的会被保留在优化线程中,但保持不变。优化期间以及优化后,所有被标记为无效的观测数据都会被丢弃,附录有详细的优化细节。

    E、局部关键帧筛选

    为了使重构保持简洁,局部地图构建尽量检测冗余的关键帧,删除它们。这样对BA过程会有很大帮助,因为随着关键帧数量的增加,BA优化的复杂度也随之增加。当算法在同一场景下运行时,关键帧的数量则会控制在一个有限的情况下,只有当场景内容改变了,关键帧的数量才会增加,这样一来,就增加了系统的可持续操作性。如果关键帧Kc中90%的点都可以被其他至少三个关键帧同时观测到,那认为Kc的存在是冗余的,我们则将其删除。尺度条件保证了地图点以最准确的方式保持它们对应的关键帧(这句翻译没理解透:The scale condition ensures that map points maintain keyframes from which they are measured with most accuracy.)这个策略受Tan等人的工作[24]的启发,在这项工作中,作者在经过一系列变化检测后即将关键帧删除。

    闭环检测

    闭环检测线程抽取 K i K_i Ki——最后一帧局部地图关键帧,用于检测和闭合回环。具体步骤如下:

    A、候选关键帧

    我们先计算Ki的词袋向量和它在covisibility graph中相邻图像(θmin=30)的相似度,保留最低分值Smin。然后,我们检索图像识别数据库,丢掉那些分值低于Smin的关键帧。这和DBoW2中均值化分值的操作类似,可以获得好的鲁棒性,DBoW2中计算的是前一帧图像,而我们是使用的covisibility信息。另外,所有连接到Ki的关键帧都会从结果中删除。为了获得候选回环,我们必须检测3个一致的候选回环(covisibility graph中相连的关键帧)。如果对Ki来说环境样子都差不多,就可能有几个候选回环。

    B、计算相似变换

    单目SLAM系统有7个自由度,3个平移,3个旋转,1个尺度因子 [6]。因此,闭合回环,我们需要计算从当前关键帧Ki到回环关键帧Kl的相似变换,以获得回环的累积误差。计算相似变换也可以作为回环的几何验证。

    我们先计算ORB特征关联的当前关键帧的地图云点和回环候选关键帧的对应关系,具体步骤如第3部分E节所示。此时,对每个候选回环,我们有了一个3D到3D的对应关系。我们对每个候选回环执行RANSAC迭代,通过Horn方法(如论文[42])找到相似变换。如果我们用足够的有效数据找到相似变换Sil,我们就可以优化它,并搜索更多的对应关系。如果Sil有足够的有效数据,我们再优化它,直到Kl回环被接受。

    C、回环融合

    回环矫正的第一步是融合重复的地图云点,在covisibility graph中插入与回环相关的的新边缘。先通过相似变换Sil矫正当前关键帧位姿Tiw,这种矫正方法应用于所有与Ki相邻的关键帧,这样回环两端就可以对齐。然后,回环关键帧及其近邻能观测到的所有地图云点都映射到Ki及其近邻中,并在映射的区域附近小范围内搜索它的对应匹配点,如第5部分D节所述。所有匹配的地图云点和计算Sil过程中的有效数据进行融合。融合过程中所有的关键帧将会更新它们在covisibility graph中的边缘,创建的新边缘将用于回环检测。

    D、Essential Graph优化

    为了有效地闭合回环,我们通过Essential Graph优化位姿图,如第三部分D节所示,这样可以将回环闭合的误差分散到图像中去。优化程序通过相似变换校正尺度偏移,如论文[6]。误差和成本计算如附录所示。优化过后,每一个地图云点都根据关键帧的校正进行变换。

    实验

    我们采用NewCollege[39]的大场景机器人图像序列对本文提出的系统进行了较全面的实验评估,首先采用TUM的室内16个手持RGB-D数据集[38]对系统的总体性能进行了评估,包括算法的定位精度,重定位和程序运行能力;然后,用KITTI的10个汽车户外图像数据集[40],评估算法在实时大场景下的操作及其定位精度和位姿图的优化效率。

    算法运行在Intel Core i7-4700MQ (4核@2.40GHz)和8GB RAM的实验平台上,运算速率可达到实时,且以帧率对图像进行准确处理。ORB-SLAM有3个主线程,它们和其他ROS线程并行运行,由于引入了ROS操作系统,因此算法结果具有一定的随机性,针对这个原因,我们在一些实验中公布了算法运行的中间结果。

    A、基于Newcollege数据集测试系统性能

    NewCollege数据集[39]包含了一个2.2公里的校园的机器人图像序列。它是由双目相机拍摄,帧率为20fps,分辨率512x38。图像序列中包含几个回环和快速的旋转,这对单目视觉定位非常具有挑战性。据我们所知,目前没有单目系统可以处理整个图像序列。例如论文[7],尽管其算法可以实现回环检测,也可以应用于大场景环境,但只有小部分序列图像能够显示单目结果。.
    如图4显示的是我们的算法检测到的闭合回路,从图中可以看出,我们选择的有效数据点都支持相似性变换。图5则对比了回环闭合前后的环境地图重构状况。其中,红色标注的是局部地图,回环检测后可以看到其两端扩展到连接整个运行轨迹。图6是以实时帧率速度运行整个图像序列后的全局地图,从图中可以看出,后边的大回环并没有完全闭合,它从另外一个方向穿过,位置识别程序没能发现闭合回路。

    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    我们统计了ORB_SLAM算法每个线程所用的时间。表1显示了算法跟踪和局部构图的时间。可以看出,跟踪的帧率大概在25-30Hz,这是跟踪局部地图所需的最多时间。如果需要的话,这个时间还可以更快,只要减少局部地图中所包含的关键帧数量即可。局部地图构建线程中需时最高的是局部BA优化。局部BA的时间根据机器人探索环境的状态变动,即在未探索环境下所需时间多,在已经探索过的环境下运行所需时间少,因为在未知环境中如果跟踪线程插入一个新的关键帧,BA优化会被中断,如第5部分E节所示。如果不需要插入新的关键帧,局部BA优化则会执行大量已经设置的迭代程序。

    在这里插入图片描述

    表2显示了6个闭合回路的结果。可以看到回环检测是如何亚线性地随关键帧数量的增多而增加。这主要是由于高效的数据库检索,表2中只比较了具有相同图像单词的图像子集,由此可见用于位置识别词袋模型的潜力。我们的Essential Graoh中包含的边缘是关键帧数量的5倍,它是一个稀疏图。

    在这里插入图片描述

    B、基于TUM RGB-D标准库的定位精度

    TUM RGB-D数据集[38]是一个用于估计摄像头定位精度的优秀数据库,它提供了许多图像序列,还包括外部运动捕捉系统提供的对应轨迹真值。我们去掉那些不适合纯单目SLAM系统的图像序列,这些序列包含强烈的旋转,没有纹理或没有运动。

    为了验证算法性能,我们选择了最近提出的直接法半稠密LSD-SLAM(论文[10])和经典算法PTAM(论文[4])作为对比。除此之外,我们还比较了由RGBD-SLAM(论文[43])算法生成的轨迹。为了在相同的基准下比较ORB-SLAM,LSD-SLAM和PTAM,我们用相似变换对齐关键帧轨迹,在尺度未知的情况下,检测轨迹的绝对误差(论文[38])。对RGBD-SLAM算法,我们通过相机坐标变换来对齐轨迹,也采用同样的方法检测尺度是否重构良好。LSD-SLAM从随机深度值开始初始化,然后随机值逐渐收敛,因此与基准对比的时候,我们会丢掉前10个关键帧。对于PTAM算法,我们从一个好的初始化中,手动选择两个关键帧。表3 是对我们选择的16个图像序列运行5次的中间结果。
    TUM RGB-D数据集[38]是一个用于估计摄像头定位精度的优秀数据库,它提供了许多图像序列,还包括外部运动捕捉系统提供的对应轨迹真值。我们去掉那些不适合纯单目SLAM系统的图像序列,这些序列包含强烈的旋转,没有纹理或没有运动。

    为了验证算法性能,我们选择了最近提出的直接法半稠密LSD-SLAM(论文[10])和经典算法PTAM(论文[4])作为对比。除此之外,我们还比较了由RGBD-SLAM(论文[43])算法生成的轨迹。为了在相同的基准下比较ORB-SLAM,LSD-SLAM和PTAM,我们用相似变换对齐关键帧轨迹,在尺度未知的情况下,检测轨迹的绝对误差(论文[38])。对RGBD-SLAM算法,我们通过相机坐标变换来对齐轨迹,也采用同样的方法检测尺度是否重构良好。LSD-SLAM从随机深度值开始初始化,然后随机值逐渐收敛,因此与基准对比的时候,我们会丢掉前10个关键帧。对于PTAM算法,我们从一个好的初始化中,手动选择两个关键帧。表3 是对我们选择的16个图像序列运行5次的中间结果。

    从表中可以看出,ORB-SLAM可以处理所有的图像序列,除了fr3 nostructure texture far (fr3 nstr tex far)以外。这是一个平面的场景,相机的轨迹在这种情况下有两种可能,正如论文[27]中的描述的。我们的初始化方法检测到这种模棱两可的情况,为了保证算法的安全运行选择不进行初始化。PTAM初始化有时会选择对的方案,有些可能会选择错的方案,且导致的错误可能不能接受。我们没有注意到LSD-SLAM的2种不同的重构方案,但在这个图像序列出现的错误非常多。针对其他的图像序列,PTAM和LSD-SLAM算法的鲁棒性都比我们的方法差,且分别有八组序列和三组序列中地图点容易跟踪丢失。

    关于精度问题,没有回环检测期间,ORB-SLAM和PTAM算法的定位精度相当,但回环检测成功后,ORB-SLAM算法将达到更高的定位精度,正如在图像序列fr3 nostructure texture near withloop (fr3 nstr tex near)中表现的。非常意外的一个结果是PTAM和ORB-SLAM都非常明显地表现出精度高于LSD-SLAM和RGBD-SLAM。一个可能的原因是它们将地图的优化过程简化为一个单纯的姿态图优化过程,这样就造成了传感器测量信息的丢失,但在我们的算法中,采用BA优化,同时通过传感器测量优化相机的姿态和地图的云点位置,这是解决运动到结构[2]的经典标准算法。。我们将在第9部分B节进一步讨论了这个结果。另一个有趣的结果是在图像序列fr2 desk with person 和 fr3 walking xyz中,LSD-SLAM对动态物体的鲁棒性相比ORB-SLAM差一些。

    我们注意到RGBD-SLAM在图像序列fr2上尺度上有一个偏差,用7自由度对齐轨迹则误差明显减少。最后我们注意到Engle等人在论文[10]中提出在f2_xyz上PTAM的精度比LSD-SLAM算法低,RMSE是24.28cm。然而,论文没有给出足够的细节说明如何获得这些结果的,因此我们没有办法复现它。

    在这里插入图片描述

    C、基于TUM RGB-D标准数据库的重定位

    我们在TUM RGB-D数据集上进行了两组重定位实验。在第一个实验中,我们选择fr2_xyz图像序列,通过前30秒构建了一个地图,然后对后来的每一帧图像都进行全局重定位,并评估重构出来的相机位姿精度。我们对PTAM算法进行了相同的实验。如图7所示是创建初始地图的关键帧,重定位的图像帧位姿和这些帧对应的真值。从图中可以看出PTAM算法只能够对重定位关键帧附近的图像帧,这是因为其算法中重定位方法并不具备不变形导致的。表4显示了PTAM算法和ORB_SLAM算法相对地面真值的误差。从表中数据可以看出,ORB-SLAM比PTAM可以更精准地多定位2倍的图像帧。在第2个实验中,我们采用fr3_sitting_xyz图像序列来初始化地图,然后用fr3_walking_xyz图像序列重定位所有的图像帧。这是一个颇具挑战性的实验,由于图像中有人移动,会造成图像局部区域的遮挡。在该试验中,PTAM并没有实现重定位,而ORB-SLAM重定位了78%的图像帧,如表4所示。图8显示了ORB-SLAM重定位的一些实验图例。

    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    D、基于TUM RGB-D标准数据集测试算法的运行生命

    之前的重定位实验表明我们的系统可以从非常不同的视角定位地图,在中等动态环境中的鲁棒性也较好。这个特性和关键帧筛选程序使得算法在不同的视角和局部动态环境中能够一直运行到图像结束。

    在全静态场景情况下,即使相机从不同视角观测场景,ORB-SLAM也可以使关键帧数量保持在一个有限的水平内。我们在一个自定义的图像序列中验证了这一点,手持相机在93秒以内都拍摄同一张桌子,但视角一直变换,形成一个轨迹。我们对比了我们地图的关键帧数量和PTAM生成的关键帧,如图9所示。可以看到PTAM一直都在插入关键帧,而ORB-SLAM会删除冗余的关键帧,将其总数保持在一个稳定的范围内。

    在这里插入图片描述

    当然,在整个程序运行过程中,静态环境下的正常操作是任何SLAM系统的一个基本要求,更引人关注的是动态环境下的状况。我们在几个fr3的图像序列中分析了ORB-SLAM系统的状况,图像序列有:sitting xyz, sitting halfsphere, sitting rpy, walking xyz, walking halfspehere 和walking rpy。所有的视频中,相机都对着桌子,但运动轨迹不同,拍摄场景中有人在移动,椅子也被移动了。如图10(a)所示是ORB_SLAM算法生成的地图中所有关键帧的总数量,图10(b)显示从图像帧中创建或删除关键帧,从中可以看出从关键帧到地图构建需要多久时间。可以看到前2个图像序列中新看到(增加)场景时地图的大小一直在增加。图10(b)是前2个视频中创建的关键帧。在视频sitting_rpy和walking_xyz中,地图没有增加,地图是通过已有场景创建。相反,在最后两个视频中,有更多的关键帧插入但没有在场景中表示出来,可能由于场景的动态变化。图10(C)是关键帧的柱状图,它们是从视频中挑选出来的。大部分的关键帧被筛选程序删除了,只有一小部分留下来了。ORB-SLAM有大量关键帧的生成策略,在未知环境下非常有用;后面系统会生成一个小的子集来代表这些关键帧。

    在这里插入图片描述

    在整个实验中,我们系统的地图根据场景上内容来增加,而不是根据时间,它可以存储场景的动态变化,对场景的理解非常有用。

    E、基于KITTI数据集测试算法在大场景大回环下的性能对比

    KITTI数据集中里程计的数据包括11个视频,它的获取是在一个住宅区驾驶汽车,基准精度非常高,有一个GPS和一个Velodyne Laser Scanner。这个数据集对单目系统非常有挑战性,因为视频中有快速旋转,区域内有大量树叶,这使数据关联变得更困难,而且车速相对较快,视频记录的频率为10fps。除了视频01外,ORB-SLAM可以处理其他所有的视频,01是高速路上的视频,可追踪的物体非常少。视频00,02,05,06,07,09,有闭环回路,系统可以检测到,并使它闭合。其中视频09的闭环只能在视频的最后几个图像帧里检测到,并不是每次都能成功检测到(结果显示的是针对其被检测到的运行情况)。
    对于轨迹与基准的定性比较如图11和12所示。在TUM RGB-D数据集中,我们可以通过相似变换对齐轨迹的关键帧和基准。图11是定性比较的结果,图12是论文[25]中的最新单目SLAM在视频00,05,06,07和08上执行的结果。除了08有一些偏移以外,ORB-SLAM在这些视频上的轨迹都很精准。

    在这里插入图片描述
    在这里插入图片描述

    表5显示了每个视频的关键帧轨迹中间的RMSE误差。我们基于地图尺寸提供了轨迹的误差。结果表明我们的轨迹误差是地图尺寸的1%左右。大致范围低的是视频03的0.3%高的是视频08的5%。视频08中没有闭环,漂移也没办法纠正,因为闭环控制需要获得更精确的重构。

    在这里插入图片描述

    在本次实验中,我们还确认了到底全局BA的20层迭代最终能优化多少地图重构,相关细节如附录所示。我们还注意到全局BA优化可以稍微增加闭环轨迹的精度,但这对开环轨迹有负面影响,这意味着我们的系统已经非常精确了。在有些应用中,如果需要非常精确的结果我们的算法会提供一组匹配,需要定义一个比较强的相机网络,一个初始估计,这样全局BA优化迭代次数就会变少。

    最后讲一下我们算法的闭环检测和用于essential graph边缘的θmin的效率。我们选择视频09(一段非常长的图像序列,在最后有一个闭环),然后评估不同的闭环检测算法。表6是关键帧轨迹RMSE和不同情况下没有闭环检测优化所用的时间,表中的相关内容包括:如果直接采用全局BA优化(20层或100层迭代)的情况,如果只用位姿图优化(10层迭代不同数量的边缘)的情况,如果先用位姿图优化再执行全局BA优化的情况。结果表明,在闭环检测之前,算法的RMSE误差较大,以至于BA优化没办法收敛,即便是迭代100次之后后误差仍旧非常大。另一方面,essential graph优化收敛速度很快,而且结果也更精确。θmin对精度影响并不大,减少边缘的数量会明显降低精度。位姿图优化后再执行一个BA优化则可以增加精度,但时间也增加了。

    在这里插入图片描述

    结论和讨论

    A、结论

    本文中,我们提出了一个新的单目SLAM系统,并详细介绍了其组成模块,最后基于公共数据库对其性能进行了全方位的测试。通过实验得知,我们的系统可以处理室内与室外的图像序列,能够用于汽车、机器人和手持设备上。其定位精度在室内小场景中约为1厘米,室外大场景的应用是几米(前提是我们与真实轨迹尺度对齐的情况下)。

    由Klein和Murray[4]提出的PTAM算法被认为是目前最精准的单目实时SLAM方法。PTAM后端是BA优化,这是众所周知的离线SFM(从运动到结构)问题[2]的经典解法。PTAM算法和Mouragnon[3]早期作品的主要贡献是将BA算法引入到机器人SLAM框架下,并具有良好的实时性。而本文的主要贡献是将PTAM算法的适用性进一步扩展,使其可以应用于原来不可应用的场景下。为了实现这一目标,我们整合了前面几年的优秀作品,引入新的想法和算法,从头设计了一种新的单目SALM系统所用到的技术包括Gálvez-López和Tardós提出的论文[5]中的闭环检测,Strasdat等人在论文[6],[7]中提出的的闭环检测程序和covisibility graph,Kuemmerle等人在论文[37]中提出的g2o优化框架以及Rubble等人提出的ORB特征[9]。到目前为止就我们所知,本文提出的ORB_SLAM方法的定位精度最高,也是最可靠最完整的单目SLAM系统。我们提出的新的生成和删除关键帧策略,允许每个几帧就创建一个关键帧,然后当关键帧冗余时则删除。这样的构图方式很灵活,在外界条件很差的情况下可以保证系统正常运行,比如相机作纯旋转运动或快速移动时。当算法在相同场景下运行时,地图在只有拍摄到新内容的情况下才会增长,可以从我们的长期构图结果中看到这个特性。

    最后,我们还展示了ORB特征具有很好的识别能力,可识别剧烈视角变换情况下的场景信息。此外,它们能够被非常快速的提取和匹配(不需要多线程或GPU加速),这就使得跟踪和地图构建更加实时精确。

    B、离散/特征SLAM方法与稠密/直接SLAM方法对比

    最近,DTAM[44]和LSD-SLAM[10]提出了一种实时单目SALM算法,算法直接利用图像像素的亮度信息进行摄像头的定位与优化,并重构稠密或半稠密的环境地图。这类方法即为直接法,直接方法不需要特征提取,可以避免人工匹配。他们对图像模糊,弱纹理环境和像论文[45]这样的高频纹理环境的鲁棒性更好。与由稀疏点构建的地图相比,比如ORB-SLAM或PTAM算法,稠密/直接法SLAM对相机定位之外的其他应用任务可能更有用途。

    部分重译: 然而,直接方法有他们自己的局限。首先,这些方法假设真实场景中的物体的像是由该物体本身的表面反射模型产生的,因此,算法采用的光度一致性寻找匹配点的思路就限制了匹配点之间的基线距离,通常都比特征匹配点的基线要窄。这对重构的精度影响很大,因为重构需要较宽的基线来减少深度的不确定性。如果直接建模不准确,则可能会受到快门,自动增益和自动曝光的影响(如TUM RGB-D 的对比测试)。最后,由于直接方法计算要求较高,因此为了满足计算速度,DTAM算法采用地图增量式扩张的方法,而LSD-SLAM则丢掉传感器测量信息,将地图优化降低为对位姿图的优化。

    相反,基于特征的方法可以在更宽的基线上匹配特征,主要得益于特征匹配算法较好地视图不变特性。BA优化和相机位姿优化,地图云点通过传感器测量进行融合。在运动结构估计中,论文[46]已经指出了基于特征的方法相比直接方法的优势。在我们的实验第8部分B节中也直接提供了证据,,表明基于特征的定位精度更高。未来单目SLAM应该会整合两种最好的方法。

    C、后续

    我们系统的精度可以通过结合无限远点跟踪来进一步增强。这些在视图中看不到的平行线交点,并没有包含在本文算法构建的地图中,但对相机的旋转非常有用[21]。

    另外一种方法是将稀疏地图更新到一个更加稠密的地图。由于我们关键帧的选择机制,关键帧组成了一个紧凑的地图,地图具有非常高精度的位姿信息和丰富的covisibility信息。所以,ORB-SLAM稀疏地图是一个非常优秀的初始估计框架,比稠密地图更好。这个方向的首次尝试在论文[47]中有详细描述。

    附录:非线性优化

    • Bundle Adjustment (BA)
      地图云点3D位置 X w , j ∈ R 3 X_{w,j}∈R^3 Xw,jR3,关键帧位姿 T i w ∈ S E ( 3 ) T_{iw}∈SE(3) TiwSE(3),w表示世界坐标,通过匹配的关键点 X i , j ∈ R 2 X_{i,j}∈R^2 Xi,jR2减少重投影误差。地图云点j在关键帧i中的误差是:
      在这里插入图片描述
      其中 π i π_i πi是影射函数:
      在这里插入图片描述
      其中, R i w ∈ S O ( 3 ) R_{iw}∈SO(3) RiwSO(3) t i w ∈ R 3 t_{iw}∈R3 tiwR3,分别表示 T i w T_{iw} Tiw的旋转和平移部分, f i , u , f i , v ) f_{i,u} , f_{i,v}) fi,u,fi,v ( c i , u , c i , v ) (c_{i,u} , c_{i,v}) ci,u,ci,v分别是相机i的焦点距离和主点。
      代价函数:
      在这里插入图片描述
      ρ h ρ_h ρh是Huber鲁棒损失函数, Ω i , j = δ i , j 2 I 2 x 2 Ω_{i,j}=δ_{i,j}^2I_{2x2} Ωi,j=δi,j2I2x2是协方差矩阵,与检测关键点的尺度有关。在全局BA中(用于地图初始化,请参见第IV节在第VIII-E节),我们优化了所有点和关键帧。但第一个关键帧除外,这些关键帧保持为原点。在局部BA中(请参见VI-D节),局部区域中包含的所有点均得到优化,而关键帧的子集是固定的。 在姿态优化,或者motion-only BA,(见V)将所有点固定,仅将优化相机姿态。

    • Sim(3)约束下的姿势图优化[6]:给定二进制边的姿势图(请参阅第VII-D节)我们将误差定义为:
      在这里插入图片描述

      其中, S i j S_{ij} Sij是在姿势图优化并将比例因子设置为1之前,从SE(3)姿势计算出的两个关键帧之间的相对Sim(3)变换。在闭环边的情况下,该相对变换通过Horn角[42]的方法。 l o g S i m 3 log_{Sim3} logSim3 [48]转换为切线空间,因此误差是 R 7 R^7 R7维的向量。 目标是“优化Sim(3)关键帧姿势,以最小化损失”功能:
      在这里插入图片描述

      其中 Λ i ; j Λ_{i; j} Λi;j是边缘的信息矩阵,如[48]所示,我们将其设置为恒等式。 我们修复了回路闭合关键帧,以修复7度规的自由度。 尽管“此方法”是完整BA的粗略近似,但我们在VIII-E节中通过实验证明,它比BA具有显着更快和更好的收敛性。

    • Relative Sim(3) Optimization
      给定关键帧1和关键帧2之间的一组n个匹配 i ↔ j i\leftrightarrow j ij(关键点及其关联的3D映射点),我们要优化相对Sim(3)变换 S 12 S_{12} S12(请参见VII-B部分),以最大程度地减小图像间重投影误差:
      在这里插入图片描述
      最小化损失函数:
      在这里插入图片描述
      其中 Ω 1 ; i Ω_{1; i} Ω1;i Ω 2 ; i Ω_{2; i} Ω2;i是与比例相关的协方差矩阵,与图像1和图像2中的关键点的方差有关。 在此优化中点是固定的。

    参考:
    【泡泡机器人翻译专栏】ORB-SLAM:精确多功能单目SLAM系统(一)
    【泡泡机器人翻译专栏】ORB-SLAM:精确多功能单目SLAM系统(二)

    展开全文
  • ORB stitch

    2016-06-01 18:07:51
    基于ORB的图像拼接代码
  • VI_ORB_SLAM2:基于ORB-SLAM2的单目/立体视觉惯性ORB-SLAM 该存储库包括Visual-Inertial ORB-SLAM的Monocular版本和Stereo版本。 这两个是的和的。 有关详细信息,您可以参考Examples/Monocular/mono_euroc_VI.cc和...
  • ORB:基于Opencv的ORB功能Exct
  • ORB-SLAM2 在线构建稠密点云(一)

    万次阅读 多人点赞 2020-02-08 11:11:54
    1、为ORB-SLAM添加点云线程 由于ORB-SLAM在构建的时候只在地图中保留了特征点,对于使用RGB-D相机的小伙伴而言,更希望得到一个点云地图,因此我们单独添加一个线程用于维护点云地图,将ORB-SLAM生成的关键帧传入...

    在这一篇博客中我们(当然不是我啦,网络上有很多的资源,我只是整合了一下,原创性的知识还是来至于广大研究人员)在ROS环境下读取深度相机的数据,然后基于ORB-SLAM2这个框架实现在线构建点云地图和八叉树地图的构建(Octomap)。博文分三个部分:第一部分讲解如何在ORB-SLAM上添加点云建图线程;第二部分讲解如何在ROS下利用深度相机在线构建点云地图;第三部分讲解如何将点云地图在线转换为八叉树地图。注意这里涉及到两个代码,一个是我们在ORB_SLAM2的基础上修改得到的 ORB_SLAM2_PointCloud[2] 和运行的ROS节点 orbslam2_pointcloud[3] ,这两个代码我都放在了码云上面供大家下载。

    1、ORB_SLAM2_PointCloud

    由于ORB-SLAM2在构建的时候只在地图中保留了特征点,对于使用RGB-D相机的小伙伴而言,更希望得到一个点云地图。好在ORB-SLAM2这个框架结构清晰,只需要单独添加一个线程用于维护点云地图,将ORB-SLAM2生成的关键帧传入点云地图构建线程,利用传入关键帧来生成点云地图,这就是基本思路了。其实这个工作高翔博士在早期就已经做过了,并上传到了github中。

    1.1 代码安装

    修改的内容都基本一样,你可以直接load高博修改的版本,也可以在这个地址上修改我使用的版本[2]。下载代码以后,请首先参考ORB-SLAM2的安装方式安装相关依赖。然后对代码进行编译。由于添加了点云构建线程,因此还需要安装点云库(点云的安装参考这个博客)。下载代码后按照按照原版ORB_SLAM2的编译方式进行编译即可。

    1.2 数据集运行

    与原版的ORB-SLAM启动方式一样,使用如下命令就可以直接启动,你就会看到这样的效果:(使用TUM的RGBD数据集)

    ./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml ~/dataset/rgbd_dataset_freiburg1_room/  ~/dataset/rgbd_dataset_freiburg1_room/associate.txt
    

    ORB_SLAM_PointCloud_TUM


    在这里插入图片描述
    运行结束以后会在你的home目录下生成一个名为 “resultPointCloudFile.pcd” 的文件。
    在这里插入图片描述
    这个文件可以通过新建一个终端利用PCL库自带的显示脚本查看:

     pcl_viewer resultPointCloudFile.pcd 
    

    在这里插入图片描述

    2、ROS下运行ORB_SLAM2_PointCloud

    运行思路,将修改后的 ORB_SLAM2_PointCloud 编译的库拷贝到ROS下并新建一个ROS节点,通过这个节点读取相机的数据,然后调用 ORB_SLAM2_PointCloud 的接口,实现利用RGBD相机实时构建地图的功能。 ROS节点通过新增的函数接口,读取点云线程生成的点云地图,发布到指定的topic上面,供其他节点调用。该代码可在[3]下载。

    2.1 新建ROS包

    在ROS工作空间下下新建一个名为 orbslam2_pointcloud 的包,将修改后的 ORB-SLAM_PointCloud 编译的库文件拷贝到ROS下新建一个包,并将所有的头文件拷贝到你所构建的ROS包下的include中。
    具体操作为:
    1、把 libORB_SLAM2.so 拷贝到 orbslam2_pointcloud ROS包中去,覆盖lib目录下的文件
    2、把  ORB-SLAM_PointCloud 中的头文件拷贝到 orbslam2_pointcloud ROS包中去,覆盖include目录下的文件
    3、新建一个config文件夹,然后把相机的内参写为ORB_SLAM2的参数文件格式(这里我使用的是ASTRA的那款RGBD相机),基本只需要复制一个ORB_SLAM2的配置文件过来修改相机的内参即可。 
    在这里插入图片描述

    %YAML:1.0
    
    #--------------------------------------------------------------------------------------------
    # Camera Parameters. Adjust them!
    #--------------------------------------------------------------------------------------------
    
    # Camera calibration and distortion parameters (OpenCV) 
    Camera.fx: 515.2888
    Camera.fy: 517.6610
    Camera.cx: 317.9098
    Camera.cy: 241.5734
    
    Camera.k1:  0.039128
    Camera.k2:  -0.136553
    Camera.p1:  -0.000142
    Camera.p2:  -0.000765
    Camera.k3:  0
    
    Camera.width: 640
    Camera.height: 480
    
    # Camera frames per second 
    Camera.fps: 30.0
    
    # IR projector baseline times fx (aprox.)
    Camera.bf: 40.0
    
    # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
    Camera.RGB: 1
    
    # Close/Far threshold. Baseline times.
    ThDepth: 50.0
    
    # Deptmap values factor 
    DepthMapFactor: 1.0
    
    #--------------------------------------------------------------------------------------------
    # ORB Parameters
    #--------------------------------------------------------------------------------------------
    
    # ORB Extractor: Number of features per image
    ORBextractor.nFeatures: 1000
    
    # ORB Extractor: Scale factor between levels in the scale pyramid 	
    ORBextractor.scaleFactor: 1.2
    
    # ORB Extractor: Number of levels in the scale pyramid	
    ORBextractor.nLevels: 8
    
    # ORB Extractor: Fast threshold
    # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
    # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
    # You can lower these values if your images have low contrast			
    ORBextractor.iniThFAST: 20
    ORBextractor.minThFAST: 7
    
    #--------------------------------------------------------------------------------------------
    # Viewer Parameters
    #--------------------------------------------------------------------------------------------
    Viewer.KeyFrameSize: 0.05
    Viewer.KeyFrameLineWidth: 1
    Viewer.GraphLineWidth: 0.9
    Viewer.PointSize:2
    Viewer.CameraSize: 0.08
    Viewer.CameraLineWidth: 3
    Viewer.ViewpointX: 0
    Viewer.ViewpointY: -0.7
    Viewer.ViewpointZ: -1.8
    Viewer.ViewpointF: 500
    
    

    2.2 编写ROS节点

    这一步需要把相机的彩色图像和深度图像读取进来,然后传递给ORB_SLAM2的接口函数(orbslam->TrackRGBD(color,depth,ros::Time::now().toSec()); ),这里涉及到同步接受相机的彩色图topic和深度图topic的问题,因此需要使用ROS中的软同步机制 ***“message_filters”***。这里我将主要的代码贴出,这个代码也是从高翔博士的代码中修改的。

    #include <stdlib.h>
    #include <stdio.h>
    #include <iostream>
    #include <sstream>
    #include <string>
    #include <vector>
    #include <cmath>
    #include <mutex>
    #include <thread>
    #include <chrono>
    #include <boost/concept_check.hpp>
    
    #include <pcl/point_cloud.h>
    #include <pcl/point_types.h>
    #include <pcl/io/pcd_io.h>
    #include <pcl/visualization/cloud_viewer.h>
    #include<pcl/visualization/pcl_visualizer.h>
    #include <pcl_conversions/pcl_conversions.h>
    #include<pcl/filters/voxel_grid.h>
    #include<pcl/filters/passthrough.h>
    
    #include <opencv2/opencv.hpp>
    
    #include <ros/ros.h>
    #include <ros/spinner.h>
    #include <sensor_msgs/CameraInfo.h>
    #include <sensor_msgs/Image.h>
    #include <sensor_msgs/PointCloud2.h>
    #include <tf/transform_broadcaster.h>
    #include <cv_bridge/cv_bridge.h>
    
    #include <image_transport/image_transport.h>
    #include <image_transport/subscriber_filter.h>
    
    #include <message_filters/subscriber.h>
    #include <message_filters/synchronizer.h>
    #include <message_filters/sync_policies/exact_time.h>
    #include <message_filters/sync_policies/approximate_time.h>
    
    //#include <kinect2_bridge/kinect2_definitions.h>
    
    #include "orbslam2_pointcloud/System.h"
    
    using namespace std;
    
    enum ORBSLAM_MODE
    {
        Mono = 0,
        RGBD ,
        STEREO
    };
    		  
    class Receiver
    {
        public:
    			enum Mode
    			{
    				NOTHING = 0,
    				IMAGE ,
    				DEPTH,
    				BOTH,
    				CLOUD
    			};
    
        private:
    			std::mutex lock;
    
    			//const std::string topicColor, topicDepth,cameraParamFile , orbVocFile; 
    			bool mbuseExact, mbuseCompressed;
    			bool mborbshow;
    			char mborbmode;
    
    			Mode memode;
    			bool mbupdateImage, mbupdateCloud; //code flage 
    			bool mbview,mbsave,mbrunning,mbpublishPointCloud;
    	 
                     
    			const size_t queueSize;
    
    			cv::Mat mcolor, mdepth;
    			cv::Mat cameraMatrixColor, cameraMatrixDepth;
    			cv::Mat lookupX, lookupY;
    
    			typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ExactSyncPolicy;
    			typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> ApproximateSyncPolicy;
    
    			ros::NodeHandle nh;
    			ros::AsyncSpinner spinner;
    			ros::Publisher pub_pointcloud;
    		  
    			image_transport::ImageTransport it;
    			image_transport::SubscriberFilter *subImageColor, *subImageDepth;
    			message_filters::Subscriber<sensor_msgs::CameraInfo> *subCameraInfoColor, *subCameraInfoDepth;
    
    			message_filters::Synchronizer<ExactSyncPolicy> *syncExact;
    			message_filters::Synchronizer<ApproximateSyncPolicy> *syncApproximate;
    
    			std::thread imageViewerThread;
    			
    
    			pcl::PointCloud<pcl::PointXYZRGBA> ::Ptr outputMap;
    			pcl::PCDWriter writer;
    			std::ostringstream oss;
    			std::vector<int> params;
     
    	//ORB-SLAM slam
                      ORB_SLAM2::System*orbslam = nullptr;
    
    public:
            Receiver(const std::string &topicColor, const std::string &topicDepth,
    			   const std::string &cameraParamFile,const std::string orbVocFile,
    			   const char orbmode, const bool orbshow)
                          :queueSize(5),nh("~"), spinner(0), it(nh)
    		{
    		
    			mbpublishPointCloud = false;
    			memode =NOTHING;
    			mbupdateImage = false;
    			mbupdateCloud= false;
    			mbsave= false;
    			mbrunning= true;
    
    			mbuseExact = false;  //
    			mbuseCompressed = false;
    
    			mborbmode =orbmode;
    			mborbshow = orbshow;
    
    			cameraMatrixColor = cv::Mat::zeros(3, 3, CV_64F); // 相机内参数矩阵
    			cameraMatrixDepth = cv::Mat::zeros(3, 3, CV_64F);
    					
    				
    			if(mborbmode == RGBD)
    			{
    				orbslam = new ORB_SLAM2::System(orbVocFile,cameraParamFile,ORB_SLAM2::System::RGBD,mborbshow);
    				mbpublishPointCloud = true;
    			}
    			else if(mborbmode == Mono)
    			{
    				orbslam = new ORB_SLAM2::System(orbVocFile,cameraParamFile,ORB_SLAM2::System::MONOCULAR,mborbshow);
    			}
    			else if(mborbmode == STEREO)
    			{
    				orbslam = new ORB_SLAM2::System(orbVocFile,cameraParamFile,ORB_SLAM2::System::STEREO,mborbshow);
    			}
    			else
    				;
    					
    			pub_pointcloud= nh.advertise<sensor_msgs::PointCloud2> ("PointCloudOutput", 10); //创建ROS的发布节点
    				
    		
    			std::string topicCameraInfoColor = topicColor.substr(0, topicColor.rfind('/')) + "/camera_info";
    			std::string topicCameraInfoDepth = topicDepth.substr(0, topicDepth.rfind('/')) + "/camera_info";
    
    			image_transport::TransportHints hints(mbuseCompressed ? "compressed" : "raw");
    			subImageColor = new image_transport::SubscriberFilter(it, topicColor, queueSize, hints);
    			subImageDepth = new image_transport::SubscriberFilter(it, topicDepth, queueSize, hints);
    			subCameraInfoColor = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoColor, queueSize);
    			subCameraInfoDepth = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh, topicCameraInfoDepth, queueSize);
    
    			cout<<"topicColor: "<<topicColor<<endl;
    			cout<<"topicDepth: "<<topicDepth<<endl;
    			cout<<"subCameraInfoColor: "<<topicCameraInfoColor<<endl;
    			cout<<"subCameraInfoDepth: "<<topicCameraInfoDepth<<endl;
    			cout<<"cameraParamFile: "<<cameraParamFile<<endl<<endl;
    
    			if(mbuseExact)
    			{
    				syncExact = new message_filters::Synchronizer<ExactSyncPolicy>(ExactSyncPolicy(queueSize), *subImageColor, *subImageDepth, *subCameraInfoColor, *subCameraInfoDepth);
    				syncExact->registerCallback(boost::bind(&Receiver::callback, this, _1, _2, _3, _4));
    			}
    			else
    			{
    				syncApproximate = new message_filters::Synchronizer<ApproximateSyncPolicy>(ApproximateSyncPolicy(queueSize), *subImageColor, *subImageDepth, *subCameraInfoColor, *subCameraInfoDepth);
    				syncApproximate->registerCallback(boost::bind(&Receiver::callback, this, _1, _2, _3, _4));
    			}
    		}
    
    		~Receiver()
    		{
    			if(orbslam)
    			{
    			orbslam->Shutdown();
    			delete orbslam;
    			}
    		}
    
    		void run()
    		{
    		start();
    		stop();
    		}
    
    
    private:
    void start()
    {
    	spinner.start();
    
    	std::chrono::milliseconds duration(100);
    	while(!mbupdateImage || !mbupdateCloud)
    	{
    		if(!ros::ok())
    		{
    			return;
    		}
    		std::this_thread::sleep_for(duration);
    		cout<<" wait publisher publish the topic .... "<<endl;
    	}
    
    	cv::Mat color, depth, depthDisp, combined; // local varibale
    	
    	std::chrono::time_point<std::chrono::high_resolution_clock> start, now; 
    	double fps = 0;
    	size_t frameCount = 0;
    	std::ostringstream oss;
    	
    	const cv::Point pos(5, 15); //信息显示的位置
    	const cv::Scalar colorText = CV_RGB(255, 255, 255);
    	const double sizeText = 0.5;
    	const int lineText = 1;
    	const int font = cv::FONT_HERSHEY_SIMPLEX;
    	
    	cout << "starting..."<<endl;;
    	
    	start = std::chrono::high_resolution_clock::now();
    	
    	
    	for(; mbrunning && ros::ok();)
    	{	
    		if(mbupdateImage)//数据发送到ORB-SLAM
    		{
    			lock.lock();
    			color = this->mcolor;
    			depth = this->mdepth;
    			mbupdateImage = false;
    			lock.unlock();
    			if(orbslam)
    			{
    				if(mborbmode == RGBD)
    				{
    					orbslam->TrackRGBD(color,depth,ros::Time::now().toSec()); 
    				}
    				else if(mborbmode == Mono)
    				{
    					orbslam->TrackMonocular(color,ros::Time::now().toSec());
    				}
    				else if(mborbmode == STEREO)
    				{
    					orbslam->TrackStereo(color,depth,ros::Time::now().toSec());
    				}
    				else
    				;	
    			}
    			
    			++frameCount; //计算帧频率
    			now = std::chrono::high_resolution_clock::now();
    			double elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() / 1000.0;
    			if(elapsed >= 1.0)
    			{
    				fps = frameCount / elapsed;
    				oss.str("");
    				oss << "fps: " << fps << " ( " << elapsed / frameCount * 1000.0 << " ms)";
    				start = now;
    				frameCount = 0;
    			
    			}
    			
    			if(mbpublishPointCloud)
    			{
    				orbslam->getPointCloudMap(outputMap);
    				pointcloudpublish(*outputMap);
    			}
    			mbview  = true;
    		}
    		
    		if(mbview ) //优先跟新ORB track 
    		{
    			mbview = false;
    			if(memode == NOTHING )
    			{
    				;
    			}
    			else if(memode == IMAGE )
    				{
    				cv::putText(color, oss.str(), pos, font, sizeText, colorText, lineText, CV_AA);
    				cv::imshow("color image", color);
    				cv::waitKey(1);
    			}
    			else if(memode == DEPTH )
    			{
    				dispDepth(color, depthDisp, 1); //maxValue = 1
    				cv::putText(depthDisp, oss.str(), pos, font, sizeText, colorText, lineText, CV_AA);
    				cv::imshow("depth image",depthDisp);
    			}
    			else if(memode == BOTH )
    			{
    				combine(color, depth, combined);
    				cv::putText(combined, oss.str(), pos, font, sizeText, colorText, lineText, CV_AA);
    				cv::imshow("combined image",combined);
    			}
    			else if(memode == CLOUD )
    			{
    				//generatePointCloud(depth, color, cloud);					
    				//cloudViewer();
    			}
    		}
    	}
    }
    void stop()
    {
    		cv::destroyAllWindows();  // endup the file 
    		cv::waitKey(100);
    		spinner.stop();
    
    		if(mbuseExact)
    		{
    			delete syncExact;
    		}
    		else
    		{
    			delete syncApproximate;
    		}
    
    		delete subImageColor;
    		delete subImageDepth;
    		delete subCameraInfoColor;
    		delete subCameraInfoDepth;
    
    		mbrunning = false;
    		if(memode == BOTH)
    		{
    			imageViewerThread.join();
    		}
    }
    
    void callback(const sensor_msgs::Image::ConstPtr imageColor, const sensor_msgs::Image::ConstPtr imageDepth,
    	      const sensor_msgs::CameraInfo::ConstPtr cameraInfoColor, const sensor_msgs::CameraInfo::ConstPtr cameraInfoDepth)
     {
            cv::Mat color, depth;
    
            //readCameraInfo(cameraInfoColor, cameraMatrixColor);
            //readCameraInfo(cameraInfoDepth, cameraMatrixDepth);
          //  readImage(imageColor, color);
         //   readImage(imageDepth, depth);
    		cv_bridge::CvImageConstPtr pCvImage;
    
    			pCvImage = cv_bridge::toCvShare(imageColor, "rgb8");
    			pCvImage->image.copyTo(color);
    			pCvImage = cv_bridge::toCvShare(imageDepth, imageDepth->encoding); //imageDepth->encoding
    			pCvImage->image.copyTo(depth);
            // IR image input
            if(color.type() == CV_16U)
            {
                  cv::Mat tmp;
                  color.convertTo(tmp, CV_8U, 0.02);
                  cv::cvtColor(tmp, color, CV_GRAY2BGR);
            }
    
            lock.lock();
            this->mcolor = color;
            this->mdepth = depth;
            mbupdateImage = true;
            mbupdateCloud = true;
            lock.unlock();
     }
    
      void readImage(const sensor_msgs::Image::ConstPtr msgImage, cv::Mat &image) const
      {
                cv_bridge::CvImageConstPtr pCvImage;
                pCvImage = cv_bridge::toCvShare(msgImage, msgImage->encoding);
                pCvImage->image.copyTo(image);
      }
    
      void readCameraInfo(const sensor_msgs::CameraInfo::ConstPtr cameraInfo, cv::Mat &cameraMatrix) const
      {
            double *itC = cameraMatrix.ptr<double>(0, 0);
            for(size_t i = 0; i < 9; ++i, ++itC)
            {
                *itC = cameraInfo->K[i];
            }
      }
     
    /**
     * @ 将深度图像映射成彩色图
     * 
     */
      void dispDepth(const cv::Mat &in, cv::Mat &out, const float maxValue)
      {
        cv::Mat tmp = cv::Mat(in.rows, in.cols, CV_8U);
        const uint32_t maxInt = 255;
    
      //  #pragma omp parallel for
        for(int r = 0; r < in.rows; ++r)
        {
          const uint16_t *itI = in.ptr<uint16_t>(r);
          uint8_t *itO = tmp.ptr<uint8_t>(r);
    
          for(int c = 0; c < in.cols; ++c, ++itI, ++itO)
          {
            *itO = (uint8_t)std::min((*itI * maxInt / maxValue), 255.0f);
          }
        }
    
        cv::applyColorMap(tmp, out, cv::COLORMAP_JET);
      }
    /**
     * @ 将深度图像叠加在彩色图像上
     * 
     */
      void combine(const cv::Mat &inC, const cv::Mat &inD, cv::Mat &out)
      {
        out = cv::Mat(inC.rows, inC.cols, CV_8UC3);
    
     //   #pragma omp parallel for
        for(int r = 0; r < inC.rows; ++r)
        {
          const cv::Vec3b
          *itC = inC.ptr<cv::Vec3b>(r),
           *itD = inD.ptr<cv::Vec3b>(r);
          cv::Vec3b *itO = out.ptr<cv::Vec3b>(r);
    
          for(int c = 0; c < inC.cols; ++c, ++itC, ++itD, ++itO)
          {
            itO->val[0] = (itC->val[0] + itD->val[0]) >> 1;
            itO->val[1] = (itC->val[1] + itD->val[1]) >> 1;
            itO->val[2] = (itC->val[2] + itD->val[2]) >> 1;
          }
        }
      }
    void pointcloudpublish(pcl::PointCloud<pcl::PointXYZRGBA> inputMap )
     { 
    	//pcl::PointCloud<pcl::PointXYZ> cloud; 	
    	sensor_msgs::PointCloud2 output;   //声明的输出的点云的格式
    	//pcl::toPCLPointCloud2(*p,p2); //转换到 PCLPointCloud2 类型
    	//pcl_conversions::fromPCL(p2, output);    //转换到 ros 下的格式
    	//pcl::io::loadPCDFile ("/home/crp/catkin_ws/test.pcd", cloud);  
    	toROSMsg(inputMap,output);
    	output.header.stamp=ros::Time::now();
    	output.header.frame_id  ="world";
    	pub_pointcloud.publish(output);
     }
    
    };
    
    int main(int argc, char **argv)
    {
        std::string topicColor ,topicDepth ,cameraParamFile;
        ros::init(argc, argv, "orbslam_ros", ros::init_options::AnonymousName);
        if(!ros::ok())
        {
    	    return 0;
        }   
        if(ros::param::get("~topicColor" ,topicColor))
    	    ;
        else
    	    topicColor = "/kinect2/qhd/image_color_rect";
    
        if(ros::param::get("~topicDepth" ,topicDepth))
    	    ;
        else
    	    topicDepth = "/kinect2/qhd/image_depth_rect";
        if(ros::param::get("~cameraParamFile" ,cameraParamFile))
    	    ;
        else
    	cameraParamFile= "/home/crp/catkin_ws/src/orbslam2_ros/kinect2_qhd.yaml" ;
     
        string orbVoc = "/home/crp/catkin_ws/src/SLAM/orbslam2_pointcloud/ORBvoc.txt" ;
        bool orbmode = RGBD;
        bool orbshow = true;
        Receiver receiver(topicColor, topicDepth, cameraParamFile,orbVoc,orbmode, orbshow);
        cout<<"starting receiver..."<<endl;
        receiver.run();
        ros::shutdown();
        return 0;
    }
    
    

    2.3编译运行

    启动相机节点

    roslaunch astra_launch astra.launch
    

    启动SLAM节点

    roslaunch orbslam2_pointcloud astra.launch
    

    ORB-SLAM Astra相机在线构建octomap

    注:当你换了电脑以后你必须要在要新的电脑上编译依次 ORB_SLAM2_PointCloud 包并将 “libORB_SLAM2.so” “libDBoW2.so” “libg2o.so”重新覆盖ROS包中的三个连接库,否则编译的时候会提示你没有定义一大堆东西。

    3、发布Octomap

    在ROS中启动octomap的服务,让他固定接受一个点云topic并转化为Octomap。具体操作为:新建一个 octomapTransform.launch 文件,填入以下内容

    <?xml version="1.0"?>
    <launch>
      <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    
        <!-- resolution in meters per pixel -->
        <param name="resolution" value="0.05" />
    
        <!-- name of the fixed frame, needs to be "/map" for SLAM -->
        <param name="frame_id" type="string" value="/world" />
    
        <!-- max range / depth resolution of the kinect in meter -->
        <param name="sensor_model/max_range" value="50.0" />
        <param name="latch" value="true" />
    
        <!-- max/min height for occupancy map, should be in meters -->
        <param name="pointcloud_max_z" value="1000" />
        <param name="pointcloud_min_z" value="0" />
    
        <!-- topic from where pointcloud2 messages are subscribed -->
        <remap from="/cloud_in" to="/orbslam2_ros/LocaPointCloud" />
    
      </node>
    </launch>
    

    ORB Octomap

    关于Octomap的安装和启动方法可以查看我另外一篇博客(Octomap 在ROS环境下实时显示)。

    更新日期 2020-8-19
    写博客时受限于本人的水平,上述的方法有一点笨拙。这里还有一种方法可以简便的运行ORB-SLAM2 到达上诉效果。
    1、首先参考第二篇博客ORB-SLAM2 在线构建稠密点云(二), 解决ORB-SLAM2自带ROS环境编译问题,成功运行ORB-SLAM2 RGB-D在线模式,
    2、然后把上述解决ORB-SLAM2 ROS环境的方法套用在这个写有RGB-D建图的代码 ORB_SLAM2_PointCloud [2] 里面,这样就不需要编写单独的ROS节点来调用ORB-SLAM2的库了。

    上一篇 :Octomap 在ROS环境下实时显示    下一篇 :ORB-SLAM2 在线构建稠密点云(二)

    [1] 高翔博士开源的代码: https://github.com/gaoxiang12/ORBSLAM2_with_pointcloud_map
    [2] 我自己修改后的代码: https://gitee.com/cenruping/ORB_SLAM2_PointCloud
    [3] 对应ROS节点的代码: https://gitee.com/cenruping/orbslam2_pointcloud

    如果大家觉得文章对你有所帮助,麻烦大家帮忙点个赞。O(∩_∩)O

    欢迎大家在评论区交流讨论(cenruping@vip.qq.com)

    展开全文
  • circleci-orb-example 赛普拉斯CircleCI Orb示例 该存储库展示了如何使用以最小的配置在CircleCI上运行Cypress。 参见文件 。 该配置是最小的,因为它使用了Cypress orb的命令,但是您可以看到它如何扩展到使用 ...
  • ORB_SLAM3:ORB-SLAM3

    2021-05-10 18:38:19
    ORB-SLAM3 V0.4:Beta版,2021年4月21日 作者:卡洛斯·坎波斯(Carlos Campos),理查德·埃维拉(Richard Elvira),胡安·J·戈麦斯·罗德里格斯(Juan J.GómezRodríguez),( ,( 。 描述了每个版本的功能...
  • 枢轴网格javascript库。 网站: WIP。... var orb = require ( 'orb' ) ; var pgrid = new orb . pgrid ( config ) ; // query var q = pgrid . query ( ) . Manufacturer ( 'Adventure Works' )
  • SLAM十四讲用一天看完了,顺便花一天看了下ORB,brief,ORB-SLAM2原文,现在开始学习里面具体代码。今天主要看ORB特征提取,放vs中的代码: #include <iostream> #include <opencv2/opencv.hpp> #include ...
  • ORB_SLAM3:ORB-SLAM3-源码

    2021-03-17 17:49:34
    ORB-SLAM3 V0.3:Beta版,2020年9月4日 作者:卡洛斯·坎波斯(Carlos Campos),理查德·埃维拉(Richard Elvira),胡安·J·戈麦斯·罗德里格斯(Juan J.GómezRodríguez),( ,( 。 描述了每个版本的功能。...
  • orb算法源码.cpp

    2021-04-20 17:01:50
    orb算法源码
  • ORB-开源

    2021-04-25 18:49:26
    ORB(对象关系桥)是一种全Java O / RM数据库持久性解决方案。
  • ORB-SLAM 它是一个完整的 SLAM 系统,包括视觉里程计、跟踪、回环检测,是一种完全基于稀疏特征点的单目 SLAM 系统,同时还有单目、双目、RGBD 相机的接口。资源包含ORB-SLAM2和ORB-SLAM3详细的中文注释版本的源码。
  • 天体游乐场 使用Orb的示例仓库
  • ORB 算法理解

    2015-10-30 11:33:11
    根据ORB an effiient alternative to SIFT or SURF论文,对ORB算法的理解
  • OrbSlam2-reconstruction:OrbSlam2加入pmvs和重建
  • ORB测试 介绍 这是使用检测平面物体的测试。 我根据编写了代码,以供自学。 用法 有一个名为 ``make_opencv.sh'' 的 bash 脚本可用于编译源代码: bash make_opencv.sh orb_match.cpp 编译源代码后,只需键入以下...
  • ORB特征

    千次阅读 2017-07-14 16:26:43
    本文大部分内容来自ORB特征提取详解,在此基础上加上自己对原论文的理解。ORB(Oriented FAST and Rotated BRIEF)是一种快速提取特征点和描述子的算法。其特征检测基于FAST,采用BRIEF描述子并加以改进。基于《ORB...
  • 赛普拉斯CircleCI Orb并行示例 使用赛普拉斯CircleCI Orb快速并行运行测试 请参见使用安装依赖项并并行运行测试的 。
  • Perl ORB-开源

    2021-04-24 10:19:29
    Perl ORB是Perl的CORBA ORB的实现。 它完全用Perl编写,并支持Perl客户端和服务器。
  • Orb项目的入门模板。 使用在CircleCI上自动构建,测试和发布 。 每个目录中都有其他自述文件。 资源 -此的官方注册表页面,其中描述了所有版本,执行程序,命令和作业。 -用于使用和创建。 如何贡献 我们欢迎您并...
  • ORB-SLAM3-注意 ORB-SLAM3原始注解 代码量太大,注释不完整,重点关注原理。
  • ORB-SLAM2 ORB-SLAM2 作者: 、 、 和 ( )。 原始实现可以在找到。 ORB-SLAM2 ROS 节点 这是用于单目、立体和RGB-D相机的 ORB-SLAM2 实时 SLAM 库的 ROS 实现,用于计算相机轨迹和稀疏 3D 重建(在具有真实比例的...
  • Ptr<ORB> orb = ORB::create( );

    千次阅读 2018-07-25 15:23:53
    Ptr&... orb = ORB::create( 500, 1.2f, 8, 31, 0, 2, ORB::HARRIS_SCORE,31,20 ); 新手学习笔记: static Ptr&lt;ORB&gt; cv::ORB::create ( int nfeatures = 500, float scaleFa...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 28,354
精华内容 11,341
关键字:

orb