mirror of
https://github.com/opencv/opencv.git
synced 2024-12-26 18:58:16 +08:00
2c4bbb313c
Conflicts: cmake/OpenCVConfig.cmake cmake/OpenCVLegacyOptions.cmake modules/contrib/src/retina.cpp modules/gpu/doc/camera_calibration_and_3d_reconstruction.rst modules/gpu/doc/video.rst modules/gpu/src/speckle_filtering.cpp modules/python/src2/cv2.cv.hpp modules/python/test/test2.py samples/python/watershed.py
3701 lines
106 KiB
C++
3701 lines
106 KiB
C++
/*M///////////////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
|
|
//
|
|
// By downloading, copying, installing or using the software you agree to this license.
|
|
// If you do not agree to this license, do not download, install,
|
|
// copy or use the software.
|
|
//
|
|
//
|
|
// Intel License Agreement
|
|
// For Open Source Computer Vision Library
|
|
//
|
|
// Copyright (C) 2000, Intel Corporation, all rights reserved.
|
|
// Third party copyrights are property of their respective owners.
|
|
//
|
|
// Redistribution and use in source and binary forms, with or without modification,
|
|
// are permitted provided that the following conditions are met:
|
|
//
|
|
// * Redistribution's of source code must retain the above copyright notice,
|
|
// this list of conditions and the following disclaimer.
|
|
//
|
|
// * Redistribution's in binary form must reproduce the above copyright notice,
|
|
// this list of conditions and the following disclaimer in the documentation
|
|
// and/or other materials provided with the distribution.
|
|
//
|
|
// * The name of Intel Corporation may not be used to endorse or promote products
|
|
// derived from this software without specific prior written permission.
|
|
//
|
|
// This software is provided by the copyright holders and contributors "as is" and
|
|
// any express or implied warranties, including, but not limited to, the implied
|
|
// warranties of merchantability and fitness for a particular purpose are disclaimed.
|
|
// In no event shall the Intel Corporation or contributors be liable for any direct,
|
|
// indirect, incidental, special, exemplary, or consequential damages
|
|
// (including, but not limited to, procurement of substitute goods or services;
|
|
// loss of use, data, or profits; or business interruption) however caused
|
|
// and on any theory of liability, whether in contract, strict liability,
|
|
// or tort (including negligence or otherwise) arising in any way out of
|
|
// the use of this software, even if advised of the possibility of such damage.
|
|
//
|
|
//M*/
|
|
|
|
|
|
#include "precomp.hpp"
|
|
#include <float.h>
|
|
#include <limits.h>
|
|
|
|
/* Valery Mosyagin */
|
|
|
|
#undef quad
|
|
|
|
#define EPS64D 1e-9
|
|
|
|
int cvComputeEssentialMatrix( CvMatr32f rotMatr,
|
|
CvMatr32f transVect,
|
|
CvMatr32f essMatr);
|
|
|
|
int cvConvertEssential2Fundamental( CvMatr32f essMatr,
|
|
CvMatr32f fundMatr,
|
|
CvMatr32f cameraMatr1,
|
|
CvMatr32f cameraMatr2);
|
|
|
|
int cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr,
|
|
CvPoint3D32f* epipole1,
|
|
CvPoint3D32f* epipole2);
|
|
|
|
void icvTestPoint( CvPoint2D64d testPoint,
|
|
CvVect64d line1,CvVect64d line2,
|
|
CvPoint2D64d basePoint,
|
|
int* result);
|
|
|
|
|
|
|
|
int icvGetSymPoint3D( CvPoint3D64d pointCorner,
|
|
CvPoint3D64d point1,
|
|
CvPoint3D64d point2,
|
|
CvPoint3D64d *pointSym2)
|
|
{
|
|
double len1,len2;
|
|
double alpha;
|
|
icvGetPieceLength3D(pointCorner,point1,&len1);
|
|
if( len1 < EPS64D )
|
|
{
|
|
return CV_BADARG_ERR;
|
|
}
|
|
icvGetPieceLength3D(pointCorner,point2,&len2);
|
|
alpha = len2 / len1;
|
|
|
|
pointSym2->x = pointCorner.x + alpha*(point1.x - pointCorner.x);
|
|
pointSym2->y = pointCorner.y + alpha*(point1.y - pointCorner.y);
|
|
pointSym2->z = pointCorner.z + alpha*(point1.z - pointCorner.z);
|
|
return CV_NO_ERR;
|
|
}
|
|
|
|
/* author Valery Mosyagin */
|
|
|
|
/* Compute 3D point for scanline and alpha betta */
|
|
int icvCompute3DPoint( double alpha,double betta,
|
|
CvStereoLineCoeff* coeffs,
|
|
CvPoint3D64d* point)
|
|
{
|
|
|
|
double partX;
|
|
double partY;
|
|
double partZ;
|
|
double partAll;
|
|
double invPartAll;
|
|
|
|
double alphabetta = alpha*betta;
|
|
|
|
partAll = alpha - betta;
|
|
if( fabs(partAll) > 0.00001 ) /* alpha must be > betta */
|
|
{
|
|
|
|
partX = coeffs->Xcoef + coeffs->XcoefA *alpha +
|
|
coeffs->XcoefB*betta + coeffs->XcoefAB*alphabetta;
|
|
|
|
partY = coeffs->Ycoef + coeffs->YcoefA *alpha +
|
|
coeffs->YcoefB*betta + coeffs->YcoefAB*alphabetta;
|
|
|
|
partZ = coeffs->Zcoef + coeffs->ZcoefA *alpha +
|
|
coeffs->ZcoefB*betta + coeffs->ZcoefAB*alphabetta;
|
|
|
|
invPartAll = 1.0 / partAll;
|
|
|
|
point->x = partX * invPartAll;
|
|
point->y = partY * invPartAll;
|
|
point->z = partZ * invPartAll;
|
|
return CV_NO_ERR;
|
|
}
|
|
else
|
|
{
|
|
return CV_BADFACTOR_ERR;
|
|
}
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------------------*/
|
|
|
|
/* Compute rotate matrix and trans vector for change system */
|
|
int icvCreateConvertMatrVect( CvMatr64d rotMatr1,
|
|
CvMatr64d transVect1,
|
|
CvMatr64d rotMatr2,
|
|
CvMatr64d transVect2,
|
|
CvMatr64d convRotMatr,
|
|
CvMatr64d convTransVect)
|
|
{
|
|
double invRotMatr2[9];
|
|
double tmpVect[3];
|
|
|
|
|
|
icvInvertMatrix_64d(rotMatr2,3,invRotMatr2);
|
|
/* Test for error */
|
|
|
|
icvMulMatrix_64d( rotMatr1,
|
|
3,3,
|
|
invRotMatr2,
|
|
3,3,
|
|
convRotMatr);
|
|
|
|
icvMulMatrix_64d( convRotMatr,
|
|
3,3,
|
|
transVect2,
|
|
1,3,
|
|
tmpVect);
|
|
|
|
icvSubVector_64d(transVect1,tmpVect,convTransVect,3);
|
|
|
|
|
|
return CV_NO_ERR;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------------------*/
|
|
|
|
/* Compute point coordinates in other system */
|
|
int icvConvertPointSystem(CvPoint3D64d M2,
|
|
CvPoint3D64d* M1,
|
|
CvMatr64d rotMatr,
|
|
CvMatr64d transVect
|
|
)
|
|
{
|
|
double tmpVect[3];
|
|
|
|
icvMulMatrix_64d( rotMatr,
|
|
3,3,
|
|
(double*)&M2,
|
|
1,3,
|
|
tmpVect);
|
|
|
|
icvAddVector_64d(tmpVect,transVect,(double*)M1,3);
|
|
|
|
return CV_NO_ERR;
|
|
}
|
|
/*--------------------------------------------------------------------------------------*/
|
|
static int icvComputeCoeffForStereoV3( double quad1[4][2],
|
|
double quad2[4][2],
|
|
int numScanlines,
|
|
CvMatr64d camMatr1,
|
|
CvMatr64d rotMatr1,
|
|
CvMatr64d transVect1,
|
|
CvMatr64d camMatr2,
|
|
CvMatr64d rotMatr2,
|
|
CvMatr64d transVect2,
|
|
CvStereoLineCoeff* startCoeffs,
|
|
int* needSwapCamera)
|
|
{
|
|
/* For each pair */
|
|
/* In this function we must define position of cameras */
|
|
|
|
CvPoint2D64d point1;
|
|
CvPoint2D64d point2;
|
|
CvPoint2D64d point3;
|
|
CvPoint2D64d point4;
|
|
|
|
int currLine;
|
|
*needSwapCamera = 0;
|
|
for( currLine = 0; currLine < numScanlines; currLine++ )
|
|
{
|
|
/* Compute points */
|
|
double alpha = ((double)currLine)/((double)(numScanlines)); /* maybe - 1 */
|
|
|
|
point1.x = (1.0 - alpha) * quad1[0][0] + alpha * quad1[3][0];
|
|
point1.y = (1.0 - alpha) * quad1[0][1] + alpha * quad1[3][1];
|
|
|
|
point2.x = (1.0 - alpha) * quad1[1][0] + alpha * quad1[2][0];
|
|
point2.y = (1.0 - alpha) * quad1[1][1] + alpha * quad1[2][1];
|
|
|
|
point3.x = (1.0 - alpha) * quad2[0][0] + alpha * quad2[3][0];
|
|
point3.y = (1.0 - alpha) * quad2[0][1] + alpha * quad2[3][1];
|
|
|
|
point4.x = (1.0 - alpha) * quad2[1][0] + alpha * quad2[2][0];
|
|
point4.y = (1.0 - alpha) * quad2[1][1] + alpha * quad2[2][1];
|
|
|
|
/* We can compute coeffs for this line */
|
|
icvComCoeffForLine( point1,
|
|
point2,
|
|
point3,
|
|
point4,
|
|
camMatr1,
|
|
rotMatr1,
|
|
transVect1,
|
|
camMatr2,
|
|
rotMatr2,
|
|
transVect2,
|
|
&startCoeffs[currLine],
|
|
needSwapCamera);
|
|
}
|
|
return CV_NO_ERR;
|
|
}
|
|
/*--------------------------------------------------------------------------------------*/
|
|
static int icvComputeCoeffForStereoNew( double quad1[4][2],
|
|
double quad2[4][2],
|
|
int numScanlines,
|
|
CvMatr32f camMatr1,
|
|
CvMatr32f rotMatr1,
|
|
CvMatr32f transVect1,
|
|
CvMatr32f camMatr2,
|
|
CvStereoLineCoeff* startCoeffs,
|
|
int* needSwapCamera)
|
|
{
|
|
/* Convert data */
|
|
|
|
double camMatr1_64d[9];
|
|
double camMatr2_64d[9];
|
|
|
|
double rotMatr1_64d[9];
|
|
double transVect1_64d[3];
|
|
|
|
double rotMatr2_64d[9];
|
|
double transVect2_64d[3];
|
|
|
|
icvCvt_32f_64d(camMatr1,camMatr1_64d,9);
|
|
icvCvt_32f_64d(camMatr2,camMatr2_64d,9);
|
|
|
|
icvCvt_32f_64d(rotMatr1,rotMatr1_64d,9);
|
|
icvCvt_32f_64d(transVect1,transVect1_64d,3);
|
|
|
|
rotMatr2_64d[0] = 1;
|
|
rotMatr2_64d[1] = 0;
|
|
rotMatr2_64d[2] = 0;
|
|
rotMatr2_64d[3] = 0;
|
|
rotMatr2_64d[4] = 1;
|
|
rotMatr2_64d[5] = 0;
|
|
rotMatr2_64d[6] = 0;
|
|
rotMatr2_64d[7] = 0;
|
|
rotMatr2_64d[8] = 1;
|
|
|
|
transVect2_64d[0] = 0;
|
|
transVect2_64d[1] = 0;
|
|
transVect2_64d[2] = 0;
|
|
|
|
int status = icvComputeCoeffForStereoV3( quad1,
|
|
quad2,
|
|
numScanlines,
|
|
camMatr1_64d,
|
|
rotMatr1_64d,
|
|
transVect1_64d,
|
|
camMatr2_64d,
|
|
rotMatr2_64d,
|
|
transVect2_64d,
|
|
startCoeffs,
|
|
needSwapCamera);
|
|
|
|
|
|
return status;
|
|
|
|
}
|
|
/*--------------------------------------------------------------------------------------*/
|
|
int icvComputeCoeffForStereo( CvStereoCamera* stereoCamera)
|
|
{
|
|
double quad1[4][2];
|
|
double quad2[4][2];
|
|
|
|
int i;
|
|
for( i = 0; i < 4; i++ )
|
|
{
|
|
quad1[i][0] = stereoCamera->quad[0][i].x;
|
|
quad1[i][1] = stereoCamera->quad[0][i].y;
|
|
|
|
quad2[i][0] = stereoCamera->quad[1][i].x;
|
|
quad2[i][1] = stereoCamera->quad[1][i].y;
|
|
}
|
|
|
|
icvComputeCoeffForStereoNew( quad1,
|
|
quad2,
|
|
stereoCamera->warpSize.height,
|
|
stereoCamera->camera[0]->matrix,
|
|
stereoCamera->rotMatrix,
|
|
stereoCamera->transVector,
|
|
stereoCamera->camera[1]->matrix,
|
|
stereoCamera->lineCoeffs,
|
|
&(stereoCamera->needSwapCameras));
|
|
return CV_OK;
|
|
}
|
|
|
|
|
|
/*--------------------------------------------------------------------------------------*/
|
|
int icvComCoeffForLine( CvPoint2D64d point1,
|
|
CvPoint2D64d point2,
|
|
CvPoint2D64d point3,
|
|
CvPoint2D64d point4,
|
|
CvMatr64d camMatr1,
|
|
CvMatr64d rotMatr1,
|
|
CvMatr64d transVect1,
|
|
CvMatr64d camMatr2,
|
|
CvMatr64d rotMatr2,
|
|
CvMatr64d transVect2,
|
|
CvStereoLineCoeff* coeffs,
|
|
int* needSwapCamera)
|
|
{
|
|
/* Get direction for all points */
|
|
/* Direction for camera 1 */
|
|
|
|
CvPoint3D64f direct1;
|
|
CvPoint3D64f direct2;
|
|
CvPoint3D64f camPoint1;
|
|
|
|
CvPoint3D64f directS3;
|
|
CvPoint3D64f directS4;
|
|
CvPoint3D64f direct3;
|
|
CvPoint3D64f direct4;
|
|
CvPoint3D64f camPoint2;
|
|
|
|
icvGetDirectionForPoint( point1,
|
|
camMatr1,
|
|
&direct1);
|
|
|
|
icvGetDirectionForPoint( point2,
|
|
camMatr1,
|
|
&direct2);
|
|
|
|
/* Direction for camera 2 */
|
|
|
|
icvGetDirectionForPoint( point3,
|
|
camMatr2,
|
|
&directS3);
|
|
|
|
icvGetDirectionForPoint( point4,
|
|
camMatr2,
|
|
&directS4);
|
|
|
|
/* Create convertion for camera 2: two direction and camera point */
|
|
|
|
double convRotMatr[9];
|
|
double convTransVect[3];
|
|
|
|
icvCreateConvertMatrVect( rotMatr1,
|
|
transVect1,
|
|
rotMatr2,
|
|
transVect2,
|
|
convRotMatr,
|
|
convTransVect);
|
|
|
|
CvPoint3D64f zeroVect;
|
|
zeroVect.x = zeroVect.y = zeroVect.z = 0.0;
|
|
camPoint1.x = camPoint1.y = camPoint1.z = 0.0;
|
|
|
|
icvConvertPointSystem(directS3,&direct3,convRotMatr,convTransVect);
|
|
icvConvertPointSystem(directS4,&direct4,convRotMatr,convTransVect);
|
|
icvConvertPointSystem(zeroVect,&camPoint2,convRotMatr,convTransVect);
|
|
|
|
CvPoint3D64f pointB;
|
|
|
|
int postype = 0;
|
|
|
|
/* Changed order */
|
|
/* Compute point B: xB,yB,zB */
|
|
icvGetCrossLines(camPoint1,direct2,
|
|
camPoint2,direct3,
|
|
&pointB);
|
|
|
|
if( pointB.z < 0 )/* If negative use other lines for cross */
|
|
{
|
|
postype = 1;
|
|
icvGetCrossLines(camPoint1,direct1,
|
|
camPoint2,direct4,
|
|
&pointB);
|
|
}
|
|
|
|
CvPoint3D64d pointNewA;
|
|
CvPoint3D64d pointNewC;
|
|
|
|
pointNewA.x = pointNewA.y = pointNewA.z = 0;
|
|
pointNewC.x = pointNewC.y = pointNewC.z = 0;
|
|
|
|
if( postype == 0 )
|
|
{
|
|
icvGetSymPoint3D( camPoint1,
|
|
direct1,
|
|
pointB,
|
|
&pointNewA);
|
|
|
|
icvGetSymPoint3D( camPoint2,
|
|
direct4,
|
|
pointB,
|
|
&pointNewC);
|
|
}
|
|
else
|
|
{/* In this case we must change cameras */
|
|
*needSwapCamera = 1;
|
|
icvGetSymPoint3D( camPoint2,
|
|
direct3,
|
|
pointB,
|
|
&pointNewA);
|
|
|
|
icvGetSymPoint3D( camPoint1,
|
|
direct2,
|
|
pointB,
|
|
&pointNewC);
|
|
}
|
|
|
|
|
|
double gamma;
|
|
|
|
double xA,yA,zA;
|
|
double xB,yB,zB;
|
|
double xC,yC,zC;
|
|
|
|
xA = pointNewA.x;
|
|
yA = pointNewA.y;
|
|
zA = pointNewA.z;
|
|
|
|
xB = pointB.x;
|
|
yB = pointB.y;
|
|
zB = pointB.z;
|
|
|
|
xC = pointNewC.x;
|
|
yC = pointNewC.y;
|
|
zC = pointNewC.z;
|
|
|
|
double len1,len2;
|
|
len1 = sqrt( (xA-xB)*(xA-xB) + (yA-yB)*(yA-yB) + (zA-zB)*(zA-zB) );
|
|
len2 = sqrt( (xB-xC)*(xB-xC) + (yB-yC)*(yB-yC) + (zB-zC)*(zB-zC) );
|
|
gamma = len2 / len1;
|
|
|
|
icvComputeStereoLineCoeffs( pointNewA,
|
|
pointB,
|
|
camPoint1,
|
|
gamma,
|
|
coeffs);
|
|
|
|
return CV_NO_ERR;
|
|
}
|
|
|
|
|
|
/*--------------------------------------------------------------------------------------*/
|
|
|
|
int icvGetDirectionForPoint( CvPoint2D64d point,
|
|
CvMatr64d camMatr,
|
|
CvPoint3D64d* direct)
|
|
{
|
|
/* */
|
|
double invMatr[9];
|
|
|
|
/* Invert matrix */
|
|
|
|
icvInvertMatrix_64d(camMatr,3,invMatr);
|
|
/* TEST FOR ERRORS */
|
|
|
|
double vect[3];
|
|
vect[0] = point.x;
|
|
vect[1] = point.y;
|
|
vect[2] = 1;
|
|
|
|
/* Mul matr */
|
|
icvMulMatrix_64d( invMatr,
|
|
3,3,
|
|
vect,
|
|
1,3,
|
|
(double*)direct);
|
|
|
|
return CV_NO_ERR;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------------------*/
|
|
|
|
int icvGetCrossLines(CvPoint3D64d point11,CvPoint3D64d point12,
|
|
CvPoint3D64d point21,CvPoint3D64d point22,
|
|
CvPoint3D64d* midPoint)
|
|
{
|
|
double xM,yM,zM;
|
|
double xN,yN,zN;
|
|
|
|
double xA,yA,zA;
|
|
double xB,yB,zB;
|
|
|
|
double xC,yC,zC;
|
|
double xD,yD,zD;
|
|
|
|
xA = point11.x;
|
|
yA = point11.y;
|
|
zA = point11.z;
|
|
|
|
xB = point12.x;
|
|
yB = point12.y;
|
|
zB = point12.z;
|
|
|
|
xC = point21.x;
|
|
yC = point21.y;
|
|
zC = point21.z;
|
|
|
|
xD = point22.x;
|
|
yD = point22.y;
|
|
zD = point22.z;
|
|
|
|
double a11,a12,a21,a22;
|
|
double b1,b2;
|
|
|
|
a11 = (xB-xA)*(xB-xA)+(yB-yA)*(yB-yA)+(zB-zA)*(zB-zA);
|
|
a12 = -(xD-xC)*(xB-xA)-(yD-yC)*(yB-yA)-(zD-zC)*(zB-zA);
|
|
a21 = (xB-xA)*(xD-xC)+(yB-yA)*(yD-yC)+(zB-zA)*(zD-zC);
|
|
a22 = -(xD-xC)*(xD-xC)-(yD-yC)*(yD-yC)-(zD-zC)*(zD-zC);
|
|
b1 = -( (xA-xC)*(xB-xA)+(yA-yC)*(yB-yA)+(zA-zC)*(zB-zA) );
|
|
b2 = -( (xA-xC)*(xD-xC)+(yA-yC)*(yD-yC)+(zA-zC)*(zD-zC) );
|
|
|
|
double delta;
|
|
double deltaA,deltaB;
|
|
double alpha,betta;
|
|
|
|
delta = a11*a22-a12*a21;
|
|
|
|
if( fabs(delta) < EPS64D )
|
|
{
|
|
/*return ERROR;*/
|
|
}
|
|
|
|
deltaA = b1*a22-b2*a12;
|
|
deltaB = a11*b2-b1*a21;
|
|
|
|
alpha = deltaA / delta;
|
|
betta = deltaB / delta;
|
|
|
|
xM = xA+alpha*(xB-xA);
|
|
yM = yA+alpha*(yB-yA);
|
|
zM = zA+alpha*(zB-zA);
|
|
|
|
xN = xC+betta*(xD-xC);
|
|
yN = yC+betta*(yD-yC);
|
|
zN = zC+betta*(zD-zC);
|
|
|
|
/* Compute middle point */
|
|
midPoint->x = (xM + xN) * 0.5;
|
|
midPoint->y = (yM + yN) * 0.5;
|
|
midPoint->z = (zM + zN) * 0.5;
|
|
|
|
return CV_NO_ERR;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------------------*/
|
|
|
|
int icvComputeStereoLineCoeffs( CvPoint3D64d pointA,
|
|
CvPoint3D64d pointB,
|
|
CvPoint3D64d pointCam1,
|
|
double gamma,
|
|
CvStereoLineCoeff* coeffs)
|
|
{
|
|
double x1,y1,z1;
|
|
|
|
x1 = pointCam1.x;
|
|
y1 = pointCam1.y;
|
|
z1 = pointCam1.z;
|
|
|
|
double xA,yA,zA;
|
|
double xB,yB,zB;
|
|
|
|
xA = pointA.x;
|
|
yA = pointA.y;
|
|
zA = pointA.z;
|
|
|
|
xB = pointB.x;
|
|
yB = pointB.y;
|
|
zB = pointB.z;
|
|
|
|
if( gamma > 0 )
|
|
{
|
|
coeffs->Xcoef = -x1 + xA;
|
|
coeffs->XcoefA = xB + x1 - xA;
|
|
coeffs->XcoefB = -xA - gamma * x1 + gamma * xA;
|
|
coeffs->XcoefAB = -xB + xA + gamma * xB - gamma * xA;
|
|
|
|
coeffs->Ycoef = -y1 + yA;
|
|
coeffs->YcoefA = yB + y1 - yA;
|
|
coeffs->YcoefB = -yA - gamma * y1 + gamma * yA;
|
|
coeffs->YcoefAB = -yB + yA + gamma * yB - gamma * yA;
|
|
|
|
coeffs->Zcoef = -z1 + zA;
|
|
coeffs->ZcoefA = zB + z1 - zA;
|
|
coeffs->ZcoefB = -zA - gamma * z1 + gamma * zA;
|
|
coeffs->ZcoefAB = -zB + zA + gamma * zB - gamma * zA;
|
|
}
|
|
else
|
|
{
|
|
gamma = - gamma;
|
|
coeffs->Xcoef = -( -x1 + xA);
|
|
coeffs->XcoefB = -( xB + x1 - xA);
|
|
coeffs->XcoefA = -( -xA - gamma * x1 + gamma * xA);
|
|
coeffs->XcoefAB = -( -xB + xA + gamma * xB - gamma * xA);
|
|
|
|
coeffs->Ycoef = -( -y1 + yA);
|
|
coeffs->YcoefB = -( yB + y1 - yA);
|
|
coeffs->YcoefA = -( -yA - gamma * y1 + gamma * yA);
|
|
coeffs->YcoefAB = -( -yB + yA + gamma * yB - gamma * yA);
|
|
|
|
coeffs->Zcoef = -( -z1 + zA);
|
|
coeffs->ZcoefB = -( zB + z1 - zA);
|
|
coeffs->ZcoefA = -( -zA - gamma * z1 + gamma * zA);
|
|
coeffs->ZcoefAB = -( -zB + zA + gamma * zB - gamma * zA);
|
|
}
|
|
|
|
|
|
|
|
return CV_NO_ERR;
|
|
}
|
|
/*--------------------------------------------------------------------------------------*/
|
|
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
|
|
/* This function get minimum angle started at point which contains rect */
|
|
int icvGetAngleLine( CvPoint2D64d startPoint, CvSize imageSize,CvPoint2D64d *point1,CvPoint2D64d *point2)
|
|
{
|
|
/* Get crosslines with image corners */
|
|
|
|
/* Find four lines */
|
|
|
|
CvPoint2D64d pa,pb,pc,pd;
|
|
|
|
pa.x = 0;
|
|
pa.y = 0;
|
|
|
|
pb.x = imageSize.width-1;
|
|
pb.y = 0;
|
|
|
|
pd.x = imageSize.width-1;
|
|
pd.y = imageSize.height-1;
|
|
|
|
pc.x = 0;
|
|
pc.y = imageSize.height-1;
|
|
|
|
/* We can compute points for angle */
|
|
/* Test for place section */
|
|
|
|
if( startPoint.x < 0 )
|
|
{/* 1,4,7 */
|
|
if( startPoint.y < 0)
|
|
{/* 1 */
|
|
*point1 = pb;
|
|
*point2 = pc;
|
|
}
|
|
else if( startPoint.y > imageSize.height-1 )
|
|
{/* 7 */
|
|
*point1 = pa;
|
|
*point2 = pd;
|
|
}
|
|
else
|
|
{/* 4 */
|
|
*point1 = pa;
|
|
*point2 = pc;
|
|
}
|
|
}
|
|
else if ( startPoint.x > imageSize.width-1 )
|
|
{/* 3,6,9 */
|
|
if( startPoint.y < 0 )
|
|
{/* 3 */
|
|
*point1 = pa;
|
|
*point2 = pd;
|
|
}
|
|
else if ( startPoint.y > imageSize.height-1 )
|
|
{/* 9 */
|
|
*point1 = pb;
|
|
*point2 = pc;
|
|
}
|
|
else
|
|
{/* 6 */
|
|
*point1 = pb;
|
|
*point2 = pd;
|
|
}
|
|
}
|
|
else
|
|
{/* 2,5,8 */
|
|
if( startPoint.y < 0 )
|
|
{/* 2 */
|
|
if( startPoint.x < imageSize.width/2 )
|
|
{
|
|
*point1 = pb;
|
|
*point2 = pa;
|
|
}
|
|
else
|
|
{
|
|
*point1 = pa;
|
|
*point2 = pb;
|
|
}
|
|
}
|
|
else if( startPoint.y > imageSize.height-1 )
|
|
{/* 8 */
|
|
if( startPoint.x < imageSize.width/2 )
|
|
{
|
|
*point1 = pc;
|
|
*point2 = pd;
|
|
}
|
|
else
|
|
{
|
|
*point1 = pd;
|
|
*point2 = pc;
|
|
}
|
|
}
|
|
else
|
|
{/* 5 - point in the image */
|
|
return 2;
|
|
}
|
|
}
|
|
return 0;
|
|
}/* GetAngleLine */
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
|
|
void icvGetCoefForPiece( CvPoint2D64d p_start,CvPoint2D64d p_end,
|
|
double *a,double *b,double *c,
|
|
int* result)
|
|
{
|
|
double det;
|
|
double detA,detB,detC;
|
|
|
|
det = p_start.x*p_end.y+p_end.x+p_start.y-p_end.y-p_start.y*p_end.x-p_start.x;
|
|
if( fabs(det) < EPS64D)/* Error */
|
|
{
|
|
*result = 0;
|
|
return;
|
|
}
|
|
|
|
detA = p_start.y - p_end.y;
|
|
detB = p_end.x - p_start.x;
|
|
detC = p_start.x*p_end.y - p_end.x*p_start.y;
|
|
|
|
double invDet = 1.0 / det;
|
|
*a = detA * invDet;
|
|
*b = detB * invDet;
|
|
*c = detC * invDet;
|
|
|
|
*result = 1;
|
|
return;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
|
|
/* Get common area of rectifying */
|
|
static void icvGetCommonArea( CvSize imageSize,
|
|
CvPoint3D64d epipole1,CvPoint3D64d epipole2,
|
|
CvMatr64d fundMatr,
|
|
CvVect64d coeff11,CvVect64d coeff12,
|
|
CvVect64d coeff21,CvVect64d coeff22,
|
|
int* result)
|
|
{
|
|
int res = 0;
|
|
CvPoint2D64d point11;
|
|
CvPoint2D64d point12;
|
|
CvPoint2D64d point21;
|
|
CvPoint2D64d point22;
|
|
|
|
double corr11[3];
|
|
double corr12[3];
|
|
double corr21[3];
|
|
double corr22[3];
|
|
|
|
double pointW11[3];
|
|
double pointW12[3];
|
|
double pointW21[3];
|
|
double pointW22[3];
|
|
|
|
double transFundMatr[3*3];
|
|
/* Compute transpose of fundamental matrix */
|
|
icvTransposeMatrix_64d( fundMatr, 3, 3, transFundMatr );
|
|
|
|
CvPoint2D64d epipole1_2d;
|
|
CvPoint2D64d epipole2_2d;
|
|
|
|
if( fabs(epipole1.z) < 1e-8 )
|
|
{/* epipole1 in infinity */
|
|
*result = 0;
|
|
return;
|
|
}
|
|
epipole1_2d.x = epipole1.x / epipole1.z;
|
|
epipole1_2d.y = epipole1.y / epipole1.z;
|
|
|
|
if( fabs(epipole2.z) < 1e-8 )
|
|
{/* epipole2 in infinity */
|
|
*result = 0;
|
|
return;
|
|
}
|
|
epipole2_2d.x = epipole2.x / epipole2.z;
|
|
epipole2_2d.y = epipole2.y / epipole2.z;
|
|
|
|
int stat = icvGetAngleLine( epipole1_2d, imageSize,&point11,&point12);
|
|
if( stat == 2 )
|
|
{
|
|
/* No angle */
|
|
*result = 0;
|
|
return;
|
|
}
|
|
|
|
stat = icvGetAngleLine( epipole2_2d, imageSize,&point21,&point22);
|
|
if( stat == 2 )
|
|
{
|
|
/* No angle */
|
|
*result = 0;
|
|
return;
|
|
}
|
|
|
|
/* ============= Computation for line 1 ================ */
|
|
/* Find correspondence line for angle points11 */
|
|
/* corr21 = Fund'*p1 */
|
|
|
|
pointW11[0] = point11.x;
|
|
pointW11[1] = point11.y;
|
|
pointW11[2] = 1.0;
|
|
|
|
icvTransformVector_64d( transFundMatr, /* !!! Modified from not transposed */
|
|
pointW11,
|
|
corr21,
|
|
3,3);
|
|
|
|
/* Find crossing of line with image 2 */
|
|
CvPoint2D64d start;
|
|
CvPoint2D64d end;
|
|
icvGetCrossRectDirect( imageSize,
|
|
corr21[0],corr21[1],corr21[2],
|
|
&start,&end,
|
|
&res);
|
|
|
|
if( res == 0 )
|
|
{/* We have not cross */
|
|
/* We must define new angle */
|
|
|
|
pointW21[0] = point21.x;
|
|
pointW21[1] = point21.y;
|
|
pointW21[2] = 1.0;
|
|
|
|
/* Find correspondence line for this angle points */
|
|
/* We know point and try to get corr line */
|
|
/* For point21 */
|
|
/* corr11 = Fund * p21 */
|
|
|
|
icvTransformVector_64d( fundMatr, /* !!! Modified */
|
|
pointW21,
|
|
corr11,
|
|
3,3);
|
|
|
|
/* We have cross. And it's result cross for up line. Set result coefs */
|
|
|
|
/* Set coefs for line 1 image 1 */
|
|
coeff11[0] = corr11[0];
|
|
coeff11[1] = corr11[1];
|
|
coeff11[2] = corr11[2];
|
|
|
|
/* Set coefs for line 1 image 2 */
|
|
icvGetCoefForPiece( epipole2_2d,point21,
|
|
&coeff21[0],&coeff21[1],&coeff21[2],
|
|
&res);
|
|
if( res == 0 )
|
|
{
|
|
*result = 0;
|
|
return;/* Error */
|
|
}
|
|
}
|
|
else
|
|
{/* Line 1 cross image 2 */
|
|
/* Set coefs for line 1 image 1 */
|
|
icvGetCoefForPiece( epipole1_2d,point11,
|
|
&coeff11[0],&coeff11[1],&coeff11[2],
|
|
&res);
|
|
if( res == 0 )
|
|
{
|
|
*result = 0;
|
|
return;/* Error */
|
|
}
|
|
|
|
/* Set coefs for line 1 image 2 */
|
|
coeff21[0] = corr21[0];
|
|
coeff21[1] = corr21[1];
|
|
coeff21[2] = corr21[2];
|
|
|
|
}
|
|
|
|
/* ============= Computation for line 2 ================ */
|
|
/* Find correspondence line for angle points11 */
|
|
/* corr22 = Fund*p2 */
|
|
|
|
pointW12[0] = point12.x;
|
|
pointW12[1] = point12.y;
|
|
pointW12[2] = 1.0;
|
|
|
|
icvTransformVector_64d( transFundMatr,
|
|
pointW12,
|
|
corr22,
|
|
3,3);
|
|
|
|
/* Find crossing of line with image 2 */
|
|
icvGetCrossRectDirect( imageSize,
|
|
corr22[0],corr22[1],corr22[2],
|
|
&start,&end,
|
|
&res);
|
|
|
|
if( res == 0 )
|
|
{/* We have not cross */
|
|
/* We must define new angle */
|
|
|
|
pointW22[0] = point22.x;
|
|
pointW22[1] = point22.y;
|
|
pointW22[2] = 1.0;
|
|
|
|
/* Find correspondence line for this angle points */
|
|
/* We know point and try to get corr line */
|
|
/* For point21 */
|
|
/* corr2 = Fund' * p1 */
|
|
|
|
icvTransformVector_64d( fundMatr,
|
|
pointW22,
|
|
corr12,
|
|
3,3);
|
|
|
|
|
|
/* We have cross. And it's result cross for down line. Set result coefs */
|
|
|
|
/* Set coefs for line 2 image 1 */
|
|
coeff12[0] = corr12[0];
|
|
coeff12[1] = corr12[1];
|
|
coeff12[2] = corr12[2];
|
|
|
|
/* Set coefs for line 1 image 2 */
|
|
icvGetCoefForPiece( epipole2_2d,point22,
|
|
&coeff22[0],&coeff22[1],&coeff22[2],
|
|
&res);
|
|
if( res == 0 )
|
|
{
|
|
*result = 0;
|
|
return;/* Error */
|
|
}
|
|
}
|
|
else
|
|
{/* Line 2 cross image 2 */
|
|
/* Set coefs for line 2 image 1 */
|
|
icvGetCoefForPiece( epipole1_2d,point12,
|
|
&coeff12[0],&coeff12[1],&coeff12[2],
|
|
&res);
|
|
if( res == 0 )
|
|
{
|
|
*result = 0;
|
|
return;/* Error */
|
|
}
|
|
|
|
/* Set coefs for line 1 image 2 */
|
|
coeff22[0] = corr22[0];
|
|
coeff22[1] = corr22[1];
|
|
coeff22[2] = corr22[2];
|
|
|
|
}
|
|
|
|
/* Now we know common area */
|
|
|
|
return;
|
|
|
|
}/* GetCommonArea */
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
|
|
/* Get cross for direction1 and direction2 */
|
|
/* Result = 1 - cross */
|
|
/* Result = 2 - parallel and not equal */
|
|
/* Result = 3 - parallel and equal */
|
|
|
|
void icvGetCrossDirectDirect( CvVect64d direct1,CvVect64d direct2,
|
|
CvPoint2D64d *cross,int* result)
|
|
{
|
|
double det = direct1[0]*direct2[1] - direct2[0]*direct1[1];
|
|
double detx = -direct1[2]*direct2[1] + direct1[1]*direct2[2];
|
|
|
|
if( fabs(det) > EPS64D )
|
|
{/* Have cross */
|
|
cross->x = detx/det;
|
|
cross->y = (-direct1[0]*direct2[2] + direct2[0]*direct1[2])/det;
|
|
*result = 1;
|
|
}
|
|
else
|
|
{/* may be parallel */
|
|
if( fabs(detx) > EPS64D )
|
|
{/* parallel and not equal */
|
|
*result = 2;
|
|
}
|
|
else
|
|
{/* equals */
|
|
*result = 3;
|
|
}
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
|
|
/* Get cross for piece p1,p2 and direction a,b,c */
|
|
/* Result = 0 - no cross */
|
|
/* Result = 1 - cross */
|
|
/* Result = 2 - parallel and not equal */
|
|
/* Result = 3 - parallel and equal */
|
|
|
|
void icvGetCrossPieceDirect( CvPoint2D64d p_start,CvPoint2D64d p_end,
|
|
double a,double b,double c,
|
|
CvPoint2D64d *cross,int* result)
|
|
{
|
|
|
|
if( (a*p_start.x + b*p_start.y + c) * (a*p_end.x + b*p_end.y + c) <= 0 )
|
|
{/* Have cross */
|
|
double det;
|
|
double detxc,detyc;
|
|
|
|
det = a * (p_end.x - p_start.x) + b * (p_end.y - p_start.y);
|
|
|
|
if( fabs(det) < EPS64D )
|
|
{/* lines are parallel and may be equal or line is point */
|
|
if( fabs(a*p_start.x + b*p_start.y + c) < EPS64D )
|
|
{/* line is point or not diff */
|
|
*result = 3;
|
|
return;
|
|
}
|
|
else
|
|
{
|
|
*result = 2;
|
|
}
|
|
return;
|
|
}
|
|
|
|
detxc = b*(p_end.y*p_start.x - p_start.y*p_end.x) + c*(p_start.x - p_end.x);
|
|
detyc = a*(p_end.x*p_start.y - p_start.x*p_end.y) + c*(p_start.y - p_end.y);
|
|
|
|
cross->x = detxc / det;
|
|
cross->y = detyc / det;
|
|
*result = 1;
|
|
|
|
}
|
|
else
|
|
{
|
|
*result = 0;
|
|
}
|
|
return;
|
|
}
|
|
/*--------------------------------------------------------------------------------------*/
|
|
|
|
void icvGetCrossPiecePiece( CvPoint2D64d p1_start,CvPoint2D64d p1_end,
|
|
CvPoint2D64d p2_start,CvPoint2D64d p2_end,
|
|
CvPoint2D64d* cross,
|
|
int* result)
|
|
{
|
|
double ex1,ey1,ex2,ey2;
|
|
double px1,py1,px2,py2;
|
|
double del;
|
|
double delA,delB,delX,delY;
|
|
double alpha,betta;
|
|
|
|
ex1 = p1_start.x;
|
|
ey1 = p1_start.y;
|
|
ex2 = p1_end.x;
|
|
ey2 = p1_end.y;
|
|
|
|
px1 = p2_start.x;
|
|
py1 = p2_start.y;
|
|
px2 = p2_end.x;
|
|
py2 = p2_end.y;
|
|
|
|
del = (py1-py2)*(ex1-ex2)-(px1-px2)*(ey1-ey2);
|
|
if( fabs(del) <= EPS64D )
|
|
{/* May be they are parallel !!! */
|
|
*result = 0;
|
|
return;
|
|
}
|
|
|
|
delA = (ey1-ey2)*(ex1-px1) + (ex1-ex2)*(py1-ey1);
|
|
delB = (py1-py2)*(ex1-px1) + (px1-px2)*(py1-ey1);
|
|
|
|
alpha = delA / del;
|
|
betta = delB / del;
|
|
|
|
if( alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0)
|
|
{
|
|
*result = 0;
|
|
return;
|
|
}
|
|
|
|
delX = (px1-px2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2))+
|
|
(ex1-ex2)*(px1*(py1-py2)-py1*(px1-px2));
|
|
|
|
delY = (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2))+
|
|
(ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2));
|
|
|
|
cross->x = delX / del;
|
|
cross->y = delY / del;
|
|
|
|
*result = 1;
|
|
return;
|
|
}
|
|
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
|
|
void icvGetPieceLength(CvPoint2D64d point1,CvPoint2D64d point2,double* dist)
|
|
{
|
|
double dx = point2.x - point1.x;
|
|
double dy = point2.y - point1.y;
|
|
*dist = sqrt( dx*dx + dy*dy );
|
|
return;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
|
|
void icvGetPieceLength3D(CvPoint3D64d point1,CvPoint3D64d point2,double* dist)
|
|
{
|
|
double dx = point2.x - point1.x;
|
|
double dy = point2.y - point1.y;
|
|
double dz = point2.z - point1.z;
|
|
*dist = sqrt( dx*dx + dy*dy + dz*dz );
|
|
return;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
|
|
/* Find line from epipole which cross image rect */
|
|
/* Find points of cross 0 or 1 or 2. Return number of points in cross */
|
|
void icvGetCrossRectDirect( CvSize imageSize,
|
|
double a,double b,double c,
|
|
CvPoint2D64d *start,CvPoint2D64d *end,
|
|
int* result)
|
|
{
|
|
CvPoint2D64d frameBeg;
|
|
CvPoint2D64d frameEnd;
|
|
CvPoint2D64d cross[4];
|
|
int haveCross[4];
|
|
|
|
haveCross[0] = 0;
|
|
haveCross[1] = 0;
|
|
haveCross[2] = 0;
|
|
haveCross[3] = 0;
|
|
|
|
frameBeg.x = 0;
|
|
frameBeg.y = 0;
|
|
frameEnd.x = imageSize.width;
|
|
frameEnd.y = 0;
|
|
|
|
icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[0],&haveCross[0]);
|
|
|
|
frameBeg.x = imageSize.width;
|
|
frameBeg.y = 0;
|
|
frameEnd.x = imageSize.width;
|
|
frameEnd.y = imageSize.height;
|
|
icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[1],&haveCross[1]);
|
|
|
|
frameBeg.x = imageSize.width;
|
|
frameBeg.y = imageSize.height;
|
|
frameEnd.x = 0;
|
|
frameEnd.y = imageSize.height;
|
|
icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[2],&haveCross[2]);
|
|
|
|
frameBeg.x = 0;
|
|
frameBeg.y = imageSize.height;
|
|
frameEnd.x = 0;
|
|
frameEnd.y = 0;
|
|
icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[3],&haveCross[3]);
|
|
|
|
double maxDist;
|
|
|
|
int maxI=0,maxJ=0;
|
|
|
|
|
|
int i,j;
|
|
|
|
maxDist = -1.0;
|
|
|
|
double distance;
|
|
|
|
for( i = 0; i < 3; i++ )
|
|
{
|
|
if( haveCross[i] == 1 )
|
|
{
|
|
for( j = i + 1; j < 4; j++ )
|
|
{
|
|
if( haveCross[j] == 1)
|
|
{/* Compute dist */
|
|
icvGetPieceLength(cross[i],cross[j],&distance);
|
|
if( distance > maxDist )
|
|
{
|
|
maxI = i;
|
|
maxJ = j;
|
|
maxDist = distance;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if( maxDist >= 0 )
|
|
{/* We have cross */
|
|
*start = cross[maxI];
|
|
*result = 1;
|
|
if( maxDist > 0 )
|
|
{
|
|
*end = cross[maxJ];
|
|
*result = 2;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
*result = 0;
|
|
}
|
|
|
|
return;
|
|
}/* GetCrossRectDirect */
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
void icvProjectPointToImage( CvPoint3D64d point,
|
|
CvMatr64d camMatr,CvMatr64d rotMatr,CvVect64d transVect,
|
|
CvPoint2D64d* projPoint)
|
|
{
|
|
|
|
double tmpVect1[3];
|
|
double tmpVect2[3];
|
|
|
|
icvMulMatrix_64d ( rotMatr,
|
|
3,3,
|
|
(double*)&point,
|
|
1,3,
|
|
tmpVect1);
|
|
|
|
icvAddVector_64d ( tmpVect1, transVect,tmpVect2, 3);
|
|
|
|
icvMulMatrix_64d ( camMatr,
|
|
3,3,
|
|
tmpVect2,
|
|
1,3,
|
|
tmpVect1);
|
|
|
|
projPoint->x = tmpVect1[0] / tmpVect1[2];
|
|
projPoint->y = tmpVect1[1] / tmpVect1[2];
|
|
|
|
return;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
/* Get quads for transform images */
|
|
void icvGetQuadsTransform(
|
|
CvSize imageSize,
|
|
CvMatr64d camMatr1,
|
|
CvMatr64d rotMatr1,
|
|
CvVect64d transVect1,
|
|
CvMatr64d camMatr2,
|
|
CvMatr64d rotMatr2,
|
|
CvVect64d transVect2,
|
|
CvSize* warpSize,
|
|
double quad1[4][2],
|
|
double quad2[4][2],
|
|
CvMatr64d fundMatr,
|
|
CvPoint3D64d* epipole1,
|
|
CvPoint3D64d* epipole2
|
|
)
|
|
{
|
|
/* First compute fundamental matrix and epipoles */
|
|
int res;
|
|
|
|
|
|
/* Compute epipoles and fundamental matrix using new functions */
|
|
{
|
|
double convRotMatr[9];
|
|
double convTransVect[3];
|
|
|
|
icvCreateConvertMatrVect( rotMatr1,
|
|
transVect1,
|
|
rotMatr2,
|
|
transVect2,
|
|
convRotMatr,
|
|
convTransVect);
|
|
float convRotMatr_32f[9];
|
|
float convTransVect_32f[3];
|
|
|
|
icvCvt_64d_32f(convRotMatr,convRotMatr_32f,9);
|
|
icvCvt_64d_32f(convTransVect,convTransVect_32f,3);
|
|
|
|
/* We know R and t */
|
|
/* Compute essential matrix */
|
|
float essMatr[9];
|
|
float fundMatr_32f[9];
|
|
|
|
float camMatr1_32f[9];
|
|
float camMatr2_32f[9];
|
|
|
|
icvCvt_64d_32f(camMatr1,camMatr1_32f,9);
|
|
icvCvt_64d_32f(camMatr2,camMatr2_32f,9);
|
|
|
|
cvComputeEssentialMatrix( convRotMatr_32f,
|
|
convTransVect_32f,
|
|
essMatr);
|
|
|
|
cvConvertEssential2Fundamental( essMatr,
|
|
fundMatr_32f,
|
|
camMatr1_32f,
|
|
camMatr2_32f);
|
|
|
|
CvPoint3D32f epipole1_32f;
|
|
CvPoint3D32f epipole2_32f;
|
|
|
|
cvComputeEpipolesFromFundMatrix( fundMatr_32f,
|
|
&epipole1_32f,
|
|
&epipole2_32f);
|
|
/* copy to 64d epipoles */
|
|
epipole1->x = epipole1_32f.x;
|
|
epipole1->y = epipole1_32f.y;
|
|
epipole1->z = epipole1_32f.z;
|
|
|
|
epipole2->x = epipole2_32f.x;
|
|
epipole2->y = epipole2_32f.y;
|
|
epipole2->z = epipole2_32f.z;
|
|
|
|
/* Convert fundamental matrix */
|
|
icvCvt_32f_64d(fundMatr_32f,fundMatr,9);
|
|
}
|
|
|
|
double coeff11[3];
|
|
double coeff12[3];
|
|
double coeff21[3];
|
|
double coeff22[3];
|
|
|
|
icvGetCommonArea( imageSize,
|
|
*epipole1,*epipole2,
|
|
fundMatr,
|
|
coeff11,coeff12,
|
|
coeff21,coeff22,
|
|
&res);
|
|
|
|
CvPoint2D64d point11, point12,point21, point22;
|
|
double width1,width2;
|
|
double height1,height2;
|
|
double tmpHeight1,tmpHeight2;
|
|
|
|
CvPoint2D64d epipole1_2d;
|
|
CvPoint2D64d epipole2_2d;
|
|
|
|
/* ----- Image 1 ----- */
|
|
if( fabs(epipole1->z) < 1e-8 )
|
|
{
|
|
return;
|
|
}
|
|
epipole1_2d.x = epipole1->x / epipole1->z;
|
|
epipole1_2d.y = epipole1->y / epipole1->z;
|
|
|
|
icvGetCutPiece( coeff11,coeff12,
|
|
epipole1_2d,
|
|
imageSize,
|
|
&point11,&point12,
|
|
&point21,&point22,
|
|
&res);
|
|
|
|
/* Compute distance */
|
|
icvGetPieceLength(point11,point21,&width1);
|
|
icvGetPieceLength(point11,point12,&tmpHeight1);
|
|
icvGetPieceLength(point21,point22,&tmpHeight2);
|
|
height1 = MAX(tmpHeight1,tmpHeight2);
|
|
|
|
quad1[0][0] = point11.x;
|
|
quad1[0][1] = point11.y;
|
|
|
|
quad1[1][0] = point21.x;
|
|
quad1[1][1] = point21.y;
|
|
|
|
quad1[2][0] = point22.x;
|
|
quad1[2][1] = point22.y;
|
|
|
|
quad1[3][0] = point12.x;
|
|
quad1[3][1] = point12.y;
|
|
|
|
/* ----- Image 2 ----- */
|
|
if( fabs(epipole2->z) < 1e-8 )
|
|
{
|
|
return;
|
|
}
|
|
epipole2_2d.x = epipole2->x / epipole2->z;
|
|
epipole2_2d.y = epipole2->y / epipole2->z;
|
|
|
|
icvGetCutPiece( coeff21,coeff22,
|
|
epipole2_2d,
|
|
imageSize,
|
|
&point11,&point12,
|
|
&point21,&point22,
|
|
&res);
|
|
|
|
/* Compute distance */
|
|
icvGetPieceLength(point11,point21,&width2);
|
|
icvGetPieceLength(point11,point12,&tmpHeight1);
|
|
icvGetPieceLength(point21,point22,&tmpHeight2);
|
|
height2 = MAX(tmpHeight1,tmpHeight2);
|
|
|
|
quad2[0][0] = point11.x;
|
|
quad2[0][1] = point11.y;
|
|
|
|
quad2[1][0] = point21.x;
|
|
quad2[1][1] = point21.y;
|
|
|
|
quad2[2][0] = point22.x;
|
|
quad2[2][1] = point22.y;
|
|
|
|
quad2[3][0] = point12.x;
|
|
quad2[3][1] = point12.y;
|
|
|
|
|
|
/*=======================================================*/
|
|
/* This is a new additional way to compute quads. */
|
|
/* We must correct quads */
|
|
{
|
|
double convRotMatr[9];
|
|
double convTransVect[3];
|
|
|
|
double newQuad1[4][2];
|
|
double newQuad2[4][2];
|
|
|
|
|
|
icvCreateConvertMatrVect( rotMatr1,
|
|
transVect1,
|
|
rotMatr2,
|
|
transVect2,
|
|
convRotMatr,
|
|
convTransVect);
|
|
|
|
/* -------------Compute for first image-------------- */
|
|
CvPoint2D32f pointb1;
|
|
CvPoint2D32f pointe1;
|
|
|
|
CvPoint2D32f pointb2;
|
|
CvPoint2D32f pointe2;
|
|
|
|
pointb1.x = (float)quad1[0][0];
|
|
pointb1.y = (float)quad1[0][1];
|
|
|
|
pointe1.x = (float)quad1[3][0];
|
|
pointe1.y = (float)quad1[3][1];
|
|
|
|
icvComputeeInfiniteProject1(convRotMatr,
|
|
camMatr1,
|
|
camMatr2,
|
|
pointb1,
|
|
&pointb2);
|
|
|
|
icvComputeeInfiniteProject1(convRotMatr,
|
|
camMatr1,
|
|
camMatr2,
|
|
pointe1,
|
|
&pointe2);
|
|
|
|
/* JUST TEST FOR POINT */
|
|
|
|
/* Compute distances */
|
|
double dxOld,dyOld;
|
|
double dxNew,dyNew;
|
|
double distOld,distNew;
|
|
|
|
dxOld = quad2[1][0] - quad2[0][0];
|
|
dyOld = quad2[1][1] - quad2[0][1];
|
|
distOld = dxOld*dxOld + dyOld*dyOld;
|
|
|
|
dxNew = quad2[1][0] - pointb2.x;
|
|
dyNew = quad2[1][1] - pointb2.y;
|
|
distNew = dxNew*dxNew + dyNew*dyNew;
|
|
|
|
if( distNew > distOld )
|
|
{/* Get new points for second quad */
|
|
newQuad2[0][0] = pointb2.x;
|
|
newQuad2[0][1] = pointb2.y;
|
|
newQuad2[3][0] = pointe2.x;
|
|
newQuad2[3][1] = pointe2.y;
|
|
newQuad1[0][0] = quad1[0][0];
|
|
newQuad1[0][1] = quad1[0][1];
|
|
newQuad1[3][0] = quad1[3][0];
|
|
newQuad1[3][1] = quad1[3][1];
|
|
}
|
|
else
|
|
{/* Get new points for first quad */
|
|
|
|
pointb2.x = (float)quad2[0][0];
|
|
pointb2.y = (float)quad2[0][1];
|
|
|
|
pointe2.x = (float)quad2[3][0];
|
|
pointe2.y = (float)quad2[3][1];
|
|
|
|
icvComputeeInfiniteProject2(convRotMatr,
|
|
camMatr1,
|
|
camMatr2,
|
|
&pointb1,
|
|
pointb2);
|
|
|
|
icvComputeeInfiniteProject2(convRotMatr,
|
|
camMatr1,
|
|
camMatr2,
|
|
&pointe1,
|
|
pointe2);
|
|
|
|
|
|
/* JUST TEST FOR POINT */
|
|
|
|
newQuad2[0][0] = quad2[0][0];
|
|
newQuad2[0][1] = quad2[0][1];
|
|
newQuad2[3][0] = quad2[3][0];
|
|
newQuad2[3][1] = quad2[3][1];
|
|
|
|
newQuad1[0][0] = pointb1.x;
|
|
newQuad1[0][1] = pointb1.y;
|
|
newQuad1[3][0] = pointe1.x;
|
|
newQuad1[3][1] = pointe1.y;
|
|
}
|
|
|
|
/* -------------Compute for second image-------------- */
|
|
pointb1.x = (float)quad1[1][0];
|
|
pointb1.y = (float)quad1[1][1];
|
|
|
|
pointe1.x = (float)quad1[2][0];
|
|
pointe1.y = (float)quad1[2][1];
|
|
|
|
icvComputeeInfiniteProject1(convRotMatr,
|
|
camMatr1,
|
|
camMatr2,
|
|
pointb1,
|
|
&pointb2);
|
|
|
|
icvComputeeInfiniteProject1(convRotMatr,
|
|
camMatr1,
|
|
camMatr2,
|
|
pointe1,
|
|
&pointe2);
|
|
|
|
/* Compute distances */
|
|
|
|
dxOld = quad2[0][0] - quad2[1][0];
|
|
dyOld = quad2[0][1] - quad2[1][1];
|
|
distOld = dxOld*dxOld + dyOld*dyOld;
|
|
|
|
dxNew = quad2[0][0] - pointb2.x;
|
|
dyNew = quad2[0][1] - pointb2.y;
|
|
distNew = dxNew*dxNew + dyNew*dyNew;
|
|
|
|
if( distNew > distOld )
|
|
{/* Get new points for second quad */
|
|
newQuad2[1][0] = pointb2.x;
|
|
newQuad2[1][1] = pointb2.y;
|
|
newQuad2[2][0] = pointe2.x;
|
|
newQuad2[2][1] = pointe2.y;
|
|
newQuad1[1][0] = quad1[1][0];
|
|
newQuad1[1][1] = quad1[1][1];
|
|
newQuad1[2][0] = quad1[2][0];
|
|
newQuad1[2][1] = quad1[2][1];
|
|
}
|
|
else
|
|
{/* Get new points for first quad */
|
|
|
|
pointb2.x = (float)quad2[1][0];
|
|
pointb2.y = (float)quad2[1][1];
|
|
|
|
pointe2.x = (float)quad2[2][0];
|
|
pointe2.y = (float)quad2[2][1];
|
|
|
|
icvComputeeInfiniteProject2(convRotMatr,
|
|
camMatr1,
|
|
camMatr2,
|
|
&pointb1,
|
|
pointb2);
|
|
|
|
icvComputeeInfiniteProject2(convRotMatr,
|
|
camMatr1,
|
|
camMatr2,
|
|
&pointe1,
|
|
pointe2);
|
|
|
|
newQuad2[1][0] = quad2[1][0];
|
|
newQuad2[1][1] = quad2[1][1];
|
|
newQuad2[2][0] = quad2[2][0];
|
|
newQuad2[2][1] = quad2[2][1];
|
|
|
|
newQuad1[1][0] = pointb1.x;
|
|
newQuad1[1][1] = pointb1.y;
|
|
newQuad1[2][0] = pointe1.x;
|
|
newQuad1[2][1] = pointe1.y;
|
|
}
|
|
|
|
|
|
|
|
/*-------------------------------------------------------------------------------*/
|
|
|
|
/* Copy new quads to old quad */
|
|
int i;
|
|
for( i = 0; i < 4; i++ )
|
|
{
|
|
{
|
|
quad1[i][0] = newQuad1[i][0];
|
|
quad1[i][1] = newQuad1[i][1];
|
|
quad2[i][0] = newQuad2[i][0];
|
|
quad2[i][1] = newQuad2[i][1];
|
|
}
|
|
}
|
|
}
|
|
/*=======================================================*/
|
|
|
|
double warpWidth,warpHeight;
|
|
|
|
warpWidth = MAX(width1,width2);
|
|
warpHeight = MAX(height1,height2);
|
|
|
|
warpSize->width = (int)warpWidth;
|
|
warpSize->height = (int)warpHeight;
|
|
|
|
warpSize->width = cvRound(warpWidth-1);
|
|
warpSize->height = cvRound(warpHeight-1);
|
|
|
|
/* !!! by Valery Mosyagin. this lines added just for test no warp */
|
|
warpSize->width = imageSize.width;
|
|
warpSize->height = imageSize.height;
|
|
|
|
return;
|
|
}
|
|
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
|
|
static void icvGetQuadsTransformNew( CvSize imageSize,
|
|
CvMatr32f camMatr1,
|
|
CvMatr32f camMatr2,
|
|
CvMatr32f rotMatr1,
|
|
CvVect32f transVect1,
|
|
CvSize* warpSize,
|
|
double quad1[4][2],
|
|
double quad2[4][2],
|
|
CvMatr32f fundMatr,
|
|
CvPoint3D32f* epipole1,
|
|
CvPoint3D32f* epipole2
|
|
)
|
|
{
|
|
/* Convert data */
|
|
/* Convert camera matrix */
|
|
double camMatr1_64d[9];
|
|
double camMatr2_64d[9];
|
|
double rotMatr1_64d[9];
|
|
double transVect1_64d[3];
|
|
double rotMatr2_64d[9];
|
|
double transVect2_64d[3];
|
|
double fundMatr_64d[9];
|
|
CvPoint3D64d epipole1_64d;
|
|
CvPoint3D64d epipole2_64d;
|
|
|
|
icvCvt_32f_64d(camMatr1,camMatr1_64d,9);
|
|
icvCvt_32f_64d(camMatr2,camMatr2_64d,9);
|
|
icvCvt_32f_64d(rotMatr1,rotMatr1_64d,9);
|
|
icvCvt_32f_64d(transVect1,transVect1_64d,3);
|
|
|
|
/* Create vector and matrix */
|
|
|
|
rotMatr2_64d[0] = 1;
|
|
rotMatr2_64d[1] = 0;
|
|
rotMatr2_64d[2] = 0;
|
|
rotMatr2_64d[3] = 0;
|
|
rotMatr2_64d[4] = 1;
|
|
rotMatr2_64d[5] = 0;
|
|
rotMatr2_64d[6] = 0;
|
|
rotMatr2_64d[7] = 0;
|
|
rotMatr2_64d[8] = 1;
|
|
|
|
transVect2_64d[0] = 0;
|
|
transVect2_64d[1] = 0;
|
|
transVect2_64d[2] = 0;
|
|
|
|
icvGetQuadsTransform( imageSize,
|
|
camMatr1_64d,
|
|
rotMatr1_64d,
|
|
transVect1_64d,
|
|
camMatr2_64d,
|
|
rotMatr2_64d,
|
|
transVect2_64d,
|
|
warpSize,
|
|
quad1,
|
|
quad2,
|
|
fundMatr_64d,
|
|
&epipole1_64d,
|
|
&epipole2_64d
|
|
);
|
|
|
|
/* Convert epipoles */
|
|
epipole1->x = (float)(epipole1_64d.x);
|
|
epipole1->y = (float)(epipole1_64d.y);
|
|
epipole1->z = (float)(epipole1_64d.z);
|
|
|
|
epipole2->x = (float)(epipole2_64d.x);
|
|
epipole2->y = (float)(epipole2_64d.y);
|
|
epipole2->z = (float)(epipole2_64d.z);
|
|
|
|
/* Convert fundamental matrix */
|
|
icvCvt_64d_32f(fundMatr_64d,fundMatr,9);
|
|
|
|
return;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
void icvGetQuadsTransformStruct( CvStereoCamera* stereoCamera)
|
|
{
|
|
/* Wrapper for icvGetQuadsTransformNew */
|
|
|
|
|
|
double quad1[4][2];
|
|
double quad2[4][2];
|
|
|
|
icvGetQuadsTransformNew( cvSize(cvRound(stereoCamera->camera[0]->imgSize[0]),cvRound(stereoCamera->camera[0]->imgSize[1])),
|
|
stereoCamera->camera[0]->matrix,
|
|
stereoCamera->camera[1]->matrix,
|
|
stereoCamera->rotMatrix,
|
|
stereoCamera->transVector,
|
|
&(stereoCamera->warpSize),
|
|
quad1,
|
|
quad2,
|
|
stereoCamera->fundMatr,
|
|
&(stereoCamera->epipole[0]),
|
|
&(stereoCamera->epipole[1])
|
|
);
|
|
|
|
int i;
|
|
for( i = 0; i < 4; i++ )
|
|
{
|
|
stereoCamera->quad[0][i] = cvPoint2D32f(quad1[i][0],quad1[i][1]);
|
|
stereoCamera->quad[1][i] = cvPoint2D32f(quad2[i][0],quad2[i][1]);
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
void icvComputeStereoParamsForCameras(CvStereoCamera* stereoCamera)
|
|
{
|
|
/* For given intrinsic and extrinsic parameters computes rest parameters
|
|
** such as fundamental matrix. warping coeffs, epipoles, ...
|
|
*/
|
|
|
|
|
|
/* compute rotate matrix and translate vector */
|
|
double rotMatr1[9];
|
|
double rotMatr2[9];
|
|
|
|
double transVect1[3];
|
|
double transVect2[3];
|
|
|
|
double convRotMatr[9];
|
|
double convTransVect[3];
|
|
|
|
/* fill matrices */
|
|
icvCvt_32f_64d(stereoCamera->camera[0]->rotMatr,rotMatr1,9);
|
|
icvCvt_32f_64d(stereoCamera->camera[1]->rotMatr,rotMatr2,9);
|
|
|
|
icvCvt_32f_64d(stereoCamera->camera[0]->transVect,transVect1,3);
|
|
icvCvt_32f_64d(stereoCamera->camera[1]->transVect,transVect2,3);
|
|
|
|
icvCreateConvertMatrVect( rotMatr1,
|
|
transVect1,
|
|
rotMatr2,
|
|
transVect2,
|
|
convRotMatr,
|
|
convTransVect);
|
|
|
|
/* copy to stereo camera params */
|
|
icvCvt_64d_32f(convRotMatr,stereoCamera->rotMatrix,9);
|
|
icvCvt_64d_32f(convTransVect,stereoCamera->transVector,3);
|
|
|
|
|
|
icvGetQuadsTransformStruct(stereoCamera);
|
|
icvComputeRestStereoParams(stereoCamera);
|
|
}
|
|
|
|
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
|
|
/* Get cut line for one image */
|
|
void icvGetCutPiece( CvVect64d areaLineCoef1,CvVect64d areaLineCoef2,
|
|
CvPoint2D64d epipole,
|
|
CvSize imageSize,
|
|
CvPoint2D64d* point11,CvPoint2D64d* point12,
|
|
CvPoint2D64d* point21,CvPoint2D64d* point22,
|
|
int* result)
|
|
{
|
|
/* Compute nearest cut line to epipole */
|
|
/* Get corners inside sector */
|
|
/* Collect all candidate point */
|
|
|
|
CvPoint2D64d candPoints[8];
|
|
CvPoint2D64d midPoint;
|
|
int numPoints = 0;
|
|
int res;
|
|
int i;
|
|
|
|
double cutLine1[3];
|
|
double cutLine2[3];
|
|
|
|
/* Find middle line of sector */
|
|
double midLine[3]={0,0,0};
|
|
|
|
|
|
/* Different way */
|
|
CvPoint2D64d pointOnLine1; pointOnLine1.x = pointOnLine1.y = 0;
|
|
CvPoint2D64d pointOnLine2; pointOnLine2.x = pointOnLine2.y = 0;
|
|
|
|
CvPoint2D64d start1,end1;
|
|
|
|
icvGetCrossRectDirect( imageSize,
|
|
areaLineCoef1[0],areaLineCoef1[1],areaLineCoef1[2],
|
|
&start1,&end1,&res);
|
|
if( res > 0 )
|
|
{
|
|
pointOnLine1 = start1;
|
|
}
|
|
|
|
icvGetCrossRectDirect( imageSize,
|
|
areaLineCoef2[0],areaLineCoef2[1],areaLineCoef2[2],
|
|
&start1,&end1,&res);
|
|
if( res > 0 )
|
|
{
|
|
pointOnLine2 = start1;
|
|
}
|
|
|
|
icvGetMiddleAnglePoint(epipole,pointOnLine1,pointOnLine2,&midPoint);
|
|
|
|
icvGetCoefForPiece(epipole,midPoint,&midLine[0],&midLine[1],&midLine[2],&res);
|
|
|
|
/* Test corner points */
|
|
CvPoint2D64d cornerPoint;
|
|
CvPoint2D64d tmpPoints[2];
|
|
|
|
cornerPoint.x = 0;
|
|
cornerPoint.y = 0;
|
|
icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
|
|
if( res == 1 )
|
|
{/* Add point */
|
|
candPoints[numPoints] = cornerPoint;
|
|
numPoints++;
|
|
}
|
|
|
|
cornerPoint.x = imageSize.width;
|
|
cornerPoint.y = 0;
|
|
icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
|
|
if( res == 1 )
|
|
{/* Add point */
|
|
candPoints[numPoints] = cornerPoint;
|
|
numPoints++;
|
|
}
|
|
|
|
cornerPoint.x = imageSize.width;
|
|
cornerPoint.y = imageSize.height;
|
|
icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
|
|
if( res == 1 )
|
|
{/* Add point */
|
|
candPoints[numPoints] = cornerPoint;
|
|
numPoints++;
|
|
}
|
|
|
|
cornerPoint.x = 0;
|
|
cornerPoint.y = imageSize.height;
|
|
icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res);
|
|
if( res == 1 )
|
|
{/* Add point */
|
|
candPoints[numPoints] = cornerPoint;
|
|
numPoints++;
|
|
}
|
|
|
|
/* Find cross line 1 with image border */
|
|
icvGetCrossRectDirect( imageSize,
|
|
areaLineCoef1[0],areaLineCoef1[1],areaLineCoef1[2],
|
|
&tmpPoints[0], &tmpPoints[1],
|
|
&res);
|
|
for( i = 0; i < res; i++ )
|
|
{
|
|
candPoints[numPoints++] = tmpPoints[i];
|
|
}
|
|
|
|
/* Find cross line 2 with image border */
|
|
icvGetCrossRectDirect( imageSize,
|
|
areaLineCoef2[0],areaLineCoef2[1],areaLineCoef2[2],
|
|
&tmpPoints[0], &tmpPoints[1],
|
|
&res);
|
|
|
|
for( i = 0; i < res; i++ )
|
|
{
|
|
candPoints[numPoints++] = tmpPoints[i];
|
|
}
|
|
|
|
if( numPoints < 2 )
|
|
{
|
|
*result = 0;
|
|
return;/* Error. Not enought points */
|
|
}
|
|
/* Project all points to middle line and get max and min */
|
|
|
|
CvPoint2D64d projPoint;
|
|
CvPoint2D64d minPoint; minPoint.x = minPoint.y = FLT_MAX;
|
|
CvPoint2D64d maxPoint; maxPoint.x = maxPoint.y = -FLT_MAX;
|
|
|
|
|
|
double dist;
|
|
double maxDist = 0;
|
|
double minDist = 10000000;
|
|
|
|
|
|
for( i = 0; i < numPoints; i++ )
|
|
{
|
|
icvProjectPointToDirect(candPoints[i], midLine, &projPoint);
|
|
icvGetPieceLength(epipole,projPoint,&dist);
|
|
if( dist < minDist)
|
|
{
|
|
minDist = dist;
|
|
minPoint = projPoint;
|
|
}
|
|
|
|
if( dist > maxDist)
|
|
{
|
|
maxDist = dist;
|
|
maxPoint = projPoint;
|
|
}
|
|
}
|
|
|
|
/* We know maximum and minimum points. Now we can compute cut lines */
|
|
|
|
icvGetNormalDirect(midLine,minPoint,cutLine1);
|
|
icvGetNormalDirect(midLine,maxPoint,cutLine2);
|
|
|
|
/* Test for begin of line. */
|
|
CvPoint2D64d tmpPoint2;
|
|
|
|
/* Get cross with */
|
|
icvGetCrossDirectDirect(areaLineCoef1,cutLine1,point11,&res);
|
|
icvGetCrossDirectDirect(areaLineCoef2,cutLine1,point12,&res);
|
|
|
|
icvGetCrossDirectDirect(areaLineCoef1,cutLine2,point21,&res);
|
|
icvGetCrossDirectDirect(areaLineCoef2,cutLine2,point22,&res);
|
|
|
|
if( epipole.x > imageSize.width * 0.5 )
|
|
{/* Need to change points */
|
|
tmpPoint2 = *point11;
|
|
*point11 = *point21;
|
|
*point21 = tmpPoint2;
|
|
|
|
tmpPoint2 = *point12;
|
|
*point12 = *point22;
|
|
*point22 = tmpPoint2;
|
|
}
|
|
|
|
return;
|
|
}
|
|
/*---------------------------------------------------------------------------------------*/
|
|
/* Get middle angle */
|
|
void icvGetMiddleAnglePoint( CvPoint2D64d basePoint,
|
|
CvPoint2D64d point1,CvPoint2D64d point2,
|
|
CvPoint2D64d* midPoint)
|
|
{/* !!! May be need to return error */
|
|
|
|
double dist1;
|
|
double dist2;
|
|
icvGetPieceLength(basePoint,point1,&dist1);
|
|
icvGetPieceLength(basePoint,point2,&dist2);
|
|
CvPoint2D64d pointNew1;
|
|
CvPoint2D64d pointNew2;
|
|
double alpha = dist2/dist1;
|
|
|
|
pointNew1.x = basePoint.x + (1.0/alpha) * ( point2.x - basePoint.x );
|
|
pointNew1.y = basePoint.y + (1.0/alpha) * ( point2.y - basePoint.y );
|
|
|
|
pointNew2.x = basePoint.x + alpha * ( point1.x - basePoint.x );
|
|
pointNew2.y = basePoint.y + alpha * ( point1.y - basePoint.y );
|
|
|
|
int res;
|
|
icvGetCrossPiecePiece(point1,point2,pointNew1,pointNew2,midPoint,&res);
|
|
|
|
return;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
/* Get normal direct to direct in line */
|
|
void icvGetNormalDirect(CvVect64d direct,CvPoint2D64d point,CvVect64d normDirect)
|
|
{
|
|
normDirect[0] = direct[1];
|
|
normDirect[1] = - direct[0];
|
|
normDirect[2] = -(normDirect[0]*point.x + normDirect[1]*point.y);
|
|
return;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
CV_IMPL double icvGetVect(CvPoint2D64d basePoint,CvPoint2D64d point1,CvPoint2D64d point2)
|
|
{
|
|
return (point1.x - basePoint.x)*(point2.y - basePoint.y) -
|
|
(point2.x - basePoint.x)*(point1.y - basePoint.y);
|
|
}
|
|
/*---------------------------------------------------------------------------------------*/
|
|
/* Test for point in sector */
|
|
/* Return 0 - point not inside sector */
|
|
/* Return 1 - point inside sector */
|
|
void icvTestPoint( CvPoint2D64d testPoint,
|
|
CvVect64d line1,CvVect64d line2,
|
|
CvPoint2D64d basePoint,
|
|
int* result)
|
|
{
|
|
CvPoint2D64d point1,point2;
|
|
|
|
icvProjectPointToDirect(testPoint,line1,&point1);
|
|
icvProjectPointToDirect(testPoint,line2,&point2);
|
|
|
|
double sign1 = icvGetVect(basePoint,point1,point2);
|
|
double sign2 = icvGetVect(basePoint,point1,testPoint);
|
|
if( sign1 * sign2 > 0 )
|
|
{/* Correct for first line */
|
|
sign1 = - sign1;
|
|
sign2 = icvGetVect(basePoint,point2,testPoint);
|
|
if( sign1 * sign2 > 0 )
|
|
{/* Correct for both lines */
|
|
*result = 1;
|
|
}
|
|
else
|
|
{
|
|
*result = 0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
*result = 0;
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
/* Project point to line */
|
|
void icvProjectPointToDirect( CvPoint2D64d point,CvVect64d lineCoeff,
|
|
CvPoint2D64d* projectPoint)
|
|
{
|
|
double a = lineCoeff[0];
|
|
double b = lineCoeff[1];
|
|
|
|
double det = 1.0 / ( a*a + b*b );
|
|
double delta = a*point.y - b*point.x;
|
|
|
|
projectPoint->x = ( -a*lineCoeff[2] - b * delta ) * det;
|
|
projectPoint->y = ( -b*lineCoeff[2] + a * delta ) * det ;
|
|
|
|
return;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------------------*/
|
|
/* Get distance from point to direction */
|
|
void icvGetDistanceFromPointToDirect( CvPoint2D64d point,CvVect64d lineCoef,double*dist)
|
|
{
|
|
CvPoint2D64d tmpPoint;
|
|
icvProjectPointToDirect(point,lineCoef,&tmpPoint);
|
|
double dx = point.x - tmpPoint.x;
|
|
double dy = point.y - tmpPoint.y;
|
|
*dist = sqrt(dx*dx+dy*dy);
|
|
return;
|
|
}
|
|
/*---------------------------------------------------------------------------------------*/
|
|
|
|
CV_IMPL IplImage* icvCreateIsometricImage( IplImage* src, IplImage* dst,
|
|
int desired_depth, int desired_num_channels )
|
|
{
|
|
CvSize src_size ;
|
|
src_size.width = src->width;
|
|
src_size.height = src->height;
|
|
|
|
CvSize dst_size = src_size;
|
|
|
|
if( dst )
|
|
{
|
|
dst_size.width = dst->width;
|
|
dst_size.height = dst->height;
|
|
}
|
|
|
|
if( !dst || dst->depth != desired_depth ||
|
|
dst->nChannels != desired_num_channels ||
|
|
dst_size.width != src_size.width ||
|
|
dst_size.height != src_size.height )
|
|
{
|
|
cvReleaseImage( &dst );
|
|
dst = cvCreateImage( src_size, desired_depth, desired_num_channels );
|
|
CvRect rect = cvRect(0,0,src_size.width,src_size.height);
|
|
cvSetImageROI( dst, rect );
|
|
|
|
}
|
|
|
|
return dst;
|
|
}
|
|
|
|
static int
|
|
icvCvt_32f_64d( float *src, double *dst, int size )
|
|
{
|
|
int t;
|
|
|
|
if( !src || !dst )
|
|
return CV_NULLPTR_ERR;
|
|
if( size <= 0 )
|
|
return CV_BADRANGE_ERR;
|
|
|
|
for( t = 0; t < size; t++ )
|
|
{
|
|
dst[t] = (double) (src[t]);
|
|
}
|
|
|
|
return CV_OK;
|
|
}
|
|
|
|
/*======================================================================================*/
|
|
/* Type conversion double -> float */
|
|
static int
|
|
icvCvt_64d_32f( double *src, float *dst, int size )
|
|
{
|
|
int t;
|
|
|
|
if( !src || !dst )
|
|
return CV_NULLPTR_ERR;
|
|
if( size <= 0 )
|
|
return CV_BADRANGE_ERR;
|
|
|
|
for( t = 0; t < size; t++ )
|
|
{
|
|
dst[t] = (float) (src[t]);
|
|
}
|
|
|
|
return CV_OK;
|
|
}
|
|
|
|
/*----------------------------------------------------------------------------------*/
|
|
|
|
#if 0
|
|
/* Find line which cross frame by line(a,b,c) */
|
|
static void FindLineForEpiline( CvSize imageSize,
|
|
float a,float b,float c,
|
|
CvPoint2D32f *start,CvPoint2D32f *end,
|
|
int*)
|
|
{
|
|
CvPoint2D32f frameBeg;
|
|
|
|
CvPoint2D32f frameEnd;
|
|
CvPoint2D32f cross[4];
|
|
int haveCross[4];
|
|
float dist;
|
|
|
|
haveCross[0] = 0;
|
|
haveCross[1] = 0;
|
|
haveCross[2] = 0;
|
|
haveCross[3] = 0;
|
|
|
|
frameBeg.x = 0;
|
|
frameBeg.y = 0;
|
|
frameEnd.x = (float)(imageSize.width);
|
|
frameEnd.y = 0;
|
|
haveCross[0] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[0]);
|
|
|
|
frameBeg.x = (float)(imageSize.width);
|
|
frameBeg.y = 0;
|
|
frameEnd.x = (float)(imageSize.width);
|
|
frameEnd.y = (float)(imageSize.height);
|
|
haveCross[1] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[1]);
|
|
|
|
frameBeg.x = (float)(imageSize.width);
|
|
frameBeg.y = (float)(imageSize.height);
|
|
frameEnd.x = 0;
|
|
frameEnd.y = (float)(imageSize.height);
|
|
haveCross[2] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[2]);
|
|
|
|
frameBeg.x = 0;
|
|
frameBeg.y = (float)(imageSize.height);
|
|
frameEnd.x = 0;
|
|
frameEnd.y = 0;
|
|
haveCross[3] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[3]);
|
|
|
|
int n;
|
|
float minDist = (float)(INT_MAX);
|
|
float maxDist = (float)(INT_MIN);
|
|
|
|
int maxN = -1;
|
|
int minN = -1;
|
|
|
|
double midPointX = imageSize.width / 2.0;
|
|
double midPointY = imageSize.height / 2.0;
|
|
|
|
for( n = 0; n < 4; n++ )
|
|
{
|
|
if( haveCross[n] > 0 )
|
|
{
|
|
dist = (float)((midPointX - cross[n].x)*(midPointX - cross[n].x) +
|
|
(midPointY - cross[n].y)*(midPointY - cross[n].y));
|
|
|
|
if( dist < minDist )
|
|
{
|
|
minDist = dist;
|
|
minN = n;
|
|
}
|
|
|
|
if( dist > maxDist )
|
|
{
|
|
maxDist = dist;
|
|
maxN = n;
|
|
}
|
|
}
|
|
}
|
|
|
|
if( minN >= 0 && maxN >= 0 && (minN != maxN) )
|
|
{
|
|
*start = cross[minN];
|
|
*end = cross[maxN];
|
|
}
|
|
else
|
|
{
|
|
start->x = 0;
|
|
start->y = 0;
|
|
end->x = 0;
|
|
end->y = 0;
|
|
}
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
/*----------------------------------------------------------------------------------*/
|
|
static int GetAngleLinee( CvPoint2D32f epipole, CvSize imageSize,CvPoint2D32f point1,CvPoint2D32f point2)
|
|
{
|
|
float width = (float)(imageSize.width);
|
|
float height = (float)(imageSize.height);
|
|
|
|
/* Get crosslines with image corners */
|
|
|
|
/* Find four lines */
|
|
|
|
CvPoint2D32f pa,pb,pc,pd;
|
|
|
|
pa.x = 0;
|
|
pa.y = 0;
|
|
|
|
pb.x = width;
|
|
pb.y = 0;
|
|
|
|
pd.x = width;
|
|
pd.y = height;
|
|
|
|
pc.x = 0;
|
|
pc.y = height;
|
|
|
|
/* We can compute points for angle */
|
|
/* Test for place section */
|
|
|
|
float x,y;
|
|
x = epipole.x;
|
|
y = epipole.y;
|
|
|
|
if( x < 0 )
|
|
{/* 1,4,7 */
|
|
if( y < 0)
|
|
{/* 1 */
|
|
point1 = pb;
|
|
point2 = pc;
|
|
}
|
|
else if( y > height )
|
|
{/* 7 */
|
|
point1 = pa;
|
|
point2 = pd;
|
|
}
|
|
else
|
|
{/* 4 */
|
|
point1 = pa;
|
|
point2 = pc;
|
|
}
|
|
}
|
|
else if ( x > width )
|
|
{/* 3,6,9 */
|
|
if( y < 0 )
|
|
{/* 3 */
|
|
point1 = pa;
|
|
point2 = pd;
|
|
}
|
|
else if ( y > height )
|
|
{/* 9 */
|
|
point1 = pc;
|
|
point2 = pb;
|
|
}
|
|
else
|
|
{/* 6 */
|
|
point1 = pb;
|
|
point2 = pd;
|
|
}
|
|
}
|
|
else
|
|
{/* 2,5,8 */
|
|
if( y < 0 )
|
|
{/* 2 */
|
|
point1 = pa;
|
|
point2 = pb;
|
|
}
|
|
else if( y > height )
|
|
{/* 8 */
|
|
point1 = pc;
|
|
point2 = pd;
|
|
}
|
|
else
|
|
{/* 5 - point in the image */
|
|
return 2;
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
return 0;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------------------*/
|
|
static void icvComputePerspectiveCoeffs(const CvPoint2D32f srcQuad[4],const CvPoint2D32f dstQuad[4],double coeffs[3][3])
|
|
{/* Computes perspective coeffs for transformation from src to dst quad */
|
|
|
|
|
|
CV_FUNCNAME( "icvComputePerspectiveCoeffs" );
|
|
|
|
__BEGIN__;
|
|
|
|
double A[64];
|
|
double b[8];
|
|
double c[8];
|
|
CvPoint2D32f pt[4];
|
|
int i;
|
|
|
|
pt[0] = srcQuad[0];
|
|
pt[1] = srcQuad[1];
|
|
pt[2] = srcQuad[2];
|
|
pt[3] = srcQuad[3];
|
|
|
|
for( i = 0; i < 4; i++ )
|
|
{
|
|
#if 0
|
|
double x = dstQuad[i].x;
|
|
double y = dstQuad[i].y;
|
|
double X = pt[i].x;
|
|
double Y = pt[i].y;
|
|
#else
|
|
double x = pt[i].x;
|
|
double y = pt[i].y;
|
|
double X = dstQuad[i].x;
|
|
double Y = dstQuad[i].y;
|
|
#endif
|
|
double* a = A + i*16;
|
|
|
|
a[0] = x;
|
|
a[1] = y;
|
|
a[2] = 1;
|
|
a[3] = 0;
|
|
a[4] = 0;
|
|
a[5] = 0;
|
|
a[6] = -X*x;
|
|
a[7] = -X*y;
|
|
|
|
a += 8;
|
|
|
|
a[0] = 0;
|
|
a[1] = 0;
|
|
a[2] = 0;
|
|
a[3] = x;
|
|
a[4] = y;
|
|
a[5] = 1;
|
|
a[6] = -Y*x;
|
|
a[7] = -Y*y;
|
|
|
|
b[i*2] = X;
|
|
b[i*2 + 1] = Y;
|
|
}
|
|
|
|
{
|
|
double invA[64];
|
|
CvMat matA = cvMat( 8, 8, CV_64F, A );
|
|
CvMat matInvA = cvMat( 8, 8, CV_64F, invA );
|
|
CvMat matB = cvMat( 8, 1, CV_64F, b );
|
|
CvMat matX = cvMat( 8, 1, CV_64F, c );
|
|
|
|
CV_CALL( cvPseudoInverse( &matA, &matInvA ));
|
|
CV_CALL( cvMatMulAdd( &matInvA, &matB, 0, &matX ));
|
|
}
|
|
|
|
coeffs[0][0] = c[0];
|
|
coeffs[0][1] = c[1];
|
|
coeffs[0][2] = c[2];
|
|
coeffs[1][0] = c[3];
|
|
coeffs[1][1] = c[4];
|
|
coeffs[1][2] = c[5];
|
|
coeffs[2][0] = c[6];
|
|
coeffs[2][1] = c[7];
|
|
coeffs[2][2] = 1.0;
|
|
|
|
__END__;
|
|
|
|
return;
|
|
}
|
|
#endif
|
|
|
|
/*--------------------------------------------------------------------------------------*/
|
|
|
|
CV_IMPL void cvComputePerspectiveMap(const double c[3][3], CvArr* rectMapX, CvArr* rectMapY )
|
|
{
|
|
CV_FUNCNAME( "cvComputePerspectiveMap" );
|
|
|
|
__BEGIN__;
|
|
|
|
CvSize size;
|
|
CvMat stubx, *mapx = (CvMat*)rectMapX;
|
|
CvMat stuby, *mapy = (CvMat*)rectMapY;
|
|
int i, j;
|
|
|
|
CV_CALL( mapx = cvGetMat( mapx, &stubx ));
|
|
CV_CALL( mapy = cvGetMat( mapy, &stuby ));
|
|
|
|
if( CV_MAT_TYPE( mapx->type ) != CV_32FC1 || CV_MAT_TYPE( mapy->type ) != CV_32FC1 )
|
|
CV_ERROR( CV_StsUnsupportedFormat, "" );
|
|
|
|
size = cvGetMatSize(mapx);
|
|
assert( fabs(c[2][2] - 1.) < FLT_EPSILON );
|
|
|
|
for( i = 0; i < size.height; i++ )
|
|
{
|
|
float* mx = (float*)(mapx->data.ptr + mapx->step*i);
|
|
float* my = (float*)(mapy->data.ptr + mapy->step*i);
|
|
|
|
for( j = 0; j < size.width; j++ )
|
|
{
|
|
double w = 1./(c[2][0]*j + c[2][1]*i + 1.);
|
|
double x = (c[0][0]*j + c[0][1]*i + c[0][2])*w;
|
|
double y = (c[1][0]*j + c[1][1]*i + c[1][2])*w;
|
|
|
|
mx[j] = (float)x;
|
|
my[j] = (float)y;
|
|
}
|
|
}
|
|
|
|
__END__;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------------------------*/
|
|
|
|
CV_IMPL void cvInitPerspectiveTransform( CvSize size, const CvPoint2D32f quad[4], double matrix[3][3],
|
|
CvArr* rectMap )
|
|
{
|
|
/* Computes Perspective Transform coeffs and map if need
|
|
for given image size and given result quad */
|
|
CV_FUNCNAME( "cvInitPerspectiveTransform" );
|
|
|
|
__BEGIN__;
|
|
|
|
double A[64];
|
|
double b[8];
|
|
double c[8];
|
|
CvPoint2D32f pt[4];
|
|
CvMat mapstub, *map = (CvMat*)rectMap;
|
|
int i, j;
|
|
|
|
if( map )
|
|
{
|
|
CV_CALL( map = cvGetMat( map, &mapstub ));
|
|
|
|
if( CV_MAT_TYPE( map->type ) != CV_32FC2 )
|
|
CV_ERROR( CV_StsUnsupportedFormat, "" );
|
|
|
|
if( map->width != size.width || map->height != size.height )
|
|
CV_ERROR( CV_StsUnmatchedSizes, "" );
|
|
}
|
|
|
|
pt[0] = cvPoint2D32f( 0, 0 );
|
|
pt[1] = cvPoint2D32f( size.width, 0 );
|
|
pt[2] = cvPoint2D32f( size.width, size.height );
|
|
pt[3] = cvPoint2D32f( 0, size.height );
|
|
|
|
for( i = 0; i < 4; i++ )
|
|
{
|
|
#if 0
|
|
double x = quad[i].x;
|
|
double y = quad[i].y;
|
|
double X = pt[i].x;
|
|
double Y = pt[i].y;
|
|
#else
|
|
double x = pt[i].x;
|
|
double y = pt[i].y;
|
|
double X = quad[i].x;
|
|
double Y = quad[i].y;
|
|
#endif
|
|
double* a = A + i*16;
|
|
|
|
a[0] = x;
|
|
a[1] = y;
|
|
a[2] = 1;
|
|
a[3] = 0;
|
|
a[4] = 0;
|
|
a[5] = 0;
|
|
a[6] = -X*x;
|
|
a[7] = -X*y;
|
|
|
|
a += 8;
|
|
|
|
a[0] = 0;
|
|
a[1] = 0;
|
|
a[2] = 0;
|
|
a[3] = x;
|
|
a[4] = y;
|
|
a[5] = 1;
|
|
a[6] = -Y*x;
|
|
a[7] = -Y*y;
|
|
|
|
b[i*2] = X;
|
|
b[i*2 + 1] = Y;
|
|
}
|
|
|
|
{
|
|
double invA[64];
|
|
CvMat matA = cvMat( 8, 8, CV_64F, A );
|
|
CvMat matInvA = cvMat( 8, 8, CV_64F, invA );
|
|
CvMat matB = cvMat( 8, 1, CV_64F, b );
|
|
CvMat matX = cvMat( 8, 1, CV_64F, c );
|
|
|
|
CV_CALL( cvPseudoInverse( &matA, &matInvA ));
|
|
CV_CALL( cvMatMulAdd( &matInvA, &matB, 0, &matX ));
|
|
}
|
|
|
|
matrix[0][0] = c[0];
|
|
matrix[0][1] = c[1];
|
|
matrix[0][2] = c[2];
|
|
matrix[1][0] = c[3];
|
|
matrix[1][1] = c[4];
|
|
matrix[1][2] = c[5];
|
|
matrix[2][0] = c[6];
|
|
matrix[2][1] = c[7];
|
|
matrix[2][2] = 1.0;
|
|
|
|
if( map )
|
|
{
|
|
for( i = 0; i < size.height; i++ )
|
|
{
|
|
CvPoint2D32f* maprow = (CvPoint2D32f*)(map->data.ptr + map->step*i);
|
|
for( j = 0; j < size.width; j++ )
|
|
{
|
|
double w = 1./(c[6]*j + c[7]*i + 1.);
|
|
double x = (c[0]*j + c[1]*i + c[2])*w;
|
|
double y = (c[3]*j + c[4]*i + c[5])*w;
|
|
|
|
maprow[j].x = (float)x;
|
|
maprow[j].y = (float)y;
|
|
}
|
|
}
|
|
}
|
|
|
|
__END__;
|
|
|
|
return;
|
|
}
|
|
|
|
|
|
/*-----------------------------------------------------------------------*/
|
|
/* Compute projected infinite point for second image if first image point is known */
|
|
void icvComputeeInfiniteProject1( CvMatr64d rotMatr,
|
|
CvMatr64d camMatr1,
|
|
CvMatr64d camMatr2,
|
|
CvPoint2D32f point1,
|
|
CvPoint2D32f* point2)
|
|
{
|
|
double invMatr1[9];
|
|
icvInvertMatrix_64d(camMatr1,3,invMatr1);
|
|
double P1[3];
|
|
double p1[3];
|
|
p1[0] = (double)(point1.x);
|
|
p1[1] = (double)(point1.y);
|
|
p1[2] = 1;
|
|
|
|
icvMulMatrix_64d( invMatr1,
|
|
3,3,
|
|
p1,
|
|
1,3,
|
|
P1);
|
|
|
|
double invR[9];
|
|
icvTransposeMatrix_64d( rotMatr, 3, 3, invR );
|
|
|
|
/* Change system 1 to system 2 */
|
|
double P2[3];
|
|
icvMulMatrix_64d( invR,
|
|
3,3,
|
|
P1,
|
|
1,3,
|
|
P2);
|
|
|
|
/* Now we can project this point to image 2 */
|
|
double projP[3];
|
|
|
|
icvMulMatrix_64d( camMatr2,
|
|
3,3,
|
|
P2,
|
|
1,3,
|
|
projP);
|
|
|
|
point2->x = (float)(projP[0] / projP[2]);
|
|
point2->y = (float)(projP[1] / projP[2]);
|
|
|
|
return;
|
|
}
|
|
|
|
/*-----------------------------------------------------------------------*/
|
|
/* Compute projected infinite point for second image if first image point is known */
|
|
void icvComputeeInfiniteProject2( CvMatr64d rotMatr,
|
|
CvMatr64d camMatr1,
|
|
CvMatr64d camMatr2,
|
|
CvPoint2D32f* point1,
|
|
CvPoint2D32f point2)
|
|
{
|
|
double invMatr2[9];
|
|
icvInvertMatrix_64d(camMatr2,3,invMatr2);
|
|
double P2[3];
|
|
double p2[3];
|
|
p2[0] = (double)(point2.x);
|
|
p2[1] = (double)(point2.y);
|
|
p2[2] = 1;
|
|
|
|
icvMulMatrix_64d( invMatr2,
|
|
3,3,
|
|
p2,
|
|
1,3,
|
|
P2);
|
|
|
|
/* Change system 1 to system 2 */
|
|
|
|
double P1[3];
|
|
icvMulMatrix_64d( rotMatr,
|
|
3,3,
|
|
P2,
|
|
1,3,
|
|
P1);
|
|
|
|
/* Now we can project this point to image 2 */
|
|
double projP[3];
|
|
|
|
icvMulMatrix_64d( camMatr1,
|
|
3,3,
|
|
P1,
|
|
1,3,
|
|
projP);
|
|
|
|
point1->x = (float)(projP[0] / projP[2]);
|
|
point1->y = (float)(projP[1] / projP[2]);
|
|
|
|
return;
|
|
}
|
|
|
|
/* Select best R and t for given cameras, points, ... */
|
|
/* For both cameras */
|
|
static int icvSelectBestRt( int numImages,
|
|
int* numPoints,
|
|
CvPoint2D32f* imagePoints1,
|
|
CvPoint2D32f* imagePoints2,
|
|
CvPoint3D32f* objectPoints,
|
|
|
|
CvMatr32f cameraMatrix1,
|
|
CvVect32f distortion1,
|
|
CvMatr32f rotMatrs1,
|
|
CvVect32f transVects1,
|
|
|
|
CvMatr32f cameraMatrix2,
|
|
CvVect32f distortion2,
|
|
CvMatr32f rotMatrs2,
|
|
CvVect32f transVects2,
|
|
|
|
CvMatr32f bestRotMatr,
|
|
CvVect32f bestTransVect
|
|
)
|
|
{
|
|
|
|
/* Need to convert input data 32 -> 64 */
|
|
CvPoint3D64d* objectPoints_64d;
|
|
|
|
double* rotMatrs1_64d;
|
|
double* rotMatrs2_64d;
|
|
|
|
double* transVects1_64d;
|
|
double* transVects2_64d;
|
|
|
|
double cameraMatrix1_64d[9];
|
|
double cameraMatrix2_64d[9];
|
|
|
|
double distortion1_64d[4];
|
|
double distortion2_64d[4];
|
|
|
|
/* allocate memory for 64d data */
|
|
int totalNum = 0;
|
|
|
|
for(int i = 0; i < numImages; i++ )
|
|
{
|
|
totalNum += numPoints[i];
|
|
}
|
|
|
|
objectPoints_64d = (CvPoint3D64d*)calloc(totalNum,sizeof(CvPoint3D64d));
|
|
|
|
rotMatrs1_64d = (double*)calloc(numImages,sizeof(double)*9);
|
|
rotMatrs2_64d = (double*)calloc(numImages,sizeof(double)*9);
|
|
|
|
transVects1_64d = (double*)calloc(numImages,sizeof(double)*3);
|
|
transVects2_64d = (double*)calloc(numImages,sizeof(double)*3);
|
|
|
|
/* Convert input data to 64d */
|
|
|
|
icvCvt_32f_64d((float*)objectPoints, (double*)objectPoints_64d, totalNum*3);
|
|
|
|
icvCvt_32f_64d(rotMatrs1, rotMatrs1_64d, numImages*9);
|
|
icvCvt_32f_64d(rotMatrs2, rotMatrs2_64d, numImages*9);
|
|
|
|
icvCvt_32f_64d(transVects1, transVects1_64d, numImages*3);
|
|
icvCvt_32f_64d(transVects2, transVects2_64d, numImages*3);
|
|
|
|
/* Convert to arrays */
|
|
icvCvt_32f_64d(cameraMatrix1, cameraMatrix1_64d, 9);
|
|
icvCvt_32f_64d(cameraMatrix2, cameraMatrix2_64d, 9);
|
|
|
|
icvCvt_32f_64d(distortion1, distortion1_64d, 4);
|
|
icvCvt_32f_64d(distortion2, distortion2_64d, 4);
|
|
|
|
|
|
/* for each R and t compute error for image pair */
|
|
float* errors;
|
|
|
|
errors = (float*)calloc(numImages*numImages,sizeof(float));
|
|
if( errors == 0 )
|
|
{
|
|
return CV_OUTOFMEM_ERR;
|
|
}
|
|
|
|
int currImagePair;
|
|
int currRt;
|
|
for( currRt = 0; currRt < numImages; currRt++ )
|
|
{
|
|
int begPoint = 0;
|
|
for(currImagePair = 0; currImagePair < numImages; currImagePair++ )
|
|
{
|
|
/* For current R,t R,t compute relative position of cameras */
|
|
|
|
double convRotMatr[9];
|
|
double convTransVect[3];
|
|
|
|
icvCreateConvertMatrVect( rotMatrs1_64d + currRt*9,
|
|
transVects1_64d + currRt*3,
|
|
rotMatrs2_64d + currRt*9,
|
|
transVects2_64d + currRt*3,
|
|
convRotMatr,
|
|
convTransVect);
|
|
|
|
/* Project points using relative position of cameras */
|
|
|
|
double convRotMatr2[9];
|
|
double convTransVect2[3];
|
|
|
|
convRotMatr2[0] = 1;
|
|
convRotMatr2[1] = 0;
|
|
convRotMatr2[2] = 0;
|
|
|
|
convRotMatr2[3] = 0;
|
|
convRotMatr2[4] = 1;
|
|
convRotMatr2[5] = 0;
|
|
|
|
convRotMatr2[6] = 0;
|
|
convRotMatr2[7] = 0;
|
|
convRotMatr2[8] = 1;
|
|
|
|
convTransVect2[0] = 0;
|
|
convTransVect2[1] = 0;
|
|
convTransVect2[2] = 0;
|
|
|
|
/* Compute error for given pair and Rt */
|
|
/* We must project points to image and compute error */
|
|
|
|
CvPoint2D64d* projImagePoints1;
|
|
CvPoint2D64d* projImagePoints2;
|
|
|
|
CvPoint3D64d* points1;
|
|
CvPoint3D64d* points2;
|
|
|
|
int numberPnt;
|
|
numberPnt = numPoints[currImagePair];
|
|
projImagePoints1 = (CvPoint2D64d*)calloc(numberPnt,sizeof(CvPoint2D64d));
|
|
projImagePoints2 = (CvPoint2D64d*)calloc(numberPnt,sizeof(CvPoint2D64d));
|
|
|
|
points1 = (CvPoint3D64d*)calloc(numberPnt,sizeof(CvPoint3D64d));
|
|
points2 = (CvPoint3D64d*)calloc(numberPnt,sizeof(CvPoint3D64d));
|
|
|
|
/* Transform object points to first camera position */
|
|
for(int i = 0; i < numberPnt; i++ )
|
|
{
|
|
/* Create second camera point */
|
|
CvPoint3D64d tmpPoint;
|
|
tmpPoint.x = (double)(objectPoints[i].x);
|
|
tmpPoint.y = (double)(objectPoints[i].y);
|
|
tmpPoint.z = (double)(objectPoints[i].z);
|
|
|
|
icvConvertPointSystem( tmpPoint,
|
|
points2+i,
|
|
rotMatrs2_64d + currImagePair*9,
|
|
transVects2_64d + currImagePair*3);
|
|
|
|
/* Create first camera point using R, t */
|
|
icvConvertPointSystem( points2[i],
|
|
points1+i,
|
|
convRotMatr,
|
|
convTransVect);
|
|
|
|
CvPoint3D64d tmpPoint2 = { 0, 0, 0 };
|
|
icvConvertPointSystem( tmpPoint,
|
|
&tmpPoint2,
|
|
rotMatrs1_64d + currImagePair*9,
|
|
transVects1_64d + currImagePair*3);
|
|
/*double err;
|
|
double dx,dy,dz;
|
|
dx = tmpPoint2.x - points1[i].x;
|
|
dy = tmpPoint2.y - points1[i].y;
|
|
dz = tmpPoint2.z - points1[i].z;
|
|
err = sqrt(dx*dx + dy*dy + dz*dz);*/
|
|
}
|
|
|
|
#if 0
|
|
cvProjectPointsSimple( numPoints[currImagePair],
|
|
objectPoints_64d + begPoint,
|
|
rotMatrs1_64d + currRt*9,
|
|
transVects1_64d + currRt*3,
|
|
cameraMatrix1_64d,
|
|
distortion1_64d,
|
|
projImagePoints1);
|
|
|
|
cvProjectPointsSimple( numPoints[currImagePair],
|
|
objectPoints_64d + begPoint,
|
|
rotMatrs2_64d + currRt*9,
|
|
transVects2_64d + currRt*3,
|
|
cameraMatrix2_64d,
|
|
distortion2_64d,
|
|
projImagePoints2);
|
|
#endif
|
|
|
|
/* Project with no translate and no rotation */
|
|
|
|
#if 0
|
|
{
|
|
double nodist[4] = {0,0,0,0};
|
|
cvProjectPointsSimple( numPoints[currImagePair],
|
|
points1,
|
|
convRotMatr2,
|
|
convTransVect2,
|
|
cameraMatrix1_64d,
|
|
nodist,
|
|
projImagePoints1);
|
|
|
|
cvProjectPointsSimple( numPoints[currImagePair],
|
|
points2,
|
|
convRotMatr2,
|
|
convTransVect2,
|
|
cameraMatrix2_64d,
|
|
nodist,
|
|
projImagePoints2);
|
|
|
|
}
|
|
#endif
|
|
|
|
cvProjectPointsSimple( numPoints[currImagePair],
|
|
points1,
|
|
convRotMatr2,
|
|
convTransVect2,
|
|
cameraMatrix1_64d,
|
|
distortion1_64d,
|
|
projImagePoints1);
|
|
|
|
cvProjectPointsSimple( numPoints[currImagePair],
|
|
points2,
|
|
convRotMatr2,
|
|
convTransVect2,
|
|
cameraMatrix2_64d,
|
|
distortion2_64d,
|
|
projImagePoints2);
|
|
|
|
/* points are projected. Compute error */
|
|
|
|
int currPoint;
|
|
double err1 = 0;
|
|
double err2 = 0;
|
|
double err;
|
|
for( currPoint = 0; currPoint < numberPnt; currPoint++ )
|
|
{
|
|
double len1,len2;
|
|
double dx1,dy1;
|
|
dx1 = imagePoints1[begPoint+currPoint].x - projImagePoints1[currPoint].x;
|
|
dy1 = imagePoints1[begPoint+currPoint].y - projImagePoints1[currPoint].y;
|
|
len1 = sqrt(dx1*dx1 + dy1*dy1);
|
|
err1 += len1;
|
|
|
|
double dx2,dy2;
|
|
dx2 = imagePoints2[begPoint+currPoint].x - projImagePoints2[currPoint].x;
|
|
dy2 = imagePoints2[begPoint+currPoint].y - projImagePoints2[currPoint].y;
|
|
len2 = sqrt(dx2*dx2 + dy2*dy2);
|
|
err2 += len2;
|
|
}
|
|
|
|
err1 /= (float)(numberPnt);
|
|
err2 /= (float)(numberPnt);
|
|
|
|
err = (err1+err2) * 0.5;
|
|
begPoint += numberPnt;
|
|
|
|
/* Set this error to */
|
|
errors[numImages*currImagePair+currRt] = (float)err;
|
|
|
|
free(points1);
|
|
free(points2);
|
|
free(projImagePoints1);
|
|
free(projImagePoints2);
|
|
}
|
|
}
|
|
|
|
/* Just select R and t with minimal average error */
|
|
|
|
int bestnumRt = 0;
|
|
float minError = 0;/* Just for no warnings. Uses 'first' flag. */
|
|
int first = 1;
|
|
for( currRt = 0; currRt < numImages; currRt++ )
|
|
{
|
|
float avErr = 0;
|
|
for(currImagePair = 0; currImagePair < numImages; currImagePair++ )
|
|
{
|
|
avErr += errors[numImages*currImagePair+currRt];
|
|
}
|
|
avErr /= (float)(numImages);
|
|
|
|
if( first )
|
|
{
|
|
bestnumRt = 0;
|
|
minError = avErr;
|
|
first = 0;
|
|
}
|
|
else
|
|
{
|
|
if( avErr < minError )
|
|
{
|
|
bestnumRt = currRt;
|
|
minError = avErr;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
double bestRotMatr_64d[9];
|
|
double bestTransVect_64d[3];
|
|
|
|
icvCreateConvertMatrVect( rotMatrs1_64d + bestnumRt * 9,
|
|
transVects1_64d + bestnumRt * 3,
|
|
rotMatrs2_64d + bestnumRt * 9,
|
|
transVects2_64d + bestnumRt * 3,
|
|
bestRotMatr_64d,
|
|
bestTransVect_64d);
|
|
|
|
icvCvt_64d_32f(bestRotMatr_64d,bestRotMatr,9);
|
|
icvCvt_64d_32f(bestTransVect_64d,bestTransVect,3);
|
|
|
|
|
|
free(errors);
|
|
|
|
return CV_OK;
|
|
}
|
|
|
|
|
|
/* ----------------- Stereo calibration functions --------------------- */
|
|
|
|
float icvDefinePointPosition(CvPoint2D32f point1,CvPoint2D32f point2,CvPoint2D32f point)
|
|
{
|
|
float ax = point2.x - point1.x;
|
|
float ay = point2.y - point1.y;
|
|
|
|
float bx = point.x - point1.x;
|
|
float by = point.y - point1.y;
|
|
|
|
return (ax*by - ay*bx);
|
|
}
|
|
|
|
/* Convert function for stereo warping */
|
|
int icvConvertWarpCoordinates(double coeffs[3][3],
|
|
CvPoint2D32f* cameraPoint,
|
|
CvPoint2D32f* warpPoint,
|
|
int direction)
|
|
{
|
|
double x,y;
|
|
double det;
|
|
if( direction == CV_WARP_TO_CAMERA )
|
|
{/* convert from camera image to warped image coordinates */
|
|
x = warpPoint->x;
|
|
y = warpPoint->y;
|
|
|
|
det = (coeffs[2][0] * x + coeffs[2][1] * y + coeffs[2][2]);
|
|
if( fabs(det) > 1e-8 )
|
|
{
|
|
cameraPoint->x = (float)((coeffs[0][0] * x + coeffs[0][1] * y + coeffs[0][2]) / det);
|
|
cameraPoint->y = (float)((coeffs[1][0] * x + coeffs[1][1] * y + coeffs[1][2]) / det);
|
|
return CV_OK;
|
|
}
|
|
}
|
|
else if( direction == CV_CAMERA_TO_WARP )
|
|
{/* convert from warped image to camera image coordinates */
|
|
x = cameraPoint->x;
|
|
y = cameraPoint->y;
|
|
|
|
det = (coeffs[2][0]*x-coeffs[0][0])*(coeffs[2][1]*y-coeffs[1][1])-(coeffs[2][1]*x-coeffs[0][1])*(coeffs[2][0]*y-coeffs[1][0]);
|
|
|
|
if( fabs(det) > 1e-8 )
|
|
{
|
|
warpPoint->x = (float)(((coeffs[0][2]-coeffs[2][2]*x)*(coeffs[2][1]*y-coeffs[1][1])-(coeffs[2][1]*x-coeffs[0][1])*(coeffs[1][2]-coeffs[2][2]*y))/det);
|
|
warpPoint->y = (float)(((coeffs[2][0]*x-coeffs[0][0])*(coeffs[1][2]-coeffs[2][2]*y)-(coeffs[0][2]-coeffs[2][2]*x)*(coeffs[2][0]*y-coeffs[1][0]))/det);
|
|
return CV_OK;
|
|
}
|
|
}
|
|
|
|
return CV_BADFACTOR_ERR;
|
|
}
|
|
|
|
/* Compute stereo params using some camera params */
|
|
/* by Valery Mosyagin. int ComputeRestStereoParams(StereoParams *stereoparams) */
|
|
int icvComputeRestStereoParams(CvStereoCamera *stereoparams)
|
|
{
|
|
|
|
|
|
icvGetQuadsTransformStruct(stereoparams);
|
|
|
|
cvInitPerspectiveTransform( stereoparams->warpSize,
|
|
stereoparams->quad[0],
|
|
stereoparams->coeffs[0],
|
|
0);
|
|
|
|
cvInitPerspectiveTransform( stereoparams->warpSize,
|
|
stereoparams->quad[1],
|
|
stereoparams->coeffs[1],
|
|
0);
|
|
|
|
/* Create border for warped images */
|
|
CvPoint2D32f corns[4];
|
|
corns[0].x = 0;
|
|
corns[0].y = 0;
|
|
|
|
corns[1].x = (float)(stereoparams->camera[0]->imgSize[0]-1);
|
|
corns[1].y = 0;
|
|
|
|
corns[2].x = (float)(stereoparams->camera[0]->imgSize[0]-1);
|
|
corns[2].y = (float)(stereoparams->camera[0]->imgSize[1]-1);
|
|
|
|
corns[3].x = 0;
|
|
corns[3].y = (float)(stereoparams->camera[0]->imgSize[1]-1);
|
|
|
|
for(int i = 0; i < 4; i++ )
|
|
{
|
|
/* For first camera */
|
|
icvConvertWarpCoordinates( stereoparams->coeffs[0],
|
|
corns+i,
|
|
stereoparams->border[0]+i,
|
|
CV_CAMERA_TO_WARP);
|
|
|
|
/* For second camera */
|
|
icvConvertWarpCoordinates( stereoparams->coeffs[1],
|
|
corns+i,
|
|
stereoparams->border[1]+i,
|
|
CV_CAMERA_TO_WARP);
|
|
}
|
|
|
|
/* Test compute */
|
|
{
|
|
CvPoint2D32f warpPoints[4];
|
|
warpPoints[0] = cvPoint2D32f(0,0);
|
|
warpPoints[1] = cvPoint2D32f(stereoparams->warpSize.width-1,0);
|
|
warpPoints[2] = cvPoint2D32f(stereoparams->warpSize.width-1,stereoparams->warpSize.height-1);
|
|
warpPoints[3] = cvPoint2D32f(0,stereoparams->warpSize.height-1);
|
|
|
|
CvPoint2D32f camPoints1[4];
|
|
CvPoint2D32f camPoints2[4];
|
|
|
|
for( int i = 0; i < 4; i++ )
|
|
{
|
|
icvConvertWarpCoordinates(stereoparams->coeffs[0],
|
|
camPoints1+i,
|
|
warpPoints+i,
|
|
CV_WARP_TO_CAMERA);
|
|
|
|
icvConvertWarpCoordinates(stereoparams->coeffs[1],
|
|
camPoints2+i,
|
|
warpPoints+i,
|
|
CV_WARP_TO_CAMERA);
|
|
}
|
|
}
|
|
|
|
|
|
/* Allocate memory for scanlines coeffs */
|
|
|
|
stereoparams->lineCoeffs = (CvStereoLineCoeff*)calloc(stereoparams->warpSize.height,sizeof(CvStereoLineCoeff));
|
|
|
|
/* Compute coeffs for epilines */
|
|
|
|
icvComputeCoeffForStereo( stereoparams);
|
|
|
|
/* all coeffs are known */
|
|
return CV_OK;
|
|
}
|
|
|
|
/*-------------------------------------------------------------------------------------------*/
|
|
|
|
int icvStereoCalibration( int numImages,
|
|
int* nums,
|
|
CvSize imageSize,
|
|
CvPoint2D32f* imagePoints1,
|
|
CvPoint2D32f* imagePoints2,
|
|
CvPoint3D32f* objectPoints,
|
|
CvStereoCamera* stereoparams
|
|
)
|
|
{
|
|
/* Firstly we must calibrate both cameras */
|
|
/* Alocate memory for data */
|
|
/* Allocate for translate vectors */
|
|
float* transVects1;
|
|
float* transVects2;
|
|
float* rotMatrs1;
|
|
float* rotMatrs2;
|
|
|
|
transVects1 = (float*)calloc(numImages,sizeof(float)*3);
|
|
transVects2 = (float*)calloc(numImages,sizeof(float)*3);
|
|
|
|
rotMatrs1 = (float*)calloc(numImages,sizeof(float)*9);
|
|
rotMatrs2 = (float*)calloc(numImages,sizeof(float)*9);
|
|
|
|
/* Calibrate first camera */
|
|
cvCalibrateCamera( numImages,
|
|
nums,
|
|
imageSize,
|
|
imagePoints1,
|
|
objectPoints,
|
|
stereoparams->camera[0]->distortion,
|
|
stereoparams->camera[0]->matrix,
|
|
transVects1,
|
|
rotMatrs1,
|
|
1);
|
|
|
|
/* Calibrate second camera */
|
|
cvCalibrateCamera( numImages,
|
|
nums,
|
|
imageSize,
|
|
imagePoints2,
|
|
objectPoints,
|
|
stereoparams->camera[1]->distortion,
|
|
stereoparams->camera[1]->matrix,
|
|
transVects2,
|
|
rotMatrs2,
|
|
1);
|
|
|
|
/* Cameras are calibrated */
|
|
|
|
stereoparams->camera[0]->imgSize[0] = (float)imageSize.width;
|
|
stereoparams->camera[0]->imgSize[1] = (float)imageSize.height;
|
|
|
|
stereoparams->camera[1]->imgSize[0] = (float)imageSize.width;
|
|
stereoparams->camera[1]->imgSize[1] = (float)imageSize.height;
|
|
|
|
icvSelectBestRt( numImages,
|
|
nums,
|
|
imagePoints1,
|
|
imagePoints2,
|
|
objectPoints,
|
|
stereoparams->camera[0]->matrix,
|
|
stereoparams->camera[0]->distortion,
|
|
rotMatrs1,
|
|
transVects1,
|
|
stereoparams->camera[1]->matrix,
|
|
stereoparams->camera[1]->distortion,
|
|
rotMatrs2,
|
|
transVects2,
|
|
stereoparams->rotMatrix,
|
|
stereoparams->transVector
|
|
);
|
|
|
|
/* Free memory */
|
|
free(transVects1);
|
|
free(transVects2);
|
|
free(rotMatrs1);
|
|
free(rotMatrs2);
|
|
|
|
icvComputeRestStereoParams(stereoparams);
|
|
|
|
return CV_NO_ERR;
|
|
}
|
|
|
|
#if 0
|
|
/* Find line from epipole */
|
|
static void FindLine(CvPoint2D32f epipole,CvSize imageSize,CvPoint2D32f point,CvPoint2D32f *start,CvPoint2D32f *end)
|
|
{
|
|
CvPoint2D32f frameBeg;
|
|
CvPoint2D32f frameEnd;
|
|
CvPoint2D32f cross[4];
|
|
int haveCross[4];
|
|
float dist;
|
|
|
|
haveCross[0] = 0;
|
|
haveCross[1] = 0;
|
|
haveCross[2] = 0;
|
|
haveCross[3] = 0;
|
|
|
|
frameBeg.x = 0;
|
|
frameBeg.y = 0;
|
|
frameEnd.x = (float)(imageSize.width);
|
|
frameEnd.y = 0;
|
|
haveCross[0] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[0]);
|
|
|
|
frameBeg.x = (float)(imageSize.width);
|
|
frameBeg.y = 0;
|
|
frameEnd.x = (float)(imageSize.width);
|
|
frameEnd.y = (float)(imageSize.height);
|
|
haveCross[1] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[1]);
|
|
|
|
frameBeg.x = (float)(imageSize.width);
|
|
frameBeg.y = (float)(imageSize.height);
|
|
frameEnd.x = 0;
|
|
frameEnd.y = (float)(imageSize.height);
|
|
haveCross[2] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[2]);
|
|
|
|
frameBeg.x = 0;
|
|
frameBeg.y = (float)(imageSize.height);
|
|
frameEnd.x = 0;
|
|
frameEnd.y = 0;
|
|
haveCross[3] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[3]);
|
|
|
|
int n;
|
|
float minDist = (float)(INT_MAX);
|
|
float maxDist = (float)(INT_MIN);
|
|
|
|
int maxN = -1;
|
|
int minN = -1;
|
|
|
|
for( n = 0; n < 4; n++ )
|
|
{
|
|
if( haveCross[n] > 0 )
|
|
{
|
|
dist = (epipole.x - cross[n].x)*(epipole.x - cross[n].x) +
|
|
(epipole.y - cross[n].y)*(epipole.y - cross[n].y);
|
|
|
|
if( dist < minDist )
|
|
{
|
|
minDist = dist;
|
|
minN = n;
|
|
}
|
|
|
|
if( dist > maxDist )
|
|
{
|
|
maxDist = dist;
|
|
maxN = n;
|
|
}
|
|
}
|
|
}
|
|
|
|
if( minN >= 0 && maxN >= 0 && (minN != maxN) )
|
|
{
|
|
*start = cross[minN];
|
|
*end = cross[maxN];
|
|
}
|
|
else
|
|
{
|
|
start->x = 0;
|
|
start->y = 0;
|
|
end->x = 0;
|
|
end->y = 0;
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
/* Find line which cross frame by line(a,b,c) */
|
|
static void FindLineForEpiline(CvSize imageSize,float a,float b,float c,CvPoint2D32f *start,CvPoint2D32f *end)
|
|
{
|
|
CvPoint2D32f frameBeg;
|
|
CvPoint2D32f frameEnd;
|
|
CvPoint2D32f cross[4];
|
|
int haveCross[4];
|
|
float dist;
|
|
|
|
haveCross[0] = 0;
|
|
haveCross[1] = 0;
|
|
haveCross[2] = 0;
|
|
haveCross[3] = 0;
|
|
|
|
frameBeg.x = 0;
|
|
frameBeg.y = 0;
|
|
frameEnd.x = (float)(imageSize.width);
|
|
frameEnd.y = 0;
|
|
haveCross[0] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[0]);
|
|
|
|
frameBeg.x = (float)(imageSize.width);
|
|
frameBeg.y = 0;
|
|
frameEnd.x = (float)(imageSize.width);
|
|
frameEnd.y = (float)(imageSize.height);
|
|
haveCross[1] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[1]);
|
|
|
|
frameBeg.x = (float)(imageSize.width);
|
|
frameBeg.y = (float)(imageSize.height);
|
|
frameEnd.x = 0;
|
|
frameEnd.y = (float)(imageSize.height);
|
|
haveCross[2] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[2]);
|
|
|
|
frameBeg.x = 0;
|
|
frameBeg.y = (float)(imageSize.height);
|
|
frameEnd.x = 0;
|
|
frameEnd.y = 0;
|
|
haveCross[3] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[3]);
|
|
|
|
int n;
|
|
float minDist = (float)(INT_MAX);
|
|
float maxDist = (float)(INT_MIN);
|
|
|
|
int maxN = -1;
|
|
int minN = -1;
|
|
|
|
double midPointX = imageSize.width / 2.0;
|
|
double midPointY = imageSize.height / 2.0;
|
|
|
|
for( n = 0; n < 4; n++ )
|
|
{
|
|
if( haveCross[n] > 0 )
|
|
{
|
|
dist = (float)((midPointX - cross[n].x)*(midPointX - cross[n].x) +
|
|
(midPointY - cross[n].y)*(midPointY - cross[n].y));
|
|
|
|
if( dist < minDist )
|
|
{
|
|
minDist = dist;
|
|
minN = n;
|
|
}
|
|
|
|
if( dist > maxDist )
|
|
{
|
|
maxDist = dist;
|
|
maxN = n;
|
|
}
|
|
}
|
|
}
|
|
|
|
if( minN >= 0 && maxN >= 0 && (minN != maxN) )
|
|
{
|
|
*start = cross[minN];
|
|
*end = cross[maxN];
|
|
}
|
|
else
|
|
{
|
|
start->x = 0;
|
|
start->y = 0;
|
|
end->x = 0;
|
|
end->y = 0;
|
|
}
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
/* Cross lines */
|
|
static int GetCrossLines(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f p2_start,CvPoint2D32f p2_end,CvPoint2D32f *cross)
|
|
{
|
|
double ex1,ey1,ex2,ey2;
|
|
double px1,py1,px2,py2;
|
|
double del;
|
|
double delA,delB,delX,delY;
|
|
double alpha,betta;
|
|
|
|
ex1 = p1_start.x;
|
|
ey1 = p1_start.y;
|
|
ex2 = p1_end.x;
|
|
ey2 = p1_end.y;
|
|
|
|
px1 = p2_start.x;
|
|
py1 = p2_start.y;
|
|
px2 = p2_end.x;
|
|
py2 = p2_end.y;
|
|
|
|
del = (ex1-ex2)*(py2-py1)+(ey2-ey1)*(px2-px1);
|
|
if( del == 0)
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
delA = (px1-ex1)*(py1-py2) + (ey1-py1)*(px1-px2);
|
|
delB = (ex1-px1)*(ey1-ey2) + (py1-ey1)*(ex1-ex2);
|
|
|
|
alpha = delA / del;
|
|
betta = -delB / del;
|
|
|
|
if( alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0)
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
delX = (ex1-ex2)*(py1*(px1-px2)-px1*(py1-py2))+
|
|
(px1-px2)*(ex1*(ey1-ey2)-ey1*(ex1-ex2));
|
|
|
|
delY = (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2))+
|
|
(py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2));
|
|
|
|
cross->x = (float)( delX / del);
|
|
cross->y = (float)(-delY / del);
|
|
return 1;
|
|
}
|
|
#endif
|
|
|
|
int icvGetCrossPieceVector(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f v2_start,CvPoint2D32f v2_end,CvPoint2D32f *cross)
|
|
{
|
|
double ex1 = p1_start.x;
|
|
double ey1 = p1_start.y;
|
|
double ex2 = p1_end.x;
|
|
double ey2 = p1_end.y;
|
|
|
|
double px1 = v2_start.x;
|
|
double py1 = v2_start.y;
|
|
double px2 = v2_end.x;
|
|
double py2 = v2_end.y;
|
|
|
|
double del = (ex1-ex2)*(py2-py1)+(ey2-ey1)*(px2-px1);
|
|
if( del == 0)
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
double delA = (px1-ex1)*(py1-py2) + (ey1-py1)*(px1-px2);
|
|
//double delB = (ex1-px1)*(ey1-ey2) + (py1-ey1)*(ex1-ex2);
|
|
|
|
double alpha = delA / del;
|
|
//double betta = -delB / del;
|
|
|
|
if( alpha < 0 || alpha > 1.0 )
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
double delX = (ex1-ex2)*(py1*(px1-px2)-px1*(py1-py2))+
|
|
(px1-px2)*(ex1*(ey1-ey2)-ey1*(ex1-ex2));
|
|
|
|
double delY = (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2))+
|
|
(py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2));
|
|
|
|
cross->x = (float)( delX / del);
|
|
cross->y = (float)(-delY / del);
|
|
return 1;
|
|
}
|
|
|
|
|
|
int icvGetCrossLineDirect(CvPoint2D32f p1,CvPoint2D32f p2,float a,float b,float c,CvPoint2D32f* cross)
|
|
{
|
|
double del;
|
|
double delX,delY,delA;
|
|
|
|
double px1,px2,py1,py2;
|
|
double X,Y,alpha;
|
|
|
|
px1 = p1.x;
|
|
py1 = p1.y;
|
|
|
|
px2 = p2.x;
|
|
py2 = p2.y;
|
|
|
|
del = a * (px2 - px1) + b * (py2-py1);
|
|
if( del == 0 )
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
delA = - c - a*px1 - b*py1;
|
|
alpha = delA / del;
|
|
|
|
if( alpha < 0 || alpha > 1.0 )
|
|
{
|
|
return -1;/* no cross */
|
|
}
|
|
|
|
delX = b * (py1*(px1-px2) - px1*(py1-py2)) + c * (px1-px2);
|
|
delY = a * (px1*(py1-py2) - py1*(px1-px2)) + c * (py1-py2);
|
|
|
|
X = delX / del;
|
|
Y = delY / del;
|
|
|
|
cross->x = (float)X;
|
|
cross->y = (float)Y;
|
|
|
|
return 1;
|
|
}
|
|
|
|
#if 0
|
|
static int cvComputeEpipoles( CvMatr32f camMatr1, CvMatr32f camMatr2,
|
|
CvMatr32f rotMatr1, CvMatr32f rotMatr2,
|
|
CvVect32f transVect1,CvVect32f transVect2,
|
|
CvVect32f epipole1,
|
|
CvVect32f epipole2)
|
|
{
|
|
|
|
/* Copy matrix */
|
|
|
|
CvMat ccamMatr1 = cvMat(3,3,CV_MAT32F,camMatr1);
|
|
CvMat ccamMatr2 = cvMat(3,3,CV_MAT32F,camMatr2);
|
|
CvMat crotMatr1 = cvMat(3,3,CV_MAT32F,rotMatr1);
|
|
CvMat crotMatr2 = cvMat(3,3,CV_MAT32F,rotMatr2);
|
|
CvMat ctransVect1 = cvMat(3,1,CV_MAT32F,transVect1);
|
|
CvMat ctransVect2 = cvMat(3,1,CV_MAT32F,transVect2);
|
|
CvMat cepipole1 = cvMat(3,1,CV_MAT32F,epipole1);
|
|
CvMat cepipole2 = cvMat(3,1,CV_MAT32F,epipole2);
|
|
|
|
|
|
CvMat cmatrP1 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrP1);
|
|
CvMat cmatrP2 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrP2);
|
|
CvMat cvectp1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&cvectp1);
|
|
CvMat cvectp2 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&cvectp2);
|
|
CvMat ctmpF1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpF1);
|
|
CvMat ctmpM1 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpM1);
|
|
CvMat ctmpM2 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpM2);
|
|
CvMat cinvP1 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cinvP1);
|
|
CvMat cinvP2 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cinvP2);
|
|
CvMat ctmpMatr = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpMatr);
|
|
CvMat ctmpVect1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpVect1);
|
|
CvMat ctmpVect2 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpVect2);
|
|
CvMat cmatrF1 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrF1);
|
|
CvMat ctmpF = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpF);
|
|
CvMat ctmpE1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpE1);
|
|
CvMat ctmpE2 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpE2);
|
|
|
|
/* Compute first */
|
|
cvmMul( &ccamMatr1, &crotMatr1, &cmatrP1);
|
|
cvmInvert( &cmatrP1,&cinvP1 );
|
|
cvmMul( &ccamMatr1, &ctransVect1, &cvectp1 );
|
|
|
|
/* Compute second */
|
|
cvmMul( &ccamMatr2, &crotMatr2, &cmatrP2 );
|
|
cvmInvert( &cmatrP2,&cinvP2 );
|
|
cvmMul( &ccamMatr2, &ctransVect2, &cvectp2 );
|
|
|
|
cvmMul( &cmatrP1, &cinvP2, &ctmpM1);
|
|
cvmMul( &ctmpM1, &cvectp2, &ctmpVect1);
|
|
cvmSub( &cvectp1,&ctmpVect1,&ctmpE1);
|
|
|
|
cvmMul( &cmatrP2, &cinvP1, &ctmpM2);
|
|
cvmMul( &ctmpM2, &cvectp1, &ctmpVect2);
|
|
cvmSub( &cvectp2, &ctmpVect2, &ctmpE2);
|
|
|
|
|
|
/* Need scale */
|
|
|
|
cvmScale(&ctmpE1,&cepipole1,1.0);
|
|
cvmScale(&ctmpE2,&cepipole2,1.0);
|
|
|
|
cvmFree(&cmatrP1);
|
|
cvmFree(&cmatrP1);
|
|
cvmFree(&cvectp1);
|
|
cvmFree(&cvectp2);
|
|
cvmFree(&ctmpF1);
|
|
cvmFree(&ctmpM1);
|
|
cvmFree(&ctmpM2);
|
|
cvmFree(&cinvP1);
|
|
cvmFree(&cinvP2);
|
|
cvmFree(&ctmpMatr);
|
|
cvmFree(&ctmpVect1);
|
|
cvmFree(&ctmpVect2);
|
|
cvmFree(&cmatrF1);
|
|
cvmFree(&ctmpF);
|
|
cvmFree(&ctmpE1);
|
|
cvmFree(&ctmpE2);
|
|
|
|
return CV_NO_ERR;
|
|
}/* cvComputeEpipoles */
|
|
#endif
|
|
|
|
/* Compute epipoles for fundamental matrix */
|
|
int cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr,
|
|
CvPoint3D32f* epipole1,
|
|
CvPoint3D32f* epipole2)
|
|
{
|
|
/* Decompose fundamental matrix using SVD ( A = U W V') */
|
|
CvMat fundMatrC = cvMat(3,3,CV_MAT32F,fundMatr);
|
|
|
|
CvMat* matrW = cvCreateMat(3,3,CV_MAT32F);
|
|
CvMat* matrU = cvCreateMat(3,3,CV_MAT32F);
|
|
CvMat* matrV = cvCreateMat(3,3,CV_MAT32F);
|
|
|
|
/* From svd we need just last vector of U and V or last row from U' and V' */
|
|
/* We get transposed matrixes U and V */
|
|
cvSVD(&fundMatrC,matrW,matrU,matrV,CV_SVD_V_T|CV_SVD_U_T);
|
|
|
|
/* Get last row from U' and compute epipole1 */
|
|
epipole1->x = matrU->data.fl[6];
|
|
epipole1->y = matrU->data.fl[7];
|
|
epipole1->z = matrU->data.fl[8];
|
|
|
|
/* Get last row from V' and compute epipole2 */
|
|
epipole2->x = matrV->data.fl[6];
|
|
epipole2->y = matrV->data.fl[7];
|
|
epipole2->z = matrV->data.fl[8];
|
|
|
|
cvReleaseMat(&matrW);
|
|
cvReleaseMat(&matrU);
|
|
cvReleaseMat(&matrV);
|
|
return CV_OK;
|
|
}
|
|
|
|
int cvConvertEssential2Fundamental( CvMatr32f essMatr,
|
|
CvMatr32f fundMatr,
|
|
CvMatr32f cameraMatr1,
|
|
CvMatr32f cameraMatr2)
|
|
{/* Fund = inv(CM1') * Ess * inv(CM2) */
|
|
|
|
CvMat essMatrC = cvMat(3,3,CV_MAT32F,essMatr);
|
|
CvMat fundMatrC = cvMat(3,3,CV_MAT32F,fundMatr);
|
|
CvMat cameraMatr1C = cvMat(3,3,CV_MAT32F,cameraMatr1);
|
|
CvMat cameraMatr2C = cvMat(3,3,CV_MAT32F,cameraMatr2);
|
|
|
|
CvMat* invCM2 = cvCreateMat(3,3,CV_MAT32F);
|
|
CvMat* tmpMatr = cvCreateMat(3,3,CV_MAT32F);
|
|
CvMat* invCM1T = cvCreateMat(3,3,CV_MAT32F);
|
|
|
|
cvTranspose(&cameraMatr1C,tmpMatr);
|
|
cvInvert(tmpMatr,invCM1T);
|
|
cvmMul(invCM1T,&essMatrC,tmpMatr);
|
|
cvInvert(&cameraMatr2C,invCM2);
|
|
cvmMul(tmpMatr,invCM2,&fundMatrC);
|
|
|
|
/* Scale fundamental matrix */
|
|
double scale;
|
|
scale = 1.0/fundMatrC.data.fl[8];
|
|
cvConvertScale(&fundMatrC,&fundMatrC,scale);
|
|
|
|
cvReleaseMat(&invCM2);
|
|
cvReleaseMat(&tmpMatr);
|
|
cvReleaseMat(&invCM1T);
|
|
|
|
return CV_OK;
|
|
}
|
|
|
|
/* Compute essential matrix */
|
|
|
|
int cvComputeEssentialMatrix( CvMatr32f rotMatr,
|
|
CvMatr32f transVect,
|
|
CvMatr32f essMatr)
|
|
{
|
|
float transMatr[9];
|
|
|
|
/* Make antisymmetric matrix from transpose vector */
|
|
transMatr[0] = 0;
|
|
transMatr[1] = - transVect[2];
|
|
transMatr[2] = transVect[1];
|
|
|
|
transMatr[3] = transVect[2];
|
|
transMatr[4] = 0;
|
|
transMatr[5] = - transVect[0];
|
|
|
|
transMatr[6] = - transVect[1];
|
|
transMatr[7] = transVect[0];
|
|
transMatr[8] = 0;
|
|
|
|
icvMulMatrix_32f(transMatr,3,3,rotMatr,3,3,essMatr);
|
|
|
|
return CV_OK;
|
|
}
|