张正友相机标定，opencv中calibrate函数内部具体实现过程附具体代码（二）——calibrate第一步-----求内外参数

求解内外参数的步骤

求解内外参数需要进行如下几步：
（1）求解单应性矩阵H
（2）根据H初始化V
（3）根据V求解出b向量
（4）根据b向量求解出B矩阵
（5）根据B矩阵求出封闭解得到内参
（6）根据内参、H求出外参
注意：
1.如果读者不知道H、V、b、B的含义，请仔细阅读推倒张正友的论文，求解单应性矩阵H的过程参考David Kriegman 的文章Homography Estimation。
2.后文代码中的B就是b向量，为了方便，省略了B矩阵的构造，直接用b向量求封闭解得到内参。

main函数中添加函数

 int main(){
 //新添加的内容，之前的省略了
    uv=image_points_seq;
    xy1=object_points;

    InitHomogeneous_Matrix(uv,uv1,xy1,M_alluv1,M_allxy1,image_count,board_size);//齐次化并且矩阵化
    SolveH(uv1,xy1,H,image_count,board_size);//求解单应性矩阵

    int onePic_twoVec = 2 * image_count;
    Mat V=Mat(onePic_twoVec,6,CV_32FC1,Scalar::all(0));
    Mat B=Mat(6,1,CV_32FC1,Scalar::all(0));

    SolveVij_B(H,V,B,image_count,board_size);//构建Vij方程求解向量b
    SolveCameraMatrix(B,cameraMatrix,scale);//求解内参
    cout<<cameraMatrix<<endl;
    SolveOutCamera(tvecsMat,rvecsMat,cameraMatrix,H,scale,R_r,Jett,tvecsMat1,outcan,image_count);//求解外参
    }

InitHomogeneous_Matrix函数

为后面求解单应性矩阵做准备，将检测的角点，和对应的三维点齐次化，其中三维点开始我们初始化为（x，y，0），为了方便计算，我们将应该齐次化为（x，y，0，1）改成齐次化为（x，y，1），因为后面旋转矩阵的第三个列向量可以利用旋转向量的前两个列向量求得。

void InitHomogeneous_Matrix(vector<vector<Point2f> >& uv, vector<vector<Point3f> >& uv1,
                            vector<vector<Point3f> >& xy1, vector<vector<Mat> >& M_alluv1,
                            vector<vector<Mat> >& M_allxy1, int& image_count, Size& board_size)
{
    int onePicPoints=board_size.width*board_size.height;//一张图片54个点

    for(int a=0;a<image_count;a++)//每个三维点齐次化
    {
        for(int b=0;b<onePicPoints;b++)
        {
            xy1[a][b].z=1;
        }
    }

    for(int a=0;a<image_count;a++)//每个角点齐次化
    {
        vector<Point3f> uv1part;
        for(int b=0;b<onePicPoints;b++)
        {
            Point3f pointtemp;
            pointtemp.x = uv[a][b].x;
            pointtemp.y = uv[a][b].y;
            pointtemp.z = 1;
            uv1part.push_back(pointtemp);
        }
        uv1.push_back(uv1part);
    }

    for(int a=0;a<image_count;a++)//角点矩阵化
    {
        vector<Mat> Muv1;
        for(int b=0;b<onePicPoints;b++)
        {
            Mat Ma=Mat(3,1,CV_32FC1,Scalar::all(0));
            Ma.at<float>(0,0)=uv1[a][b].x;
            Ma.at<float>(1,0)=uv1[a][b].y;
            Ma.at<float>(2,0)=uv1[a][b].z;
            Muv1.push_back(Ma);
        }
        M_alluv1.push_back(Muv1);
    }

    for(int a=0;a<image_count;a++)//三维点矩阵化
    {
        vector<Mat> Mxy1;
        for(int b=0;b<onePicPoints;b++)
        {
            Mat Mb=Mat(3,1,CV_32FC1,Scalar::all(0));
            Mb.at<float>(0,0)=xy1[a][b].x;
            Mb.at<float>(1,0)=xy1[a][b].y;
            Mb.at<float>(2,0)=xy1[a][b].z;
            Mxy1.push_back(Mb);
        }
        M_allxy1.push_back(Mxy1);
    }
}

SolveH函数

此函数就是求解单应性矩阵，利用已知的二维角点和三维点，求出对应的单应性矩阵H

void SolveH(vector<vector<Point3f> >& uv1, vector<vector<Point3f> >& xy1,
            vector<Mat>& H, int& image_count, Size& board_size)
{
    int onePicPoints = board_size.width*board_size.height;
    int onePoint_twoVec = 2*onePicPoints;//一个点产生两个向量，ax，ay
    vector<Mat> A_all;

    for(int a=0;a<image_count;a++)
    {
        Mat A = Mat(onePoint_twoVec,9,CV_32FC1,Scalar::all(1));
        for(int b=0;b<onePicPoints;b++)
        {
            Mat A1 = Mat(1,9,CV_32FC1,Scalar::all(1));
            Mat A2 = Mat(1,9,CV_32FC1,Scalar::all(1));

            A1.at<float>(0,0)=-xy1[a][b].x;
            A1.at<float>(0,1)=-xy1[a][b].y;
            A1.at<float>(0,2)=-1;
            A1.at<float>(0,3)=0;
            A1.at<float>(0,4)=0;
            A1.at<float>(0,5)=0;
            A1.at<float>(0,6)=xy1[a][b].x*uv1[a][b].x;
            A1.at<float>(0,7)=xy1[a][b].y*uv1[a][b].x;
            A1.at<float>(0,8)=uv1[a][b].x;

            A2.at<float>(0,0)=0;
            A2.at<float>(0,1)=0;
            A2.at<float>(0,2)=0;
            A2.at<float>(0,3)=-xy1[a][b].x;
            A2.at<float>(0,4)=-xy1[a][b].y;
            A2.at<float>(0,5)=-1;
            A2.at<float>(0,6)=xy1[a][b].x*uv1[a][b].y;
            A2.at<float>(0,7)=xy1[a][b].y*uv1[a][b].y;
            A2.at<float>(0,8)=uv1[a][b].y;

            A1.copyTo(A.row(b));
            A2.copyTo(A.row(onePoint_twoVec-b-1));
        }
        A_all.push_back(A);
    }

    for(int a=0;a<image_count;a++)
    {
        Mat Hcol = Mat(9,1,CV_32FC1,Scalar::all(0));
        Mat H33 = Mat(3,3,CV_32FC1,Scalar::all(0));
        SVD::solveZ(A_all[a],Hcol);
        H33.at<float>(0,0)=Hcol.at<float>(0,0);
        H33.at<float>(0,1)=Hcol.at<float>(1,0);
        H33.at<float>(0,2)=Hcol.at<float>(2,0);
        H33.at<float>(1,0)=Hcol.at<float>(3,0);
        H33.at<float>(1,1)=Hcol.at<float>(4,0);
        H33.at<float>(1,2)=Hcol.at<float>(5,0);
        H33.at<float>(2,0)=Hcol.at<float>(6,0);
        H33.at<float>(2,1)=Hcol.at<float>(7,0);
        H33.at<float>(2,2)=Hcol.at<float>(8,0);
        H.push_back(H33);
    }
}

SolveVij_B函数

初始化V矩阵，求解b

void SolveVij_B(vector<Mat>& H, Mat& V, Mat& B, int& image_count, Size& board_size)
{
    int onePic_twoVec = 2 * image_count;
    for(int a=0;a<image_count;a++)
    {
        Mat V12=Mat(1,6,CV_32FC1,Scalar::all(0));
        Mat V11=Mat(1,6,CV_32FC1,Scalar::all(0));
        Mat V22=Mat(1,6,CV_32FC1,Scalar::all(0));
        Mat V11_22=Mat(1,6,CV_32FC1,Scalar::all(0));

        V12.at<float>(0,0)=H[a].at<float>(0,0)*H[a].at<float>(0,1);
        V12.at<float>(0,1)=H[a].at<float>(0,0)*H[a].at<float>(1,1)+H[a].at<float>(1,0)*H[a].at<float>(0,1);
        V12.at<float>(0,2)=H[a].at<float>(1,0)*H[a].at<float>(1,1);
        V12.at<float>(0,3)=H[a].at<float>(2,0)*H[a].at<float>(0,1)+H[a].at<float>(0,0)*H[a].at<float>(2,1);
        V12.at<float>(0,4)=H[a].at<float>(2,0)*H[a].at<float>(1,1)+H[a].at<float>(1,0)*H[a].at<float>(2,1);
        V12.at<float>(0,5)=H[a].at<float>(2,0)*H[a].at<float>(2,1);

        V11.at<float>(0,0)=H[a].at<float>(0,0)*H[a].at<float>(0,0);
        V11.at<float>(0,1)=H[a].at<float>(0,0)*H[a].at<float>(1,0)+H[a].at<float>(1,0)*H[a].at<float>(0,0);
        V11.at<float>(0,2)=H[a].at<float>(1,0)*H[a].at<float>(1,0);
        V11.at<float>(0,3)=H[a].at<float>(2,0)*H[a].at<float>(0,0)+H[a].at<float>(0,0)*H[a].at<float>(2,0);
        V11.at<float>(0,4)=H[a].at<float>(2,0)*H[a].at<float>(1,0)+H[a].at<float>(1,0)*H[a].at<float>(2,0);
        V11.at<float>(0,5)=H[a].at<float>(2,0)*H[a].at<float>(2,0);

        V22.at<float>(0,0)=H[a].at<float>(0,1)*H[a].at<float>(0,1);
        V22.at<float>(0,1)=H[a].at<float>(0,1)*H[a].at<float>(1,1)+H[a].at<float>(1,1)*H[a].at<float>(0,1);
        V22.at<float>(0,2)=H[a].at<float>(1,1)*H[a].at<float>(1,1);
        V22.at<float>(0,3)=H[a].at<float>(2,1)*H[a].at<float>(0,1)+H[a].at<float>(0,1)*H[a].at<float>(2,1);
        V22.at<float>(0,4)=H[a].at<float>(2,1)*H[a].at<float>(1,1)+H[a].at<float>(1,1)*H[a].at<float>(2,1);
        V22.at<float>(0,5)=H[a].at<float>(2,1)*H[a].at<float>(2,1);

        V11_22=V11-V22;

        V12.copyTo(V.row(a));
        V11_22.copyTo(V.row(onePic_twoVec-a-1));
    }
    SVD::solveZ(V,B);
}

SolveCameraMatrix函数

根据封闭解求内参，封闭解是需要读者手动在草稿纸上算的，在此提供一个思路，B矩阵（因为B是3*3的对称矩阵，所以就取出了6个元素构成向量b），根据B11（B的一行一列的元素）求出a，B12求出c，B22求出b，B13和B23求出u0和v0，B33求出scale。这段代码写的比较冗余，读者可自行更改。

void SolveCameraMatrix(Mat& B,Mat& cameraMatrix,float& scale)
{
    float v0,aa,bb,cc,u0;

    v0=(B.at<float>(1,0)*B.at<float>(3,0)-B.at<float>(0,0)*B.at<float>(4,0))/(B.at<float>(0,0)*B.at<float>(2,0)-B.at<float>(1,0)*B.at<float>(1,0));
    scale=B.at<float>(5,0)-(B.at<float>(3,0)*B.at<float>(3,0)+v0*(B.at<float>(1,0)*B.at<float>(3,0)-B.at<float>(0,0)*B.at<float>(4,0)))/B.at<float>(0,0);
    aa=sqrt(scale/B.at<float>(0,0));
    bb=sqrt(scale*B.at<float>(0,0)/(B.at<float>(0,0)*B.at<float>(2,0)-B.at<float>(1,0)*B.at<float>(1,0)));
    cc=-(B.at<float>(1,0)*aa*aa*bb/scale);
    u0=cc*v0/bb-B.at<float>(3,0)*aa*aa/scale;

    cameraMatrix.at<float>(0,0)=aa;
    cameraMatrix.at<float>(0,1)=cc;
    cameraMatrix.at<float>(0,2)=u0;
    cameraMatrix.at<float>(1,0)=0;
    cameraMatrix.at<float>(1,1)=bb;
    cameraMatrix.at<float>(1,2)=v0;
    cameraMatrix.at<float>(2,0)=0;
    cameraMatrix.at<float>(2,1)=0;
    cameraMatrix.at<float>(2,2)=1;
}

SolveOutCamera函数

根据内参和H求解外参

void SolveOutCamera(vector<Mat>& tvecsMat, vector<Mat>& rvecsMat, Mat& cameraMatrix, vector<Mat>& H,
                    float& scale, vector<Mat>& R_r, vector<Mat>& Jett, vector<Mat>& tvecsMat1,
                    vector<Mat>& outcan,int& image_count)
{
    for(int a=0;a<image_count;a++)
    {
        Mat R_r_part;
        Mat Jet_part;
        Mat translate;
        Mat rotation0=Mat(3,3,CV_32FC1,Scalar::all(0));
        Mat rotation1;
        Mat rotation2;
        Mat rotation3;

        rotation1=scale*cameraMatrix.inv()*H[a].col(0);
        rotation2=scale*cameraMatrix.inv()*H[a].col(1);
        rotation3=rotation1.cross(rotation2);

        rotation1.copyTo(rotation0.col(0));
        rotation2.copyTo(rotation0.col(1));
        rotation3.copyTo(rotation0.col(2));
        translate=scale*cameraMatrix.inv()*H[a].col(2);
        rvecsMat.push_back(translate);

        Rodrigues(rotation0,R_r_part,Jet_part);
        R_r.push_back(R_r_part);
        tvecsMat.push_back(R_r_part);
        Jett.push_back(Jet_part);
        tvecsMat1.push_back(rotation0);

     /*   for(int b =0;b<3;b++)
        {
            cout<(b,0)<<",";
            cout<(b,1)<<",";
            cout<(b,2)<<",";
            cout<(b,0)<<",";
        }
        cout<

    }

    for(int a=0;a<image_count;a++)
    {
        Mat outcan_part=Mat(3,3,CV_32FC1,Scalar::all(0));
        (tvecsMat1[a].col(0)).copyTo(outcan_part.col(0));
        (tvecsMat1[a].col(1)).copyTo(outcan_part.col(1));
        (rvecsMat[a].col(0)).copyTo(outcan_part.col(2));
        outcan.push_back(outcan_part);
    }
}

转载请注明出处。
下一章博客将利用ceres进行第一次优化内外参数。