基于ROS机器人的3D物体识别与三维重建(二) Kinect2相机标定与点云获取

相关的代码资料：
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

1、介绍

工欲先利其事，必先利其器。如果说三维物体识别与三维重建是机器人功能任务实现的先验基础，那么准确的相机标定和高质量的点云数据则是影响物体识别精度和重建效果的最重要因素之一。

机器人是一般通过RGB-D相机等深度获取设备采集环境信息， 但这些深度设备在制作装配过程中必然会有制造误差，因此获取的数据不会十分精确。

源数据的误差将在三维重建和物体识别过程地特征提取、匹配等一系列环节进行累积，将严重影响最终的实现效果。因此，深入了解相机的成像过程、对相机进行准确的标定和获取高精度的点云具有重要意义。

该部分将对针孔相机模型及其畸变特性进行原理阐述，随后介绍相机标定的流程，最后通过实验完成 Kinect2相机标定，并完成点云数据的获取。

2 相机成像模型

2.1 针孔相机模型与畸变修正

在进行相机标定之前，需详细了解相机的成像过程。相机成像是将三维世界的空间点映射到二维图像上的像素，最简单普遍的模型就是针孔相机模型，亦称为小孔成像。
（来自视觉SLAM十四讲）

针孔相机模型中涉及了4个坐标系，分别是世界坐标系、相机坐标系、成像坐标系以及像素坐标系。上图描述了各符号代表的物体意义，其中O代表了相机的光心，并以O为坐标原点、光轴作为 轴建立了相机坐标系。同时，以光轴在成像平面的交点作原点构建二维成像坐标系，以图像左上角为原点构建像素坐标系。

设相机坐标系下观测到一点 ，其坐标 ，对应到成像平面上为点 ,坐标为 。由相似三角形原理可得：

变换后得到:

上式为变换到成像平面的坐标，需要进一步的将该坐标转换到像素坐标系内。成像坐标系原点在图像中心，而像素坐标系原点则在图像左上角，并且像素坐标系的图像是成像平面的图像量化缩放后得到的，在横轴和竖轴上差两个缩放系数。设定平面两个方向上的缩放系数为 ，原点平移了 ,那么像素坐标系的坐标 与成像平面上的坐标关系为：



则：

写成矩阵形式

上方包含fx,fy,cx,cy 的矩阵K称为内参数矩阵，通常情况下，相机出厂时厂家会给定这些参数。但为使相机获取高质量的图像，在使用过程中仍需完成标定环节求解这些参数。通过内参的纠正，相机可以获取较好的数据，但却是基于相机坐标系的，对于机器人的操作而言，需要将数据转换到世界坐标系中，因此引入了旋转矩阵R和平移矩阵t等外参将相机在世界坐标系中的姿态进行表述，从而可以将相机坐标系的数据转换到世界坐标系。

相机在制作过程中普遍都会使用透镜，引入透镜会对成像过程产生新的影响，比如透镜会影响光线的传播，而且透镜的安装位置与成像平面的平行度误差会导致小孔成像模型不能够精确描述。因此，我们引入了畸变的概念对小孔成像模型进行修正。

畸变可以分为径向畸变和切向畸变，径向畸变是由于透镜的形状引起的，它可以分为桶形畸变和枕形畸变，如下图:

径向畸变的畸变程度会随着距光轴中心的距离增加，使用多项式函数来对畸变的坐标进行表示，这里引入了 三个参数，对于普通的相机通常只需要使用 两个参数就可以很好的拟合:

切向畸变是由于相机制作过程中透镜中心平面与成像平面之间平行度误差引起的，如下图所示:

对于切向畸变我们可以引入 和 两个参数进行修正：

因此，对于任一相机坐标系下的空间点 ，我们可以将其转换为归一化平面的点 ,联合径向畸变和切向畸变各参数对其进行修正，得到下式：

下式将归一化平面中通过畸变模型修正后的坐标转换到像素坐标系：

可以看出，只要完成对四个畸变参数的求解就可以对相机畸变进行修正。以上便是对针孔相机模型及其畸变修正的探讨。本文采用的Kinect2相机精度较好，所以在相机标定环节忽略相机畸变的影响，而只对内参数求解。同时，由于相机的外参（位姿变换矩阵）属于动态参数，故亦不对其进行求解。

2.2 RGB-D相机测量原理

上述介绍了相机的成像原理，但是普通相机获取的只有平面RGB信息，在成像过程中丢失了深度信息。由于本文采用Kinect v2相机进行深度的获取，所以接下来对RGB-D相机模型进行简要介绍。可以分为结构光和飞行时间法（Time-of-flight, ToF）两类。

两类相机都需要向检测目标发射红外线光束，不同的是，结构光相机（如Kinect v1,RealSense等）是通过相机结构光图案计算距离。ToF类相机通过发射光束脉冲计算发射和接收之间的光束飞行时间，如下图所示。

（来自SLAM十四讲）

3 Kinect2相机标定

3.1 张正友相机标定法

相机标定的方法有很多，其中张正友相机标定方法使用范围最广，而且操作简单，精度可以达到亚像素级，因此将使用张正友相机标定法进行相机标定。

在相机模型一节中针对针孔相机模型进行了介绍，得知世界坐标系一点映射成像素图像需要通过旋转变换矩阵转将其转换到相机坐标系下，再通过三角形近似投影到成像平面上，最后经过内参数矩阵转换为像素坐标系下的点。可以由以下公式进行表示：

其中内参矩阵包含fx 、fy 、cx 和cy，刚体变换矩阵包含旋转矩阵R和平移向量t。张正友相机标定方法的核心是假设了模型平面在Z=0处，得到下式，从而提供了一种简易实用的初值求解方法，得到初值后代入优化模型进行迭代求解。

其中r1和r2是旋转矩阵R的前两列，将等式右侧前两个矩阵的乘积定义为H。因为假设在一个平面上，所以平面之间的映射可以通过单应矩阵表示，故H亦为单应矩阵。

可以看出，单应矩阵为相机内参矩阵与改进后的刚体变换矩阵的乘积。通过像素坐标和世界坐标对应关系，经过一系列变换，便可以求出单应矩阵，然后进行矩阵分解，求解出内参矩阵和刚体变换矩阵。也就完成了相机的内外参的标定。对于具有畸变的相机模型，我们得到的单应矩阵只是增加了畸变参数，解法同无畸变的模型类似。

一般情况下，矩阵分解得到的结果通常作为初值进行优化，构建重投影代价函数通过LM等迭代优化的方法进行求解

3.2 kinect2配置安装与标定配准

这部分基于Dell Inspiron 3558电脑、Intel® Core™ i5-5200U CPU、NVIDIA　GeForce 920M显卡、nvidia-384驱动、Ubuntu16.04操作系统和ROS Kinetic。因为是在ROS环境中进行的，在相机标定之前，我们需要提前安装ROS和配置Kinect v2相机。

在ROS中使用Kinect v2需要编译libfreenect2驱动，并在ROS的工作空间创建IAI_Kinect2功能包。IAI功能包共包含bridge、calibration、registration、viewer共四个模块，各个模块的功能分别是图像数据转换、相机标定、图像配准以及图像查看。

下图是配置好Kinect2相机后在libfreenect2中运行Protonect采集的4幅图像，包括红外图像、RGB图像、深度图像和配准图像。

上图是通过OpenNI2获取Kinect硬件设备获取的信息。然而，我们是基于ROS开展的，所以我们需要运行IAI功能包中的kinect2_bridge连通与ROS中的数据转换，获取的点云数据如下图所示。

可以看到人像的边缘有很大的轮廓阴影和噪声，这是由于没有对相机进行标定引起的，并且深度图和彩色图没有进行配准，所以有很大的误差。

由上述Kinect2相机获取的图像类型可知，只需要对红外相机、RGB相机、深度图进行标定并将深度图和红外图进行映射纠正就可以得到较高质量的数据了。这部分的标定主要基于IAI功能包的Calibration，采用的棋盘格为7x9，正方形边长0.025米。

接下来，将对三种图像和深度图与RGB图的配准进行标定：

（1）RGB标定
首先，运行标定程序，设定参数为color，模式为record，会出现以下图界面，特征角点可以很稳定的被识别出来。

标定采集图像越多，最终的计算得到的参数也就越好。这里我们采集12张图像，采集的图像需要遍布相机的视觉范围，并且需要不同的距离和一定程度的旋转，采集的图片数据如下图

以上述的12幅图像作为数据源进行计算，最后输出结果存放在calib_color.yaml文件中，标定的参数如下表：

（2）IR图像标定
同RGB的标定类似，需要采集图像后进行参数计算，标定界面和采集的图像分别如下图所示。

最后通过标定程序的calibrate模式对这些图像进行计算，得到calib_ir.yaml文件，标定参数如下表所示：

（3）Sync配准

在得到RGB图像和IR红外图像的标定参数后完成了两个相机内参的矫正，但Kinect2相机有RGB相机和IR相机分别获取彩色图像和深度图像，在生产装配的过程中，两个相机的位置关系已经默认给出，集成在Kinect2的芯片中自动计算配准两幅图片。但有时候仍存在误差，这就需要进行两幅图像的配准标定。

Sync的配准需要采集在同一时间戳下RGB图和IR图，通过特征点的位置重投影误差计算两个相机的标定参数。

标定界面下图所示，共采集了8帧的彩色图和红外图。

最后计算得到calib_pose.yaml文件，该文件的参数描述了两个相机采集的图像之间的变换关系，包括本质矩阵、基础矩阵、旋转矩阵和平移矩阵等参数，下图2.3.8为终端中输出的结果，其中本质矩阵是旋转矩阵与平移矩阵的反对称矩阵的乘积，基础矩阵与本质矩阵之间相差一个内参矩阵.

(4) 深度图标定
深度图的标定主要计算深度误差，在标定前需要加载彩色图、红外图、和帧同步的标定参数，使用标定包的calibrate直接计算即可，得到的参数depthShif为0.0156,如下图中终端所示，其中方差为0.0005、标准差为0.023、均方根为0.028，可见该深度漂移的估计值满足要求，可以使用
完成上方四个步骤的标定工作后得到4个yaml文件，只需重新启动kinect2相机时加载这些参数即可得到标定参数修正后的点云图像，如下图所示，同上方采集的人影点云图相比，标定后采集的图像没有明显的阴影轮廓，深度图和彩色图已经基本重合。需要说明的是，本文采用的打印棋盘格进行标定，在实际生产应用当中，通常会使用高精度的标定板，需要保证标定工具的平行度误差

4 点云数据获取

4.1 PCL点云库介绍

在进行点云数据获取之前，需要了解一下PCL点云库。如果说OpenCV是二维图像处理的一把利器，那么PCL就是3D视觉的一把瑞士军刀。它原本是ROS的一个库，由于在3D数据处理的优越性和使用人数的增多，后单独开发成一个库，由Willow Garage等公司维护。它实现了大多点云数据下通用的算法，涉及到点云获取、滤波、分割、配准、特征提取、物体识别、表面重建、可视化等。

这些PCL模块连接着系统硬件和上层应用，在具体应用时的架构：

在安装PCL点云库之前需要安装一些依赖项，下面介绍几个重要的依赖项：
1）Boost: C++标准库扩展的一些库。
2）Eigen: 以矩阵为主要操作对象的线性代数和数值分析的C++库。
3）FLANN：快速最近邻算法的开源库，实现了很多查找算法。
4）VTK：用于计算机图形学和可视化。
5) CUDA: 全称是（Compute Unified Device Architecture），是NVIDIA公司开发的并行计算平台，使得利用GPU并行计算复杂的求解问题。因为本文研究的点云数据对象涉及计算量巨大，因此本文安装了CUDA8.0来并行计算，对相关算法的运行进行加速。

4.2 基于图像的点云获取

Kinect2采集的数据信息主要包括彩色图像和深度图像，而不是直接的点云信息。这需要我们进行数据转换。通过相机模型章节对空间点到像素平面映射的描述公式，再通过深度图像的深度信息， 很容易通过计算来得到点云在相机坐标系下的位置。

u,v为彩色图像中像素位置，d为深度图像中的深度值Xc YcZc代表某点在相机坐标系下的位置。fx fy cx cy和depthsacle为相机的内参。通过上方式子便可以将像素坐标转换成相机坐标系的坐标，随后将 通过旋转变换矩阵T将相机坐标系的点运转换到世界坐标系，这里的T也就是相机的外参R、t。并将对应的RGB三通道的数值进行复制，便得到了彩色的点云数据信息。

(这里是当时学习《视觉SLAM十四讲》的例程)
下面将通过TUM数据集的5帧数据（包括每一帧对应的 RGB图、深度图、相机位姿），来进行点云拼接的实验，实现的算法是在上述的公式的基础上将每一帧的数据添加到pcl::PointCloud中。

TUM数据集中已经给定了相机的内参，数据分别为：cx = 325.5 ,cy = 253.5 ,fx = 518.0 , fy = 519.0 , depthScale = 1000.0。

相机分别对应的位姿用x、y、z三个空间位置变量和一个表示旋转的四元数进行表示，位姿数据如下表所示：

根据点云深度的公式编写点云拼接的的程序，定义PCL中点云格式为pcl::PointXYZRGB的PointCloud,遍历5帧图像读取深度图和RGB图，通过上文的公式求x、y、z、B、G和R等分量，通过push_back函数将其加入PointCloud中，最后通过PCL库中IO模块的savePCDfileBinary函数将点云数据保存为pcd格式的文件。

运行程序后，生成的文件中共包含1081843个点，可通过安装pcl_viewer打开pcd文件查看效果

4.3 ROS环境中的点云获取

之前完成了从RGB图和深度图中获取点云数据，但需要手动编写程序来获取点云，接下来介绍一种便捷的方式，实现在ROS环境中获取点云数据，并对点云数据采用体素滤波的方法进行降采样，提高点云质量。

IAI的bridge功能包中包含了从彩色图和深度图中获取点云的转换功能，直接将点云数据发布在话题/kinect2/qhd/depth/points中。在终端输入roscore启动ROSMaster，运行kinect2_bridge节点进行Kinect相机数据获取，在Rviz的Display面板添加PointCloud2数据类型，订阅Kinect2发布的/kinect2/qhd/depth/points数据。

可以看出该点云数据有很多噪点，而且点云数量巨大，很多算法应用中并不需要对如此多数据点进行处理，而其余的点便会成为冗余数据，计算时间大幅提升降低了实时性，因此对采集的点云数据进行降采样也就十分有必要。

通过ros的话题编程，实现了一个降采样的节点downSampling。该节点先将订阅的sensor_msgs::PointCloud2数据格式转换成pcl::PCLPointCloud2，然后声明pcl::VoxelGridpcl::PCLPointCloud2对象，在该对象的成员函数中输入pcl::PCLPointCloud2的数据、设置体素大小为1cm进行voxelFilter,最后再将滤波后的点云转换成sensor_msgs::PointCloud2发布后用于其他节点订阅。

下篇博客节介绍在gazebo中搭建仿真环境，基于ROS使用linemod等方法进行3D物体识别