cvcalibration.cpp

opencv库在TI DM6437上的移植,目前包括两个库cv.lib和cxcore.lib的工程

        double h[3], v[3], d1[3], d2[3];
        double n[4] = {0,0,0,0};
        CvMat _m, _M;
        cvGetCols( obj_points, &_M, pos, pos + count );
        cvGetCols( img_points, &_m, pos, pos + count );
        pos += count;

        CV_CALL( cvFindHomography( &_M, &_m, &_H ));
        
        H[0] -= H[6]*a[2]; H[1] -= H[7]*a[2]; H[2] -= H[8]*a[2];
        H[3] -= H[6]*a[5]; H[4] -= H[7]*a[5]; H[5] -= H[8]*a[5];

        for( j = 0; j < 3; j++ )
        {
            double t0 = H[j*3], t1 = H[j*3+1];
            h[j] = t0; v[j] = t1;
            d1[j] = (t0 + t1)*0.5;
            d2[j] = (t0 - t1)*0.5;
            n[0] += t0*t0; n[1] += t1*t1;
            n[2] += d1[j]*d1[j]; n[3] += d2[j]*d2[j];
        }

        for( j = 0; j < 4; j++ )
            n[j] = 1./sqrt(n[j]);

        for( j = 0; j < 3; j++ )
        {
            h[j] *= n[0]; v[j] *= n[1];
            d1[j] *= n[2]; d2[j] *= n[3];
        }

        Ap[0] = h[0]*v[0]; Ap[1] = h[1]*v[1];
        Ap[2] = d1[0]*d2[0]; Ap[3] = d1[1]*d2[1];
        bp[0] = -h[2]*v[2]; bp[1] = -d1[2]*d2[2];
    }

    // while it is not about gradient descent search,
    // the formula is the same: f = inv(At*A)*At*b
    icvGaussNewton( _A, _b, &_f, &_AtA, &_Atb, &_AtAW, &_AtAV );
    a[0] = sqrt(fabs(1./f[0]));
    a[4] = sqrt(fabs(1./f[1]));
    if( aspect_ratio != 0 )
    {
        double tf = (a[0] + a[4])/(aspect_ratio + 1.);
        a[0] = aspect_ratio*tf;
        a[4] = tf;
    }

    cvConvert( &_a, intrinsic_matrix );

    __END__;

    cvReleaseMat( &_A );
    cvReleaseMat( &_b );
}


/* finds intrinsic and extrinsic camera parameters
   from a few views of known calibration pattern */
CV_IMPL void
cvCalibrateCamera2( const CvMat* obj_points,
                    const CvMat* img_points,
                    const CvMat* point_counts,
                    CvSize image_size,
                    CvMat* A, CvMat* dist_coeffs,
                    CvMat* r_vecs, CvMat* t_vecs,
                    int flags )
{
    double alpha_smooth = 0.4;
    
    CvMat *counts = 0, *_M = 0, *_m = 0;
    CvMat *_Ji = 0, *_Je = 0, *_JtJ = 0, *_JtErr = 0, *_JtJW = 0, *_JtJV = 0;
    CvMat *_param = 0, *_param_innov = 0, *_err = 0;
    
    CV_FUNCNAME( "cvCalibrateCamera2" );

    __BEGIN__;

    double a[9];
    CvMat _a = cvMat( 3, 3, CV_64F, a ), _k;
    CvMat _Mi, _mi, _ri, _ti, _part;
    CvMat _dpdr, _dpdt, _dpdf, _dpdc, _dpdk;
    CvMat _sr_part = cvMat( 1, 3, CV_64F ), _st_part = cvMat( 1, 3, CV_64F ), _r_part, _t_part;
    int i, j, pos, iter, img_count, count = 0, max_count = 0, total = 0, param_count;
    int r_depth = 0, t_depth = 0, r_step = 0, t_step = 0, cn, dims;
    int output_r_matrices = 0;
    double aspect_ratio = 0.;

    if( !CV_IS_MAT(obj_points) || !CV_IS_MAT(img_points) ||
        !CV_IS_MAT(point_counts) || !CV_IS_MAT(A) || !CV_IS_MAT(dist_coeffs) )
        CV_ERROR( CV_StsBadArg, "One of required vector arguments is not a valid matrix" );

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

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

    CV_CALL( counts = cvCreateMat( point_counts->rows, point_counts->width, CV_32SC1 ));
    cvCopy( point_counts, counts );

    img_count = counts->rows + counts->cols - 1;

    if( r_vecs )
    {
        r_depth = CV_MAT_DEPTH(r_vecs->type);
        r_step = r_vecs->rows == 1 ? 3*CV_ELEM_SIZE(r_depth) : r_vecs->step;
        cn = CV_MAT_CN(r_vecs->type);
        if( !CV_IS_MAT(r_vecs) || r_depth != CV_32F && r_depth != CV_64F ||
            (r_vecs->rows != img_count || r_vecs->cols*cn != 3 && r_vecs->cols*cn != 9) &&
            (r_vecs->rows != 1 || r_vecs->cols != img_count || 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" );
        output_r_matrices = r_vecs->rows == img_count && r_vecs->cols*cn == 9;
    }

    if( t_vecs )
    {
        t_depth = CV_MAT_DEPTH(t_vecs->type);
        t_step = t_vecs->rows == 1 ? 3*CV_ELEM_SIZE(t_depth) : t_vecs->step;
        cn = CV_MAT_CN(t_vecs->type);
        if( !CV_IS_MAT(t_vecs) || t_depth != CV_32F && t_depth != CV_64F ||
            (t_vecs->rows != img_count || t_vecs->cols*cn != 3) &&
            (t_vecs->rows != 1 || t_vecs->cols != img_count || 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(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1 ||
        A->rows != 3 || A->cols != 3 )
        CV_ERROR( CV_StsBadArg,
            "Intrinsic parameters must be 3x3 floating-point matrix" );

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

    for( i = 0; i < img_count; i++ )
    {
        int temp_count = counts->data.i[i];
        if( temp_count < 4 )
        {
            char buf[100];
            sprintf( buf, "The number of points in the view #%d is <4", i );
            CV_ERROR( CV_StsOutOfRange, buf );
        }
        max_count = MAX( max_count, temp_count );
        total += temp_count;
    }

    dims = CV_MAT_CN(obj_points->type)*(obj_points->rows == 1 ? 1 : obj_points->cols);

    if( CV_MAT_DEPTH(obj_points->type) != CV_32F &&
        CV_MAT_DEPTH(obj_points->type) != CV_64F ||
        (obj_points->rows != total || dims != 3 && dims != 2) &&
        (obj_points->rows != 1 || obj_points->cols != total || (dims != 3 && dims != 2)) )
        CV_ERROR( CV_StsBadArg, "Object points must be 1xn or nx1, 2- or 3-channel matrix, "
                                "or nx3 or nx2 single-channel matrix" );

    cn = CV_MAT_CN(img_points->type);
    if( CV_MAT_DEPTH(img_points->type) != CV_32F &&
        CV_MAT_DEPTH(img_points->type) != CV_64F ||
        (img_points->rows != total || img_points->cols*cn != 2) &&
        (img_points->rows != 1 || img_points->cols != total || cn != 2) )
        CV_ERROR( CV_StsBadArg, "Image points must be 1xn or nx1, 2-channel matrix, "
                                "or nx2 single-channel matrix" );

    CV_CALL( _M = cvCreateMat( 1, total, CV_64FC3 ));
    CV_CALL( _m = cvCreateMat( 1, total, CV_64FC2 ));

    CV_CALL( cvConvertPointsHomogenious( obj_points, _M ));
    CV_CALL( cvConvertPointsHomogenious( img_points, _m ));

    param_count = 8 + img_count*6;
    CV_CALL( _param = cvCreateMat( param_count, 1, CV_64FC1 ));
    CV_CALL( _param_innov = cvCreateMat( param_count, 1, CV_64FC1 ));
    CV_CALL( _JtJ = cvCreateMat( param_count, param_count, CV_64FC1 ));
    CV_CALL( _JtErr = cvCreateMat( param_count, 1, CV_64FC1 ));
    CV_CALL( _JtJW = cvCreateMat( param_count, 1, CV_64FC1 ));
    CV_CALL( _JtJV = cvCreateMat( param_count, param_count, CV_64FC1 ));
    CV_CALL( _Ji = cvCreateMat( max_count*2, 8, CV_64FC1 ));
    CV_CALL( _Je = cvCreateMat( max_count*2, 6, CV_64FC1 ));
    CV_CALL( _err = cvCreateMat( max_count*2, 1, CV_64FC1 ));
    
    cvGetCols( _Je, &_dpdr, 0, 3 );
    cvGetCols( _Je, &_dpdt, 3, 6 );
    cvGetCols( _Ji, &_dpdf, 0, 2 );
    cvGetCols( _Ji, &_dpdc, 2, 4 );
    cvGetCols( _Ji, &_dpdk, 4, flags & CV_CALIB_ZERO_TANGENT_DIST ? 6 : 8 );
    cvZero( _Ji );

    // 1. initialize intrinsic parameters
    if( flags & CV_CALIB_USE_INTRINSIC_GUESS )
    {
        cvConvert( A, &_a );
        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] >= image_size.width ||
            a[5] < 0 || a[5] >= image_size.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 )
            aspect_ratio = a[0]/a[4];
    }
    else
    {
        if( dims == 3 )
        {
            CvScalar mean, sdv;
            cvAvgSdv( _M, &mean, &sdv );
            if( fabs(mean.val[2]) > 1e-5 && fabs(mean.val[2] - 1) > 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*)(_M->data.db + i*3))->z = 0.;

        if( flags & CV_CALIB_FIX_ASPECT_RATIO )
        {
            aspect_ratio = cvmGet(A,0,0);
            aspect_ratio /= cvmGet(A,1,1);
            if( aspect_ratio < 0.01 || aspect_ratio > 100 )
                CV_ERROR( CV_StsOutOfRange,
                    "The specified aspect ratio (=a(0,0)/a(1,1)) is incorrect" );
        }
        icvInitIntrinsicParams2D( _M, _m, counts, image_size, &_a, aspect_ratio );
    }

    _k = cvMat( dist_coeffs->rows, dist_coeffs->cols, CV_64FC1, _param->data.db + 4 );
    cvZero( &_k );

    // 2. initialize extrinsic parameters
    for( i = 0, pos = 0; i < img_count; i++, pos += count )
    {
        count = counts->data.i[i];
        _ri = cvMat( 1, 3, CV_64FC1, _param->data.db + 8 + i*6 );
        _ti = cvMat( 1, 3, CV_64FC1, _param->data.db + 8 + i*6 + 3 );

        cvGetCols( _M, &_Mi, pos, pos + count );
        cvGetCols( _m, &_mi, pos, pos + count );
        cvFindExtrinsicCameraParams2( &_Mi, &_mi, &_a, &_k, &_ri, &_ti );
    }

    _param->data.db[0] = a[0];
    _param->data.db[1] = a[4];
    _param->data.db[2] = a[2];
    _param->data.db[3] = a[5];

    // 3. run the optimization
    for( iter = 0; iter < 30; iter++ )
    {
        double* jj = _JtJ->data.db;
        double change;

        for( i = 0, pos = 0; i < img_count; i++, pos += count )
        {
            count = counts->data.i[i];
            _ri = cvMat( 1, 3, CV_64FC1, _param->data.db + 8 + i*6);
            _ti = cvMat( 1, 3, CV_64FC1, _param->data.db + 8 + i*6 + 3);

            cvGetCols( _M, &_Mi, pos, pos + count );
            _mi = cvMat( count*2, 1, CV_64FC1, _m->data.db + pos*2 );

            _dpdr.rows = _dpdt.rows = _dpdf.rows = _dpdc.rows = _dpdk.rows = count*2;

            _err->cols = 1;
            _err->rows = count*2;
            cvReshape( _err, _err, 2, count );
            cvProjectPoints2( &_Mi, &_ri, &_ti, &_a, &_k, _err, &_dpdr, &_dpdt, &_dpdf,
                              flags & CV_CALIB_FIX_PRINCIPAL_POINT ? 0 : &_dpdc, &_dpdk );

            // alter dpdf in case if only one of the focal
            // parameters (fy) is independent variable
            if( flags & CV_CALIB_FIX_ASPECT_RATIO )
                for( j = 0; j < count; j++ )
                {
                    double* dpdf_j = (double*)(_dpdf.data.ptr + j*_dpdf.step*2);
                    dpdf_j[1] = dpdf_j[0]*aspect_ratio;
                    dpdf_j[0] = 0.;
                }

            cvReshape( _err, _err, 1, count*2 );
            cvSub( &_mi, _err, _err );
            
            _Je->rows = _Ji->rows = count*2;
            
            cvGetSubRect( _JtJ, &_part, cvRect(0,0,8,8) );
            cvGEMM( _Ji, _Ji, 1, &_part, i > 0, &_part, CV_GEMM_A_T );

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

            cvGetRows( _JtErr, &_part, 0, 8 );
            cvGEMM( _Ji, _err, 1, &_part, i > 0, &_part, CV_GEMM_A_T );

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

        // make the matrix JtJ exactly symmetrical and add missing zeros
        for( i = 0; i < param_count; i++ )
        {
            int mj = i < 8 ? param_count : ((i - 8)/6)*6 + 14;
            for( j = i+1; j < mj; j++ )
                jj[j*param_count + i] = jj[i*param_count + j];
            for( ; j < param_count; j++ )
                jj[j*param_count + i] = jj[i*param_count + j] = 0;
        }

        cvSVD( _JtJ, _JtJW, 0, _JtJV, CV_SVD_MODIFY_A + CV_SVD_V_T );
        cvSVBkSb( _JtJW, _JtJV, _JtJV, _JtErr, _param_innov, CV_SVD_U_T + CV_SVD_V_T );

        cvScale( _param_innov, _param_innov, 1. - pow(1. - alpha_smooth, iter + 1.) );
        cvGetRows( _param_innov, &_part, 0, 4 );
        change = cvNorm( &_part );
        cvGetRows( _param, &_part, 0, 4 );
        change /= cvNorm( &_part );

        if( flags & CV_CALIB_FIX_PRINCIPAL_POINT )
            _param_innov->data.db[2] = _param_innov->data.db[3] = 0.;

        if( flags & CV_CALIB_ZERO_TANGENT_DIST )
            _param_innov->data.db[6] = _param_innov->data.db[7] = 0.;

        cvAdd( _param, _param_innov, _param );

        if( flags & CV_CALIB_FIX_ASPECT_RATIO )
            _param->data.db[0] = _param->data.db[1]*aspect_ratio;
        
        a[0] = _param->data.db[0];
        a[4] = _param->data.db[1];
        a[2] = _param->data.db[2];
        a[5] = _param->data.db[3];

        if( change < FLT_EPSILON )
            break;
    }

    cvConvert( &_a, A );
    cvConvert( &_k, dist_coeffs );

    _r_part = cvMat( output_r_matrices ? 3 : 1, 3, r_depth );
    _t_part = cvMat( 1, 3, t_depth );
    for( i = 0; i < img_count; i++ )
    {
        if( r_vecs )
        {
            _sr_part.data.db = _param->data.db + 8 + i*6;
            _r_part.data.ptr = r_vecs->data.ptr + i*r_step;
            if( !output_r_matrices )
                cvConvert( &_sr_part, &_r_part );
            else
            {
                cvRodrigues2( &_sr_part, &_a );
                cvConvert( &_a, &_r_part );
            }
        }
        if( t_vecs )
        {
            _st_part.data.db = _param->data.db + 8 + i*6 + 3;
            _t_part.data.ptr = t_vecs->data.ptr + i*t_step;
            cvConvert( &_st_part, &_t_part );
        }
    }

    __END__;

    cvReleaseMat( &counts );
    cvReleaseMat( &_M );
    cvReleaseMat( &_m );
    cvReleaseMat( &_param );
    cvReleaseMat( &_param_innov );
    cvReleaseMat( &_JtJ );
    cvReleaseMat( &_JtErr );
    cvReleaseMat( &_JtJW );
    cvReleaseMat( &_JtJV );
    cvReleaseMat( &_Ji );
    cvReleaseMat( &_Je );
    cvReleaseMat( &_err );
}

/* End of file. */