cvepilines.cpp.svn-base

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

    double camMatr1_64d[9];
    double camMatr2_64d[9];
    double rotMatr1_64d[9];
    double transVect1_64d[3];
    double rotMatr2_64d[9];
    double transVect2_64d[3];
    double fundMatr_64d[9];
    CvPoint3D64d epipole1_64d;
    CvPoint3D64d epipole2_64d;

    icvCvt_32f_64d(camMatr1,camMatr1_64d,9);
    icvCvt_32f_64d(camMatr2,camMatr2_64d,9);
    icvCvt_32f_64d(rotMatr1,rotMatr1_64d,9);
    icvCvt_32f_64d(transVect1,transVect1_64d,3);

    /* Create vector and matrix */

    rotMatr2_64d[0] = 1;
    rotMatr2_64d[1] = 0;
    rotMatr2_64d[2] = 0;
    rotMatr2_64d[3] = 0;
    rotMatr2_64d[4] = 1;
    rotMatr2_64d[5] = 0;
    rotMatr2_64d[6] = 0;
    rotMatr2_64d[7] = 0;
    rotMatr2_64d[8] = 1;

    transVect2_64d[0] = 0;
    transVect2_64d[1] = 0;
    transVect2_64d[2] = 0;

    icvGetQuadsTransform(   imageSize,
                            camMatr1_64d,
                            rotMatr1_64d,
                            transVect1_64d,
                            camMatr2_64d,
                            rotMatr2_64d,
                            transVect2_64d,
                            warpSize,
                            quad1,
                            quad2,
                            fundMatr_64d,
                            &epipole1_64d,
                            &epipole2_64d
                        );

    /* Convert epipoles */
    epipole1->x = (float)(epipole1_64d.x);
    epipole1->y = (float)(epipole1_64d.y);
    epipole1->z = (float)(epipole1_64d.z);

    epipole2->x = (float)(epipole2_64d.x);
    epipole2->y = (float)(epipole2_64d.y);
    epipole2->z = (float)(epipole2_64d.z);

    /* Convert fundamental matrix */
    icvCvt_64d_32f(fundMatr_64d,fundMatr,9);
    
    return;
}

/*---------------------------------------------------------------------------------------*/
void icvGetQuadsTransformStruct(  CvStereoCamera* stereoCamera)
{
    /* Wrapper for icvGetQuadsTransformNew */


    double  quad1[4][2];
    double  quad2[4][2];

    icvGetQuadsTransformNew(     cvSize(cvRound(stereoCamera->camera[0]->imgSize[0]),cvRound(stereoCamera->camera[0]->imgSize[1])),
                            stereoCamera->camera[0]->matrix,
                            stereoCamera->camera[1]->matrix,
                            stereoCamera->rotMatrix,
                            stereoCamera->transVector,
                            &(stereoCamera->warpSize),
                            quad1,
                            quad2,
                            stereoCamera->fundMatr,
                            &(stereoCamera->epipole[0]),
                            &(stereoCamera->epipole[1])
                        );

    int i;
    for( i = 0; i < 4; i++ )
    {
        stereoCamera->quad[0][i] = cvPoint2D32f(quad1[i][0],quad1[i][1]);
        stereoCamera->quad[1][i] = cvPoint2D32f(quad2[i][0],quad2[i][1]);
    }

    return;
}

/*---------------------------------------------------------------------------------------*/
void icvComputeStereoParamsForCameras(CvStereoCamera* stereoCamera)
{
    /* For given intrinsic and extrinsic parameters computes rest parameters 
    **   such as fundamental matrix. warping coeffs, epipoles, ...
    */


    /* compute rotate matrix and translate vector */
    double rotMatr1[9];
    double rotMatr2[9];

    double transVect1[3];
    double transVect2[3];

    double convRotMatr[9];
    double convTransVect[3];

    /* fill matrices */
    icvCvt_32f_64d(stereoCamera->camera[0]->rotMatr,rotMatr1,9);
    icvCvt_32f_64d(stereoCamera->camera[1]->rotMatr,rotMatr2,9);

    icvCvt_32f_64d(stereoCamera->camera[0]->transVect,transVect1,3);
    icvCvt_32f_64d(stereoCamera->camera[1]->transVect,transVect2,3);
        
    icvCreateConvertMatrVect(   rotMatr1,
                                transVect1,
                                rotMatr2,
                                transVect2,
                                convRotMatr,
                                convTransVect);
    
    /* copy to stereo camera params */
    icvCvt_64d_32f(convRotMatr,stereoCamera->rotMatrix,9);
    icvCvt_64d_32f(convTransVect,stereoCamera->transVector,3);


    icvGetQuadsTransformStruct(stereoCamera);
    icvComputeRestStereoParams(stereoCamera);
}



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

/* Get cut line for one image */
void icvGetCutPiece(   CvVect64d areaLineCoef1,CvVect64d areaLineCoef2,
                    CvPoint2D64d epipole,
                    CvSize imageSize,
                    CvPoint2D64d* point11,CvPoint2D64d* point12,
                    CvPoint2D64d* point21,CvPoint2D64d* point22,
                    int* result)
{
    /* Compute nearest cut line to epipole */
    /* Get corners inside sector */
    /* Collect all candidate point */

    CvPoint2D64d candPoints[8];
    CvPoint2D64d midPoint;
    int numPoints = 0;
    int res;
    int i;

    double cutLine1[3];
    double cutLine2[3];

    /* Find middle line of sector */
    double midLine[3];

    
    /* Different way  */
    CvPoint2D64d pointOnLine1;  pointOnLine1.x = pointOnLine1.y = 0;
    CvPoint2D64d pointOnLine2;  pointOnLine2.x = pointOnLine2.y = 0;

    CvPoint2D64d start1,end1;

    icvGetCrossRectDirect( imageSize,
                        areaLineCoef1[0],areaLineCoef1[1],areaLineCoef1[2],
                        &start1,&end1,&res);
    if( res > 0 )
    {
        pointOnLine1 = start1;
    }

    icvGetCrossRectDirect( imageSize,
                        areaLineCoef2[0],areaLineCoef2[1],areaLineCoef2[2],
                        &start1,&end1,&res);
    if( res > 0 )
    {
        pointOnLine2 = start1;
    }

    icvGetMiddleAnglePoint(epipole,pointOnLine1,pointOnLine2,&midPoint);

    icvGetCoefForPiece(epipole,midPoint,&midLine[0],&midLine[1],&midLine[2],&res);

    /* Test corner points */
    CvPoint2D64d cornerPoint;
    CvPoint2D64d tmpPoints[2];

    cornerPoint.x = 0;
    cornerPoint.y = 0;
    icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
    if( res == 1 )
    {/* Add point */
        candPoints[numPoints] = cornerPoint;
        numPoints++;
    }

    cornerPoint.x = imageSize.width;
    cornerPoint.y = 0;
    icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
    if( res == 1 )
    {/* Add point */
        candPoints[numPoints] = cornerPoint;
        numPoints++;
    }
    
    cornerPoint.x = imageSize.width;
    cornerPoint.y = imageSize.height;
    icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
    if( res == 1 )
    {/* Add point */
        candPoints[numPoints] = cornerPoint;
        numPoints++;
    }

    cornerPoint.x = 0;
    cornerPoint.y = imageSize.height;
    icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
    if( res == 1 )
    {/* Add point */
        candPoints[numPoints] = cornerPoint;
        numPoints++;
    }

    /* Find cross line 1 with image border */
    icvGetCrossRectDirect( imageSize,
                        areaLineCoef1[0],areaLineCoef1[1],areaLineCoef1[2],
                        &tmpPoints[0], &tmpPoints[1],
                        &res);
    for( i = 0; i < res; i++ )
    {
        candPoints[numPoints++] = tmpPoints[i];
    }

    /* Find cross line 2 with image border */
    icvGetCrossRectDirect( imageSize,
                        areaLineCoef2[0],areaLineCoef2[1],areaLineCoef2[2],
                        &tmpPoints[0], &tmpPoints[1],
                        &res);
    
    for( i = 0; i < res; i++ )
    {
        candPoints[numPoints++] = tmpPoints[i];
    }

    if( numPoints < 2 )
    {
        *result = 0;
        return;/* Error. Not enought points */
    }
    /* Project all points to middle line and get max and min */

    CvPoint2D64d projPoint;
    CvPoint2D64d minPoint; minPoint.x = minPoint.y = FLT_MAX;
    CvPoint2D64d maxPoint; maxPoint.x = maxPoint.y = -FLT_MAX;


    double dist;
    double maxDist = 0;
    double minDist = 10000000;

    
    for( i = 0; i < numPoints; i++ )
    {
        icvProjectPointToDirect(candPoints[i], midLine, &projPoint);
        icvGetPieceLength(epipole,projPoint,&dist);
        if( dist < minDist)
        {
            minDist = dist;
            minPoint = projPoint;
        }

        if( dist > maxDist)
        {
            maxDist = dist;
            maxPoint = projPoint;
        }
    }

    /* We know maximum and minimum points. Now we can compute cut lines */
    
    icvGetNormalDirect(midLine,minPoint,cutLine1);
    icvGetNormalDirect(midLine,maxPoint,cutLine2);

    /* Test for begin of line. */
    CvPoint2D64d tmpPoint2;

    /* Get cross with */
    icvGetCrossDirectDirect(areaLineCoef1,cutLine1,point11,&res);
    icvGetCrossDirectDirect(areaLineCoef2,cutLine1,point12,&res);

    icvGetCrossDirectDirect(areaLineCoef1,cutLine2,point21,&res);
    icvGetCrossDirectDirect(areaLineCoef2,cutLine2,point22,&res);

    if( epipole.x > imageSize.width * 0.5 )
    {/* Need to change points */
        tmpPoint2 = *point11;
        *point11 = *point21;
        *point21 = tmpPoint2;

        tmpPoint2 = *point12;
        *point12 = *point22;
        *point22 = tmpPoint2;
    }

    return;
}
/*---------------------------------------------------------------------------------------*/
/* Get middle angle */
void icvGetMiddleAnglePoint(   CvPoint2D64d basePoint,
                            CvPoint2D64d point1,CvPoint2D64d point2,
                            CvPoint2D64d* midPoint)
{/* !!! May be need to return error */
    
    double dist1;
    double dist2;
    icvGetPieceLength(basePoint,point1,&dist1);
    icvGetPieceLength(basePoint,point2,&dist2);
    CvPoint2D64d pointNew1;
    CvPoint2D64d pointNew2;
    double alpha = dist2/dist1;

    pointNew1.x = basePoint.x + (1.0/alpha) * ( point2.x - basePoint.x );
    pointNew1.y = basePoint.y + (1.0/alpha) * ( point2.y - basePoint.y );

    pointNew2.x = basePoint.x + alpha * ( point1.x - basePoint.x );
    pointNew2.y = basePoint.y + alpha * ( point1.y - basePoint.y );

    int res;
    icvGetCrossPiecePiece(point1,point2,pointNew1,pointNew2,midPoint,&res);

    return;
}

/*---------------------------------------------------------------------------------------*/
/* Get normal direct to direct in line */
void icvGetNormalDirect(CvVect64d direct,CvPoint2D64d point,CvVect64d normDirect)
{
    normDirect[0] =   direct[1];
    normDirect[1] = - direct[0];
    normDirect[2] = -(normDirect[0]*point.x + normDirect[1]*point.y);  
    return;
}

/*---------------------------------------------------------------------------------------*/
CV_IMPL double icvGetVect(CvPoint2D64d basePoint,CvPoint2D64d point1,CvPoint2D64d point2)
{
    return  (point1.x - basePoint.x)*(point2.y - basePoint.y) -
            (point2.x - basePoint.x)*(point1.y - basePoint.y);
}
/*---------------------------------------------------------------------------------------*/
/* Test for point in sector           */
/* Return 0 - point not inside sector */
/* Return 1 - point inside sector     */
void icvTestPoint( CvPoint2D64d testPoint,
                CvVect64d line1,CvVect64d line2,
                CvPoint2D64d basePoint,
                int* result)
{
    CvPoint2D64d point1,point2;

    icvProjectPointToDirect(testPoint,line1,&point1);
    icvProjectPointToDirect(testPoint,line2,&point2);

    double sign1 = icvGetVect(basePoint,point1,point2);
    double sign2 = icvGetVect(basePoint,point1,testPoint);
    if( sign1 * sign2 > 0 )
    {/* Correct for first line */
        sign1 = - sign1;
        sign2 = icvGetVect(basePoint,point2,testPoint);
        if( sign1 * sign2 > 0 )
        {/* Correct for both lines */
            *result = 1;
        }
        else
        {
            *result = 0;
        }
    }
    else
    {
        *result = 0;
    }
    
    return;
}

/*---------------------------------------------------------------------------------------*/
/* Project point to line */
void icvProjectPointToDirect(  CvPoint2D64d point,CvVect64d lineCoeff,
                            CvPoint2D64d* projectPoint)
{
    double a = lineCoeff[0];
    double b = lineCoeff[1];
    
    double det =  1.0 / ( a*a + b*b );
    double delta =  a*point.y - b*point.x;

    projectPoint->x = ( -a*lineCoeff[2] - b * delta ) * det;
    projectPoint->y = ( -b*lineCoeff[2] + a * delta ) * det ;

    return;
}

/*---------------------------------------------------------------------------------------*/
/* Get distance from point to direction */
void icvGetDistanceFromPointToDirect( CvPoint2D64d point,CvVect64d lineCoef,double*dist)
{
    CvPoint2D64d tmpPoint;
    icvProjectPointToDirect(point,lineCoef,&tmpPoint);
    double dx = point.x - tmpPoint.x;
    double dy = point.y - tmpPoint.y;
    *dist = sqrt(dx*dx+dy*dy);
    return;
}
/*---------------------------------------------------------------------------------------*/

CV_IMPL IplIma