cvfundam.cpp

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

/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "_cv.h"

/* Evaluation of Fundamental Matrix from point correspondences.
   The original code has been written by Valery Mosyagin */

/* The algorithms (except for RANSAC) and the notation have been taken from
   Zhengyou Zhang's research report
   "Determining the Epipolar Geometry and its Uncertainty: A Review"
   that can be found at http://www-sop.inria.fr/robotvis/personnel/zzhang/zzhang-eng.html */

/************************************** 7-point algorithm *******************************/
static int
icvFMatrix_7Point( const CvPoint2D64f* m0, const CvPoint2D64f* m1, double* fmatrix )
{
    double a[7*9], w[7], v[9*9], c[4], r[3];
    double* f1, *f2;
    double t0, t1, t2;
    CvMat A = cvMat( 7, 9, CV_64F, a );
    CvMat V = cvMat( 9, 9, CV_64F, v );
    CvMat W = cvMat( 7, 1, CV_64F, w );
    CvMat coeffs = cvMat( 1, 4, CV_64F, c );
    CvMat roots = cvMat( 1, 3, CV_64F, r );
    int i, k, n;

    assert( m0 && m1 && fmatrix );

    // form a linear system: i-th row of A(=a) represents
    // the equation: (m1[i], 1)'*F*(m0[i], 1) = 0
    for( i = 0; i < 7; i++ )
    {
        double x0 = m0[i].x, y0 = m0[i].y;
        double x1 = m1[i].x, y1 = m1[i].y;

        a[i*9+0] = x1*x0;
        a[i*9+1] = x1*y0;
        a[i*9+2] = x1;
        a[i*9+3] = y1*x0;
        a[i*9+4] = y1*y0;
        a[i*9+5] = y1;
        a[i*9+6] = x0;
        a[i*9+7] = y0;
        a[i*9+8] = 1;
    }

    // A*(f11 f12 ... f33)' = 0 is singular (7 equations for 9 variables), so
    // the solution is linear subspace of dimensionality 2.
    // => use the last two singular vectors as a basis of the space
    // (according to SVD properties)
    cvSVD( &A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );
    f1 = v + 7*9;
    f2 = v + 8*9;

    // f1, f2 is a basis => lambda*f1 + mu*f2 is an arbitrary f. matrix.
    // as it is determined up to a scale, normalize lambda & mu (lambda + mu = 1),
    // so f ~ lambda*f1 + (1 - lambda)*f2.
    // use the additional constraint det(f) = det(lambda*f1 + (1-lambda)*f2) to find lambda.
    // it will be a cubic equation.
    // find c - polynomial coefficients.
    for( i = 0; i < 9; i++ )
        f1[i] -= f2[i];

    t0 = f2[4]*f2[8] - f2[5]*f2[7];
    t1 = f2[3]*f2[8] - f2[5]*f2[6];
    t2 = f2[3]*f2[7] - f2[4]*f2[6];

    c[3] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2;

    c[2] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2 -
           f1[3]*(f2[1]*f2[8] - f2[2]*f2[7]) +
           f1[4]*(f2[0]*f2[8] - f2[2]*f2[6]) -
           f1[5]*(f2[0]*f2[7] - f2[1]*f2[6]) +
           f1[6]*(f2[1]*f2[5] - f2[2]*f2[4]) -
           f1[7]*(f2[0]*f2[5] - f2[2]*f2[3]) +
           f1[8]*(f2[0]*f2[4] - f2[1]*f2[3]);

    t0 = f1[4]*f1[8] - f1[5]*f1[7];
    t1 = f1[3]*f1[8] - f1[5]*f1[6];
    t2 = f1[3]*f1[7] - f1[4]*f1[6];

    c[1] = f2[0]*t0 - f2[1]*t1 + f2[2]*t2 -
           f2[3]*(f1[1]*f1[8] - f1[2]*f1[7]) +
           f2[4]*(f1[0]*f1[8] - f1[2]*f1[6]) -
           f2[5]*(f1[0]*f1[7] - f1[1]*f1[6]) +
           f2[6]*(f1[1]*f1[5] - f1[2]*f1[4]) -
           f2[7]*(f1[0]*f1[5] - f1[2]*f1[3]) +
           f2[8]*(f1[0]*f1[4] - f1[1]*f1[3]);

    c[0] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2;

    // solve the cubic equation; there can be 1 to 3 roots ...
    n = cvSolveCubic( &coeffs, &roots );

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

    for( k = 0; k < n; k++, fmatrix += 9 )
    {
        // for each root form the fundamental matrix
        double lambda = r[k], mu = 1.;
        double s = f1[8]*r[k] + f2[8];

        // normalize each matrix, so that F(3,3) (~fmatrix[8]) == 1
        if( fabs(s) > DBL_EPSILON )
        {
            mu = 1./s;
            lambda *= mu;
            fmatrix[8] = 1.;
        }
        else
            fmatrix[8] = 0.;

        for( i = 0; i < 8; i++ )
            fmatrix[i] = f1[i]*lambda + f2[i]*mu;
    }

    return n;
}


/*************************************** 8-point algorithm ******************************/
static int
icvFMatrix_8Point( const CvPoint2D64f* m0, const CvPoint2D64f* m1,
                   const uchar* mask, int count, double* fmatrix )
{
    int result = 0;
    CvMat* A = 0;

    double w[9], v[9*9];
    CvMat W = cvMat( 1, 9, CV_64F, w);
    CvMat V = cvMat( 9, 9, CV_64F, v);
    CvMat U, F0, TF;

    int i, good_count = 0;
    CvPoint2D64f m0c = {0,0}, m1c = {0,0};
    double t, scale0 = 0, scale1 = 0;
    double* a;
    int a_step;

    CV_FUNCNAME( "icvFMatrix_8Point" );

    __BEGIN__;

    assert( m0 && m1 && fmatrix );

    // compute centers and average distances for each of the two point sets
    for( i = 0; i < count; i++ )
        if( !mask || mask[i] )
        {
            double x = m0[i].x, y = m0[i].y;
            m0c.x += x; m0c.y += y;

            x = m1[i].x, y = m1[i].y;
            m1c.x += x; m1c.y += y;
            good_count++;
        }

    if( good_count < 8 )
        EXIT;

    // calculate the normalizing transformations for each of the point sets:
    // after the transformation each set will have the mass center at the coordinate origin
    // and the average distance from the origin will be ~sqrt(2).
    t = 1./good_count;
    m0c.x *= t; m0c.y *= t;
    m1c.x *= t; m1c.y *= t;

    for( i = 0; i < count; i++ )
        if( !mask || mask[i] )
        {
            double x = m0[i].x - m0c.x, y = m0[i].y - m0c.y;
            scale0 += sqrt(x*x + y*y);

            x = fabs(m1[i].x - m1c.x), y = fabs(m1[i].y - m1c.y);
            scale1 += sqrt(x*x + y*y);
        }

    scale0 *= t;
    scale1 *= t;

    if( scale0 < FLT_EPSILON || scale1 < FLT_EPSILON )
        EXIT;

    scale0 = sqrt(2.)/scale0;
    scale1 = sqrt(2.)/scale1;

    CV_CALL( A = cvCreateMat( good_count, 9, CV_64F ));
    a = A->data.db;
    a_step = A->step / sizeof(a[0]);

    // form a linear system: for each selected pair of points m0 & m1,
    // the row of A(=a) represents the equation: (m1, 1)'*F*(m0, 1) = 0
    for( i = 0; i < count; i++ )
    {
        if( !mask || mask[i] )
        {
            double x0 = (m0[i].x - m0c.x)*scale0;
            double y0 = (m0[i].y - m0c.y)*scale0;
            double x1 = (m1[i].x - m1c.x)*scale1;
            double y1 = (m1[i].y - m1c.y)*scale1;

            a[0] = x1*x0;
            a[1] = x1*y0;
            a[2] = x1;
            a[3] = y1*x0;
            a[4] = y1*y0;
            a[5] = y1;
            a[6] = x0;
            a[7] = y0;
            a[8] = 1;
            a += a_step;
        }
    }

    cvSVD( A, &W, 0, &V, CV_SVD_MODIFY_A + CV_SVD_V_T );

    for( i = 0; i < 8; i++ )
    {
        if( fabs(w[i]) < FLT_EPSILON )
            break;
    }

    if( i < 7 )
        EXIT;

    F0 = cvMat( 3, 3, CV_64F, v + 9*8 ); // take the last column of v as a solution of Af = 0

    // make F0 singular (of rank 2) by decomposing it with SVD,
    // zeroing the last diagonal element of W and then composing the matrices back.

    // use v as a temporary storage for different 3x3 matrices
    W = U = V = TF = F0;
    W.data.db = v;
    U.data.db = v + 9;
    V.data.db = v + 18;
    TF.data.db = v + 27;

    cvSVD( &F0, &W, &U, &V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
    W.data.db[8] = 0.;

    // F0 <- U*diag([W(1), W(2), 0])*V'
    cvGEMM( &U, &W, 1., 0, 0., &TF, CV_GEMM_A_T );
    cvGEMM( &TF, &V, 1., 0, 0., &F0, 0/*CV_GEMM_B_T*/ );

    // apply the transformation that is inverse
    // to what we used to normalize the point coordinates
    {
        double tt0[] = { scale0, 0, -scale0*m0c.x, 0, scale0, -scale0*m0c.y, 0, 0, 1 };
        double tt1[] = { scale1, 0, -scale1*m1c.x, 0, scale1, -scale1*m1c.y, 0, 0, 1 };
        CvMat T0, T1;
        T0 = T1 = F0;
        T0.data.db = tt0;
        T1.data.db = tt1;

        // F0 <- T1'*F0*T0
        cvGEMM( &T1, &F0, 1., 0, 0., &TF, CV_GEMM_A_T );
        F0.data.db = fmatrix;
        cvGEMM( &TF, &T0, 1., 0, 0., &F0, 0 );

        // make F(3,3) = 1
        if( fabs(F0.data.db[8]) > FLT_EPSILON )
            cvScale( &F0, &F0, 1./F0.data.db[8] );
    }

    result = 1;

    __END__;

    cvReleaseMat( &A );
    return result;
}


/************************************ RANSAC algorithm **********************************/
static int
icvFMatrix_RANSAC( 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;
    uchar* curr_mask = 0;
    uchar* temp_mask = 0;

    CV_FUNCNAME( "icvFMatrix_RANSAC" );

    __BEGIN__;

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

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

    threshold *= threshold;

    CV_CALL( curr_mask = (uchar*)cvAlloc( count ));
    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 matrices 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, s0, s1;

                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];

                s1 = a*a + b*b;
                d1 = m1[i].x*a + m1[i].y*b + c;

                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];

                s0 = a*a + b*b;
                d0 = m0[i].x*a + m0[i].y*b + c;

                curr_mask[i] = d1*d1 < threshold*s1 && d0*d0 < threshold*s0;
                good_count += curr_mask[i];
            }

            if( good_count > MAX( best_good_count, 6 ) )
            {
                double ep, lp, lep;