epilines.cpp

/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/


#include "precomp.hpp"
#include <float.h>
#include <limits.h>

/* Valery Mosyagin */

#undef quad

#define EPS64D 1e-9

int cvComputeEssentialMatrix(  CvMatr32f rotMatr,
                                    CvMatr32f transVect,
                                    CvMatr32f essMatr);

int cvConvertEssential2Fundamental( CvMatr32f essMatr,
                                         CvMatr32f fundMatr,
                                         CvMatr32f cameraMatr1,
                                         CvMatr32f cameraMatr2);

int cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr,
                                         CvPoint3D32f* epipole1,
                                         CvPoint3D32f* epipole2);

void icvTestPoint( CvPoint2D64d testPoint,
                CvVect64d line1,CvVect64d line2,
                CvPoint2D64d basePoint,
                int* result);



int icvGetSymPoint3D(  CvPoint3D64d pointCorner,
                            CvPoint3D64d point1,
                            CvPoint3D64d point2,
                            CvPoint3D64d *pointSym2)
{
    double len1,len2;
    double alpha;
    icvGetPieceLength3D(pointCorner,point1,&len1);
    if( len1 < EPS64D )
    {
        return CV_BADARG_ERR;
    }
    icvGetPieceLength3D(pointCorner,point2,&len2);
    alpha = len2 / len1;

    pointSym2->x = pointCorner.x + alpha*(point1.x - pointCorner.x);
    pointSym2->y = pointCorner.y + alpha*(point1.y - pointCorner.y);
    pointSym2->z = pointCorner.z + alpha*(point1.z - pointCorner.z);
    return CV_NO_ERR;
}

/*  author Valery Mosyagin */

/* Compute 3D point for scanline and alpha betta */
int icvCompute3DPoint( double alpha,double betta,
                            CvStereoLineCoeff* coeffs,
                            CvPoint3D64d* point)
{

    double partX;
    double partY;
    double partZ;
    double partAll;
    double invPartAll;

    double alphabetta = alpha*betta;

    partAll = alpha - betta;
    if( fabs(partAll) > 0.00001  ) /* alpha must be > betta */
    {

        partX   = coeffs->Xcoef        + coeffs->XcoefA *alpha +
                  coeffs->XcoefB*betta + coeffs->XcoefAB*alphabetta;

        partY   = coeffs->Ycoef        + coeffs->YcoefA *alpha +
                  coeffs->YcoefB*betta + coeffs->YcoefAB*alphabetta;

        partZ   = coeffs->Zcoef        + coeffs->ZcoefA *alpha +
                  coeffs->ZcoefB*betta + coeffs->ZcoefAB*alphabetta;

        invPartAll = 1.0 / partAll;

        point->x = partX * invPartAll;
        point->y = partY * invPartAll;
        point->z = partZ * invPartAll;
        return CV_NO_ERR;
    }
    else
    {
        return CV_BADFACTOR_ERR;
    }
}

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

/* Compute rotate matrix and trans vector for change system */
int icvCreateConvertMatrVect( CvMatr64d     rotMatr1,
                                CvMatr64d     transVect1,
                                CvMatr64d     rotMatr2,
                                CvMatr64d     transVect2,
                                CvMatr64d     convRotMatr,
                                CvMatr64d     convTransVect)
{
    double invRotMatr2[9];
    double tmpVect[3];


    icvInvertMatrix_64d(rotMatr2,3,invRotMatr2);
    /* Test for error */

    icvMulMatrix_64d(   rotMatr1,
                        3,3,
                        invRotMatr2,
                        3,3,
                        convRotMatr);

    icvMulMatrix_64d(   convRotMatr,
                        3,3,
                        transVect2,
                        1,3,
                        tmpVect);

    icvSubVector_64d(transVect1,tmpVect,convTransVect,3);


    return CV_NO_ERR;
}

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

/* Compute point coordinates in other system */
int icvConvertPointSystem(CvPoint3D64d  M2,
                            CvPoint3D64d* M1,
                            CvMatr64d     rotMatr,
                            CvMatr64d     transVect
                            )
{
    double tmpVect[3];

    icvMulMatrix_64d(   rotMatr,
                        3,3,
                        (double*)&M2,
                        1,3,
                        tmpVect);

    icvAddVector_64d(tmpVect,transVect,(double*)M1,3);

    return CV_NO_ERR;
}
/*--------------------------------------------------------------------------------------*/
static int icvComputeCoeffForStereoV3( double quad1[4][2],
                                double quad2[4][2],
                                int    numScanlines,
                                CvMatr64d    camMatr1,
                                CvMatr64d    rotMatr1,
                                CvMatr64d    transVect1,
                                CvMatr64d    camMatr2,
                                CvMatr64d    rotMatr2,
                                CvMatr64d    transVect2,
                                CvStereoLineCoeff*    startCoeffs,
                                int* needSwapCamera)
{
    /* For each pair */
    /* In this function we must define position of cameras */

    CvPoint2D64d point1;
    CvPoint2D64d point2;
    CvPoint2D64d point3;
    CvPoint2D64d point4;

    int currLine;
    *needSwapCamera = 0;
    for( currLine = 0; currLine < numScanlines; currLine++ )
    {
        /* Compute points */
        double alpha = ((double)currLine)/((double)(numScanlines)); /* maybe - 1 */

        point1.x = (1.0 - alpha) * quad1[0][0] + alpha * quad1[3][0];
        point1.y = (1.0 - alpha) * quad1[0][1] + alpha * quad1[3][1];

        point2.x = (1.0 - alpha) * quad1[1][0] + alpha * quad1[2][0];
        point2.y = (1.0 - alpha) * quad1[1][1] + alpha * quad1[2][1];

        point3.x = (1.0 - alpha) * quad2[0][0] + alpha * quad2[3][0];
        point3.y = (1.0 - alpha) * quad2[0][1] + alpha * quad2[3][1];

        point4.x = (1.0 - alpha) * quad2[1][0] + alpha * quad2[2][0];
        point4.y = (1.0 - alpha) * quad2[1][1] + alpha * quad2[2][1];

        /* We can compute coeffs for this line */
        icvComCoeffForLine(    point1,
                            point2,
                            point3,
                            point4,
                            camMatr1,
                            rotMatr1,
                            transVect1,
                            camMatr2,
                            rotMatr2,
                            transVect2,
                            &startCoeffs[currLine],
                            needSwapCamera);
    }
    return CV_NO_ERR;
}
/*--------------------------------------------------------------------------------------*/
static int icvComputeCoeffForStereoNew(   double quad1[4][2],
                                        double quad2[4][2],
                                        int    numScanlines,
                                        CvMatr32f    camMatr1,
                                        CvMatr32f    rotMatr1,
                                        CvMatr32f    transVect1,
                                        CvMatr32f    camMatr2,
                                        CvStereoLineCoeff*    startCoeffs,
                                        int* needSwapCamera)
{
    /* Convert data */

    double camMatr1_64d[9];
    double camMatr2_64d[9];

    double rotMatr1_64d[9];
    double transVect1_64d[3];

    double rotMatr2_64d[9];
    double transVect2_64d[3];

    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);

    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;

    int status = icvComputeCoeffForStereoV3( quad1,
                                                quad2,
                                                numScanlines,
                                                camMatr1_64d,
                                                rotMatr1_64d,
                                                transVect1_64d,
                                                camMatr2_64d,
                                                rotMatr2_64d,
                                                transVect2_64d,
                                                startCoeffs,
                                                needSwapCamera);


    return status;

}
/*--------------------------------------------------------------------------------------*/
int icvComputeCoeffForStereo(  CvStereoCamera* stereoCamera)
{
    double quad1[4][2];
    double quad2[4][2];

    int i;
    for( i = 0; i < 4; i++ )
    {
        quad1[i][0] = stereoCamera->quad[0][i].x;
        quad1[i][1] = stereoCamera->quad[0][i].y;

        quad2[i][0] = stereoCamera->quad[1][i].x;
        quad2[i][1] = stereoCamera->quad[1][i].y;
    }

    icvComputeCoeffForStereoNew(        quad1,
                                        quad2,
                                        stereoCamera->warpSize.height,
                                        stereoCamera->camera[0]->matrix,
                                        stereoCamera->rotMatrix,
                                        stereoCamera->transVector,
                                        stereoCamera->camera[1]->matrix,
                                        stereoCamera->lineCoeffs,
                                        &(stereoCamera->needSwapCameras));
    return CV_OK;
}


/*--------------------------------------------------------------------------------------*/
int icvComCoeffForLine(   CvPoint2D64d point1,
                            CvPoint2D64d point2,
                            CvPoint2D64d point3,
                            CvPoint2D64d point4,
                            CvMatr64d    camMatr1,
                            CvMatr64d    rotMatr1,
                            CvMatr64d    transVect1,
                            CvMatr64d    camMatr2,
                            CvMatr64d    rotMatr2,
                            CvMatr64d    transVect2,
                            CvStereoLineCoeff* coeffs,
                            int* needSwapCamera)
{
    /* Get direction for all points */
    /* Direction for camera 1 */

    CvPoint3D64f direct1;
    CvPoint3D64f direct2;
    CvPoint3D64f camPoint1;

    CvPoint3D64f directS3;
    CvPoint3D64f directS4;
    CvPoint3D64f direct3;
    CvPoint3D64f direct4;
    CvPoint3D64f camPoint2;

    icvGetDirectionForPoint(   point1,
                            camMatr1,
                            &direct1);

    icvGetDirectionForPoint(   point2,
                            camMatr1,
                            &direct2);

    /* Direction for camera 2 */

    icvGetDirectionForPoint(   point3,
                            camMatr2,
                            &directS3);

    icvGetDirectionForPoint(   point4,
                            camMatr2,
                            &directS4);

    /* Create convertion for camera 2: two direction and camera point */

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

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

    CvPoint3D64f zeroVect;
    zeroVect.x = zeroVect.y = zeroVect.z = 0.0;
    camPoint1.x = camPoint1.y = camPoint1.z = 0.0;

    icvConvertPointSystem(directS3,&direct3,convRotMatr,convTransVect);
    icvConvertPointSystem(directS4,&direct4,convRotMatr,convTransVect);
    icvConvertPointSystem(zeroVect,&camPoint2,convRotMatr,convTransVect);

    CvPoint3D64f pointB;

    int postype = 0;

    /* Changed order */
    /* Compute point B: xB,yB,zB */
    icvGetCrossLines(camPoint1,direct2,
                  camPoint2,direct3,
                  &pointB);

    if( pointB.z < 0 )/* If negative use other lines for cross */
    {
        postype = 1;
        icvGetCrossLines(camPoint1,direct1,
                      camPoint2,direct4,
                      &pointB);
    }

    CvPoint3D64d pointNewA;
    CvPoint3D64d pointNewC;

    pointNewA.x = pointNewA.y = pointNewA.z = 0;
    pointNewC.x = pointNewC.y = pointNewC.z = 0;

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

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

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


    double gamma;

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

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

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

    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,
                                pointB,
                                camPoint1,
                                gamma,
                                coeffs);

    return CV_NO_ERR;
}


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

int 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;
}

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

int 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;
}

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

int 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 */
static void icvGetCommonArea( CvSize imageSize,
                    CvPoint3D64d epipole1,CvPoint3D64d epipole2,
                    CvMatr64d fundMatr,
                    CvVect64d coeff11,CvVect64d coeff12,
                    CvVect64d coeff21,CvVect64d coeff22,
                    int* result)
{
    int res = 0;
    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 )
    {
        /* 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;
    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 */
        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 */
        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 */
        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 */
        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 */

/*---------------------------------------------------------------------------------------*/
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;
}


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

static 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 */
    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 = {0, 0};
    int numPoints = 0;
    int res;
    int i;

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

    /* Find middle line of sector */
    double midLine[3]={0,0,0};


    /* 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 IplImage* icvCreateIsometricImage( IplImage* src, IplImage* dst,
                                       int desired_depth, int desired_num_channels )
{
    CvSize src_size ;
    src_size.width = src->width;
    src_size.height = src->height;

    CvSize dst_size = src_size;

    if( dst )
    {
        dst_size.width = dst->width;
        dst_size.height = dst->height;
    }

    if( !dst || dst->depth != desired_depth ||
        dst->nChannels != desired_num_channels ||
        dst_size.width != src_size.width ||
        dst_size.height != src_size.height )
    {
        cvReleaseImage( &dst );
        dst = cvCreateImage( src_size, desired_depth, desired_num_channels );
        CvRect rect = cvRect(0,0,src_size.width,src_size.height);
        cvSetImageROI( dst, rect );

    }

    return dst;
}

static int
icvCvt_32f_64d( float *src, double *dst, int size )
{
    int t;

    if( !src || !dst )
        return CV_NULLPTR_ERR;
    if( size <= 0 )
        return CV_BADRANGE_ERR;

    for( t = 0; t < size; t++ )
    {
        dst[t] = (double) (src[t]);
    }

    return CV_OK;
}

/*======================================================================================*/
/* Type conversion double -> float */
static int
icvCvt_64d_32f( double *src, float *dst, int size )
{
    int t;

    if( !src || !dst )
        return CV_NULLPTR_ERR;
    if( size <= 0 )
        return CV_BADRANGE_ERR;

    for( t = 0; t < size; t++ )
    {
        dst[t] = (float) (src[t]);
    }

    return CV_OK;
}

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

#if 0
/* Find line which cross frame by line(a,b,c) */
static void FindLineForEpiline(    CvSize imageSize,
                            float a,float b,float c,
                            CvPoint2D32f *start,CvPoint2D32f *end,
                            int*)
{
    CvPoint2D32f frameBeg;

    CvPoint2D32f frameEnd;
    CvPoint2D32f cross[4];
    int     haveCross[4];
    float   dist;

    haveCross[0] = 0;
    haveCross[1] = 0;
    haveCross[2] = 0;
    haveCross[3] = 0;

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

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

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

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

    int n;
    float minDist = (float)(INT_MAX);
    float maxDist = (float)(INT_MIN);

    int maxN = -1;
    int minN = -1;

    double midPointX = imageSize.width  / 2.0;
    double midPointY = imageSize.height / 2.0;

    for( n = 0; n < 4; n++ )
    {
        if( haveCross[n] > 0 )
        {
            dist =  (float)((midPointX - cross[n].x)*(midPointX - cross[n].x) +
                            (midPointY - cross[n].y)*(midPointY - cross[n].y));

            if( dist < minDist )
            {
                minDist = dist;
                minN = n;
            }

            if( dist > maxDist )
            {
                maxDist = dist;
                maxN = n;
            }
        }
    }

    if( minN >= 0 && maxN >= 0 && (minN != maxN) )
    {
        *start = cross[minN];
        *end   = cross[maxN];
    }
    else
    {
        start->x = 0;
        start->y = 0;
        end->x = 0;
        end->y = 0;
    }

    return;

}


/*----------------------------------------------------------------------------------*/
static int GetAngleLinee( CvPoint2D32f epipole, CvSize imageSize,CvPoint2D32f point1,CvPoint2D32f point2)
{
    float width  = (float)(imageSize.width);
    float height = (float)(imageSize.height);

    /* Get crosslines with image corners */

    /* Find four lines */

    CvPoint2D32f pa,pb,pc,pd;

    pa.x = 0;
    pa.y = 0;

    pb.x = width;
    pb.y = 0;

    pd.x = width;
    pd.y = height;

    pc.x = 0;
    pc.y = height;

    /* We can compute points for angle */
    /* Test for place section */

    float x,y;
    x = epipole.x;
    y = epipole.y;

    if( x < 0 )
    {/* 1,4,7 */
        if( y < 0)
        {/* 1 */
            point1 = pb;
            point2 = pc;
        }
        else if( y > height )
        {/* 7 */
            point1 = pa;
            point2 = pd;
        }
        else
        {/* 4 */
            point1 = pa;
            point2 = pc;
        }
    }
    else if ( x > width )
    {/* 3,6,9 */
        if( y < 0 )
        {/* 3 */
            point1 = pa;
            point2 = pd;
        }
        else if ( y > height )
        {/* 9 */
            point1 = pc;
            point2 = pb;
        }
        else
        {/* 6 */
            point1 = pb;
            point2 = pd;
        }
    }
    else
    {/* 2,5,8 */
        if( y < 0 )
        {/* 2 */
            point1 = pa;
            point2 = pb;
        }
        else if( y > height )
        {/* 8 */
            point1 = pc;
            point2 = pd;
        }
        else
        {/* 5 - point in the image */
            return 2;
        }


    }


    return 0;
}

/*--------------------------------------------------------------------------------------*/
static void icvComputePerspectiveCoeffs(const CvPoint2D32f srcQuad[4],const CvPoint2D32f dstQuad[4],double coeffs[3][3])
{/* Computes perspective coeffs for transformation from src to dst quad */


    CV_FUNCNAME( "icvComputePerspectiveCoeffs" );

    __BEGIN__;

    double A[64];
    double b[8];
    double c[8];
    CvPoint2D32f pt[4];
    int i;

    pt[0] = srcQuad[0];
    pt[1] = srcQuad[1];
    pt[2] = srcQuad[2];
    pt[3] = srcQuad[3];

    for( i = 0; i < 4; i++ )
    {
#if 0
        double x = dstQuad[i].x;
        double y = dstQuad[i].y;
        double X = pt[i].x;
        double Y = pt[i].y;
#else
        double x = pt[i].x;
        double y = pt[i].y;
        double X = dstQuad[i].x;
        double Y = dstQuad[i].y;
#endif
        double* a = A + i*16;

        a[0] = x;
        a[1] = y;
        a[2] = 1;
        a[3] = 0;
        a[4] = 0;
        a[5] = 0;
        a[6] = -X*x;
        a[7] = -X*y;

        a += 8;

        a[0] = 0;
        a[1] = 0;
        a[2] = 0;
        a[3] = x;
        a[4] = y;
        a[5] = 1;
        a[6] = -Y*x;
        a[7] = -Y*y;

        b[i*2] = X;
        b[i*2 + 1] = Y;
    }

    {
    double invA[64];
    CvMat matA = cvMat( 8, 8, CV_64F, A );
    CvMat matInvA = cvMat( 8, 8, CV_64F, invA );
    CvMat matB = cvMat( 8, 1, CV_64F, b );
    CvMat matX = cvMat( 8, 1, CV_64F, c );

    CV_CALL( cvPseudoInverse( &matA, &matInvA ));
    CV_CALL( cvMatMulAdd( &matInvA, &matB, 0, &matX ));
    }

    coeffs[0][0] = c[0];
    coeffs[0][1] = c[1];
    coeffs[0][2] = c[2];
    coeffs[1][0] = c[3];
    coeffs[1][1] = c[4];
    coeffs[1][2] = c[5];
    coeffs[2][0] = c[6];
    coeffs[2][1] = c[7];
    coeffs[2][2] = 1.0;

    __END__;

    return;
}
#endif

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

CV_IMPL void cvComputePerspectiveMap(const double c[3][3], CvArr* rectMapX, CvArr* rectMapY )
{
    CV_FUNCNAME( "cvComputePerspectiveMap" );

    __BEGIN__;

    CvSize size;
    CvMat  stubx, *mapx = (CvMat*)rectMapX;
    CvMat  stuby, *mapy = (CvMat*)rectMapY;
    int i, j;

    CV_CALL( mapx = cvGetMat( mapx, &stubx ));
    CV_CALL( mapy = cvGetMat( mapy, &stuby ));

    if( CV_MAT_TYPE( mapx->type ) != CV_32FC1 || CV_MAT_TYPE( mapy->type ) != CV_32FC1 )
        CV_ERROR( CV_StsUnsupportedFormat, "" );

    size = cvGetMatSize(mapx);
    assert( fabs(c[2][2] - 1.) < FLT_EPSILON );

    for( i = 0; i < size.height; i++ )
    {
        float* mx = (float*)(mapx->data.ptr + mapx->step*i);
        float* my = (float*)(mapy->data.ptr + mapy->step*i);

        for( j = 0; j < size.width; j++ )
        {
            double w = 1./(c[2][0]*j + c[2][1]*i + 1.);
            double x = (c[0][0]*j + c[0][1]*i + c[0][2])*w;
            double y = (c[1][0]*j + c[1][1]*i + c[1][2])*w;

            mx[j] = (float)x;
            my[j] = (float)y;
        }
    }

    __END__;
}

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

CV_IMPL void cvInitPerspectiveTransform( CvSize size, const CvPoint2D32f quad[4], double matrix[3][3],
                                              CvArr* rectMap )
{
    /* Computes Perspective Transform coeffs and map if need
        for given image size and given result quad */
    CV_FUNCNAME( "cvInitPerspectiveTransform" );

    __BEGIN__;

    double A[64];
    double b[8];
    double c[8];
    CvPoint2D32f pt[4];
    CvMat  mapstub, *map = (CvMat*)rectMap;
    int i, j;

    if( map )
    {
        CV_CALL( map = cvGetMat( map, &mapstub ));

        if( CV_MAT_TYPE( map->type ) != CV_32FC2 )
            CV_ERROR( CV_StsUnsupportedFormat, "" );

        if( map->width != size.width || map->height != size.height )
            CV_ERROR( CV_StsUnmatchedSizes, "" );
    }

    pt[0] = cvPoint2D32f( 0, 0 );
    pt[1] = cvPoint2D32f( size.width, 0 );
    pt[2] = cvPoint2D32f( size.width, size.height );
    pt[3] = cvPoint2D32f( 0, size.height );

    for( i = 0; i < 4; i++ )
    {
#if 0
        double x = quad[i].x;
        double y = quad[i].y;
        double X = pt[i].x;
        double Y = pt[i].y;
#else
        double x = pt[i].x;
        double y = pt[i].y;
        double X = quad[i].x;
        double Y = quad[i].y;
#endif
        double* a = A + i*16;

        a[0] = x;
        a[1] = y;
        a[2] = 1;
        a[3] = 0;
        a[4] = 0;
        a[5] = 0;
        a[6] = -X*x;
        a[7] = -X*y;

        a += 8;

        a[0] = 0;
        a[1] = 0;
        a[2] = 0;
        a[3] = x;
        a[4] = y;
        a[5] = 1;
        a[6] = -Y*x;
        a[7] = -Y*y;

        b[i*2] = X;
        b[i*2 + 1] = Y;
    }

    {
    double invA[64];
    CvMat matA = cvMat( 8, 8, CV_64F, A );
    CvMat matInvA = cvMat( 8, 8, CV_64F, invA );
    CvMat matB = cvMat( 8, 1, CV_64F, b );
    CvMat matX = cvMat( 8, 1, CV_64F, c );

    CV_CALL( cvPseudoInverse( &matA, &matInvA ));
    CV_CALL( cvMatMulAdd( &matInvA, &matB, 0, &matX ));
    }

    matrix[0][0] = c[0];
    matrix[0][1] = c[1];
    matrix[0][2] = c[2];
    matrix[1][0] = c[3];
    matrix[1][1] = c[4];
    matrix[1][2] = c[5];
    matrix[2][0] = c[6];
    matrix[2][1] = c[7];
    matrix[2][2] = 1.0;

    if( map )
    {
        for( i = 0; i < size.height; i++ )
        {
            CvPoint2D32f* maprow = (CvPoint2D32f*)(map->data.ptr + map->step*i);
            for( j = 0; j < size.width; j++ )
            {
                double w = 1./(c[6]*j + c[7]*i + 1.);
                double x = (c[0]*j + c[1]*i + c[2])*w;
                double y = (c[3]*j + c[4]*i + c[5])*w;

                maprow[j].x = (float)x;
                maprow[j].y = (float)y;
            }
        }
    }

    __END__;

    return;
}


/*-----------------------------------------------------------------------*/
/* Compute projected infinite point for second image if first image point is known */
void icvComputeeInfiniteProject1(   CvMatr64d     rotMatr,
                                    CvMatr64d     camMatr1,
                                    CvMatr64d     camMatr2,
                                    CvPoint2D32f  point1,
                                    CvPoint2D32f* point2)
{
    double invMatr1[9];
    icvInvertMatrix_64d(camMatr1,3,invMatr1);
    double P1[3];
    double p1[3];
    p1[0] = (double)(point1.x);
    p1[1] = (double)(point1.y);
    p1[2] = 1;

    icvMulMatrix_64d(   invMatr1,
                        3,3,
                        p1,
                        1,3,
                        P1);

    double invR[9];
    icvTransposeMatrix_64d( rotMatr, 3, 3, invR );

    /* Change system 1 to system 2 */
    double P2[3];
    icvMulMatrix_64d(   invR,
                        3,3,
                        P1,
                        1,3,
                        P2);

    /* Now we can project this point to image 2 */
    double projP[3];

    icvMulMatrix_64d(   camMatr2,
                        3,3,
                        P2,
                        1,3,
                        projP);

    point2->x = (float)(projP[0] / projP[2]);
    point2->y = (float)(projP[1] / projP[2]);

    return;
}

/*-----------------------------------------------------------------------*/
/* Compute projected infinite point for second image if first image point is known */
void icvComputeeInfiniteProject2(   CvMatr64d     rotMatr,
                                    CvMatr64d     camMatr1,
                                    CvMatr64d     camMatr2,
                                    CvPoint2D32f*  point1,
                                    CvPoint2D32f point2)
{
    double invMatr2[9];
    icvInvertMatrix_64d(camMatr2,3,invMatr2);
    double P2[3];
    double p2[3];
    p2[0] = (double)(point2.x);
    p2[1] = (double)(point2.y);
    p2[2] = 1;

    icvMulMatrix_64d(   invMatr2,
                        3,3,
                        p2,
                        1,3,
                        P2);

    /* Change system 1 to system 2 */

    double P1[3];
    icvMulMatrix_64d(   rotMatr,
                        3,3,
                        P2,
                        1,3,
                        P1);

    /* Now we can project this point to image 2 */
    double projP[3];

    icvMulMatrix_64d(   camMatr1,
                        3,3,
                        P1,
                        1,3,
                        projP);

    point1->x = (float)(projP[0] / projP[2]);
    point1->y = (float)(projP[1] / projP[2]);

    return;
}

/* Select best R and t for given cameras, points, ... */
/* For both cameras */
static int icvSelectBestRt(         int           numImages,
                                    int*          numPoints,
                                    CvPoint2D32f* imagePoints1,
                                    CvPoint2D32f* imagePoints2,
                                    CvPoint3D32f* objectPoints,

                                    CvMatr32f     cameraMatrix1,
                                    CvVect32f     distortion1,
                                    CvMatr32f     rotMatrs1,
                                    CvVect32f     transVects1,

                                    CvMatr32f     cameraMatrix2,
                                    CvVect32f     distortion2,
                                    CvMatr32f     rotMatrs2,
                                    CvVect32f     transVects2,

                                    CvMatr32f     bestRotMatr,
                                    CvVect32f     bestTransVect
                                    )
{

    /* Need to convert input data 32 -> 64 */
    CvPoint3D64d* objectPoints_64d;

    double* rotMatrs1_64d;
    double* rotMatrs2_64d;

    double* transVects1_64d;
    double* transVects2_64d;

    double cameraMatrix1_64d[9];
    double cameraMatrix2_64d[9];

    double distortion1_64d[4];
    double distortion2_64d[4];

    /* allocate memory for 64d data */
    int totalNum = 0;

    for(int i = 0; i < numImages; i++ )
    {
        totalNum += numPoints[i];
    }

    objectPoints_64d = (CvPoint3D64d*)calloc(totalNum,sizeof(CvPoint3D64d));

    rotMatrs1_64d    = (double*)calloc(numImages,sizeof(double)*9);
    rotMatrs2_64d    = (double*)calloc(numImages,sizeof(double)*9);

    transVects1_64d  = (double*)calloc(numImages,sizeof(double)*3);
    transVects2_64d  = (double*)calloc(numImages,sizeof(double)*3);

    /* Convert input data to 64d */

    icvCvt_32f_64d((float*)objectPoints, (double*)objectPoints_64d,  totalNum*3);

    icvCvt_32f_64d(rotMatrs1, rotMatrs1_64d,  numImages*9);
    icvCvt_32f_64d(rotMatrs2, rotMatrs2_64d,  numImages*9);

    icvCvt_32f_64d(transVects1, transVects1_64d,  numImages*3);
    icvCvt_32f_64d(transVects2, transVects2_64d,  numImages*3);

    /* Convert to arrays */
    icvCvt_32f_64d(cameraMatrix1, cameraMatrix1_64d, 9);
    icvCvt_32f_64d(cameraMatrix2, cameraMatrix2_64d, 9);

    icvCvt_32f_64d(distortion1, distortion1_64d, 4);
    icvCvt_32f_64d(distortion2, distortion2_64d, 4);


    /* for each R and t compute error for image pair */
    float* errors;

    errors = (float*)calloc(numImages*numImages,sizeof(float));
    if( errors == 0 )
    {
        return CV_OUTOFMEM_ERR;
    }

    int currImagePair;
    int currRt;
    for( currRt = 0; currRt < numImages; currRt++ )
    {
        int begPoint = 0;
        for(currImagePair = 0; currImagePair < numImages; currImagePair++ )
        {
            /* For current R,t R,t compute relative position of cameras */

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

            icvCreateConvertMatrVect( rotMatrs1_64d + currRt*9,
                                      transVects1_64d + currRt*3,
                                      rotMatrs2_64d + currRt*9,
                                      transVects2_64d + currRt*3,
                                      convRotMatr,
                                      convTransVect);

            /* Project points using relative position of cameras */

            double convRotMatr2[9];
            double convTransVect2[3];

            convRotMatr2[0] = 1;
            convRotMatr2[1] = 0;
            convRotMatr2[2] = 0;

            convRotMatr2[3] = 0;
            convRotMatr2[4] = 1;
            convRotMatr2[5] = 0;

            convRotMatr2[6] = 0;
            convRotMatr2[7] = 0;
            convRotMatr2[8] = 1;

            convTransVect2[0] = 0;
            convTransVect2[1] = 0;
            convTransVect2[2] = 0;

            /* Compute error for given pair and Rt */
            /* We must project points to image and compute error */

            CvPoint2D64d* projImagePoints1;
            CvPoint2D64d* projImagePoints2;

            CvPoint3D64d* points1;
            CvPoint3D64d* points2;

            int numberPnt;
            numberPnt = numPoints[currImagePair];
            projImagePoints1 = (CvPoint2D64d*)calloc(numberPnt,sizeof(CvPoint2D64d));
            projImagePoints2 = (CvPoint2D64d*)calloc(numberPnt,sizeof(CvPoint2D64d));

            points1 = (CvPoint3D64d*)calloc(numberPnt,sizeof(CvPoint3D64d));
            points2 = (CvPoint3D64d*)calloc(numberPnt,sizeof(CvPoint3D64d));

            /* Transform object points to first camera position */
            for(int i = 0; i < numberPnt; i++ )
            {
                /* Create second camera point */
                CvPoint3D64d tmpPoint;
                tmpPoint.x = (double)(objectPoints[i].x);
                tmpPoint.y = (double)(objectPoints[i].y);
                tmpPoint.z = (double)(objectPoints[i].z);

                icvConvertPointSystem(  tmpPoint,
                                        points2+i,
                                        rotMatrs2_64d + currImagePair*9,
                                        transVects2_64d + currImagePair*3);

                /* Create first camera point using R, t */
                icvConvertPointSystem(  points2[i],
                                        points1+i,
                                        convRotMatr,
                                        convTransVect);

                CvPoint3D64d tmpPoint2 = { 0, 0, 0 };
                icvConvertPointSystem(  tmpPoint,
                                        &tmpPoint2,
                                        rotMatrs1_64d + currImagePair*9,
                                        transVects1_64d + currImagePair*3);
                /*double err;
                double dx,dy,dz;
                dx = tmpPoint2.x - points1[i].x;
                dy = tmpPoint2.y - points1[i].y;
                dz = tmpPoint2.z - points1[i].z;
                err = sqrt(dx*dx + dy*dy + dz*dz);*/
            }

#if 0
            cvProjectPointsSimple(  numPoints[currImagePair],
                                    objectPoints_64d + begPoint,
                                    rotMatrs1_64d + currRt*9,
                                    transVects1_64d + currRt*3,
                                    cameraMatrix1_64d,
                                    distortion1_64d,
                                    projImagePoints1);

            cvProjectPointsSimple(  numPoints[currImagePair],
                                    objectPoints_64d + begPoint,
                                    rotMatrs2_64d + currRt*9,
                                    transVects2_64d + currRt*3,
                                    cameraMatrix2_64d,
                                    distortion2_64d,
                                    projImagePoints2);
#endif

            /* Project with no translate and no rotation */

#if 0
            {
                double nodist[4] = {0,0,0,0};
                cvProjectPointsSimple(  numPoints[currImagePair],
                                        points1,
                                        convRotMatr2,
                                        convTransVect2,
                                        cameraMatrix1_64d,
                                        nodist,
                                        projImagePoints1);

                cvProjectPointsSimple(  numPoints[currImagePair],
                                        points2,
                                        convRotMatr2,
                                        convTransVect2,
                                        cameraMatrix2_64d,
                                        nodist,
                                        projImagePoints2);

            }
#endif

            cvProjectPointsSimple(  numPoints[currImagePair],
                                    points1,
                                    convRotMatr2,
                                    convTransVect2,
                                    cameraMatrix1_64d,
                                    distortion1_64d,
                                    projImagePoints1);

            cvProjectPointsSimple(  numPoints[currImagePair],
                                    points2,
                                    convRotMatr2,
                                    convTransVect2,
                                    cameraMatrix2_64d,
                                    distortion2_64d,
                                    projImagePoints2);

            /* points are projected. Compute error */

            int currPoint;
            double err1 = 0;
            double err2 = 0;
            double err;
            for( currPoint = 0; currPoint < numberPnt; currPoint++ )
            {
                double len1,len2;
                double dx1,dy1;
                dx1 = imagePoints1[begPoint+currPoint].x - projImagePoints1[currPoint].x;
                dy1 = imagePoints1[begPoint+currPoint].y - projImagePoints1[currPoint].y;
                len1 = sqrt(dx1*dx1 + dy1*dy1);
                err1 += len1;

                double dx2,dy2;
                dx2 = imagePoints2[begPoint+currPoint].x - projImagePoints2[currPoint].x;
                dy2 = imagePoints2[begPoint+currPoint].y - projImagePoints2[currPoint].y;
                len2 = sqrt(dx2*dx2 + dy2*dy2);
                err2 += len2;
            }

            err1 /= (float)(numberPnt);
            err2 /= (float)(numberPnt);

            err = (err1+err2) * 0.5;
            begPoint += numberPnt;

            /* Set this error to */
            errors[numImages*currImagePair+currRt] = (float)err;

            free(points1);
            free(points2);
            free(projImagePoints1);
            free(projImagePoints2);
        }
    }

    /* Just select R and t with minimal average error */

    int bestnumRt = 0;
    float minError = 0;/* Just for no warnings. Uses 'first' flag. */
    int first = 1;
    for( currRt = 0; currRt < numImages; currRt++ )
    {
        float avErr = 0;
        for(currImagePair = 0; currImagePair < numImages; currImagePair++ )
        {
            avErr += errors[numImages*currImagePair+currRt];
        }
        avErr /= (float)(numImages);

        if( first )
        {
            bestnumRt = 0;
            minError = avErr;
            first = 0;
        }
        else
        {
            if( avErr < minError )
            {
                bestnumRt = currRt;
                minError = avErr;
            }
        }

    }

    double bestRotMatr_64d[9];
    double bestTransVect_64d[3];

    icvCreateConvertMatrVect( rotMatrs1_64d + bestnumRt * 9,
                              transVects1_64d + bestnumRt * 3,
                              rotMatrs2_64d + bestnumRt * 9,
                              transVects2_64d + bestnumRt * 3,
                              bestRotMatr_64d,
                              bestTransVect_64d);

    icvCvt_64d_32f(bestRotMatr_64d,bestRotMatr,9);
    icvCvt_64d_32f(bestTransVect_64d,bestTransVect,3);


    free(errors);

    return CV_OK;
}


/* ----------------- Stereo calibration functions --------------------- */

float icvDefinePointPosition(CvPoint2D32f point1,CvPoint2D32f point2,CvPoint2D32f point)
{
    float ax = point2.x - point1.x;
    float ay = point2.y - point1.y;

    float bx = point.x - point1.x;
    float by = point.y - point1.y;

    return (ax*by - ay*bx);
}

/* Convert function for stereo warping */
int icvConvertWarpCoordinates(double coeffs[3][3],
                                CvPoint2D32f* cameraPoint,
                                CvPoint2D32f* warpPoint,
                                int direction)
{
    double x,y;
    double det;
    if( direction == CV_WARP_TO_CAMERA )
    {/* convert from camera image to warped image coordinates */
        x = warpPoint->x;
        y = warpPoint->y;

        det = (coeffs[2][0] * x + coeffs[2][1] * y + coeffs[2][2]);
        if( fabs(det) > 1e-8 )
        {
            cameraPoint->x = (float)((coeffs[0][0] * x + coeffs[0][1] * y + coeffs[0][2]) / det);
            cameraPoint->y = (float)((coeffs[1][0] * x + coeffs[1][1] * y + coeffs[1][2]) / det);
            return CV_OK;
        }
    }
    else if( direction == CV_CAMERA_TO_WARP )
    {/* convert from warped image to camera image coordinates */
        x = cameraPoint->x;
        y = cameraPoint->y;

        det = (coeffs[2][0]*x-coeffs[0][0])*(coeffs[2][1]*y-coeffs[1][1])-(coeffs[2][1]*x-coeffs[0][1])*(coeffs[2][0]*y-coeffs[1][0]);

        if( fabs(det) > 1e-8 )
        {
            warpPoint->x = (float)(((coeffs[0][2]-coeffs[2][2]*x)*(coeffs[2][1]*y-coeffs[1][1])-(coeffs[2][1]*x-coeffs[0][1])*(coeffs[1][2]-coeffs[2][2]*y))/det);
            warpPoint->y = (float)(((coeffs[2][0]*x-coeffs[0][0])*(coeffs[1][2]-coeffs[2][2]*y)-(coeffs[0][2]-coeffs[2][2]*x)*(coeffs[2][0]*y-coeffs[1][0]))/det);
            return CV_OK;
        }
    }

    return CV_BADFACTOR_ERR;
}

/* Compute stereo params using some camera params */
/* by Valery Mosyagin. int ComputeRestStereoParams(StereoParams *stereoparams) */
int icvComputeRestStereoParams(CvStereoCamera *stereoparams)
{


    icvGetQuadsTransformStruct(stereoparams);

    cvInitPerspectiveTransform( stereoparams->warpSize,
                                stereoparams->quad[0],
                                stereoparams->coeffs[0],
                                0);

    cvInitPerspectiveTransform( stereoparams->warpSize,
                                stereoparams->quad[1],
                                stereoparams->coeffs[1],
                                0);

    /* Create border for warped images */
    CvPoint2D32f corns[4];
    corns[0].x = 0;
    corns[0].y = 0;

    corns[1].x = (float)(stereoparams->camera[0]->imgSize[0]-1);
    corns[1].y = 0;

    corns[2].x = (float)(stereoparams->camera[0]->imgSize[0]-1);
    corns[2].y = (float)(stereoparams->camera[0]->imgSize[1]-1);

    corns[3].x = 0;
    corns[3].y = (float)(stereoparams->camera[0]->imgSize[1]-1);

    for(int i = 0; i < 4; i++ )
    {
        /* For first camera */
        icvConvertWarpCoordinates( stereoparams->coeffs[0],
                                corns+i,
                                stereoparams->border[0]+i,
                                CV_CAMERA_TO_WARP);

        /* For second camera */
        icvConvertWarpCoordinates( stereoparams->coeffs[1],
                                corns+i,
                                stereoparams->border[1]+i,
                                CV_CAMERA_TO_WARP);
    }

    /* Test compute  */
    {
        CvPoint2D32f warpPoints[4];
        warpPoints[0] = cvPoint2D32f(0,0);
        warpPoints[1] = cvPoint2D32f(stereoparams->warpSize.width-1,0);
        warpPoints[2] = cvPoint2D32f(stereoparams->warpSize.width-1,stereoparams->warpSize.height-1);
        warpPoints[3] = cvPoint2D32f(0,stereoparams->warpSize.height-1);

        CvPoint2D32f camPoints1[4];
        CvPoint2D32f camPoints2[4];

        for( int i = 0; i < 4; i++ )
        {
            icvConvertWarpCoordinates(stereoparams->coeffs[0],
                                camPoints1+i,
                                warpPoints+i,
                                CV_WARP_TO_CAMERA);

            icvConvertWarpCoordinates(stereoparams->coeffs[1],
                                camPoints2+i,
                                warpPoints+i,
                                CV_WARP_TO_CAMERA);
        }
    }


    /* Allocate memory for scanlines coeffs */

    stereoparams->lineCoeffs = (CvStereoLineCoeff*)calloc(stereoparams->warpSize.height,sizeof(CvStereoLineCoeff));

    /* Compute coeffs for epilines  */

    icvComputeCoeffForStereo( stereoparams);

    /* all coeffs are known */
    return CV_OK;
}

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

int icvStereoCalibration( int numImages,
                            int* nums,
                            CvSize imageSize,
                            CvPoint2D32f* imagePoints1,
                            CvPoint2D32f* imagePoints2,
                            CvPoint3D32f* objectPoints,
                            CvStereoCamera* stereoparams
                           )
{
    /* Firstly we must calibrate both cameras */
    /*  Alocate memory for data */
    /* Allocate for translate vectors */
    float* transVects1;
    float* transVects2;
    float* rotMatrs1;
    float* rotMatrs2;

    transVects1 = (float*)calloc(numImages,sizeof(float)*3);
    transVects2 = (float*)calloc(numImages,sizeof(float)*3);

    rotMatrs1   = (float*)calloc(numImages,sizeof(float)*9);
    rotMatrs2   = (float*)calloc(numImages,sizeof(float)*9);

    /* Calibrate first camera */
    cvCalibrateCamera(  numImages,
                        nums,
                        imageSize,
                        imagePoints1,
                        objectPoints,
                        stereoparams->camera[0]->distortion,
                        stereoparams->camera[0]->matrix,
                        transVects1,
                        rotMatrs1,
                        1);

    /* Calibrate second camera */
    cvCalibrateCamera(  numImages,
                        nums,
                        imageSize,
                        imagePoints2,
                        objectPoints,
                        stereoparams->camera[1]->distortion,
                        stereoparams->camera[1]->matrix,
                        transVects2,
                        rotMatrs2,
                        1);

    /* Cameras are calibrated */

    stereoparams->camera[0]->imgSize[0] = (float)imageSize.width;
    stereoparams->camera[0]->imgSize[1] = (float)imageSize.height;

    stereoparams->camera[1]->imgSize[0] = (float)imageSize.width;
    stereoparams->camera[1]->imgSize[1] = (float)imageSize.height;

    icvSelectBestRt(    numImages,
                        nums,
                        imagePoints1,
                        imagePoints2,
                        objectPoints,
                        stereoparams->camera[0]->matrix,
                        stereoparams->camera[0]->distortion,
                        rotMatrs1,
                        transVects1,
                        stereoparams->camera[1]->matrix,
                        stereoparams->camera[1]->distortion,
                        rotMatrs2,
                        transVects2,
                        stereoparams->rotMatrix,
                        stereoparams->transVector
                        );

    /* Free memory */
    free(transVects1);
    free(transVects2);
    free(rotMatrs1);
    free(rotMatrs2);

    icvComputeRestStereoParams(stereoparams);

    return CV_NO_ERR;
}

#if 0
/* Find line from epipole */
static void FindLine(CvPoint2D32f epipole,CvSize imageSize,CvPoint2D32f point,CvPoint2D32f *start,CvPoint2D32f *end)
{
    CvPoint2D32f frameBeg;
    CvPoint2D32f frameEnd;
    CvPoint2D32f cross[4];
    int     haveCross[4];
    float   dist;

    haveCross[0] = 0;
    haveCross[1] = 0;
    haveCross[2] = 0;
    haveCross[3] = 0;

    frameBeg.x = 0;
    frameBeg.y = 0;
    frameEnd.x = (float)(imageSize.width);
    frameEnd.y = 0;
    haveCross[0] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[0]);

    frameBeg.x = (float)(imageSize.width);
    frameBeg.y = 0;
    frameEnd.x = (float)(imageSize.width);
    frameEnd.y = (float)(imageSize.height);
    haveCross[1] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[1]);

    frameBeg.x = (float)(imageSize.width);
    frameBeg.y = (float)(imageSize.height);
    frameEnd.x = 0;
    frameEnd.y = (float)(imageSize.height);
    haveCross[2] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[2]);

    frameBeg.x = 0;
    frameBeg.y = (float)(imageSize.height);
    frameEnd.x = 0;
    frameEnd.y = 0;
    haveCross[3] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[3]);

    int n;
    float minDist = (float)(INT_MAX);
    float maxDist = (float)(INT_MIN);

    int maxN = -1;
    int minN = -1;

    for( n = 0; n < 4; n++ )
    {
        if( haveCross[n] > 0 )
        {
            dist =  (epipole.x - cross[n].x)*(epipole.x - cross[n].x) +
                    (epipole.y - cross[n].y)*(epipole.y - cross[n].y);

            if( dist < minDist )
            {
                minDist = dist;
                minN = n;
            }

            if( dist > maxDist )
            {
                maxDist = dist;
                maxN = n;
            }
        }
    }

    if( minN >= 0 && maxN >= 0 && (minN != maxN) )
    {
        *start = cross[minN];
        *end   = cross[maxN];
    }
    else
    {
        start->x = 0;
        start->y = 0;
        end->x = 0;
        end->y = 0;
    }

    return;
}

/* Find line which cross frame by line(a,b,c) */
static void FindLineForEpiline(CvSize imageSize,float a,float b,float c,CvPoint2D32f *start,CvPoint2D32f *end)
{
    CvPoint2D32f frameBeg;
    CvPoint2D32f frameEnd;
    CvPoint2D32f cross[4];
    int     haveCross[4];
    float   dist;

    haveCross[0] = 0;
    haveCross[1] = 0;
    haveCross[2] = 0;
    haveCross[3] = 0;

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

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

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

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

    int n;
    float minDist = (float)(INT_MAX);
    float maxDist = (float)(INT_MIN);

    int maxN = -1;
    int minN = -1;

    double midPointX = imageSize.width  / 2.0;
    double midPointY = imageSize.height / 2.0;

    for( n = 0; n < 4; n++ )
    {
        if( haveCross[n] > 0 )
        {
            dist =  (float)((midPointX - cross[n].x)*(midPointX - cross[n].x) +
                            (midPointY - cross[n].y)*(midPointY - cross[n].y));

            if( dist < minDist )
            {
                minDist = dist;
                minN = n;
            }

            if( dist > maxDist )
            {
                maxDist = dist;
                maxN = n;
            }
        }
    }

    if( minN >= 0 && maxN >= 0 && (minN != maxN) )
    {
        *start = cross[minN];
        *end   = cross[maxN];
    }
    else
    {
        start->x = 0;
        start->y = 0;
        end->x = 0;
        end->y = 0;
    }

    return;

}

/* Cross lines */
static int GetCrossLines(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f p2_start,CvPoint2D32f p2_end,CvPoint2D32f *cross)
{
    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 = (ex1-ex2)*(py2-py1)+(ey2-ey1)*(px2-px1);
    if( del == 0)
    {
        return -1;
    }

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

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

    if( alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0)
    {
        return -1;
    }

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

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

    cross->x = (float)( delX / del);
    cross->y = (float)(-delY / del);
    return 1;
}
#endif

int icvGetCrossPieceVector(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f v2_start,CvPoint2D32f v2_end,CvPoint2D32f *cross)
{
    double ex1 = p1_start.x;
    double ey1 = p1_start.y;
    double ex2 = p1_end.x;
    double ey2 = p1_end.y;

    double px1 = v2_start.x;
    double py1 = v2_start.y;
    double px2 = v2_end.x;
    double py2 = v2_end.y;

    double del = (ex1-ex2)*(py2-py1)+(ey2-ey1)*(px2-px1);
    if( del == 0)
    {
        return -1;
    }

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

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

    if( alpha < 0 || alpha > 1.0 )
    {
        return -1;
    }

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

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

    cross->x = (float)( delX / del);
    cross->y = (float)(-delY / del);
    return 1;
}


int icvGetCrossLineDirect(CvPoint2D32f p1,CvPoint2D32f p2,float a,float b,float c,CvPoint2D32f* cross)
{
    double del;
    double delX,delY,delA;

    double px1,px2,py1,py2;
    double X,Y,alpha;

    px1 = p1.x;
    py1 = p1.y;

    px2 = p2.x;
    py2 = p2.y;

    del = a * (px2 - px1) + b * (py2-py1);
    if( del == 0 )
    {
        return -1;
    }

    delA = - c - a*px1 - b*py1;
    alpha = delA / del;

    if( alpha < 0 || alpha > 1.0 )
    {
        return -1;/* no cross */
    }

    delX = b * (py1*(px1-px2) - px1*(py1-py2)) + c * (px1-px2);
    delY = a * (px1*(py1-py2) - py1*(px1-px2)) + c * (py1-py2);

    X = delX / del;
    Y = delY / del;

    cross->x = (float)X;
    cross->y = (float)Y;

    return 1;
}

#if 0
static int cvComputeEpipoles( CvMatr32f camMatr1,  CvMatr32f camMatr2,
                            CvMatr32f rotMatr1,  CvMatr32f rotMatr2,
                            CvVect32f transVect1,CvVect32f transVect2,
                            CvVect32f epipole1,
                            CvVect32f epipole2)
{

    /* Copy matrix */

    CvMat ccamMatr1 = cvMat(3,3,CV_MAT32F,camMatr1);
    CvMat ccamMatr2 = cvMat(3,3,CV_MAT32F,camMatr2);
    CvMat crotMatr1 = cvMat(3,3,CV_MAT32F,rotMatr1);
    CvMat crotMatr2 = cvMat(3,3,CV_MAT32F,rotMatr2);
    CvMat ctransVect1 = cvMat(3,1,CV_MAT32F,transVect1);
    CvMat ctransVect2 = cvMat(3,1,CV_MAT32F,transVect2);
    CvMat cepipole1 = cvMat(3,1,CV_MAT32F,epipole1);
    CvMat cepipole2 = cvMat(3,1,CV_MAT32F,epipole2);


    CvMat cmatrP1   = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrP1);
    CvMat cmatrP2   = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrP2);
    CvMat cvectp1   = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&cvectp1);
    CvMat cvectp2   = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&cvectp2);
    CvMat ctmpF1    = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpF1);
    CvMat ctmpM1    = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpM1);
    CvMat ctmpM2    = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpM2);
    CvMat cinvP1    = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cinvP1);
    CvMat cinvP2    = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cinvP2);
    CvMat ctmpMatr  = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpMatr);
    CvMat ctmpVect1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpVect1);
    CvMat ctmpVect2 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpVect2);
    CvMat cmatrF1   = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrF1);
    CvMat ctmpF     = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpF);
    CvMat ctmpE1    = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpE1);
    CvMat ctmpE2    = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpE2);

    /* Compute first */
    cvmMul( &ccamMatr1, &crotMatr1, &cmatrP1);
    cvmInvert( &cmatrP1,&cinvP1 );
    cvmMul( &ccamMatr1, &ctransVect1, &cvectp1 );

    /* Compute second */
    cvmMul( &ccamMatr2, &crotMatr2, &cmatrP2 );
    cvmInvert( &cmatrP2,&cinvP2 );
    cvmMul( &ccamMatr2, &ctransVect2, &cvectp2 );

    cvmMul( &cmatrP1, &cinvP2, &ctmpM1);
    cvmMul( &ctmpM1, &cvectp2, &ctmpVect1);
    cvmSub( &cvectp1,&ctmpVect1,&ctmpE1);

    cvmMul( &cmatrP2, &cinvP1, &ctmpM2);
    cvmMul( &ctmpM2, &cvectp1, &ctmpVect2);
    cvmSub( &cvectp2, &ctmpVect2, &ctmpE2);


    /* Need scale */

    cvmScale(&ctmpE1,&cepipole1,1.0);
    cvmScale(&ctmpE2,&cepipole2,1.0);

    cvmFree(&cmatrP1);
    cvmFree(&cmatrP1);
    cvmFree(&cvectp1);
    cvmFree(&cvectp2);
    cvmFree(&ctmpF1);
    cvmFree(&ctmpM1);
    cvmFree(&ctmpM2);
    cvmFree(&cinvP1);
    cvmFree(&cinvP2);
    cvmFree(&ctmpMatr);
    cvmFree(&ctmpVect1);
    cvmFree(&ctmpVect2);
    cvmFree(&cmatrF1);
    cvmFree(&ctmpF);
    cvmFree(&ctmpE1);
    cvmFree(&ctmpE2);

    return CV_NO_ERR;
}/* cvComputeEpipoles */
#endif

/* Compute epipoles for fundamental matrix */
int cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr,
                                         CvPoint3D32f* epipole1,
                                         CvPoint3D32f* epipole2)
{
    /* Decompose fundamental matrix using SVD ( A = U W V') */
    CvMat fundMatrC = cvMat(3,3,CV_MAT32F,fundMatr);

    CvMat* matrW = cvCreateMat(3,3,CV_MAT32F);
    CvMat* matrU = cvCreateMat(3,3,CV_MAT32F);
    CvMat* matrV = cvCreateMat(3,3,CV_MAT32F);

    /* From svd we need just last vector of U and V or last row from U' and V' */
    /* We get transposed matrixes U and V */
    cvSVD(&fundMatrC,matrW,matrU,matrV,CV_SVD_V_T|CV_SVD_U_T);

    /* Get last row from U' and compute epipole1 */
    epipole1->x = matrU->data.fl[6];
    epipole1->y = matrU->data.fl[7];
    epipole1->z = matrU->data.fl[8];

    /* Get last row from V' and compute epipole2 */
    epipole2->x = matrV->data.fl[6];
    epipole2->y = matrV->data.fl[7];
    epipole2->z = matrV->data.fl[8];

    cvReleaseMat(&matrW);
    cvReleaseMat(&matrU);
    cvReleaseMat(&matrV);
    return CV_OK;
}

int cvConvertEssential2Fundamental( CvMatr32f essMatr,
                                         CvMatr32f fundMatr,
                                         CvMatr32f cameraMatr1,
                                         CvMatr32f cameraMatr2)
{/* Fund = inv(CM1') * Ess * inv(CM2) */

    CvMat essMatrC     = cvMat(3,3,CV_MAT32F,essMatr);
    CvMat fundMatrC    = cvMat(3,3,CV_MAT32F,fundMatr);
    CvMat cameraMatr1C = cvMat(3,3,CV_MAT32F,cameraMatr1);
    CvMat cameraMatr2C = cvMat(3,3,CV_MAT32F,cameraMatr2);

    CvMat* invCM2  = cvCreateMat(3,3,CV_MAT32F);
    CvMat* tmpMatr = cvCreateMat(3,3,CV_MAT32F);
    CvMat* invCM1T = cvCreateMat(3,3,CV_MAT32F);

    cvTranspose(&cameraMatr1C,tmpMatr);
    cvInvert(tmpMatr,invCM1T);
    cvmMul(invCM1T,&essMatrC,tmpMatr);
    cvInvert(&cameraMatr2C,invCM2);
    cvmMul(tmpMatr,invCM2,&fundMatrC);

    /* Scale fundamental matrix */
    double scale;
    scale = 1.0/fundMatrC.data.fl[8];
    cvConvertScale(&fundMatrC,&fundMatrC,scale);

    cvReleaseMat(&invCM2);
    cvReleaseMat(&tmpMatr);
    cvReleaseMat(&invCM1T);

    return CV_OK;
}

/* Compute essential matrix */

int cvComputeEssentialMatrix(  CvMatr32f rotMatr,
                                    CvMatr32f transVect,
                                    CvMatr32f essMatr)
{
    float transMatr[9];

    /* Make antisymmetric matrix from transpose vector */
    transMatr[0] =   0;
    transMatr[1] = - transVect[2];
    transMatr[2] =   transVect[1];

    transMatr[3] =   transVect[2];
    transMatr[4] =   0;
    transMatr[5] = - transVect[0];

    transMatr[6] = - transVect[1];
    transMatr[7] =   transVect[0];
    transMatr[8] =   0;

    icvMulMatrix_32f(transMatr,3,3,rotMatr,3,3,essMatr);

    return CV_OK;
}