双目相机标定OpenCV源码讲解

背景介绍

暑假接近两个月的时间做了一个实习项目，项目内容是双目视觉识别物体面积和距离，现在做下笔记，日后要用的话，重新学也比较方便。

所述内容

这里所记载的应该只是代码的使用说明及注意事项，不然只用OpenCV源码效果根本不好，但是照着做得到可用的程序应该不难，效果我做的也还行。
数学原理啥的网上很多，自己找就行了，并且只是用的话即使不会数学原理也不要紧，我感觉就是学了，理解的差不多了，但最后好像没咋用到。
搞这个还是先做出点能看的东西吧，不然做着做着就会没动力了。最后谢谢公司里的学长的指导，还有个整天吃凉皮的学长。

参考资料

OpenCV官网函数注释，贼有用
https://docs.opencv.org/2.4/modules/calib3d/doc/calib3d.html

OpenCV官方自带源码
在OpenCV安装好之后的opencv\sources\samples\cpp文件夹下面的stereo_calib.cpp和stereo_match.cpp。我的源码可能改了一点。

OpenCV官方自带图片
在OpenCV安装好之后的opencv\sources\samples\data文件夹下面的标定图片，
就是left01…，right01…，因为是双目，所以左右都要。

摄像机标定部分代码

代码思路

1.对左右的图用findChessboardCorners()函数识别棋盘角点，若左右对应的图都能识别到角点，就认为是可用的一对图片。将序号存储起来，用initCameraMatrix2D函数求出两个相机内参矩阵。
2.用stereoCalibrate()函数求出两个相机之间的转换关系，以及相关的旋转，平移，本征等矩阵。
3.求矫正误差，以及输出矫正之后的图(有画极线)

代码中的其他函数

1. 42-55行的print_help()没啥好说的
2. 343-356行的readStringList()用于读入图片名称，也没啥说的

static bool readStringList( const string& filename, vector<string>& l )//第一个输入参数是.xml的文件名称，第二个输入参数是要得到的图片名称
{
    l.resize(0);//首先初始化
    FileStorage fs(filename, FileStorage::READ);//filestorage类是用来操作.xml文件的，READ是读，APPEND是追加写，WRITE是覆盖写
    if( !fs.isOpened() )
        return false;
    FileNode n = fs.getFirstTopLevelNode();//得到fs的初始序号
    if( n.type() != FileNode::SEQ )//判断是不是初始序号
        return false;
    FileNodeIterator it = n.begin(), it_end = n.end();//序号迭代器
    for( ; it != it_end; ++it )
        l.push_back((string)*it);
    return true;
}

3. 358-393，main函数，没什么东西，就是给CommandLineParser给变量赋值，但我自己写的时候，就直接赋值了，反正效果都一样。重要的就是StereoCalib()函数，也就是上面那一大串代码。

int main(int argc, char** argv)
{
/*	argc = 6;
	argv[0] = "张正友标定法";
	argv[1] = "-w";
	argv[2] = "9";
	argv[3] = "-h";
	argv[4] = "6";
	argv[5] = "stereo_calib.xml";*/
    Size boardSize; //定义棋盘的长和宽的格数
    string imagelistfn;//得到.xml文件的文件名
    bool showRectified;//是否能进行图像校正
    cv::CommandLineParser parser(argc, argv, "{w|8|}{h|6|}{s|19|}{nr||}{help||}{@input|./data/stereo_calib.xml|}");//对应变量赋值
    if (parser.has("help"))//如果得到了help参数，输出help；
        return print_help();
    showRectified = !parser.has("nr");
    imagelistfn = parser.get<string>("@input");//得到.xml文件的文件名
    boardSize.width = parser.get<int>("w");
    boardSize.height = parser.get<int>("h");
    float squareSize = parser.get<float>("s");//方格的边长大小
    if (!parser.check())//判断输入的命令行参数是否正确
    {
        parser.printErrors();
        return 1;
    }
    vector<string> imagelist;//储存图片的名称序列
    bool ok = readStringList(imagelistfn, imagelist);//得到.xml中的图片名字，并写入imagelist
    if(!ok || imagelist.empty())
    {
        cout << "can not open " << imagelistfn << " or the string list is empty" << endl;
        return print_help();
    }

    StereoCalib(imagelist, boardSize, squareSize, true, true, showRectified);
    return 0;
}

其实如果非要源码的话，棋盘格的大小，长宽方向个数，读入的文件名称 要相应改一下，用你自己的数据。

找角点&求内参

对代码的注释就写在代码后面了

    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<vector<Point2f> > imagePoints[2];//存储图像的棋盘点序列
    vector<vector<Point3f> > objectPoints;//存储物体的实际点序列
    Size imageSize;//图像的大小

    int i, j, k, nimages = (int)imagelist.size()/2;//i，j，k用于迭代，nimage代表图像的对数；j存储好的图像的数量

    imagePoints[0].resize(nimages);//存储左边图像点，设置存储的图片数
    imagePoints[1].resize(nimages);//存储右边图像点，设置存储的图片数
    vector<string> goodImageList;//储存可以用的图像名称

    for( i = j = 0; i < nimages; i++ )//循环所有组图像寻找棋盘点
    {
        for( k = 0; k < 2; k++ )//循环左右两边的图像
        {
			stringstream filename;//获取图像名称
			filename<<"./data/"<< imagelist[i*2+k];
            Mat img = imread(filename.str(), 0);//获取图像
            if(img.empty())
                break;
            if( imageSize == Size() )
                imageSize = img.size();
            else if( img.size() != imageSize )//不能存在两幅像素大小不一的图像
            {
                cout << "The image " << filename.str() << " has the size different from the first image size. Skipping the pair\n";
                break;
            }
            bool found = false;
            vector<Point2f>& 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);//如果找不到，就扩大一倍，最大就两倍，可以自己设置maxscale
                found = findChessboardCorners(timg, boardSize, corners,
                    CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE);//寻找角点
                if( found )//如果找到了，就进行下一步
                {
                    if( scale > 1 )
                    {
                        Mat cornersMat(corners);//cornersMat用引用实现对corners的缩放
                        cornersMat *= 1./scale;//复原角点位置
                    }
                    break;
                }
            }
            if( displayCorners )//如果要画角点位置的话就画出来
            {
                cout << filename.str() << endl;
                Mat cimg, cimg1;
//				cout << img.channels()<
                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);

如果是用自己的相机拍的图，或者觉得效果不够好的话，要改的函数就是cornerSubPix()亚像素角点的函数

其中的criteria下面说明了，两个参数maxCount和epsilon即源码中的30和0.01指的是迭代次数和迭代精度，winSize是搜索窗口边长的一半，源码里这项是11X11。我用自己相机的时候设置的是winSize=5x5，maxCount=100,epsilon=0.01 。如果到时候看角点在图上标地不准时，可以修改迭代参数。

顺带一提的是，我们求得的内参(其实现在还没求完)就是官网上的右边的第一个矩阵。要注意的是这和许多数学原理教程里的形式不同，官网里有参数解释。
这里重要在于可以先检验一下你的矫正效果怎么样，**可以看到cx,cy是主点在图像上的坐标，理想下应该是图像尺寸的一半，**我自己相机的像素是640x480，所以理想是cx=320，cy=240。我下面的M1，M2是我的内参输出结果，对比一下还能忍受就行了。fx,fy是用各自像素单位表示的各自方向上焦距，这个除非是你知道你相机的CCD尺寸和焦距，否则也不好判别矫正效果，反正如果你左右两个相机一样的话，输出的M1,M2的fx和fy应该是要差不多的。

求外参

先上代码和注释吧

                    double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],       //进行标定,rms表示方均根误差，其中的参数可以修改
                                    cameraMatrix[0], distCoeffs[0],
                                    cameraMatrix[1], distCoeffs[1],
                                    imageSize, R, T, E, F,
                                    stereoCalibrate_flag,
                                    TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, stereoparameters.stereoCalibrateMaxtimes, stereoparameters.stereoCalibrateEpsilon) );
                                          err_1=rms;
                    cout << "done with RMS error=" << rms << endl;

                  double err = 0;//记录误差
                        int npoints = 0;
                        vector<Vec3f> lines[2];//记录极线
                        for( i = 0; i < nimages; i++ )
                        {
                            int npt = (int)imagePoints[0][i].size();//把第i对图像棋盘点数记录下来
                            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;
                        }
                        err_2=err/npoints;
                        cout << "average epipolar err = " <<  err/npoints << endl;


                        // save intrinsic parameters
                        FileStorage fs("intrinsics.yml", FileStorage::WRITE);
                        if( fs.isOpened() )
                        {
                            //cout << "row:" << distCoeffs[0].rows << "   " << distCoeffs[0].cols << endl;
//                            uchar *p2 = distCoeffs[0].ptr(0);
//                                        for(int j=0;j<10;j++)
//                                        {
//                                            cout<<(int)p2[j]<
//                                        }
                            fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] <<
                                "M2" << cameraMatrix[1] << "D2" << distCoeffs[1];
                            fs.release();
                        }//把摄像机内参数矩阵和畸变矩阵记录下来，放入intrinsics文件中
                        else
                            cout << "Error: can not save the intrinsic parameters\n";                      

开始重点之一了
stereoCalibrate()函数计算相机的畸变矫正参数和其他由此得到的矩阵，这里的参数要注意。

先说迭代终止条件，和之前那个意思一样，不过我给的参数是500和0.00001。
然后来重点了，如果你用的是自己的相机的话(非工业相机)，一定不要按源码里的flag给参数!!! 一定不要按源码里的flag给参数!!! 一定不要按源码里的flag给参数!!!
否则你得到的就是一坨**。当然，如果你的相机能满足参数里的那些同主点，同焦距等等的话，就当我没说吧（反正我的双目就是学长给的两个绑在一起的单目）
我给的参数是只有K3,K4,K5。这个函数得到的方均根误差个人感觉1及以下就行了 。这里得到的畸变矫正参数就是之前我输出文件图里的D1,D2，这个貌似看不太出来矫正效果(反正对于我的双目来说)。
之后的192-214行就是画个极线，在后面就是filestorage输出内参文件，在你编译的那个文件夹打开intrinsics。yml，就是我上面图那个样子。

求矫正映射矩阵

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]);//计算对应矩阵，R1,R2,P1,P2用于将两个图像平面转移至同一平面，便于极线的绘制

    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<double>(1, 3)) > fabs(P2.at<double>(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<Point2f> 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]));//把棋盘坐标放到allimgpt容器中
        }
        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]);//计算出重映射参数,R1,P1等意义与之前不同
    initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]);

又来一个重点了，**这里的参数alpha很重要，解释可以看源码。源码里给的是1，但是我给的是0，**区别在于0的话，只会映射出有效区域，而1的话会映射所有像素区域，像我这种畸变很大的镜头就一定要给0，区别会在下面展示。270-285行是用单应性矩阵对参数进行重新计算，源码中没有用到，我试了一下感觉效果差不多，也还是没用。之后的initUndistortRectifyMap就是得到rmap这个映射矩阵，这在后面用于矫正图像。

效果如下，其实alpha=1的效果没这么差，只是我这组标定图拍的不是很好。然后这里是检验矫正效果最关键的地方，两侧的同一点应该要在同一条直线上，就是所谓的极线对齐。还有就是拍摄标定图时的注意事项，光线均匀一点就行，最重要的是要在景深范围内拍，别拍糊的图，还有最好在工作环境下拍，就是说，你会在什么环境下使用这个双目，那就最好在什么环境下标定，否则会影响后续使用效果。


从输出的extrinsics。yml文件中可以看到P1，P2这两个矩阵。这是判断矫正参数的最后一个重要方法。P1，P2就是之前M1,M2的矫正后的结果，P1对应M1。虽然它们是3x4的，但除了P2的（4,1）是有值的，矩阵的第四列其他都是0。当矫正效果好时P1和M1和cx和cy应当值相近，也就是P1和M1的(3,1)和(3,2)的值应该不会差很多，(1,1),(2,2)也应当相近。另外P2的(4,1)除以(1,1)得到的是两个摄像机之间光轴的距离，这是可以自己用尺子量的。除得结果的单位是你棋盘格的长度单位。

以下是我所有标定用到的参数，仅供参考，棋盘格参数一定要看你用的什么标定板再给。

之后还有一小段代码就是画极线的再输出的，因为极线啥的上面说了，这一部分也没啥好钻研的，用就行了，就不放代码了。

后记

双目标定就此结束了，之后还有立体匹配和三维重建的部分。数学原理啥的网上实在太多，在看着要不要写笔记吧，这就是一篇笔记，如果有人觉得有用的话，那也是件好事。