OpenCV-15投影与三维视觉

1 前言

本章我们将会探讨三维视觉，首先会研究三维到二维的投影变换以及其逆变换，然后会介绍多相机立体景深感知。在这个过程中，我们会使用到前一章介绍的一些知识，包括相机内参矩阵(Camera Intrinsics Matrix)M，扭曲系数(Distortion Coefficients)，旋转矩阵R，平移向量T，以及单应矩阵(Homography Matrix)H。就像在前一章中提到，并在附录B中进一步描述，opencv_contrib包含全景相机和多相机（ccalib模块）的进一步标定算法，不同类型的标定模式（aruro和ccalib模块），以及最后的色彩平衡和去噪算法（xphoto）。这里不会深入讨论这些实验特性，但是这些工具能够很好的改善立体标定和视觉。

首先我们通过标定的相机讨论三维世界内的投影，并回顾仿射变换和投影变换。然后通过一个例子介绍如何获得地面的鸟瞰图，这在机器人或者许多其他视觉应用中很常见的一个问题。然后我们会更详细的介绍上一章提到的函数cv::solvePnP()，会深入讨论函数内部寻找图像上一个已知三维物体的三维姿态使用到的算法。

认识这些基本概念后，我们将会讨论三维几何学和多成像器。通常在不使用多张图片的情况下，没有可靠的方法完成校准任务，以及提取三维信息。最明显的例子就是立体视觉(Stereo Vision)，即使用多个图像重建三维场景。在立体视觉中，通过匹配同一时间从不同角度拍摄的多幅图像中对应的特征，从而用它们的差异分析并计算得到深度信息。另外一个例子是运动结构(Structure from motion)，即值使用一个相机在不同时间从不同的位置拍摄目标物体。在立体视觉中，我们主要关心的是相同特征的差异(Disparity Effects)，即三角测量，并将其作为线索计算距离。而在运动结构中，我们通过计算基础矩阵(Fundamental Matrix)将不同的视图关联在一起，并将其作为理解场景的线索。

因为所有的这些问题都以某种方式依赖于从三维世界想二维图像平面投影的能力，或者依赖于相机，因此我们将在上一章节的基础上学习本章的相关知识。

2 投影

在完成相机校准之后就可以将真实世界中的点投影到图像上，也就是说给丁一个在和相机关联的物理坐标系下的点的坐标，就能计算出它在成像器上的坐标，此时单位为像素。这个转换过程在OpenCV中通过如下函数实现。

// objectPoints：模型坐标系下的点坐标数据
//     可以是矩阵，单通道时，尺寸为3✖️N或者N✖️3，3通道时尺寸为1✖️N或者N✖️1
//     可以是vector
// rvec：旋转向量，Rodrigues形式，即向量的方向表示旋转轴，向量的模表示旋转弧度
// tvec：平移向量
// cameraMatrix：相机内参矩阵，尺寸为3✖️3
// distCoeffs：扭曲系数，元素数为4、5或者8的向量，或者传入cv::noArray()
// imagePoints：图像坐标系中的对应点坐标
//     数据格式和objectPoints数据格式有对应关系，单位为像素
//     可以是矩阵，单通道时，尺寸为2✖️N或者N✖️2，双通道时尺寸为1✖️N或者N✖️1
//     可以是vector
// jacobian：偏导数矩阵，下文详细介绍
//     可以传cv::noArray()，或者尺寸为2N✖️(10+nDistCoeff)的矩阵
// aspectRatio：缩放系数，即fx/fy的比值
void cv::projectPoints(cv::InputArray objectPoints,
                       cv::InputArray rvec, cv::InputArray tvec,
                       cv::InputArray cameraMatrix, cv::InputArray distCoeffs,
                       cv::OutputArray imagePoints,
                       cv::OutputArray jacobian = cv::noArray(),
                       double aspectRatio = 0);

尽管该函数参数很多，但理解后并不复杂。该函数被设计用于处理刚性物体上的点投影，也就是说可以使用基于物体自身的原点建立直角坐标系，并表示所有的点坐标。然后通过旋转和平移描述模型坐标系和相机坐标系的关系。实际上函数cv::calibrateCamera()内部也会调用该函数。

如果在实际应用中求解相机坐标系下点点坐标很容易，可以使用这些坐标作为参数objectPoints的值，并将旋转向量rvec和平移向量tvec的所有元素设置为0。参数cameraMatrix和distCoeffs都可以通过函数cv::calibrateCamera()计算得到。

参数jacobian内将被填充的数据是每个点的位置相对于旋转向量、相机向量的每个元素，以及相对于相机内参矩阵的部分元素，和扭曲矩阵的偏导数。也就是说他的尺寸应该等于2N✖️(10+nDistCoeff)，其中N表示点点个数，nDistCoeff表示扭曲系数的个数。通常我们不会使用该参数，直接传入cv::noArray()即可，这里暂不详细探讨该矩阵的用途。该矩阵的计算示意图如下，

参数aspectRatio用于计算jacobian的值，只有在调用函数cv::calibrateCamera()和cv::stereoCalibrate()时使用了固定缩放比时才使用该值，其他时候设置为0即可。

3 仿射变换和透视变换

在OpenCV的程序中以及其他应用中，通常会遇到两类变换，分别是仿射变换和透视变换。OpenCV中的这些程序将一个点列表，或者整幅图中的点映射到另外的位置，通常还会沿着路径进行亚像素插值。你可以这样理解，仿射变换能够将矩形映射为任意的平行四边形，而透视变换更广泛，它可以将矩形映射成为任意的梯形。

透视变换(Perspective Transformation)和透视投影(Perspective Projection)紧密相关，透视投影沿着交于一点即投影中心的直线，将三维世界坐标系中的点投影到二维图像平面中。透视变换也是一种特殊的单应性(Homography)，它关联了同一个三维物体在两个不同的投影平面上的不同图像，对于复杂的情况而言，如投影平面和三维物体相交，也可以人文这种特殊的单应性关联了两个不同的投影中心。

和投影变换相关的函数如下表。

绘图相关函数	描述
函数	说明
cv::transform()	对点集合作仿射变换
cv::warpAffine()	对整个图像作仿射变换
cv::getAffineTransform()	根据点集计算仿射变换矩阵
cv::getRotationMatrix2D()	计算旋转变换的矩阵
cv::perspectiveTransform()	对点集合作透视变换
cv::warpPerspective()	对整个图像作透视变换
cv::getPerspectiveTransform()	获取透视变换矩阵的参数

3.1 鸟瞰图变换实例

机器人导航，热别是用于规划目的时的一个常见任务就是将摄像机获取的场景视图转换为从上向下俯看的鸟瞰图。如在下图中一副相机试图被转换为了鸟瞰图，接下来就可以在该图中作规划以及导航，也可能还会再融合一些由声纳、激光测距仪或者相似的在运动平面内操作的传感器获取到的世界信息。

上图中图a是相机捕获到的道路场景视图，并使用激光测距仪将车前方的道路识别出来，并使用框线标记。图b中使用视觉算法将平坦的类似道路区域分割。图c中将分割出的道路转换为鸟瞰图并与激光鸟瞰图进行融合。

为了获取鸟瞰图，我们需要使用前文提到的相机校准技术获取到相机的矩阵和扭曲参数。在本例中我们从磁盘文件中获取这些数据。我们在地面上放置一个棋盘图片，并使用它来获取微型机器车获取的地平面图像，然后将该图像重新映射成为鸟瞰图。算法的主要流程如下。

• 读取相机的内参和扭曲模型。
• 寻找地平面上的一个已知模型，在本例中为棋盘。至少在亚像素精度上获取到4个点。
• 使用前面章节提到的函数cv::getPerspectiveTransform()计算地平面试图的单应矩阵H。
• 使用前面章节提到的函数cv::warpPerspective()，以及cv::WARP_INVERSE_MAP和cv::INTER_LINEAR标记获取目标对象的平行观察视图，即鸟瞰图。

示例程序BirdEyeView包含完整的代码。

使用计算得到的单应矩阵和额外的高度参数，我们就可以移除棋盘，然后遥控这小车四处逛了。下图是该示例程序的效果图，左图为平视图像，右图为计算得到的鸟瞰图。

4 三维姿态估计

估计三维模型的位置可以通过单个或者多个相机，对于使用多个相机的场景，可以使用每个相机观察到图像之间的关系来推测模型的位置，如使用三角测量技术。这种技术的优势是它可以处理未知模型甚至是未知场景的情况，缺点是需要多个相机。对于使用单个相机的场景，它只能处理已知模型的场景。

4.1 单摄像机姿态估计

在下图a中，已知模型中的一些关键点在模型坐标系下的坐标，当从某个角度观察模型时，如在图b中，我们可以找到这些关键点。为了确定相机和模型的关系，那最直接的线索就是这些关键点一定位于一条从成像器上感光元件发出穿过光圈点射线上。当然仅仅分析单个点我们不能够确定某个点距离相机点距离，但是通过多个点得到的约束，如图c，一个刚体满足所有约束的解只有1个。实际上对于那些有某种内在对称性的模型而言，情况将更复杂，可能存在多个解满足这些约束。取决于模型的对称性，这些解可能是离散的，也可能是连续的。

4.1.1 计算已知模型的姿态

OpenCV提供函数cv::solvePnP()和cv::solvePnPRansac()来计算已知模型的姿态，前一章已经介绍了如何使用它们计算棋盘等校准设备的姿态，实际上它们还适用于更通用的N点投影问题(PNP)。在上图的a图中，圆圈标记出的是已知其模型坐标系下的相对坐标的特征点，同时我们也可以认为从不同视角观察该模型时能够成功的识别出这些点。

在已知这些信息的前提下，上图b幅图可以提取出对应的特征点，并计算他们在图像坐标系下的坐标。由于在相机坐标系中，模型上的这些点都一定位于一条从成像中心穿过成像器上某个感光点的射线上，只有一种姿态能够使得模型上的这些特征点在成像平面上的投影和获取的实际图像相匹配。当然如果模型本身有一定的对称性，将会有多个解。为了能够更直观的理解映射过程，可以观察上图的c幅图像，每个发现的特征都能够确定一个特定的射线，所有的这些约束要同时满足时，相机只能够有特定的姿态。函数cv::solvePnP()的使用见示例程序BirdEyeView。

实际上在计算姿态的过程中并没有必要识别所有的甚至是大部分特征，对一个特定的物理位置也没有必要只关联一个特征。实践中关键点检测器只能从一个小角度窗口识别特征，因此在图像中的一个位置使用多个描述符来捕获从不同位置感知特征的方式也是一个不错的方案。

PNP问题并不总有1个唯一的解，有两种情况下无法得到可靠的结果。第一种情况是用于计算的点不足时，尽管理论上最少3个点配对关系就能够计算出结果，但是实际上由于寻找特征点所使用的方法（如关键点匹配方法）自身精度的问题，它们的位置结果会包含噪声，因此当点很少时，这些噪声可能使得计算结果严重偏离实际值。就经验而言，最好有十多个或者更多的匹配关系。第二种情况是当模型距离相机十分远时，此时这些约束特征位置的射线会趋于平行。而射线之间的差异确保了模型具有唯一的尺度，或者也可以说确保了模型到相机的距离具有唯一解。

这种估计物体姿态和距离的单目方法和我们眼睛在看远处物理时的工作原理很类似。这也是为什么在没有其他上下文确定物体实际大小时，也就是无法确定其内部特征点在模型坐标系中的绝对位置时，不能够测量目标的距离。这也是强迫透视(Forced Perspective)幻觉产生的原因，例如建筑物在高层安装更小的窗户就是为了使其从地面看上去更高。下一节会讨论立体成像，它使用两个或者更多的相机来消除这种最终的不确定性，并且能够同时推断出没有上下文支撑的未知物体的姿态和距离。

5 立体成像

我们应该已经对眼睛的立体成像能力有所认识，而计算机则通过如下方式模拟这种能力。通过找到两个成像器得到图片中点的对应关系，以及相机的基线距离，我们就能够计算这些点的三维位置。尽管寻找对应点的计算成本很高，但是我们可以利用系统的几何特征尽可能的缩小搜索空间。在使用两个相机的实际应用中，立体成像包含如下几步。

• 使用数学方法移除透镜的径向和切向畸变，这也被称为去畸变(Undistortion)，在前一章节中有详细描述。
• 调整相机之间的角度和距离，这一步被称为校正(Rectification)。使得两个相机捕获到的图像是校正后的(Rectified)并且是行对齐的，行对齐意味着两个相机的成像平面在空间上是共面的，并且传感器的对应行是共线的。
• 假定相机是沿着x轴左右排列的，则寻找左右相机视图上的相同特征，当然你也可以沿着y轴上下排列相机，这一步也称为匹配(Correspondence)。这一步的输出是一个差分图，记录了同一个特征分别从左右相机观察到的在成像平面x轴上的差异。
• 如果我们已知相机的空间排列位置，则就可以通过三角测量法(Triangulation)将该上一步得到的视差图转换距离视图。这一步称为重投影(Reprojection)，输出的是深度图。

这里首先介绍最后一步，然后再切入前几步的内容。

5.1 三角测量

在下图所示的理想立体成像装置中，假定我们有一个完美不产生畸变，对齐的并且已经标定好的系统，其中两个图像平面完全共面，光轴完全平行。这里光轴指的是从投影中心O出发经过主点C的射线，也被称为主射线(Principal Ray)。已知两个光轴之间的距离，并且两个成像器的焦距相同，即fl = fr。需要注意的主点C指的是光轴与成像平面的交点，而不是图像中心，实际上真实的系统中，由于无法避免的误差，它们并不是同一个点。这里我们假定主点Cleft和Cright的坐标都已经经过校准，并且在它们自己的图像坐标系下，具有相同的坐标。

此外，假定成像器的每一排像素感知单元都是对齐的，即在同一条线上。这里设置了很多假定条件，需要注意系统的校准过程就是在这些假设条件不成立时，如果通过数学的手段来校准这些误差。假定存在一点P，它在左右两个成像装置上的投影点分别是Pl和Pr，它们偏离主点的x轴方向距离分别是xl和xr，即差分距离为d = xl - xr。则在上图简化后的模型中可根据相似三角形的原理得到如下公式，并计算出深度Z。

需要注意的是这里假定左右两侧光轴相较于无穷远处上述公式才成立，如果主轴在有限距离内相交，则深度的计算公式如下，这个点在后面的内容中会再讨论。

因为深度和差分距离呈反比，它们之间为非线性关系。因此如下图所示，当差分距离接近0的时候，即使很小的改变得到的深度结果也相差很大，但是当差分距离很大时，较小的增幅已经不容易对距离计算结果造成影响。也就是说立体视觉系统只对那些接近相机的物体具有较高的分辨率。

下图是在OpenCV中立体视觉使用的三维坐标系统，需要注意的是这是一个右手坐标系，即当你的大指指向x轴正方向，食指指向y轴正方向时，中指指向大就是z轴正方向。左右成像器的坐标原点都位于左上角，它们的坐标分别表示为(xl，yl)和(xr，yr)。投影中心分别为Ol和Or，其中Ol为相机坐标系的原点，主光轴交于图像平面于点(cx，cy)，需要注意它并不位于该成像平面的中心。经过数学上的校准，两个相机和成像平面共面，并且是行对齐的，它们之间的距离已知为T，它们的焦距都为f。

在理解简化的模型后，我们需要处理更棘手的问题，就是理解真实的不完美相机装置是如何映射到理想的完美系统中。如前文提到，实际的成像装置不可能满足苛刻的假设条件，我们需要使用数学手段找出一种投影和变形映射关系将实际得到的两幅图像校准至通过完美的成像系统应该得到的图像。在设计你的立体成像装置时，你应该尽可能的向完美条件靠近。假如相差太远，如两个成像传感器平面偏离程度太高，那么为了校准系统使用的数学计算会使得图像发生较大的变形。这会减少或者消除校准后图像的立体重叠区域，从而使得图像深度信息难以计算。

当然有时我们会刻意将摄像机稍微倾斜，使得两个光轴在有限距离内相交。例如当在一个较近范围内需要更高的分辨率时，尽管通过这种方式处理后，经过数学上的对准使得到的视差可能为负值，但是就可以保证感兴趣的深度在更好的深度分辨率内。

此外为了得到好的结果，需要保证两个相机同步获取图像。否则如果场景或者相机自身发生移动，那么它们得到的图像位置就会发生偏移。

下图展示了真实的双相机立体系统，接下来会先了解一些术语和基本概念，我们再研究如何通过数学手段对系统进行校正从而得到我们想要的结果。

5.2 对极几何

立体成像系统的基本几何模型被称为对极几何(Epipolar Geometry)，它本质上就是两个针孔模型的组合，其中每个针孔模型都表示一个相机。需要注意的是这些相机有着自己的畸变，关于相机畸变可以参考前一章的内容。如下图所示，在该系统中一些感兴趣的点被称为极点(Epipoles)。极点可以用于缩小在立体相机系统中得到的两张图片中对应点的位置，但是在深入原理之前我们先了解它的定义，同时学习一些相关的术语。

下图简单的描述了立体成像系统，其中每个相机都有自己的投影中心Ol和Or，以及自己的投影平面πl和πr。物理空间中的点P在两个平面上的投影点分别为pl和pr。两个投影中心的连线分别交于投影平面点el和er，它们被称为极点(Epipole)。观察点P和极点el及er构成的平面称为极面(Epipolar Plan)，而plel和prer被称为极线(Epipolar Line)。

对于任意一个投影平面上的一点而言，它在三维空间中对应的点可能出现在从其自身的投影中心到该投影点的连线的任意位置，因为当个成像装置是无法感知到其距离的。例如对于pl而言，它在三维物理空间中的对应点可能出现在射线OlPl上Pl以后的任意位置。但是有趣的是射线OlPl在另一个投影平面πr上的投影正好是极线erpr。也就是说在某个成像器上观察到的点在物理空间中所关联的所有潜在点，在另一个成像器上的投影位置位于穿越极点及对应点的连线之上。

立体相机对极几何的一些特征概括如下。

• 三维物理空间中的每个点都位于一个极平面上，该平面和两个投影平面的交线就是极线。
• 对于一个图像上的特征，它在另外一个图像上的对应点一定位于对应的极线之上，这被称为极线约束(Epipolar Constraint)。
• 极线约束意味着一旦我们构建了立体成像系统的对极几何，则在两个图像中搜索匹配点点过程可以从二维的整幅图像搜索简化至沿着极线的二位搜索。这不仅极大的降低了计算成本，还能帮助我们过滤掉一些假样本。
• 顺序保持特征。即如果点A和B在一副图像中水平依次出现，则它们在另外一副图像中也以相同的顺序出现。即使由于遮挡等原因导致某个相机遗漏了点，但是顺序依旧保持不变。

5.3 本征矩阵和基本矩阵

在介绍如何使用OpenCV提供的函数计算极线之前，我们先熟悉两个重要的概念，本征矩阵E(Essential Matrices)和基础矩阵F(Fundamental Matrices)。如下图所示，矩阵E包含在世界坐标系下将两个成像系统关联在一起的旋转R和平移T信息，而基础矩阵F在上述信息的基础之上还包含两个相机的基本特征，如畸变系数等。因此矩阵F可以关联两个成像器得到的图像中以像素为坐标单位的匹配点。

需要注意这里仿射变换的参考坐标系为成像平面的坐标系，即以成像平面的中心点为原点，轴方向和相机坐标系对应轴方向平行的坐标系，描述的是将左侧的成像平面变换到右侧成像平面位置的过程。

进一步说本征矩阵E仅仅描述的是立体成像系统中的几何关系，并不关心传感器自身的特征。它关联的坐标是以两个相机坐标系为参考，即描述了观察点P在左右相机视角下的观察到的点pl和pr的关系。而基础矩阵关联的是同样的视角下，点P在左右两个成像器生成的图片中对应点ql和qr的关系，它们的坐标是像素，以图像坐标系为参考。

5.3.1 本征矩阵的数学知识

本征矩阵(Essential Matrix)计算的是点P在左右相机视角下观察到的点pl和pr点关系，其参考坐标系为各自的相机坐标系。点P在左右相机成像平面上的投影在各自的相机坐标系内为坐标分别表示为Pl和Pr，右侧相机相对于左侧相机的平移向量为T，旋转矩阵为R，则Pr = R ✖️ (Pl - T)。此外假定一个平面的法向量为n，法向量和平面的交点为a，则对于平面内的所有点而言向量(x - a)和法向量n的内积为0。

以左侧相机坐标系为参考，在极平面中已经包含向量Pl和T，通过向量的外积计算极平面的法向量，即n = Pl ✖️ T，这里使用右手法则判断法向量的方向。则根据上文提到的公式，如下等式成立。

再回忆下我们的目标是通过计算Pl和Pr之间的关系来计算这两个点在左右相机采集到的图像中的矫正坐标ql和qr之间的联系。通过等式Pr = R ✖️ (Pl - T)推导出(Pl - T) = R^-1 ✖️ Pr，旋转矩阵属于正交矩阵，其逆矩阵等于转置矩阵，因此可以推导出如下公式。

两个向量叉乘可以改写成如下矩阵乘法形式。

另外前文的两个向量外积为0时，其结果和方向无关，因此叉乘的顺序可以颠倒。另外根据上述公式以及复合矩阵转置矩阵的计算公式可以推导出如下等式。

这里R✖️S就是本征矩阵的定义，因此上述等式可以简写为如下形式。

成像平面上的坐标pl、pr和点在各自相机坐标系下的Pl、Pr直接的计算方法为首先根据投影公式计算pl = (fl / zl) ✖️ Pl，pr = (fr / zr) ✖️ Pr，再将每个分量的值除以最后一个分量的值。最后可以得到如下公式。

在上面的等式中看上去给定pr和pl其中任意一项，都能够求出另外一项的值，但是其实矩阵E是非满秩矩阵，该3✖️3矩阵的秩为2，因此实际上计算的结果将是一条线。本征矩阵中包含5个有效参数，共有中三个用于描述旋转信息，两个用于描述平移信息，不包含缩放信息。另外本征矩阵还有两个额外约束，第一个是其行列式为0，因为它是秩亏矩阵。第二个是它的两个非零奇异值相等，因为矩阵S是对称的，矩阵R是正交旋转矩阵。这样本征矩阵就有7个有效约束，再次强调本征矩阵不包含任何相机的内在参数，因此它描述的映射关系是物理坐标系或者相机坐标系，而不是图像坐标系。

5.3.2 基础矩阵的数学知识

前文已经提到过本征矩阵仅包含两个相机的相对位置信息，但是在实际应用中，通常我们更关系特征点在图像坐标系中的位置。为了寻找立体相机系统中某个图像中的一个点与另一个图像中极线的关系，需要引入两个相机的内在参数信息。假定点P是观测点在某个成像平面上的投影点，点q表示该点在该相机生成的图像中的坐标，M是该相机的内参矩阵，则q = M ✖️ P，同理p = M^-1✖️q。则根据前文的等式可以推导出如下公式。

这里定义如下基础矩阵。

则上式可以化简为如下形式。

值得注意的是当处理的图像是已经经过校正的，并且每个点都通过焦距经过标准化处理，则此时相机的内参矩阵就算一个单位矩阵，本征矩阵E和基础矩阵F相等。

简而言之，基础矩阵F和本征矩阵E类似，不同之处在于矩阵E处理的是物理空间中的坐标，而矩阵F处理的是像素空间中的坐标。和本征矩阵E一样，基础矩阵的秩只有2，它包含7个有效参数，每个极点贡献两个参数，剩下三个参数描述了两个像平面的单独应性关系，该矩阵同样不包含缩放信息。

5.3.3 OpenCV中的接口

OpenCV中计算立体成像系统中基本矩阵的方法和上一章标定单个相机时计算单应性矩阵的方法类似，都需要准备一组对应点的坐标。OpenCV提供的计算立体成像系统基本矩阵的函数原型如下。

// points1：立体成像系统中某个图像中特征点坐标
// points2：立体成像系统中另一个图像中对应的特征点坐标
// method：Method for computing mtx
// param1：RANSAC最大距离
// param2：RANSAC/LmedS confidence
// mask：蒙板矩阵，用于标识有效样本
cv::Mat cv::findFundamentalMat(cv::InputArray points1, cv::InputArray points2,
                               int method = cv::FM_RANSAC,
                               double param1 = 3.0, double param2 = 0.99,
                               cv::OutputArray mask = cv::noArray());

参数points1和points2中的数据支持N✖️3或者是三通道格式，当传入该类型数据时，算法内部会自动应用透视除法，即传入点P(x, y, z)，实际算法处理后的坐标为p(x/z, y/z, 1)。特殊情况当你传入P(x, y , 0)时，会被转化为p(x, y, 1)，和直接传入点P(x, y)是一样的效果。通常情况下传入二维坐标点即可。

参数method的值决定了函数内部使用的具体算法，不同的取值对样本数量的要求可能并不相同，所有可用的枚举值如下表。其中RANSAC算法可参考论文《Random sample consensus: A paradigm for model fitting with applications to image analysis and automated cartography》，最小中值算法可参考论文《Least median of squares regression》，LMedS算法可参考论文《Robust line fitting using LmedS clustering》。

参数method的取值	所需要的点	采用的算法
cv::FM_7POINT	N = 7	7点算法
cv::FM_8POINT	N ≥ 8	8点算法
cv::FM_RANSAC	N ≥ 8	RANSAC算法
cv::FM_LMEDS	N ≥ 8	LMedS算法

7点算法使用7个样本，它基于矩阵F的秩为2的这个条件来限制计算结果，这种算法的优势是因为计算得到的矩阵秩一定为2，因此不会存在一个非零的小特征值。但是它的缺点是这种约束可能并不能够得到唯一解，因此可能返回3个矩阵，也就是返回结果是一个9✖️3的矩阵。8点算法将矩阵F作为线性方程组求解，如果传入点样本超过8个，则会对所有的点计算线性最小二乘解。这两种算法都存在一个问题，就是它们对异常值非常敏感，对于8点算法而言，即使提供的点远大于8个仍然存在这个问题。而RANSAC算法和LMedS算法就能很好的处理异常值，算法的细节在前面章节已经讲过，这里不再赘述。简单的讲这两种算法在计算过程中都能够识别并忽略异常样本，具有很好的稳定性，当使用这两种方法时最好提供更多的样本点。

参数param1只有在选择RANSAC算法时才有意义，它约束了点到极线的最大像素距离，超过该距离的点被认为是异常离群点。参数param2只有在选择RANSAC算法和LMedS算法时才有意义，它值得是计算结果的置信度，会影响算法的迭代次数。

函数的返回值类型为cv::Mat，通常它是一个和参数points中数据具有相同精度的3✖️3矩阵。但是当选择7点算法时，它可能是9✖️3的矩阵。该结果可用作为函数cv::computeCorrespondEplilines()的输入参数计算某个图像内的特定点在另外一个图像里对应的极线。或者通过函数cv::stereoRectifyUncalibrated()计算校正变换。示例程序FundamentalMatrix使用RANSAC算法计算基本矩阵，其核心代码如下。

需要注意的是当样本数量没有达到算法的要求，或者包含重复的点，或者有多个点共线或者共面时，就不会得到足够的约束条件，函数findFundamentalMat内部的算法就无法计算出基本矩阵，函数的返回值会是cv::noArray()。因此在使用该函数时，保持检查返回值的习惯非常重要。

5.4 计算极线

OpenCV提供函数用于计算立体相机系统内一个图像中的点在另外一个图像中对应的极线，返回的极线通过一个包含三个分量的向量(a, b, c)表示，它所表示的极线等式如下。

计算极线函数原型如下。

// points：立体相机系统内某个图像内的特定点坐标，N✖️1或者1✖️N的浮点型双通道矩阵，或者包含Point2f数据的向量
// whichImage：点的来源图像在立体系统内的索引，取值为1或者2，和函数cv::find FundamentalMat()中的points1和points2对应
// F：立体成像系统的基本矩阵，通过函数cv::findFundamentalMat()计算
// lines：计算的在另一个图像内的极线数组，返回结果会经过标准化处理确保a平方和b平方的和为1
void cv::computeCorrespondEpilines(cv::InputArray points, int whichImage,
                                   cv::InputArray F, cv::OutputArray lines);

5.5 立体标定

立体标定(Stereo Calibration)是计算物理空间内立体成像系统两个相机之间关系的过程，而立体校正(Stereo Rectification)是对相机获得的原始结果进行重映射以还原一个前文描述的理想立体成像系统应当获得的图像。进过校正两个相机的光轴或者说主光线相互平行，也可以说是相交于无穷远处。当然我们也可以讲两个相机的图像标定为其他配置，但是这里我们只关注最常见和最简单的情况，即两个独立相机的光轴相交于无穷远处。

立体标定的核心问题是找到前文描述的两个摄像机之间的旋转矩阵和平移向量。OpenCV提供函数cv::stereoCalibrate()计算这些属性，该函数类似上一章介绍的函数cv::calibrateCamera()，但是新函数可以计算两个相机的内参、本征和基础矩阵，以及畸变系数。和函数cv::calibrateCamera()的另一个主要的区别是，函数cv::calibrateCamera()计算的是相机和一系列棋盘视图之间的旋转和平移向量，而函数cv::stereoCalibrate()计算的是能够关联左右相机的旋转矩阵和平移向量。

我们已经了解了如何计算本征矩阵和基础矩阵，下面需要计算的是两个相机之间的旋转矩阵R和T。假定在模型坐标系中存在点P，我们可以分别通过两个相机的视角及其对应的坐标系，使用单相机标定函数计算各自的旋转矩阵和平移向量。则我们可以构建等式Pl = Rl✖️P + Tl，以及Pr = Rr✖️P + Tr。前文已经推导过，两个相机内的点可以通过等式Pl = R^T^✖️(Pr - T)关联，通过解这三个等式可以得到如下公式。这里需要注意P、R等变量的含义。

给定多组棋盘角点或者类似的标定模型在两个独立相机下的图片，函数cv::stereoCalibrate()内部调用函数cv::calibrateCamera()分别计算每个相机自己的旋转和平移参数。最后再通过上述等式计算我们需要的能够关联两个独立相机的旋转矩阵和平移向量。因为图像噪声的存在和舍入误差的处理，每组棋盘视图数据计算出的R和T都存在轻微的误差，该函数会使用它们的中值作为初始值，并使用稳健的LM(Levenberg-Marquardt)迭代算法通过寻找标定点再两个摄像机视图内重投影误差的局部最小值的方法确定最后的解，并将这些数据写入到指定的地址中。再次明确立体标定计算的旋转矩阵只能将右侧的相机变换到和左侧相机位于同一个平面上，但是它并不能保证两个相机是行对齐的，我们将在下一小节介绍立体校正时讨论行对齐是如何实现的。

尽管函数cv::stereoCalibrate()有很多参数，但是它们都很容易理解，并且很多和前一章介绍的单个相机标定函数cv::calibrateCamera()内含义一致，立体相机标定函数原型如下。

// 返回值：重投影的误差
// objectPoints：标定版组上特征点在模型坐标系下的坐标
// imagePoints1：标定版组上特征点在第一个相机采集的图像内的坐标
// imagePoints2：标定版组上特征点在第二个相机采集的图像内的坐标
// cameraMatrix1：第一个相机的内参矩阵
// distCoeffs1：第一个相机的扭曲系数
// cameraMatrix2：第二个相机的内参矩阵
// distCoeffs2：第二个相机的扭曲系数
// imageSize：待处理的图像大小
// R：联系两个独立相机的旋转矩阵
// T：联系两个独立相机的平移向量
// E：立体成像系统的本征矩阵
// F：立体成像系统的基础矩阵
// criteria：函数内部迭代的终止条件
// flags：定制算法的特殊标记，下文介绍
double cv::stereoCalibrate(cv::InputArrayOfArrays objectPoints,
                           cv::InputArrayOfArrays imagePoints1,
                           cv::InputArrayOfArrays imagePoints2,
                           cv::InputOutputArray cameraMatrix1,
                           cv::InputOutputArray distCoeffs1,
                           cv::InputOutputArray cameraMatrix2,
                           cv::InputOutputArray distCoeffs2,
                           cv::Size imageSize, cv::OutputArray R,
                           cv::OutputArray T, cv::OutputArray E,
                           cv::OutputArray F,
 cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::COUNT 
                                              | cv::TermCriteria::EPS, 30, 1e-6),
                           int flags = cv::CALIB_FIX_INTRINSIC);

参数objectPoints、imagePoints1和imagePoints2包含多个标定视图，数组的每个一级元素都包含一份标定版视图采集到的样本数据。其中objectPoints包含的数据应当是三维坐标，但是在大多数场景下z轴的坐标分量都是0。这和函数cv::calibrateCamera()类似，虽然通常使用的是一个标定板平面，实际上你还可以使用3维的标定模型，只是这并不常见。另外这三个数组中的数据应当是一一对应的，即数组的大小相等，数组内相同序号的样本集合都来自于同一时刻被观察的标定模型，和其在两个独立相机上投影的图像。通常而言参数重的尾标1表示其数据来源于立体系统中的左侧相机，而尾标2表示来源于右侧相机。当然你也可以在提供数据时交换这两组数据，此时你也需要以相反的方式去对待函数的计算结果。实际上更重要的事情是如何从数学上调整两个相机使得它们的扫描线行对齐，从而得到好的标定结果。

需要注意的是如果你使用一个棋盘或者圆点网格图像对两个相机进行标定，那么参数imagePoints1和imagePoints2可以通过函数cv::findChessboardCorners()，或者另外一个角点(圆点)网格搜寻函数获取。

参数cameraMatrix1和cameraMatrix2是3✖️3的相机内参矩阵，参数distCoeffs1和distCoeffs2表示对应相机的畸变系数，可能包含4、5或者7个元素。这些矩阵中首先包含的是两个径向的扭曲系数，然后是2个切向的扭曲系数，最后是剩余的高阶径向扭曲系数，更多信息可以参考上一章相机标定的内容。

参数flags决定了函数处理相机内参矩阵和畸变矩阵的方式，当标记包含值cv::CALIB_FIX_INTRINSIC时，函数将会直接使用这些值参与后续的计算，而不会再重复计算相机的这些属性。如果参数flags被设置为cv::CALIB_USE_INTRINSIC_GUESS，函数将会使用用户提供的数据作为初始值对它们进行优化，并将优化的结果写入到原矩阵中。当这两个选项都未被设置时，函数内部将会调用函数cvStereoCalibrate()计算这些信息。当然你也可以调用函数cvStereoCalibrate()计算相机的内参数、外参数以及立体参数。除了函数cv::calibrateCamera()中介绍的所有flags的值都可用之外，函数cv::stereoCalibrate()还支持值cv::CALIB_SAME_FOCAL_LENGTH。它相对于另外一个互斥选项cv::CALIB_FIX_FOCAL_LENGTH而言限制条件没有那么严格，前者仅限制两个摄像机的焦距相等，而后者要求它们与矩阵cameraMatrix1和cameraMatrix2中定义的值相同。当使用cv::CALIB_SAME_FOCAL_LENGTH时，算法会自动计算两个相机共享的未知焦距。

需要注意的是尝试一次性计算多个参数可能会使得结果产生分歧，从而得到无意义的值。求解方程组有时候是一门艺术，你必须验证结果。你可以在标定和校正的示例程序中看到一些这样的考虑和检查，如在示例程序FundamentalMatrix中通过极线约束对校正结果进行检查。

参数imageSize的单位是像素，只有需要计算或者优化相机的内参矩阵和扭曲系数时该值才有意义，即参数不包含cv::CALIB_FIX_INTRINSIC时该值才有意义。

函数运行完毕后，参数R和T中填充的值指的是相对于相机1的坐标系，将相机1变换到相机2的位置所经历的旋转变换和平移信息。表示本征矩阵的参数E和表示基础矩阵的参数F如果未设置为cv::noArray()，则函数会计算这两个3✖️3的矩阵并将它们写入目标位置。参数termCrit和前文其他函数遇到的同类含义一致，定义算法在经历多少次迭代，或者两次计算的差值低于某个阈值时结束算法。当定义精度阈值时，该函数考虑的是使用当前计算的参数进行重投影得到的整体误差。和函数cv::calibrateCamera()类似，该算法计算内部变量的方向是使得所有可用的标定视图中，所有的点整体重投影误差最小，不同的是函数cv::stereoCalibrate()考虑的是两个相机而不是一个相机。该计算出的整体重投影误差将被作为函数的返回值。

一旦我们有了旋转信息R和平移向量T，或者知道了基础矩阵F，我们就可以用这些参数来校正两幅立体图像，从而使得极线沿着图像行对准，并且扫描线在两个图像上是相同的。尽管R和T不能够限定一个唯一的立体校正，在下一小节中我们将介绍如何使用这些参数以及一些其他的额外约束来校正图像。

5.6 立体校正

当两个图像平面完全对齐时计算立体视差时最容易的，但是正如前文所描述那样，完美的对齐配置要求立体系统的两个相机成像平面完全共面，并且每行成像传感器都必须严格共线，然而这在真实的立体系统中几乎不可能实现。为了校正立体系统，需要选择一个特定的平面作为校正后两个相机的成像平面，具体的选择取决于校正使用的算法，接下来我们将会介绍OpenCV采用的两种算法。

最终通过校正，我们希望两个相机采集图片处理后的行是对齐的，从而提高在两幅图像内寻找匹配点点可靠性、易计算性和可追踪性。具体的讲通过这种方式，在某副图像中寻找另外一副图像中某个点的匹配点时，只需要沿着该副图像的某一行寻找，从而加强可靠性和计算效率。在一个公共的像平面对齐两幅图像的水平行的结果是极点将位于无穷远处，也就是说两个相机的主光轴是平行的。因为存在无穷多个可以选择的公共像平面，我们需要一些额外约束来做出最终的决定。例如基于最大化两个相机图像重叠面积，或者基于最小化畸变效果的约束。

对于立体系统中的左右相机，对齐两个像平面分别会得到8项结果。对于每个相机我们会得到一个畸变向量distCoeffs，一个旋转矩阵Rrect，校正前的相机矩阵M以及校正后的相机矩阵Mrect。根据这些结果我们可以通过函数cv::initUndistortRectifyMap()计算出一个映射，该映射描述了在创建校正图像时从原始图像内部插值的位置。需要注意在OpenCV内部只有当极点位于图像矩形外部时，立体校正才能实现，因此对于基线非常宽，或者多个相机偏向彼此的程度太高的系统中，校正算法可能无法工作。

计算这些结果的方式很多，OpenCV实现了其中的两种。第一种是Hartley的算法，它能够使用基础矩阵计算出未标定的立体结果。第二种时Bouguet算法，它使用两个标定后的相机计算旋转和平移参数。Hartley的算法可以使用单个相机记录的运动推导出立体结构，但是可能比Bouguet的校正算法产生更严重的图像畸变。

需要注意的是当你能够采用标定模式的时候，例如在机械臂或者安全摄像机装备上，使用Bouguet的算法更自然。

5.6.1 未标定的立体校正-Hartley算法

Hartley的算法尝试找到一种能够将极点映射到无穷远处的单应性，并且确保两个立体图像直接的计算误差最小。大体的思路是通过匹配两幅图像中的对应点。通过这种方式可以绕过计算两个相机内部参数的步骤，因为这些参数信息都隐含在点的匹配中。这样我们就只需要计算基础矩阵，该矩阵可以通过前文描述的函数cv::findFundamentalMat()以及7组及以上的匹配点计算。另外也可以通过函数cv::stereoCalibrate()计算。

该算法的好处是我们可以通过观察场景中的点来执行在线立体校正。该算法的缺点是我们无法得到模型的尺度信息。例如，如果我们使用棋盘标定板生成匹配点，我们无法确定模型离相机的距离是100米还是100厘米。由于我们不知道相机的内在参数，因此相机可能有着不同的焦距，倾斜向上，不同的投影中心，或者不同的主点。从而导致我们只能在三维空间中重建出无穷多个投影变换中的一种。这就意味着痛一个模型在不同尺度下，或者在不同的投影变换状态下能够在成像平面上投影出相同的二维坐标。

这两种不确定类型在下图中都有对应的说明，作图描绘了不同尺寸的投影模型在不同距离处得到了相同的投影结果。右图描绘了当相机的焦距不相同时，同一个模型的不同投影变换状态得到了相同的投影结果。

在已知基础矩阵F后，Hartley的算法主要步骤如下。详细步骤参考论文《Theory and practice of projective rectification》。

第一步，使用基础矩阵F和等式F✖️el = 0，以及等式er^T^✖️F = 0分别计算两个极点el和er。

第二步，我们需要计算其中一个单应矩阵Hr，它将右极点映射到无穷远处，使用齐次坐标表示未(1, 0, 0)^T^。因为单应性包含7个约束，需要注意这里假定不存在缩放的情况。我们使用其中三个约束映射极点，还剩4个自由度。这些自由度的某些组合可能使得图像发生严重畸变。为了寻找到一个较好的单应矩阵Hr，我们在图上选择一个点，确保此处发生的畸变最小，并且只允许刚性旋转和平移出现，不允许有剪切变换。一个合理的候选点是图像的原点，另外我们假定极点位于x轴上，即er = (k, 0, 1)，下文会介绍如何通过选择矩阵完成这个任务。则可以一个如下临时矩阵。

第三步，对于在右图中选择的兴趣点，这里选择的是图像原点，我们构建一个平移向量T将该点平移到左侧图像的原点，同时构建一个旋转向量R将极点旋转至第二步里面我们假设的位置er = (k, 0, 1)。则单应性矩阵Hr = G✖️R✖️T。这里T是通过向量平移向量T构建的4✖️4的仿射变换矩阵。

第四步，接下来我们计算出另外一个单应性矩阵Hl，它能够将左极点映射到无穷远处，并且将两幅图像的像素行对齐。和第二部一样，我们使用简单的方式来将极点映射到无穷远处。为了能够对准像素行，我们基于一个这样的事实，当两幅图片的像素行对齐后，这两幅图像中所有匹配点之间的整体距离最小。也就是说整体的视差和最小，则我们可以得到如下公式。这两个单应性矩阵就定义了立体校正。

OpenCV中实现该算法的函数原型如下，它的命名或者不是那么准确，因为该函数并不会校正未标定的立体系统图片对，它只会计算用于校正的两个单应性矩阵。

// points1：来自于第一副图像的样本点
// points2：来自于第二幅图像的样本点，需要和points1对应
// F：两个相机之间的基础矩阵
// imgSize：图像的大小
// H1：立体校正第一个相机采集结果的单应性矩阵
// H2：立体校正第二个相机采集结果的单应性矩阵
// threshold：离群点的判定阈值，如果从某点到对应极线的距离超过该值，则对应的点就会被视为异常点，
//    将不会作用于最终点计算结果
bool cv::stereoRectifyUncalibrated(cv::InputArray points1, cv::InputArray points2,
                                   cv::InputArray F, cv::Size imgSize,
                                   cv::OutputArray H1, cv::OutputArray H2,
                                   double threshold = 5.0);

需要注意的是Hartley的算法用于处理已经通过单个相机标定函数分别处理并校正后的立体系统时能够得到最好的效果。当图像中的畸变非常大时，该算法甚至不能够工作。这种不需要关系相机标定的函数依赖的输入确实已经预先标定并校正的图像，这看上去十分讽刺。此外还有一个未标定的三维方法可以参考文献《Self-calibration and metric 3D reconstruction from uncalibrated image sequences》。

如果两个相机的内在参数大致相同，并且它们具有接近水平对齐的像平面平行的完美配置时，Hartlay的算法得到的结果和下文标定相机进行立体校正得到的结果很接近。如果我们知道被观察模型的大小，或者其三维几何，则我们可以得到和通过标定相机实现方案一样的结果。

5.6.2 标定的立体校正-Bouguet的算法

给定关联两幅立体图像的旋转矩阵R和平移向量T，Bouguet算法尝试最小化两幅图像各自的重投影差值，即得到最小的重投影畸变，并且最大化两个视图的公共视野。

为了使图像重投影产生的畸变最小，我们将从左相机到右相机所需要的旋转角度平分，使得左右相机分别背离彼此旋转同样的角度使得它们的像平面共面，并分别构建旋转矩阵rl和rr。此时每个相机各自的主光轴都和原始两个相机的主光轴向量的矢量和方向一致。接下来就需要对齐两幅图像的像素行，我们定义矩阵Rrect将左侧相机的极点映射到无穷远处，并将极线水平对齐。首先使用极点向量e1创建一个旋转矩阵，将主点(cx, cy)定义为左侧图像的原点，两个相机投影中心的平移向量为T，则极点向量e1的计算公式如下。

向量e2必须和e1正交，但是这种选择有很多。一个好的选择是选取同时和向量e1以及主光轴正交的向量，该向量位于像平面上。则我们可以通过向量e1和主光轴的单位向量的叉乘来计算，对结果进行标准化后得到如下向量。

显然向量e3和e1及e2正交，其计算公式如下。

则能够将左侧相机内的极点映射到无穷远处的矩阵可以通过如下方式构建。

首先定义未变换之前的水平方向为x轴建立的图像坐标系为旧坐标系，变换后水平方向为x轴建立的图像坐标系为新坐标系。这里e1是极点方向在图像坐标系下的单位向量，这里使用了其转置结果，意味着使用了行向量，也就是列向量表示方式的逆矩阵。在右乘待变换点的前提下，e1表示为列向量时表示矩阵Rrect是将点从新坐标系映射到旧坐标系中。二这里取其逆矩阵旧可以将点从旧坐标系映射至新坐标系中，这样就可以将极点坐标映射到新坐标系的x轴上，进而映射到无穷远处。

上述矩阵将极线变换至水平方向，并将极点映射至无穷远处。则可以通过如下方式对齐两个相机的行。

我们也会计算左右两个相机校正后的内参矩阵Mrectl和Mrectr，并对其应用各自的投影矩阵pl及pr后将结果Pl和Pr返回。

正如前文介绍单个相机标定那样，这里的al和ar允许像素存在一定的偏斜，但在现代摄像机上几乎不存在这种偏斜，因此该值几乎总为0。该矩阵将一个三维齐次坐标系中的点映射到一个二维齐次坐标系中。其计算公式如下。

实际图像中的坐标(x, y) = (x/w, y/w)。通过相机的内参矩阵，同样能够把一个点在图像中的坐标重投影到三维空间中，重投影矩阵Q构造方式如下。

上述矩阵中的参数几乎都来自于左侧图像，除了c^‘^x是右图中主点的x轴方向坐标分量。如果主光轴相交于无穷远处，则c^‘^x = cx，也就是矩阵Q的右下角分量为0。给定一个二维的齐次坐标点，和其对应的视差d，我们可以使用如下公式将点重投影至三维空间中。

最后计算出该点在三维空间中对应的位置为(X/W, Y/W, Z/W)。

应用Bouguet校正算法能够得到理想立体配置下对应结果，算法会为旋转后的图像选择新的图像中心和边界，并确保两个图像重叠的视野区域最大。具体的做法是通过一个均匀的相机中心(Uniform Camera Center)，并选择两幅图像区域最大的公共宽和高来设置新的立体观察平面。

OpenCV中实现Bouget算法的原型如下，该函数的命名可能并不准确，因为它并不能校正立体图像，而是计算校正过程所需要的参数。给定两个相机的内参矩阵和畸变系数，以及它们之间的平移和旋转信息，该函数能够计算出校正立体图像所需的校正矩阵、投影矩阵、和重投影矩阵。通过这些信息我们能够从立体相机系统采集的图片对中提取模型的深度信息。

// cameraMatrix1：1号相机校正后的内参矩阵
// distCoeffs1：1号相机的畸变系数
// cameraMatrix2：2号相机校正后的内参矩阵
// distCoeffs2：2号相机的畸变系数
// imageSize： 相机标定时标定板的大小
// R：以1号相机坐标系为参考，从1号相机变换到2号相机相同姿态的旋转矩阵
// T：以1号相机坐标系为参考，从1号相机变换到2号相机相同位置的平移向量
// R1：1号相机的3x3矫正矩阵
// R2：2号相机的3x3矫正矩阵
// P1：1号相机的3x4投影矩阵，
// P2：2号相机的3x4投影矩阵
// Q：4x4重投影矩阵
// flags：设置立体相机组件姿态的标记，默认完美配置，即像平面共面，像素行共线
// alpha：缩放系数，-1执行默认逻辑，自定义的值取值范围为[0,1]
// newImageSize：矫正后的图像大小，设置为（0，0）时保持和输入图像相同的尺寸
// validPixROI1：1号图像中的有效像素区域，设置为0时表示全部有效
// validPixROI2：2号图像中的有效像素区域，设置为0时表示全部有效
void cv::stereoRectify(cv::InputArray cameraMatrix1, cv::InputArray distCoeffs1,
                       cv::InputArray cameraMatrix2, cv::InputArray distCoeffs2,
                       cv::Size imageSize, cv::InputArray R, cv::InputArray T,    
                       cv::OutputArray R1, cv::OutputArray R2,
                       cv::OutputArray P1, cv::OutputArray P2,
                       cv::OutputArray Q,
                       int flags = cv::CALIB_ZERO_DISPARITY,
                       double alpha = -1, cv::Size newImageSize = cv::Size(),
                       cv::Rect* validPixROI1 = 0, cv::Rect* validPixROI2 = 0);

参数cameraMatrix1、distCoeffs1、cameraMatrix2、distCoeffs2、R和T都可以通过函数cv::stereoCalibrate()计算。

参数flags设置了无穷远处的视差，默认值为0，即相机完美匹配，像平面共面，像素行共线的情况。正如前文提到有时为了提高立体相机系统在特定距离上的深度分辨率，我们可能需要刻意放置相机，使它们朝着彼此方向稍微倾斜，需要注意此时主光轴仍是平行的。此时视差为0的点就在有限距离内，这种情况下我们就可以将该参数设置为其他值。

如果参数flags未设置cv::CALIB_ZERO_DISPARITY，在立体校正时就必须小心处理。立体校正时基于主点(cx, cy)，即主光轴和像平面的交点。在计算深度时我们都需要对特征点的坐标进行修正，即xl = xl - cx_left，xr = xr - cx_right。当相机处于完美配置下，即参数flags默认值为cv::CALIB_ZERO_DISPARITY时，左右图像的主点坐标相同，即cx_left = cx_right，当flags设置为其他值时cx_left != cx_right。因此在使用深度计算公式Z = f✖️Tx/d时，对于参数flags设置为cv::CALIB_ZERO_DISPARITY的场景，d = xl - xr，其他情况下则d = xl - cx_left - xr + cx_right。

在后续校正立体图像调用函数initUndistortRectifyMap计算映射矩阵时，参数newImageSize作为该函数的输入参数。将它设置为比原图更大的尺寸有助于保留更多原图中的细节，特别是当存在较大的径向扭曲时这样做能够取得较好的效果。

5.6.3 校正映射

在计算出立体相机校正所必要的参数后，可以调用函数cv::initUndistorRectifyMap()分别为左右两个相机构建校正查询映射矩阵。和其他图像映射函数一样，在原图像中逐像素计算其在目标图像中的浮点位置的方式不能命中目标图像内的所有像素，这种方式处理后的结果看上去就像是一块瑞士奶酪️。因此我们需要将采用相反的流程，对于目标图像中的每个具有整型坐标的像素，我们反向查找它在原始图像中对应的浮点型坐标，并根据其周围的像素进行插值计算。这种插值方式通常使用双线性插值，可以通过前文介绍的函数cv::remap()实现。

正如下图中公式流描述的那样，校正过程是一个从图c向图a反向映射过程(Reverse Mapping)。对于图c中的每个像素而言，首先在未校正的图像b中找到对应的坐标，然后再到原属图像a中查找对应坐标，然后再根据该坐标附近的像素进行插值计算，并将结果填回到目标图像c中对应的位置。在目标图像c填充完成后，通常我们会对图像进行裁剪，从而突出左右两幅图像中重叠的区域。

需要注意的是在上图中，图c到图b的映射首先使用校正后的相机矩阵Mrect从校正后的图像坐标系映射至校正后的相机坐标系，再通过校正矩阵R映射到未校正前的相机坐标系。图b到图a的映射首先使用扭曲系数将未扭曲的像平面坐标映射至真实相机获取到的扭曲图像对应位置，然后再使用原始相机内参矩阵M将坐标映射至模型坐标系中。

计算校正映射的函数原型图像，通常我们需要为别对左右图像调用一次该函数，计算其映射矩阵。

// cameraMatrix：相机自身的内参矩阵
// distCoeffs：相机系统的扭曲系数，支持5✖️1矩阵
// R：3✖️3的校正矩阵
// newCameraMatrix：校正后的3✖️3相机内参矩阵矩阵，详情见下文
// size：无畸变的图像大小，决定映射矩阵map的尺寸
// m1type：映射矩阵map1的格式，可选CV_32C1，CV_16SC2，CV_16SC2
// map1：第一个映射表，描述主要的坐标
// map2：第二个映射表，描述更精细插值信息
void cv::initUndistortRectifyMap(cv::InputArray cameraMatrix,
                                 cv::InputArray distCoeffs,  
                                 cv::InputArray R,
                                 cv::InputArray newCameraMatrix,
                                 cv::Size size, int m1type,
                                 cv::OutputArray map1, cv::OutputArray map2);

参数newCameraMatrix表示校正后的相机内参矩阵矩阵，在立体相机系统经过校正后，其原始像平面和理想的像平面通常存在一定夹角，该矩阵可以将坐标从原始像平面投影至理想像平面。

如果我们使用函数cv::stereoRectify()校正立体相机，可以直接使用该函数的部分输出参数作为函数initUndistortRectifyMap的输入参数。可以将前者的输出参数Rl和Rr作为后者的输入参数R。将前者的3✖️4输出参数Pl和Pr作为后者的输入参数newCameraMatrix，后者内部会读取该矩阵的前3✖️3部分数据。将前者的输出参数newImageSize作为后者的输入参数size。

如果我们使用函数cv::stereoRectifyUncalibrated()标定立体相机，则我们需要对单应性进行一定的预处理。尽管在理论上和实践中可以在没有相机内参数的情况下校正立体相机，但是OpenCV中并未提供这类函数。如果在调用该函数之前未计算得到新的相机内参矩阵newCameraMatrix，则在调用该函数时直接将该变量设置为cameraMatrix，并且在传递参数R时首先使用如下公式预处理。并且还需要在参数distCoeffs中填入畸变系数。

函数cv::initUndistortRectifyMap执行完毕后会在参数map1和map2中填入映射矩阵，这两个矩阵定义了对于目标图像中的每一个像素在原始素材中的插值信息，并且它们可以作为函数cv::remap()的入参，从而计算出校正后的图像。下图演示了前文所述的校正过程，可以观察到分别对立体相机系统左右相机采集到的图像经过校正后，在得到的校正结果中，特征点已经水平对齐。

5.7 立体匹配

匹配三维空间中的对应点只能在立体相机得到两张图像的重叠区域内进行计算，这也是为什么尽可能将两个独立相机排列成接近完美的水平前向平行能够得到更好的结果，当然如果你是这方面的专家则可以违背该原则从而追求更精确的结果。如果能够确定相机的物理坐标，或者是场景中物体的大小，则可以使用两幅图像中对应点点三角差分计算出物体的深度信息，其中三角差分的计算公式如下。在缺失上述物理条件的情况下，我们只能计算出深度和位置的比例关系，如果没有相机的内参系数，在使用Hartley的算法时只能计算出投影变换意义上点的位置。

OpenCV中实现了两个不同的立体匹配算法，它们共享同一个对象接口。第一个算法是块匹配(Block Matching, BM)算法，它和Kurt Konolige发明的算法类似，是最高效和快速的算法，具体可参考论文《Small vision system: Hardware and implementation》。它使用一个较小的绝对值之差和(Sum of Absolute Difference, SAD)的窗口在校正后的左右立体图像中搜索匹配的点。该算法只搜索两个图像之间的强匹配，也就是强纹理。因此在一个例如户外森林的强纹理场景中每个像素都可能被计算出深度信息，在一个例如室内走廊的低纹理场景中，只能计算出很少点的深度。

第二个算法被称为半全局块状匹配算法(Semi-Global Block Matching, SGBM)，它是SGM算法的一个变体，可以在论文《Stereo Processing by Semiglobal Matching and Mutual Information》找到关于SGM算法的详细介绍。它和BM算法的区别主要有两个方面，第一该算法使用Birchfield-Tomasi度量在次像素的尺度上计算匹配点，Birchfield-Tomasi度量的细节可以参考论文《Depth discontinuities by pixel-to-pixel stereo》。第二个区别是该算法尝试对计算出的深度信息应用一个全局的平滑约束，该约束基于感兴趣区域的大量一维约束近似计算。这两个算法是互补的，BM算法尽管更快，SGBM算法却能提供更高的可靠性和精确性。

尽管两个立体匹配算法实现细节不同，但是它们都提供了一个基础能力，就是将立体系统校正后的左右图像转化为深度图。深度图中每个像素表示其在三维空间中对应的点到相机的距离，在OpenCV中通常，它表示的是立体系统中校正后的左侧相机图像，只不过这种惯例不是很常用。在OpenCV中这两个算法相关的类为cv::StereoBM和cv::StereoSGBM。

5.7.1 块匹配

OpenCV中块匹配的实现是对立体计算规范技术的稍加修改的版本，基本机制就是校正并对准图像，从而只需要在每行上进行比较，并且之后在两幅图像中搜索像素行进行像素组的匹配。当然有一些额外的细节从不同方面可以使算法工作得更好或者运行得更快。最终的结果是一个快速的，相对可靠的算法，并且仍然在很多应用程序内大量使用。

块匹配算法需要使用无畸变的，校正后的图像，其工作流程主要分为如下三步。

• 预处理阶段，主要是标准化图像亮度，并且加强纹理。
• 沿着水平极线使用SAD窗口进行匹配查找。
• 后处理以消除不良匹配。

在预处理阶段，我们对输入图像进行标准化处理，从而减少不同光照条件引入的影响。在这一步骤中需要使用到一个5✖️5，7✖️7(默认)，…，21✖️21(最大)尺寸的窗口。窗口中间像素的亮度值Ic的计算公式如下。其中I是窗口中所有像素的平均亮度，Icap是一个正值，它表示亮度的上限，默认为30。当然处理这种处理方案外，还可以将输入图像转换为x轴的Sobel导数，这种方式同样能够消除和环境亮度相关的误差。

接下来使用一个滑动SAD窗口来计算匹配像素，在右图的对应行上寻找最佳的匹配结果。经过校正后，每一行就是一条极线，因此对于左图的每个像素而言，它在右图的匹配点一定在同一行上，即它们的y轴方向坐标分量相同。如果特征存在足够强的可被检测到的纹理，并且他在右侧相机的视角中未被遮挡，则能够在右图中找到匹配的点。如下图所示，如果左图中待查找点像素坐标为(x0, y0)，则对于水平前向平行的完美立体系统而言，该点在右侧相机视图中对应点一定在y0行，并在位于x0点之前，包括x0点。其中x0点为0视差点，沿着x轴负方向，视差不断变大。图中下部分表示的时基于窗口的特征匹配函数。另外对于主光轴在有限距离内相交的立体相机而言，匹配点可能为负视差，即匹配点在x0的右侧。因此使用该算法时需要定义最小视差。

视差搜索会基于一个预定义的搜索范围，默认值时64像素。计算的视差结果具有4位分辨率的离散次像素精度，即除了整数部分外，它还可以表示64个级别的小数精度。当输出图像格式为32位浮点型时，将返回小数格式的结果。当输出图像格式为16位整型数据时，将会把小数部分转化为定点数，即乘16后取整，并封装在返回结果中。

设置最小和最大视差会建立一个双目视界(Horopter)，立体匹配算法的搜索区域将会覆盖这个三维空间。下图演示了三组不同的视差搜索范围，每个视差在三维空间中都定义了一个距离相机特定距离，且平行于成像平面的特定平面。如果在这个深度范围内并未搜索到匹配点，则在深度图中将会标记为0视差，即表现为一个视差空洞，表示深度未知。你可以通过减小摄像机之间的距离，减小焦距，增加立体搜索范围，或者增加像素宽度来扩大双面视界。

双目视界还隐含一条约束，即顺序约束(Order Constraint)，定义了从在左右两幅图中特征出现的顺序不会发生变化。由于遮挡和噪声的存在，可能某些特征在左相机能够观察到，但是在右相机不能观察到。尽管因此可能导致特征丢失，但是它们出现的顺序一定相同。其中右图相较于左图缺失的特征被称为缺失特征(Missing Featuees)，右图相较于左图多出的特征被称为插入特征(Insertion)。下图演示了这种顺序约束，三个特征点以相同的顺序出现在左右相机的视图中。

已知最小的视差增量，我们可以通过如下公式计算出深度分辨率。

在得到初步匹配结果后就进入了后过滤阶段，该阶段需要过滤掉错误匹配。上图底部展示了沿着最小视差到最大视差搜索特征的典型匹配函数响应。匹配项通常具有副瓣包裹中心峰值的特点。OpenCV通过uniqueness ratio参数来使用匹配函数模式。该比例本质上强制要求当前像素的匹配值需要大雨以某种余量方式观察到的最小匹配值。

下图演示了沿着扫描线进行的立体匹配，其中扫描线放大的部分在图幅中部放大显示，在图幅下部对匹配分配进行可视化展示。

为了确保有足够的纹理克服在匹配过程中产生的随机噪声，OpenCV引入了纹理阈值的概念。这只是对SAD窗口响应的一个约束，使响应低于某个最小值的匹配被排除。

最后，块匹配在物体的边界处可能会引发一些问题，因为在某侧匹配窗口捕获到的是前景图，而在另外一侧中捕获到的是背景图。这会产生一个具有视差大值和小值的区域，我们称为色斑(Speckle)。为了防止这种边界匹配，我们在色斑窗口(Speckle Window)上设置色斑检测器。具体的方法是将每个像素当作是一个可变范围漫水灌溉联通组件的构建基础。该可变范围漫水灌溉只有当某个相邻像素的亮度在包含当前像素的亮度值的某个区间内时，才将该相邻像素纳入联通分量。如果最后计算的联通分量比预先定义好的色斑窗口小，则该区域被认为是色斑。通常该可变区间的大小是一个小值，大多数时候是1或者2，超过和等于4的值并不常见。

5.7.2 使用函数cv::StereoBM计算视差

OpenCV中实现块匹配算法的接口类是cv::StereoBM，它继承于类cv::StereoMatcher，其中cv::StereoMatcher的部分定义如下。

class StereoMatcher : public Algorithm {
public:
  // 深度映射的返回数据格式可能为CV_16UC1，其中包含DISP_SHIFT位表示小数的定点数据
  enum {
    DISP_SHIFT = 4,
    DISP_SCALE = (1 << DISP_SHIFT)
  };

  // 深度图计算函数，输入位两个校正后的8位灰度图，输出位16位定点格式深度图
  virtual void compute(InputArray left, InputArray right, OutputArray disparity) = 0;

  // 最小视差，通常为0，即两个相机的主光轴相交于无穷远处
  virtual int getMinDisparity() const = 0;
  virtual void setMinDisparity(int minDisparity) = 0;

  // 能够搜索的视差个数
  virtual int getNumDisparities() const = 0;
  virtual void setNumDisparities(int numDisparities) = 0;

  // 块匹配的SAD窗口大小
  virtual int getBlockSize() const = 0;
  virtual void setBlockSize(int blockSize) = 0;

  // 散斑窗口的大小，联通分量低于该值时被认为是散斑
  virtual int getSpeckleWindowSize() const = 0;
  virtual void setSpeckleWindowSize(int speckleWindowSize) = 0;

  // 邻域像素亮度之间所运行的最大偏差，在灌水填充时会用到
  virtual int  getSpeckleRange() const = 0;
  virtual void setSpeckleRange(int speckleRange) = 0;
  ...
};

cv::StereoBM的部分定义如下。

class cv::StereoBM : public cv::StereoMatcher {
  enum {
    PREFILTER_NORMALIZED_RESPONSE = 0,
    PREFILTER_XSOBEL = 1
  };

  // 预过滤器类型，可选值为PREFILTER_NORMALIZED_RESPONSE和PREFILTER_XSOBEL
  virtual int getPreFilterType() const = 0;
  virtual void setPreFilterType(int preFilterType) = 0;

  // 选则PREFILTER_NORMALIZED_RESPONSE预过滤器时使用的窗口大小
  virtual int getPreFilterSize() const = 0;
  virtual void setPreFilterSize(int preFilterSize) = 0;

  // 预过滤器处理后的饱和度阈值
  virtual int getPreFilterCap() const  = 0;
  virtual void setPreFilterCap(int preFilterCap) = 0;

  // 纹理阈值，块纹理特征(梯度的绝对值之和)低于该值的像素将会被标记为深度未定义
  virtual int getTextureThreshold() const = 0;
  virtual void setTextureThreshold(int textureThreshold) = 0;

  // 如果成本函数在指定的视差范围内没有发现明确的优胜后选项，则像素的深度将会标记为未定义。
  // 而该参数定义了明确的界限，即最好和次好的后选项目之间的百分比。
  virtual int getUniquenessRatio() const = 0;
  virtual void setUniquenessRatio(int uniquenessRatio) = 0;

  // 构建函数，需要注意这里的第一个参数默认值为0，使用时必须指定一个有意义的值，
  // 后者构建实例后通过相关接口设置
  static Ptr create(int numDisparities = 0, int blockSize = 21);
};

其中参数blockSize越大，得到的错误匹配会更少。需要注意不仅算法的计算成本和SAD窗口的面积成正相关，而且隐含的假设即视差在窗口内是一致的还会引起其他问题。在不连续处即物体的边缘这种假设将不成立，可能无法找到匹配点。随着窗口大小的增加，这些空白区域的厚度也增加。同时更大的窗口可能会使得计算出的深度图变得模糊，意味着被观察对象的剪影也会变得更平滑，以近似的方式捕获真正的剪影。

通常在使用函数create()成功构建cv::StereoBM实例后，都会想要对预处理参数和后处理参数进行个性化定制。预处理器主要解决的是移除由于光照或者其他环境变化导致两幅图像在使用SAD窗口处理时产生的误差。枚举选项PREFILTER_NORMALIZED_RESPONSE表示简单对输入图像标准化处理，而PREFILTER_XSOBEL会将输入图像转化为其在x方向的一阶索贝尔导数。

后处理滤镜主要是移除异常和噪声。参数textureThreshold指定了对于某个特征能够进入后续的匹配步骤的对应SAD窗口响应最低值。而参数uniquenessRatio定义了明确的最佳匹配项和次好的匹配结果之间的关系，它们的具体约束由以下公式给出。这里d和d分别表示最佳和次佳的匹配项。通常该参数的取值范围为[5, 15]。

参数speckleWindowSize和speckleRange在后处理阶段一起过滤了明显不同于附近区域的孤立小散斑。其中speckleWindowSize定义了这些散斑的大小，speckleRange定义了散斑边界的深度差值上限。该值将会直接用于深度的比较，因此如果你使用了定点格式的深度数据，那么在比较时该值将会被乘以16。

当配置好cv::StereoBM实例后，就可以通过参数函数compute()计算深度图，得到的深度图将会使用定点表示法，其中4位表示小数部分，因此在使用这些数据时需要除以16。

5.7.3 半全局块匹配

OpenCV提供的另一个匹配算法是半全局匹配算法(Semi-Global Block Matching, SGBM)，它是SGM算法的变体，更多关于SGM算法的细节可以参考论文《Stereo Processing by Semiglobal Matching and Mutual Information》。SGM算法的发展较于块匹配算法几乎要晚10年，但是它的计算成本远高于块匹配算法。在当前硬件的性能条件下，即使在只有CPU资源的场景中，BM算法也能够应用与相对较高分辨率的图像中，并且能够以可接受的帧率处理实时视频。但是SGBM算法的计算时间成本要高出约一个数量级，因此通常被认为不适合实时视频应用。但是对于SGBM算法已经有FPGA的实现版本，它甚至可以在非常大的图像中以很高的速率运行。BM也有类似的FPGA的实现，同样支持GPU加速，并且已经被OpenCV支持。

SGM算法引入的最终要的思想就是使用互信息(Mutual Information)作为局部对应的更优越度量，和沿着除水平的极限以外其余方向执行的一致性约束。在OpenCV的实现版本中使用了Birchfield-Tomasi度量，在原论文中它被当作一个更简单的选项使用。在更高的层面上，这些额外的约束让算法在左右两幅图像处于复杂变化的光照和其他影响因素下具有更好的稳定性，同时实施更强的几何约束能够帮助消除误差。

和BM算法一样，SGBM处理的数据也是无畸变校正后的立体图像对，他的主要步骤如下。

• 就像StereoBM使用PREFILTER_XSOBEL模式预处理每幅图像那样，对于每个像素，该算法会使用Birchfield-Tomasi度量计算匹配左侧图像某个像素left_image(x, y)和右图像对应像素right_image(x-d, y)的成本C(x, y, d)。使用0初始化3D成本映射累加器S(x, y, d)。
• 对于下图中的3、5或者8个方向的情况，使用一个递归流程将每个r的计算值S(r)(x, y, d)累加到S(x, y, d)中。为了优化内存流和减少内存的使用，前三个或者前五个方向(W, E, N, NW, NE)在一次前向传递中合并处理，对于8方向模式而言，剩余的三个方向(S, SW, SE)会在第二次传递中处理。在3或者5方向模式中，不会存储所有像素的C(x, y, d)和S(x, y, d)数据，每次只需要存储缓存的最后三行或者四行。
• 完成S(x, y, d)的计算后，d(x, y)为S(x, y, d)取最小值的变量值。我们使用在StereoBM中讲到的一样的唯一性检查(Uniqueness)和次像素插值方法。
• 执行左-右检查确保左至右和右至左的对应是一致的，将不完美匹配标记为无效视差。
• 像StereoBM算法那样，使用函数cv::filterSpecles过滤散斑。

和其他任意立体匹配算法一样，该算法的关键元素是对于任意的视差，如何为每个像素分配成本，这里使用函数S(x, y, d)。本质上这与块匹配算法里面做的事情一致，但是SGBM算法中有一些新的变化。第一个变化是我们使用了一些更优的次像素Birchfield-Tomasi度量来比较像素，而不是简单的计算绝对差值。第二个变化是我们引入了一个非常重要的视差连续假设，即相邻像素具有相同或者相似的视差，同时我们使用更小的块尺寸，有时是3✖️3或者5✖️5，而BM算法使用了更大，完全独立的窗口，大于等于11✖️11。对于BM算法而言这些大的窗口会导致一些问题，在比较两个窗口时，当已知正确的视差时，必须首先假定这两个窗口应当匹配，这就意味着存在单个视差能够解释两个窗口之间的关系。在不连续处，即某个物体的边缘这种假设不成立，因此在这些地方将会产生很多问题。

在SGBM算法中，如下图对于BM算法使用大窗口的替代方案是使用较小的窗口(用于补偿噪声)结合路径。这些路径理论上是从图像边缘延伸至单个像素，并且理论上包含从边缘到该像素的所有可能路径。SGBM关注的是沿着路径成本最低的那类路径，和BM算法关注最低成本窗口类似。路径成本的计算方式是对于路径上每个像素，分别以它们为中心计算成本并累积，并且根据相邻像素视差发生变化的程度累积一个惩罚值，无变化时无惩罚值，视差越大惩罚值越大。

对于一个简单场景中的一些点，下图将一些可能的路径可视化。路径AGHIJ到达台灯链条的端点。沿着这个路径视差变化缓慢，因此在如EFGHIJ或者DHIJ等路径上，台灯链条这个路线是更优选择。为了计算点J的成本，关于点I我们需要知道的是BGHI是到点I的最低成本路径。实际上这些路径都是由一个个独立的像素组成的。

为了了解在没有沿着大量路径求和的情况下，这其实是不可能完成的任务，如何计算某些像素一个特定视差的成本，我们只需要意识到如果我们知道某个相邻像素在不同视差下的成本，唯一可能的情况就是到这个像素的最小成本路径来源于其中一个相邻像素。首先我们尽可能的计算这些像素所有可能视差的成本，找到最佳的视差，然后移动至其相邻像素。

我们应该意识到我们这里刻意忽略了一个重要的问题，那就是即使一个边缘像素，它的最低成本路径也有可能起源于图像对面。实际上并没有完全正确的起点，并且按序沿着每个方向处理整幅图像也是可能的。因此实际的算法并不会以逐像素的方式处理，但是具体算法的准确实现细节已经超过了本文的讨论范围，可以自行查阅文献。

理想情况下，我们需要考虑到达某个像素的每条可能路径。理论上这些路径不限于单个像素向其周围的8个相邻像素的连接方向，而是以任意角度到达这些像素。实际上，如下图，我们在计算时会限制路径可能的方向。理论上，为了追求更好的结果，最好将这个数值限制为8或者原论文中的16。对于很多真实场景而言，这样能够显著提高结果的质量，但是也会显著增加计算时间成本。在实践中，通常我们限制路径方向为5，甚至为3从而在大多数场景中得到质量损失不大的结果。单纯的讲，该算法的计算成本和考虑的路径数量是成线性相关的。但是有一个显著非线性相关的影响很容易被忽视，那就是当使用8个以上路径方向时，该算法为了处理巨大数组，需要存储所有像素的成本值C(x, y, d)和S(x, y, d)，这会导致该算法运行的速度更慢。

5.7.4 使用函数StereoSGBM计算视差

和类cv::StereoBM类似，OpenCV将半全局块匹配算法相关的参数封装在类StereoSGBM中，并提供重载后的接口compute用于计算视差。

class StereoSGBM : public StereoMatcher {
public:
  // 路径的搜索方向个数
  enum {
    // 5方向
    MODE_SGBM = 0,
    // 8方向
    MODE_HH = 1,
    // 3 方向，运行速度最快，质量最低
    MODE_SGBM_3WAY = 2
  };

  // 参考函数StereoBM
  virtual int  getPreFilterCap() const                 = 0;
  virtual void setPreFilterCap(int preFilterCap)       = 0;

  // 参考函数StereoBM
  virtual int  getUniquenessRatio() const              = 0;
  virtual void setUniquenessRatio(int uniquenessRatio) = 0;

  // 和相邻像素视差的差值等于1时，成本函数计算过程中需要累积的惩罚值
  virtual int  getP1() const                           = 0;
  virtual void setP1(int P1)                           = 0;

  // 和相邻像素视差的差值大于1时，成本函数计算过程中需要累积的惩罚值
  virtual int  getP2() const                           = 0;
  virtual void setP2(int P2)                           = 0;

  // 算法路径搜索方向数，从枚举值MODE_SGBM, MODE_HH, MODE_SGBM_3WAY中选择
  virtual int getMode() const                          = 0;
  virtual void setMode(int mode)                       = 0;

  // 构造函数
  // minDisparity：含义和StereoBM中一致，通常设置为0，表示立体系统两个相机主光轴相交于无穷远处
  // numDisparities：含义和StereoBM中一致，搜索的视差数量
  // blockSize：含义和StereoBM中一致，另外必须为奇数，但是这里通常设置为更小的数，3、或者5，
  //   甚至可以模拟原论文将其设置为1
  // P1、P2：设置为0时算法内部会根据图像分辨率和参数blockSize计算一个较优值
  // disp12MaxDiff：可信阈值，当匹配候选项从左到右的计算与从右到左的计算结果超过该值
  //   时会被标记为深度未知
  // preFilterCap、uniquenessRatio、speckleWindowSize、speckleRange：
  //   含义和StereoBM中一致
  // mode：算法路径搜索方向数
static Ptr create(int minDisparity, int numDisparities,
                              int blockSize, int P1 = 0, int P2 = 0,
                              int disp12MaxDiff = 0,
                              int preFilterCap = 0, int uniquenessRatio = 0,
                              int speckleWindowSize = 0, int speckleRange = 0,
                              int mode = StereoSGBM::MODE_SGBM);};

需要注意的是如前文所讲参数mode的选择会影响算法对内存的消耗，当取值为MODE_HH时，消耗的内参显著提高。实际的内存大致为图像的面积乘以可能的视差数量的乘积。对于分辨率为640✖️480的图像而言，如果存在1024个可能的视差。则如果每个视差结果使用16位存储，则总内存占用约为600MB，对于一个高清图像，内存占用可达4GB。

5.8 立体标定、校正和匹配

示例程序Comprehensive集成了立体标定、校正和匹配功能，他从一个名为《list.txt》的文件中读取一系列圆形网格模式图。这些图是从立体相机采集到的左右图像对，它们被用于标定相机，从而校正图像。需要注意仍然假定立体相机系统是大致理想匹配的，从而使得每个相机本质上拥有同样的视野。这有助于避免极点位于图像内部，并且同样有利于在最小化重投影误差的同时最大化立体相机的重叠区域。当极点位于某个图像内部时，OpenCV目前还不能够校正立体图像，更多的细节可以参考论文《A simple and efficient rectification method for general motion》。

示例程序首先读取了图像对，并查找次像素精度的圆形标记点，并对那些所有的标记点都成功被找到的图像设置目标或者图像点坐标。可以选择将这个过程显示在屏幕上。接下来对于这些计算好的坐标，调用函数cv::stereoCalibrate()标定相机，这一步可以计算得到单个相机的内参矩阵_M，畸变向量_D，同样能够计算出两个相机之间的旋转矩阵_R，平移向量_T，本征矩阵_E和基础矩阵_F。

接下来是一个小插曲，示例程序通过计算某个图像内的点和另外一副图像中的距离的累积值来评价标定的准确度。首先通过函数cv::undistortPoints()计算得到未畸变的图像，然后使用函数cv::computeCorrespondEpilines()计算对应的极线，接下来约束m2^t*F*m1=0来校验标定的质量。

代码接下来可以使用未标定的(Hartley)算法cv::stereoRectifyUncalibrated()或者标定后的(Bouguet)算法cv::stereoRectify()计算校正映射所需的参数。如果使用了未标定的校正，代码允许从头计算所需的基本居住，或者直接使用立体标定得到的基本矩阵。接下来示例程序使用函数cv::remap()计算校正后的图像。并且在图像对上绘制线条，用于帮助我们理解被校正的图像是如何被对齐的。

最后在校正完图像后，调用函数cv::StereoSGBM计算视差图，示例代码支持处理水平对齐或者垂直对齐的立体相机系统，但是对于垂直对齐的系统而言，函数cv::StereoSGBM只能够计算未标定的校正图像的视差，当然你也可以自己添加额外的代码转置图像从而应对这种情况。对于水平对齐的相机系统而言，无论校正的立体图像对是否经过标定，该算法都能够计算出视差图。

5.9 三维重投影和深度图

许多算法直接使用视差映射，例如检测物体是否在一张桌子上或者伸出桌面。但是对于三维形状匹配，三维模型学习，机器人抓取等场景，我们需要进行三维重建，或者需要深度映射。幸运的是目前为止我们建立的立体机制让这个任务变得简单。回想在立体相机标定时介绍的4✖️4重投影矩阵Q。以及通过视差d和一个二维的点P(x, y)，我们能够使用如下公式计算出三维景深。

最后通过透视除法得到最终的三维坐标(X/W, Y/w, Z/W)。值得注意的是矩阵Q隐含了相机的视线是否收敛或者说主光轴是否相交，以及相机的基线和两幅图像中的主点。因此我们不需要明确解释相机是否是收敛的或者是前向平行的，而是简单通过矩阵乘法提取深度。OpenCV提供了两个函数来完成这个功能。第一个函数我们已经很熟悉，它操作的是一系列点和它们关联的视差集合。其定义如下。

// src：双通道或者三通道的数组，或者2d、3d的向量
// dst：计算出的深度数组，大小和src相同，并且一一对应
// Q：3✖️3或者4✖️4重投影矩阵
void cv::perspectiveTransform(cv::InputArray src, cv::OutputArray dst,
                              cv::InputArray Q);

第二个函数处理的是整幅图像，其定义如下。

// disparity：视差图矩阵，格式为U8, S16, S32, 或者F32
// _3dImage：计算出的3维坐标，通道数为3，分别表示xyz坐标
// Q：3✖️3或者4✖️4重投影矩阵，通过函数stereoRectify()计算
// handleMissingValues：无法计算出对应点深度时输出矩阵的赋值策略
//    为true时如果未计算出深度，将会填充一个很大的值，目前是10000
//    为false时如果未计算出深度，则在输出图像中不会出现这个点
// ddepth：3维坐标输出矩阵_3dImage的数据格式，可以是CV_16S，CV_32S或者CV_32F（默认值）
void cv::reprojectImageTo3D(cv::InputArray disparity, cv::OutputArray _3dImage,
                            cv::InputArray Q, cv::bool handleMissingValues = false,
                            cv::int ddepth = -1);

当然上述两个函数都能够处理任意的透视变换，它们可以通过函数cv::stereoRectify直接计算，或者在此基础上叠加任意的三维旋转、平移变换等。对一个杯子和椅子素材的视差图（使用SGBM算法计算）和二维坐标组合调用函数reprojectImageTo3D的处理结果如下。

6 来自运动的结构

在移动机器人和更广泛的视频图像分析领域，如出列来自手持摄像机的数据，来自运动的结构(Structure From Motion)是一个重要的话题。来自运动的结构是一个很宽的话题，并且这个方向已经做了大量的研究工作。然而我们可以通过简单的观察完成大量的工作，在静态的场景中，使用移动相机在两个不同时刻采集到的图像和使用两个相机在不同位置同一时刻采集到的数据并没有区别。因此，我们所有的直觉以及数学和算法机制都可以迅速移植到这种环境下。当然这里有一个很关键的词就是静态场景，但是在实际的应用中场景可以是静态的，也可以是近似静态的，其中少量的移动点可以通过稳定拟合的方式将它们当作异常点处理。

考虑一个相机穿越建筑物的场景，如果环境中存在大量的可被识别的特征，那我们就能够在连续的帧内计算出对应点。例如我们可以使用OpenCV实现光流技术而提供的函数cv::calcOpticalFlowPyrLK()寻找匹配项。如果我们能够在帧间追踪到足够多的点，我们就能够重建摄像机的运动轨迹。前文提到我们在三维重建的时候需要知道立体系统的本征矩阵E，他可以通过基础矩阵F和相机的内参矩阵M计算得到。我们可以从移动的相机获取到的视频流内连续的一对帧中提取出这些信息。在知道相机的移动轨迹后，我们就能够继续构建建筑物的整体三维结构，并且计算在该建筑物上所有上述特征的位置。在本文撰写时，全新的模块opencv_contrib/modules/sfm包含了一个即用型的SfM管线实现和对应的使用教程。它是2015年谷歌代码之夏(Google Summer of Code, GSoC)的项目成果。在后记B中还会有更详细的示例代码，其中使用了libmv和Ceres库，并且附带这两个库的下载方法。

7 二维和三维直线拟合

本章最后要讨论的一个话题是常规的线拟合。该话题可能处于很多原因，或者在很多背景下被提及。我们在这里讨论它是因为一个非常频繁的使用场景是分析三维点，尽管这里介绍的函数也能用于拟合二维线。线性拟合算法通常使用静态的稳健性技术，详情可以参考论文《Robust line fitting using LmedS clustering》、《Robust regression methods for computer vision: A review》、《Robust Regression and Outlier Detection》。OpenCV提供如下函数实现线性拟合。

// points：二维或者三维点，格式可以是N✖️2或者2✖️N的矩阵，或者是Point2d的向量等
// line：拟合得到的线，数组元素可以是Vec4f (2d)或者Vec6f (3d)
// distType：距离度量类型，参考下文
// param：在使用特定距离度量类型时传入的常量参数C，参考下文
// reps：精度半径
// aeps：精度角度
void cv::fitLine(cv::InputArray points, cv::OutputArray line,
                 int distType, double param, double reps, double aeps);

参数distType指定了线性拟合时，在计算全局最小距离时的度量方法，其取值和具体计算公式如下图。参数param指定的就是图中各个公式中的系数C的值。如果该参数被设置为0，则算法会使用图中的默认值。

参数line指定了一个用于保存拟合线的矩阵，当参数point中存储的是二维点时，该参数中的元素类型为包含4个浮点型数据的STL风格数组，如cv::Vec4f。当参数point中存储的是三维点时，该参数中的元素类型为包含6个浮点型数据的STL风格数组，如cv::Vec6f。对于4个元素的场景，可以表示为(vx, vy, x0, y0)，其中(vx, vy)表示和拟合线平行的单位向量，(x0, y0)表示这条线上的一个点。类似的对于6个元素的场景，可以表示为(vx, vy, vz, x0, y0, z0)，其中(vx, vy, vz)表示和拟合线平行的单位向量，(x0, y0, z0)表示这条线上的一个点。参数reps表示点的精确度，即(x0, y0, z0)的精确度。而参数aeps表示方向的精确度，即(vx, vy, vz)的精确度。在OpenCV的文档中，这两个参数的建议取值为0.01。

示例程序线性拟合首先直线附近的二维噪声点进行合成，然后引入一些对直线没有任何影响的随机点，也就是离群点，最后通过这些点拟合出一条直线并将其显示出来。函数cv::fitLine可以很好的忽略离群点，这在测量可能被高噪声和传感器故障等因素破坏的应用程序中很重要。

8 小结

本章论文的开通我们回顾了相机系统的几何结构，并介绍了将点从三维世界中映射到成像平面的基本映射，这被称为投影变换。在一些场景中，特别是当我们知道一个点集位于某个平面上时，这个变换是可逆的。尽管在更普遍的情况下这种变换是不可逆的，但是当我们在多个图像中确定同样的点集后，我们仍然可以重建三维场景，或者判断已知目标的姿态。在本系列文章的后记B中还会介绍在opencv_contrib中定义的其他标定算法，它们用于处理全景相机和多个相机(位于模块ccalib)，不同类型校准模式(位于模块aruco和ccalib中)，色彩平衡和降噪(位于模块xphoto中)场景。

接下来我们介绍了如果使用这种相似的几何信息重建立体深度测量的，为了使计算结果更加可靠，我们需要计算立体系统的单个摄像机之间的准确排列关系，这一步称为立体标定（Stereo Calibration）。立体相机标定完成后，就可以从OpenCV提供了两个算法中选择一个，并计算视差值，并最终通过重投影变换计算出深度值。这两个方法中块匹配算法更快，但是对场景覆盖的完整性更低。而半全局块匹配算法能够得到质量远高于块匹配算法的结果，但是会显著提高计算时间成本。

最后我们介绍了一些有用的功能，包括三维点和线的处理，例如投影变换，重投影和线拟合。