双目测距
双目视觉测距
Write By Champrin From 2022-12-09 To 2022-12-31
GUET Evolution Team Visual Group
双目图像生成三维点云
双目视觉测距原理，数学推导及三维重建资源
双目立体视觉：块匹配视差图计算（WLS滤波）
《Learning OpenCV3》ch19：双目标定与立体匹配
单目系统对目标物体距离并非真正意义上的测量，准确度较低
双目系统对目标物体距离感知是一种绝对的测量，而非估算
双目检测原理
视差(disparity)
双目摄像头的原理与人眼相似。人眼能够感知物体的远近，是由于两只眼睛对同一个物体呈现的图像存在差异，也称“视差”。
物体距离越远，视差越小；反之，视差越大。视差的大小对应着物体与眼睛之间距离的远近
视差是同一个空间点在两个相机成像中对应的x坐标的差值，它可以通过编码成灰度图来反映出距离的远近，离镜头越近的灰度越亮
上图中：人在前，椰子树在后
双目的角度不一样（见最下方图像），左侧相机成像中人在树的右侧，右侧相机成像中人在树的左侧
对比两幅图像：人眼观察树的时候视差小，而观察人时视差大（见最右侧图像），这是因为与树的距离远，与人的距离近
这就是双目三角测距的原理。
理想双目相机成像模型
理想双目相机成像模型下：左右目相机的光轴平行、图像坐标系平面共面且平行于基线、左右目相机的参数一致（焦距、内参等）等
\(P\)为目标上的一点，其在左、右目相机的图像坐标系中对应点为\(P_L\)、\(P_R\)
\(O_L\)、\(O_R\)为左、右目的图像坐标系原点，也即左、右目CCD图像平面的中心
\(X_L\)为\(P_L\)到\(O_L\)的水平距离，\(X_R\)为\(P_R\)到\(O_R\)的水平距离
\(b\)为基线，表示左、右目镜头中心之间的距离
\(f\)为左、右目相机的焦距
\(z\)为双目测距所需求得的距离，也即深度信息
设\(disparity\)为视差，即有\(disparity = X_L - X_R\)
如图可得，根据几何关系有：
根据相似三角形：
根据以上理想模型，可见，要求得目标点的距离\(z\)有三个条件：
- 相机焦距\(f\)
- 左、右目镜头中心之间的距离(基线)\(b\)
- 视差\(disparity = X_L - X_R\)
一般双目相机成像模型
理想模型只是理想模型，现实中存在这种理想模型的可能性？
理解好理想模型，为解决现实中的一般双目相机成像模型问题提供了基础
在一般双目相机成像模型中，左右目相机的光轴是不平行的、图像坐标系平面是不共面、左右目相机的参数是不一致的（焦距、内参等）等
\(O_1-x_1y_1z_1\)、\(O_2-x_2y_2z_2\)为左、右目相机的相机坐标系
\(O_L-X_LY_L\)、\(O_R-X_RY_R\)为左、右目相机的图像坐标系
\(O_L\)、\(O_R\)为左、右目的图像坐标系原点，也即左、右目CCD图像平面的中心
设左目相机的相机坐标系也为世界坐标系
\(P\)为目标上的一点，其在世界坐标系(左目相机的相机坐标系)下的坐标为\((x,y,z)\)，
在右目相机的相机坐标系下的坐标为\((x_r,y_r,z_r)\)，
在左、右目相机的图像坐标系中对应点为\(P_1(x_L,y_L)\)、\(P_2(x_R,y_R)\)
\(z\)为双目测距所需求得的距离(在世界坐标系下的\(z\)轴坐标)，也即深度信息
\(b\)为基线，表示左、右目镜头中心之间的距离
\(f_L\)、\(f_R\)为左、右目相机的焦距
设\(disparity\)为视差，即有\(disparity = X_L - X_R\)
根据孔成像模型以及相似三角形：
对于左目相机坐标系(世界坐标系)有：
整理：
对于右目相机坐标系有：
整理：
提前说明：经过双目相机标定后可获得左目相对于右目相机坐标系的旋转矩阵\(R\)及平移矩阵\(t\)
将右目相机坐标系中的点转为世界坐标系，也即转为左目相机坐标系上的点就有：
其中：
展开：
联立\((1.1)(1.2)(1.3)\)：
其中，据参考资料所述，\((1.5)\)所计算的距离误差与\((1.4)\)相比较大
原因是：一般在实际中，放置左右两个相机时，会使其保持在同一水平线上，若有扰动时，用\(y\)轴坐标来算\(z\)(即为\((1.5)\))，\(y_L\)或\(y_R\)其中的一个值会非常的小，同时它俩又做分母，就会使得算出来的\(z\)变大
因此，一般使用\((1.4)\)
极线约束
为了计算视差，就需要像左右图像的像素点互相匹配
比如，在左目所成图像中的一个像素点，在右目所成图像中如何确定这个像素的位置？
遍历整个右图像显然是不现实的，此时就需用到极限约束
在理想情况下，左、右目相机图像坐标系平面共面且光轴平行，参数相同
\(O_1\)、\(O_2\)是左、右目相机坐标系原点，\(P\)为目标上的一点
这三个点形成了三维空间中的一个平面\(PO_1O_2\)，称为极平面(Epipolar plane)
\(P\)在左、右目相机的图像坐标系中对应点为\(P_1\)、\(P_2\)，\(O_1O_2\)在左、右目相机图像坐标系平面的交点为\(e_1\)、\(e_2\)
极平面和左、右目图像坐标系平面相交于两条直线\(P_1e_1\)、\(P_2e_2\)，这两条直线称为极线(Epipolar line)
极线约束(Epipolar Constraint)描述的是当同一个点投影到两个不同视角的图像上时，像点、相机光心在投影模型下形成的约束
也就是若已知左目成像(投影)点\(P1\)，那么对应右目成像(投影)点\(P2\)一定在相对于\(P1\)的极线\(P_2e_2\)上
这就是极线约束，可以缩小特征点匹配时搜索范围
但是在一般情况下，左、右目的极线不共线也不水平，
已知左目成像(投影)点，那么就需要计算在右目成像平面的极线对应的直线方程，问题还是相对复杂的
例如：左图中的三个十字标志点，在右图中对应的极线(搜索区域)是右图中的三条白色直线
我们看到这三条直线并不是水平的，如果进行逐点搜索效率非常低。
极线校正
除了建立一般双目相机成像模型去计算距离的方法，还有一种方法是将一般双目相机成像模型变为理想双目相机成像模型，这个过程就称为极线校正
极线矫正是在把两个图像坐标系不共面的平面，经过旋转相机坐标系，将这两个平面重新投影到一个共面的平面、共面的平面平行于基线、两条光轴互相平行，以此转化为理想情况的模型
图中红绿虚线所围表示校正后的平面
经过这样的校正，使得空间某一点的两条极线共线且水平，也就使得了空间某一点在左右两幅图中的像素位置位于同一行
因为左右两幅图中的像素点在同一行，也就是已知左目成像(投影)点情况下，寻找右目对应成像(投影)点只需要按行号搜索，也就是只需要沿着水平的极线方向搜索对应点就可以了，这样的搜索效率明显非常快
立体校正
总的来说，立体校正是通过左右目各自的相机内参以及畸变系数和左右目相机坐标系的旋转矩阵和平移向量，先畸变矫正，后进行极线校正，使得左右相机成像与理想模型一致。
附：MATLAB 标定后所展现的立体校正后的效果
可见每个像素点可由对应极线一一联系起来
立体匹配
极线校正是为了匹配左右两幅图中的像素点提供了方便，如何匹配像素点还要另找方法
但是由于要保证两个相机参数完全一致是不现实的，并且外界光照变化和视角不同的影响，使得单个像素点鲁棒性很差
所以匹配工作是一项很重要的事情，这也关系着双目视觉测距的准确性
大部分立体匹配算法的计算过程可以分成以下几个阶段：匹配代价计算、代价聚合、视差优化、视差细化
立体匹配是立体视觉中一个很难的部分，主要困难在于：
- 图像中可能存在重复纹理和弱纹理，这些区域很难匹配正确
- 由于左右相机的拍摄位置不同，图像中几乎必然存在遮挡区域，在遮挡区域，左图中有一些像素点在右图中并没有对应的点，反之亦然
- 左右相机所接收的光照情况不同
- 过度曝光区域难以匹配
- 倾斜表面、弯曲表面、非朗伯体表面
- 较高的图像噪声等
常用的立体匹配方法基本上可以分为两类：
- 局部方法（例如，BM、SGM、ELAS、Patch Match等）
- 非局部方法，即全局方法（例如，Dynamic Programming、Graph Cut、Belief Propagation等）
局部方法计算量小，但匹配质量相对较低
全局方法省略了代价聚合而采用了优化能量函数的方法，匹配质量较高，但是计算量也比较大
目前OpenCV中已经实现的方法有BM(cuda)、binaryBM、SGBM(cuda)、binarySGBM、Bellief Propogation(cuda)、Constant Space Bellief Propogation(cuda)这几种方法
比较好用的是SGBM算法，其中匹配代价部分使用的是具有一定像素采样不变性的BT代价（原图+梯度图），并且涵盖了SGM的代价聚合方法
由于这一部分原理比较深奥，暂时先浅入了解使用立体匹配算法时，各个参数的如何选取对应会有什么效果
有关深入原理，之后再补充学习
【双目视觉】 立体匹配算法原理之“代价函数”
【双目视觉】 立体匹配算法原理之“代价空间与聚合、视差计算”
双目立体匹配算法：SGM
双目系统的难点
摘自双目视觉测距原理，数学推导及三维重建资源
摘下来以便以后遇到问题，查看是否是遇到了这些难点
1.计算量非常大，对计算单元的性能要求非常高，这使得双目系统的产品化、小型化的难度较大。所以在芯片或FPGA上解决双目的计算问题难度比较大。国际上使用双目的研究机构或厂商，绝大多数是使用服务器进行图像处理与计算，也有部分将算法进行简化后，使用FPGA进行处理。
2.1 对环境光照非常敏感。双目立体视觉法依赖环境中的自然光线采集图像，而由于光照角度变化、光照强度变化等环境因素的影响，拍摄的两张图片亮度差别会比较大，这会对匹配算法提出很大的挑战。
2.3 计算复杂度高。该方法需要逐像素匹配；又因为上述多种因素的影响，为保证匹配结果的鲁棒性，需要在算法中增加大量的错误剔除策略，因此对算法要求较高，想要实现可靠商用难度大，计算量较大。
2.4 相机基线限制了测量范围。测量范围和基线（两个摄像头间距）关系很大：基线越大，测量范围越远；基线越小，测量范围越近。所以基线在一定程度上限制了该深度相机的测量范围。
双目测距流程
双目测距的大致流程：
1=>operation: 双目标定
2=>operation: 立体校正(含消除畸变)
3=>operation: 立体匹配
4=>operation: 视差计算
5=>operation: 深度计算(3D坐标)计算
1->2->3->4->5
双目标定
目的是获得双目相机的：
- 内参 左右目的内参矩阵
- 外参 左目到右目的旋转矩阵和平移矩阵，或者说是右摄像头相对于左摄像头的旋转矩阵和平移矩阵
- 畸变系数 左右目的径向畸变系数(k1，k2，k3)和切向畸变系数(p1，p2)，以及其他一些畸变类型
《Learning OpenCV》中对于外参(旋转矩阵和平移矩阵)的图示是这样的:
畸变矫正
void cv::undistort(
InputArray src,
OutputArray dst,
InputArray cameraMatrix,
InputArray distCoeffs,
InputArray newCameraMatrix = noArray()
)
src: 输入image
dst: 与输入同大小、类型的输出去畸变图像
cameraMatrix: 内参K
distCoeffs: 畸变系数
newCameraMatrix: 默认与输入的K相同，但也可以手动进行scale和shift变换，即:利用getOptimalNewCameraMatrix设置参数获取新的投影矩阵.
极线校正
极线校正的目的是将拍摄于同一场景的左右两个视图进行数学上的投影变换，使得两个图像平面共面且平行于基线，简称共面行对准。经过这样的校正过程之后，两幅图中的极线就会完全水平，从而导致空间中的同一个点在左右两幅图中的像素位置位于同一行。在达到共面行对准以后就可以应用三角原理计算距离。
OpenCV中实现了cv::stereoRectify()函数做立体校正，内部采用的是Bouguet的极线校正算法；
立体校正后，将图片拼接，绘制等距离的平行线检验校正的结果
立体匹配
参考资料：
关于双目立体视觉的三大基本算法及发展现状的总结
OpenCV SGBM 文档
OpenCV3.2 双目摄像头标定与SGBM算法验证
