/*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 */
#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;
}
/*--------------------------------------------------------------------------------------*/
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;
}
/*--------------------------------------------------------------------------------------*/
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 */
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;
if( postype == 0 )
{
icvGetSymPoint3D( *((CvPoint3D64d*)camPoint1),
*((CvPoint3D64d*)direct1),
*((CvPoint3D64d*)pointB),
&pointNewA);
icvGetSymPoint3D( *((CvPoint3D64d*)camPoint2),
*((CvPoint3D64d*)direct4),
*((CvPoint3D64d*)pointB),
&pointNewC);
}
else
{/* In this case we must change cameras */
*needSwapCamera = 1;
icvGetSymPoint3D( *((CvPoint3D64d*)camPoint2),
*((CvPoint3D64d*)direct3),
*((CvPoint3D64d*)pointB),
&pointNewA);
icvGetSymPoint3D( *((CvPoint3D64d*)camPoint1),
*((CvPoint3D64d*)direct2),
*((CvPoint3D64d*)pointB),
&pointNewC);
}
double gamma;
double x1,y1,z1;
x1 = camPoint1[0];
y1 = camPoint1[1];
z1 = camPoint1[2];
double xA,yA,zA;
double xB,yB,zB;
double xC,yC,zC;
xA = pointNewA.x;
yA = pointNewA.y;
zA = pointNewA.z;
xB = pointB[0];
yB = pointB[1];
zB = pointB[2];
xC = pointNewC.x;
yC = pointNewC.y;
zC = pointNewC.z;
double len1,len2;
len1 = sqrt( (xA-xB)*(xA-xB) + (yA-yB)*(yA-yB) + (zA-zB)*(zA-zB) );
len2 = sqrt( (xB-xC)*(xB-xC) + (yB-yC)*(yB-yC) + (zB-zC)*(zB-zC) );
gamma = len2 / len1;
icvComputeStereoLineCoeffs( pointNewA,
*((CvPoint3D64d*)pointB),
*((CvPoint3D64d*)camPoint1),
gamma,
coeffs);
return CV_NO_ERR;
}
/*--------------------------------------------------------------------------------------*/
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 */
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;
}
/*---------------------------------------------------------------------------------------*/
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;
int numPoints = 0;
int res;
int i;
double cutLine1[3];
double cutLine2[3];
/* Find middle line of sector */
double midLine[3];
/* Different way */
CvPoint2D64d pointOnLine1; pointOnLine1.x = pointOnLine1.y = 0;
CvPoint2D64d pointOnLine2; pointOnLine2.x = pointOnLine2.y = 0;
CvPoint2D64d start1,end1;
icvGetCrossRectDirect( imageSize,
areaLineCoef1[0],areaLineCoef1[1],areaLineCoef1[2],
&start1,&end1,&res);
if( res > 0 )
{
pointOnLine1 = start1;
}
icvGetCrossRectDirect( imageSize,
areaLineCoef2[0],areaLineCoef2[1],areaLineCoef2[2],
&start1,&end1,&res);
if( res > 0 )
{
pointOnLine2 = start1;
}
icvGetMiddleAnglePoint(epipole,pointOnLine1,pointOnLine2,&midPoint);
icvGetCoefForPiece(epipole,midPoint,&midLine[0],&midLine[1],&midLine[2],&res);
/* Test corner points */
CvPoint2D64d cornerPoint;
CvPoint2D64d tmpPoints[2];
cornerPoint.x = 0;
cornerPoint.y = 0;
icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
if( res == 1 )
{/* Add point */
candPoints[numPoints] = cornerPoint;
numPoints++;
}
cornerPoint.x = imageSize.width;
cornerPoint.y = 0;
icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
if( res == 1 )
{/* Add point */
candPoints[numPoints] = cornerPoint;
numPoints++;
}
cornerPoint.x = imageSize.width;
cornerPoint.y = imageSize.height;
icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
if( res == 1 )
{/* Add point */
candPoints[numPoints] = cornerPoint;
numPoints++;
}
cornerPoint.x = 0;
cornerPoint.y = imageSize.height;
icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
if( res == 1 )
{/* Add point */
candPoints[numPoints] = cornerPoint;
numPoints++;
}
/* Find cross line 1 with image border */
icvGetCrossRectDirect( imageSize,
areaLineCoef1[0],areaLineCoef1[1],areaLineCoef1[2],
&tmpPoints[0], &tmpPoints[1],
&res);
for( i = 0; i < res; i++ )
{
candPoints[numPoints++] = tmpPoints[i];
}
/* Find cross line 2 with image border */
icvGetCrossRectDirect( imageSize,
areaLineCoef2[0],areaLineCoef2[1],areaLineCoef2[2],
&tmpPoints[0], &tmpPoints[1],
&res);
for( i = 0; i < res; i++ )
{
candPoints[numPoints++] = tmpPoints[i];
}
if( numPoints < 2 )
{
*result = 0;
return;/* Error. Not enought points */
}
/* Project all points to middle line and get max and min */
CvPoint2D64d projPoint;
CvPoint2D64d minPoint; minPoint.x = minPoint.y = FLT_MAX;
CvPoint2D64d maxPoint; maxPoint.x = maxPoint.y = -FLT_MAX;
double dist;
double maxDist = 0;
double minDist = 10000000;
for( i = 0; i < numPoints; i++ )
{
icvProjectPointToDirect(candPoints[i], midLine, &projPoint);
icvGetPieceLength(epipole,projPoint,&dist);
if( dist < minDist)
{
minDist = dist;
minPoint = projPoint;
}
if( dist > maxDist)
{
maxDist = dist;
maxPoint = projPoint;
}
}
/* We know maximum and minimum points. Now we can compute cut lines */
icvGetNormalDirect(midLine,minPoint,cutLine1);
icvGetNormalDirect(midLine,maxPoint,cutLine2);
/* Test for begin of line. */
CvPoint2D64d tmpPoint2;
/* Get cross with */
icvGetCrossDirectDirect(areaLineCoef1,cutLine1,point11,&res);
icvGetCrossDirectDirect(areaLineCoef2,cutLine1,point12,&res);
icvGetCrossDirectDirect(areaLineCoef1,cutLine2,point21,&res);
icvGetCrossDirectDirect(areaLineCoef2,cutLine2,point22,&res);
if( epipole.x > imageSize.width * 0.5 )
{/* Need to change points */
tmpPoint2 = *point11;
*point11 = *point21;
*point21 = tmpPoint2;
tmpPoint2 = *point12;
*point12 = *point22;
*point22 = tmpPoint2;
}
return;
}
/*---------------------------------------------------------------------------------------*/
/* Get middle angle */
void icvGetMiddleAnglePoint( CvPoint2D64d basePoint,
CvPoint2D64d point1,CvPoint2D64d point2,
CvPoint2D64d* midPoint)
{/* !!! May be need to return error */
double dist1;
double dist2;
icvGetPieceLength(basePoint,point1,&dist1);
icvGetPieceLength(basePoint,point2,&dist2);
CvPoint2D64d pointNew1;
CvPoint2D64d pointNew2;
double alpha = dist2/dist1;
pointNew1.x = basePoint.x + (1.0/alpha) * ( point2.x - basePoint.x );
pointNew1.y = basePoint.y + (1.0/alpha) * ( point2.y - basePoint.y );
pointNew2.x = basePoint.x + alpha * ( point1.x - basePoint.x );
pointNew2.y = basePoint.y + alpha * ( point1.y - basePoint.y );
int res;
icvGetCrossPiecePiece(point1,point2,pointNew1,pointNew2,midPoint,&res);
return;
}
/*---------------------------------------------------------------------------------------*/
/* Get normal direct to direct in line */
void icvGetNormalDirect(CvVect64d direct,CvPoint2D64d point,CvVect64d normDirect)
{
normDirect[0] = direct[1];
normDirect[1] = - direct[0];
normDirect[2] = -(normDirect[0]*point.x + normDirect[1]*point.y);
return;
}
/*---------------------------------------------------------------------------------------*/
CV_IMPL double icvGetVect(CvPoint2D64d basePoint,CvPoint2D64d point1,CvPoint2D64d point2)
{
return (point1.x - basePoint.x)*(point2.y - basePoint.y) -
(point2.x - basePoint.x)*(point1.y - basePoint.y);
}
/*---------------------------------------------------------------------------------------*/
/* Test for point in sector */
/* Return 0 - point not inside sector */
/* Return 1 - point inside sector */
void icvTestPoint( CvPoint2D64d testPoint,
CvVect64d line1,CvVect64d line2,
CvPoint2D64d basePoint,
int* result)
{
CvPoint2D64d point1,point2;
icvProjectPointToDirect(testPoint,line1,&point1);
icvProjectPointToDirect(testPoint,line2,&point2);
double sign1 = icvGetVect(basePoint,point1,point2);
double sign2 = icvGetVect(basePoint,point1,testPoint);
if( sign1 * sign2 > 0 )
{/* Correct for first line */
sign1 = - sign1;
sign2 = icvGetVect(basePoint,point2,testPoint);
if( sign1 * sign2 > 0 )
{/* Correct for both lines */
*result = 1;
}
else
{
*result = 0;
}
}
else
{
*result = 0;
}
return;
}
/*---------------------------------------------------------------------------------------*/
/* Project point to line */
void icvProjectPointToDirect( CvPoint2D64d point,CvVect64d lineCoeff,
CvPoint2D64d* projectPoint)
{
double a = lineCoeff[0];
double b = lineCoeff[1];
double det = 1.0 / ( a*a + b*b );
double delta = a*point.y - b*point.x;
projectPoint->x = ( -a*lineCoeff[2] - b * delta ) * det;
projectPoint->y = ( -b*lineCoeff[2] + a * delta ) * det ;
return;
}
/*---------------------------------------------------------------------------------------*/
/* Get distance from point to direction */
void icvGetDistanceFromPointToDirect( CvPoint2D64d point,CvVect64d lineCoef,double*dist)
{
CvPoint2D64d tmpPoint;
icvProjectPointToDirect(point,lineCoef,&tmpPoint);
double dx = point.x - tmpPoint.x;
double dy = point.y - tmpPoint.y;
*dist = sqrt(dx*dx+dy*dy);
return;
}
/*---------------------------------------------------------------------------------------*/
CV_IMPL 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 != dst_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;
}
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 */
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;
}
/*----------------------------------------------------------------------------------*/
/* Find line which cross frame by line(a,b,c) */
void FindLineForEpiline( CvSize imageSize,
float a,float b,float c,
CvPoint2D32f *start,CvPoint2D32f *end,
int* result)
{
result = result;
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;
}
/*----------------------------------------------------------------------------------*/
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;
}
/*--------------------------------------------------------------------------------------*/
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;
}
/*--------------------------------------------------------------------------------------*/
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 */
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;
int i;
for( 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 */
int i;
for( 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);
int i;
for( 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;
}
/* Find line from epipole */
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) */
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 */
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;
}
int icvGetCrossPieceVector(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f v2_start,CvPoint2D32f v2_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 = v2_start.x;
py1 = v2_start.y;
px2 = v2_end.x;
py2 = v2_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 )
{
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;
}
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;
}
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 */
/* 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;
}