cvepilines.cpp.svn-base

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

/*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 "_cvaux.h"
#include "cvtypes.h"
#include <float.h>
#include <limits.h>
#include "cv.h"

/* Valery Mosyagin */

#define EPS64D 1e-9

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

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

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

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



CvStatus 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 */
CvStatus 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 */
CvStatus 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 */
CvStatus 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;
}
/*--------------------------------------------------------------------------------------*/
CvStatus 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;    
}
/*--------------------------------------------------------------------------------------*/
CvStatus 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,9);

    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;

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


    return status;

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



/*--------------------------------------------------------------------------------------*/
CvStatus 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 */
    
    double direct1[3];
    double direct2[3];
    double camPoint1[3];
    
    double directS3[3];
    double directS4[3];
    double direct3[3];
    double direct4[3];
    double camPoint2[3];
    
    icvGetDirectionForPoint(   point1,
                            camMatr1,
                            (CvPoint3D64d*)direct1);
    
    icvGetDirectionForPoint(   point2,
                            camMatr1,
                            (CvPoint3D64d*)direct2);

    /* Direction for camera 2 */

    icvGetDirectionForPoint(   point3,
                            camMatr2,
                            (CvPoint3D64d*)directS3);
    
    icvGetDirectionForPoint(   point4,
                            camMatr2,
                            (CvPoint3D64d*)directS4);

    /* Create convertion for camera 2: two direction and camera point */
    
    double convRotMatr[9];
    double convTransVect[3];

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

    double zeroVect[3];
    zeroVect[0] = zeroVect[1] = zeroVect[2] = 0.0;
    camPoint1[0] = camPoint1[1] = camPoint1[2] = 0.0;
    
    icvConvertPointSystem(*((CvPoint3D64d*)directS3),(CvPoint3D64d*)direct3,convRotMatr,convTransVect);
    icvConvertPointSystem(*((CvPoint3D64d*)directS4),(CvPoint3D64d*)direct4,convRotMatr,convTransVect);
    icvConvertPointSystem(*((CvPoint3D64d*)zeroVect),(CvPoint3D64d*)camPoint2,convRotMatr,convTransVect);

    double pointB[3];
        
    int postype = 0;
    
    /* Changed order */
    /* Compute point B: xB,yB,zB */
    icvGetCrossLines(*((CvPoint3D64d*)camPoint1),*((CvPoint3D64d*)direct2),
                  *((CvPoint3D64d*)camPoint2),*((CvPoint3D64d*)direct3),
                  (CvPoint3D64d*)pointB);

    if( pointB[2] < 0 )/* If negative use other lines for cross */
    {
        postype = 1;
        icvGetCrossLines(*((CvPoint3D64d*)camPoint1),*((CvPoint3D64d*)direct1),
                      *((CvPoint3D64d*)camPoint2),*((CvPoint3D64d*)direct4),
                      (CvPoint3D64d*)pointB);
    }

    CvPoint3D64d pointNewA;
    CvPoint3D64d pointNewC;

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