cvcalibration.cpp.svn-base

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

        icvDetMatrix3x3_64d(rotMatr, &detRotMatr);

        if( fabs(detRotMatr - 1.0) < bigeps )
        {
            /* norm(in' * in - eye(3)) < bigeps */
            icvInvertMatrix_64d(rotMatr,3,invRotMatr);
            icvMulMatrix_64d(rotMatr,3,3,invRotMatr,3,3,tmpmat3);
            icvSetIdentity_64d(tmpEye3, 3, 3);
            icvSubMatrix_64d(tmpmat3,tmpEye3,tmpmat3,3,3);

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

            tmpMat = cvMat( 3, 3, CV_64FC1, tmpmat3 );
            matS = cvMat( 3, 1, CV_64FC1, matrS );
            cvSVD( &tmpMat, &matS );
            
            if( matrS[0] < bigeps )/* norm(matr) is max of singular value of matr  */
            {
                icvTrace_64d(rotMatr, 3, 3, &trace);

                trace       = (trace - 1.0) / 2.0;

                if( trace > 1 )
                {
                    theta = 0;
                }
                else
                {
                    if( trace < -1 )
                    {
                        theta = -CV_PI;
                    }
                    else
                    {
                        theta = acos(trace);
                    }
                }


                icvSetZero_64d(dtrdR,9,1);
                
                dtrdR[0]= 0.5f;
                dtrdR[4]= 0.5f;
                dtrdR[8]= 0.5f;

                if( sin(theta) >= 1e-5 )
                {
                    /* Test for error sqrt(x) x >= 0 */

                    vth = (double)(1/(2*sin(theta)));
                    
                    om1[0] = rotMatr[2 * 3 + 1] - rotMatr[1 * 3 + 2];
                    om1[1] = rotMatr[0 * 3 + 2] - rotMatr[2 * 3 + 0];
                    om1[2] = rotMatr[1 * 3 + 0] - rotMatr[0 * 3 + 1];

                    icvScaleVector_64d(om1,om,3,vth);

                    icvScaleVector_64d(om,rotVect,3,theta);

                    if( Jacobian != 0 )
                    {
                        dthetadtr = (double)(-1 / sqrt(1. - trace * trace));

                        icvScaleVector_64d(dtrdR, dthetadR, 9, dthetadtr);
                        

                        dvar1dtheta[0] = (double)(-vth*cos(theta)/sin(theta));
                        dvar1dtheta[1] = 1;

                        icvSetZero_64d(dvardR,9,5);
                        icvMulMatrix_64d(dvar1dtheta,1,2,dthetadR,9,1,dvardR + 9 * 3);

                        dvardR[0 * 9 + 5] =  1;
                        dvardR[0 * 9 + 7] = -1;
                        dvardR[1 * 9 + 2] = -1;
                        dvardR[1 * 9 + 6] =  1;
                        dvardR[2 * 9 + 1] =  1;
                        dvardR[2 * 9 + 3] = -1;
                        
                        icvSetZero_64d(dvar2dvar,5,4);

                        dvar2dvar[0 * 5 + 0] = vth;
                        dvar2dvar[1 * 5 + 1] = vth;
                        dvar2dvar[2 * 5 + 2] = vth;

                        dvar2dvar[0 * 5 + 3] = om1[0];
                        dvar2dvar[1 * 5 + 3] = om1[1];
                        dvar2dvar[2 * 5 + 3] = om1[2];
                        dvar2dvar[3 * 5 + 4] = 1;

                        /* Calculate dout = domegadvar2 * dvar2dvar * dvardR */

                        icvSetZero_64d(domegadvar2,4,3);

                        domegadvar2[0 * 4 + 0] = theta;
                        domegadvar2[1 * 4 + 1] = theta;
                        domegadvar2[2 * 4 + 2] = theta;

                        domegadvar2[0 * 4 + 3] = om[0];
                        domegadvar2[1 * 4 + 3] = om[1];
                        domegadvar2[2 * 4 + 3] = om[2];

                        icvMulMatrix_64d(domegadvar2,4,3,dvar2dvar,5,4,tmp53);

                        icvMulMatrix_64d(tmp53,5,3,dvardR,9,5,Jacobian);
                    }
                }
                else
                {
                    if( trace > 0 )
                    {
                        icvSetZero_64d(rotVect, 3, 1);

                        if( Jacobian != 0 )
                        {
                            icvSetZero_64d(Jacobian, 9, 3);

                            Jacobian[0 * 9 + 5] =  0.5f;
                            Jacobian[0 * 9 + 7] = -0.5f;
                            Jacobian[1 * 9 + 2] = -0.5f;
                            Jacobian[1 * 9 + 6] =  0.5f;
                            Jacobian[2 * 9 + 1] =  0.5f;
                            Jacobian[2 * 9 + 3] = -0.5f;
                        }
                    }
                    else
                    {
                        tmp3[0] = (double)(sqrt((rotMatr[0 * 3 + 0] + 1.) * 0.5));
                        tmp3[1] = (double)(sqrt((rotMatr[1 * 3 + 1] + 1.) * 0.5));
                        tmp3[2] = (double)(sqrt((rotMatr[2 * 3 + 2] + 1.) * 0.5));

                        if( rotMatr[0 * 3 + 1] < 0 )
                        {
                            tmp3[1] = - tmp3[1];
                        }

                        if( rotMatr[0 * 3 + 2] < 0 )
                        {
                            tmp3[2] = - tmp3[2];
                        }

                        icvCopyVector_64d(tmp3,3,rotVect);


                        if( Jacobian != 0 )
                        {
                            /* !!! Jacobian is not defined in this section */
                            /* If it's need it is an error. */
                            /* We must to set Jacobian to NaN */
                            /* NaN - not a number (1.#QNAN) */
                            icvSetZero_64d(Jacobian,9,3);
                        }
                    }
                }
            }
            else
            {/* max sing > bigeps */
                return CV_BADFACTOR_ERR;
            }

        }/* test for bigeps */
        else
        {/* det > bigeps */
            return CV_BADFACTOR_ERR;
        }
        
    }/* Convertion type */

    return CV_NO_ERR;
}

/*======================================================================================*/

CvStatus
icvNormalizeImagePoints( int numPoints,
                         CvPoint2D64d * ImagePoints,
                         CvVect64d focalLength,
                         CvPoint2D64d principalPoint,
                         CvVect64d distortion, CvPoint3D64d * resImagePoints )
{
    int t;
    int iter;
    double k1, k2;
    double p1, p2;
    double tmpr;
    double k_radial;
    double x, y;
    double delta_x, delta_y;
    CvMatr64d r_2;
    CvPoint2D64d *desPnt;

    r_2 = icvCreateVector_64d( numPoints );

    desPnt = (CvPoint2D64d *) icvCreateVector_64d( numPoints * 2 );

    k1 = distortion[0];
    k2 = distortion[1];

    p1 = distortion[2];
    p2 = distortion[3];

    /* Shift points to principal point and use focal length */
    for( t = 0; t < numPoints; t++ )
    {
        resImagePoints[t].x = (ImagePoints[t].x - principalPoint.x) / focalLength[0];
        resImagePoints[t].y = (ImagePoints[t].y - principalPoint.y) / focalLength[1];
        resImagePoints[t].z = 0;
        desPnt[t].x = (ImagePoints[t].x - principalPoint.x) / focalLength[0];
        desPnt[t].y = (ImagePoints[t].y - principalPoint.y) / focalLength[1];
    }

    /* Compensate lens distortion */

    for( iter = 0; iter < 5; iter++ )
    {
        for( t = 0; t < numPoints; t++ )
        {
            x = resImagePoints[t].x;
            y = resImagePoints[t].y;

            tmpr = x * x + y * y;

            k_radial = 1 + k1 * tmpr + k2 * tmpr * tmpr;
            r_2[t] = tmpr;

            delta_x = 2 * p1 * x * y + p2 * (tmpr + 2 * x * x);
            delta_y = p1 * (tmpr + 2 * y * y) + 2 * p2 * x * y;

            x = (desPnt[t].x - delta_x) / k_radial;
            y = (desPnt[t].y - delta_y) / k_radial;

            resImagePoints[t].x = x;
            resImagePoints[t].y = y;
        }
    }

    icvDeleteVector( r_2 );
    icvDeleteVector( desPnt );

    return CV_NO_ERR;
}

/*======================================================================================*/
/* Computes the rigid motion transformation Y = R*X+T, where R = rodrigues(om) */
CvStatus
icvRigidMotionTransform( int numPoints,
                         CvPoint3D64d * objectPoints,
                         CvVect64d rotVect, CvVect64d transVect,
                         CvPoint3D64d * rigidMotionTrans, /* Output points */
                         CvMatr64d derivMotionRot, CvMatr64d derivMotionTrans )
{
    CvMatr64d tmpMatr;
    CvMatr64d tmpMatr2;
    CvMatr64d dYdR;
    int t;
    double x, y, z;

    double rotMatr[3 * 3];
    double dRdom[3 * 9];

    tmpMatr = icvCreateMatrix_64d( numPoints, 3 );
    tmpMatr2 = icvCreateMatrix_64d( numPoints, 3 );
    dYdR = icvCreateMatrix_64d( 9, 3 * numPoints );

    icvRodrigues_64d( rotMatr, rotVect, dRdom, CV_RODRIGUES_V2M );

    icvTransposeMatrix_64d( (double *) objectPoints, 3, numPoints, tmpMatr );

    icvMulMatrix_64d( rotMatr, 3, 3, tmpMatr, numPoints, 3, tmpMatr2 );

    icvTransposeMatrix_64d( tmpMatr2, numPoints, 3, (double *) rigidMotionTrans );

    for( t = 0; t < numPoints; t++ )
    {
        icvAddVector_64d( (double *) (rigidMotionTrans + t),
                           transVect, (double *) (rigidMotionTrans + t), 3 );
    }

    icvSetZero_64d( dYdR, 9, 3 * numPoints );

    for( t = 0; t < numPoints; t++ )
    {
        icvSetIdentity_64d( derivMotionTrans + t * 9, 3, 3 );
    }

    for( t = 0; t < numPoints; t++ )
    {
        x = objectPoints[t].x;
        y = objectPoints[t].y;
        z = objectPoints[t].z;

        dYdR[t * 27 + 0] = x;
        dYdR[t * 27 + 3] = y;
        dYdR[t * 27 + 6] = z;

        dYdR[t * 27 + 10] = x;
        dYdR[t * 27 + 13] = y;
        dYdR[t * 27 + 16] = z;

        dYdR[t * 27 + 20] = x;
        dYdR[t * 27 + 23] = y;
        dYdR[t * 27 + 26] = z;
    }

    icvMulMatrix_64d( dYdR, 9, numPoints * 3, dRdom, 3, 9, derivMotionRot );

    icvDeleteMatrix( dYdR );
    icvDeleteMatrix( tmpMatr );
    icvDeleteMatrix( tmpMatr2 );

    return CV_NO_ERR;
}

/*======================================================================================*/

CvStatus
icvProjectPoints( int numPoints,
                  CvPoint3D64d * objectPoints,
                  CvVect64d rotVect,
                  CvVect64d transVect,
                  CvVect64d focalLength,
                  CvPoint2D64d principalPoint,
                  CvVect64d distortion,
                  CvPoint2D64d * imagePoints,
                  CvVect64d derivPointsRot,
                  CvVect64d derivPointsTrans,
                  CvVect64d derivPointsFocal,
                  CvVect64d derivPointsPrincipal, CvVect64d derivPointsDistort )
{
    CvPoint3D64d *Y;
    CvPoint2D64d *x;

    CvMatr64d trans_x;
    CvMatr64d dYdom;
    CvMatr64d dYdT;

    CvVect64d inv_Z;
    CvVect64d bb;
    CvVect64d cc;

    CvMatr64d dxdom;
    CvMatr64d dxdT;

    CvVect64d r2;
    CvVect64d r4;
    CvVect64d cdist;
    double tmp;
    double tmpx, tmpy;

    CvMatr64d tmpn3;
    CvMatr64d dr2dom;
    CvMatr64d dr2dT;
    CvMatr64d dr4dom;
    CvMatr64d dr4dT;
    CvMatr64d dcdistdom;
    CvMatr64d dcdistdT;
    CvMatr64d dcdistdk;
    CvMatr64d xd1;
    CvMatr64d xd2;
    CvMatr64d dxd1dom;
    CvVect64d tmpn;
    CvMatr64d dxd1dT;
    CvMatr64d dxd1dk;
    CvVect64d a1;
    CvVect64d a2;
    CvVect64d a3;
    CvMatr64d delta_x;
    CvVect64d aa;
    CvMatr64d ddelta_xdom;
    CvMatr64d ddelta_xdT;
    CvMatr64d ddelta_xdk;
    CvMatr64d dxd2dom;
    CvMatr64d dxd2dT;
    CvMatr64d dxd2dk;

    double tmp3[3];
    double tmp6[6];
    int t;

    Y = (CvPoint3D64d *) icvCreateVector_64d( numPoints * 3 );
    x = (CvPoint2D64d *) icvCreateVector_64d( numPoints * 2 );
    trans_x = icvCreateMatrix_64d( numPoints, 2 );
    dYdom = icvCreateMatrix_64d( 3, 3 * numPoints );
    dYdT = icvCreateMatrix_64d( 3, 3 * numPoints );
    inv_Z = icvCreateVector_64d( numPoints );
    dxdom = icvCreateMatrix_64d( 3, 2 * numPoints );
    dxdT = icvCreateMatrix_64d( 3, 2 * numPoints );
    tmpn3 = icvCreateMatrix_64d( 3, numPoints );
    dr2dom = icvCreateMatrix_64d( 3, numPoints );
    dr2dT = icvCreateMatrix_64d( 3, numPoints );
    r2 = icvCreateVector_64d( numPoints );
    r4 = icvCreateVector_64d( numPoints );
    cdist = icvCreateVector_64d( numPoints );
    dr4dom = icvCreateMatrix_64d( 3, numPoints );
    dr4dT = icvCreateMatrix_64d( 3, numPoints );
    dcdistdom = icvCreateMatrix_64d( 3, numPoints );
    dcdistdT = icvCreateMatrix_64d( 3, numPoints );
    dcdistdk = icvCreateMatrix_64d( 4, numPoints );
    xd1 = icvCreateMatrix_64d( numPoints, 2 );
    xd2 = icvCreateMatrix_64d( numPoints, 2 );
    dxd1dom = icvCreateMatrix_64d( 3, 2 * numPoints );
    dxd1dT = icvCreateMatrix_64d( 3, 2 * numPoints );
    dxd1dk = icvCreateMatrix_64d( 4, 2 * numPoints );
    a1 = icvCreateVector_64d( numPoints );