cvfundam.cpp.svn-base

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

                int currPnt = 0;
                for( i = 0; i < numPoint; i++ )
                {
                    if( dists1->data.db[i] < threshold && dists2->data.db[i] < threshold )
                    {
                        CvMat wPnt;
                        CvMat bestPnt;
                        cvGetCol( wpoints1,&wPnt, i );
                        cvGetCol( bestPoints1,&bestPnt, currPnt );
                        cvCopy(&wPnt,&bestPnt);
                        cvGetCol( wpoints2,&wPnt, i );
                        cvGetCol( bestPoints2,&bestPnt, currPnt );
                        cvCopy(&wPnt,&bestPnt);
                        currPnt++;
                                        
                        if( status )
                            cvmSet(status,0,i,1.0);
                    }
                    else
                    {
                        if( status )
                            cvmSet(status,0,i,0.0);
                    }

                }
                numGoodPoints = currPnt;
		    }

            /* we have best 7-point fundamental matrix. */
            /* and best points */
            /* use these points to improove matrix */

            /* Test number of points. And if number >=8 improove found fundamental matrix  */

            if( numGoodPoints < 7 )
            {
                /* Fundamental matrix not found */
                numFundMatr = 0;
            }
            else
            {
                if( numGoodPoints > 7 )
                {
                    /* Found >= 8 point. Improove matrix */

                    CvMat wbestPnts1;
                    CvMat wbestPnts2;

                    cvGetSubArr( bestPoints1, &wbestPnts1, cvRect(0,0,numGoodPoints,3) );
                    cvGetSubArr( bestPoints2, &wbestPnts2, cvRect(0,0,numGoodPoints,3) );

                    /* best points was collectet. Improve fundamental matrix */
                    /* Just use 8-point algorithm */
                    double impFundMatr_dat[9];
                    CvMat impFundMatr;
                    impFundMatr = cvMat(3,3,CV_64F,impFundMatr_dat);
                    numFundMatr = icvComputeFundamental8Point(&wbestPnts1,&wbestPnts2,&impFundMatr);

                    cvConvert(&impFundMatr,fundMatr);        
                }
                else
                {
                    /* 7 point. Just copy to result */
                    cvConvert(&bestFund,fundMatr);
                    numFundMatr = 1;
                }
            }

        }
    }

    __END__;

    /* free allocated memory */
    
    cvReleaseMat(&corrLines1);
    cvReleaseMat(&corrLines2);
    cvReleaseMat(&wpoints1);
    cvReleaseMat(&wpoints2);
    cvReleaseMat(&bestPoints1);
    cvReleaseMat(&bestPoints2);
    cvReleaseMat(&dists1);
    cvReleaseMat(&dists2);
    cvReleaseMat(&distsSq1);
    cvReleaseMat(&distsSq2);
    cvReleaseMat(&allDists);
    cvFree((void**)&flags);
    cvFree((void**)&bestFlags);

    return numFundMatr;
}//icvComputeFundamentalLMedS

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



void icvMakeFundamentalSingular(CvMat* fundMatr)
{
    CV_FUNCNAME( "icvFundSingular" );
    __BEGIN__;

    if( !CV_IS_MAT(fundMatr) )
    {
        CV_ERROR(CV_StsBadPoint,"Input data is not matrix");
    }
    
    if( fundMatr->rows != 3 || fundMatr->cols != 3 )
    {
        CV_ERROR( CV_StsBadSize, "Size of fundametal matrix must be 3x3" );
    }

    
    {/* Apply singularity condition */
        CvMat matrFU;
        CvMat matrFW;
        CvMat matrFVt;
        CvMat tmpMatr;
        CvMat preFundMatr;
        double matrFU_dat[9];
        double matrFW_dat[9];
        double matrFVt_dat[9];
        double tmpMatr_dat[9];
        double preFundMatr_dat[9];

        
        matrFU  = cvMat(3,3,CV_64F,matrFU_dat);
        matrFW  = cvMat(3,3,CV_64F,matrFW_dat);
        matrFVt = cvMat(3,3,CV_64F,matrFVt_dat);
        tmpMatr = cvMat(3,3,CV_64F,tmpMatr_dat);
        preFundMatr = cvMat(3,3,CV_64F,preFundMatr_dat);

        cvConvert(fundMatr,&preFundMatr);
        cvSVD( &preFundMatr, &matrFW, &matrFU, &matrFVt, CV_SVD_V_T );
        cvmSet(&matrFW,2,2,0);
        /* multiply U*W*V' */

        cvmMul(&matrFU,&matrFW,&tmpMatr);
        cvmMul(&tmpMatr,&matrFVt,&preFundMatr);
        cvConvert(&preFundMatr,fundMatr);
    }
    

    __END__;
}


/*=====================================================================================*/
/* Normalize points for computing fundamental matrix */
/* and compute transform matrix */
/* Points:  2xN  */
/* Matrix:  3x3 */
/* place centroid of points to (0,0) */
/* set mean distance from (0,0) by sqrt(2) */

void icvNormalizeFundPoints( CvMat* points, CvMat* transfMatr )
{
    CvMat* subwpointsx = 0;
    CvMat* subwpointsy = 0;
    CvMat* sqdists     = 0;
    CvMat* pointsxx    = 0;
    CvMat* pointsyy    = 0;

	int numPoint;
	int type;
	double shiftx,shifty; 
	double meand;
	double scale;

	CvMat tmpwpointsx;
    CvMat tmpwpointsy;

	CvScalar sumx;
    CvScalar sumy;

    CV_FUNCNAME( "icvNormalizeFundPoints" );
    __BEGIN__;
    
    /* Test for correct input data */

    if( !CV_IS_MAT(points) || !CV_IS_MAT(transfMatr) )
    {
        CV_ERROR(CV_StsBadPoint,"Input data is not matrixes");
    }
    
    numPoint = points->cols;
        
    type = points->type;
    
    if( numPoint < 1 )
    {
        CV_ERROR( CV_StsBadSize, "Number of points must be at least 1" );
    }

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

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

    CV_CALL( subwpointsx  =  cvCreateMat(1,numPoint,CV_64F) );
    CV_CALL( subwpointsy  =  cvCreateMat(1,numPoint,CV_64F) );
    CV_CALL( sqdists      =  cvCreateMat(1,numPoint,CV_64F) );
    CV_CALL( pointsxx     =  cvCreateMat(1,numPoint,CV_64F) );
    CV_CALL( pointsyy     =  cvCreateMat(1,numPoint,CV_64F) );

    /* get x,y coordinates of points */
    
    {
        cvGetRow( points, &tmpwpointsx, 0 );
        cvGetRow( points, &tmpwpointsy, 1 );

        /* Copy to working data 64F */
        cvConvert(&tmpwpointsx,subwpointsx);
        cvConvert(&tmpwpointsy,subwpointsy);
    }

    /* Compute center of points */

    sumx = cvSum(subwpointsx);
    sumy = cvSum(subwpointsy);

    sumx.val[0] /= (double)numPoint;
    sumy.val[0] /= (double)numPoint;

    shiftx = sumx.val[0];
    shifty = sumy.val[0];

    /* Shift points center to 0 */

    cvSubS( subwpointsx, sumx, subwpointsx);
    cvSubS( subwpointsy, sumy, subwpointsy);

    /* Compute x*x and y*y */        

    cvMul(subwpointsx,subwpointsx,pointsxx);
    cvMul(subwpointsy,subwpointsy,pointsyy);
    
    /* add  */
    cvAdd( pointsxx, pointsyy, sqdists);

    /* compute sqrt of each component*/
    
    cvPow(sqdists,sqdists,0.5);

    /* in vector sqdists we have distances */
    /* compute mean value and scale */
    
    meand = cvMean(sqdists);    
    
    if( fabs(meand) > 1e-8  )
    {
        scale = 0.70710678118654752440084436210485/meand;
    }
    else
    {
        scale = 1.0;
    }

    /* scale working points */    
    cvScale(subwpointsx,subwpointsx,scale);
    cvScale(subwpointsy,subwpointsy,scale);

    /* copy output data */
    {
        cvGetRow( points, &tmpwpointsx, 0 );
        cvGetRow( points, &tmpwpointsy, 1 );

        /* Copy to output data 64F */
        cvConvert(subwpointsx,&tmpwpointsx);
        cvConvert(subwpointsy,&tmpwpointsy);
    }

    /* Set transform matrix */
    
    cvmSet(transfMatr,0,0, scale);
    cvmSet(transfMatr,0,1, 0);
    cvmSet(transfMatr,0,2, -scale*shiftx);

    cvmSet(transfMatr,1,0, 0);
    cvmSet(transfMatr,1,1, scale);
    cvmSet(transfMatr,1,2, -scale*shifty);

    cvmSet(transfMatr,2,0, 0);
    cvmSet(transfMatr,2,1, 0);
    cvmSet(transfMatr,2,2, 1);

    __END__;
    
    /* Free data */
    cvReleaseMat(&subwpointsx);
    cvReleaseMat(&subwpointsy);
    cvReleaseMat(&sqdists);
    cvReleaseMat(&pointsxx);
    cvReleaseMat(&pointsyy);

}
/*=====================================================================================*/
// Solve cubic equation and returns number of roots
int cvSolveCubic(CvMat* coeffs,CvMat* result)
{
    return icvSolveCubic(coeffs, result);
}

/*=====================================================================================*/
// Solve cubic equation and returns number of roots
// Also returns 0 if all values are possible
// Test for very big coefficients
// Input params 1x3 or 1x4
int icvSolveCubic(CvMat* coeffs,CvMat* result)
{/* solve a*x^3 + b+x^2 + c*x + d = 0 */
    /* coeffs a,b,c,d or b,c,d if a== 1*/
    /* test input params */
    /* result 2x3  */

    int numRoots = 0;

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

    if( !CV_IS_MAT(coeffs) || !CV_IS_MAT(result) )
    {
        CV_ERROR(CV_StsBadPoint,"Not a matrixes");
    }

    if( !(coeffs->rows == 1 && (coeffs->cols == 3 || coeffs->cols == 4) ))
    {
        CV_ERROR( CV_StsBadSize, "Number of coeffs must be 3 or 4" );
    }


    double squares[6];
    double cc[4];
    cc[0] = cvmGet(coeffs,0,0);
    cc[1] = cvmGet(coeffs,0,1);
    cc[2] = cvmGet(coeffs,0,2);

    if( fabs(cc[0]) > FLT_MAX || fabs(cc[1]) > FLT_MAX || fabs(cc[2]) > FLT_MAX )
    {
        return 0;//Coeffs too big
    }

    double a0,a1,a2;
    if( coeffs->cols == 3 )
    {
        a0 = cc[0];
        a1 = cc[1];
        a2 = cc[2];
        numRoots = icvCubicV(a0,a1,a2,squares);
    }
    else
    {// We have for coeffs
        /* Test for very big coeffs */
        cc[3] = cvmGet(coeffs,0,3);

        if( fabs(cc[3]) > FLT_MAX )
        {
            return 0;//Coeffs too big
        }

        double a = cc[0];
        if( fabs(a) > FLT_MIN)
        {
            a = 1. / a;
            a0 = cc[1] * a;
            a1 = cc[2] * a;
            a2 = cc[3] * a;
            numRoots = icvCubicV(a0,a1,a2,squares);
        }
        else
        {// It's a square eqaution.
            double a,b,c;
            a = cc[1];
            b = cc[2];
            c = cc[3];
            if( fabs(a) > 1e-8 )
            {
                double D;
                D = b*b-4*a*c;
                if( D > FLT_MIN )
                {// Two roots
                    numRoots = 2;
                    squares[0] = (-b + sqrt(D))/(2*a);
                    squares[1] = 0;
                    squares[2] = (-b - sqrt(D))/(2*a);
                    squares[3] = 0;
                }
                else
                {
                    if( D < FLT_MIN  )
                    {/* Two Im values */
                        numRoots = 2;
                        squares[0] = (-b)/(2*a);
                        squares[1] = (  sqrt(-D))/(2*a);

                        squares[2] = (-b)/(2*a);
                        squares[3] = ( -sqrt(-D))/(2*a);
                    }
                    else
                    {/* D==0 */
                        numRoots = 2;
                        squares[0] = (-b)/(2*a);
                        squares[1] = 0;
                        squares[2] = (-b)/(2*a);
                        squares[3] = 0;
                    }
                }
            }
            else
            {// Linear equation
                if( fabs(b) > FLT_MIN )
                {
                    squares[0] = -c/b;
                    squares[1] = 0;
                    numRoots = 1;
                }
                else
                {
                    if( fabs(c) > FLT_MIN)
                    {
                        numRoots = 0;
                    }
                    else
                    {