돛단배

/* finds intrinsic and extrinsic camera parameters
   from a few views of known calibration pattern */
CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
                    const CvMat* imagePoints, const CvMat* npoints,
                    CvSize imageSize, CvMat* cameraMatrix, CvMat* distCoeffs,
                    CvMat* rvecs, CvMat* tvecs, int flags )
{
    const int NINTRINSIC = 9;
    Ptr<CvMat> matM, _m, _Ji, _Je, _err;
    CvLevMarq solver;
    double reprojErr = 0;

    double A[9], k[5] = {0,0,0,0,0};
    CvMat matA = cvMat(3, 3, CV_64F, A), _k;
    int i, nimages, maxPoints = 0, ni = 0, pos, total = 0, nparams, npstep, cn;
    double aspectRatio = 0.;

    // 0. check the parameters & allocate buffers
    if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(imagePoints) ||
        !CV_IS_MAT(npoints) || !CV_IS_MAT(cameraMatrix) || !CV_IS_MAT(distCoeffs) )
        CV_Error( CV_StsBadArg, "One of required vector arguments is not a valid matrix" );

    if( imageSize.width <= 0 || imageSize.height <= 0 )
        CV_Error( CV_StsOutOfRange, "image width and height must be positive" );

    if( CV_MAT_TYPE(npoints->type) != CV_32SC1 ||
        (npoints->rows != 1 && npoints->cols != 1) )
        CV_Error( CV_StsUnsupportedFormat,
            "the array of point counters must be 1-dimensional integer vector" );

    nimages = npoints->rows*npoints->cols;
    npstep = npoints->rows == 1 ? 1 : npoints->step/CV_ELEM_SIZE(npoints->type);

    if( rvecs )
    {
        cn = CV_MAT_CN(rvecs->type);
        if( !CV_IS_MAT(rvecs) ||
            (CV_MAT_DEPTH(rvecs->type) != CV_32F && CV_MAT_DEPTH(rvecs->type) != CV_64F) ||
            ((rvecs->rows != nimages || (rvecs->cols*cn != 3 && rvecs->cols*cn != 9)) &&
            (rvecs->rows != 1 || rvecs->cols != nimages || cn != 3)) )
            CV_Error( CV_StsBadArg, "the output array of rotation vectors must be 3-channel "
                "1xn or nx1 array or 1-channel nx3 or nx9 array, where n is the number of views" );
    }

    if( tvecs )
    {
        cn = CV_MAT_CN(tvecs->type);
        if( !CV_IS_MAT(tvecs) ||
            (CV_MAT_DEPTH(tvecs->type) != CV_32F && CV_MAT_DEPTH(tvecs->type) != CV_64F) ||
            ((tvecs->rows != nimages || tvecs->cols*cn != 3) &&
            (tvecs->rows != 1 || tvecs->cols != nimages || cn != 3)) )
            CV_Error( CV_StsBadArg, "the output array of translation vectors must be 3-channel "
                "1xn or nx1 array or 1-channel nx3 array, where n is the number of views" );
    }

    if( (CV_MAT_TYPE(cameraMatrix->type) != CV_32FC1 &&
        CV_MAT_TYPE(cameraMatrix->type) != CV_64FC1) ||
        cameraMatrix->rows != 3 || cameraMatrix->cols != 3 )
        CV_Error( CV_StsBadArg,
            "Intrinsic parameters must be 3x3 floating-point matrix" );

    if( (CV_MAT_TYPE(distCoeffs->type) != CV_32FC1 &&
        CV_MAT_TYPE(distCoeffs->type) != CV_64FC1) ||
        (distCoeffs->cols != 1 && distCoeffs->rows != 1) ||
        (distCoeffs->cols*distCoeffs->rows != 4 &&
        distCoeffs->cols*distCoeffs->rows != 5) )
        CV_Error( CV_StsBadArg,
            "Distortion coefficients must be 4x1, 1x4, 5x1 or 1x5 floating-point matrix" );

    for( i = 0; i < nimages; i++ )
    {
        ni = npoints->data.i[i*npstep];
        if( ni < 4 )
        {
            char buf[100];
            sprintf( buf, "The number of points in the view #%d is < 4", i );
            CV_Error( CV_StsOutOfRange, buf );
        }
        maxPoints = MAX( maxPoints, ni );
        total += ni;
    }

    matM = cvCreateMat( 1, total, CV_64FC3 );
    _m = cvCreateMat( 1, total, CV_64FC2 );

    cvConvertPointsHomogeneous( objectPoints, matM );
    cvConvertPointsHomogeneous( imagePoints, _m );

    nparams = NINTRINSIC + nimages*6;
    _Ji = cvCreateMat( maxPoints*2, NINTRINSIC, CV_64FC1 );
    _Je = cvCreateMat( maxPoints*2, 6, CV_64FC1 );
    _err = cvCreateMat( maxPoints*2, 1, CV_64FC1 );
    cvZero( _Ji );

    _k = cvMat( distCoeffs->rows, distCoeffs->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k);
    if( distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 4 )
        flags |= CV_CALIB_FIX_K3;

    // 1. initialize intrinsic parameters & LM solver
    if( flags & CV_CALIB_USE_INTRINSIC_GUESS )
    {
        cvConvert( cameraMatrix, &matA );
        if( A[0] <= 0 || A[4] <= 0 )
            CV_Error( CV_StsOutOfRange, "Focal length (fx and fy) must be positive" );
        if( A[2] < 0 || A[2] >= imageSize.width ||
            A[5] < 0 || A[5] >= imageSize.height )
            CV_Error( CV_StsOutOfRange, "Principal point must be within the image" );
        if( fabs(A[1]) > 1e-5 )
            CV_Error( CV_StsOutOfRange, "Non-zero skew is not supported by the function" );
        if( fabs(A[3]) > 1e-5 || fabs(A[6]) > 1e-5 ||
            fabs(A[7]) > 1e-5 || fabs(A[8]-1) > 1e-5 )
            CV_Error( CV_StsOutOfRange,
                "The intrinsic matrix must have [fx 0 cx; 0 fy cy; 0 0 1] shape" );
        A[1] = A[3] = A[6] = A[7] = 0.;
        A[8] = 1.;

        if( flags & CV_CALIB_FIX_ASPECT_RATIO )
            aspectRatio = A[0]/A[4];
        cvConvert( distCoeffs, &_k );
    }
    else
    {
        CvScalar mean, sdv;
        cvAvgSdv( matM, &mean, &sdv );
        if( fabs(mean.val[2]) > 1e-5 || fabs(sdv.val[2]) > 1e-5 )
            CV_Error( CV_StsBadArg,
            "For non-planar calibration rigs the initial intrinsic matrix must be specified" );
        for( i = 0; i < total; i++ )
            ((CvPoint3D64f*)matM->data.db)[i].z = 0.;

        if( flags & CV_CALIB_FIX_ASPECT_RATIO )
        {
            aspectRatio = cvmGet(cameraMatrix,0,0);
            aspectRatio /= cvmGet(cameraMatrix,1,1);
            if( aspectRatio < 0.01 || aspectRatio > 100 )
                CV_Error( CV_StsOutOfRange,
                    "The specified aspect ratio (=A[0][0]/A[1][1]) is incorrect" );
        }
        cvInitIntrinsicParams2D( matM, _m, npoints, imageSize, &matA, aspectRatio );
    }

    solver.init( nparams, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,DBL_EPSILON) );

    {
    double* param = solver.param->data.db;
    uchar* mask = solver.mask->data.ptr;

    param[0] = A[0]; param[1] = A[4]; param[2] = A[2]; param[3] = A[5];
    param[4] = k[0]; param[5] = k[1]; param[6] = k[2]; param[7] = k[3];
    param[8] = k[4];

    if( flags & CV_CALIB_FIX_FOCAL_LENGTH )
        mask[0] = mask[1] = 0;
    if( flags & CV_CALIB_FIX_PRINCIPAL_POINT )
        mask[2] = mask[3] = 0;
    if( flags & CV_CALIB_ZERO_TANGENT_DIST )
    {
        param[6] = param[7] = 0;
        mask[6] = mask[7] = 0;
    }
    if( flags & CV_CALIB_FIX_K1 )
        mask[4] = 0;
    if( flags & CV_CALIB_FIX_K2 )
        mask[5] = 0;
    if( flags & CV_CALIB_FIX_K3 )
        mask[8] = 0;
    }

    // 2. initialize extrinsic parameters
    for( i = 0, pos = 0; i < nimages; i++, pos += ni )
    {
        CvMat _Mi, _mi, _ri, _ti;
        ni = npoints->data.i[i*npstep];

        cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 );
        cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 );

        cvGetCols( matM, &_Mi, pos, pos + ni );
        cvGetCols( _m, &_mi, pos, pos + ni );

        cvFindExtrinsicCameraParams2( &_Mi, &_mi, &matA, &_k, &_ri, &_ti );
    }

    // 3. run the optimization
    for(;;)
    {
        const CvMat* _param = 0;
        CvMat *_JtJ = 0, *_JtErr = 0;
        double* _errNorm = 0;
        bool proceed = solver.updateAlt( _param, _JtJ, _JtErr, _errNorm );
        double *param = solver.param->data.db, *pparam = solver.prevParam->data.db;

        if( flags & CV_CALIB_FIX_ASPECT_RATIO )
        {
            param[0] = param[1]*aspectRatio;
            pparam[0] = pparam[1]*aspectRatio;
        }

        A[0] = param[0]; A[4] = param[1];
        A[2] = param[2]; A[5] = param[3];
        k[0] = param[4]; k[1] = param[5]; k[2] = param[6];
        k[3] = param[7];
        k[4] = param[8];

        if( !proceed )
            break;
       
        reprojErr = 0;

        for( i = 0, pos = 0; i < nimages; i++, pos += ni )
        {
            CvMat _Mi, _mi, _ri, _ti, _dpdr, _dpdt, _dpdf, _dpdc, _dpdk, _mp, _part;
            ni = npoints->data.i[i*npstep];

            cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 );
            cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 );

            cvGetCols( matM, &_Mi, pos, pos + ni );
            cvGetCols( _m, &_mi, pos, pos + ni );

            _Je->rows = _Ji->rows = _err->rows = ni*2;
            cvGetCols( _Je, &_dpdr, 0, 3 );
            cvGetCols( _Je, &_dpdt, 3, 6 );
            cvGetCols( _Ji, &_dpdf, 0, 2 );
            cvGetCols( _Ji, &_dpdc, 2, 4 );
            cvGetCols( _Ji, &_dpdk, 4, NINTRINSIC );
            cvReshape( _err, &_mp, 2, 1 );

            if( _JtJ || _JtErr )
            {
                cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, &_k, &_mp, &_dpdr, &_dpdt,
                                  (flags & CV_CALIB_FIX_FOCAL_LENGTH) ? 0 : &_dpdf,
                                  (flags & CV_CALIB_FIX_PRINCIPAL_POINT) ? 0 : &_dpdc, &_dpdk,
                                  (flags & CV_CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0);
            }
            else
                cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, &_k, &_mp );

            cvSub( &_mp, &_mi, &_mp );

            if( _JtJ || _JtErr )
            {
                cvGetSubRect( _JtJ, &_part, cvRect(0,0,NINTRINSIC,NINTRINSIC) );
                cvGEMM( _Ji, _Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );

                cvGetSubRect( _JtJ, &_part, cvRect(NINTRINSIC+i*6,NINTRINSIC+i*6,6,6) );
                cvGEMM( _Je, _Je, 1, 0, 0, &_part, CV_GEMM_A_T );

                cvGetSubRect( _JtJ, &_part, cvRect(NINTRINSIC+i*6,0,6,NINTRINSIC) );
                cvGEMM( _Ji, _Je, 1, 0, 0, &_part, CV_GEMM_A_T );

                cvGetRows( _JtErr, &_part, 0, NINTRINSIC );
                cvGEMM( _Ji, _err, 1, &_part, 1, &_part, CV_GEMM_A_T );

                cvGetRows( _JtErr, &_part, NINTRINSIC + i*6, NINTRINSIC + (i+1)*6 );
                cvGEMM( _Je, _err, 1, 0, 0, &_part, CV_GEMM_A_T );
            }

            double errNorm = cvNorm( &_mp, 0, CV_L2 );
            reprojErr += errNorm*errNorm;
        }
        if( _errNorm )
            *_errNorm = reprojErr;
    }

    // 4. store the results
    cvConvert( &matA, cameraMatrix );
    cvConvert( &_k, distCoeffs );

    for( i = 0; i < nimages; i++ )
    {
        CvMat src, dst;
        if( rvecs )
        {
            src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 );
            if( rvecs->rows == nimages && rvecs->cols*CV_MAT_CN(rvecs->type) == 9 )
            {
                dst = cvMat( 3, 3, CV_MAT_DEPTH(rvecs->type),
                    rvecs->data.ptr + rvecs->step*i );
                cvRodrigues2( &src, &matA );
                cvConvert( &matA, &dst );
            }
            else
            {
                dst = cvMat( 3, 1, CV_MAT_DEPTH(rvecs->type), rvecs->rows == 1 ?
                    rvecs->data.ptr + i*CV_ELEM_SIZE(rvecs->type) :
                    rvecs->data.ptr + rvecs->step*i );
                cvConvert( &src, &dst );
            }
        }
        if( tvecs )
        {
            src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 + 3 );
            dst = cvMat( 3, 1, CV_MAT_TYPE(tvecs->type), tvecs->rows == 1 ?
                    tvecs->data.ptr + i*CV_ELEM_SIZE(tvecs->type) :
                    tvecs->data.ptr + tvecs->step*i );
            cvConvert( &src, &dst );
         }
    }
   
    return reprojErr;
}

            // # 6. camera calibration
           
            int numPair = indexI.size(); // number of pairs of corresponding points
            int numView = 1; // number of calibration pattern views
            //                int numPoint = numPair * numView; // total number of points in all views
           
             if( numPair < 4 )
             {
                 cout << "Didn't get enough points" << endl;
                 continue;
             }
                    
            // input to camera calibration
            CvMat* pointI = cvCreateMat( numPair, 2, CV_32FC1 );
            CvMat* pointW = cvCreateMat( numPair, 3, CV_32FC1 );   
            // integer "1*numView" vector
            CvMat* countsP = cvCreateMat( numView, 1, CV_32SC1 );
            // the sum of vector elements must match the size of objectPoints and imagePoints
            //                cvmSet( countsP, 0, 0, numPair );                
           
            cvSet( countsP, cvScalar(numPair) );
           
            cout << "yes? " << endl;
           
            // output from camera calibration
            // intrinsic parameters
            CvMat* matrixCamera = cvCreateMat( 3, 3, CV_32FC1 ); // camera matrix of intrinsic parameter
            CvMat* coeffDistortion = cvCreateMat( 4, 1, CV_32FC1); // distortion coefficients
            // extrinsic parameters
            CvMat* vectorR  = cvCreateMat( 1, 3, CV_32FC1 ); // rotation vector
            CvMat* matrixR  = cvCreateMat( 3, 3, CV_32FC1 ); // rotation matrix
            CvMat* vectorT  = cvCreateMat( 1, 3, CV_32FC1 ); // translation vector
           
           
            for( int n = 0;  n < numPair;  n++ )
            {
                cvmSet( pointI, n, 0, p[indexI[n]].x );
                cvmSet( pointI, n, 1, p[indexI[n]].y );
               
                cvmSet( pointW, n, 0, world[indexW[n]].x );
                cvmSet( pointW, n, 1, world[indexW[n]].y );
                cvmSet( pointW, n, 2, 1.0 );
            }
           
            // find the camera intrinsic and extrinsic parameters
            // cvCalibrateCamera2(<#const CvMat * object_points#>, <#const CvMat * image_points#>, <#const CvMat * point_counts#>, <#CvSize image_size#>, <#CvMat * camera_matrix#>, <#CvMat * distortion_coeffs#>, <#CvMat * rotation_vectors#>, <#CvMat * translation_vectors#>, <#int flags#>)
            cvCalibrateCamera2( pointW, pointI, countsP, cvGetSize(iplInput), matrixCamera, coeffDistortion, vectorR, vectorT /* , flag */ );
           
            cout << endl << "camera matrix " << endl
            << "fx=" << cvmGet(matrixCamera,0, 0) << " " << cvmGet(matrixCamera,0, 1) << " cx=" << cvmGet(matrixCamera,0,2) << endl
            << cvmGet(matrixCamera,1, 0) << " fy=" << cvmGet(matrixCamera,1, 1) << " cy=" << cvmGet(matrixCamera,1,2) << endl
            << cvmGet(matrixCamera,2, 0) << " " << cvmGet(matrixCamera,2, 1) << " " << cvmGet(matrixCamera,2,2) << endl;
           
            cout << endl << "lens distortion " << endl
            << "k1 = " << cvmGet(coeffDistortion,0, 0) << endl
            << "k2 = " << cvmGet(coeffDistortion,1, 0) << endl
            << "p1 = " << cvmGet(coeffDistortion,2, 0) << endl
            << "p2 = " << cvmGet(coeffDistortion,3, 0) << endl;
           
            cout << endl << "rotation vector " << endl
            << cvmGet(vectorR,0,0) << "  " << cvmGet(vectorR,0,1) << "  " << cvmGet(vectorR,0,2) << endl;
           
            // cvRodrigues2(<#const CvMat * src#>, <#CvMat * dst#>, <#CvMat * jacobian#>)
            cvRodrigues2( vectorR, matrixR, 0 );
            cout << endl << "rotation matrix " << endl
            << cvmGet(matrixR,0,0) << "  " << cvmGet(matrixR,0,1) << "  " << cvmGet(matrixR,0,2) << endl
            << cvmGet(matrixR,1,0) << "  " << cvmGet(matrixR,1,1) << "  " << cvmGet(matrixR,1,2) << endl
            << cvmGet(matrixR,2,0) << "  " << cvmGet(matrixR,2,1) << "  " << cvmGet(matrixR,2,2) << endl;
           
            cout << endl << "translation vector " << endl
            << cvmGet(vectorT,0,0) << "  " << cvmGet(vectorT,0,1) << "  " << cvmGet(vectorT,0,2) << endl;
           
           
            // reprojection of world points to the image frame
            CvMat* pointReI = cvCreateMat( numPair, 2, CV_32FC1 );
            // project 3D points on to an image plane
           
            // cvProjectPoints2(<#const CvMat * object_points#>, <#const CvMat * rotation_vector#>, <#const CvMat * translation_vector#>, <#const CvMat * camera_matrix#>, <#const CvMat * distortion_coeffs#>, <#CvMat * image_points#>, <#CvMat * dpdrot#>, <#CvMat * dpdt#>, <#CvMat * dpdf#>, <#CvMat * dpdc#>, <#CvMat * dpddist#>, <#double aspect_ratio#>)
            cvProjectPoints2( pointW, vectorR, vectorT, matrixCamera, coeffDistortion, pointReI );
           
            for( int n = 0; n < numPair; n++ )
            {
                float rex = cvmGet( pointReI, n, 0 ); // x-coordinate of the image frame
                float rey = cvmGet( pointReI, n, 1 ); // y-coordinate of the image frame
               
                CvPoint reproj = cvPoint( cvRound(rex), cvRound(rey) );
               
                cvCircle( iplInput, reproj, 3, CV_RGB(100,200,100), 2 );
            }