基于Kinect Azure的多相机数据采集（三）

在基于Kinect Azure的多相机数据采集（一）和基于Kinect Azure的多相机数据采集（二）中，我们总结了用Kinect Azure相机采集三维彩色点云的方法以及相机之间的同步方法。如果说相机同步是实现多相机采集系统的基础，那么点云配准部分就是该系统的重点部分。最终的采集质量，全靠准确的点云配准结果来保证。因此，这篇文章继续总结采集系统的下一部分——点云配准部分。如有表述错误或不严谨的部分，欢迎各位博主指正。

一、点云配准

何为点云配准？通俗来讲，就是将不同坐标系的点云全部搬移到同一坐标系中。对于多个不同位置且已经同步的相机，每个相机都会得到不同角度下的一幅三维图像，为了将多个相机采集到的不同角度的点云合成一幅完整的点云，此时就需要进行点云配准。
那么如何进行点云配准？首先我们来思考如何将两个相机采集到的点云搬移到同一个坐标系。对于主设备来说，它所采集到的点云坐标是它的深度传感器坐标系下的坐标，对于从属设备来说，它所采集到的点云坐标也是它的深度传感器坐标系下的坐标。此时要做的就是求得从属设备的深度传感器坐标系与主设备的深度传感器坐标系之间的转换关系，即两个坐标系间的外参。而已知每台设备的深度传感器和颜色传感器之间的外参，那么只需要求得两台设备颜色传感器坐标系之间的外参，就可以将不同坐标系的点云搬移到同一坐标系下，即实现点云配准。那么如何获取两台设备颜色传感器之间的外参呢？主要有以下两种方法：相机标定和粗匹配+精匹配。

二、相机标定

目前使用的比较多的标定方法是张正友标定法（黑白棋盘格标定）。对于单个传感器而言，该方法可以计算传感器的内参、畸变参数以及传感器坐标系和世界坐标系之间的外参。对于两个传感器而言，可以计算它们之间的外参。该小节首先总结张正友标定法的原理，再说明如何通过opencv库进行实现。

1、张正友标定法原理

通过采集到的棋盘格的角点在像素坐标系和世界坐标系的坐标，可以建立如下的关系式（坐标系之间的转换关系在Kinect Azure的多相机数据采集（一）中已说明）：
sm=K[R,t]M,令H=λK[R,t]，则m=HMsm=K[R,t]M, 令 H=\lambda K[R,t]，则m=HM sm=K[R,t]M,令H=λK[R,t]，则m=HM其中，s为比例因子，m为像素坐标系坐标，M为世界坐标系坐标（棋盘格平面为Z=0）。H为8自由度的单应性矩阵，理论上由四对匹配角点可解。
由于H=[h1,h2,h3]=λK[r1,r2,t]H=[h_{1},h_{2},h_{3}]=\lambda K[r_{1},r_{2},t]H=[h1,h2,h3]=λK[r1,r2,t]r1=λK−1h1，r2=λK−1h2，t=λK−1h3r_{1}=\lambda K^{-1}h_{1}，r_{2}=\lambda K^{-1}h_{2}，t=\lambda K^{-1}h_{3}r1=λK−1h1，r2=λK−1h2，t=λK−1h3 且R具有正交性和归一性，所以r1⋅r2=0⇒h1TK−TK−1h2=0r_{1}\cdot r_{2}=0\Rightarrow h_{1}^{T}K^{-T}K^{-1}h_{2}=0 r1⋅r2=0⇒h1TK−TK−1h2=0∥r1∥=∥r2∥⇒h1TK−TK−1h1=h2TK−TK−1h2\left\|r_{1}\right\|=\left\|r_{2}\right\| \Rightarrow h_{1}^{T} K^{-T} K^{-1} h_{1}=h_{2}^{T} K^{-T} K^{-1} h_{2}∥r1∥=∥r2∥⇒h1TK−TK−1h1=h2TK−TK−1h2 一个H矩阵可以提供两个方程，对于5自由度的K，至少需要三幅图片可以求解。但上述的推导结果是基于理想情况下的解，由于可能存在高斯噪声，所以使用最大似然估计进行优化。采用更多的图片（20幅有效图片），每幅图片有更多角点（8*6棋盘格）。
根据上述原理可以求得单个传感器的外参，分别求出两个设备的颜色传感器的外参，就可以计算出两个颜色传感器间的外参。

2、相机标定实现

可以通过opencv库实现相机标定，引入头文件calib3d.hpp。使用opencv库实现相机标定的步骤如下。

相机获取一帧图像，由图像格式转为Mat矩阵
利用findChessboardCorners()函数识别图像中的棋盘格角点位置
drawChessboardCorners()函数可以查看识别的角点位置是否准确
当有足够多张图后，调用calibrateCamera()以获取单相机的内外参和畸变系数；调用stereoCalibrate()以获取双相机间的外参（R、t）

三、粗匹配+精匹配

点云配准还可以通过粗匹配+精匹配的方法进行实现。粗匹配，对于两个设备而言，分别在采集的图像上找到特征点，并对特征点进行匹配，再利用SLAM中对极几何的方法，求出一组粗略的外参解。精匹配，利用icp方法迭代求解，将粗匹配计算得到的解作为icp算法的初始解，调用pcl库，可通过icp算法求出精确的外参。

1、粗匹配

可以通过特征点匹配算法，先在图像上找到匹配的特征点。再通过SLAM对极几何求出粗略解。
特征点匹配算法可选择sift算法或orb算法。sift（尺度不变特征变换）算法，顾名思义，其将一些特殊的点，不会因为视角的改变、光照的变化、噪音的干扰而消失的点，作为特征点。而orb（Oriented Fast and Rotated Brief ）算法，其通过Fast算法找特征点，找出那些卓尔不群的点，即拿一个点跟它周围的点比较，如果它和其中大部分的点都不一样就可以认为它是一个特征点。因此，orb算法提取特征点的速度更快，但sift算法的鲁棒性更好。
可以通过opencv库实现orb/sift算法,需引入头文件features2d.hpp。

	orb	sift
创建对象	Ptr orb = ORB::create()；	SiftFeatureDetector siftDetect; SiftDescriptorExtractor descriptor;
提取特征点	orb->detect();	siftDetect.detect();
计算描述子	orb->compute();	descriptor.compute();
创建用于匹配的对象	BFMatcher matcher();	Ptr matcher = DescriptorMatcher::create();
匹配特征点	matcher.match();	matcher->match();

2、SLAM对极几何

上述粗匹配过程结束后，可以得到一系列匹配的像素点，可以通过对极几何的方法求出R、t的一组解。用p1,p2p_{1},p_{2}p1,p2来表示两组匹配的像素点坐标，用PPP表示p1p_{1}p1对应的相机坐标系下的坐标，则有如下关系式：s1p1=KP,s2p2=K(RP+t)s_{1} p_{1}=K P, s_{2} p_{2}=K(R P+t) s1p1=KP,s2p2=K(RP+t)x1=s1K−1p1,x2=s2K−1p2,x2=Rx1+tx_{1}=s_{1} K^{-1} p_{1}, x_{2}=s_{2} K^{-1} p_{2}, x_{2}=R x_{1}+t x1=s1K−1p1,x2=s2K−1p2,x2=Rx1+tt∧x2=t∧Rx1+t∧t=t∧Rx1t^{\wedge} x_{2}=t^{\wedge} R x_{1}+t^{\wedge} t=t^{\wedge} R x_{1} t∧x2=t∧Rx1+t∧t=t∧Rx1x2Tt∧Rx1=x2Tt∧x2=0x_{2}^{T} t^{\wedge} R x_{1}=x_{2}^{T} t^{\wedge} x_{2}=0 x2Tt∧Rx1=x2Tt∧x2=0p2TK−Tt∧RK−1p1=0p_{2}^{T} K^{-T} t^{\wedge} R K^{-1} p_{1}=0 p2TK−Tt∧RK−1p1=0E=t∧R(本质矩阵)，F=K−TEK−1(基础矩阵)E=t^{\wedge}R(本质矩阵)，F=K^{-T}EK^{-1}(基础矩阵)E=t∧R(本质矩阵)，F=K−TEK−1(基础矩阵)

E/F为8自由度矩阵，每对匹配点提供一个方程，至少8个点可以求解出E/F.匹配点对过多时可以用求解超定方程的方法求E/F
E的奇异值满足[σ,σ,0][\sigma,\sigma,0][σ,σ,0]，σ为尺度因子，可通过计算EETEE^{T}EET的特征值来证明
对E进行SVD分解，E=UΣVT=(UΣ1UT)∗(UΣ2VT),Σ=Σ1Σ2E=U \Sigma V^{T}=\left(U \Sigma_{1} U^{T}\right) *\left(U \Sigma_{2} V^{T}\right), \Sigma=\Sigma_{1} \Sigma_{2}E=UΣVT=(UΣ1UT)∗(UΣ2VT),Σ=Σ1Σ2，前者为反对称矩阵（t^），后者为正交矩阵（R）
由于尺度因子的存在，求出的t可能有正负；由于旋转方向的不同，R也存在两种情况。因此存在四组解，但只有一组解满足P在两个坐标系的深度均为正，可通过三角测量法对深度进行计算
三角测量法：
s1p1=KP,s2p2=K(RP+t)s_{1} p_{1}=K P, s_{2} p_{2}=K(R P+t) s1p1=KP,s2p2=K(RP+t)s2K−1p2=R(s1K−1p1+t)s_{2} K^{-1} p_{2}=R\left(s_{1} K^{-1} p_{1}+t\right) s2K−1p2=R(s1K−1p1+t)p2∧KR(s1K−1p1+t)=s2p2∧KK−1p2=0→p_{2}^{\wedge} K R\left(s_{1} K^{-1} p_{1}+t\right)=s_{2} p_{2}^{\wedge} K K^{-1} p_{2}=\overrightarrow{0} p2∧KR(s1K−1p1+t)=s2p2∧KK−1p2=0s1p2∧KRK−1p1=−p2∧KRts_{1} p_{2}^{\wedge} K R K^{-1} p_{1}=-p_{2}^{\wedge} K R t s1p2∧KRK−1p1=−p2∧KRt
求出s1后，带入求出P，进而求出s2，若s1和s2均大于0，则该组解为正确的R、t.
也可通过opencv库的findEssentialMat/findFundamentalMat函数求解E/F矩阵，通过recoverPose函数求解R、t

3、精匹配

利用ICP（point2point，point2plane等）算法，将粗匹配求解得到的外参作为ICP算法的初始解，调用PCL库，实现ICP算法，从而求得外参。
通过上述的相机标定或者粗匹配+精匹配的方法，可以得到颜色传感器间的外参，从而计算出两台设备深度传感器间的外参，并且可以通过外参，将两幅点云数据搬移到同一坐标系中，合成一幅完整的三维图像。
至此，整个系统就搭建完成了，再次回顾一下系统的整个流程。首先要对多个相机进行同步，使它们获取到的数据是同一时刻的。然后，每个相机分别采集并处理数据。最后，通过点云配准的方式将多个设备采集到的不同角度下的三维图像，搬移到同一坐标系，合成一幅完整的点云数据。
采集系统完成后，可以继续搭建完整的采集、压缩、传输、显示系统。通过windows多线程的使用，将数据采集、数据处理、数据传输划分为多个线程，使采集到的数据可以在客户端中实时进行显示。