cvfundam.cpp.svn-base

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

            int t;
            for( t = 0; t < 9; t++ )
            {
                double f1,f2;
                f1 = cvmGet(&matrVV,t,7);
                f2 = cvmGet(&matrVV,t,8);

                double s = f1 * sol + (1-sol) * f2;
                cvmSet(fundMatr,numberRoots*3 + t/3,t%3,s);
            }
            numberRoots++;

            if( fundMatr->rows == 3 )/* not enough space to store more than one root */
                break;
        }
    }

    /* scale fundamental matrix */
    for( i = 0; i < numberRoots; i++ )
    {

        double fsc;
        fsc = cvmGet(fundMatr,i*3+2,2);
        if( fabs(fsc) > 1e-8 )
        {
            CvMat subFund;
            cvGetSubArr( fundMatr, &subFund, cvRect(0,i*3,3,3) );
            cvmScale(&subFund,&subFund,1.0/fsc);
        }
    }

    __END__;

    cvReleaseMat(&squares);


    return numberRoots;
}    


/*=====================================================================================*/

int icvComputeFundamental8Point(CvMat* points1,CvMat* points2, CvMat* fundMatr)
{
    CvMat* wpoints[2]={0,0};
    CvMat* preFundMatr = 0;
    CvMat* sqdists = 0;
    CvMat* matrA = 0;
    CvMat* matrU = 0;
    CvMat* matrW = 0;
    CvMat* matrV = 0;

    int numFundMatrs = 0;

    CV_FUNCNAME( "icvComputeFundamental8Point" );
    __BEGIN__;
    
    /* Test correct of input data */

    if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2)|| !CV_IS_MAT(fundMatr))
    {
        CV_ERROR(CV_StsBadPoint,"Not a matrixes");
    }

    if( !CV_ARE_TYPES_EQ( points1, points2 ))
    {
        CV_ERROR( CV_StsUnmatchedSizes, "Data types of points unmatched" );    
    }

    int numPoint;
    numPoint = points1->cols;
    
    int type;
    type = points1->type;
    
    if( numPoint != points2->cols )
    {
        CV_ERROR( CV_StsBadSize, "Number of points not equal" );
    }
    if( numPoint < 8 )
    {
        CV_ERROR( CV_StsBadSize, "Number of points must be at least 8" );
    }

    if( points1->rows > 3 || points1->rows < 2 )
    {
        CV_ERROR( CV_StsBadSize, "Number of coordinates of points1 must be 2 or 3" );
    }

    if( points2->rows > 3 || points2->rows < 2 )
    {
        CV_ERROR( CV_StsBadSize, "Number of coordinates of points2 must be 2 or 3" );
    }

    if( fundMatr->rows != 3 || fundMatr->cols != 3 )
    {
        CV_ERROR( CV_StsBadSize, "Size of fundMatr must be 3x3" );
    }

    /* allocate data */
    CV_CALL( wpoints[0] = cvCreateMat(2,numPoint,CV_64F) );
    CV_CALL( wpoints[1] = cvCreateMat(2,numPoint,CV_64F) );
    CV_CALL( matrA = cvCreateMat(numPoint,9,CV_64F) );
    CV_CALL( preFundMatr = cvCreateMat(3,3,CV_64F) );
    CV_CALL( matrU = cvCreateMat(numPoint,numPoint,CV_64F) );
    CV_CALL( matrW = cvCreateMat(numPoint,9,CV_64F) );
    CV_CALL( matrV = cvCreateMat(9,9,CV_64F) );
    CV_CALL( sqdists = cvCreateMat(1,numPoint,CV_64F) );

    /* Create working points with just x,y */

    CvMat transfMatr[2];
    double transfMatr1_dat[9];
    double transfMatr2_dat[9];
    transfMatr[0] = cvMat(3,3,CV_64F,transfMatr1_dat);
    transfMatr[1] = cvMat(3,3,CV_64F,transfMatr2_dat);
    
    {/* transform to x,y.  tranform point and compute transform matrixes */
        icvMake2DPoints(points1,wpoints[0]);
        icvMake2DPoints(points2,wpoints[1]);

        icvNormalizeFundPoints( wpoints[0],&transfMatr[0]);
        icvNormalizeFundPoints( wpoints[1],&transfMatr[1]);
        
        /* we have normalized working points wpoints[0] and wpoints[1] */
        /* build matrix A from points coordinates */

        int currPoint;
        for( currPoint = 0; currPoint < numPoint; currPoint++ )
        {
            CvMat rowA;
            double x1,y1;
            double x2,y2;

            x1 = cvmGet(wpoints[0],0,currPoint);
            y1 = cvmGet(wpoints[0],1,currPoint);
            x2 = cvmGet(wpoints[1],0,currPoint);
            y2 = cvmGet(wpoints[1],1,currPoint);

            cvGetRow(matrA,&rowA,currPoint);
            rowA.data.db[0] = x1*x2;
            rowA.data.db[1] = x1*y2;
            rowA.data.db[2] = x1;

            rowA.data.db[3] = y1*x2;
            rowA.data.db[4] = y1*y2;
            rowA.data.db[5] = y1;

            rowA.data.db[6] = x2;
            rowA.data.db[7] = y2;
            rowA.data.db[8] = 1;
        }
    }

    /* We have matrix A. Compute svd(A). We need last column of V */

    
    cvSVD( matrA, matrW, matrU, matrV, CV_SVD_V_T );/* get transposed matrix V */

    /* Compute number of non zero eigen values */
    int numEig;
    numEig = 0;
    {
        int i;
        for( i = 0; i < 8; i++ )
        {
            if( cvmGet(matrW,i,i) < 1e-8 )
            {
                break;
            }
        }

        numEig = i;

    }

    if( numEig < 5 )
    {/* Bad points */
        numFundMatrs = 0;
    }
    else
    {
        numFundMatrs = 1;

        /* copy last row of matrix V to precomputed fundamental matrix */

        CvMat preF;
        cvGetRow(matrV,&preF,8);
        cvReshape(&preF,preFundMatr,1,3);

        /* Apply singularity condition */
        icvMakeFundamentalSingular(preFundMatr);
    
        /* Denormalize fundamental matrix */
        /* Compute transformation for normalization */

        CvMat wfundMatr;
        double wfundMatr_dat[9];
        wfundMatr = cvMat(3,3,CV_64F,wfundMatr_dat);
    
        {/*  Freal = T1'*Fpre*T2 */
            double tmpMatr_dat[9];
            double tmptransfMatr_dat[9];
        
            CvMat tmpMatr = cvMat(3,3,CV_64F,tmpMatr_dat);
            CvMat tmptransfMatr = cvMat(3,3,CV_64F,tmptransfMatr_dat);

            cvTranspose(&transfMatr[0],&tmptransfMatr);
        
            cvmMul(&tmptransfMatr,preFundMatr,&tmpMatr);
            cvmMul(&tmpMatr,&transfMatr[1],&wfundMatr);
        }

        /* scale fundamental matrix */
        double fsc;
        fsc = cvmGet(&wfundMatr,2,2);
        if( fabs(fsc) > 1.0e-8 )
        {
            cvmScale(&wfundMatr,&wfundMatr,1.0/fsc);
        }
    
        /* copy result fundamental matrix */
        cvConvert( &wfundMatr, fundMatr );

    }


    __END__;
    
    cvReleaseMat(&matrU);
    cvReleaseMat(&matrW);
    cvReleaseMat(&matrV);
    cvReleaseMat(&preFundMatr);
    cvReleaseMat(&matrA);
    cvReleaseMat(&wpoints[0]);
    cvReleaseMat(&wpoints[1]);
    cvReleaseMat(&sqdists);

    return numFundMatrs;
}

/*=====================================================================================*/

/* Computes fundamental matrix using RANSAC method */
/*  */
int icvComputeFundamentalRANSAC(   CvMat* points1,CvMat* points2,
                                        CvMat* fundMatr,
                                        double threshold,/* Threshold for good points */
										double p,/* Probability of good result. */
                                        CvMat* status)    
{
    CvMat* wpoints1 = 0;
    CvMat* wpoints2 = 0;
    CvMat* corrLines1 = 0;
    CvMat* corrLines2 = 0;
    CvMat* bestPoints1 = 0;
    CvMat* bestPoints2 = 0;

    int* flags = 0;
    int* bestFlags = 0;
    int numFundMatr = 0;
    
    CV_FUNCNAME( "icvComputeFundamentalRANSAC" );
    __BEGIN__;

    /* Test correct of input data */

    if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2)|| !CV_IS_MAT(fundMatr))
    {
        CV_ERROR(CV_StsBadPoint,"points1 or points2 or funMatr are not a matrixes");
    }

    int numPoint;
    numPoint = points1->cols;
    
    int type;
    type = points1->type;
    
    if( numPoint != points2->cols )
    {
        CV_ERROR( CV_StsBadSize, "Number of points not equals" );
    }
    if( numPoint < 8 )
    {
        CV_ERROR( CV_StsBadSize, "Number of points must be >= 8" );
    }

    if( points1->rows != 2 && points1->rows != 3 )
    {
        CV_ERROR( CV_StsBadSize, "Number of coordinates of points1 must be 2 or 3" );
    }

    if( points2->rows != 2 && points2->rows != 3 )
    {
        CV_ERROR( CV_StsBadSize, "Number of coordinates of points2 must be 2 or 3" );
    }

    if( fundMatr->rows != 3 || fundMatr->cols != 3 )
    {
        CV_ERROR( CV_StsBadSize, "Size of fundMatr must be 3x3" );
    }

    if( status )
    {/* status is present test it */
        if( !CV_IS_MAT(status) )
        {
            CV_ERROR(CV_StsBadPoint,"status is not a matrix");
        }

        if( status->cols != numPoint || status->rows != 1 )
        {
            CV_ERROR(CV_StsBadPoint,"Size of status must be 1xN");
        }
    }
       
    /* We will choose adaptive number of N (samples) */

    /* Convert points to 64F working points */

    CV_CALL( flags = (int*)cvAlloc(numPoint * sizeof(int)) );
    CV_CALL( bestFlags = (int*)cvAlloc(numPoint * sizeof(int)) );
    CV_CALL( wpoints1 = cvCreateMat(3,numPoint,CV_64F) );
    CV_CALL( wpoints2 = cvCreateMat(3,numPoint,CV_64F) );
    CV_CALL( corrLines1 = cvCreateMat(3,numPoint,CV_64F));
    CV_CALL( corrLines2 = cvCreateMat(3,numPoint,CV_64F));
    CV_CALL( bestPoints1 = cvCreateMat(3,numPoint,CV_64F));
    CV_CALL( bestPoints2 = cvCreateMat(3,numPoint,CV_64F));

    icvMake3DPoints(points1,wpoints1);
    icvMake3DPoints(points2,wpoints2);
        
    {
        int wasCount = 0;  //count of choosing samples
        int maxGoodPoints = 0;
        int numGoodPoints = 0;

        double bestFund_dat[9];
        CvMat  bestFund;
        bestFund = cvMat(3,3,CV_64F,bestFund_dat);

        /* choosen points */        
        int NumSamples = 500;/* Initial need number of samples */
        while( wasCount < NumSamples )
        {
            /* select samples */
            int randNumbs[7];
            int i;
            int newnum;
            int pres;
            for( i = 0; i < 7; i++ )
            {
                do
                {
                    newnum = rand()%numPoint;

                    /* test this number */
                    pres = 0;
                    int test;
                    for( test = 0; test < i; test++ )
                    {
                        if( randNumbs[test] == newnum )
                        {
                            pres = 1;
                            break;
                        }
                    }
                }
                while( pres );
                randNumbs[i] = newnum;
            }
            /* random numbers of points was generated */
            /* select points */

            double selPoints1_dat[2*7];
            double selPoints2_dat[2*7];
            CvMat selPoints1;
            CvMat selPoints2;
            selPoints1 = cvMat(2,7,CV_64F,selPoints1_dat);
            selPoints2 = cvMat(2,7,CV_64F,selPoints2_dat);
            /* copy points */
            int t;
            for( t = 0; t < 7; t++ )
            {
                double x,y;
                x = cvmGet(wpoints1,0,randNumbs[t]);
                y = cvmGet(wpoints1,1,randNumbs[t]);
                cvmSet(&selPoints1,0,t,x);
                cvmSet(&selPoints1,1,t,y);
                
                x = cvmGet(wpoints2,0,randNumbs[t]);
                y = cvmGet(wpoints2,1,randNumbs[t]);
                cvmSet(&selPoints2,0,t,x);
                cvmSet(&selPoints2,1,t,y);
            }

            /* Compute fundamental matrix using 7-points algorithm */

            double fundTriple_dat[27];
            CvMat fundTriple;
            fundTriple = cvMat(9,3,CV_64F,fundTriple_dat);

            int numFund = icvComputeFundamental7Point(&selPoints1,&selPoints2,&fundTriple);

            //double fund7_dat[9];
            CvMat fund7;
            //fund7 = cvMat(3,3,CV_64F,fund7_dat);

            /* get sub matrix */
            for( int currFund = 0; currFund < numFund; currFund++ )
            {
                cvGetSubArr(&fundTriple,&fund7,cvRect(0,currFund*3,3,3));
                {
                    /* Compute inliers for this fundamental matrix */
                    /* Compute distances for points and correspond lines */
                    {
                        /* Create corresponde lines */
                    
                        icvComputeCorrespondEpilines(wpoints1,2,&fund7,corrLines2);
                        icvComputeCorrespondEpilines(wpoints2,1,&fund7,corrLines1);
                        /* compute distances for points and number of good points */
                        int i;
                        numGoodPoints = 0;
                        for( i = 0; i < numPoint; i++ )
                        {
                            CvMat pnt1,pnt2;
                            CvMat lin1,lin2;
                            cvGetCol(wpoints1,&pnt1,i);
                            cvGetCol(corrLines1,&lin1,i);
                            cvGetCol(wpoints2,&pnt2,i);