Overview

欢迎访问 持续更新：https://spatial-ai.net/stereo-vision-reconstruct.html

双目立体视觉的整体流程包括：

• 图像采集
• 双目标定
• 双目矫正
• 立体匹配
• 三维重建

1. 图像采集

双目相机采集 左右目图像

2. 双目标定

通过 双目标定工具 对双目相机进行标定，得到如下结果参数：

内参	外参
相机矩阵 $K_1,K_2$	旋转矩阵 $R$
畸变系数 $D_1,D_2$	平移向量 $t$

《Learning OpenCV》中对于 Translation 和 Rotation 的图示是这样的:

示例代码：

cv::Matx33d K1, K2, R;
cv::Vec3d T;
cv::Vec4d D1, D2;

int flag = 0;
flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
flag |= cv::fisheye::CALIB_CHECK_COND;
flag |= cv::fisheye::CALIB_FIX_SKEW;

cv::fisheye::stereoCalibrate(
        obj_points_, img_points_l_, img_points_r_,
        K1, D1, K2, D2, img_size_, R, T,
        flag, cv::TermCriteria(3, 12, 0));

3. 双目矫正

双目矫正 主要包括两方面：畸变矫正 和 立体矫正 。

利用 OpenCV的函数，主要分为

• stereoRectify
• initUndistortRectifyMap
• remap

stereoRectify

根据双目标定的结果 $K_1,K_2,D_1,D_2,R,t$，利用 OpenCV函数 stereoRectify，计算得到如下参数

• 左目 矫正矩阵(旋转矩阵) $R_1$ (3x3)
• 右目 矫正矩阵(旋转矩阵) $R_2$ (3x3)
• 左目 投影矩阵 $P_1$ (3x4)
• 右目 投影矩阵 $P_2$ (3x4)
• disparity-to-depth 映射矩阵 $Q$ (4x4)

其中，

左右目投影矩阵（horizontal stereo, ${c_x}_1^{\prime }={c_x}_2^{\prime }$ if CV_CALIB_ZERO_DISPARITY is set）

$$P_1=\left[\begin{array}{cccc}{f}^{\prime }& 0& {c_x}_1^{\prime }& 0\\ 0& f^{\prime }& c_y^{\prime }& 0\\ 0& 0& 1& 0\end{array}\right]$$

$$P_2=\left[\begin{array}{cccc}{f}^{\prime }& 0& {c_x}_2^{\prime }& t_x^{\prime }\cdot f^{\prime }\\ 0& f^{\prime }& c_y^{\prime }& 0\\ 0& 0& 1& 0\end{array}\right]$$

where

$$t_x^{\prime }=-B$$

disparity-to-depth 映射矩阵

$$Q=\left[\begin{array}{cccc}1& 0& 0& -{c_x}_1^{\prime }\\ 0& 1& 0& -c_y^{\prime }\\ 0& 0& 0& f^{\prime }\\ 0& 0& -\frac{1}{t_x^{\prime }}& \frac{{c_x}_1^{\prime }-{c_x}_2^{\prime }}{t_x^{\prime }}\end{array}\right]$$

通过 $P_2$ 可计算出 基线 长度:

$$\begin{array}{r}\text{baseline}=B=-t_x^{\prime }=-\frac{{P_2}_{(03)}}{f^{\prime }}\end{array}$$

示例代码：

cv::Mat R1, R2, P1, P2, Q;
cv::fisheye::stereoRectify(
        K1, D1, K2, D2, img_size_, R, T,
        R1, R2, P1, P2, Q,
        CV_CALIB_ZERO_DISPARITY, img_size_, 0.0, 1.1);

CameraInfo DKRP

参考：sensor_msgs/CameraInfo Message

• D: distortion parameters.

  • For “plumb_bob”, the 5 parameters are: (k1, k2, t1, t2, k3)

• K: Intrinsic camera matrix for the raw (distorted) images.

  • Projects 3D points in the camera coordinate frame to 2D pixel coordinates using the focal lengths (fx, fy) and principal point (cx, cy).
    $$\mathbf{K}=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$$

• R: Rectification matrix (stereo cameras only).

  • A rotation matrix aligning the camera coordinate system to the ideal stereo image plane so that epipolar lines in both stereo images are parallel.
  • For monocular cameras $\mathbf{R}=\mathbf{I}$

• P: Projection/camera matrix.

  • For monocular cameras
    $$\mathbf{P}=\left[\begin{array}{cccc}{f}_x& 0& c_x& 0\\ 0& f_y& c_y& 0\\ 0& 0& 1& 0\end{array}\right]$$
  • For a stereo pair, the fourth column [Tx Ty 0]’ is related to the position of the optical center of the second camera in the first camera’s frame. We assume Tz = 0 so both cameras are in the same stereo image plane.
    • The first camera
      $$\mathbf{P}=\left[\begin{array}{cccc}{f}_x^{\prime }& 0& c_x^{\prime }& 0\\ 0& f_y^{\prime }& c_y^{\prime }& 0\\ 0& 0& 1& 0\end{array}\right]$$
    • The second camera
      $$\mathbf{P}=\left[\begin{array}{cccc}{f}_x^{\prime }& 0& c_x^{\prime }& -f_x^{\prime }\cdot B\\ 0& f_y^{\prime }& c_y^{\prime }& 0\\ 0& 0& 1& 0\end{array}\right]$$
  • Given a 3D point $[XYZ{]}^{\prime }$, the projection $(x,y)$ of the point onto the rectified image is given by:

  $$\left[\begin{array}{c}u\\ v\\ w\end{array}\right]=\mathbf{P}\cdot \left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right],\{\begin{array}{l}x=\frac{u}{w}\\ y=\frac{v}{w}\end{array}$$

initUndistortRectifyMap

左右目 分别利用 OpenCV函数 initUndistortRectifyMap 计算 the undistortion and rectification transformation map，得到

• 左目map: $ma{p}_1^l,ma{p}_2^l$
• 右目map: $ma{p}_1^r,ma{p}_2^r$

示例代码：

cv::fisheye::initUndistortRectifyMap(K1, D1, R1, P1, img_size, CV_16SC2, rect_map_[0][0], rect_map_[0][1]);
cv::fisheye::initUndistortRectifyMap(K2, D2, R2, P2, img_size, CV_16SC2, rect_map_[1][0], rect_map_[1][1]);

Remap

左右目 分别利用 OpenCV函数 remap 并根据 左右目map 对左右目图像进行 去畸变 和 立体矫正，得到 左右目矫正图像

示例代码：

cv::remap(img_l, img_rect_l, rect_map_[0][0], rect_map_[0][1], cv::INTER_LINEAR);
cv::remap(img_r, img_rect_r, rect_map_[1][0], rect_map_[1][1], cv::INTER_LINEAR);

4. 立体匹配

根据双目矫正图像，通过 BM或SGM等立体匹配算法 对其进行立体匹配，计算 视差图

视差计算

通过 OpenCV函数 stereoBM (block matching algorithm)，生成 视差图(Disparity Map) (CV_16S or CV_32F)

disparity map from stereoBM of OpenCV :
It has the same size as the input images. When disptype == CV_16S, the map is a 16-bit signed single-channel image, containing disparity values scaled by 16. To get the true disparity values from such fixed-point representation, you will need to divide each disp element by 16. If disptype == CV_32F, the disparity map will already contain the real disparity values on output.

So if you’ve chosen disptype = CV_16S during computation, you can access a pixel at pixel-position (X,Y) by: short pixVal = disparity.at<short>(Y,X);, while the disparity value is float disparity = pixVal / 16.0f;; if you’ve chosen disptype = CV_32F during computation, you can access the disparity directly: float disparity = disparity.at<float>(Y,X);

• Disparity Map
• Disparity map post-filtering

5. 三维重建

（1）算法1：根据视差图，利用 $f^{\prime }$ 和 $B$ 通过几何关系计算 深度值，并利用相机内参计算 三维坐标

根据上图相似三角形关系，得

$$\frac{Z}{B}=\frac{Z-f}{B-d_w}⟹Z=\frac{Bf}{d_w}$$

其中，$f$ 和 $d_w$ 分别为 成像平面的焦距和视差，单位均为 物理单位，将其转换为 像素单位，上式写为

$$Z=\frac{B{f}^{\prime }}{d_p}$$

其中，

$$d_p=(O_r-u_r)+(u_l-O_l)=(u_l-u_r)+(O_r-O_l)$$

最终，深度计算公式如下，通过遍历图像可生成 深度图

$$Z=\text{depth}=\frac{B\cdot f^{\prime }}{d_p}\text{with}{d}_p=\text{disp}(u,v)+({c_x}_2^{\prime }-{c_x}_1^{\prime })$$

根据 小孔成像模型，已知 $Z$ 和 相机内参 可计算出 三维点坐标，从而可生成 三维点云

$$\begin{array}{r}\{\begin{array}{l}Z=\text{depth}=\frac{f^{\prime }\cdot B}{d_p}\\ X=\frac{u-{c_x}_1^{\prime }}{f^{\prime }}\cdot Z\\ Y=\frac{v-{c_y}^{\prime }}{f^{\prime }}\cdot Z\end{array}\end{array}\text{或}\begin{array}{r}\{\begin{array}{l}\text{bd}=\frac{B}{d_p}\\ Z=\text{depth}=f^{\prime }\cdot \text{bd}\\ X=(u-{c_x}_1^{\prime })\cdot \text{bd}\\ Y=(u-{c_y}^{\prime })\cdot \text{bd}\end{array}\end{array}$$

其中，$\text{disp}(u,v)$ 代表 视差图 坐标值

（2）算法2：根据视差图，利用 $Q$ 矩阵 计算 三维点坐标（reprojectImageTo3D）

$$\left[\begin{array}{c}{X}^{\prime }\\ Y^{\prime }\\ Z^{\prime }\\ W\end{array}\right]=Q\cdot \left[\begin{array}{c}u\\ v\\ \text{disp}(u,v)\\ 1\end{array}\right]$$

最终，三维点坐标为

$$\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\left[\begin{array}{c}\frac{X^{\prime }}{W}\\ \frac{Y^{\prime }}{W}\\ \frac{Z^{\prime }}{W}\end{array}\right]$$

深度图 图像类型

• 单位meter --> 32FC1
• 单位millimeter --> 16UC1

总结

一、三维重建概述

     三维重建主要是研究如何从得到的匹配点中计算出相机的投影矩阵(如果是外部标定的话，就是求出相机的外部参数)以及如何计算出匹配点的三维坐标。

    目前研究的进展与之还相差较远。研究人员为了能够表达三维空间信息，目前较多地采用三维矢量图形来替代三维位图。主要的重构方法有如下几种：

    (1)、空间点的重建

    这是三维重构中的最基本的方法。空间任一点在两个摄像机中分别成像，得到该点在两个图像中的对应坐标，在知道两摄像机的参数矩阵的条件下，通过建立以该点的世界坐标为未知数的 4 个线性方程，可以用最小二乘法求解得该点的世界坐标。有一种简化计算的办法，选取两个内部参数完全相同的摄像机，将他们平行配置，使他们的光轴相互平行。这种方法计算相当简单，不需要进行外部参数标定。但一般情况下，两个摄像机的内部参数完全相同(包括两摄像机的拍摄状态也相同)是很难选到。摄像机安装时无法看到光轴和成像平面，故难以实施。

   (2)、空间直线、空间二次曲线的重建

   空间直线和空间曲线也是组成空间图形的主要基元。根据空间直线射影变换前后仍是直线的性质，以此来分析空间直线与图像中的直线之间的关系。空间直线在CCD上的成像可以这样认为，空间直线与摄像机光心构成的平面与成像平面的交线，两台摄像机有两个这样的平面，这两个平面的交线就是空间直线。空间二次曲线是空间二次曲面与平面的交线，因此二次曲线的平面曲线，在CCD上的成像可以认为是由空间二次曲线与光心组成的锥面与成像平面的交线。两台摄像机有两个这样的锥面，求这两个空间锥面的交线，就是空间曲线。

   (3)、全像素的三维重建

   对图像中的每个像素都进行三维重建，这当然是最理想的情况，目前要实施这种重构，需要相当严格的测量条件，且仅限于对某一具体对象，但效果并不理想。对此人们正在进行大量的研究，还有待于进一步的努力。

二、空间点的三维重建

   1、基本模型

   空间点三维重建的基本模型如图(4.1)所示，对于空间物体表面任意一点P，如果用C1摄像机观察，看到它在C1摄像机的图像点位于p1，但他们无法由p1得知P的三维位置，事实上，在O1P(O1为C1摄像机的光心)连线上任意一点P’的图像点都是p1，因此，由p1点的位置，我们只知道空间点位于O1P1与O2P2两条直线的交点，即它们的三维位置是唯一确定的。

假如我们能够得到物体表面上所有点的三维坐标，则三维物体的形状与位置就是唯一确定的，在某些简单场合，例如三维物体时一个多面体，我们只需要知道它的各个顶点的三维坐标与相邻关系，则该多面体的形状与位置是唯一确定的。因此，用立体视觉的方法获取三维点的坐标是十分基础的。

   2、最小二乘法求解三维坐标值

   下面我们来具体介绍求解三维点坐标的算法。在进行讨论之前，我们要明确一下几点基本假设：空间任意点P在两个摄像机C1与C2上的图像点平p1与p2已经从两个图像中分别检测出来。即已知p1与p2为空间同一点P的对应点。至于对应点是如何找到的，这属于图像配准问题。还有一个假设就是，摄像机C1与C2已标定，他们的投影矩阵分别为M1与M2，于是有：

其中，(u1,v1,1)与(u2,v2,1)分别为P1与P2点在各自图像中的图像齐次坐标；(X,y,Z,1)为P点在世界坐标系下的齐次坐标；mijk为Mk的第i行第j列元素。式(4.1)与式(4.2)消去Zc1与Zc2得到关于X,Y,Z的四个线性方程：

由于空间点P是O1p1与O2p2的交点，它必然同时满足式(4.3)，式(4.4)，我们可以联立这四个式子，从而求出P点的坐标(X,Y,Z)。四个方程，三个未知数，也就是说这个方程组只有三个独立的方程，这是因为我们已经假设P1与P2点是空间同一点P的对应点，因此已经假设了直线O1p1与O2p2一定相交，或者说，这四个方程必定有解，而且解是唯一的。而实际应用中，由于数据总是有噪声的，我们可以用最小二乘法求出P的三维点坐标(X,Y,Z)。
3、视差测距法

   以上我们介绍了空间点三维重建的一般方法，为了更清楚的了解上述过程的物理意义，下面我们讨论在一种简单的摄像机配置下的空间点重建方法，视差测距法。此方法要求选用两个内部参数完全相同的摄像机，将它们平行放置，使它们的光轴相互平行，另有一对坐标轴共线，两个成像平面共面，两摄像机的光心有一个固定的距离d，这样求解图像点的世界坐标时只涉及摄像机内参数。原理如图(4.2)所示：

在上述特殊摄像机配置下，假设C1坐标系为O1x1y1z1，C2坐标系为O2x2y2z2，焦距为f，摄像机间距为d，任何空间点P的坐标在C1下为(x1,y1,z1)，在C2坐标系下为(x2,y2,z2)，在左摄像机中的图像点左边为(u1,v1)，在右摄像机中的图像点坐标为(u2,v2)。

   根据式(4.5)，式(4.6)的摄影比例关系，对于左右摄像机有如下关系：

世界坐标系与摄像机坐标系的关系可表述如下：

联立上两式可得：

进而可得：

由式(4.5)和式(4.8)可计算的空间点的三维坐标：

式(4.9)即为双目立体视觉重构三维空间点的方法，可见，通过图像对视差和像点坐标就可以恢复出物体的三维坐标。但是由于对设备配置的要求比较苛刻，只是被应用在一些简单的视觉系统。本文采用的还是针对一般摄像机参数的三维重建方法，这样大大扩展了带云台的摄像机的视野范围，从而增强了移动机器人视觉系统的灵活性。

ZYNQ上基于FPGA加速的 双目立体视觉 三维重建 生成 实时深度图彩虹图 （zedboard板）

1.在ZYNQ上完成双目采集成像，PS+PL协同处理。
2.在ZYNQ上完成相机标定，镜头校准。
3.在ZYNQ上完成立体匹配算法，目前已实现局部立体匹配BM以及半全局立体匹配SGBM
4.在ZYNQ上完成了立体匹配后的深度图转换彩虹图，即彩色深度图
5.在ZYNQ上也完成了测距算法，但是目前没有识别算法，只能取图像一点计算距离。精度在10mm内。有效距离目前测试在5m内。
6.优化了所有的算法，目前接近720p的图像质量，并且帧率在30fps左右。

详细的实现过程会出一个专栏，每周更一次，从零开始的项目搭建ZYNQ上的实时三维重建。

部分实验过程记录：

标定过程：