基于ROS机器人的3D物体识别与三维重建(一) 介绍篇

基于ROS机器人的3D物体识别与三维重建(一） 介绍篇

由来：清理电脑硬盘，发现了当时做毕设的一些资料，所以打算整理一下资料和代码写成专栏，记录下当时的暗金岁月，尽管现在实验室的做的项目已经不是这个方向了。当时保研结束后就一直在搞了，没什么编程理论功底，从头学的C++、Linux 、ROS、python、Opencv、PCL这些编程工具，然后看3D视觉工坊、泡泡机器人、古月居、深蓝学院上的各种资料，当时线代没学好，看高翔的视觉SLAM十四讲看了好久，尤其李群李代数那一块，公式推导真心繁琐，尽管现在看来比较简单，当时还是很煎熬的。

相关的代码资料：
https://github.com/Rayso777（后续会陆续整理上传）
视频：
1、ElasticFusion TUM数据集＆buntu16.04+kinect2演示流程.
2、RTAB-MAP实时三维重建-Kinect2
3、RTAB-MAP三维重建-基于gazebo仿真
4、ORB-SLAM2室内稀疏三维重建-基于gazebo仿真
5、ORB-SLAM2室外三维重建-gezebo仿真
6、三维重建视频集锦-视觉SLAM

一、专栏大纲介绍

3D物体识别和三维重建是机器人完成上层功能任务的基础，对机器人后续功能任务的完成具有重要意义。该系列专栏，主要对以下几个方面进行介绍。

• 1、相机的成像模型和畸变修正，通过张氏标定法实现对Kinect2相机的标定，并完成了点云数据的获取。
• 2、通过基于ORB特征的二维模板匹配和基于LINEMOD算法的三维模板实现了对3D物体的识别，并针对桌面物体检测这类特定问题进行代码实现。（当然，现在可以用pointnet++三维识别系列或者yolo的二维系列啦，当时刚接触用的最原始的方法）
• 3、基于Turtlebot仿真机器人和Gazebo搭建三维重建的仿真环境，实现了ORB-SLAM2稀疏重建和RTAB-MAP稠密重建，并通过Kinect2实现了基于ElasticFuison算法的实时三维重建。

(二) Kinect2相机标定与点云数据获取
（三）gazebo仿真环境搭建
（四）基于ROS的3D物体识别
（五）ORB-SLAM2与RTAB-MAP三维重建
（六）ElasticFusion实时重建

二、ROS介绍

ROS（Robot Operating System）原本是斯坦福大学与Willow Garage公司合作的一个机器人项目，后由Willow Garage公司对其进行推动。现在是OSFR的开源项目，并对其进行维护。ROS最初的目标是提高在机器人研发过程中软件代码的复用率，作为一个操作系统，它对机器人的各种硬件设备诸如激光雷达、RGBD传感器等进行了封装，不同的硬件设备可以通过topic、service、action等与Master进行通讯，以便上层的应用程序进行调用,实现运动规划、SLAM、物体识别等功能。

基于ROS开发的机器人有很多，从用于实验室研究的PR2、NAO、turtlebot系列到工业机器人的四大家族ABB、KUKA、FANUC、YASKAWA用于产品的研发，ROS从学术研究到商业应用，发展十分迅速。

这主要得益于ROS以下的几个特点：

• 1、采用松耦合的架构：采用了点对点的通讯，将各个程序作为结点Node，便于模块化的修改调整。
• 2、支持多种语言：ROS支持Python、C++等多种语言，可以很方便的进行开发，提高效率。
• 3、功能包丰富：ROS集成了很多开源的功能包，像PCL、OpenCV之类的视觉库，还有rviz、rqt、
  gazebo等可视化与仿真工具，以及MoveIt运动规划库和众多的SLAM算法等。可以很方便的进行商业开发和研究，不需要进行重复的造轮子工作，用户只需要针对自己感兴趣的部分进行修改调整即可，十分适合研究者进行原型设计，把更多的时间投入到创造性的工作当中。

目前，已经提出了ROS2.0版本，主要用于商业应用，解决ROS1.0实时性差的问题。随着ROS2.0的完善，相信会有更多的研究者和商业开发采用ROS进行工作，它已经成为了机器人开发的主要趋势。

三、三维物体识别介绍

机器人的3D物体识别通常会通过相机或其他深度获取设备从世界环境中感知信息、自动识别和定位目标的过程，并可以对目标进行位姿估计。

对于机器人的三维物体识别系统而言，必须解决以下问题：
（1）通过传感器获取原始数据；
（2）寻找一种可以对物体构建模型的方法；
（3）人工或者通过机器学习寻找模型描述特征；
（4）寻找一种匹配方法对输入的数据和数据库的模型进行匹配。

而对于机器人而言，成功识别出物体只是最基本的内容，同时还需要对物体的尺寸大小和6自由度位姿进行估计，以便为机器人的上层功能提供基础决断信息，这对机器人的物体抓取放置、避障等有着重大意义。

3D物体识别和位姿估计主要分为以下四类：
1、模板匹配( Template Matching )；
2、形状匹配( Shape Matching )；
3、点云配准( Point Cloud Registration)；
4、机器学习( Machine Learning )。

模板匹配
模板匹配方法中十分著名的便是2012年 Hinterstoisser提出的LINEMOD算法，模板由目标物体颜色边界特征和表面法向特征组成，不依赖纹理信息，可以对弱纹理物体进行位姿估计。
2015年Yoshinori提出了旋转累加特征，定义了一种新型模板，它由面内微小角度旋转的模板加权得到，使得匹配过程可以容忍一定旋转角度的误差。
Hodan提出了一种由多个三元组组成的模板，通过在待识别物体的深度图建立格点，随机选取的三个格点并计算相应的表面法向量和各自深度之间的差值组成模板的三元组，而且还结合了哈希编码和哈希投票增加了算法的实时性。
形状匹配中，Drost在2010年提出的PPF(Point Pear Feature)算法十分具有开创意义，它是一种基于有向点对特征的3D形状描述子，主要通过点对之间的特征关系对物体6D姿态进行假设投票。
Choi在2012年点对特征进行了改进，向其中添加了RGB信息，因此在非结构化的环境中鲁棒性得以提升。

点云配准
点云配准主要分为局部特征和全局特征。

上图展示了最为经典的三种主流点云配算法原理，同时包括了局部特征和全局特征。

局部特征是指仅对物体表面某部分提取设计的的几何方法提取特征，全局特征是对目标点云簇构建一个基于全体模型的特征，同二维图像的识别原理类似，3D物体识别也是通过提取特征进行匹配识别，不同的是二维图像的特征是基于平面的，而3D物体识别是基于三维物体提取特征。

点特征直方图（Point Feature Histogram , PFH）描述子为很多的研究工作提供了灵感，该算法有很好的识别能力，但是特征提取时的计算时间很长，实时性方面受到了限制。

随后，在PFH的基础上，提出了快速点特征直方图（Fast Point Feature Histogram,FPFH），不但保留了前者高性能的识别能力，还缩短的特征的计算时间。

上图中的VFH[描述子是基于全局特征的，它是将相机视点与所有的点对进行信息统计，该方法的计算时间远远降低，在实时的应用中有很好的效果。除了上述的三种主要特征描述子外，近些年来研究者们基于这些最初的工作，研发了许多基于全局和局部的特征描述，如下图所示。

机器学习
关于基于机器学习的物体识别，主要是通过SVM以及统计学习、随机森林和霍夫投票相结合对RGB或者RGBD图像进行特征提取预测，还有通过深度学习与点云相融合进行特征提取从而进行物体识别的方法。

深度学习如AlexNet、GoogleNet、VGG、ResNet等等等大量网络模型相继出现，但这都是针对于二维图像进行的工作。（现在shufflenet、mobolenet、sqeezenet越来越多的backbone被提出来，而且Transformer还来个Vision Transformer跨界，好多研究者基于transformer进行CV任务，效果还可以，不过优点难训练。前几天刚看到何凯明大神出了一篇MAE,解决transformer大模型训练的问题，感觉transformer NLP CV audio要大一统了？）

随着深度获取设备的普及应用，PoseCNN、PointNet和PointRCNN等针对点云物体识别的深度学习模型也相继问世。（现在有Point-GNN网络，将图神经网络用于点云数据的任务中，还都取得了SOTA的效果，多用于点云分割）

下图展示了深度学习中PoseCNN的算法流程。

其中基于模型和模板的模板匹配算法受到模板数据库里面的物体种类和数量限制，并且只是通过阈值来判断物体检测结果，在杂乱堆叠的环境中还不够稳定。形状匹配算法虽然在一定程度上可以提高非结构化环境的鲁棒性，但在实时性方面有所限制。点云配准中点云描述符大多使用点的距离和法向量信息构建局部或者全局特征，因此，对噪声特别敏感，其精确性对点云数据和法向量的估计有很强的依赖。

而且Kinect等RGBD相机获取的点云受光照、遮挡、透明度等影响，点云描述符的质量会有所降低。

应用深度学习和机器学习算法来获取三维物体特征进行物体识别是近年来新兴的研究领域，在二维图像识别上已经取得了显著的成果。

但目前在工业应用上，模板匹配算法，具有较强的稳定性。

随着计算机计算能力的提升和深度学习相关算法的研究，点云深度学习相融合的方法将是未来工业应用的主流。

四、机器人三维重建研究现状

对机器人而言，三维重建主要用于构建场景信息和各类地图（稀疏、半稀疏、稠密），以使得机器人可以知道它在世界环境中的位姿，并且可以感知周围障碍物的位置信息，通过路径规划，从而实现机器人的定位、导航、避障和路径规划，这也就是SLAM（即同时定位与建图）需要解决的问题。

vSLAM建立的地图可以分为稀疏特征点地图、八叉树地图、TSDF（截断符号距离场）地图以及三角网格地图和Surfel地图等。

根据传感器类型的不同，可以分为使用激光雷达的激光SLAM和使用深度相机的视觉SLAM。高精度的多线激光雷达价格昂贵，并且在获取环境信息过程中丢失色彩信息。并且，随着Kinect等便宜的深度获取设备的普及，视觉SLAM也成为了研究热点。

在重建过程中，准确的获取像素点的深度信息是十分重要的，当前的视觉SLAM建图采用的相机深度获取原理如下：

• 1、单目相机移动后进行三角量测以得到像素的距离。
• 2、双目相机通过视差计算像素的距离。
• 3、RGBD通过结构光（如 Kinect v1）或者TOF(如Kinect v2)方法获取距离。

前两种方式称为立体视觉（Stero Vision），单目和双目获取深度信息需要花费大量的计算时间，并且最后得到的深度相较于RGBD并不是十分准确。而RGB-D相机在量程范围、光照影响等方面都有所限制。因此对不同的场景进行三维重建时需要综合选择相机类型。

因为相机的选型也将影响视觉SLAM方案的确定，一般从单目、双目、深度等不同相机的角度切入。

在单目视觉领域，MonoSLAM是具有开创性的第一个单目SLAM系统，之后的许多工作都基于此算法进行开展。它以EKF（即扩展卡尔曼滤波器）作为后端，以稀疏特征点的追踪作为前端。但它存在特征点容易丢失、路标数量有限制，因此适应的场景范围很小。

PTAM（Parallel Tracking and Mapping，它十分重要的一个思想就是将追踪和建图进行了并行处理。并且在后端舍弃了EKF而采用了后端优化的方法，引入关键帧，对待估计的地图和轨迹进行优化。

ORB-SLAM，到达了特征点法实现SLAM的顶峰，它支持单目、双目、RGB-D等多张类型的相机。它采用了多线程结构，包括了视觉里程计、后端与回环检测ORB字典。采用ORB特征是该算法最显著的优势，ORB可以在保证旋转和缩放的不变性同时，还不会像SIFT、SURF等特征一样计算耗时很长。

相较于单目和双目相机实现的SLAM三维重建，RGB-D实现SLAM的的原理要简单的多，并且可以在CPU上很好的实现。RTAB-MAP（Real Time Appearance-Based Mapping）是RGB-D SLAM中十分经典的一个算法，它包含了视觉里程计、回环检测、Pose Gragh的后端优化和点云三角网格地图。并且，自从Kinect等RGB-D深度获取设备诞生以来，KinectFusion、ElasticFusion等Fusion系列RGB-D算法也火热发展。

目前，三维重建的主要研究方向大致分为了稀疏重建和稠密重建两大类别。

接下来的博文，介绍：Kinect2相机标定与点云数据获取