使用局部结构特定的形状和外观上下文的姿态估计
标题：Pose Estimation using Local Structure-Specific Shape and Appearance Context
作者：Anders Glent Buch1, Dirk Kraft1, Joni-Kristian Kamarainen2, Henrik Gordon Petersen1 and Norbert Kruger
编译：点云侠
来源：arXiv 2017
本文仅做学术分享，如有侵权，请联系删除。欢迎各位加入免费知识星球，获取PDF论文，欢迎转发朋友圈。内容如有错误欢迎评论留言，未经允许请勿转载！
公众号致力于分享点云处理，SLAM，三维视觉，高精地图相关的文章与技术，欢迎各位加入我们，一起每交流一起进步,有兴趣的可联系微信：920177957。本文来自群友点云侠同学的分享，未经作者允许请勿转载，欢迎各位同学积极分享和交流。
摘要
我们解决了使用结构特定的局部描述符来估计两个模型之间的对准姿态的问题。我们的描述符是使用2D图像数据和3D上下文形状数据的组合生成的，产生了一组半局部描述符，包含边缘和纹理结构的丰富外观和形状信息。这是通过定义描述描述符邻域的特征空间关系来实现的。通过定量评估，我们表明，与最先进的方法相比，我们的描述符提供了很高的鉴别能力。此外，我们展示了如何利用它来估计两个点集之间的对齐姿态。为了验证我们的方法，我们在受控场景和真实场景中进行了实验。
引言
在计算机视觉领域中，确定两个模型表面之间的对准变换问题已经有了广泛的研究。在机器人操作应用中，将对象模型实例化到场景中对于进一步处理任务的成功至关重要。一个相关的应用是物体的抓取和操纵，这需要视觉系统具有很高的精确度。这个过程的准确性对于更高级别的操作(如放置或装配)任务也是至关重要的。
在配准或拼接问题中，使用同一对象或场景模型的多个视图来构建更完整的场景，需要视图之间非常精确的对齐，以便结果可用。同样的方法可以应用于多摄像机设置中外部摄像机参数的估计。
用于解决这些问题的方法已经应用于带有颜色信息的2D图像域以及深度图像或RGB-D数据的3D数据中。对于这些领域，通常采用特征驱动的方法，该方法基于试图去除两个模型之间的虚假特征对应，从而有可能使用剩余的真实点到点对应来计算对准变换。广泛接受的是，局部描述符在这个过程中提供了最高的稳定性，因为它们对杂乱和遮挡具有耐受性[1]
对于图像数据，通常会检测兴趣点或关键点，然后使用局部邻域的外观与上下文图像信息进行增强，从而生成最终的图像描述符[2]-[6]。关键点检测是为了定位不同的兴趣点，从而使得提取过程近似视点不变。通过围绕关键点建立外观描述来确保辨别能力。基于关键点的模型描述往往非常稀疏，因为密度越高，单个特征描述符的独特性就越低。
对于3D数据或点云，在过去几十年中已经开发了各种形状描述符[7]–[12]。这些通常是为完整的数据集构建的，即在模型上的每一点，尽管存在特征选择方法[13]，[14]。对于形状描述符，几何不变量如相对距离或角度可以用来描述一个点的局部邻域。因为形状描述符通常是为所有点创建的，所以这些表示是密集的。
在本文中，我们介绍了一种新的包含RGB数据的3D模型描述符变体。我们的特征处理的输入可以是密集的立体重建或RGB-D图像(见图1)，提供外观和形状数据。我们的目标是以有效的方式组合来自外观和形状域的输入。这将在第3节中详细介绍。
图1 姿态估计系统处理流程图，左:输入RGB-D图像。顶部中间:通过单基因过滤分离外观信息的同一性，产生局部大小、方向和相位为三倍的图像。底部中间:ECV原始提取和他们的3D重建对应物。线段用蓝色标记，纹理用绿色标记。使用该数据，生成ECV上下文描述符。右上角:存储的对象模型和输入场景之间的ECV上下文描述符对应。右下角:物体和场景之间的对准姿态估计。
关键点检测是通过使用早期认知视觉(ECV)系统将单个像素分类为不同类别来实现的。ECV表示的密度介于稀疏图像关键点描述符和密集3D形状描述符之间。这样做的好处是可以更详细地捕捉模型形状，同时保持高水平的辨别能力。为了进一步增加后者，我们在ECV模型的基础上定义了上下文描述符。正如将在下面的部分中展示的，所有这些结合在一起，允许在精确度和速度方面进行有效的姿态估计。
早期的工作已经将ECV特征应用于一系列计算机视觉应用。为了精确地表示场景，文献[15]提出了一种实时提取ECV纹理图元的方法，为每个图元提供不确定性信息。在文献[16]中，自我运动估计是目标，并且ECV边缘结构用于表示观察场景中的边缘信息。在文献[17]中，ECV基元被嵌入到一个概率框架中，该框架允许使用生成模型进行对象识别。最近，ECV特征已经被应用于抓取未知物体的任务中[18]，其中基于边缘和纹理信息的场景表示被用于直接产生稳定的抓取。
本文的贡献是一个模型描述，包含外观和形状信息及其对不同姿态估计任务的适用性，如第4节中描述的对象实例化、场景配准和摄像机校准。我们提出了一个图像处理管道，它在边缘和纹理域生成特征。利用外观和形状信息的互补能力，构建了一种新型具有高分辨力的局部3D描述符。我们在速度优化的RANSAC [19]程序中使用我们的描述符，这表明了我们系统的实际可用性。从三个方面论证了表示的效率:
- 使用外观和形状来描述一个点，
- 关键点被分类为边缘/纹理类型，提供了一个依赖于结构的描述符，
- 关键点密度高，允许比许多其他图像描述符更多的形状信息。
在受控实验装置和真实场景中对我们的系统进行验证，与最先进的方法进行了比较，表明我们的表示法比其他表示法能更有效地处理一系列估计问题。通过展示1)描述符在大的视点变化下提供了大量的真实对应，2)如何有效地解决具有大的观察差异的两个模型之间的姿态估计问题来支持这一主张。
2.特征描述符
这里展示的作品建立在ECV模型中包含的观察空间之上。我们首先简要描述ECV特征提取过程，然后介绍我们的上下文描述符。
A.ECV基本框架
由ECV过程产生的原子特征被称为基元。通过单基因信号的旋转不变滤波方法从图像中提取ECV基元[20]，并随后在3D数据中重建。单基因信号滤波基于局部邻域出现的频谱，在每个像素位置提取局部幅度、方向和相位的三元组，这种维度的扩展被称为同一性的分裂。
使用六边形网格对多点触控图像进行细分，关键点被定位为每个网格单元中具有最大幅度响应的像素。图像的拖尾分裂引出内在维度的概念[21]。单元中光谱方差的测量给出了关于局部图像结构的维数的信息。幅度的微小变化表示均匀的斑块，而方向的微小变化表示边缘或线状结构。纹理区域的特点是大小和方向变化很大。通过对一个单元中的标识值进行适当的阈值化，该单元可以被分类为属于同质块、边缘或纹理区域[22]。给出了关于图像不同部分代表什么感测模态的低级但有价值的信息。由于属于同质斑块的像素本质上是模糊的，因此在这项工作中只使用了边缘区域和纹理区域。图2展示出了示例对象的提取的边缘/纹理图元的可视化。我们称边缘图元为线段，或简称为线段(蓝色)，纹理表面图元为纹理元素(绿色)。在该图和图1中，直接使用来自对准的深度传感器的形状数据完成3D重建过程。从RGB-D数据中提取3D ECV图元已经在GPU上实现，允许实时操作[15]。
图2不同ECV特征的可视化。左:输入RGB-D模型。中间:蓝色的ECV部分。右图:绿色ECV纹理。每个ECV图元包括一个几何图形和一个外观组件。此时，几何部分由3D位置和方向组成。对于线段，方向定义为沿边缘的方向向量，对于纹理子，方向定义为局部表面区域的法向向量。
B.ECV上下文描述符
为了增加ECV特征的辨别能力，我们生成一个基于外观和几何的图形特征的空间邻域描述符。在每个特征位置，计算半径为r的欧几里德邻域内ECV模型上所有特征点对之间的关系，并将结果保存为直方图。
在方向为o1,o2的p1，p2两点之间使用以下几何关系:
方向向量之间的余弦:Rg1=o1*o2
第一个方向向量和点对点方向向量之间的余弦值:
第二个方向向量和点对点方向向量之间的余弦值:
使用一般术语“方向向量”，因为这可以是表面法线(纹理子)或方向(线段)。在图3中，这些几何关系被可视化为一对纹理子。一个重要的细节是任意两点的排序，几何关系Rg是不对称的，因为它们的符号是根据哪个特征点被认为是第一(p1)和第二(p2)而变化。我们通过选择p1作为最接近我们正在为其计算上下文描述符的源特征的点来解决这个问题。
图3特征点之间使用的三种几何关系。在这种情况下是具有相关法向量的纹理子。不使用角度值，使用余弦值来节省计算时间。
对于外观部分，为所有三个RGB颜色通道创建单独的直方图。对于几何关系，取区域内所有可能的点对，计算出三个强度梯度。对于三个颜色通道，将这些外观关系表示为Ra1,Ra2,Ra3。同样，根据到源点的距离成对排序点。总之，这给出了六个独立的直方图，三个基于几何 Rg 和三个基于外观的，它们都集合在最终的描述符向量中。每个直方图由16个面元组成，总描述符维数为96。在本文的其余部分，使用r= 0.025 m，我们发现这是局部性和鉴别能力之间的合理折衷。
与许多其他关于3D形状描述的工作相比，我们的上下文描述符公式只对ECV特征点(线段和纹理子)起作用，而不对底层的点云起作用。这有两个优点:1) 邻域中的点数减少，从而导致计算速度加快；2)通过使用被分类为线/纹理结构的点，避免使用均匀的表面点，这不会增加辨别能力。我们有意地保持上下文描述符的相对简单，这既是为了加快过程，也是为了显示ECV特征对于可靠的上下文描述的潜力。显然，这可以通过系统评估用于描述特征上下文的替代操作符来进一步改进。典型Kinect场景的完整ECV上下文表示的提取大约需要1秒钟，目前正致力于将实现移植到GPU进行实时提取。
3. 姿势估计
在形式上，姿态估计的目标是估计变换 T ，该变换最小化对象模型P上的每个点p和场景模型Q中的对应点q之间的平方距离之和:
在上面的等式中，使用点的齐次表示，以允许矩阵向量乘法。姿态估计通常是用一种健壮的和容忍异常值的方法来解决的，比如RANSAC。处理这一问题的一种常见方式是基于特征对应，其中迭代运行以下内容:
(1) 通过特征描述子的最近邻匹配，找到P中n ≥ 3个随机目标点及其在中的对应点。
(2) 使用n个采样对应来估计假设变换T。
(3) 将假设转换应用于对象模型P。
(4) 通过欧式距离阈值，在变换后的P和Q之间的空间最近邻搜索来寻找内点，如果内点的数量太少，回到步骤(1)。
(5) 使用内点对应关系重新估计假设转换。
(6) 计算内点对应点对之间的距离epi(T)，如果距离达到目前为止的最小值，则T作为最终的转换。
在许多情况下，如果epi低于预定义的收敛阈值，可以通过停止来优化算法。否则，算法将运行指定的迭代次数。
在每次迭代的第一步之后，通过实施低级几何约束(对于目标物体和场景上的对应点，根据它们在各自的空间里之间的欧式距离是不变的这一几何一致性)来对上面的RANSAC姿态估计过程进行修改。具体来说，就是对由物体和场景模型上的n个采样点形成的虚拟多边形的边长之间的比率进行检查。将特征对应采样的点表示为pi(i=1,2.....n)，对象多边形的边长如下所示d(p,i)，同样的方法计算场景多边形的边长d(q,i)。然后，通过n个多边形边长之间的比率来计算相对相异向量delta：
如果两个多边形完全匹配，则delta=0。
相异向量示意图
预期最大偏差低于某个阈值t_poly
因此，对RANSAC的修改是在步骤(1)和步骤(2)插入以下步骤：
计算采样多边形边长之间的相异向量delta，如果||delta||<=t_poly返回步骤(1)。这一验证步骤比使用步骤(2)~(6)实施完全几何约束的成本要低很多，t_poly越接近1，迭代次数就越少；但是，当噪声存在时，会增加排除正确位置的风险。
假设该步骤没有过滤掉正确对齐模型的假设姿势，我们可以期待同样的成功概率，但是时间要短得多。很明显，这个假设只在相当精确的几何观测时成立，这样多边形确实是等距的。如果使用的传感器显示出大的深度误差或产生大的失真效应，考虑到这些不准确性，阈值t_poly需要设置得更高。鉴于目前传感器的质量，我们预计这个问题的范围有限。我们使用t_poly=0.25，从而允许25 %的最大边缘长度差异。
我们对RANSAC的修改是基于一个简单的标准，即应该立即过滤掉错误的假设，从而节省时间来生成更多数量的假设姿态。该方法在拒绝外点阶段可以被认为是分层的，因为该方法在通常的基于内侧的拒绝阶段之前引入了一个不需要姿势的初步的低水平的基于多边形的拒绝。
给定期望成功概率 p和期望w内联分数，计算所需RANSAC迭代次数k的公式为[19]:
在我们的实验中，步骤1中采样n=3个点。使用保守的内联分数估计w=0.05，期望成功率p=0.99，给出k=37000。欧氏内联阈值设置为0.01 m，所需的内联点数设置为对象模型点总数的50 %。
4.实验结果
我们提出了三种不同的实验验证我们的方法。在4.1部分，我们提出了一个系统的评估方法来确定两个模型之间真实特征对应的数量。在4.2部分，我们将我们的估计过程应用于配准场景的两个视图的问题。在4.3部分给出实验结果。
4.1评估分析
有效的姿态估计的一个关键要素是在要对准的模型之间建立对应关系。在这个初始实验中，我们概述了一个测试两个模型之间对应关系的算法。一般来说，我们不能先验地确定两点是否对应，这是评估过程的目标。在文献[27]中，如果最近的和第二最近的特征匹配距离之间的比率低，则在估计期间建立对应关系。这确保了对应关系的唯一性，但只是增加了对应关系为真的概率。
在这里，我们颠倒这个过程:我们从执行两个模型的对齐开始，然后我们检查最初计算的特征对应关系中的哪一个是正确的，简单地通过阈值化在初始对应关系计算阶段匹配的点对之间的欧几里德距离。这受到点云库(PCL)启发[28]。更具体地说，我们执行以下操作:
(1) 为两个模型生成特征描述符，并计算每个对象特征的最近匹配场景特征。这是最初假设的对应集C_hyp；
(2) 执行两个模型的精确对齐，以使每个对象表面点与其真正对应的场景表面点对齐(参见下文)；
(3) 循环所有假设的对应关系，并执行阈值处理:如果对齐的匹配点对之间的欧几里德距离低于给定的阈值，将其存储为真正的对应关系。结果是一组简化的对应关系C_true，很可能是真正的对应关系；
(4) 将真实对应分数计算为：C-true/C-hyp。
如果模型足够接近，可以在第2步使用局部方法，如迭代最近点法[29]，[30]。否则，可以基于步骤(1)中生成的对应关系，应用稳健的估计方法，如RANSAC。步骤(3)中的欧几里德阈值应该被设置为至少对准的预期平均精度，以便补偿估计的不准确性。
在图4中，我们显示了同一对象的两个视图之间基于对齐的对应拒绝的整个过程。上图用白线显示了初始假设集 C_hyp 的子集，下图用白线显示了剩余对应关系C_true的子集，以及用红色覆盖对齐模型的ICP结果。
图4 顶部:两个对象模型之间的ECV上下文描述符对应。底部:在ICP对准(对准最左边的模型用红色覆盖)和欧几里德距离阈值化之后，两个对象模型之间的过滤的对应关系。出于显示目的，两种情况下仅显示25个随机选择的对应关系。
该图以及图1和图2中的对象取自包含大量对象模型以及一些示例场景的Kinect视图的RGB-D数据库[31]。数据库中的所有对象都是在一个转盘上捕获的，帧之间只有几度的位移，并且来自三个不同的高度，每个对象有几百个视图。这里我们使用了框架1和15，它们被移动了大约30度。
我们对从RGB-D数据库中随机选择的一组对象执行了此过程，所有这些对象都使用由15帧分隔的两个视图。对于每个对象，我们使用0.01 m的欧几里得阈值来计算对应分数。我们将ECV特征描述符与图像域(SIFT [2]和SURF [6])以及3D形状域(Spin Image[7]和FPFH [10])中的现有描述符进行比较。对于SIFT/SURF，使用标准设置的OpenCV。SURF需要用户指定的Hessian阈值来进行关键点检测，将其设置为500。对于形状描述符，半径设置为r = 0.025 m用PCL实现。图5中报告了计算的对应分数，所有分数的平均值显示在最右侧。
图5对于十个典型对象，使用五个不同的描述符(包括我们自己的描述符)的真实对应的分数。图表最右边的部分显示了所有测试用例的平均值。
图5中的结果并不令人惊讶。事实上，像“香蕉”和“海绵”这样的形状主导的物体最好用形状描述符来描述。另一方面，纹理对象，如“粘合剂”和“谷物”，最好由基于外观的图像描述符捕捉。在一个例子中，“海绵”，FPFH形状描述符提供了比我们提出的描述符稍好的辨别能力，这是预期的，因为该对象的外观部分非常模糊。对于对象“帽”和“订书机”，SIFT产生了更好的效果。然而，在后一种情况下，SIFT描述符仅在八个检测到的关键点上计算，使得对象模型极其稀疏。相比之下，ECV处理为这个对象产生113个关键点。从这项研究中得出结论，我们的描述符总体上优于其他描述符，这意味着在一系列具有不同形状和外观的对象上的平均表现。总集的平均对应分数为27 %，而第二好的SURF的平均分数为15 %。
4.2场景配准和校准
使用鲁棒的2D描述符[32]，[33]已经成功地实现了用于配准或拼接场景的多个图像的算法。多个3D模型的配准也有其实际用途，例如用于从多个视图构建模型。另一个应用是多摄像机设置的校准，其中为了估计相对摄像机姿态，必须记录场景的多个视图。
如果场景中有足够的形状信息，则无需使用标记就可以非常精确地确定相对相机变换。在图6中，我们展示了将我们改进的RANSAC应用于具有宽基线间隔的场景的两个视图的结果。对于这类任务，由于有限的重叠，我们降低了所需的内点数量。在这个实验中，我们将最左边场景中的内侧分数设置为点数的10 %。
图6使用优化的RANSAC算法配准同一场景的两个不同视图。顶部:输入Kinect视图，显示使用ECV上下文描述符计算的25个随机选择的对应关系。左下方:配准结果，顶部图像中最左侧场景视图的对齐版本用红色覆盖。右下角:对齐点集的缩放。请注意对齐的高质量，这是在没有细化的情况下执行的。
由于迭代次数多，我们实现了非常精确的比对。配准时间低于1秒，这部分是通过我们改进的RANSAC程序实现的，部分是由于ECV代表的低密度。图6中最左边和最右边的场景分别由1255和1448个ECV上下文描述符描述。
在表1中，与标准的RANSAC算法相比，给出了两个相关的统计数据。这些数字是上述估计问题100次运行的平均值。为了能够比较时间，将两种情况下的迭代次数都设置为5000。观察到，改进的RANSAC平均比标准RANSAC快15倍。这是在不损害对齐质量的情况下实现的，这可以从平均拟合误差看出，该误差被计算为阈值内联数的平均值epi。
表1与标准RANSAC相比，改进的RANSAC的定时和平均内拟合误差，使用图6中场景配准问题的100次运行计算。
4.3真实环境中的实验
在这一节中，我们介绍了我们实验室机器人装置的物体姿态估计结果。在我们进行的所有实验中，单个物体的估计时间保持在0.1秒以下。我们目前使用本工作中提出的姿态估计算法来对我们研究所开发的机器人抓取策略进行基准测试
(1)模型表示:由于ECV处理需要图像来捕捉外观，我们从四个不同的角度渲染对象。从视图中提取的ECV特征被反投影到3D模型形状，之后为该视图建立上下文描述符。在姿态估计期间，我们使用场景中具有最佳匹配的视图。
2)颜色校准:本实验的一个重要实际考虑因素是，所提供的对象模型中的颜色表示与我们设置中捕获的场景之间可能存在差异。这是因为对象/场景模型是在不同的照明条件下捕获的。由于对尝试复制捕捉对象时使用的照明条件不感兴趣，设计了一个简单的解决方案来对齐颜色空间。
通过估计一个3乘3的矩阵 A ，可以转换存储的对象模型的颜色值，使它们与我们的颜色条件一致。通过在物体模型和包含该物体的实验室场景之间标记一组对应的像素RGB值估计A。A有九个自由参数，我们用了十个彩色像素来估计。得到的颜色校准矩阵给出了使用ECV上下文描述符获得有效对应所需的颜色空间对齐。
图7展示了两个对象的姿态估计结果。为了完整起见，我们展示了一个对象的原始版本和颜色校准版本，这在估计过程中使用。估计的物体姿态对于通过抓取的机器人操作来说足够精确。
图7姿势估计结果来自我们的设置。左:输入Kinect场景。右上:输入纹理化的KIT对象(最左边的实例)和它们的颜色校准版本(最右边的实例)。右下:两个对象的估计结果，对齐的对象用红色覆盖。
5. 结论和未来工作
提出了一种用于精确估计两个3D模型之间的对准姿态的系统。模型描述基于外观和形状数据。使用早期认知视觉系统，将输入数据分成边缘和中间的独立纹理域。通过在这些视觉形式上定义上下文描述符，实现了对场景的高度区分性解释。
为了以有效的方式利用我们的表示，提出了一个基于RANSAC的估计过程，该过程基于使用低级几何约束快速拒绝异常对应。我们的实验表明，这使得估计过程几乎比标准RANSAC快15倍，而不损害估计精度。
通过定量评估表明，我们的描述符在图像域和形状域中的表现几乎是现有描述符的两倍，因此使我们的基于上下文的描述符具有高度的区分性。对于十个非常不同的对象类，我们获得了27 %的平均对应分数。最后，通过两个实际设置中的精确校准结果，展示了我们系统的实际可用性。该系统目前正在我们研究所的机器人抓取应用中使用，将来我们计划使用我们的系统在多个摄像机和机器人之间进行精确校准。
在未来的工作中，应该进行彻底的调查，以优化ECV特征的上下文描述符的公式。在这项工作中，我们选择了一种简单的组织程序设计方法，结合了简单的角度和RGB关系。虽然这已被证明是有效的，但上下文描述符无疑可以通过替代的基于几何和外观的差分度量来改进，可能使用目前仅用于局部图像结构分类的局部幅度、方向和相位。
代码
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
// Types
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointCloudT;
typedef pcl::FPFHSignature33 FeatureT;
typedef pcl::FPFHEstimation<PointNT,PointNT,FeatureT> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;
typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> ColorHandlerT;
// Align a rigid object to a scene with clutter and occlusions
int
main (int argc, char **argv)
{
// Point clouds
PointCloudT::Ptr object (new PointCloudT);
PointCloudT::Ptr object_aligned (new PointCloudT);
PointCloudT::Ptr scene (new PointCloudT);
FeatureCloudT::Ptr object_features (new FeatureCloudT);
FeatureCloudT::Ptr scene_features (new FeatureCloudT);
// Get input object and scene
if (argc != 3)
{
pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
return (1);
}
// 加载目标物体和场景点云
pcl::console::print_highlight ("Loading point clouds...\n");
if (pcl::io::loadPCDFile<PointNT> (argv[1], *object) < 0 ||
pcl::io::loadPCDFile<PointNT> (argv[2], *scene) < 0)
{
pcl::console::print_error ("Error loading object/scene file!\n");
return (1);
}
// 下采样
pcl::console::print_highlight ("Downsampling...\n");
pcl::VoxelGrid<PointNT> grid;
const float leaf = 0.005f;
grid.setLeafSize (leaf, leaf, leaf);
grid.setInputCloud (object);
grid.filter (*object);
grid.setInputCloud (scene);
grid.filter (*scene);
// 估计场景法线
pcl::console::print_highlight ("Estimating scene normals...\n");
pcl::NormalEstimation<PointNT,PointNT> nest;
nest.setRadiusSearch (0.01);
nest.setInputCloud (scene);
nest.compute (*scene);
// 特征估计
pcl::console::print_highlight ("Estimating features...\n");
FeatureEstimationT fest;
fest.setRadiusSearch (0.025);
fest.setInputCloud (object);
fest.setInputNormals (object);
fest.compute (*object_features);
fest.setInputCloud (scene);
fest.setInputNormals (scene);
fest.compute (*scene_features);
// 实施配准
pcl::console::print_highlight ("Starting alignment...\n");
pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align;
align.setInputSource (object);
align.setSourceFeatures (object_features);
align.setInputTarget (scene);
align.setTargetFeatures (scene_features);
align.setMaximumIterations (50000); // 采样一致性迭代次数
align.setNumberOfSamples (3); // 创建假设所需的样本数
align.setCorrespondenceRandomness (5); // 使用的临近特征点的数目
align.setSimilarityThreshold (0.9f); // 多边形边长度相似度阈值
align.setMaxCorrespondenceDistance (2.5f * 0.005); // 判断是否为内点的距离阈值
align.setInlierFraction (0.25f); //接受位姿假设所需的内点比例
{
pcl::ScopeTime t("Alignment");
align.align (*object_aligned);
}
if (align.hasConverged ())
{
// Print results
printf ("\n");
Eigen::Matrix4f transformation = align.getFinalTransformation ();
pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
pcl::console::print_info ("\n");
pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
pcl::console::print_info ("\n");
pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());
// Show alignment
pcl::visualization::PCLVisualizer visu("点云库PCL学习教程第二版-鲁棒位姿估计");
int v1(0),v2(0);
visu.createViewPort(0,0,0.5,1,v1);
visu.createViewPort(0.5,0,1,1,v2);
visu.setBackgroundColor(255,255,255,v1);
visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene",v1);
visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned",v1);
visu.addPointCloud(object,ColorHandlerT (object, 0.0, 255.0, 0.0), "object_before_aligned",v2);
visu.addPointCloud(scene,ColorHandlerT (scene, 0.0, 0.0, 255.0), "scene_v2",v2);
visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"scene");
visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"object_aligned");
visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"object_before_aligned");
visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"scene_v2");
visu.spin ();
}
else
{
pcl::console::print_error ("Alignment failed!\n");
return (1);
}
return (0);
}参考文献
向上滑动阅览
[1] R. Osada, T. Funkhouser, B. Chazelle, and D. Dobkin, “Shape distri-butions,” ACM Transactions on Graphics (TOG), vol. 21, pp. 807–832,Oct. 2002.
[2] D. Lowe, “Object recognition from local scale-invariant features,” in IEEE International Conference on Computer Vision (ICCV), vol. 2,pp. 1150 –1157, 1999.
[3] S. Belongie, J. Malik, and J. Puzicha, “Shape matching and object recognition using shape contexts,” IEEE Transactions Pattern Analysis and Machine Intelligence (PAMI), vol. 24, pp. 509–522, Apr. 2002.
[4] K. Mikolajczyk and C. Schmid, “A performance evaluation of local descriptors,” IEEE Transactions Pattern Analysis and Machine Intel-ligence (PAMI), vol. 27, no. 10, pp. 1615–1630, 2005.
[5] N. Dalal and B. Triggs, “Histograms of oriented gradients for human detection,” in IEEE Conference on Computer Vision and Pattern Recognition (CVPR), vol. 1, pp. 886–893, 2005.
[6] H. Bay, T. Tuytelaars, and L. V . Gool, “Surf: Speeded up robust features,” in IEEE European Conference on Computer Vision (ECCV),May 2006.
[7] A. E. Johnson and M. Hebert, “Using spin images for efficient object recognition in cluttered 3D scenes,” IEEE Transactions Pattern Analysis and Machine Intelligence (PAMI), vol. 21, pp. 433–449, May1999.
[8] G. Hetzel, B. Leibe, P . Levi, and B. Schiele, “3D object recognition from range images using local feature histograms,” in IEEE Confer-ence on Computer Vision and Pattern Recognition (CVPR), vol. 2,pp. II–394–II–399, 2001.
[9] A. Frome, D. Huber, R. Kolluri, T. Bulow, and J. Malik, “Recognizing objects in range data using regional point descriptors,” in IEEE European Conference on Computer Vision (ECCV), May 2004.
[10] R. B. Rusu, N. Blodow, and M. Beetz, “Fast point feature histograms(FPFH) for 3D registration,” in IEEE International Conference on Robotics and Automation (ICRA), pp. 3212 –3217, May 2009.
[11] P . Bariya and K. Nishino, “Scale-hierarchical 3D object recognition in cluttered scenes,” in IEEE Conference on Computer Vision and Pattern Recognition (CVPR), pp. 1657 –1664, June 2010.
[12] B. Drost, M. Ulrich, N. Navab, and S. Ilic, “Model globally, match locally: Efficient and robust 3D object recognition,” in IEEE Confer-ence on Computer Vision and Pattern Recognition (CVPR), pp. 998–1005, June 2010.
[13] N. Gelfand, N. J. Mitra, L. J. Guibas, and H. Pottmann, “Robust global registration,” in Proceedings of the third Eurographics symposium on Geometry processing, 2005.
[14] R. B. Rusu, N. Blodow, Z. C. Marton, and M. Beetz, “Aligning point cloud views using persistent feature histograms,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),pp. 3384–3391, 2008.
[15] S. M. Olesen, S. Lyder, D. Kraft, N. Krüger, and J. B. Jessen, “Real-time extraction of surface patches with associated uncertainties by means of kinect cameras,” Journal of Real-Time Image Processing,pp. 1–14, 2012.
[16] F. Pilz, N. Pugeault, and N. Krüger, “Comparison of point and line features and their combination for rigid body motion estimation,” in Statistical and Geometrical Approaches to Visual Motion Analysis,vol. 5604, pp. 280–304, 2009.
[17] R. Detry, N. Pugeault, and J. Piater, “A probabilistic framework for 3D visual object representation,” IEEE Transactions Pattern Analysis and Machine Intelligence (PAMI), vol. 31, no. 10, pp. 1790–1803, 2009.[18] G. Kootstra, M. Popovi´ c, J. A. Jørgensen, K. Kuklinski, K. Miatliuk,
D. Kragic, and N. Krüger, “Enabling grasping of unknown objects through a synergistic use of edge and surface information,” Interna-tional Journal of Robotics Research (IJRR), vol. 31, no. 10, pp. 1190–1213, 2012.
[19] M. Fischler and R. Bolles, “Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography,” Communications of the ACM, vol. 24, no. 6, pp. 381–395, 1981.
[20] M. Felsberg and G. Sommer, “The monogenic signal,” IEEE Transac-tions on Signal Processing, vol. 49, pp. 3136–3144, December 2001.
[21] M. Felsberg, S. Kalkan, and N. Krüger, “Continuous dimensionality characterization of image structures,” Image and Vision Computing,vol. 27, pp. 628–636, 2009.
[22] N. Pugeault, F. Wörgötter, and N. Krüger, “Visual primitives: Lo-cal, condensed, and semantically rich visual descriptors and their applications in robotics,” International Journal of Humanoid Robotics (Special Issue on Cognitive Humanoid Vision), vol. 7, no. 3, pp. 379–405, 2010.
[23] D. Nistér, “Preemptive ransac for live structure and motion estimation,”Machine Vision and Applications, vol. 16, no. 5, pp. 321–329, 2005.
[24] O. Chum, J. Matas, and J. Kittler, “Locally optimized ransac,” in DAGM Symposium, pp. 236–243, 2003.
[25] O. Chum and J. Matas, “Matching with PROSAC — progressive sample consensus,” in IEEE Conference on Computer Vision and Pattern Recognition (CVPR), pp. 220–226, 2005.
[26] M. Muja and D. G. Lowe, “Fast approximate nearest neighbors with automatic algorithm configuration,” in International Conference on Computer Vision Theory and Applications (VISAPP), pp. 331–340,2009.
[27] D. G. Lowe, “Distinctive Image Features from Scale-Invariant Key-points,” International Journal of Computer Vision (IJCV), vol. 2,no. 60, pp. 91–110, 2004.
[28] R. B. Rusu and S. Cousins, “3D is here: Point Cloud Library (PCL),” in IEEE International Conference on Robotics and Automation (ICRA),May 9-13 2011.
[29] P . J. Besl and N. D. McKay, “A method for registration of 3-d shapes,”IEEE Transactions Pattern Analysis and Machine Intelligence (PAMI),vol. 14, pp. 239–256, Feb. 1992.
[30] Z. Zhang, “Iterative point matching for registration of free-form curves and surfaces,” International Journal of Computer Vision (IJCV),vol. 13, pp. 119–152, Oct. 1994.
[31] K. Lai, L. Bo, X. Ren, and D. Fox, “A large-scale hierarchical multi-view rgb-d object dataset,” in IEEE International Conference on Robotics and Automation (ICRA), pp. 1817–1824, May 2011.
[32] H. Shum and R. Szeliski, “Systems and experiment paper: Construc-tion of panoramic image mosaics with global and local alignment,”International Journal of Computer Vision (IJCV), vol. 36, no. 2,pp. 101–130, 2000.
[33] M. Brown and D. Lowe, “Recognising panoramas,” in IEEE Interna-tional Conference on Computer Vision (ICCV), pp. 1218–1225, 2003.
[34] A. Kasper, Z. Xue, and R. Dillmann, “The kit object models database:An object model database for object recognition, localization and manipulation in service robotics,” International Journal of Robotics Research (IJRR), vol. 31, no. 8, pp. 927–934, 2012.
本文代码在PCL中已经实现，代码及实例数据由本文编译作者点云侠提供，有兴趣的点云小伙伴可在后台发送“知识星球”按要求申请加入星球，免费加入下载代码和实例数据。
如果你对本文感兴趣，请后台发送“知识星球”获取二维码，务必按照“姓名+学校/公司+研究方向”备注加入免费知识星球，免费下载pdf文档，和更多热爱分享的小伙伴一起交流吧！