cvcalibration.cpp.svn-base

来自「非结构化路识别」· SVN-BASE 代码 · 共 2,039 行 · 第 1/5 页

    X_mean[2] = X_mean[2] / numPoints;


    /*  fill matrix Y */
    for( t = 0; t < numPoints; t++ )
    {
        matrY[numPoints * 0 + t] = X_kk_trans[numPoints * 0 + t] - X_mean[0];
        matrY[numPoints * 1 + t] = X_kk_trans[numPoints * 1 + t] - X_mean[1];
        matrY[numPoints * 2 + t] = X_kk_trans[numPoints * 2 + t] - X_mean[2];
    }

    /* Compute matrix YY */

    icvMulTransMatrixL_64d( matrY, numPoints, 3, matrYY );

    /*  Compute SVD of YY only */

    icvCopyMatrix_64d( matrYY, 3, 3, matrU );

    /* icvSVDecomposition_64d( matrU, 3,
                             3,
                             matrS, 
                             matrV ); */

    matU = cvMat( 3, 3, CV_MAT64D, matrU );
    matV = cvMat( 3, 3, CV_MAT64D, matrV );
    matS = cvMat( 3, 1, CV_MAT64D, matrS );
    cvSVD( &matU, &matS, 0, &matV );

    r = matrS[2] / matrS[1];

    if( r < 0.0001 )
    {                           /* Test for planarity */
        /* Transorm the plane to bring it in the Z=0 plane */

        icvTransposeMatrix_64d( matrV, 3, 3, R_trans );

       double r_norm = sqrt((double)R_trans[2]*R_trans[2] +
                            (double)R_trans[5]*R_trans[5]);

        if( r_norm < 1e-6 )
        {
            R_trans[0] = 1; R_trans[1] = 0; R_trans[2] = 0;
            R_trans[3] = 0; R_trans[4] = 1; R_trans[5] = 0;
            R_trans[6] = 0; R_trans[7] = 0; R_trans[8] = 1;
        }

        icvDetMatrix3x3_64d( R_trans, &det );

        if( det < 0 )
        {
            icvScaleMatrix_64d( R_trans, R_trans, 3, 3, -1 );

        }

        icvMulMatrix_64d( R_trans, 3, 3, X_mean, 1, 3, T_trans );
        icvScaleMatrix_64d( T_trans, T_trans, 1, 3, -1 );

        icvMulMatrix_64d( R_trans, 3, 3, X_kk_trans, numPoints, 3, X_new );

        for( t = 0; t < numPoints; t++ )
        {
            X_new[numPoints * 0 + t] += T_trans[0];
        }

        for( t = 0; t < numPoints; t++ )
        {
            X_new[numPoints * 1 + t] += T_trans[1];
        }

        for( t = 0; t < numPoints; t++ )
        {
            X_new[numPoints * 2 + t] += T_trans[2];
        }

        /* Compute the planar homography */
        for( t = 0; t < numPoints; t++ )
        {
            im2Pnt[t].x = resImagePoints[t].x;
            im2Pnt[t].y = resImagePoints[t].y;
            ob2Pnt[t].x = X_new[0 * numPoints + t];
            ob2Pnt[t].y = X_new[1 * numPoints + t];
        }

        icvFindHomography( numPoints, imageSize, im2Pnt, ob2Pnt, Homography );


        /* De-embed the motion parameters from the homography */
        for( t = 0; t < 3; t++ )
        {
            tmp3[t] = Homography[t * 3 + 0];
        }
        tmpf1 = icvNormVector_64d( tmp3, 3 );

        for( t = 0; t < 3; t++ )
        {
            tmp3[t] = Homography[t * 3 + 1];
        }
        tmpf2 = icvNormVector_64d( tmp3, 3 );
        
        sc = (tmpf1 + tmpf2) * 0.5f;

        icvScaleVector_64d( Homography, Homography, 9, 1.0f / sc );

        
        /* Scale part of homography */
        for( t = 0; t < 3; t++ )
        {
            tmp3[t] = Homography[t * 3 + 0];
        }
        tmpf1 = icvNormVector_64d( tmp3, 3 );

        for( t = 0; t < 3; t++ )
        {
            tmp3[t] = Homography[t * 3 + 1];
        }
        tmpf2 = icvNormVector_64d( tmp3, 3 );

        for( t = 0; t < 3; t++ )
        {
            Homography[t*3+0] = Homography[t*3+0]/tmpf1;
        }

        for( t = 0; t < 3; t++ )
        {
            Homography[t*3+1] = Homography[t*3+1]/tmpf2;
        }

        /* fill rotate matrix */
        icvCopyVector_64d( Homography, 9, rotMatr );

        tmp3f1[0] = Homography[0];
        tmp3f1[1] = Homography[3]; 
        tmp3f1[2] = Homography[6];

        tmp3f2[0] = Homography[1];
        tmp3f2[1] = Homography[4];
        tmp3f2[2] = Homography[7];

        icvCrossProduct2L_64d( tmp3f1, tmp3f2, tmp3 );

        rotMatr[0] = Homography[0];
        rotMatr[3] = Homography[3];
        rotMatr[6] = Homography[6];

        rotMatr[1] = Homography[1];
        rotMatr[4] = Homography[4];
        rotMatr[7] = Homography[7];

        rotMatr[2] = tmp3[0];
        rotMatr[5] = tmp3[1];
        rotMatr[8] = tmp3[2];

        icvRodrigues_64d( rotMatr, omckk, Jacobian, CV_RODRIGUES_M2V );

        icvRodrigues_64d( rotMatr, omckk, Jacobian, CV_RODRIGUES_V2M );

        transVect[0] = Homography[2];
        transVect[1] = Homography[5];
        transVect[2] = Homography[8];

        icvMulMatrix_64d( rotMatr, 3, 3, T_trans, 1, 3, tmp3 );

        icvAddVector_64d( transVect, tmp3, transVect, 3 );

        /* Compute rotation matrix */
        icvMulMatrix_64d( rotMatr, 3, 3, R_trans, 3, 3, tmpRotMatr );

        icvRodrigues_64d( tmpRotMatr, rotVect, Jacobian, CV_RODRIGUES_M2V );

    }
    else
    {
        /* Non planar structure detected */
        
        int i;
//        J = zeros(2*Np,12);
        CvMatr64d matrJ;
        CvMatr64d matrJtrans;
        CvMatr64d matrJJ;
        matrJ = icvCreateMatrix_64d( 12, 2 * numPoints );
        matrJtrans = icvCreateMatrix_64d( 2 * numPoints,12 );
        matrJJ = icvCreateMatrix_64d(12,12);
        icvSetZero_64d( matrJ,      12, 2 * numPoints );
        icvSetZero_64d( matrJtrans, 2 * numPoints, 12 );

//        xX = (ones(3,1)*xn(1,:)).*X_kk;
        CvMatr64d matrxX;
        matrxX = icvCreateMatrix_64d(numPoints,3);
        /* Fill matrix */
        for( i = 0; i < numPoints; i++ )
        {
            matrxX[i]             = resImagePoints[i].x * X_kk_trans[i];
            matrxX[numPoints+i]   = resImagePoints[i].x * X_kk_trans[numPoints+i];
            matrxX[numPoints*2+i] = resImagePoints[i].x * X_kk_trans[numPoints*2+i];
        }
        
//        yX = (ones(3,1)*xn(2,:)).*X_kk;
        CvMatr64d matryX;
        matryX = icvCreateMatrix_64d(numPoints,3);
        /* Fill matrix */
        for( i = 0; i < numPoints; i++ )
        {
            matryX[i]             = X_kk_trans[i]             * resImagePoints[i].y;
            matryX[numPoints+i]   = X_kk_trans[numPoints+i]   * resImagePoints[i].y;
            matryX[numPoints*2+i] = X_kk_trans[numPoints*2+i] * resImagePoints[i].y;
        }

        for( i = 0; i < numPoints; i++ )
        {
//        J(1:2:end,[1 4 7]) = -X_kk';
            matrJ[i*12*2+0] = -objectPoints[i].x;
            matrJ[i*12*2+3] = -objectPoints[i].y;
            matrJ[i*12*2+6] = -objectPoints[i].z;

//        J(2:2:end,[2 5 8]) = X_kk';
            matrJ[i*12*2+12+1] = objectPoints[i].x;
            matrJ[i*12*2+12+4] = objectPoints[i].y;
            matrJ[i*12*2+12+7] = objectPoints[i].z;

//        J(1:2:end,[3 6 9]) = xX';
            matrJ[i*12*2+2] = matrxX[i];
            matrJ[i*12*2+5] = matrxX[numPoints+i];
            matrJ[i*12*2+8] = matrxX[numPoints*2+i];

//        J(2:2:end,[3 6 9]) = -yX';
            matrJ[i*12*2+12+2] = -matryX[i];
            matrJ[i*12*2+12+5] = -matryX[numPoints+i];
            matrJ[i*12*2+12+8] = -matryX[numPoints*2+i];

//        J(1:2:end,12) = xn(1,:)';
            matrJ[i*12*2+11] = resImagePoints[i].x;

//        J(2:2:end,12) = -xn(2,:)';
            matrJ[i*12*2+12+11] = -resImagePoints[i].y;

//        J(1:2:end,10) = -ones(Np,1);
            matrJ[i*12*2+9] = -1;

//        J(2:2:end,11) = ones(Np,1);
            matrJ[i*12*2+12+10] = 1;

        }

        /* compute trans matrix J */
        icvTransposeMatrix_64d(matrJ,12,numPoints*2,matrJtrans);


//        JJ = J'*J;
        icvMulMatrix_64d(matrJtrans,numPoints*2,12,matrJ,12,numPoints*2,matrJJ);
        
//        [U,S,V] = svd(JJ);
        CvMat* matrVm;
        matrVm = cvCreateMat(12,12,CV_64FC1);

        CvMat* matrJJm;
        matrJJm = cvCreateMat(12,12,CV_64FC1);

        /* Copy matrix JJ */
        for( i = 0; i < 12*12; i++ )
        {
            CvScalar value;
            value.val[0] = matrJJ[i];
            cvSetAt(matrJJm,value,i/12,i%12);
        }

        CvMat* matrWm;
        matrWm = cvCreateMat(12,12,CV_64FC1);

        cvSVD(matrJJm,matrWm,0,matrVm);
//        cvSVD( CvArr* aarr, CvArr* warr, CvArr* uarr, CvArr* varr, int flags )


//        RR = reshape(V(1:9,12),3,3);
        CvMat* matrRR;
        matrRR = cvCreateMat(3,3,CV_64FC1);
        /* Copy matrix elements (reshape) */
        for( i = 0; i < 9; i++ )
        {
            CvScalar value;
            value = cvGetAt( matrVm, i, 11 );
            cvSetAt(matrRR,value,i%3,i/3);
        }
        double det;
        det = cvDet(matrRR);

        

//        if det(RR) < 0,
//          V(:,12) = -V(:,12);
//          RR = -RR;
//        end;
        if( det < 0 )
        {
            /* Change sign in column of matrix matrVm */
            CvScalar value;
            for( i = 0; i < 12; i++ )
            {
                value = cvGetAt(matrVm,i,11);
                value.val[0] = - value.val[0];
                cvSetAt(matrVm,value,i,11);
            }
            /* Change sign of RR */
            cvConvertScale(matrRR,matrRR,-1);
        }

//        [Ur,Sr,Vr] = svd(RR);
        /* Make SVD of RR*/  
        CvMat* matrUr;
        CvMat* matrVr;
        CvMat* matrWr;
        matrUr = cvCreateMat(3,3,CV_64FC1);
        matrVr = cvCreateMat(3,3,CV_64FC1);
        matrWr = cvCreateMat(3,3,CV_64FC1);
        cvSVD(matrRR,matrWr,matrUr,matrVr);


//        Rckk = Ur*Vr';
        /* Make matrix Rckk */
        CvMat* matrRckk;
        CvMat* matrVrt;
        matrRckk = cvCreateMat(3,3,CV_64FC1);

        matrVrt = cvCreateMat(3,3,CV_64FC1);
        cvTranspose(matrVr,matrVrt);

        cvMatMul(matrUr,matrVrt,matrRckk);



//        sc = norm(V(1:9,12)) / norm(Rckk(:));
        double sum;
        double norm1,norm2;
        
        sum = 0;
        for( i = 0; i < 9; i++ )
        {
            CvScalar value;
            value = cvGetAt(matrRckk,i/3,i%3);
            sum += (value.val[0] * value.val[0]);
        }
        norm1 = sqrt(sum);

        sum = 0;
        for( i = 0; i < 9; i++ )
        {
            CvScalar value;
            value = cvGetAt(matrVm,i,11);
            sum += (value.val[0] * value.val[0]);
        }
        norm2 = sqrt(sum);

        double sc;
        sc = norm2 / norm1;

//        Tckk = V(10:12,12)/sc;
        CvMat* matrTckk;
        matrTckk = cvCreateMat(3,1,CV_64FC1);
        for( i = 0; i < 3; i++ )
        {
            CvScalar value;
            value = cvGetAt(matrVm,i+9,11);
            cvSetAt(matrTckk,value,i,0);
        }
        cvConvertScale(matrTckk,matrTckk,1.0/sc);


//        omckk = rodrigues(Rckk);
        CvMat* vectomckk;
        vectomckk = cvCreateMat(3,1,CV_64FC1);

        CvMat* jacobian;

        jacobian = cvCreateMat(3,9,CV_64FC1);
        
        cvRodrigues( matrRckk, vectomckk,
                     jacobian, CV_RODRIGUES_M2V);
        
//        Rckk = rodrigues(omckk);
        cvRodrigues( matrRckk, vectomckk,
                     jacobian, CV_RODRIGUES_V2M);

        /* Copy rot vector */
        for( i = 0; i < 3; i++ )
        {
            CvScalar value;
            value = cvGetAt(vectomckk,i,0);
            rotVect[i] = value.val[0];
        }

        /* Copy  trans vect*/
        for( i = 0; i < 3; i++ )
        {
            CvScalar value;
            value = cvGetAt(matrTckk,i,0);
            transVect[i] = value.val[0];
        }


        /* Free matrices */
        icvDeleteMatrix(matrJ);
        icvDeleteMatrix(matrJtrans);
        icvDeleteMatrix(matrJJ);
        icvDeleteMatrix(matrxX);
        icvDeleteMatrix(matryX);
        
        cvRe