本文记录如何根据双目相机得到的两帧图片数据来计算出三维点云。这里主要涉及到双目视觉的成像原理，在《视觉SLAM十四讲》第2版的第五讲中有详细介绍。本文的代码根据书上给出的代码进行编写，数据来自https://github.com/gaoxiang12/slambook2/tree/master/ch5/stereo。

众所周知，单目相机是没法确定照片中的某个像素在三维空间中的具体位置的，因为它损失了深度信息。而双目相机就像人的双眼一样，可以根据左右眼看到物体的视差来确定深度，当人用一只眼睛看的时候，深度信息就变得不可靠了。

双目相机一般由左眼相机和右眼相机两个水平放置的相机组成（当然也可以上下放置，通常为水平放置）。对于双目相机，可以把两个相机分别看成针孔相机。这两个相机的光心分别记为 O L O_L OL​和 O R O_R OR​， O L O_L OL​和 O R O_R OR​的连线叫做【基线b】。

如下图所示，为双目视觉测距示意图：

已知相机内参，焦距 f f f，像素坐标系原点到成像平面坐标系原点的偏移，分别为 x x x轴和 y y y轴的偏移量记为 c x   c y c_x\ c_y cx​ cy​以及双目相机的基线长度 b b b。成像坐标系的 x x x轴和 y y y轴与相机坐标系的 x x x轴和 y y y轴同向，通常 x x x轴指向右边（在相机后面看，而不是镜头前）， y y y轴指向下边。而相机坐标系比成像坐标系多一个维度 z z z轴， z z z轴为镜头朝外方向。

对于空间中一点 P P P，其在相机坐标系下的坐标为 ( x , y , z ) (x, y, z) (x,y,z)，在像素坐标系下的坐标为 ( u , v ) (u, v) (u,v)。

根据小孔成像模型可知： { x = ( u − c x ) / f ⋅ z y = ( v − c y ) / f ⋅ z \begin{cases} x = (u-c_x)/f \cdot z\\ y = (v-c_y)/f\cdot z \end{cases} {x=(u−cx​)/f⋅zy=(v−cy​)/f⋅z​

再根据双目相机模型的相似三角形可知： z − f z = b − U L + U R b → z = f b U L − U R \frac{z-f}{z}=\frac{b-U_L+U_R}{b}\to z=\frac{fb}{U_L-U_R} zz−f​=bb−UL​+UR​​→z=UL​−UR​fb​ 其中 b b b为基线的长度（即左右相机光心的距离），记点 P P P投影到成像平面上的点为 P ′ P' P′，则 U L U_L UL​和 U R U_R UR​分别为 P ′ P' P′在成像平面坐标系下的 x x x轴坐标（因为双目相机水平放置），而在右边相机的成像平面上， P ′ P' P′位于成像平面坐标系原点的左边，所以 U R U_R UR​为负值，计算的时候要加个负号， U L − U R U_L-U_R UL​−UR​即为视差。

根据上述小孔成像模型和双目相机测深度模型就可以根据双目相机拍到的两帧图像把图像中的像素转到三维空间的点了，也就是转成点云。在OpenCV中集成了 S t e r e o S G B M StereoSGBM StereoSGBM 这个类可以直接输入两张图像计算得到每个像素点的视差。所以现在， P P P在三维空间中的坐标 ( x , y , z ) (x,y,z) (x,y,z)为需要求的值，根据上面提到的两个公式直接就可以计算出来了。

这里我把书上的代码稍微改了一点点。