cvepilines.cpp.svn-base

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

    if( postype == 0 )
    {
        icvGetSymPoint3D(   *((CvPoint3D64d*)camPoint1),
                            *((CvPoint3D64d*)direct1),
                            *((CvPoint3D64d*)pointB),
                            &pointNewA);

        icvGetSymPoint3D(   *((CvPoint3D64d*)camPoint2),
                            *((CvPoint3D64d*)direct4),
                            *((CvPoint3D64d*)pointB),
                            &pointNewC);
    }
    else
    {/* In this case we must change cameras */
        *needSwapCamera = 1;
        icvGetSymPoint3D(   *((CvPoint3D64d*)camPoint2),
                            *((CvPoint3D64d*)direct3),
                            *((CvPoint3D64d*)pointB),
                            &pointNewA);

        icvGetSymPoint3D(   *((CvPoint3D64d*)camPoint1),
                            *((CvPoint3D64d*)direct2),
                            *((CvPoint3D64d*)pointB),
                            &pointNewC);
    }


    double gamma;
    
    double x1,y1,z1;

    x1 = camPoint1[0];
    y1 = camPoint1[1];
    z1 = camPoint1[2];

    double xA,yA,zA;
    double xB,yB,zB;
    double xC,yC,zC;

    xA = pointNewA.x;
    yA = pointNewA.y;
    zA = pointNewA.z;

    xB = pointB[0];
    yB = pointB[1];
    zB = pointB[2];

    xC = pointNewC.x;
    yC = pointNewC.y;
    zC = pointNewC.z;

    double len1,len2;
    len1 = sqrt( (xA-xB)*(xA-xB) + (yA-yB)*(yA-yB) + (zA-zB)*(zA-zB) );
    len2 = sqrt( (xB-xC)*(xB-xC) + (yB-yC)*(yB-yC) + (zB-zC)*(zB-zC) );
    gamma = len2 / len1;

    icvComputeStereoLineCoeffs( pointNewA,
                                *((CvPoint3D64d*)pointB),
                                *((CvPoint3D64d*)camPoint1),
                                gamma,
                                coeffs);
    
    return CV_NO_ERR;
}


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

CvStatus icvGetDirectionForPoint(  CvPoint2D64d point,
                                CvMatr64d camMatr,
                                CvPoint3D64d* direct)
{
    /*  */
    double invMatr[9];
    
    /* Invert matrix */

    icvInvertMatrix_64d(camMatr,3,invMatr);
    /* TEST FOR ERRORS */

    double vect[3];
    vect[0] = point.x;
    vect[1] = point.y;
    vect[2] = 1;

    /* Mul matr */
    icvMulMatrix_64d(   invMatr,
                        3,3,
                        vect,
                        1,3, 
                        (double*)direct);

    return CV_NO_ERR;    
}

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

CvStatus icvGetCrossLines(CvPoint3D64d point11,CvPoint3D64d point12,
                       CvPoint3D64d point21,CvPoint3D64d point22,
                       CvPoint3D64d* midPoint)
{
    double xM,yM,zM;
    double xN,yN,zN;

    double xA,yA,zA;
    double xB,yB,zB;

    double xC,yC,zC;
    double xD,yD,zD;

    xA = point11.x;
    yA = point11.y;
    zA = point11.z;

    xB = point12.x;
    yB = point12.y;
    zB = point12.z;

    xC = point21.x;
    yC = point21.y;
    zC = point21.z;

    xD = point22.x;
    yD = point22.y;
    zD = point22.z;

    double a11,a12,a21,a22;
    double b1,b2;

    a11 =  (xB-xA)*(xB-xA)+(yB-yA)*(yB-yA)+(zB-zA)*(zB-zA);
    a12 = -(xD-xC)*(xB-xA)-(yD-yC)*(yB-yA)-(zD-zC)*(zB-zA);
    a21 =  (xB-xA)*(xD-xC)+(yB-yA)*(yD-yC)+(zB-zA)*(zD-zC);
    a22 = -(xD-xC)*(xD-xC)-(yD-yC)*(yD-yC)-(zD-zC)*(zD-zC);
    b1  = -( (xA-xC)*(xB-xA)+(yA-yC)*(yB-yA)+(zA-zC)*(zB-zA) );
    b2  = -( (xA-xC)*(xD-xC)+(yA-yC)*(yD-yC)+(zA-zC)*(zD-zC) );

    double delta;
    double deltaA,deltaB;
    double alpha,betta;

    delta  = a11*a22-a12*a21;
    
    if( fabs(delta) < EPS64D )
    {
        /*return ERROR;*/
    }

    deltaA = b1*a22-b2*a12;
    deltaB = a11*b2-b1*a21;

    alpha = deltaA / delta;
    betta = deltaB / delta;

    xM = xA+alpha*(xB-xA);
    yM = yA+alpha*(yB-yA);
    zM = zA+alpha*(zB-zA);

    xN = xC+betta*(xD-xC);
    yN = yC+betta*(yD-yC);
    zN = zC+betta*(zD-zC);

    /* Compute middle point */
    midPoint->x = (xM + xN) * 0.5;
    midPoint->y = (yM + yN) * 0.5;
    midPoint->z = (zM + zN) * 0.5;

    return CV_NO_ERR;
}

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

CvStatus icvComputeStereoLineCoeffs(   CvPoint3D64d pointA,
                                    CvPoint3D64d pointB,
                                    CvPoint3D64d pointCam1,
                                    double gamma,
                                    CvStereoLineCoeff*    coeffs)
{
    double x1,y1,z1;

    x1 = pointCam1.x;
    y1 = pointCam1.y;
    z1 = pointCam1.z;

    double xA,yA,zA;
    double xB,yB,zB;

    xA = pointA.x;
    yA = pointA.y;
    zA = pointA.z;

    xB = pointB.x;
    yB = pointB.y;
    zB = pointB.z;

    if( gamma > 0 )
    {
        coeffs->Xcoef   = -x1 + xA;
        coeffs->XcoefA  =  xB + x1 - xA;
        coeffs->XcoefB  = -xA - gamma * x1 + gamma * xA;
        coeffs->XcoefAB = -xB + xA + gamma * xB - gamma * xA;

        coeffs->Ycoef   = -y1 + yA;
        coeffs->YcoefA  =  yB + y1 - yA;
        coeffs->YcoefB  = -yA - gamma * y1 + gamma * yA;
        coeffs->YcoefAB = -yB + yA + gamma * yB - gamma * yA;

        coeffs->Zcoef   = -z1 + zA;
        coeffs->ZcoefA  =  zB + z1 - zA;
        coeffs->ZcoefB  = -zA - gamma * z1 + gamma * zA;
        coeffs->ZcoefAB = -zB + zA + gamma * zB - gamma * zA;
    }
    else
    {
        gamma = - gamma;
        coeffs->Xcoef   = -( -x1 + xA);
        coeffs->XcoefB  = -(  xB + x1 - xA);
        coeffs->XcoefA  = -( -xA - gamma * x1 + gamma * xA);
        coeffs->XcoefAB = -( -xB + xA + gamma * xB - gamma * xA);

        coeffs->Ycoef   = -( -y1 + yA);
        coeffs->YcoefB  = -(  yB + y1 - yA);
        coeffs->YcoefA  = -( -yA - gamma * y1 + gamma * yA);
        coeffs->YcoefAB = -( -yB + yA + gamma * yB - gamma * yA);

        coeffs->Zcoef   = -( -z1 + zA);
        coeffs->ZcoefB  = -(  zB + z1 - zA);
        coeffs->ZcoefA  = -( -zA - gamma * z1 + gamma * zA);
        coeffs->ZcoefAB = -( -zB + zA + gamma * zB - gamma * zA);
    }



    return CV_NO_ERR;
}
/*--------------------------------------------------------------------------------------*/


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

/* This function get minimum angle started at point which contains rect */
int icvGetAngleLine( CvPoint2D64d startPoint, CvSize imageSize,CvPoint2D64d *point1,CvPoint2D64d *point2)
{
    /* Get crosslines with image corners */

    /* Find four lines */

    CvPoint2D64d pa,pb,pc,pd;
    
    pa.x = 0;
    pa.y = 0;

    pb.x = imageSize.width-1;
    pb.y = 0;

    pd.x = imageSize.width-1;
    pd.y = imageSize.height-1;

    pc.x = 0;
    pc.y = imageSize.height-1;
    
    /* We can compute points for angle */
    /* Test for place section */
    
    if( startPoint.x < 0 )
    {/* 1,4,7 */
        if( startPoint.y < 0)
        {/* 1 */
            *point1 = pb;
            *point2 = pc;
        }
        else if( startPoint.y > imageSize.height-1 )
        {/* 7 */
            *point1 = pa;
            *point2 = pd;
        }
        else
        {/* 4 */
            *point1 = pa;
            *point2 = pc;
        }
    }
    else if ( startPoint.x > imageSize.width-1 )
    {/* 3,6,9 */
        if( startPoint.y < 0 )
        {/* 3 */
            *point1 = pa;
            *point2 = pd;
        }
        else if ( startPoint.y > imageSize.height-1 )
        {/* 9 */
            *point1 = pb;
            *point2 = pc;
        }
        else
        {/* 6 */
            *point1 = pb;
            *point2 = pd;
        }
    }
    else
    {/* 2,5,8 */
        if( startPoint.y < 0 )
        {/* 2 */
            if( startPoint.x < imageSize.width/2 )
            {
                *point1 = pb;
                *point2 = pa;
            }
            else
            {
                *point1 = pa;
                *point2 = pb;
            }
        }
        else if( startPoint.y > imageSize.height-1 )
        {/* 8 */
            if( startPoint.x < imageSize.width/2 )
            {
                *point1 = pc;
                *point2 = pd;
            }
            else
            {
                *point1 = pd;
                *point2 = pc;
            }
        }
        else
        {/* 5 - point in the image */
            return 2;
        }
    }
    return 0;
}/* GetAngleLine */

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

void icvGetCoefForPiece(   CvPoint2D64d p_start,CvPoint2D64d p_end,
                        double *a,double *b,double *c,
                        int* result)
{
    double det;
    double detA,detB,detC;

    det = p_start.x*p_end.y+p_end.x+p_start.y-p_end.y-p_start.y*p_end.x-p_start.x;
    if( fabs(det) < EPS64D)/* Error */
    {
        *result = 0;
        return;
    }

    detA = p_start.y - p_end.y;
    detB = p_end.x - p_start.x;
    detC = p_start.x*p_end.y - p_end.x*p_start.y;

    double invDet = 1.0 / det;
    *a = detA * invDet;
    *b = detB * invDet;
    *c = detC * invDet;

    *result = 1;
    return;
}

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

/* Get common area of rectifying */
void icvGetCommonArea( CvSize imageSize,
                    CvPoint3D64d epipole1,CvPoint3D64d epipole2,
                    CvMatr64d fundMatr,
                    CvVect64d coeff11,CvVect64d coeff12,
                    CvVect64d coeff21,CvVect64d coeff22,
                    int* result)
{
    CvPoint2D64d point11;
    CvPoint2D64d point12;
    CvPoint2D64d point21;
    CvPoint2D64d point22;

    double corr11[3];
    double corr12[3];
    double corr21[3];
    double corr22[3];

    double pointW11[3];
    double pointW12[3];
    double pointW21[3];
    double pointW22[3];

    double transFundMatr[3*3];
    /* Compute transpose of fundamental matrix */
    icvTransposeMatrix_64d( fundMatr, 3, 3, transFundMatr );
    
    CvPoint2D64d epipole1_2d;
    CvPoint2D64d epipole2_2d;
    
    if( fabs(epipole1.z) < 1e-8 )
    {/* epipole1 in infinity */
        *result = 0;
        return;
    }
    epipole1_2d.x = epipole1.x / epipole1.z;
    epipole1_2d.y = epipole1.y / epipole1.z;

    if( fabs(epipole2.z) < 1e-8 )
    {/* epipole2 in infinity */
        *result = 0;
        return;
    }
    epipole2_2d.x = epipole2.x / epipole2.z;
    epipole2_2d.y = epipole2.y / epipole2.z;

    int stat = icvGetAngleLine( epipole1_2d, imageSize,&point11,&point12);
    if( stat == 2 )
    {
        /* No angle */
        *result = 0;
        return;
    }

    stat = icvGetAngleLine( epipole2_2d, imageSize,&point21,&point22);
    if( stat == 2 )