cvcalibration.cpp.svn-base

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

    a2 = icvCreateVector_64d( numPoints );
    a3 = icvCreateVector_64d( numPoints );
    delta_x = icvCreateMatrix_64d( numPoints, 2 );
    tmpn = icvCreateVector_64d( numPoints );
    aa = icvCreateVector_64d( numPoints );
    bb = icvCreateVector_64d( numPoints );
    cc = icvCreateVector_64d( numPoints );
    ddelta_xdom = icvCreateMatrix_64d( 3, 2 * numPoints );
    ddelta_xdT = icvCreateMatrix_64d( 3, 2 * numPoints );
    ddelta_xdk = icvCreateMatrix_64d( 4, 2 * numPoints );
    dxd2dom = icvCreateMatrix_64d( 3, 2 * numPoints );
    dxd2dT = icvCreateMatrix_64d( 3, 2 * numPoints );
    dxd2dk = icvCreateMatrix_64d( 4, 2 * numPoints );

    icvRigidMotionTransform( numPoints, objectPoints, rotVect, transVect,
                             Y, /* Output points */
                             dYdom, dYdT );

    for( t = 0; t < numPoints; t++ )
    {
        inv_Z[t] = 1.0f / Y[t].z;
    }

    for( t = 0; t < numPoints; t++ )
    {
        x[t].x = Y[t].x * inv_Z[t];
        x[t].y = Y[t].y * inv_Z[t];
    }

    icvTransposeMatrix_64d( (double *) x, 2, numPoints, trans_x );

    for( t = 0; t < numPoints; t++ )
    {
        bb[t] = -x[t].x * inv_Z[t];
        cc[t] = -x[t].y * inv_Z[t];
    }

    for( t = 0; t < numPoints; t++ )
    {
        icvScaleVector_64d( dYdom + t * 9, dxdom + t * 6, 3, inv_Z[t] );
        icvScaleVector_64d( dYdom + t * 9 + 6, tmp3, 3, bb[t] );
        icvAddVector_64d( dxdom + t * 6, tmp3, dxdom + t * 6, 3 );
        icvScaleVector_64d( dYdom + t * 9 + 3, dxdom + t * 6 + 3, 3, inv_Z[t] );
        icvScaleVector_64d( dYdom + t * 9 + 6, tmp3, 3, cc[t] );
        icvAddVector_64d( dxdom + t * 6 + 3, tmp3, dxdom + t * 6 + 3, 3 );
    }

    for( t = 0; t < numPoints; t++ )
    {
        icvScaleVector_64d( dYdT + t * 9, dxdT + t * 6, 3, inv_Z[t] );
        icvScaleVector_64d( dYdT + t * 9 + 6, tmp3, 3, bb[t] );
        icvAddVector_64d( dxdT + t * 6, tmp3, dxdT + t * 6, 3 );
        icvScaleVector_64d( dYdT + t * 9 + 3, dxdT + t * 6 + 3, 3, inv_Z[t] );
        icvScaleVector_64d( dYdT + t * 9 + 6, tmp3, 3, cc[t] );
        icvAddVector_64d( dxdT + t * 6 + 3, tmp3, dxdT + t * 6 + 3, 3 );
    }

    /* Add Distortion */

    for( t = 0; t < numPoints; t++ )
    {
        tmp = x[t].x * x[t].x + x[t].y * x[t].y;
        r2[t] = tmp;
        r4[t] = tmp * tmp;
    }

    for( t = 0; t < numPoints; t++ )
    {
        icvScaleVector_64d( dxdom + t * 6, dr2dom + t * 3, 3, 2 * x[t].x );
        icvScaleVector_64d( dxdom + t * 6 + 3, tmp3, 3, 2 * x[t].y );
        icvAddVector_64d( dr2dom + t * 3, tmp3, dr2dom + t * 3, 3 );
    }


    for( t = 0; t < numPoints; t++ )
    {
        icvScaleVector_64d( dxdT + t * 6, dr2dT + t * 3, 3, 2 * x[t].x );
        icvScaleVector_64d( dxdT + t * 6 + 3, tmp3, 3, 2 * x[t].y );
        icvAddVector_64d( dr2dT + t * 3, tmp3, dr2dT + t * 3, 3 );
    }

    for( t = 0; t < numPoints; t++ )
    {
        icvScaleVector_64d( dr2dom + t * 3, dr4dom + t * 3, 3, r2[t] * 2 );
    }

    for( t = 0; t < numPoints; t++ )
    {
        icvScaleVector_64d( dr2dT + t * 3, dr4dT + t * 3, 3, r2[t] * 2 );
    }

    /* Radial distortion  */

    for( t = 0; t < numPoints; t++ )
    {
        cdist[t] = 1.0f + distortion[0] * r2[t] + distortion[1] * r4[t];
    }

    icvScaleVector_64d( dr2dom, dcdistdom, 3 * numPoints, distortion[0] );
    icvScaleVector_64d( dr4dom, tmpn3, 3 * numPoints, distortion[1] );
    icvAddVector_64d( dcdistdom, tmpn3, dcdistdom, 3 * numPoints );
    icvScaleVector_64d( dr2dT, dcdistdT, 3 * numPoints, distortion[0] );
    icvScaleVector_64d( dr4dT, tmpn3, 3 * numPoints, distortion[1] );
    icvAddVector_64d( dcdistdT, tmpn3, dcdistdT, 3 * numPoints );

    icvSetZero_64d( dcdistdk, 4, numPoints );
    for( t = 0; t < numPoints; t++ )
    {
        dcdistdk[t * 4 + 0] = r2[t];
        dcdistdk[t * 4 + 1] = r4[t];
    }

    icvMulVectors_64d( trans_x, cdist, xd1, numPoints );
    icvMulVectors_64d( trans_x + numPoints, cdist, xd1 + numPoints, numPoints );

    for( t = 0; t < numPoints; t++ )
    {
        icvScaleVector_64d( dcdistdom + t * 3, dxd1dom + t * 6 + 0, 3, x[t].x );
        icvScaleVector_64d( dcdistdom + t * 3, dxd1dom + t * 6 + 3, 3, x[t].y );
    }

    for( t = 0; t < numPoints; t++ )
    {
        icvScaleVector_64d( dxdom + t * 6, tmp6, 6, cdist[t] );
        icvAddVector_64d( dxd1dom + t * 6, tmp6, dxd1dom + t * 6, 6 );
    }

    for( t = 0; t < numPoints; t++ )
    {
        icvScaleVector_64d( dcdistdT + t * 3, dxd1dT + t * 6 + 0, 3, x[t].x );
        icvScaleVector_64d( dcdistdT + t * 3, dxd1dT + t * 6 + 3, 3, x[t].y );
    }

    for( t = 0; t < numPoints; t++ )
    {
        icvScaleVector_64d( dxdT + t * 6, tmp6, 6, cdist[t] );
        icvAddVector_64d( dxd1dT + t * 6, tmp6, dxd1dT + t * 6, 6 );
    }

    for( t = 0; t < numPoints; t++ )
    {
        icvScaleVector_64d( dcdistdk + t * 4, dxd1dk + t * 8 + 0, 4, x[t].x );
        icvScaleVector_64d( dcdistdk + t * 4, dxd1dk + t * 8 + 4, 4, x[t].y );
    }

    /* Tangential distortion */
    for( t = 0; t < numPoints; t++ )
    {
        tmpx = x[t].x;
        tmpy = x[t].y;
        a1[t] = 2 * tmpx * tmpy;
        a2[t] = r2[t] + 2 * tmpx * tmpx;
        a3[t] = r2[t] + 2 * tmpy * tmpy;
    }

    icvScaleVector_64d( a1, delta_x, numPoints, distortion[2] );
    icvScaleVector_64d( a2, tmpn, numPoints, distortion[3] );
    icvAddVector_64d( delta_x, tmpn, delta_x, numPoints );

    icvScaleVector_64d( a3, delta_x + numPoints, numPoints, distortion[2] );
    icvScaleVector_64d( a1, tmpn, numPoints, distortion[3] );
    icvAddVector_64d( delta_x + numPoints, tmpn, delta_x + numPoints, numPoints );

    /* aa,bb,cc */

    icvScaleVector_64d( trans_x + numPoints, tmpn, numPoints, 2 * distortion[2] );
    icvScaleVector_64d( trans_x, aa, numPoints, 6 * distortion[3] );
    icvAddVector_64d( aa, tmpn, aa, numPoints );

    icvScaleVector_64d( trans_x, tmpn, numPoints, 2 * distortion[2] );
    icvScaleVector_64d( trans_x + numPoints, bb, numPoints, 2 * distortion[3] );
    icvAddVector_64d( bb, tmpn, bb, numPoints );

    icvScaleVector_64d( trans_x + numPoints, tmpn, numPoints, 6 * distortion[2] );
    icvScaleVector_64d( trans_x, cc, numPoints, 2 * distortion[3] );
    icvAddVector_64d( cc, tmpn, cc, numPoints );


    for( t = 0; t < numPoints; t++ )
    {
        icvScaleVector_64d( dxdom + t * 6, ddelta_xdom + t * 6, 3, aa[t] );
        icvScaleVector_64d( dxdom + t * 6 + 3, tmp3, 3, bb[t] );
        icvAddVector_64d( ddelta_xdom + t * 6, tmp3, ddelta_xdom + t * 6, 3 );

        icvScaleVector_64d( dxdom + t * 6, ddelta_xdom + t * 6 + 3, 3, bb[t] );
        icvScaleVector_64d( dxdom + t * 6 + 3, tmp3, 3, cc[t] );
        icvAddVector_64d( ddelta_xdom + t * 6 + 3, tmp3, ddelta_xdom + t * 6 + 3, 3 );
    }

    for( t = 0; t < numPoints; t++ )
    {
        icvScaleVector_64d( dxdT + t * 6, ddelta_xdT + t * 6, 3, aa[t] );
        icvScaleVector_64d( dxdT + t * 6 + 3, tmp3, 3, bb[t] );
        icvAddVector_64d( ddelta_xdT + t * 6, tmp3, ddelta_xdT + t * 6, 3 );

        icvScaleVector_64d( dxdT + t * 6, ddelta_xdT + t * 6 + 3, 3, bb[t] );
        icvScaleVector_64d( dxdT + t * 6 + 3, tmp3, 3, cc[t] );
        icvAddVector_64d( ddelta_xdT + t * 6 + 3, tmp3, ddelta_xdT + t * 6 + 3, 3 );
    }

    icvSetZero_64d( ddelta_xdk, 4, 2 * numPoints );

    for( t = 0; t < numPoints; t++ )
    {
        ddelta_xdk[t * 8 + 2] = a1[t];
        ddelta_xdk[t * 8 + 3] = a2[t];
        ddelta_xdk[t * 8 + 6] = a3[t];
        ddelta_xdk[t * 8 + 7] = a1[t];
    }

    icvAddVector_64d( xd1, delta_x, xd2, 2 * numPoints );
    icvAddVector_64d( dxd1dom, ddelta_xdom, dxd2dom, 2 * numPoints * 3 );
    icvAddVector_64d( dxd1dT, ddelta_xdT, dxd2dT, 2 * numPoints * 3 );
    icvAddVector_64d( dxd1dk, ddelta_xdk, dxd2dk, 2 * numPoints * 4 );

    /* Pixel coordinates */

    for( t = 0; t < numPoints; t++ )
    {
        imagePoints[t].x = xd2[0 * numPoints + t] * focalLength[0] + principalPoint.x;
        imagePoints[t].y = xd2[1 * numPoints + t] * focalLength[1] + principalPoint.y;
    }

    for( t = 0; t < numPoints; t++ )
    {
        icvScaleVector_64d( dxd2dom + t * 6, derivPointsRot + t * 6, 3,
                             focalLength[0] );
        icvScaleVector_64d( dxd2dom + t * 6 + 3, derivPointsRot + t * 6 + 3, 3,
                             focalLength[1] );
    }

    for( t = 0; t < numPoints; t++ )
    {
        icvScaleVector_64d( dxd2dT + t * 6, derivPointsTrans + t * 6, 3,
                             focalLength[0] );
        icvScaleVector_64d( dxd2dT + t * 6 + 3, derivPointsTrans + t * 6 + 3, 3,
                             focalLength[1] );
    }

    for( t = 0; t < numPoints; t++ )
    {
        icvScaleVector_64d( dxd2dk + t * 8, derivPointsDistort + t * 8, 4,
                             focalLength[0] );
        icvScaleVector_64d( dxd2dk + t * 8 + 4, derivPointsDistort + t * 8 + 4, 4,
                             focalLength[1] );
    }

    icvSetZero_64d( derivPointsFocal, 2, 2 * numPoints );

    for( t = 0; t < numPoints; t++ )
    {
        derivPointsFocal[t * 4 + 0] = xd2[numPoints * 0 + t];
        derivPointsFocal[t * 4 + 3] = xd2[numPoints * 1 + t];
    }

    for( t = 0; t < numPoints; t++ )
    {
        icvSetIdentity_64d( derivPointsPrincipal + t * 4, 2, 2 );
    }

    icvDeleteVector( Y );
    icvDeleteVector( x );
    icvDeleteMatrix( trans_x );
    icvDeleteMatrix( dYdom );
    icvDeleteMatrix( dYdT );
    icvDeleteVector( inv_Z );
    icvDeleteMatrix( dxdom );
    icvDeleteMatrix( dxdT );
    icvDeleteMatrix( tmpn3 );
    icvDeleteMatrix( dr2dom );
    icvDeleteMatrix( dr2dT );
    icvDeleteVector( r2 );
    icvDeleteVector( r4 );
    icvDeleteVector( cdist );
    icvDeleteMatrix( dr4dom );
    icvDeleteMatrix( dr4dT );
    icvDeleteMatrix( dcdistdom );
    icvDeleteMatrix( dcdistdT );
    icvDeleteMatrix( dcdistdk );
    icvDeleteMatrix( xd1 );
    icvDeleteMatrix( xd2 );
    icvDeleteMatrix( dxd1dom );
    icvDeleteMatrix( dxd1dT );
    icvDeleteMatrix( dxd1dk );
    icvDeleteVector( a1 );
    icvDeleteVector( a2 );
    icvDeleteVector( a3 );
    icvDeleteMatrix( delta_x );
    icvDeleteVector( tmpn );
    icvDeleteVector( aa );
    icvDeleteVector( bb );
    icvDeleteVector( cc );
    icvDeleteMatrix( ddelta_xdom );
    icvDeleteMatrix( ddelta_xdT );
    icvDeleteMatrix( ddelta_xdk );
    icvDeleteMatrix( dxd2dom );
    icvDeleteMatrix( dxd2dT );
    icvDeleteMatrix( dxd2dk );

    return CV_NO_ERR;
}

#define REAL_ZERO(x) (x<0.00001 && x> -0.00001)
#define Sgn(x)  (x<0?-1:(x>0?1:0))

/*======================================================================================*/
CvStatus
icvFindExtrinsicCameraParams_64d( int numPoints,
                                  CvSize imageSize,
                                  CvPoint2D64d * imagePoints,
                                  CvPoint3D64d * objectPoints,
                                  CvVect64d focalLength,
                                  CvPoint2D64d principalPoint,
                                  CvVect64d distortion,
                                  CvVect64d rotVect, CvVect64d transVect )
{
    /* Normalizate points  */

    CvPoint3D64d *resImagePoints;
    CvMatr64d matrY;
    CvMatr64d X_kk_trans;
    CvPoint2D64d *im2Pnt;
    CvPoint2D64d *ob2Pnt;
    CvMatr64d X_new;
    int t;
    double det;
    double r;
    double tmpf1, tmpf2;
    CvPoint2D64d *projectImagePoints;
    CvVect64d derivPointsRot;
    CvVect64d derivPointsTrans;
    CvVect64d derivPointsFocal;
    CvVect64d derivPointsPrincipal;
    CvVect64d derivPointsDistort;
    CvMatr64d matrJJ;
    CvMatr64d matrJJt;

    CvPoint2D64d *ex_points;
    int iter;
    double change;
    double norm1;
    double norm2;
    double sc;

    double matrYY[3 * 3];
    double matrU[3 * 3];
    double matrV[3 * 3];
    double matrS[3];
    double X_mean[3];
    double R_trans[3 * 3];
    double Homography[3 * 3];
    double rotMatr[3 * 3];
    double T_trans[3];
    double tmp3[3];
    double tmp3f1[3];
    double tmp3f2[3];
    double Jacobian[3 * 9];
    double omckk[3];
    double tmpRotMatr[3 * 3];
    double matrJJtJJ[6 * 6];
    double invmatrJJtJJ[6 * 6];
    double tmp6[6];
    double vectParam_innov[6];
    double vect_Param_up[6];
    double vect_Param[6];

    CvMat  matU, matS, matV;

    matrY = icvCreateMatrix_64d( numPoints, 3 );
    X_new = icvCreateMatrix_64d( numPoints, 3 );
    X_kk_trans = icvCreateMatrix_64d( numPoints, 3 );
    matrJJ = icvCreateMatrix_64d( 6, 2 * numPoints );
    matrJJt = icvCreateMatrix_64d( 2 * numPoints, 6 );
    derivPointsRot = icvCreateVector_64d( 3 * 2 * numPoints );
    derivPointsTrans = icvCreateVector_64d( 3 * 2 * numPoints );
    derivPointsFocal = icvCreateVector_64d( 2 * 2 * numPoints );
    derivPointsPrincipal = icvCreateVector_64d( 2 * 2 * numPoints );
    derivPointsDistort = icvCreateVector_64d( 4 * 2 * numPoints );
    im2Pnt = (CvPoint2D64d *) icvCreateVector_64d( numPoints * 2 );
    ob2Pnt = (CvPoint2D64d *) icvCreateVector_64d( numPoints * 2 );
    projectImagePoints = (CvPoint2D64d *) icvCreateVector_64d( numPoints * 2 );
    ex_points = (CvPoint2D64d *) icvCreateVector_64d( numPoints * 2 );
    resImagePoints = (CvPoint3D64d *) icvCreateVector_64d( numPoints * 3 );

    /*  compute trans matrix of points */
    icvTransposeMatrix_64d( (double *) objectPoints, 3, numPoints, X_kk_trans );

    icvNormalizeImagePoints( numPoints,
                             imagePoints,
                             focalLength, principalPoint, distortion, resImagePoints );


    /* check for planarity of the structure */
    /*  compute mean value */

    X_mean[0] = 0;
    X_mean[1] = 0;
    X_mean[2] = 0;

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

    X_mean[0] = X_mean[0] / numPoints;
    X_mean[1] = X_mean[1] / numPoints;