cvfundam.cpp.svn-base

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

                            cvGetCol(corrLines2,&lin2,i);
                            double dist1,dist2;
                            dist1 = fabs(cvDotProduct(&pnt1,&lin1));
                            dist2 = fabs(cvDotProduct(&pnt2,&lin2));
                            flags[i] = ( dist1 < threshold && dist2 < threshold )?1:0;
                            numGoodPoints += flags[i];
                        }
                    }
                }

                if( numGoodPoints > maxGoodPoints )
                {/* good matrix */
                    cvCopy(&fund7,&bestFund);
                    maxGoodPoints = numGoodPoints;
                    /* copy best flags */
                    int i;
                    for(i=0;i<numPoint;i++)
                    {
                        bestFlags[i] = flags[i];
                    }

                    /* Recompute new number of need steps */

			        /* Adaptive number of samples to count*/
			        double ep = 1 - (double)numGoodPoints / (double)numPoint;
                    if( ep == 1 )
                    {
                        ep = 0.5;//if there is not good points set ration of outliers to 50%
                    }
            
			        double newNumSamples = log(1-p) / log(1-pow(1-ep,7));

                    if( newNumSamples < (double)NumSamples )
                    {
                        NumSamples = cvRound(newNumSamples);
                    }

                }
            }
            
            wasCount++;
        }

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

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

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

                for( i = 0; i < numPoint; i++ )
                {
                    if( bestFlags[i] )
                    {
                        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++;
                    }
                }

                CvMat wbestPnts1;
                CvMat wbestPnts2;

                cvGetSubArr( bestPoints1, &wbestPnts1, cvRect(0,0,maxGoodPoints,3) );
                cvGetSubArr( bestPoints2, &wbestPnts2, cvRect(0,0,maxGoodPoints,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);        
                //cvConvert(&bestFund,fundMatr); // This line must be deleted
            }
            else
            {
                /* 7 point. Just copy to result */
                cvConvert(&bestFund,fundMatr);
                numFundMatr = 1;
            }

            /* copy flag to status if need */
            if( status )
            {
                for( int i = 0; i < numPoint; i++)
                {
                    cvmSet(status,0,i,(double)bestFlags[i]);
                }
            }
        }


    }

    __END__;

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

    return numFundMatr;
}//icvComputeFundamentalRANSAC

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

void icvCompPointLineDists(CvMat* points,CvMat* lines,CvMat* distances)
{/* Line must be normalized */
    
    int numPoints;
    numPoints = points->cols;
    if( numPoints != lines->cols && numPoints != distances->cols )
    {
        //printf("Size if arrays not equals\n");
        return;//error
    }

    int i;
    for( i = 0; i < numPoints; i++ )
    {
        CvMat pnt;
        CvMat lin;
        cvGetCol(points,&pnt,i);
        cvGetCol(lines,&lin,i);
        double dist;
        dist = fabs(cvDotProduct(&pnt,&lin));
        cvmSet(distances,0,i,dist);
    }

    return;
}

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



#define _compVals( v1, v2 )  ((v1) < (v2))

/* Create function to sort vector */
CV_IMPLEMENT_QSORT( _SortCvMatVect, double, _compVals )

/*=====================================================================================*/
int icvComputeFundamentalLMedS(    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;
    CvMat* dists1 = 0;
    CvMat* dists2 = 0;
    CvMat* distsSq1 = 0;
    CvMat* distsSq2 = 0;
    CvMat* allDists = 0;

    int* flags = 0;
    int* bestFlags = 0;
    int numFundMatr = 0;
    
    CV_FUNCNAME( "icvComputeFundamentalLMedS" );
    __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");
    }

    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 >= 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));
    CV_CALL( dists1 = cvCreateMat(1,numPoint,CV_64F));
    CV_CALL( dists2 = cvCreateMat(1,numPoint,CV_64F));
    CV_CALL( distsSq1 = cvCreateMat(1,numPoint,CV_64F));
    CV_CALL( distsSq2 = cvCreateMat(1,numPoint,CV_64F));
    CV_CALL( allDists = cvCreateMat(1,numPoint,CV_64F));

    icvMake3DPoints(points1,wpoints1);
    icvMake3DPoints(points2,wpoints2);
    
    {
        int NumSamples = 500;//Maximux number of steps
        int wasCount = 0;  //count of choosing samples

		double goodMean = FLT_MAX;
		double currMean;

        int numGoodPoints = 0;

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

        /* choosen points */        
        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 median error for this matrix */
				    {
                        icvComputeCorrespondEpilines(wpoints1,2,&fund7,corrLines2);
                        icvComputeCorrespondEpilines(wpoints2,1,&fund7,corrLines1);

					    icvCompPointLineDists(wpoints1,corrLines1,dists1);
					    icvCompPointLineDists(wpoints2,corrLines2,dists2);

					    /* add distances for points (d1*d1+d2*d2) */
					    cvMul(dists1,dists1,distsSq1);
					    cvMul(dists2,dists2,distsSq2);

					    cvAdd(distsSq1,distsSq2,allDists);

					    /* sort distances */
					    _SortCvMatVect(allDists->data.db,numPoint,0);

					    /* get median error */
					    currMean = allDists->data.db[numPoint/2];
				    }
                }

                if( currMean < goodMean )
                {/* good matrix */
                    cvCopy(&fund7,&bestFund);
                    goodMean = currMean;

                    /* Compute number of good points using threshold */
                    int i;
                    numGoodPoints = 0;
                    for( i = 0 ; i < numPoint; i++ )
                    {
                        if( dists1->data.db[i] < threshold && dists2->data.db[i] < threshold )
                        {
                            numGoodPoints++;
                        }
                    }


                    /* Compute adaptive number of steps */
			        double ep = 1 - (double)numGoodPoints / (double)numPoint;
                    if( ep == 1 )
                    {
                        ep = 0.5;//if there is not good points set ration of outliers to 50%
                    }

			        double newNumSamples = log(1-p) / log(1-pow(1-ep,7));
                    if( newNumSamples < (double)NumSamples )
                    {
                        NumSamples = cvRound(newNumSamples);
                    }
                }
            }

            wasCount++;
        }

        /* Select just good points using threshold */
        /* Compute distances for all points for best fundamental matrix */

        /* Test if we have computed fundamental matrix*/
        if( goodMean == FLT_MAX )
        {
            numFundMatr = 0;

        }
        else
        {/* we have computed fundamental matrix */
		    {
                icvComputeCorrespondEpilines(wpoints1,2,&bestFund,corrLines2);
                icvComputeCorrespondEpilines(wpoints2,1,&bestFund,corrLines1);

			    icvCompPointLineDists(wpoints1,corrLines1,dists1);
			    icvCompPointLineDists(wpoints2,corrLines2,dists2);

                /* test dist for each point and set status for each point if need */
                int i;