장정우 카메라 표정,opencv에서calibrate 함수 내부 구체적 실현 과정 구체적 코드 첨부(二)-calibrate 첫 번째 단계---내외 파라미터 구하기

내외 파라미터를 구하는 절차

main 함수에 함수 추가

InitHomogeneous_Matrix 함수

SolveH 함수

SolveVij_B 함수

SolveCameraMatrix 함수

SolveOutCamera 함수

내외 파라미터를 구하려면

내외 파라미터를 구하려면 다음과 같은 몇 가지 절차를 진행해야 한다. (1) 구해 단응성 매트릭스 H(2) H에 따라 초기화 V(3) V에 따라 b방향량 구해(4) b방향량에 따라 B매트릭스 구해(5) B매트릭스에 따라 폐쇄해를 구하고 내참(6) 내참, H에 따라 외참의 주의를 구해야 한다.만약에 독자가 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 함수

뒤에 있는 단응성 매트릭스에 대한 준비를 하고 검측의 각점을 대응하는 3차원점과 차등화시킨다. 그 중에서 3차원점은 (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 함수

이 함수는 바로 구해 단응성 행렬이다. 이미 알고 있는 2차원 각점과 3차원 점을 이용하여 대응하는 단응성 행렬 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)<(b,1)<(b,2)<(b,0)<

    }

    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를 이용하여 처음으로 내외 파라미터를 최적화할 것이다.