3.3 IO精通级实例解析
3.3.1 ROS下进行点云数据实时获取与可视化
1.ROS下软硬件环境的搭建
以Ubuntu14.04、Indigo、Xtion Pro或Kinect1为环境。ROS、Indigo的安装请参考第二章相关章节，本节主要讲解ROS下用Xtion Pro进行点云数据获取的驱动以及相关模块的安装。点云数据获取的驱动以及相关模块安装请参考第二章。
2.ROS下点云存储模块的开发
创建package
本例源码参见第3章例9文件夹。创建实现点云存储的package，例如my＿savecloud＿tutorial，这个package依赖于pcl＿conversions、pcl＿ros、std＿msgs、roscpp和sensor＿msgs，具体操作参考第二章相关内容。在src下创建相应的node，例如save＿cloud.cpp，在save＿cloud.cpp中添加以下头文件。
回调函数
这里将点云存储为PCD格式，PCD格式的数据支持两种数据类型存储：ASCII码和BinaryCompressed（二进制）。这里使用BinaryCompressed数据类型，因其占据的存储空间小，便于提高数据存储的速度。因此，这里在回调函数中使用pcl::io::savePCDFileBinary函数实现点云存储。
主函数
在main函数中初始化ROS，并创建关于包含点云信息topic的订阅者。
编译并运行该程序
具体编译操作请参考第二章相关内容。运行该程序，首先在单独的终端中，运行roscore。
然后在新打开的两个终端里分别运行以下命令。
在运行save＿cloud node 的终端里将会输出每个PCD文件（存储的点云）的路径以及文件名，如图3-12所示，点云保存ROS模块如图3-13所示。
图3-12 save＿cloud node终端输出
图3-13 点云保存ROS模块
3.ROS下点云数据的实时可视化
此处将利用可视化应用程序RViz的可视化小部件编写应用程序，来实现点云数据的实时可视化。本例源码参见第3章例9文件夹。
首先，在指定路径下新建一个名为myviz的Package，如图3-14所示。
图3-14 新建package并输入代码
参考网站 http：//docs.ros.org/lunar/api/librviz＿tutorial/html/index.html，在新创建的 Pack-age下编写CMakeLists.txt、myviz.h、myviz.cpp等文件。
编写myviz.h文件时，关键在于定义的MyViz类要含有如下私有成员。
其中，VisualizationManager类型指针 manager＿用来创建 rviz 内已有的 display；render＿panel＿指针用于构建和布置渲染面板；Display类型指针pc＿即我们要显示的PointCloud2类型的display对象。
接下来我们要编写myviz.cpp文件。其关键步骤为初始化主要的RViz类，这里利用的VisualizationManager是Display对象的容器，用于保存主要的Ogre场景和ViewController等。
这里要注意的是，一定要先利用setFixedFrame函数改变Global Frame下FixedFrame的类型为“/camera＿rgb＿frame”，用于显示PointCloud2类型对象。
现在我们可以开始创建PointCloud2类型对象。
值得注意的是，PointCloud2类型对象要为其订阅一个topic，这里用到了Display：：setTopic（const QString＆topic，const QString＆datatype）函数，其中datatypee不是直接用displays的类型，而是＂sensor＿msgs/PointCloud2＂。
创建好所有文件后，先启动摄像头，输入如图3-15所示的代码。
图3-15 启动摄像头并输入代码
最后运行我们的可视化小部件，输入如图3-16所示的代码。
图3-16 运行可视化小部件并输入代码
此时可以看到应用程序里实时的可视化点云数据，如图3-17所示。
图3-17 点云实时可视化模块
3.3.2 自选设备scanCONTROL与PCL实时获取与可视化点云数据
项目中常常会遇到对自选点云获取设备进行集成开发应用的情况，所以本节以高端工业自动化轮廓扫描仪scanCONTROL作为自选设备，对其获取的点云数据进行实时获取并可视化。在笔者项目组的高精度三维点云数据获取系统研发中，涉及对第三方点云获取设备进行封装的案例，下面对关键模块分别进行分析。
1.开发需求与最终产品原型简介
本项目是为了对加工模型与设计模型进行对比，以达到质量检测或高精度加工的需要。基于线扫描设备，首先对线扫描的数据进行实时获取，同时与PLC通信得到机器人手臂位姿，进而同步对线扫描点云数据进行统一得到最终的三维数字化模型，完成对工件的高精度扫描，再通过与CAD设计的三维模型进行配准，通过配准误差来做出质量评估或高精度加工的决策，具体的软件架构如图3-18所示。
图3-18 软件架构
2.自选设备的驱动封装模块设计
为了利用Qt的signal和slot的机制，我们设计类LineScanner来对ScanCONTROL设备的驱动接口进行封装，其主要是通过类LineScanner中的函数StartCapturing()，实现对回调函数的注册，同时通过this指针，将对象作为用户数据传递给回调函数，在回调函数中，可以将设备获取的实时数据，通过this指针传递给当前实例对象。
在回调函数中，通过强制转换，将pUserData转换为LineScaner对象，进而做到对驱动的线程空间与用户的线程空间之间的数据交换，同时将获取的数据相关属性赋值给LineScaner 对象。这里回调函数就只进行了数据的拷贝，同时将获取数据的事件，通过调用plinescaner-＞Fire＿new（）函数执行emit New＿Frame＿comed（），发出该signal，以通知其他关注该signal 的对象的slot函数操作，以达到对实时获取数据的实时处理，例如对每帧数据的实时可视化等。
为了模块化设计，提高源码的重用性，我们对点云库中的不同数据类型的加载函数进行了封装，定义到一个类IOModel中，并将其作为一个动态链接库使用，这里是加载PLY数据的类成员函数定义，主要是通过emit Mesh＿ready（）的signal来实现该类实例对象与其他对象之间的消息传递。
动态链接库的封装，请参考本书后续章节中使用CMake来管理动态链接库封装和使用的实例。
3.源码与软件系统测试
由于商业原因，本案例只能提供与ScanCONTROL设备相关的封装类模块，具体源码见本章例10文件夹。我们结合PCL+Qt以及第三方提供的驱动和工业PLC提供的底层接口，实现了对ScanCONTROL获取的点云数据进行实时显示，利用PLC通过ADS传递的运动轨迹进行多次线扫描数据的对齐到统一坐标系中，设计的3D曲面模型导入（PLY、STL格式），利用线扫描配准后的点云数据模型与导入的3D模型进行配准进而完成位姿估计，可视化伪彩色显示扫描点云误差等功能。高精度三维点云数据获取系统运行界面如图3-19所示。
图3-19 高精度三维点云数据获取系统
3.3.3 利用Tango进行点云数据获取
1.RTAB-Map和Tango的介绍
RTAB-Map (Real-Time Appearance-Based Mapping)是一种以RGBD数据为输入，基于增量外观闭环检测的SLAM构造地图的实现。闭环检测利用词袋法来判断一幅新图像是属于上一定位点还是新的定位点的可能性，当一个闭环假设被接受时，一个新的约束被添加到该图中，然后一个图优化器将会最小化图中的误差。同时用内存管理的方法来限制闭环监测和图优化的定位点数，以便于能够满足在大规模场景中约束条件的实时限制。RTAB-Map能被单独用于手持式Kinect或者6DoF立体相机测绘，也能在配备激光测距仪的机器人上进行3DoF测绘。
Tango是Google的一个AR增强现实项目，配合其独特的移动设备和SDK可以方便地在应用中使用AR技术。Tango所采用的技术主要是三个方面，首先是运动追踪技术。兼容Tango的设备能够明白自己的位置，以及在空间内的移动方式和方向。这些都是使用运动追踪摄像头、3D深度传感器、加速度计、气压计、陀螺仪和GPS来实现的。第二、三个方面分别是深度感知和区域理解（Area Learning），这让设备能够以很高的精度来了解用户身处的区域。当这三个方面结合，用户在通过狭窄走廊并走过转角的时候，设备就能记住用户从哪里来，并追踪他朝哪里去，甚至能够获悉墙壁的位置，以及用户和物体的距离。本书中使用联想的Phab 2 Pro手机作为Tango的设备。
2.RTAB-Map在Tango直接进行点云数据获取
实验准备
本实例使用联想Phab 2 Pro手机，系统为Android 6.0.1，在手机上下载安装RTAB-Map的APP，实验下载版本为0.11.11（23）（参考https://play.google.com/store/apps/details?id=com.introlab.rtabmap）。
数据获取
在手机上打开安装好的RTAB-Map应用程序，可看到绘制着网格的空三维场景，RTAB-Map通过Tango手机自带的深度摄像头，利用红外光反射原理，可以获取所需要的点云数据，同时还会记录行进轨迹与位姿，运行结果如图3-20至图3-22所示。
图3-20 RTAB-Map运行界面1
图3-21 RTAB-Map运行界面2
图3-22 RTAB-Map运行界面3
因为Tango采用的是“参考定位”，即相对于“初始位置”的一种定位方式，不涉及卫星定位。它根据硬件设备的传感器，比如重力传感器、IMU陀螺仪等，获取移动设备相对于初始位置的“位移”和“旋转角度”，自己构建了一个“参照坐标系”，因此有较高的准确度。RTAB-Map利用Tango提供的点云数据和位姿数据，同时结合图像特征实现SLAM，并对重建的地图数据进行实时可视化。数据获取完毕单击右上角图标，进入菜单栏，单击Pause停止获取点云数据，再单击Export...，选择导出格式（目前支持PLY和OBJ）。
本案例中选择导出PLY文件格式，可在文件管理器中查看数据，之后我们将数据导出到电脑，利用MeshLab查看所获取到的数据，至此就完成了利用Tango设备进行室内点云数据的获取，读者可以利用该数据作进一步的应用处理。在MeshLab下可视化的三维重建结果如图3-23所示。
图3-23 可视化的三维重建结果
3.RTAB-Map在ROS上通过类Kinect设备进行点云数据获取
本小节参考http://wiki.ros.org/rtabmaps。
直接安装
首先在Ubuntu上安装RTAB-Map，根据不同的ROS版本选择不同的方式安装。
Lunar
Kinetic
Jade
Indigo
Hydro
注意，rtab-map＿ros Hydro系统的文件版本停留在0.8.12。要使用最新版本请参阅后文中源代码的安装方法，当加载rtabmap＿ros节点时，如果出现错误error while loading shared libraries...，则将下列代码加入～/.bashrc 文件结尾，将动态库的路径添加到环境变量，此处的路径可根据读者自己安装的ROS版本进行相应调整。
从源代码安装
在此将介绍如何在ROS Hydro/Indigo/Jade/Kinetic/Lunar（Catkin build）上从源代码安装RTAB-Map ros-pkg。RTAB-Map仅适用于PCL 1.7以上版本，在ROS Hydro/Indigo/Jade/Kinetic/Lunar中默认安装（不支持Fuerte和Groovy）。之后的例子假定在已建立ROS工作空间的基础上，这里是按照Kinetic版本安装，可以替换成你所使用的版本，如Hydro、Indigo、Jade、Lunar等。工作空间路径在～/catkin＿ws，同时～/.bashrc文件中包含如下信息。
安装RTAB-Map ros-pkg的依赖库
获取所有文件(Qt，PCL，VTK，OpenCV，...)的最简单方法是安装/卸载rtabmap二进制文件。
编译安装RTAB-Map
下载RTAB-Map的ros-pkg源码在Catkin工作空间下的src文件夹中，并用catkin＿make进行编译安装。
运行rtabmap
下面根据你的硬件与环境选择不同的命令来运行rtabmap。
Kinect for Xbox 360：
或者：
Xtion PRO Live：
Kinect v2 (Xbox One)：
启动rtabmapviz：
本实例使用华硕Xtion PRO Live，启动rtabmap得到的正常结果如图3-24所示。
图3-24 启动rtabmap得到的正常结果
如果背景变为红色，如图3-25所示，则表示里程计丢失了，无法计算测距。造成这个问题主要有以下几个因素。
图3-25 里程计丢失示意图
（1）无特征环境：白色墙壁，均匀纹理，黑暗区域等。因为没有足够的特征，所以无法计算估计位姿。
（2）空旷环境：对于像Kinect这样的传感器，良好的特征在传感器的0.4米至4米范围内。例如，如果您将传感器指向4米以内没有物体的区域，测距仪将很有可能失去跟踪。
（3）帧速率相机运动过快：如果相机移动速度过快，则连续帧之间的匹配功能会减少。根据相机的不同，RGB图像也可能会模糊不清，这会减少帧之间良好的视觉匹配次数。
（4）深度图像和RGB图像之间的错位：如果视觉特征的深度错误，则会对里程计造成负面影响，应确保深度图像正确地注册到对应的RGB图像。
要从丢失的测距中恢复，还可以使用“Detection-＞Reset odometry”（ROS上的$rosservice call/reset＿odom）重置测距。在复位之前，不要忘记更换为具有大量纹理的场景，否则测距法将无法启动（红色背景将保留在此处）。重置里程计将使RTAB地图创建一个新的地图，所以原地图应该消失，并显示一个新的地图。
3.3.4 基于Structure from Motion的点云数据获取
1.Structure from Motion简介
Structure from Motion（运动推断结构，简称SfM）一词最早在1985年由Hartley等人提出，是一种基于多视图几何基本原理的三维重建技术，旨在利用一系列包含视觉运动信息（motion signals）的二维图像或视频序列，恢复摄像机的运动信息，以及重建三维场景的点云模型，如图3-26所示。SfM技术现今已成为计算机视觉以及可视化研究范围内的一个核心问题。
图3-26 SfM技术示意图
自提出以来，SfM的发展主要经历了以下5个阶段。
1）1990年之前的理论研究期，以多视图几何理论的发展和建立为代表。
2）1990-1999年之间的初级实践期，以Hartley等人的研究工作为代表，开始在先前的理论基础之上开发SfM系统。但囿于当时计算条件的限制，没有在实践中得到应用。
3）1999-2007年的低谷期。Tomasi等人在1999年发表论文《Is Structure from motion worth pursing》，系统论述了SfM技术在当时的局限性。之后，SfM技术相关的研究在学术界逐渐受到冷落。
4）2007年，Snavely等人在ACM SIGGRAPH2017会议上发表了Photo touorism论文及Bundler系统，证实了SfM技术的可行性，在学术界再次崛起。
5）2010年起，由于廉价图像设备的普及，网络图像大数据出现。加之GPU计算能力的提高，同时解决了发展SfM技术所需的数据和计算资源问题。受惠于此，SfM技术的研究开始呈现出爆发式的增长。
图3-27 Bundler系统
一般可将SfM技术按设计方法的不同分为增量式SfM和全局式SfM两种。增量式SfM一般先选取数据集中两张图像，从exif卷标中获取焦距等信息，利用五点算法、三角定位法等算法估计摄像机的相对位置，回溯二维特征点在三维空间中的具体位置，从而实现点云模型的匹配重建。这一初始过程结束后，再加入新的二维图像进行测算、优化，重复上述过程直至所有图像处理完毕，生成三维点云。全局式SfM则采用一次性将所有图像全部加入重建队列的方法，一次性匹配所有图像的特征点。前者精度高效率低，后者则效率高精度低。一般来说，用于处理视频数据的SfM均使用增量式的结构设计，对于处理图像数据的SfM，两种结构都可以使用。目前大部分SfM系统采用的都是增量式结构，采用全局式结构的SfM系统主要有2013年的Global SfM，2014年的SG-SfM及2015年的Scalable-SfM等。
2.基于VisualSFM工具的影像数据重建
VisualSFM工具简介
VisualSFM（简称VSFM）是由Changchang Wu等人于2013年在Bundler系统的基础上开发的一套使用增量式SfM进行三维重建的GUI应用程序。重建系统集成了GPU上的SIFT（SiftGPU）、多核束调整和走向运动的线性时间增量结构等几个项目，通过利用多核并行机制进行特征检测、特征匹配和捆绑调整，相较Bundler，实现了VisualSFM的快速运行。
在稠密重建方面，VSFM集成了Yasutaka Furukawa博士团队在2010年开发的PMVS/CMVS工具链。VisualSFM的SfM输出则与其他工具一起工作，其中包括Michal Jancosek的CMP-MVS，Michael Goesele研究小组的MVE，Mathias Rothermel的SURE，Konrad Wenzel的SURE以及Zhuoliang Kang的MESHRecon。VSFM工作窗口如图3-28所示。
下载安装地址：http://ccwu.me/vsfm/
帮助文档地址：http://ccwu.me/vsfm/doc.html#usage
图3-28 VSFM工作窗口
VisualSFM三维重建流程
利用VisualSFM进行二维图像的三位重建，大致可以分为以下几步。
（1）数据采集。
（2）特征检测与图像匹配（Feature detection＆Full pairwise image matching）。
（3）稀疏重建（Sparse reconstruction）。
（4）稠密重建（Dense reconstruction）。
1.数据采集
拍摄照片注意事项
看过科研工作者们如何演示他们的SLAM/SfM系统的话，就会发现，他们都会小心翼翼地摆动手中的摄像头。这是因为，在摄像头的平移和旋转两种运动中，平移运动能够更有效地对特征点进行重建，因为远的特征点在图像坐标中移动慢，而近的移动快。所以在采集图像的过程中，尽可能从不同的位置对物体进行拍摄，同时在相邻照片中保留更多的相同景物。拍摄示例如图3-29所示。
图3-29 拍摄示例
请尽量有序地采集照片，这样在使用VisualSFM进行匹配的时候，不用进行全局匹配，只需要使用“SfM->Pairwise matching->Compute Sequence Match”就可以了。使用这种匹配模式，需要设置一个参数n（笔者使用10，理论上更小的参数应该也可以），这样一来，照片只会和最近的n张照片进行匹配，可以节省很多时间。
VisualSFM只能以JPEG格式的照片作为输入，在读取JPEG文件时，VisualSFM会根据JPEG照片中的EXIF信息计算每一张照片的相机内参，图片EXIF信息界面如图3-30所示。
EXIF信息是相机在拍照的时候自动保存在照片中的，请确保输入的JPEG文件中包含正确的EXIF信息，否则图像匹配和重建的效果会非常差。
焦距初始化（Focal length initialization）
VisualSFM中的焦距初始化是自动完成的。每一张照片的焦距是根据该照片内保存的EXIF信息（EXIF.Focallength and FocalplaneXRes/FocalplaneYRes）自动计算的，而不是根据CCD数据库计算的。所以需要注意：当你要调整照片大小时，请确保照片中的EXIF信息没有丢失。
如果照片中不包含EXIF信息，焦距会被初始化为1.2*max(width, height)。如果是这样，或是照片中的EXIF信息很差，重建效果一般都非常差，如图3-31所示，可以通过设置菜单中的“SfM-> More functions-> Set Fixed Calibration”来为所有照片设置相同且固定的相机内参［fx,cx,fy,cy］。
图3-30 图片EXIF信息界面
图3-31 设置相机内参界面
2.特征检测与图像匹配
（1）通过执行菜单栏中的“SfM->Load NView Match”命令选中一个包含图像库文件夹路径的TXT文档或通过执行“SfM->Load NView Match”命令或单击图3-32中1号按钮直接选中需要导入的图像文件，将处理好的图像导入VisualSFM。
（2）通过执行“SfM->Pairwise Matching->Compute Missing Match”命令或直接单击图3-32所示工具栏上的2号按钮进行图像匹配。
图3-32 Visual SFM 工具条(tool bar)
注意：如果是用手机拍摄的室外场景，在匹配之前勾选“SfM->More Fuctions->Use Shared Calibration”效果更好。因为在室外场景中，物体都比较远，相机内参变化不大。
VSFM同样也提供了指定配对列表进行匹配的方法，详情可以参考官网的说明。图像匹配之后的文件如图3-33所示。
图3-33 图像匹配之后产生的MAT和SIFT文件
3.稀疏重建
通过执行“SfM->Reconstruct Sparse”命令或单击工具栏上的3号按钮即可进行稀疏重建，重建效果如图3-34所示。
按照上述步骤进行稀疏重建后，理论上可以得到很好的模型。如果结果产生了多个模型，可能有两个原因：一个是各个模型之间没有共享足够多的图像特征点；另一个原因比较少见，是因为初始化不够好。要想把多个模型合成一个，只需执行菜单栏中的“SfM-＞More Functions-＞Merge Sparse Models”命令。log 信息中会提示你各个模型中有多少共享的特征点，如果特征点数量小于程序默认的最小值，则不会进行模型融合。编辑VisualSFM路径下的nv.ini文件，可以修改程序要求的最小值“param＿model＿merge＿min＿matches ”（默认值是100），把该参数改成小于模型间共享特征点数量的一个数（不宜小于10），再执行“SfM-＞More Functions-＞Merge Sparse Models”命令应该就可以融合有共享特征的模型了。如果模型之间一个共享特征都没有，那就只能从头来了。
图3-34 利用VisualSFM重建的农大东区体育馆效果
4.稠密重建
CMVS是日本的Yasutaka Furukawa（华盛顿大学的助理教授）基于SfM提出的多幅图像密集点云的提取方法。VisualSFM支持利用CMVS进行稠密重建。
下载地址：http://www.di.ens.fr/cmvs/
将cmvs.exe、pmvs2.exe、genOption.exe三个文件放进VSFM的根目录下，如图3-35所示，然后通过执行“Sfm->Reconstruct Dense”命令或直接单击工具栏上的CMVS按钮，即可开始进行稠密重建。
图3-35 稠密重建所需文件
此过程会耗费较多的时间，请耐心等待。
结束后将自动生成一个PLY文件，按Tab键即可在VSFM内查看重建效果，重建效果如图3-36和图3-37所示。
图3-36 稠密重建效果
图3-37 将重建后生成的PLY直接在Mashlab中打开的效果
注意：空闲时请务必记得使用“SfM->Save NView Match”命令保存包括3D重建在内的SfM工作进程，VisualSFM默认使用包含所有工作区信息的NVM格式。
输出格式说明
理解NVM（N-View Match）文件最简单的方法就是用TXT文本文件或Sublime Text打开，从而可以直观地看到其组织方式。文件开始每一行是一个照片的参数，参数按空格分割，依次是<File name> <focal length> <quaternion WXYZ> <camera center> <radial distortion>和0。然后每一行是一个点的信息，参数按空格分割依次是<XYZ> <RGB> <number of measurements> <List of Measurements>。一般我们只关心XYZ和RGB，取前6位数据即可。