计算机视觉大型攻略 —— 立体视觉（3）双目相机标定与校正

最新推荐文章于 2024-05-17 23:58:53 发布

linusyue

最新推荐文章于 2024-05-17 23:58:53 发布

阅读量9.5k

点赞数 10

分类专栏：立体视觉文章标签：立体匹配双目标定双目校正 Bouguet

本文链接：https://blog.csdn.net/plateros/article/details/102665505

版权

立体视觉专栏收录该内容

5 篇文章 20 订阅

订阅专栏

本文是立体视觉系列的第三部分，讲解双目标定与校正。

双目标定与校正

目的

设计立体匹配算法时，为了提高算法的效率和精度，一般需要将2D搜索空间降维到1D空间。根据对极几何的知识，已知一个相机图像上任意一点p1，在另一个相机的投影p2一定在某条极线上。双目标定与校正将两幅图像的极线调整成平行且行对齐。这样已知左图上一点p1，右图上的匹配点p2一定在相同的行上。关于立体成像的更多内容，请阅读本系列其他几篇文章。

计算左右摄的旋转平移矩阵

单目标定可以给出相机内参，而双目标定还需要计算两台摄像机之间的旋转平移矩阵(R, T)。

假设空间中有一点P，在世界坐标系 $(X_{w}, Y_{w}, Z_{w})$ 下的坐标为 $\vec{P}$ $(x_{w}, y_{w}, z_{w})$ 。在单目标定的过程中，我们获取了每幅棋盘格图像从世界坐标系到相机坐标系的旋转平移矩阵。因此如果已知某一点的世界坐标，通过旋转平移矩阵，可以计算出他在相机坐标系的坐标。

令 $\vec{P}_{l}$ 为P点在左摄相机坐标系 $(X_{l}, Y_{l}, Z_{l})$ 下的坐标， $\vec{P}_{r}$ 为P点在右摄相机坐标系 $(X_{r}, Y_{r}, Z_{r})$ 下的坐标，( $R_{l}, \vec{T_{l}}$ )为世界坐标系到左摄相机坐标系的旋转平移矩阵，（ $R_{r}, \vec{T_{r}}$ )为世界坐标系到右摄相机坐标系的旋转平移矩阵。

得到下面两个等式，

$\vec{P_{l}} = R_{l}\cdot \vec{P}+\vec{T_{l}}$

$\vec{P_{r}} = R_{r}\cdot \vec{P}+\vec{T_{r}}$

令( $R, \vec{T}$ )为左摄坐标系到右摄坐标系的旋转平移矩阵，可以得到下面的等式，

$\vec{P_{l}} = R^{T}\cdot (\vec{P_{r}}- \vec{T})$

上面三个公式进一步化简，可以消掉 $\vec{P}_{l}$ ， $\vec{P}_{r}$ ，

$R=R_{r}\cdot R_{l}^{T}$

$\vec{T}=\vec{T_{r}} - R\cdot \vec{T_{l}}$

每幅棋盘格提供一组( $R_{l}, \vec{T_{l}}$ ), ( $R_{r}, \vec{T_{r}}$ )，只要有足够多的棋盘格，这个方程就可以解出来了。考虑到噪声因素，同样可以采用单目校准时使用的Levenberg-Marquardt迭代优化。

双目校正

有了左右相机的内外参和旋转平移矩阵，我们就可以校正图像，使得极线行对齐。首先引入对极几何的概念。

对极几何

点P在左右相机上的投影分别为pl, pr。对极几何引入了极点(epipole)和极线(epipole lines)两个概念。

极点。一个像平面上(projective plane)的极点定义为另一个摄像机的投影中心在该像平面上的成像点。以上图为例，左摄像机平面的极点el，是右摄相机光心Or在左摄成像平面上的成像点。
极线。连接点P的成像点pl和el的线，称为极线。
“对极约束”。给定图像上的一点Pl，他在另一幅图像上的匹配点一定在对应的极线上。

通常双目校正算法把 $p_{l}e_{l}$ 和 $p_{r}e_{r}$ 调整成平行于 $O_{l}O_{r}$ ,且行对齐。常见算法有Hartley算法，Bouguet算法等。

这类算法通常分为两步，第一步将两个摄像机的投影平面调整成共面，但是极线没有对齐。如下图，

第二步再把极点推到无穷远，也就是说将极线对齐。如下图，极线行对齐，且平行于OlOr。极点el, er在无穷远。

Bouguet算法

首先进行第一步，将两个像平面调整共面。已知左右摄相机的旋转平移矩阵(R, T)，我们只需将一个平面按照此矩阵旋转，就可以将两个平面调整为共面。但是这样容易产生较大的畸变。Bouguet算法将这个(R, T)拆成了两部分，每个摄像机旋转一半，左摄像机旋转r1, 右摄像机旋转r2，这样就会最小化重投影畸变。

第二步行对齐。构建一个旋转矩阵 $R_{rect}$ ，令左图图像围绕PrinciplePoint(cx, cy)旋转，将极点变换到无穷远，使极线水平。 $R_{rect}$ 由三个相互正交的单位向量e1, e2, e3组成。

令新x轴与基线平行。在左摄像机相机坐标系下，基线QlQr的向量是 $\vec{T}$ 。e1的方向与 $\vec{T}$ 的方向相同。

$e_{1}=\frac{T}{\left \| T \right \|}$

令新y轴与新的x轴正交，且与旧z轴（即光轴）正交。

$e_{2}=\frac{1}{\sqrt{T_{x}^{2}+T_{y}^{2}}}(-T_{y},T_{x},0)^{T}$

新z轴与新x轴，新y轴正交，

$e_{3}=e_{1}\times e_{2}$

得到，

$R_{rect}=\begin{bmatrix} e_{1}^{T}\\ e_{2}^{T}\\ e_{3}^{T}\end{bmatrix}$

结合第一步的r1, r2，可以获得左右成像平面最终的旋转矩阵 $R_{l}$ ， $R_{r}$ 。

$R_{l}=R_{rect}r_{l}$

$R_{r}=R_{rect}r_{r}$

与图像坐标系坐标的关系，在左摄像头坐标系下，

$P_{l}=\begin{bmatrix} f_{rect,l}} & \alpha &c_{x,l}} \\ 0 & f_{y,l}&c_{y,l} \\ 0& 0& 1\end{bmatrix}\begin{bmatrix} 1&0 & 0 &0 \\ 0&1 & 0 & 0\\ 0& 0 & 1 & 0 \end{bmatrix}$

$P_{r}=\begin{bmatrix} f_{rect,r}} & \alpha &c_{x,r}} \\ 0 & f_{y,r}&c_{y,r} \\ 0& 0& 1\end{bmatrix}\begin{bmatrix} 1&0 & 0 &T_{x} \\ 0&1 & 0 & 0\\ 0& 0 & 1 & 0 \end{bmatrix}$

$P\cdot \begin{bmatrix} X\\ Y\\ Z\\ 1 \end{bmatrix} = w\begin{bmatrix} \frac{x}{w}\\ \frac{y}{w}\\ 1 \end{bmatrix}$

给定图像坐标系中两点及视差，通过投影矩阵Q，可以得到其世界坐标系的坐标，

$Q\cdot \begin{bmatrix} x\\ y\\ d\\ 1 \end{bmatrix} = W\begin{bmatrix} X\\ Y\\ Z\\ 1 \end{bmatrix}$

投影矩阵Q,

$Q=\begin{bmatrix} 1 & 0 &0 &-c_{x} \\ 0 & 1 &0 &-c_{y} \\ 0& 0 &0 & f\\ 0& 0 &-\frac{1}{T_{x}} & \frac{c_{x}-c_{x}^{'}}{T_{x}} \end{bmatrix}$

代码实例

opencv4提供的例子samples/cpp/stereo_calib.cpp. 简单修改了一下，与数据集即一起传到Github上了。

https://github.com/linusyue/calibStereo.git

程序运行参数

./calibStereo -w=9 -h=6 -s=0.02423 stereo_calib.xml

w是宽度方向角点个数，h为高度方向角点个数。注意这里角点指的是内角点，数据集棋盘格是10*7，内角点就是9*6，s是单个棋盘格的边长，stereo_calib.xml指明了棋盘图像的路径。

程序流程，

从文件中读入图像，通过findChessboardCorners找到角点的图像坐标系坐标，这个函数采用Harris算法获得角点。

103                 found = findChessboardCorners(timg, boardSize, corners,
104                     CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE);

调用cornerSubPi获取亚像素的角点坐标。

132             cornerSubPix(img, corners, Size(11,11), Size(-1,-1),
133                          TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,
134                                       30, 0.01));

获得世界坐标系坐标。与单目校准程序相同，每幅棋盘格的世界坐标系都是棋盘左上角为原点，且Z=0。

151     imagePoints[0].resize(nimages);
152     imagePoints[1].resize(nimages);
153     objectPoints.resize(nimages);
154 
155     for( i = 0; i < nimages; i++ )
156     {
157         for( j = 0; j < boardSize.height; j++ )
158             for( k = 0; k < boardSize.width; k++ )
159                 objectPoints[i].push_back(Point3f(k*squareSize, j*squareSize, 0));
160     }

根据左右摄图像的图像坐标系坐标imagePoints和世界坐标系坐标objectPoints，计算初始的内参矩阵。initCameraMatrix2D函数调用cvInitIntrinsicParams2D，该函数采用vanish points(消失点)法估计内参矩阵。单目校准采用了同样的方法获得初始的内参矩阵。详情可见上一篇文章。

162     cout << "Running stereo calibration ...\n";                                                   
163     
164     Mat cameraMatrix[2], distCoeffs[2];
165     cameraMatrix[0] = initCameraMatrix2D(objectPoints,imagePoints[0],imageSize,0);                
166     cameraMatrix[1] = initCameraMatrix2D(objectPoints,imagePoints[1],imageSize,0);

调用stereoCalibrate计算左右摄像机之间的旋转平移矩阵。具体过程和文中所讲大同小异。最后使用LM算法，求解最优的畸变系数和内参。同时该函数输出本征矩阵(Essential Matrix)E和基础矩阵F(Fundametal Matrix)，这两个矩阵LearningOpencv3 第19章有详细解释，他们是通过内参和R,T算出来的，E,F可以用来计算极线， ~~由于不是标定校准的必须过程，正文偷懒略过，~~E，F计算公式如下，

$E=\begin{bmatrix} 0 &-T_{2} &T_{1} \\ T_{2}&0 &-T_{0} \\ -T_{1}& T_{0} &0 \end{bmatrix}$

$F=cameraMatrix2^{-T}EcameraMatrix1^{-1}$

167     Mat R, T, E, F;
168 
169     double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
170                     cameraMatrix[0], distCoeffs[0],
171                     cameraMatrix[1], distCoeffs[1],
172                     imageSize, R, T, E, F,
173                     CALIB_FIX_ASPECT_RATIO +
174                     CALIB_ZERO_TANGENT_DIST +
175                     CALIB_USE_INTRINSIC_GUESS +
176                     CALIB_SAME_FOCAL_LENGTH +
177                     CALIB_RATIONAL_MODEL +
178                     CALIB_FIX_K3 + CALIB_FIX_K4 + CALIB_FIX_K5,
179                     TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-5) );
180     cout << "done with RMS error=" << rms << endl;

调用stereoRectify校正图像。输入两个相机格子的内参，畸变系数，两个相机之间的旋转平移矩阵，获取极线对齐后的R, T, R1, R2, P1, P2, Q。该算法与正文一致。

226     stereoRectify(cameraMatrix[0], distCoeffs[0],
227                   cameraMatrix[1], distCoeffs[1],
228                   imageSize, R, T, R1, R2, P1, P2, Q,
229                   CALIB_ZERO_DISPARITY, 1, imageSize, &validRoi[0], &validRoi[1]);
230 
231     fs.open("extrinsics.yml", FileStorage::WRITE);
232     if( fs.isOpened() )
233     {
234         fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q;
235         fs.release();
236     }
237     else
238         cout << "Error: can not save the extrinsic parameters\n";

最后通过initUndistortRectifyMap和remap对左右摄图片做矫正。

276     //Precompute maps for cv::remap()
277     initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, imageSize, CV_16SC2, rmap[0][0], rmap[0][1]);
278     initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]);
279