cvfundam.cpp.svn-base

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

                        /* All values are posible */
                        numRoots = 0;// !!!
                        //cvmSet(result,0,0,0);
                        //cvmSet(result,1,0,0);
                    }
                }
            }
        }
    }

    /* copy result  */
    int i;

    for( i=0;i<numRoots;i++ )
    {
        cvmSet(result,0,i,squares[i*2]);
        cvmSet(result,1,i,squares[i*2+1]);
    }
    __END__;

    return numRoots;
}

/*=====================================================================================*/
void cvMake2DPoints(CvMat* srcPoint,CvMat* dstPoint)
{
    icvMake2DPoints(srcPoint,dstPoint);
    return;
}

/* 
  Convert 2D or 3D points to 2D points

  for 3D: x = x/z;
          y = y/z
  for 2D just copy and maybe convert type

  Source and destiantion may be the same and in this case src must be 2D
*/
void icvMake2DPoints(CvMat* srcPoint,CvMat* dstPoint)
{
    CvMat* submatx = 0;
    CvMat* submaty = 0;
    CvMat* submatz = 0;

    CV_FUNCNAME( "icvMake2DPoints" );
    __BEGIN__;
    
    if( !CV_IS_MAT(srcPoint) || !CV_IS_MAT(dstPoint) )
    {
        CV_ERROR(CV_StsBadPoint,"Not a matrixes");
    }

    int numPoint;
    numPoint = srcPoint->cols;
        
    if( numPoint != dstPoint->cols )
    {
        CV_ERROR( CV_StsBadSize, "Number of points not equal" );
    }
    if( numPoint < 1 )
    {
        CV_ERROR( CV_StsBadSize, "Number of points must > 0" );
    }

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

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

    CV_CALL( submatx = cvCreateMat(1,numPoint,CV_64F) );
    CV_CALL( submaty = cvCreateMat(1,numPoint,CV_64F) );
    CV_CALL( submatz = cvCreateMat(1,numPoint,CV_64F) );

    CvMat subwpointsx;
    CvMat subwpointsy;
    
    CvMat tmpSubmatx;
    CvMat tmpSubmaty;
    CvMat tmpSubmatz;
    
    cvGetRow( dstPoint, &subwpointsx, 0 );
    cvGetRow( dstPoint, &subwpointsy, 1 );
    
    cvGetRow( srcPoint, &tmpSubmatx, 0 );
    cvGetRow( srcPoint, &tmpSubmaty, 1 );
    
    cvConvert(&tmpSubmatx,submatx);
    cvConvert(&tmpSubmaty,submaty);

    if( srcPoint->rows == 3 )
    {
        cvGetRow( srcPoint, &tmpSubmatz, 2 );
        cvConvert(&tmpSubmatz,submatz);
        
        cvDiv( submatx, submatz, &subwpointsx);
        cvDiv( submaty, submatz, &subwpointsy);
    }
    else
    {
        cvConvert(submatx,&subwpointsx);
        cvConvert(submaty,&subwpointsy);
    }

    __END__;

    cvReleaseMat(&submatx);
    cvReleaseMat(&submaty);
    cvReleaseMat(&submatz);
}

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

void cvMake3DPoints(CvMat* srcPoint,CvMat* dstPoint)
{
    icvMake3DPoints(srcPoint,dstPoint);
    return;
}


/* 
  Convert 2D or 3D points to 3D points

  for 2D: x = x;
          y = y;
          z = 1;
          
  for 3D: x = x;
          y = y;
          z = z;
          
  Source and destiantion may be the same and in this case src must be 2D
*/
void icvMake3DPoints(CvMat* srcPoint,CvMat* dstPoint)
{
    CvMat* tmpSubmatz = 0;

    CV_FUNCNAME( "icvMake3DPoints" );
    __BEGIN__;
    
    if( !CV_IS_MAT(srcPoint) || !CV_IS_MAT(dstPoint) )
    {
        CV_ERROR(CV_StsBadPoint,"Not a matrixes");
    }

    int numPoint;
    numPoint = srcPoint->cols;
        
    if( numPoint != dstPoint->cols )
    {
        CV_ERROR( CV_StsBadSize, "Number of points not equal" );
    }
    if( numPoint < 1 )
    {
        CV_ERROR( CV_StsBadSize, "Number of points must > 0" );
    }

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

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

    CV_CALL( tmpSubmatz = cvCreateMat(1,numPoint,CV_64F) );
    
    if( srcPoint->rows == 3 )
    {
        /* Just copy all points */
        cvConvert(srcPoint,dstPoint);
    }
    else
    {
        CvMat subwpointsx;
        CvMat subwpointsy;
        CvMat subwpointsz;
        
        cvGetRow( dstPoint, &subwpointsx, 0 );
        cvGetRow( dstPoint, &subwpointsy, 1 );
        cvGetRow( dstPoint, &subwpointsz, 2 );
        
        CvMat tmpSubmatx;
        CvMat tmpSubmaty;
        
        cvGetRow( srcPoint, &tmpSubmatx, 0 );
        cvGetRow( srcPoint, &tmpSubmaty, 1 );

        cvConvert( &tmpSubmatx, &subwpointsx );
        cvConvert( &tmpSubmaty, &subwpointsy );
        
        /* fill z by 1 */
        int i;
        for( i = 0; i < numPoint; i++ )
        {
            cvmSet(&subwpointsz,0,i,1.0);
        }
    }

    __END__;

    cvReleaseMat(&tmpSubmatz);
}

/*=====================================================================================*/
void cvComputeCorrespondEpilines(CvMat* points,int pointImageID,CvMat* fundMatr,CvMat* corrLines)
{
	icvComputeCorrespondEpilines(points, pointImageID, fundMatr, corrLines);
	return;
}
/*=====================================================================================*/
void icvComputeCorrespondEpilines(CvMat* points,int pointImageID,CvMat* fundMatr,CvMat* corrLines)
{

    CvMat* wpoints = 0;
    CvMat* wcorrLines = 0;

    pointImageID = 3-pointImageID;

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

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

    /*  */

    int numPoint;
    numPoint = points->cols;
    
    if( numPoint != corrLines->cols )
    {
        CV_ERROR( CV_StsBadSize, "Number of points and lines are not equal" );
    }

    if( numPoint < 1 )
    {
        CV_ERROR( CV_StsBadSize, "Number of points must > 0" );
    }

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

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

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

    double wfundMatr_dat[9];
    CvMat wfundMatr;
    wfundMatr = cvMat(3,3,CV_64F,wfundMatr_dat);
    cvConvert(fundMatr,&wfundMatr);

    if( pointImageID == 1 )
    {// get transformed fundamental matrix
        double tmpMatr_dat[9];
        CvMat tmpMatr;
        tmpMatr = cvMat(3,3,CV_64F,tmpMatr_dat);
        cvConvert(fundMatr,&tmpMatr);
        cvTranspose(&tmpMatr,&wfundMatr);
    }
    else if( pointImageID != 2 )
    {
        CV_ERROR( CV_StsBadArg, "Image ID must be 1 or 2" );
    }
    /* if wfundMatr we have good fundamental matrix */
    /* compute corr epi line for given points */

    CV_CALL( wpoints = cvCreateMat(3,numPoint,CV_64F) );
    CV_CALL( wcorrLines = cvCreateMat(3,numPoint,CV_64F) );


    /* if points has 2 coordinates trandform them to 3D */
    icvMake3DPoints(points,wpoints);

    cvmMul(&wfundMatr,wpoints,wcorrLines);

    /* normalise line coordinates */
    int i;
    for( i = 0; i < numPoint; i++ )
    {
        CvMat line;
        cvGetCol(wcorrLines,&line,i);
        double a,b;
        a = cvmGet(&line,0,0);
        b = cvmGet(&line,1,0);
        double nv;
        nv = sqrt(a*a+b*b);
        cvConvertScale(&line,&line,1.0 / nv);        
    }
    cvConvert(wcorrLines,corrLines);


    __END__;

    cvReleaseMat(&wpoints);
    cvReleaseMat(&wcorrLines);

}

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

#define SIGN(x) ( (x)<0 ? -1:((x)>0?1:0 ) )
//#define REAL_ZERO(x) ( (x) < EPSILON && (x) > -EPSILON)
#define REAL_ZERO(x) ( (x) < 1e-8 && (x) > -1e-8)

/* function return squares for cubic equation. 6 params - two for each square (Re,Im) */
int
icvCubicV( double a2, double a1, double a0, double *squares )
{
    double p, q, D, c1, c2, b1, b2, ro1, ro2, fi1, fi2;
    double x[6][3];
    int i, j, t;

    if( !squares )
        return CV_BADFACTOR_ERR;

    if( fabs(a0) > FLT_MAX || fabs(a1) > FLT_MAX || fabs(a2) > FLT_MAX )
    {
        return 0;//Coeffs too big
    }


    p = a1 - a2 * a2 / 3;
    q = (9 * a1 * a2 - 27 * a0 - 2 * a2 * a2 * a2) / 27;
    D = q * q / 4 + p * p * p / 27;

    if( fabs(p) > FLT_MAX || fabs(q) > FLT_MAX || fabs(D) > FLT_MAX )
    {
        return 0;//Coeffs too big
    }
    
    if( D < 0 )
    {

        c1 = q / 2;
        c2 = c1;
        b1 = sqrt( -D );
        b2 = -b1;

        ro1 = sqrt( c1 * c1 - D );
        ro2 = ro1;

        fi1 = atan2( b1, c1 );
        fi2 = -fi1;
    }
    else
    {

        c1 = q / 2 + sqrt( D );
        c2 = q / 2 - sqrt( D );
        b1 = 0;
        b2 = 0;

        ro1 = fabs( c1 );
        ro2 = fabs( c2 );
        fi1 = CV_PI * (1 - SIGN( c1 )) / 2;
        fi2 = CV_PI * (1 - SIGN( c2 )) / 2;
    }                           /* if */

    for( i = 0; i < 6; i++ )
    {

        x[i][0] = -a2 / 3;
        x[i][1] = 0;
        x[i][2] = 0;

        squares[i] = x[i][i % 2];
    }                           /* for */

    if( !REAL_ZERO( ro1 ))
    {

        c1 = SIGN( ro1 ) * pow( fabs( ro1 ), 1. / 3 ) -
            SIGN( ro1 ) * p / 3. * pow( fabs( ro1 ), -1. / 3 );

        c2 = SIGN( ro1 ) * pow( fabs( ro1 ), 1. / 3 ) +
            SIGN( ro1 ) * p / 3. * pow( fabs( ro1 ), -1. / 3 );
    }                           /* if */

    if( !REAL_ZERO( ro2 ))
    {

        b1 = SIGN( ro2 ) * pow( fabs( ro2 ), 1. / 3 ) -
            SIGN( ro2 ) * p / 3. * pow( fabs( ro2 ), -1. / 3 );

        b2 = SIGN( ro2 ) * pow( fabs( ro2 ), 1. / 3 ) +
            SIGN( ro2 ) * p / 3. * pow( fabs( ro2 ), -1. / 3 );
    }                           /* if */

    for( i = 0; i < 6; i++ )
    {

        if( i < 3 )
        {

            if( !REAL_ZERO( ro1 ))
            {

                x[i][0] = cos( fi1 / 3. + 2 * CV_PI * (i % 3) / 3. ) * c1 - a2 / 3;
                x[i][1] = sin( fi1 / 3. + 2 * CV_PI * (i % 3) / 3. ) * c2;
            }
            else
            {

                //x[i][2] = 1;!!!
            }                   /* if */
        }