【opencv】显微镜/投影仪 圆阵列标定板标定

由于在显微镜以及投影仪下，棋盘格角点提取会非常不准确，因此必须采用圆形阵列标定板进行标定，opencv里本身提供了圆形阵列标定版的接口，然而在显微镜以及投影仪下却提取不到，因为标定板很小（显微镜下才6mm*6mm）经过放大，圆变形很严重，因此这里通过另外的方法求取。

在显微镜视场中，前景与背景区别很大，直接固定阈值128得到二值图即可，在二值图中检测所有轮廓，对得到的轮廓做一个筛选，首先是轮廓周长（点的个数），当然也可以通过形态学，去除掉小的噪点。对剩下的轮廓求取最小外接圆，进行圆形状的一个比对，如果轮廓是圆心，则为目标

圆形状比对的一个依据参考博文：http://www.cnblogs.com/jsxyhelu/p/4503900.html

//根据轮廓点和圆心计算方差
float ComputeVariance(std::vector<cv::Point> theContour, Point2f theCenter)
{
    int a[65535],n;
    float aver,s;
    float sum=0,e=0;
    n = theContour.size();
    for(int i=0;i<n;i++)
    {
        a[i] = sqrt((theContour[i].x - theCenter.x) * (theContour[i].x - theCenter.x) + (theContour[i].y - theCenter.y) * (theContour[i].y - theCenter.y));
        sum+=a[i];
    }
    aver=sum/n;
    for(int i=0;i<n;i++)
        e+=(a[i]-aver)*(a[i]-aver);
    e/=n-1;
    s=sqrt(e);
    return e;
}

经过以上步骤，可以得到视场中的16个圆心，但是其顺序是乱的，需要按照一定的顺序进行排序，由于连通域检测时，16个圆轮廓在vector中是连续存储的并且是以Y坐标从大到小排的（如果不是这样，也可以通过算法后面的结果），先拿到2个边上的圆心坐标，计算两点线段的斜率，寻找下一个点的原则是，下一个点与上一个点线段的斜率（角度）与上一个点与上上一个点的线段斜率（角度）夹角要最小，距离要最短。但两者不能同时兼顾到，因此根据标定板圆的排列分为了2种情况，下一个点不在同一行和在同一行的两种情况，在同一行时，则主要兼顾距离最短，不在同一行时，则主要兼顾夹角最小。

bool findMicroCircle(Mat image, Size board_size, vector<Point2f> &corners)
{
    Mat imageGray;
    cvtColor(image, imageGray , CV_RGB2GRAY);

    Mat imageThreshold;
    threshold(imageGray, imageThreshold, 128, 255, CV_THRESH_BINARY);

    //提取轮廓  
    vector<vector<Point>>contours; 
    vector<Point2f> corners_temp;
    int count = 0;
    findContours(imageThreshold, contours, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE); 

    for (int i = 0; i < contours.size(); i++)
    {
        if (contours[i].size() > 200)
        {
            Point2f center;
            float radius;

            //drawContours(image, contours, i, Scalar(0,0,255), 2); 
            minEnclosingCircle(contours[i], center, radius);
            float e = ComputeVariance(contours[i], center);//方差

            if (e < 50)
            {
                count++;
                corners_temp.push_back(center);
            }
        }
    }

    //drawContours(image, contours, -1, Scalar(0,0,255), 2); 

    if (count == board_size.height * board_size.width)
    {
        int index;//下标记录
        float k1,k2;//斜率
        float angle1,angle2,angle,anglemin,dis,dismin;//前后斜率夹角
        corners.push_back(corners_temp[0]);//0，1号点压入，供计算初始斜率
        corners.push_back(corners_temp[1]);

        //k1 = (corners[0].y - corners[1].y) / (corners[0].x - corners[1].x);//初始斜率
        
       
        for (int j = 2; j < count; j++)
        {
            bool samelineFlag = true;
            anglemin = 90;//最小角度差
            dismin = 9999999;//两点最小距离

            k1 = (corners[j-1].y - corners[j-2].y) / (corners[j-1].x - corners[j-2].x);//初始斜率
            angle1 = atan(k1) * 180 / CV_PI;

            for (int i = 2; i < corners_temp.size(); i++)
            {
                if (corners_temp[i].x - corners[j-1].x == 0)//斜率为0
                {
                    k2 = 0;
                }
                else
                {
                    k2 = (corners_temp[i].y - corners[j-1].y) / (corners_temp[i].x - corners[j-1].x);//后续斜率
                }

                angle2 = atan(k2) * 180 / CV_PI;

                angle = abs(angle1 - angle2);

                dis = sqrt((corners_temp[i].y - corners[j-1].y) * (corners_temp[i].y - corners[j-1].y) + (corners_temp[i].x - corners[j-1].x) * (corners_temp[i].x - corners[j-1].x));

                int sameline = (atan(1.0 / (board_size.height-1)) * 180 / CV_PI) + 5;

                if (angle < sameline)//如果计算的角度差小于最小角度值与同一条直线判断阈值的和，则更新下标
                {
                    if (angle < sameline/4 && dis < dismin && j % board_size.width != 0 && (j-1) % board_size.width != 0)//同一条直线上，角度差不超过5度认为是同一条直线上，此时应该比较距离
                    {
                        dismin = dis;
                        index = i;
                    }
                    if (angle < anglemin && j % board_size.width == 0)//换行，只找角度差最小的
                    {
                        anglemin = angle;
                        index = i;
                    }
                    if (dis < dismin && (j-1) % board_size.width == 0)//换行后第一次，只找角度差最小的
                    {
                        dismin = dis;
                        index = i;
                    }
                }
            }
            corners.push_back(corners_temp[index]);
            circle(image, corners_temp[index], 15, Scalar(0,255,0), 3, 8, 0);
            circle(image, corners_temp[0], 15, Scalar(0,0,255), -1, 8, 0);
            circle(image, corners_temp[1], 15, Scalar(0,0,255), -1, 8, 0);

            std::vector<Point2f>::iterator it = corners_temp.begin() + index;
            corners_temp.erase(it);
        }
        return true;
    }
    else
    {
        return false;
    }   
}

其实从哪个点提取无所谓，但是要保证一个原则就是一行一行按顺序排列点即可。

标定与校正结果如下：

第1幅图像的平均误差：0.80577像素
第2幅图像的平均误差：0.701184像素
第3幅图像的平均误差：0.70272像素
第4幅图像的平均误差：0.547063像素
第5幅图像的平均误差：1.09536像素
第6幅图像的平均误差：0.76506像素
第7幅图像的平均误差：0.667793像素
第8幅图像的平均误差：0.822369像素
第9幅图像的平均误差：0.877443像素
第10幅图像的平均误差：0.856679像素
第11幅图像的平均误差：0.681006像素
总体平均误差：0.774767像素

相机内参数矩阵：
[39251.0769115962, 0, 932.5315829671392;
  0, 41185.50490132452, 694.5148137172613;
  0, 0, 1]
畸变系数：
[-24.56078184561823, -13.9232076080276, 0.004546530516996267, 0.005352287915247098, -0.007488930052855393]
第1幅图像的旋转向量：
[-2.400648046930939;
  2.39789006446686;
  -0.5030773963661545]
第1幅图像的旋转矩阵：
[0.0007116874091214198, -0.9999975463682025, 0.002097798514260035;
  -0.9165202388330997, -0.001491368632978607, -0.3999855342745764;
  0.3999876814482535, -0.001638010126739448, -0.9165190514181771]
第1幅图像的平移向量：
[1.740430650160699;
  1.980388421080897;
  213.9421545468698]
第2幅图像的旋转向量：
[2.026069655685576;
  -1.95425564704494;
  0.4404653877035846]
第2幅图像的旋转矩阵：
[0.03229009767692703, -0.9993252200120062, 0.01750583445509979;
  -0.91021254996335, -0.03663704040549276, -0.4125176858748505;
  0.4128806891599921, -0.002613793848253693, -0.9107813703631013]
第2幅图像的平移向量：
[1.434303442340692;
  1.933775134314402;
  215.7635414434346]
第3幅图像的旋转向量：
[-2.506973189718734;
  2.266959042094225;
  -0.5296331774750954]
第3幅图像的旋转矩阵：
[0.09190965608196688, -0.9949790263151063, 0.03961505158317322;
  -0.9095342846029065, -0.1000781939290523, -0.4034002233908377;
  0.4053393642995623, 0.00104512819403188, -0.9141656892797304]
第3幅图像的平移向量：
[2.115457782230888;
  2.63554510808202;
  214.3826147104438]
第4幅图像的旋转向量：
[2.091249202153444;
  -1.904469058116255;
  0.4192461330508303]
第4幅图像的旋转矩阵：
[0.08818120869555068, -0.9957969531149588, 0.02474878986851795;
  -0.9141364173315228, -0.09076976925361158, -0.3951144891014927;
  0.3957002463245116, 0.01221790311839707, -0.9182984470759549]
第4幅图像的平移向量：
[1.229216205018381;
  2.958858389701409;
  215.2865289017586]
第5幅图像的旋转向量：
[2.052248883184534;
  -1.782921024377806;
  0.516852253248877]
第5幅图像的旋转矩阵：
[0.1311653007292452, -0.9908488716301933, 0.03184615320222786;
  -0.8542562667193232, -0.129267016908198, -0.5035238515804592;
  0.5030326974018403, 0.03884008147299184, -0.8633942051089989]
第5幅图像的平移向量：
[1.211696914955099;
  1.858255483709739;
  215.9341707929622]
第6幅图像的旋转向量：
[2.096455574273039;
  -1.930547560805298;
  0.301475989544044]
第6幅图像的旋转矩阵：
[0.08784392817508158, -0.9956067885038483, -0.03241245081484015;
  -0.9383224556449042, -0.07177843894483371, -0.3382289534252786;
  0.334416526976894, 0.06012469033347367, -0.9405055066808574]
第6幅图像的平移向量：
[2.702045961192787;
  1.77720171122464;
  220.6739208255806]
第7幅图像的旋转向量：
[-2.249647067123821;
  1.790500361198483;
  0.4486109104045156]
第7幅图像的旋转矩阵：
[0.2060376646047496, -0.9740240671687818, -0.09394465040759759;
  -0.90325267668592, -0.2262354126433894, 0.3646260277678985;
  -0.3764081333126875, 0.00972906172558119, -0.926402861898644]
第7幅图像的平移向量：
[1.36362867973697;
  2.669282558564297;
  225.9698060155932]
第8幅图像的旋转向量：
[-2.15307960749997;
  2.011120796187208;
  0.09171998559648421]
第8幅图像的旋转矩阵：
[0.07581061381791343, -0.9933676827270825, 0.08644881575809901;
  -0.9813750892821667, -0.05898515769153534, 0.1828214574619851;
  -0.1765097305220621, -0.09869852119222058, -0.9793379993370511]
第8幅图像的平移向量：
[2.760921681914984;
  1.602440113356596;
  222.0324924843436]
第9幅图像的旋转向量：
[-2.441623710430115;
  1.442733098603825;
  0.01861088167794312]
第9幅图像的旋转矩阵：
[0.4943342731749869, -0.8575998295617726, 0.1419723871045915;
  -0.8536522578609274, -0.4481113765116447, 0.2654694274107283;
  -0.1640470938892193, -0.2524256852550024, -0.9536088424558804]
第9幅图像的平移向量：
[1.56167374380016;
  2.69886532502509;
  222.8328423002089]
第10幅图像的旋转向量：
[-0.3020672487178615;
  -0.3177006009417958;
  -1.748049962240801]
第10幅图像的旋转矩阵：
[-0.1947892568904155, 0.9804375716976252, 0.02827213122040473;
  -0.9077898948900995, -0.1911219165559888, 0.373349594545632;
  0.3714493937745785, 0.04705933505299129, 0.9272598162591047]
第10幅图像的平移向量：
[-0.3428851599804441;
  2.695720889270264;
  218.1581049286656]
第11幅图像的旋转向量：
[-2.501831188933102;
  1.332345638343028;
  0.3106030443469637]
第11幅图像的旋转矩阵：
[0.5492422450311522, -0.8339504535735243, -0.05347520226845928;
  -0.771624664883495, -0.5306805918268061, 0.3506757562218761;
  -0.3208244579463564, -0.1513431545953232, -0.934968938917525]
第11幅图像的平移向量：
[0.2722978014067166;
  2.75998728572356;
  218.6845829362973]

以下代码从标定图文件夹下读取标定图，进行定标，校正待标定图文件夹下的所有图片，并把校正结果保存到校正图文件夹下

//opencv2.4.9 vs2012
#include <opencv2\opencv.hpp>
#include <fstream>
#include <direct.h>

using namespace std;
using namespace cv;

//根据轮廓点和圆心计算方差
float ComputeVariance(std::vector<cv::Point> theContour, Point2f theCenter)
{
    int a[65535],n;
    float aver,s;
    float sum=0,e=0;
    n = theContour.size();
    for(int i=0;i<n;i++)
    {
        a[i] = sqrt((theContour[i].x - theCenter.x) * (theContour[i].x - theCenter.x) + (theContour[i].y - theCenter.y) * (theContour[i].y - theCenter.y));
        sum+=a[i];
    }
    aver=sum/n;
    for(int i=0;i<n;i++)
        e+=(a[i]-aver)*(a[i]-aver);
    e/=n-1;
    s=sqrt(e);
    return e;
}

bool findMicroCircle(Mat image, Size board_size, vector<Point2f> &corners)
{
    Mat imageGray;
    cvtColor(image, imageGray , CV_RGB2GRAY);

    Mat imageThreshold;
    threshold(imageGray, imageThreshold, 128, 255, CV_THRESH_BINARY);

    //提取轮廓  
    vector<vector<Point>>contours; 
    vector<Point2f> corners_temp;
    int count = 0;
    findContours(imageThreshold, contours, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE); 

    for (int i = 0; i < contours.size(); i++)
    {
        if (contours[i].size() > 200)
        {
            Point2f center;
            float radius;

            //drawContours(image, contours, i, Scalar(0,0,255), 2); 
            minEnclosingCircle(contours[i], center, radius);
            float e = ComputeVariance(contours[i], center);//方差

            if (e < 50)
            {
                count++;
                corners_temp.push_back(center);
            }
        }
    }

    //drawContours(image, contours, -1, Scalar(0,0,255), 2); 

    if (count == board_size.height * board_size.width)
    {
        int index;//下标记录
        float k1,k2;//斜率
        float angle1,angle2,angle,anglemin,dis,dismin;//前后斜率夹角
        corners.push_back(corners_temp[0]);//0，1号点压入，供计算初始斜率
        corners.push_back(corners_temp[1]);

        //k1 = (corners[0].y - corners[1].y) / (corners[0].x - corners[1].x);//初始斜率
        
       
        for (int j = 2; j < count; j++)
        {
            bool samelineFlag = true;
            anglemin = 90;//最小角度差
            dismin = 9999999;//两点最小距离

            k1 = (corners[j-1].y - corners[j-2].y) / (corners[j-1].x - corners[j-2].x);//初始斜率
            angle1 = atan(k1) * 180 / CV_PI;

            for (int i = 2; i < corners_temp.size(); i++)
            {
                if (corners_temp[i].x - corners[j-1].x == 0)//斜率为0
                {
                    k2 = 0;
                }
                else
                {
                    k2 = (corners_temp[i].y - corners[j-1].y) / (corners_temp[i].x - corners[j-1].x);//后续斜率
                }

                angle2 = atan(k2) * 180 / CV_PI;

                angle = abs(angle1 - angle2);

                dis = sqrt((corners_temp[i].y - corners[j-1].y) * (corners_temp[i].y - corners[j-1].y) + (corners_temp[i].x - corners[j-1].x) * (corners_temp[i].x - corners[j-1].x));

                int sameline = (atan(1.0 / (board_size.height-1)) * 180 / CV_PI) + 5;

                if (angle < sameline)//如果计算的角度差小于最小角度值与同一条直线判断阈值的和，则更新下标
                {
                    if (angle < sameline/4 && dis < dismin && j % board_size.width != 0 && (j-1) % board_size.width != 0)//同一条直线上，角度差不超过5度认为是同一条直线上，此时应该比较距离
                    {
                        dismin = dis;
                        index = i;
                    }
                    if (angle < anglemin && j % board_size.width == 0)//换行，只找角度差最小的
                    {
                        anglemin = angle;
                        index = i;
                    }
                    if (dis < dismin && (j-1) % board_size.width == 0)//换行后第一次，只找角度差最小的
                    {
                        dismin = dis;
                        index = i;
                    }
                }
            }
            corners.push_back(corners_temp[index]);
            //circle(image, corners_temp[index], 15, Scalar(0,255,0), 3, 8, 0);
            //circle(image, corners_temp[0], 15, Scalar(0,0,255), -1, 8, 0);
            //circle(image, corners_temp[1], 15, Scalar(0,0,255), -1, 8, 0);

            std::vector<Point2f>::iterator it = corners_temp.begin() + index;
            corners_temp.erase(it);
        }
        return true;
    }
    else
    {
        return false;
    }   
}

int main()
{
    _mkdir("校正图");

    double time0 = static_cast<double>(getTickCount());
    ofstream fout("caliberation_result.txt");  /**    保存定标结果的文件     **/

    /************************************************************************  
           读取每一幅图像，从中提取出角点，然后对角点进行亚像素精确化  
    *************************************************************************/   
    cout<<"开始提取角点………………"<<endl; 
    int image_count=  11;                    /****    图像数量     ****/   
    Size image_size;                         /****     图像的尺寸      ****/   
    Size board_size = Size(4,4);            /****    定标板上每行、列的角点数       ****/  
    vector<Point2f> corners;                  /****    缓存每幅图像上检测到的角点       ****/
    vector<vector<Point2f>>  corners_Seq;    /****  保存检测到的所有角点       ****/   
    vector<Mat>  image_Seq;


    int count = 0;
    for( int i = 0;  i < image_count ; i++)
    {
        cout<<"标定图【"<< i+1 <<"】..."<<endl;
        string imageFileName;
        std::stringstream StrStm;
        StrStm<<i+1;
        StrStm>>imageFileName;
        imageFileName += ".bmp";
        Mat image = imread("标定图/" + imageFileName);//引号中可加统一文件名前缀 
        image_size = image.size();
        //image_size = Size(image.cols , image.rows);
        /* 提取角点 */   
        Mat imageGray;
        cvtColor(image, imageGray , CV_RGB2GRAY);

        //棋盘格方式
        //bool patternfound = findChessboardCorners(image, board_size, corners,CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE+ 
        //    CALIB_CB_FAST_CHECK );
        //圆阵列方式
        //bool patternfound = findCirclesGrid(image, board_size, corners); 
        //圆阵列非对称方式
        //bool patternfound = findCirclesGrid(image, board_size, corners, CALIB_CB_ASYMMETRIC_GRID ); 
        //自定义显微镜下圆阵列方式
        bool patternfound = findMicroCircle(image, board_size, corners); 

        if (!patternfound)   
        {   
            cout<<"can not find chessboard corners!\n";  
            continue;
            exit(1);   
        } 
        else
        {   
            /* 亚像素精确化 如果是自定义显微镜下圆阵列方式则需要注释*/
            //cornerSubPix(imageGray, corners, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
            /* 绘制检测到的角点并保存 */
            Mat imageTemp = image.clone();
            for (int j = 0; j < corners.size(); j++)
            {
                if (j + 1 < corners.size())
                {
                    line(imageTemp, corners[j], corners[j + 1], Scalar(0,255,0), 2, 8, 0);
                }              
                circle( imageTemp, corners[j], 10, Scalar(0,0,255), -1, 8, 0);
            }
            string imageFileName;
            std::stringstream StrStm;
            StrStm<<i+1;
            StrStm>>imageFileName;
            imageFileName += "_corner.jpg";
            imwrite(imageFileName, imageTemp);
            cout<<"标定图【"<<i+1<<"】...end"<<endl;

            count = count + corners.size();
            corners_Seq.push_back(corners);
        }   

        corners.clear();
        image_Seq.push_back(image);
    }   
    cout<<"角点提取完成！\n"; 
    /************************************************************************  
           摄像机定标  
    *************************************************************************/   
    cout<<"开始定标………………"<<endl;   
    Size2d square_size = Size2d(1.2,1.2);                                      /**** 实际测量得到的定标板上每个棋盘格的大小   ****/  
    vector<vector<Point3f>>  object_Points;                                      /****  保存定标板上角点的三维坐标   ****/


    Mat image_points = Mat(1, count , CV_32FC2, Scalar::all(0));          /*****   保存提取的所有角点   *****/   
    vector<int>  point_counts;                                          /*****    每幅图像中角点的数量    ****/   
    Mat intrinsic_matrix = Mat(3,3, CV_32FC1, Scalar::all(0));                /*****    摄像机内参数矩阵    ****/   
    Mat distortion_coeffs = Mat(1,4, CV_32FC1, Scalar::all(0));            /* 摄像机的4个畸变系数：k1,k2,p1,p2 */ 
    vector<cv::Mat> rotation_vectors;                                      /* 每幅图像的旋转向量 */  
    vector<cv::Mat> translation_vectors;                                  /* 每幅图像的平移向量 */  

    /* 初始化定标板上角点的三维坐标 */     
    for (int t=0;t<image_count;t++) 
    {   
        vector<Point3f> tempPointSet;
        for (int i=0;i<board_size.height;i++) 
        {   
            for (int j=0;j<board_size.width;j++) 
            {   
                /* 假设定标板放在世界坐标系中z=0的平面上 */   
                Point3f tempPoint;
                tempPoint.x = i*square_size.width;
                tempPoint.y = j*square_size.height;
                tempPoint.z = 0;
                tempPointSet.push_back(tempPoint);
            }   
        }
        object_Points.push_back(tempPointSet);
    }   

    /* 初始化每幅图像中的角点数量，这里我们假设每幅图像中都可以看到完整的定标板 */   
    for (int i=0; i< image_count; i++)   
    {
        point_counts.push_back(board_size.width*board_size.height);   
    }

    /* 开始定标 */   
    calibrateCamera(object_Points, corners_Seq, image_size,  intrinsic_matrix  ,distortion_coeffs, rotation_vectors, translation_vectors, 0);   
    cout<<"定标完成！\n";   

    /************************************************************************  
           对定标结果进行评价  
    *************************************************************************/   
    cout<<"开始评价定标结果………………"<<endl;   
    double total_err = 0.0;                   /* 所有图像的平均误差的总和 */   
    double err = 0.0;                        /* 每幅图像的平均误差 */   
    vector<Point2f>  image_points2;             /****   保存重新计算得到的投影点    ****/   

    cout<<"每幅图像的定标误差："<<endl;   
    cout<<"每幅图像的定标误差："<<endl<<endl;   
    for (int i=0;  i<image_count;  i++) 
    {
        vector<Point3f> tempPointSet = object_Points[i];
        /****    通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点     ****/
        projectPoints(tempPointSet, rotation_vectors[i], translation_vectors[i], intrinsic_matrix, distortion_coeffs, image_points2);
        /* 计算新的投影点和旧的投影点之间的误差*/  
        vector<Point2f> tempImagePoint = corners_Seq[i];
        Mat tempImagePointMat = Mat(1,tempImagePoint.size(),CV_32FC2);
        Mat image_points2Mat = Mat(1,image_points2.size(), CV_32FC2);
        for (size_t i = 0 ; i != tempImagePoint.size(); i++)
        {
            image_points2Mat.at<Vec2f>(0,i) = Vec2f(image_points2[i].x, image_points2[i].y);
            tempImagePointMat.at<Vec2f>(0,i) = Vec2f(tempImagePoint[i].x, tempImagePoint[i].y);
        }
        err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
        total_err += err/=  point_counts[i];   
        cout<<"第"<<i+1<<"幅图像的平均误差："<<err<<"像素"<<endl;   
        fout<<"第"<<i+1<<"幅图像的平均误差："<<err<<"像素"<<endl;   
    }   
    cout<<"总体平均误差："<<total_err/image_count<<"像素"<<endl;   
    fout<<"总体平均误差："<<total_err/image_count<<"像素"<<endl<<endl;   
    cout<<"评价完成！"<<endl;   

    /************************************************************************  
           保存定标结果  
    *************************************************************************/   
    cout<<"开始保存定标结果………………"<<endl;       
    Mat rotation_matrix = Mat(3,3,CV_32FC1, Scalar::all(0)); /* 保存每幅图像的旋转矩阵 */   

    fout<<"相机内参数矩阵："<<endl;   
    fout<<intrinsic_matrix<<endl;   
    fout<<"畸变系数：\n";   
    fout<<distortion_coeffs<<endl;   
    for (int i=0; i<image_count; i++) 
    { 
        fout<<"第"<<i+1<<"幅图像的旋转向量："<<endl;   
        fout<<rotation_vectors[i]<<endl;   

        /* 将旋转向量转换为相对应的旋转矩阵 */   
        Rodrigues(rotation_vectors[i],rotation_matrix);   
        fout<<"第"<<i+1<<"幅图像的旋转矩阵："<<endl;   
        fout<<rotation_matrix<<endl;   
        fout<<"第"<<i+1<<"幅图像的平移向量："<<endl;   
        fout<<translation_vectors[i]<<endl;   
    }   
    cout<<"完成保存"<<endl; 
    fout<<endl;


    /************************************************************************  
           显示定标结果  
    *************************************************************************/
    Mat mapx = Mat(image_size,CV_32FC1);
    Mat mapy = Mat(image_size,CV_32FC1);
    Mat R = Mat::eye(3,3,CV_32F);
    cout<<"保存矫正图像"<<endl;
    for (int i = 0 ; i != image_count ; i++)
    {
        cout<<"标定图【"<<i+1<<"】..."<<endl;
        Mat newCameraMatrix = Mat(3,3,CV_32FC1,Scalar::all(0));
        initUndistortRectifyMap(intrinsic_matrix,distortion_coeffs,R, getOptimalNewCameraMatrix(intrinsic_matrix, distortion_coeffs, image_size, 1, image_size, 0),image_size,CV_32FC1,mapx,mapy);
        Mat t = image_Seq[i].clone();
        cv::remap(image_Seq[i],t,mapx, mapy, INTER_LINEAR);
        string imageFileName;
        std::stringstream StrStm;
        StrStm<<i+1;
        StrStm>>imageFileName;
        imageFileName += "_d.jpg";
        imwrite(imageFileName,t);
    }
    cout<<"保存结束"<<endl;

    time0 = ((double)getTickCount()-time0)/getTickFrequency();
    cout<<"标定用时:"<<time0<<"秒"<<endl;

    /************************************************************************  
           批量校正图片  
    *************************************************************************/  
    if (1)
    {
        double time1 = static_cast<double>(getTickCount());

        int image_count = 47; /****    待校正图像数量     ****/   

        for( int i = 0;  i < image_count ; i++)
        {
            cout<<"待校正图【"<< i+1 <<"】..."<<endl;
            string imageFileName;
            std::stringstream StrStm;
            StrStm<<i+1;
            StrStm>>imageFileName;
            imageFileName += ".bmp";
            Mat image = imread("待校正图/" + imageFileName);//引号中可加统一文件名前缀 
            image_size = image.size();

            cout<<"待校正图【"<< i+1 <<"】校正完毕"<<endl;

            initUndistortRectifyMap(intrinsic_matrix,distortion_coeffs,R, getOptimalNewCameraMatrix(intrinsic_matrix, distortion_coeffs, image_size, 1, image_size, 0),image_size,CV_32FC1,mapx,mapy);
            Mat t = image.clone();
            cv::remap(image, t, mapx, mapy, INTER_LINEAR);

            imwrite("校正图/" + imageFileName, t);         
        }
        cout<<"保存结束"<<endl;

        time1 = ((double)getTickCount()-time1)/getTickFrequency();
        cout<<"校正用时:"<<time1<<"秒"<<endl;
    }

    getchar();
    return 0;
}