cvepilines.cpp.svn-base

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

void icvProjectPointToImage(   CvPoint3D64d point,
                            CvMatr64d camMatr,CvMatr64d rotMatr,CvVect64d transVect,
                            CvPoint2D64d* projPoint)
{

    double tmpVect1[3];
    double tmpVect2[3];
    
    icvMulMatrix_64d (  rotMatr,
                        3,3,
                        (double*)&point,
                        1,3,
                        tmpVect1);

    icvAddVector_64d ( tmpVect1, transVect,tmpVect2, 3);

    icvMulMatrix_64d (  camMatr,
                        3,3,
                        tmpVect2,
                        1,3,
                        tmpVect1);

    projPoint->x = tmpVect1[0] / tmpVect1[2];
    projPoint->y = tmpVect1[1] / tmpVect1[2];
   
    return;
}

/*---------------------------------------------------------------------------------------*/
/* Get quads for transform images */
void icvGetQuadsTransform( 
                          CvSize        imageSize,
                        CvMatr64d     camMatr1,
                        CvMatr64d     rotMatr1,
                        CvVect64d     transVect1,
                        CvMatr64d     camMatr2,
                        CvMatr64d     rotMatr2,
                        CvVect64d     transVect2,
                        CvSize*       warpSize,
                        double quad1[4][2],
                        double quad2[4][2],
                        CvMatr64d     fundMatr,
                        CvPoint3D64d* epipole1,
                        CvPoint3D64d* epipole2
                        )
{
    /* First compute fundamental matrix and epipoles */
    int res;


    /* Compute epipoles and fundamental matrix using new functions */
    {
        double convRotMatr[9];
        double convTransVect[3];

        icvCreateConvertMatrVect( rotMatr1,
                                  transVect1,
                                  rotMatr2,
                                  transVect2,
                                  convRotMatr,
                                  convTransVect);
        float convRotMatr_32f[9];
        float convTransVect_32f[3];

        icvCvt_64d_32f(convRotMatr,convRotMatr_32f,9);
        icvCvt_64d_32f(convTransVect,convTransVect_32f,3);

        /* We know R and t */
        /* Compute essential matrix */
        float essMatr[9];
        float fundMatr_32f[9];

        float camMatr1_32f[9];
        float camMatr2_32f[9];

        icvCvt_64d_32f(camMatr1,camMatr1_32f,9);
        icvCvt_64d_32f(camMatr2,camMatr2_32f,9);

        cvComputeEssentialMatrix(   convRotMatr_32f,
                                    convTransVect_32f,
                                    essMatr);

        cvConvertEssential2Fundamental( essMatr,
                                        fundMatr_32f,
                                        camMatr1_32f,
                                        camMatr2_32f);
        
        CvPoint3D32f epipole1_32f;
        CvPoint3D32f epipole2_32f;
        
        cvComputeEpipolesFromFundMatrix( fundMatr_32f,
                                         &epipole1_32f,
                                         &epipole2_32f);
        /* copy to 64d epipoles */
        epipole1->x = epipole1_32f.x;
        epipole1->y = epipole1_32f.y;
        epipole1->z = epipole1_32f.z;

        epipole2->x = epipole2_32f.x;
        epipole2->y = epipole2_32f.y;
        epipole2->z = epipole2_32f.z;
        
        /* Convert fundamental matrix */
        icvCvt_32f_64d(fundMatr_32f,fundMatr,9);
    }

    double coeff11[3];
    double coeff12[3];
    double coeff21[3];
    double coeff22[3];

    icvGetCommonArea(   imageSize,
                        *epipole1,*epipole2,
                        fundMatr,
                        coeff11,coeff12,
                        coeff21,coeff22,
                        &res);

    CvPoint2D64d point11, point12,point21, point22;
    double width1,width2;
    double height1,height2;
    double tmpHeight1,tmpHeight2;

    CvPoint2D64d epipole1_2d;
    CvPoint2D64d epipole2_2d;

    /* ----- Image 1 ----- */
    if( fabs(epipole1->z) < 1e-8 )
    {
        return;
    }
    epipole1_2d.x = epipole1->x / epipole1->z;
    epipole1_2d.y = epipole1->y / epipole1->z;

    icvGetCutPiece( coeff11,coeff12,
                epipole1_2d,
                imageSize,
                &point11,&point12,
                &point21,&point22,
                &res);

    /* Compute distance */
    icvGetPieceLength(point11,point21,&width1);
    icvGetPieceLength(point11,point12,&tmpHeight1);
    icvGetPieceLength(point21,point22,&tmpHeight2);
    height1 = MAX(tmpHeight1,tmpHeight2);

    quad1[0][0] = point11.x;
    quad1[0][1] = point11.y;

    quad1[1][0] = point21.x;
    quad1[1][1] = point21.y;

    quad1[2][0] = point22.x;
    quad1[2][1] = point22.y;

    quad1[3][0] = point12.x;
    quad1[3][1] = point12.y;

    /* ----- Image 2 ----- */
    if( fabs(epipole2->z) < 1e-8 )
    {
        return;
    }
    epipole2_2d.x = epipole2->x / epipole2->z;
    epipole2_2d.y = epipole2->y / epipole2->z;

    icvGetCutPiece( coeff21,coeff22,
                epipole2_2d,
                imageSize,
                &point11,&point12,
                &point21,&point22,
                &res);

    /* Compute distance */
    icvGetPieceLength(point11,point21,&width2);
    icvGetPieceLength(point11,point12,&tmpHeight1);
    icvGetPieceLength(point21,point22,&tmpHeight2);
    height2 = MAX(tmpHeight1,tmpHeight2);

    quad2[0][0] = point11.x;
    quad2[0][1] = point11.y;

    quad2[1][0] = point21.x;
    quad2[1][1] = point21.y;

    quad2[2][0] = point22.x;
    quad2[2][1] = point22.y;

    quad2[3][0] = point12.x;
    quad2[3][1] = point12.y;


    /*=======================================================*/
    /* This is a new additional way to compute quads. */
    /* We must correct quads */
    {
        double convRotMatr[9];
        double convTransVect[3];

        double newQuad1[4][2];
        double newQuad2[4][2];


        icvCreateConvertMatrVect( rotMatr1,
                                  transVect1,
                                  rotMatr2,
                                  transVect2,
                                  convRotMatr,
                                  convTransVect);

        /* -------------Compute for first image-------------- */
        CvPoint2D32f pointb1;
        CvPoint2D32f pointe1;
        
        CvPoint2D32f pointb2;
        CvPoint2D32f pointe2;

        pointb1.x = (float)quad1[0][0];
        pointb1.y = (float)quad1[0][1];

        pointe1.x = (float)quad1[3][0];
        pointe1.y = (float)quad1[3][1];

        icvComputeeInfiniteProject1(convRotMatr,
                                    camMatr1,
                                    camMatr2,
                                    pointb1,
                                    &pointb2);

        icvComputeeInfiniteProject1(convRotMatr,
                                    camMatr1,
                                    camMatr2,
                                    pointe1,
                                    &pointe2);

        /*  JUST TEST FOR POINT */

        /* Compute distances */
        double dxOld,dyOld;
        double dxNew,dyNew;
        double distOld,distNew;
        
        dxOld = quad2[1][0] - quad2[0][0];
        dyOld = quad2[1][1] - quad2[0][1];
        distOld = dxOld*dxOld + dyOld*dyOld;
        
        dxNew = quad2[1][0] - pointb2.x;
        dyNew = quad2[1][1] - pointb2.y;
        distNew = dxNew*dxNew + dyNew*dyNew;

        if( distNew > distOld )
        {/* Get new points for second quad */
            newQuad2[0][0] = pointb2.x;
            newQuad2[0][1] = pointb2.y;
            newQuad2[3][0] = pointe2.x;
            newQuad2[3][1] = pointe2.y;
            newQuad1[0][0] = quad1[0][0];
            newQuad1[0][1] = quad1[0][1];
            newQuad1[3][0] = quad1[3][0];
            newQuad1[3][1] = quad1[3][1];
        }
        else
        {/* Get new points for first quad */

            pointb2.x = (float)quad2[0][0];
            pointb2.y = (float)quad2[0][1];

            pointe2.x = (float)quad2[3][0];
            pointe2.y = (float)quad2[3][1];

            icvComputeeInfiniteProject2(convRotMatr,
                                        camMatr1,
                                        camMatr2,
                                        &pointb1,
                                        pointb2);

            icvComputeeInfiniteProject2(convRotMatr,
                                        camMatr1,
                                        camMatr2,
                                        &pointe1,
                                        pointe2);


            /*  JUST TEST FOR POINT */

            newQuad2[0][0] = quad2[0][0];
            newQuad2[0][1] = quad2[0][1];
            newQuad2[3][0] = quad2[3][0];
            newQuad2[3][1] = quad2[3][1];
            
            newQuad1[0][0] = pointb1.x;
            newQuad1[0][1] = pointb1.y;
            newQuad1[3][0] = pointe1.x;
            newQuad1[3][1] = pointe1.y;
        }

        /* -------------Compute for second image-------------- */
        pointb1.x = (float)quad1[1][0];
        pointb1.y = (float)quad1[1][1];

        pointe1.x = (float)quad1[2][0];
        pointe1.y = (float)quad1[2][1];

        icvComputeeInfiniteProject1(convRotMatr,
                                    camMatr1,
                                    camMatr2,
                                    pointb1,
                                    &pointb2);

        icvComputeeInfiniteProject1(convRotMatr,
                                    camMatr1,
                                    camMatr2,
                                    pointe1,
                                    &pointe2);

        /* Compute distances */
        
        dxOld = quad2[0][0] - quad2[1][0];
        dyOld = quad2[0][1] - quad2[1][1];
        distOld = dxOld*dxOld + dyOld*dyOld;
        
        dxNew = quad2[0][0] - pointb2.x;
        dyNew = quad2[0][1] - pointb2.y;
        distNew = dxNew*dxNew + dyNew*dyNew;

        if( distNew > distOld )
        {/* Get new points for second quad */
            newQuad2[1][0] = pointb2.x;
            newQuad2[1][1] = pointb2.y;
            newQuad2[2][0] = pointe2.x;
            newQuad2[2][1] = pointe2.y;
            newQuad1[1][0] = quad1[1][0];
            newQuad1[1][1] = quad1[1][1];
            newQuad1[2][0] = quad1[2][0];
            newQuad1[2][1] = quad1[2][1];
        }
        else
        {/* Get new points for first quad */

            pointb2.x = (float)quad2[1][0];
            pointb2.y = (float)quad2[1][1];

            pointe2.x = (float)quad2[2][0];
            pointe2.y = (float)quad2[2][1];

            icvComputeeInfiniteProject2(convRotMatr,
                                        camMatr1,
                                        camMatr2,
                                        &pointb1,
                                        pointb2);

            icvComputeeInfiniteProject2(convRotMatr,
                                        camMatr1,
                                        camMatr2,
                                        &pointe1,
                                        pointe2);

            newQuad2[1][0] = quad2[1][0];
            newQuad2[1][1] = quad2[1][1];
            newQuad2[2][0] = quad2[2][0];
            newQuad2[2][1] = quad2[2][1];
            
            newQuad1[1][0] = pointb1.x;
            newQuad1[1][1] = pointb1.y;
            newQuad1[2][0] = pointe1.x;
            newQuad1[2][1] = pointe1.y;
        }



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

        /* Copy new quads to old quad */
        int i;
        for( i = 0; i < 4; i++ )
        {
            {
                quad1[i][0] = newQuad1[i][0];
                quad1[i][1] = newQuad1[i][1];
                quad2[i][0] = newQuad2[i][0];
                quad2[i][1] = newQuad2[i][1];
            }
        }
    }
    /*=======================================================*/

    double warpWidth,warpHeight;

    warpWidth  = MAX(width1,width2);
    warpHeight = MAX(height1,height2);

    warpSize->width  = (int)warpWidth;
    warpSize->height = (int)warpHeight;

    warpSize->width  = cvRound(warpWidth-1);
    warpSize->height = cvRound(warpHeight-1);

/* !!! by Valery Mosyagin. this lines added just for test no warp */
    warpSize->width  = imageSize.width;
    warpSize->height = imageSize.height;

    return;
}


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

void icvGetQuadsTransformNew(  CvSize        imageSize,
                            CvMatr32f     camMatr1,
                            CvMatr32f     camMatr2,
                            CvMatr32f     rotMatr1,
                            CvVect32f     transVect1,
                            CvSize*       warpSize,
                            double        quad1[4][2],
                            double        quad2[4][2],
                            CvMatr32f     fundMatr,
                            CvPoint3D32f* epipole1,
                            CvPoint3D32f* epipole2
                        )
{
    /* Convert data */
    /* Convert camera matrix */