双目原理

   双目视觉是利用视差原理的一种视觉方法。

   如图所示为空间中一点P在左右相机中的成像点Pleft=（Xleft,Yleft），Pright=（Xright,Yright）。将两相机固定在同一平面上，则点P在Y方向的坐标是相同的，即Yleft = Yright =Y。根据三角原理，可得：

   视差被定义为相同点在左右相机X方向的偏差，即：Disparity=Xleft-Xright。则点P在左相机坐标系下的位置可以表示为：

   因此，对于空间中任意一点，若能在严格对齐的两相机中找到其成像点，即可计算出其三维坐标。

双目标定

   双目标定和普通相机的标定一样，其标定时不仅标定出两个相机的内参，还要标定出两个相机之间的关系。因此，在进行双目标定时，需要使两个相机对同一标定板进行多次取图，分别标定出各自的内参和相对于标定板的外参，然后便可计算出两相机位置间的关系：

   其中，R为两摄像头间的旋转矩阵，T为两摄像头间的平移矩阵。Rr为右摄像头经过张氏标定得到的相对标定物的旋转矩阵，Tr为右摄像头通过张氏标定得到的相对标定物的平移向量。Rl为左摄像头经过张氏标定得到的相对相同标定物的旋转矩阵，Tl为左摄像头经过张氏标定得到的相对相同标定物的平移向量。

   上述只是理论公式，opencv的steroCalibrate函数在实现时会对多次计算的R、T进行优化，最终得到两相机间的最优变换。

双目校正

   经过双目标定后的两相机在拍摄同一对象后，需要进行双目校正。双目校正多利用极线约束，来使同一特征点位于左右相机两张图像水平方向的同一条直线上，即“把实际中非共面行对准的两幅图像，校正成共面行对准”。当然，在这个过程中也会进行一些畸变校正。利用极线约束进行双目校正后可以使特征点在两幅图像中都位于极线上，这样在进行特征点匹配时仅需要在极线上进行搜索而不需要在整个二维图像上进行搜索，大大减少了计算量。

(1)双目校正前的左右相机图像

(2)双目校正后的左右相机图像

立体匹配

   立体匹配，即将左右相机图像上的对应点匹配起来，由此计算视差。立体匹配有多种算法：

   1、根据最优化的理论方法不同，分为局部匹配算法和全局匹配算法；

   2、根据采用图像表示的基元不同，分为：

1.区域立体匹配算法

   给定一幅图像上的一点，选取该点邻域内的一个子窗口，在另一幅图像中的一个区域内，根据某种相似性，寻找与子窗口图像最相似的窗口，得到的匹配窗口中对应的像素点就为该像素的匹配点。可获得稠密视差图。

2.基于特征的立方匹配算法

   基于几何特征信息（边缘、线、轮廓、兴趣点、角点和几何基元等）提取图像的几何特征点，针对几何特征点进行视差估计，利用得到的视差信息重建三维空间场景。可获得稀疏视差图，通过插值可获得稠密视差图。算法速度快，由于需要插值算法来计算缺失像素点的视差值，应用场景受限。

3.基于相位立体匹配算法

假设在图像对应点中，其频率范围内局部相位相等，在频率范围内进行视差估计。

点集配准

   点集配准是用于将两个点云集进行匹配，计算其旋转矩阵R和平移矩阵T及尺度变换（Scale），使用两个点集上的点最大可能的重合，这个变换过程就是点集配准。

   点集配准算法的经典算法是ICP（Iterative Closet Points）

   1)ICP算法核心是最小化一个目标函数：

   （这里的表述与原文略微有些不同，原文是用四元数加上一个偏移向量来表达旋转平移变换。）就是一对对应点，总共有对对应点。这个目标函数实际上就是所有对应点之间的欧式距离的平方和。

   2)寻找对应点。可是，我们现在并不知道有哪些对应点。因此，我们在有初值的情况下，假设用初始的旋转平移矩阵对source cloud进行变换，得到的一个变换后的点云。然后将这个变换后的点云与target cloud进行比较，只要两个点云中存在距离小于一定阈值（这就是题主所说的ICP中的一个参数），我们就认为这两个点就是对应点。这也是"最邻近点"这个说法的来源。

   3)R、T优化。有了对应点之后，我们就可以用对应点对旋转R与平移T进行估计。这里R和T中只有6个自由度，而我们的对应点数量是庞大的（存在多余观测值）。因此，我们可以采用最小二乘等方法求解最优的旋转平移矩阵。一个数值优化问题，这里就不详细讲了。

   4)迭代。我们优化得到了一个新的R与T，导致了一些点转换后的位置发生变化，一些最邻近点对也相应的发生了变化。因此，我们又回到了步骤2)中的寻找最邻近点方法。2)3)步骤不停迭代进行，直到满足一些迭代终止条件，如R、T的变化量小于一定值，或者上述目标函数的变化小于一定值，或者邻近点对不再变化等。（这里也是题主所说的ICP算法中的一个参数）

   算法大致流程就是上面这样。这里的优化过程是一个贪心的策略。首先固定R跟T利用最邻近算法找到最优的点对，然后固定最优的点对来优化R和T，依次反复迭代进行。这两个步骤都使得目标函数值下降，所以ICP算法总是收敛的，这也就是原文中收敛性的证明过程。这种优化思想与K均值聚类的优化思想非常相似，固定类中心优化每个点的类别，固定每个点的类别优化类中心。