OpenCV学习笔记与代码示例（四）：双目视觉原理及实现
目录
1.双目立体视觉基本原理
1.1三维重建过程
1.2双目匹配问题
1.3对极几何
1.4基础矩阵F
1.5本质矩阵E
2.OpenCV实现
2.1双目标定后的双目相机的三维重建
2.2单目标定的双目相机的三维重建
2.3标定精度评价
3.代码示例
1.双目立体视觉基本原理
我们知道单目视觉难以获得图像的深度信息，因为目标点在投影到图像时失去了这种信息，而我们可以通过双目视觉获取深度信息，从而还原目标点的三维信息。双目立体视觉测量是基于视差原理，由多幅图像获取物体三维几何信息的方法。由双摄像机同一时刻从不同角度或由单摄像机在不同时刻从不同角度获取周围景物的两幅数字图像后，基于视差原理即可恢复物体的三维几何信息。
1.1三维重建过程
考虑一般情形，设左摄像机的摄像机中心为
，有效焦距为
，相面上的投影点为
，
为对应坐标系，右摄像机同理。通过对左右摄像机建立摄像机模型，我们可以确定左右摄像机的位置关系，从而求得三维点在左相机下的完整坐标（右相机同理）。
由摄像机模型已知相面坐标系和相机坐标系的关系：
可知，只需要求解
，就能得到三维点在相机坐标系下的坐标。已知相机坐标系和世界坐标系的关系：
![\begin{bmatrix} x_c\\ y_c\\ z_c \end{bmatrix}=\left [ R \quad t \right ]\begin{bmatrix} x_w\\ y_w\\ z_w \\1\end{bmatrix}](http://img.e-com-net.com/image/info8/f7a648f53b414ae9970888d348a46eda.gif)
若左右相机拍摄到的为同一三维点，则可以根据上式，通过矩阵乘法求得左相机坐标系与右相机坐标系之间的关系：
![\begin{bmatrix} x_r\\ y_r\\ z_r \end{bmatrix}=\left [ R'\quad t' \right ]\begin{bmatrix} x_l\\ y_l\\ z_l\\1 \end{bmatrix}](http://img.e-com-net.com/image/info8/70a2ecd2586b48a389f5965acfc7cabf.gif)
又已知右相机坐标系与右相面坐标系的关系，联立上式可得右相面坐标系和左相机坐标系的关系，从而将三维点在右相面投影的坐标转换到左相面，再与三维点原本在左相面投影的坐标联立求解，就能得到仅含有未知数
的方程，从而还原三维点在左相机坐标系下的三个坐标值
。
1.2双目匹配问题
通过上述步骤我们知道，已知某三维点在左右相面的坐标值，两个相机的内外参数，即可还原三维点的位置信息。相机的内外参可以通过标定来获取，剩下需要解决的就是确定某点在左右相面坐标系下的坐标值，左相面上的某点要对应到右相面，即双目匹配问题。双目匹配问题的解决涉及计算机视觉中的特征检测，相关算法很多，这里不作详解。（挖个坑嘿嘿）
1.3对极几何
对极几何是学习双目相机的重要知识。
如图所示，
为左右摄像机的摄像机中心，
代表左右相平面，连线
(称为基线)与两相平面的交点
称为对极点，三维点X与两个摄像机中心组成的平面称为对极平面，外极平面与左右相平面的交线
称为对极线。随着三维点X的变化，外极平面绕基线旋转，而外极点不变。该模型存在如下关系：与点X在
的投影点m相匹配的点X在
的投影点m'必在极线
上，反之亦然。
1.4基础矩阵F
利用该原理，在一幅图像上选择特征点后，在另一幅图像的对应外极线上搜寻匹配点即可，这样可以化二维搜索问题为一维搜索问题。因此，我们研究的是点到极线的映射关系，这种对极几何的代数表示就是基础矩阵F。
定义：假设有两幅由中心不重合的摄像机拍摄的图像，则基础矩阵F是唯一的3×3秩2齐次矩阵，对所有的x和匹配点x'满足：
基础矩阵F具有如下性质：
由此可知，求出精确的基础矩阵F对于双目匹配十分重要。由于F具有7个自由度，至少需要7对匹配点才能确定F。我们可以通过各种计算机视觉手段（如SIFT特征等）获取匹配点，采用不同的算法估算基础矩阵F（如RANSAC算法）。
1.5本质矩阵E
本质矩阵是归一化图像坐标下的基础矩阵的特殊形式，基础矩阵也可以看作是本质矩阵的推广。相较于基础矩阵，本质矩阵自由度只有5个，但增加了一些性质。
归一化坐标
设摄像机矩阵P以及图像坐标与世界坐标映射关系如下:
若K已标定完成，则映射关系可以表示为：
则
是图像点在归一化坐标下的表示。
![P=K\left [ R|t \right ],x=PX](http://img.e-com-net.com/image/info8/3ee91e760ac0481487f941b231b50fbf.gif){kind=small}
![\hat{x}=\left [ R|t \right ]X, \hat{x}=K^{-1}x](http://img.e-com-net.com/image/info8/03092ae5369b41189af5a522b5b656ed.gif){kind=small}
则用归一化坐标表示对应点时，本质矩阵的定义可以表示为：
基础矩阵与本质矩阵之间关系如下：
本质矩阵可以用于恢复摄像机矩阵。值得注意的是，基础矩阵具有射影多义性，而本质矩阵与之相差一个尺度因子和一个4重多义性，也就是说除全局的尺度因子外，不能被确定的解仅有4个，我们利用SVD分解可以得到四个满足的解。
2.OpenCV实现
2.1双目标定后的双目相机的三维重建
OpenCV提供了双目相机的标定函数stereoCalibrate()，该函数利用单应性找到对应匹配点，从而计算左右相机间的关系，以及基础矩阵与本质矩阵。与张正友单目标定法原理类似，该函数需要建立三维点，左右相机中的图像点三者间的单应关系。此外，还需要提前对每个相机进行标定，得到内参和畸变系数。
CV_EXPORTS_AS(stereoCalibrateExtended) double stereoCalibrate(
InputArrayOfArrays objectPoints,//标定点坐标。对于每个视图，两个摄像机需要看到相同的物体点。
InputArrayOfArrays imagePoints1, //第一个相机的图像点坐标
InputArrayOfArrays imagePoints2,//第二个相机的图像点坐标
InputOutputArray cameraMatrix1, //第一个相机的内参矩阵
InputOutputArray distCoeffs1,//第一个相机的畸变系数
InputOutputArray cameraMatrix2, //第二个相机的畸变系数
InputOutputArray distCoeffs2,//第二个相机的畸变系数
Size imageSize, //图像尺寸
InputOutputArray R,//输出左右相机的相对旋转矩阵。该矩阵将第一个相机坐标系中的点变为第二个相机
坐标系中的点
InputOutputArray T,//输出左右相机的相对平移向量，描述同上
OutputArray E, //输出本质矩阵
OutputArray F,//输出基础矩阵
OutputArray perViewErrors, 输出每个模式视图估计的RMS重投影误差（该参数为函数重载参数，可忽略）
int flags = CALIB_FIX_INTRINSIC,//flag标志，可以为0或选择设置选项
TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) );其中，flag选项可以有如下设置：
CALIB_FIX_INTRINSIC：固定相机内参矩阵和畸变系数，只估计R, T, E和F矩阵。
CALIB_USE_INTRINSIC_GUESS：根据指定的flag优化部分或所有的相机内参数。初始值由用户提供。
CALIB_USE_EXTRINSIC_GUESS：R和T包含用于进一步优化的有效初始值。否则，R和T被初始化为视图的中值(每个维度独立)。
CALIB_FIX_PRINCIPAL_POINT：在优化过程中固定主点。
CALIB_FIX_FOCAL_LENGTH：固定左右相机的
和
。
CALIB_FIX_ASPECT_RATIO：优化左右相机的
的比例。
CALIB_SAME_FOCAL_LENGTH：令右相机的
CALIB_ZERO_TANGENT_DIST：设置每个相机的切向畸变系数为零并固定。
CALIB_FIX_K1,..., CALIB_FIX_K6：在优化过程中不改变相应的径向畸变系数。如果设置了CALIB_USE_INTRINSIC_GUESS，则使用提供的distCoeffs矩阵中的系数。否则，它被设置为0。
CALIB_RATIONAL_MODEL：启用系数k4, k5和k6。为了提供向后兼容性，应该明确指定这个额外的标志，以使标定函数使用合理模型并返回8个系数。如果没有设置该标志，该函数只计算并返回5个畸变系数。
CALIB_THIN_PRISM_MODEL：s1、s2、s3和s4的系数被启用。为了提供向后兼容性，应该明确指定这个额外的标志，使标定函数使用薄棱镜模型并返回12个系数。如果没有设置该标志，该函数只计算并返回5个畸变系数。
CALIB_FIX_S1_S2_S3_S4：在优化过程中固定薄棱镜的畸变系数。如果设置了 CALIB_USE_INTRINSIC_GUESS，则使用提供的distCoeffs矩阵中的系数。否则，它被设置为0。
CALIB_TILTED_MODEL：启用系数tauX和tauY。为了提供向后兼容性，应该明确指定这个额外的标志，以使标定函数使用倾斜传感器模型并返回14个系数。如果没有设置该标志，该函数只计算并返回5个畸变系数。
CALIB_FIX_TAUX_TAUY：在优化过程中固定倾斜传感器模型的系数。如果设置了 CALIB_USE_INTRINSIC_GUESS，则使用提供的distCoeffs矩阵中的系数。否则，它被设置为0。
得到两个相机的相对姿态后,就可以计算三维点所处位置。我们可以用三角剖分的方法，该方法的基本原理如下：两视图对应点与各自摄像机中心相连形成的两条射线（投影线）相交于所求三维点。该方法实现前需要将两个视图对应的投影矩阵用世界坐标系表示，调用undistortPoints()可以实现。最后调用triangulate()函数计算三角剖分点的位置。
2.2单目标定的双目相机的三维重建
如果未经过双目相机标定，若从多视角下观察同一场景时，利用不同视角下图像点之间的关系，也可以计算出三维信息。我们需要根据计算机视觉算法提取特征点，获得三维点对应两个图像的匹配关系。获得了足够的匹配点后，我们可以调用findFundamentalMat()来计算基础矩阵F。
CV_EXPORTS_W Mat findFundamentalMat(
InputArray points1, //第一张图像点数组
InputArray points2, //第二张图像点数组
int method, //计算所用算法
double ransacReprojThreshold, //仅用于RANSAC的参数。它是点到极线的最大距离，以像素为单位，超过
这个距离的点被认为是离群点，不用于计算最终的基本矩阵。它可以设
置为1-3，这取决于点定位的准确性、图像分辨率和图像噪声。
double confidence,//仅用于RANSAC和lmed方法的参数。它指定一个期望的置信水平(概率)，估计矩阵是正
确的。
int maxIters, //鲁棒算法迭代的最大次数
OutputArray mask = noArray()//可选输出掩码
);OpenCV提供了以下算法计算基础矩阵F：
FM_7POINT为7点算法。
FM_8POINT为8点算法。
FM_RANSAC表示RANSAC算法。
FM_LMEDS用于LMedS算法。
findFundamentalMat()函数使用上面列出的四种方法之一计算基础矩阵，并返回找到的基础。通常只有一个矩阵。但是在7点算法的情况下，函数可能返回3个解。计算出的基础矩阵可以进一步传递给computeCorrespondEpilines，以找到与指定点对应的极线。也可以传递给#stereoRectifyUncalibrated来计算整流变换。
得到基础矩阵后，我们可以计算本质矩阵，从而恢复摄像机矩阵的姿态，实现该功能的函数为recoverPose()。得到两个相机的位姿关系后，处理方法与2.1的相同，这里不再赘述。
2.3标定精度评价
得到基础矩阵后，我们可以通过检验图像点是否在极线上来判断检验结果。在得到极线方程后，将图像点代入来计算误差。OpenCV提供了计算精度的案例。
// CALIBRATION QUALITY CHECK
// because the output fundamental matrix implicitly
// includes all the output information,
// we can check the quality of calibration using the
// epipolar geometry constraint: m2^t*F*m1=0
double err = 0;
int npoints = 0;
vector lines[2];
for( i = 0; i < nimages; i++ )
{
int npt = (int)imagePoints[0][i].size();
Mat imgpt[2];
for( k = 0; k < 2; k++ )
{
imgpt[k] = Mat(imagePoints[k][i]);
undistortPoints(imgpt[k], imgpt[k], cameraMatrix[k], distCoeffs[k], Mat(), cameraMatrix[k]);
computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]);
}
for( j = 0; j < npt; j++ )
{
double errij = fabs(imagePoints[0][i][j].x*lines[1][j][0] +
imagePoints[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
fabs(imagePoints[1][i][j].x*lines[0][j][0] +
imagePoints[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
err += errij;
}
npoints += npt;
}
cout << "average epipolar err = " << err/npoints << endl; 3.代码示例
官方OpenCV中给出了立体标定的完整示例，这里列出供读者学习使用。
static void
StereoCalib(const vector& imagelist, Size boardSize, float squareSize, bool displayCorners = false, bool useCalibrated=true, bool showRectified=true)
{
if( imagelist.size() % 2 != 0 )
{
cout << "Error: the image list contains odd (non-even) number of elements\n";
return;
}
const int maxScale = 2;
// ARRAY AND VECTOR STORAGE:
vector > imagePoints[2];
vector > objectPoints;
Size imageSize;
int i, j, k, nimages = (int)imagelist.size()/2;
imagePoints[0].resize(nimages);
imagePoints[1].resize(nimages);
vector goodImageList;
for( i = j = 0; i < nimages; i++ )
{
for( k = 0; k < 2; k++ )
{
const string& filename = imagelist[i*2+k];
Mat img = imread(filename, 0);
if(img.empty())
break;
if( imageSize == Size() )
imageSize = img.size();
else if( img.size() != imageSize )
{
cout << "The image " << filename << " has the size different from the first image size. Skipping the pair\n";
break;
}
bool found = false;
vector& corners = imagePoints[k][j];
for( int scale = 1; scale <= maxScale; scale++ )
{
Mat timg;
if( scale == 1 )
timg = img;
else
resize(img, timg, Size(), scale, scale, INTER_LINEAR_EXACT);
found = findChessboardCorners(timg, boardSize, corners,
CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE);
if( found )
{
if( scale > 1 )
{
Mat cornersMat(corners);
cornersMat *= 1./scale;
}
break;
}
}
if( displayCorners )
{
cout << filename << endl;
Mat cimg, cimg1;
cvtColor(img, cimg, COLOR_GRAY2BGR);
drawChessboardCorners(cimg, boardSize, corners, found);
double sf = 640./MAX(img.rows, img.cols);
resize(cimg, cimg1, Size(), sf, sf, INTER_LINEAR_EXACT);
imshow("corners", cimg1);
char c = (char)waitKey(500);
if( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit
exit(-1);
}
else
putchar('.');
if( !found )
break;
cornerSubPix(img, corners, Size(11,11), Size(-1,-1),
TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,
30, 0.01));
}
if( k == 2 )
{
goodImageList.push_back(imagelist[i*2]);
goodImageList.push_back(imagelist[i*2+1]);
j++;
}
}
cout << j << " pairs have been successfully detected.\n";
nimages = j;
if( nimages < 2 )
{
cout << "Error: too little pairs to run the calibration\n";
return;
}
imagePoints[0].resize(nimages);
imagePoints[1].resize(nimages);
objectPoints.resize(nimages);
for( i = 0; i < nimages; i++ )
{
for( j = 0; j < boardSize.height; j++ )
for( k = 0; k < boardSize.width; k++ )
objectPoints[i].push_back(Point3f(k*squareSize, j*squareSize, 0));
}
cout << "Running stereo calibration ...\n";
Mat cameraMatrix[2], distCoeffs[2];
cameraMatrix[0] = initCameraMatrix2D(objectPoints,imagePoints[0],imageSize,0);
cameraMatrix[1] = initCameraMatrix2D(objectPoints,imagePoints[1],imageSize,0);
Mat R, T, E, F;
double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
cameraMatrix[0], distCoeffs[0],
cameraMatrix[1], distCoeffs[1],
imageSize, R, T, E, F,
CALIB_FIX_ASPECT_RATIO +
CALIB_ZERO_TANGENT_DIST +
CALIB_USE_INTRINSIC_GUESS +
CALIB_SAME_FOCAL_LENGTH +
CALIB_RATIONAL_MODEL +
CALIB_FIX_K3 + CALIB_FIX_K4 + CALIB_FIX_K5,
TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-5) );
cout << "done with RMS error=" << rms << endl;
// CALIBRATION QUALITY CHECK
// because the output fundamental matrix implicitly
// includes all the output information,
// we can check the quality of calibration using the
// epipolar geometry constraint: m2^t*F*m1=0
double err = 0;
int npoints = 0;
vector lines[2];
for( i = 0; i < nimages; i++ )
{
int npt = (int)imagePoints[0][i].size();
Mat imgpt[2];
for( k = 0; k < 2; k++ )
{
imgpt[k] = Mat(imagePoints[k][i]);
undistortPoints(imgpt[k], imgpt[k], cameraMatrix[k], distCoeffs[k], Mat(), cameraMatrix[k]);
computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]);
}
for( j = 0; j < npt; j++ )
{
double errij = fabs(imagePoints[0][i][j].x*lines[1][j][0] +
imagePoints[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
fabs(imagePoints[1][i][j].x*lines[0][j][0] +
imagePoints[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
err += errij;
}
npoints += npt;
}
cout << "average epipolar err = " << err/npoints << endl;
// save intrinsic parameters
FileStorage fs("intrinsics.yml", FileStorage::WRITE);
if( fs.isOpened() )
{
fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] <<
"M2" << cameraMatrix[1] << "D2" << distCoeffs[1];
fs.release();
}
else
cout << "Error: can not save the intrinsic parameters\n";
Mat R1, R2, P1, P2, Q;
Rect validRoi[2];
stereoRectify(cameraMatrix[0], distCoeffs[0],
cameraMatrix[1], distCoeffs[1],
imageSize, R, T, R1, R2, P1, P2, Q,
CALIB_ZERO_DISPARITY, 1, imageSize, &validRoi[0], &validRoi[1]);
fs.open("extrinsics.yml", FileStorage::WRITE);
if( fs.isOpened() )
{
fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q;
fs.release();
}
else
cout << "Error: can not save the extrinsic parameters\n";
// OpenCV can handle left-right
// or up-down camera arrangements
bool isVerticalStereo = fabs(P2.at(1, 3)) > fabs(P2.at(0, 3));
// COMPUTE AND DISPLAY RECTIFICATION
if( !showRectified )
return;
Mat rmap[2][2];
// IF BY CALIBRATED (BOUGUET'S METHOD)
if( useCalibrated )
{
// we already computed everything
}
// OR ELSE HARTLEY'S METHOD
else
// use intrinsic parameters of each camera, but
// compute the rectification transformation directly
// from the fundamental matrix
{
vector allimgpt[2];
for( k = 0; k < 2; k++ )
{
for( i = 0; i < nimages; i++ )
std::copy(imagePoints[k][i].begin(), imagePoints[k][i].end(), back_inserter(allimgpt[k]));
}
F = findFundamentalMat(Mat(allimgpt[0]), Mat(allimgpt[1]), FM_8POINT, 0, 0);
Mat H1, H2;
stereoRectifyUncalibrated(Mat(allimgpt[0]), Mat(allimgpt[1]), F, imageSize, H1, H2, 3);
R1 = cameraMatrix[0].inv()*H1*cameraMatrix[0];
R2 = cameraMatrix[1].inv()*H2*cameraMatrix[1];
P1 = cameraMatrix[0];
P2 = cameraMatrix[1];
}
//Precompute maps for cv::remap()
initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, imageSize, CV_16SC2, rmap[0][0], rmap[0][1]);
initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]);
Mat canvas;
double sf;
int w, h;
if( !isVerticalStereo )
{
sf = 600./MAX(imageSize.width, imageSize.height);
w = cvRound(imageSize.width*sf);
h = cvRound(imageSize.height*sf);
canvas.create(h, w*2, CV_8UC3);
}
else
{
sf = 300./MAX(imageSize.width, imageSize.height);
w = cvRound(imageSize.width*sf);
h = cvRound(imageSize.height*sf);
canvas.create(h*2, w, CV_8UC3);
}
for( i = 0; i < nimages; i++ )
{
for( k = 0; k < 2; k++ )
{
Mat img = imread(goodImageList[i*2+k], 0), rimg, cimg;
remap(img, rimg, rmap[k][0], rmap[k][1], INTER_LINEAR);
cvtColor(rimg, cimg, COLOR_GRAY2BGR);
Mat canvasPart = !isVerticalStereo ? canvas(Rect(w*k, 0, w, h)) : canvas(Rect(0, h*k, w, h));
resize(cimg, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);
if( useCalibrated )
{
Rect vroi(cvRound(validRoi[k].x*sf), cvRound(validRoi[k].y*sf),
cvRound(validRoi[k].width*sf), cvRound(validRoi[k].height*sf));
rectangle(canvasPart, vroi, Scalar(0,0,255), 3, 8);
}
}
if( !isVerticalStereo )
for( j = 0; j < canvas.rows; j += 16 )
line(canvas, Point(0, j), Point(canvas.cols, j), Scalar(0, 255, 0), 1, 8);
else
for( j = 0; j < canvas.cols; j += 16 )
line(canvas, Point(j, 0), Point(j, canvas.rows), Scalar(0, 255, 0), 1, 8);
imshow("rectified", canvas);
char c = (char)waitKey();
if( c == 27 || c == 'q' || c == 'Q' )
break;
}
}