cvepilines.cpp.svn-base

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

    {
        /* No angle */
        *result = 0;
        return;
    }

    /* ============= Computation for line 1 ================ */
    /* Find correspondence line for angle points11 */
    /* corr21 = Fund'*p1 */

    pointW11[0] = point11.x;
    pointW11[1] = point11.y;
    pointW11[2] = 1.0;

    icvTransformVector_64d( transFundMatr, /* !!! Modified from not transposed */
                            pointW11, 
                            corr21,
                            3,3);

    /* Find crossing of line with image 2 */
    CvPoint2D64d start;
    CvPoint2D64d end;
    int res;
    icvGetCrossRectDirect( imageSize,
                        corr21[0],corr21[1],corr21[2],
                        &start,&end,
                        &res);
    
    if( res == 0 )
    {/* We have not cross */
        /* We must define new angle */

        pointW21[0] = point21.x;
        pointW21[1] = point21.y;
        pointW21[2] = 1.0;

        /* Find correspondence line for this angle points */
        /* We know point and try to get corr line */
        /* For point21 */
        /* corr11 = Fund * p21 */

        icvTransformVector_64d( fundMatr, /* !!! Modified */
                                pointW21, 
                                corr11,
                                3,3);

        /* We have cross. And it's result cross for up line. Set result coefs */

        /* Set coefs for line 1 image 1 */
        coeff11[0] = corr11[0];
        coeff11[1] = corr11[1];
        coeff11[2] = corr11[2];
        
        /* Set coefs for line 1 image 2 */
        int res;

        icvGetCoefForPiece(    epipole2_2d,point21,
                            &coeff21[0],&coeff21[1],&coeff21[2],
                            &res);
        if( res == 0 )
        {
            *result = 0;
            return;/* Error */
        }
    }
    else
    {/* Line 1 cross image 2 */
        /* Set coefs for line 1 image 1 */
        int res;

        icvGetCoefForPiece(    epipole1_2d,point11,
                            &coeff11[0],&coeff11[1],&coeff11[2],
                            &res);
        if( res == 0 )
        {
            *result = 0;
            return;/* Error */
        }
        
        /* Set coefs for line 1 image 2 */
        coeff21[0] = corr21[0];
        coeff21[1] = corr21[1];
        coeff21[2] = corr21[2];
        
    }

    /* ============= Computation for line 2 ================ */
    /* Find correspondence line for angle points11 */
    /* corr22 = Fund*p2 */

    pointW12[0] = point12.x;
    pointW12[1] = point12.y;
    pointW12[2] = 1.0;

    icvTransformVector_64d( transFundMatr,
                            pointW12, 
                            corr22,
                            3,3);

    /* Find crossing of line with image 2 */
    icvGetCrossRectDirect( imageSize,
                        corr22[0],corr22[1],corr22[2],
                        &start,&end,
                        &res);
    
    if( res == 0 )
    {/* We have not cross */
        /* We must define new angle */

        pointW22[0] = point22.x;
        pointW22[1] = point22.y;
        pointW22[2] = 1.0;

        /* Find correspondence line for this angle points */
        /* We know point and try to get corr line */
        /* For point21 */
        /* corr2 = Fund' * p1 */

        icvTransformVector_64d( fundMatr,
                                pointW22, 
                                corr12,
                                3,3);

        
        /* We have cross. And it's result cross for down line. Set result coefs */

        /* Set coefs for line 2 image 1 */
        coeff12[0] = corr12[0];
        coeff12[1] = corr12[1];
        coeff12[2] = corr12[2];
        
        /* Set coefs for line 1 image 2 */
        int res;

        icvGetCoefForPiece(    epipole2_2d,point22,
                            &coeff22[0],&coeff22[1],&coeff22[2],
                            &res);
        if( res == 0 )
        {
            *result = 0;
            return;/* Error */
        }
    }
    else
    {/* Line 2 cross image 2 */
        /* Set coefs for line 2 image 1 */
        int res;

        icvGetCoefForPiece(    epipole1_2d,point12,
                            &coeff12[0],&coeff12[1],&coeff12[2],
                            &res);
        if( res == 0 )
        {
            *result = 0;
            return;/* Error */
        }
        
        /* Set coefs for line 1 image 2 */
        coeff22[0] = corr22[0];
        coeff22[1] = corr22[1];
        coeff22[2] = corr22[2];
        
    }

    /* Now we know common area */

    return;

}/* GetCommonArea */

/*---------------------------------------------------------------------------------------*/

/* Get cross for direction1 and direction2 */
/*  Result = 1 - cross */
/*  Result = 2 - parallel and not equal */
/*  Result = 3 - parallel and equal */

void icvGetCrossDirectDirect(  CvVect64d direct1,CvVect64d direct2,
                            CvPoint2D64d *cross,int* result)
{
    double det  = direct1[0]*direct2[1] - direct2[0]*direct1[1];
    double detx = -direct1[2]*direct2[1] + direct1[1]*direct2[2];

    if( fabs(det) > EPS64D )
    {/* Have cross */
        cross->x = detx/det;
        cross->y = (-direct1[0]*direct2[2] + direct2[0]*direct1[2])/det;
        *result = 1;
    }
    else
    {/* may be parallel */
        if( fabs(detx) > EPS64D )
        {/* parallel and not equal */
            *result = 2;
        }
        else
        {/* equals */
            *result = 3;
        }
    }

    return;
}

/*---------------------------------------------------------------------------------------*/

/* Get cross for piece p1,p2 and direction a,b,c */
/*  Result = 0 - no cross */
/*  Result = 1 - cross */
/*  Result = 2 - parallel and not equal */
/*  Result = 3 - parallel and equal */

void icvGetCrossPieceDirect(   CvPoint2D64d p_start,CvPoint2D64d p_end,
                            double a,double b,double c,
                            CvPoint2D64d *cross,int* result)
{

    if( (a*p_start.x + b*p_start.y + c) * (a*p_end.x + b*p_end.y + c) <= 0 )
    {/* Have cross */
        double det;
        double detxc,detyc;
        
        det = a * (p_end.x - p_start.x) + b * (p_end.y - p_start.y);
        
        if( fabs(det) < EPS64D )
        {/* lines are parallel and may be equal or line is point */
            if(  fabs(a*p_start.x + b*p_start.y + c) < EPS64D )
            {/* line is point or not diff */
                *result = 3;
                return;
            }
            else
            {
                *result = 2;                
            }
            return;
        }

        detxc = b*(p_end.y*p_start.x - p_start.y*p_end.x) + c*(p_start.x - p_end.x);
        detyc = a*(p_end.x*p_start.y - p_start.x*p_end.y) + c*(p_start.y - p_end.y);

        cross->x = detxc / det;
        cross->y = detyc / det;
        *result = 1;

    }
    else
    {
        *result = 0;
    }
    return;
}
/*--------------------------------------------------------------------------------------*/

void icvGetCrossPiecePiece( CvPoint2D64d p1_start,CvPoint2D64d p1_end,
                            CvPoint2D64d p2_start,CvPoint2D64d p2_end,
                            CvPoint2D64d* cross,
                            int* result)
{
    double ex1,ey1,ex2,ey2;
    double px1,py1,px2,py2;
    double del;
    double delA,delB,delX,delY;
    double alpha,betta;

    ex1 = p1_start.x;
    ey1 = p1_start.y;
    ex2 = p1_end.x;
    ey2 = p1_end.y;

    px1 = p2_start.x;
    py1 = p2_start.y;
    px2 = p2_end.x;
    py2 = p2_end.y;

    del = (py1-py2)*(ex1-ex2)-(px1-px2)*(ey1-ey2);
    if( fabs(del) <= EPS64D )
    {/* May be they are parallel !!! */
        *result = 0;
        return;
    }

    delA =  (ey1-ey2)*(ex1-px1) + (ex1-ex2)*(py1-ey1);
    delB =  (py1-py2)*(ex1-px1) + (px1-px2)*(py1-ey1);

    alpha = delA / del;
    betta = delB / del;

    if( alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0)
    {
        *result = 0;
        return;
    }

    delX =  (px1-px2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2))+
            (ex1-ex2)*(px1*(py1-py2)-py1*(px1-px2));

    delY =  (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2))+
            (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2));

    cross->x = delX / del;
    cross->y = delY / del;
    
    *result = 1;
    return;
}


/*---------------------------------------------------------------------------------------*/

void icvGetPieceLength(CvPoint2D64d point1,CvPoint2D64d point2,double* dist)
{
    double dx = point2.x - point1.x;
    double dy = point2.y - point1.y;
    *dist = sqrt( dx*dx + dy*dy );
    return;
}

/*---------------------------------------------------------------------------------------*/

void icvGetPieceLength3D(CvPoint3D64d point1,CvPoint3D64d point2,double* dist)
{
    double dx = point2.x - point1.x;
    double dy = point2.y - point1.y;
    double dz = point2.z - point1.z;
    *dist = sqrt( dx*dx + dy*dy + dz*dz );
    return;
}

/*---------------------------------------------------------------------------------------*/

/* Find line from epipole which cross image rect */
/* Find points of cross 0 or 1 or 2. Return number of points in cross */
void icvGetCrossRectDirect(    CvSize imageSize,
                            double a,double b,double c,
                            CvPoint2D64d *start,CvPoint2D64d *end,
                            int* result)
{
    CvPoint2D64d frameBeg;
    CvPoint2D64d frameEnd;
    CvPoint2D64d cross[4];
    int     haveCross[4];
    
    haveCross[0] = 0;
    haveCross[1] = 0;
    haveCross[2] = 0;
    haveCross[3] = 0;

    frameBeg.x = 0;
    frameBeg.y = 0;
    frameEnd.x = imageSize.width;
    frameEnd.y = 0;

    icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[0],&haveCross[0]);    
    
    frameBeg.x = imageSize.width;
    frameBeg.y = 0;
    frameEnd.x = imageSize.width;
    frameEnd.y = imageSize.height;
    icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[1],&haveCross[1]);    

    frameBeg.x = imageSize.width;
    frameBeg.y = imageSize.height;
    frameEnd.x = 0;
    frameEnd.y = imageSize.height;
    icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[2],&haveCross[2]);    

    frameBeg.x = 0;
    frameBeg.y = imageSize.height;
    frameEnd.x = 0;
    frameEnd.y = 0;
    icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[3],&haveCross[3]);    

    double maxDist;

    int maxI=0,maxJ=0;


    int i,j;

    maxDist = -1.0;
    
    double distance;

    for( i = 0; i < 3; i++ )
    {
        if( haveCross[i] == 1 )
        {
            for( j = i + 1; j < 4; j++ )
            {
                if( haveCross[j] == 1)
                {/* Compute dist */
                    icvGetPieceLength(cross[i],cross[j],&distance);
                    if( distance > maxDist )
                    {
                        maxI = i;
                        maxJ = j;
                        maxDist = distance;
                    }
                }
            }
        }
    }

    if( maxDist >= 0 )
    {/* We have cross */
        *start = cross[maxI];
        *result = 1;
        if( maxDist > 0 )
        {
            *end   = cross[maxJ];
            *result = 2;
        }
    }
    else
    {
        *result = 0;
    }

    return;
}/* GetCrossRectDirect */

/*---------------------------------------------------------------------------------------*/