cvfundam.cpp

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

                int new_max_samples;

                // update the current best fundamental matrix and "goodness" flags
                if( mask )
                    memcpy( mask, curr_mask, count );
                memcpy( fmatrix, f, 9*sizeof(f[0]));
                best_good_count = good_count;

                // try to update (decrease) <max_samples>
                ep = (double)(count - good_count)/count;
                lp = log(1. - p);
                lep = log(1. - pow(ep,7.));
                if( lp < lep || lep >= 0 )
                    break;
                else
                {
                    new_max_samples = cvRound(lp/lep);
                    max_samples = MIN( new_max_samples, max_samples );
                }
            }
        }
    }

    if( best_good_count < 7 )
        EXIT;

    result = 1;

    // optionally, use 8-point algorithm to compute fundamental matrix using only the in-liers
    if( best_good_count >= 8 && use_8point )
        result = icvFMatrix_8Point( m0, m1, mask, count, fmatrix );

    __END__;

    cvFree( &temp_mask );
    cvFree( &curr_mask );

    return result;
}


/***************************** Least Median of Squares algorithm ************************/

static CV_IMPLEMENT_QSORT( icvSortDistances, int, CV_LT )

/* the algorithm is quite similar to RANSAC, but here we choose the matrix that gives
   the least median of d(m0[i], F'*m1[i])^2 + d(m1[i], F*m0[i])^2 (0<=i<count),
   instead of choosing the matrix that gives the least number of outliers (as it is done in RANSAC) */
static int
icvFMatrix_LMedS( const CvPoint2D64f* m0, const CvPoint2D64f* m1,
                  uchar* mask, int count, double* fmatrix,
                  double threshold, double p,
                  unsigned rng_seed, int use_8point )
{
    int result = 0;

    const int max_random_iters = 1000;
    const int sample_size = 7;

    float* dist = 0;
    uchar* curr_mask = 0;
    uchar* temp_mask = 0;

    CV_FUNCNAME( "icvFMatrix_LMedS" );

    __BEGIN__;

    double ff[9*3];
    CvRNG rng = cvRNG(rng_seed);
    int i, j, k, sample_count, max_samples = 500;
    double least_median = DBL_MAX, median;
    int best_good_count = 0;

    assert( m0 && m1 && fmatrix && 0 < p && p < 1 && threshold > 0 );

    threshold *= threshold;

    CV_CALL( curr_mask = (uchar*)cvAlloc( count ));
    CV_CALL( dist = (float*)cvAlloc( count*sizeof(dist[0]) ));

    if( !mask && use_8point )
    {
        CV_CALL( temp_mask = (uchar*)cvAlloc( count ));
        mask = temp_mask;
    }

    // find the best fundamental matrix (giving the least backprojection error)
    // by picking at most <max_samples> 7-tuples of corresponding points
    // <max_samples> may be updated (decreased) within the loop based on statistics of outliers
    for( sample_count = 0; sample_count < max_samples; sample_count++ )
    {
        int idx[sample_size], n;
        CvPoint2D64f ms0[sample_size], ms1[sample_size];

        // choose random <sample_size> (=7) points
        for( i = 0; i < sample_size; i++ )
        {
            for( k = 0; k < max_random_iters; k++ )
            {
                idx[i] = cvRandInt(&rng) % count;
                for( j = 0; j < i; j++ )
                    if( idx[j] == idx[i] )
                        break;
                if( j == i )
                {
                    ms0[i] = m0[idx[i]];
                    ms1[i] = m1[idx[i]];
                    break;
                }
            }
            if( k >= max_random_iters )
                break;
        }

        if( i < sample_size )
            continue;

        // find 1 or 3 fundamental matrix out of the 7 point correspondences
        n = icvFMatrix_7Point( ms0, ms1, ff );

        if( n < 1 || n > 3 )
            continue;

        // for each matrix calculate the backprojection error
        // (distance to the corresponding epipolar lines) for each point and thus find
        // the number of in-liers.
        for( k = 0; k < n; k++ )
        {
            const double* f = ff + k*9;
            int good_count = 0;

            for( i = 0; i < count; i++ )
            {
                double d0, d1, s;

                double a = f[0]*m0[i].x + f[1]*m0[i].y + f[2];
                double b = f[3]*m0[i].x + f[4]*m0[i].y + f[5];
                double c = f[6]*m0[i].x + f[7]*m0[i].y + f[8];

                s = 1./(a*a + b*b);
                d1 = m1[i].x*a + m1[i].y*b + c;
                d1 = s*d1*d1;

                a = f[0]*m1[i].x + f[3]*m1[i].y + f[6];
                b = f[1]*m1[i].x + f[4]*m1[i].y + f[7];
                c = f[2]*m1[i].x + f[5]*m1[i].y + f[8];

                s = 1./(a*a + b*b);
                d0 = m0[i].x*a + m0[i].y*b + c;
                d0 = s*d0*d0;

                curr_mask[i] = d1 < threshold && d0 < threshold;
                good_count += curr_mask[i];

                dist[i] = (float)(d0 + d1);
            }

            icvSortDistances( (int*)dist, count, 0 );
            median = (double)dist[count/2];

            if( median < least_median )
            {
                double ep, lp, lep;
                int new_max_samples;

                // update the current best fundamental matrix and "goodness" flags
                if( mask )
                    memcpy( mask, curr_mask, count );
                memcpy( fmatrix, f, 9*sizeof(f[0]));
                least_median = median;
                best_good_count = good_count;

                // try to update (decrease) <max_samples>
                ep = (double)(count - good_count)/count;
                lp = log(1. - p);
                lep = log(1. - pow(ep,7.));
                if( lp < lep || lep >= 0 )
                    break;
                else
                {
                    new_max_samples = cvRound(lp/lep);
                    max_samples = MIN( new_max_samples, max_samples );
                }
            }
        }
    }

    if( best_good_count < 7 )
        EXIT;

    result = 1;

    // optionally, use 8-point algorithm to compute fundamental matrix using only the in-liers
    if( best_good_count >= 8 && use_8point )
        result = icvFMatrix_8Point( m0, m1, mask, count, fmatrix );

    __END__;

    cvFree( &temp_mask );
    cvFree( &curr_mask );
    cvFree( &dist );

    return result;
}


CV_IMPL int
cvFindFundamentalMat( const CvMat* points0, const CvMat* points1,
                      CvMat* fmatrix, int method,
                      double param1, double param2, CvMat* status )
{
    const unsigned rng_seed = 0xffffffff;
    int result = 0;
    int pt_alloc_flag[2] = { 0, 0 };
    int i, k;
    CvPoint2D64f* pt[2] = { 0, 0 };
    CvMat* _status = 0;

    CV_FUNCNAME( "cvFindFundamentalMat" );

    __BEGIN__;

    int count, dims;
    int depth, cn;
    uchar* status_data = 0;
    double fmatrix_data0[9*3];
    double* fmatrix_data = 0;

    if( !CV_IS_MAT(points0) )
        CV_ERROR( !points0 ? CV_StsNullPtr : CV_StsBadArg, "points0 is not a valid matrix" );

    if( !CV_IS_MAT(points1) )
        CV_ERROR( !points1 ? CV_StsNullPtr : CV_StsBadArg, "points1 is not a valid matrix" );

    if( !CV_ARE_TYPES_EQ(points0, points1) )
        CV_ERROR( CV_StsUnmatchedFormats, "The matrices of points should have the same data type" );

    if( !CV_ARE_SIZES_EQ(points0, points1) )
        CV_ERROR( CV_StsUnmatchedSizes, "The matrices of points should have the same size" );

    depth = CV_MAT_DEPTH(points0->type);
    cn = CV_MAT_CN(points0->type);
    if( depth != CV_32S && depth != CV_32F && depth != CV_64F || cn != 1 && cn != 2 && cn != 3 )
        CV_ERROR( CV_StsUnsupportedFormat, "The format of point matrices is unsupported" );

    if( points0->rows > points0->cols )
    {
        dims = cn*points0->cols;
        count = points0->rows;
    }
    else
    {
        if( points0->rows > 1 && cn > 1 || points0->rows == 1 && cn == 1 )
            CV_ERROR( CV_StsBadSize, "The point matrices do not have a proper layout (2xn, 3xn, nx2 or nx3)" );
        dims = cn * points0->rows;
        count = points0->cols;
    }

    if( dims != 2 && dims != 3 )
        CV_ERROR( CV_StsOutOfRange, "The dimensionality of points must be 2 or 3" );

    if( method == CV_FM_7POINT && count != 7 ||
        method != CV_FM_7POINT && count < 7 + (method == CV_FM_8POINT) )
        CV_ERROR( CV_StsOutOfRange,
        "The number of points must be 7 for 7-point algorithm, "
        ">=8 for 8-point algorithm and >=7 for other algorithms" );

    if( !CV_IS_MAT(fmatrix) )
        CV_ERROR( !fmatrix ? CV_StsNullPtr : CV_StsBadArg, "fmatrix is not a valid matrix" );

    if( CV_MAT_TYPE(fmatrix->type) != CV_32FC1 && CV_MAT_TYPE(fmatrix->type) != CV_64FC1 )
        CV_ERROR( CV_StsUnsupportedFormat, "fundamental matrix must have 32fC1 or 64fC1 type" );

    if( fmatrix->cols != 3 || (fmatrix->rows != 3 && (method != CV_FM_7POINT || fmatrix->rows != 9)))
        CV_ERROR( CV_StsBadSize, "fundamental matrix must be 3x3 or 3x9 (for 7-point method only)" );

    fmatrix_data = fmatrix->data.db;
    if( !CV_IS_MAT_CONT(fmatrix->type) || CV_MAT_TYPE(fmatrix->type) != CV_64FC1 ||
        method == CV_FM_7POINT && fmatrix->rows != 9 )
        fmatrix_data = fmatrix_data0;

    if( status )
    {
        if( !CV_IS_MAT(status) )
            CV_ERROR( CV_StsBadArg, "The output status is not a valid matrix" );

        if( status->cols != 1 && status->rows != 1 || status->cols + status->rows - 1 != count )
            CV_ERROR( CV_StsUnmatchedSizes,
            "The status matrix must have the same size as the point matrices" );

        if( method == CV_FM_7POINT || method == CV_FM_8POINT )
            cvSet( status, cvScalarAll(1.) );
        else
        {
            status_data = status->data.ptr;
            if( !CV_IS_MAT_CONT(status->type) || !CV_IS_MASK_ARR(status) )
            {
                CV_CALL( _status = cvCreateMat( status->rows, status->cols, CV_8UC1 ));
                status_data = _status->data.ptr;
            }
        }
    }

    for( k = 0; k < 2; k++ )
    {
        const CvMat* spt = k == 0 ? points0 : points1;
        CvPoint2D64f* dpt = pt[k] = (CvPoint2D64f*)spt->data.db;
        int plane_stride, stride, elem_size;

        if( CV_IS_MAT_CONT(spt->type) && CV_MAT_DEPTH(spt->type) == CV_64F &&
            dims == 2 && (spt->rows == 1 || spt->rows == count) )
            continue;

        elem_size = CV_ELEM_SIZE(depth);

        if( spt->rows == dims )
        {
            plane_stride = spt->step / elem_size;
            stride = 1;
        }
        else
        {
            plane_stride = 1;
            stride = spt->rows == 1 ? dims : spt->step / elem_size;
        }

        CV_CALL( dpt = pt[k] = (CvPoint2D64f*)cvAlloc( count*sizeof(dpt[0]) ));
        pt_alloc_flag[k] = 1;

        if( depth == CV_32F )
        {
            const float* xp = spt->data.fl;
            const float* yp = xp + plane_stride;
            const float* zp = dims == 3 ? yp + plane_stride : 0;

            for( i = 0; i < count; i++ )
            {
                double x = *xp, y = *yp;
                xp += stride;
                yp += stride;
                if( dims == 3 )
                {
                    double z = *zp;
                    zp += stride;
                    z = z ? 1./z : 1.;
                    x *= z;
                    y *= z;
                }
                dpt[i].x = x;
                dpt[i].y = y;
            }
        }
        else
        {
            const double* xp = spt->data.db;
            const double* yp = xp + plane_stride;
            const double* zp = dims == 3 ? yp + plane_stride : 0;

            for( i = 0; i < count; i++ )
            {
                double x = *xp, y = *yp;
                xp += stride;
                yp += stride;
                if( dims == 3 )
                {
                    double z = *zp;
                    zp += stride;
                    z = z ? 1./z : 1.;
                    x *= z;
                    y *= z;
                }
                dpt[i].x = x;
                dpt[i].y = y;
            }
        }
    }

    if( method == CV_FM_7POINT )
        result = icvFMatrix_7Point( pt[0], pt[1], fmatrix_data );
    else if( method == CV_FM_8POINT )
        result = icvFMatrix_8Point( pt[0], pt[1], 0, count, fmatrix_data );
    else
    {
        if( param1 < 0 )
            CV_ERROR( CV_StsOutOfRange, "param1 (threshold) must be > 0" );

        if( param2 < 0 || param2 > 1 )
            CV_ERROR( CV_StsOutOfRange, "param2 (confidence level) must be between 0 and 1" );

        if( param2 < DBL_EPSILON || param2 > 1 - DBL_EPSILON )
            param2 = 0.99;

        if( method < CV_FM_RANSAC_ONLY )
            result = icvFMatrix_LMedS( pt[0], pt[1], status_data, count, fmatrix_data,
                                       param1, param2, rng_seed, method & CV_FM_8POINT );
        else
            result = icvFMatrix_RANSAC( pt[0], pt[1], status_data, count, fmatrix_data,
                                        param1, param2, rng_seed, method & CV_FM_8POINT );
    }

    if( result && fmatrix->data.db != fmatrix_data )
    {
        CvMat hdr;
        cvZero( fmatrix );
        hdr = cvMat( MIN(fmatrix->rows, result*3), fmatrix->cols, CV_64F, fmatrix_data );
        cvConvert( &hdr, fmatrix );
    }

    if( status && status_data && status->data.ptr != status_data )
        cvConvert( _status, status );

    __END__;

    cvReleaseMat( &_status );
    for( k = 0; k < 2; k++ )
        if( pt_alloc_flag[k] )
            cvFree( &pt[k] );