16acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*M/////////////////////////////////////////////////////////////////////////////////////// 26acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 36acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 46acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 56acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// By downloading, copying, installing or using the software you agree to this license. 66acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// If you do not agree to this license, do not download, install, 76acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// copy or use the software. 86acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 96acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Intel License Agreement 116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// For Open Source Computer Vision Library 126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Copyright (C) 2000, Intel Corporation, all rights reserved. 146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Third party copyrights are property of their respective owners. 156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Redistribution and use in source and binary forms, with or without modification, 176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// are permitted provided that the following conditions are met: 186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's of source code must retain the above copyright notice, 206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer. 216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's in binary form must reproduce the above copyright notice, 236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer in the documentation 246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and/or other materials provided with the distribution. 256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * The name of Intel Corporation may not be used to endorse or promote products 276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// derived from this software without specific prior written permission. 286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// This software is provided by the copyright holders and contributors "as is" and 306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// any express or implied warranties, including, but not limited to, the implied 316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// warranties of merchantability and fitness for a particular purpose are disclaimed. 326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// In no event shall the Intel Corporation or contributors be liable for any direct, 336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// indirect, incidental, special, exemplary, or consequential damages 346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (including, but not limited to, procurement of substitute goods or services; 356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// loss of use, data, or profits; or business interruption) however caused 366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and on any theory of liability, whether in contract, strict liability, 376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// or tort (including negligence or otherwise) arising in any way out of 386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the use of this software, even if advised of the possibility of such damage. 396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*/ 416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cvaux.h" 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "cvtypes.h" 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <float.h> 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <limits.h> 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "cv.h" 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Valery Mosyagin */ 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#undef quad 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define EPS64D 1e-9 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint cvComputeEssentialMatrix( CvMatr32f rotMatr, 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f transVect, 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f essMatr); 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint cvConvertEssential2Fundamental( CvMatr32f essMatr, 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f fundMatr, 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f cameraMatr1, 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f cameraMatr2); 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr, 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f* epipole1, 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f* epipole2); 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvTestPoint( CvPoint2D64d testPoint, 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvVect64d line1,CvVect64d line2, 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d basePoint, 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* result); 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvGetSymPoint3D( CvPoint3D64d pointCorner, 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d point1, 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d point2, 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d *pointSym2) 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double len1,len2; 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double alpha; 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetPieceLength3D(pointCorner,point1,&len1); 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( len1 < EPS64D ) 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_BADARG_ERR; 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetPieceLength3D(pointCorner,point2,&len2); 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn alpha = len2 / len1; 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointSym2->x = pointCorner.x + alpha*(point1.x - pointCorner.x); 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointSym2->y = pointCorner.y + alpha*(point1.y - pointCorner.y); 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointSym2->z = pointCorner.z + alpha*(point1.z - pointCorner.z); 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NO_ERR; 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* author Valery Mosyagin */ 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Compute 3D point for scanline and alpha betta */ 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvCompute3DPoint( double alpha,double betta, 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvStereoLineCoeff* coeffs, 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d* point) 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double partX; 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double partY; 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double partZ; 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double partAll; 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double invPartAll; 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double alphabetta = alpha*betta; 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn partAll = alpha - betta; 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(partAll) > 0.00001 ) /* alpha must be > betta */ 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn partX = coeffs->Xcoef + coeffs->XcoefA *alpha + 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->XcoefB*betta + coeffs->XcoefAB*alphabetta; 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn partY = coeffs->Ycoef + coeffs->YcoefA *alpha + 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->YcoefB*betta + coeffs->YcoefAB*alphabetta; 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn partZ = coeffs->Zcoef + coeffs->ZcoefA *alpha + 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->ZcoefB*betta + coeffs->ZcoefAB*alphabetta; 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn invPartAll = 1.0 / partAll; 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point->x = partX * invPartAll; 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point->y = partY * invPartAll; 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point->z = partZ * invPartAll; 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NO_ERR; 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_BADFACTOR_ERR; 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*--------------------------------------------------------------------------------------*/ 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Compute rotate matrix and trans vector for change system */ 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvCreateConvertMatrVect( CvMatr64d rotMatr1, 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d transVect1, 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d rotMatr2, 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d transVect2, 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d convRotMatr, 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d convTransVect) 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double invRotMatr2[9]; 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tmpVect[3]; 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvInvertMatrix_64d(rotMatr2,3,invRotMatr2); 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for error */ 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvMulMatrix_64d( rotMatr1, 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3,3, 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn invRotMatr2, 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3,3, 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr); 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvMulMatrix_64d( convRotMatr, 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3,3, 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect2, 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1,3, 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmpVect); 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvSubVector_64d(transVect1,tmpVect,convTransVect,3); 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NO_ERR; 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*--------------------------------------------------------------------------------------*/ 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Compute point coordinates in other system */ 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvConvertPointSystem(CvPoint3D64d M2, 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d* M1, 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d rotMatr, 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d transVect 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ) 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tmpVect[3]; 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvMulMatrix_64d( rotMatr, 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3,3, 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (double*)&M2, 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1,3, 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmpVect); 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvAddVector_64d(tmpVect,transVect,(double*)M1,3); 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NO_ERR; 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*--------------------------------------------------------------------------------------*/ 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvComputeCoeffForStereoV3( double quad1[4][2], 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double quad2[4][2], 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numScanlines, 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d camMatr1, 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d rotMatr1, 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d transVect1, 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d camMatr2, 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d rotMatr2, 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d transVect2, 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvStereoLineCoeff* startCoeffs, 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* needSwapCamera) 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* For each pair */ 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* In this function we must define position of cameras */ 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d point1; 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d point2; 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d point3; 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d point4; 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currLine; 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *needSwapCamera = 0; 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currLine = 0; currLine < numScanlines; currLine++ ) 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute points */ 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double alpha = ((double)currLine)/((double)(numScanlines)); /* maybe - 1 */ 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point1.x = (1.0 - alpha) * quad1[0][0] + alpha * quad1[3][0]; 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point1.y = (1.0 - alpha) * quad1[0][1] + alpha * quad1[3][1]; 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point2.x = (1.0 - alpha) * quad1[1][0] + alpha * quad1[2][0]; 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point2.y = (1.0 - alpha) * quad1[1][1] + alpha * quad1[2][1]; 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3.x = (1.0 - alpha) * quad2[0][0] + alpha * quad2[3][0]; 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3.y = (1.0 - alpha) * quad2[0][1] + alpha * quad2[3][1]; 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point4.x = (1.0 - alpha) * quad2[1][0] + alpha * quad2[2][0]; 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point4.y = (1.0 - alpha) * quad2[1][1] + alpha * quad2[2][1]; 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We can compute coeffs for this line */ 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComCoeffForLine( point1, 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point2, 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3, 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point4, 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr1, 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr1, 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect1, 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr2, 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2, 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect2, 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &startCoeffs[currLine], 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn needSwapCamera); 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NO_ERR; 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*--------------------------------------------------------------------------------------*/ 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvComputeCoeffForStereoNew( double quad1[4][2], 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double quad2[4][2], 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numScanlines, 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f camMatr1, 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f rotMatr1, 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f transVect1, 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f camMatr2, 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvStereoLineCoeff* startCoeffs, 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* needSwapCamera) 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Convert data */ 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double camMatr1_64d[9]; 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double camMatr2_64d[9]; 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double rotMatr1_64d[9]; 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double transVect1_64d[3]; 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double rotMatr2_64d[9]; 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double transVect2_64d[3]; 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(camMatr1,camMatr1_64d,9); 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(camMatr2,camMatr2_64d,9); 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(rotMatr1,rotMatr1_64d,9); 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(transVect1,transVect1_64d,3); 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[0] = 1; 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[1] = 0; 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[2] = 0; 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[3] = 0; 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[4] = 1; 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[5] = 0; 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[6] = 0; 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[7] = 0; 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[8] = 1; 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect2_64d[0] = 0; 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect2_64d[1] = 0; 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect2_64d[2] = 0; 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int status = icvComputeCoeffForStereoV3( quad1, 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad2, 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numScanlines, 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr1_64d, 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr1_64d, 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect1_64d, 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr2_64d, 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d, 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect2_64d, 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn startCoeffs, 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn needSwapCamera); 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return status; 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*--------------------------------------------------------------------------------------*/ 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvComputeCoeffForStereo( CvStereoCamera* stereoCamera) 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double quad1[4][2]; 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double quad2[4][2]; 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 4; i++ ) 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad1[i][0] = stereoCamera->quad[0][i].x; 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad1[i][1] = stereoCamera->quad[0][i].y; 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad2[i][0] = stereoCamera->quad[1][i].x; 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad2[i][1] = stereoCamera->quad[1][i].y; 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeCoeffForStereoNew( quad1, 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad2, 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoCamera->warpSize.height, 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoCamera->camera[0]->matrix, 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoCamera->rotMatrix, 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoCamera->transVector, 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoCamera->camera[1]->matrix, 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoCamera->lineCoeffs, 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &(stereoCamera->needSwapCameras)); 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*--------------------------------------------------------------------------------------*/ 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvComCoeffForLine( CvPoint2D64d point1, 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d point2, 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d point3, 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d point4, 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d camMatr1, 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d rotMatr1, 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d transVect1, 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d camMatr2, 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d rotMatr2, 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d transVect2, 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvStereoLineCoeff* coeffs, 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* needSwapCamera) 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Get direction for all points */ 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Direction for camera 1 */ 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double direct1[3]; 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double direct2[3]; 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double camPoint1[3]; 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double directS3[3]; 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double directS4[3]; 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double direct3[3]; 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double direct4[3]; 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double camPoint2[3]; 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetDirectionForPoint( point1, 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr1, 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CvPoint3D64d*)direct1); 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetDirectionForPoint( point2, 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr1, 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CvPoint3D64d*)direct2); 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Direction for camera 2 */ 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetDirectionForPoint( point3, 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr2, 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CvPoint3D64d*)directS3); 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetDirectionForPoint( point4, 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr2, 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CvPoint3D64d*)directS4); 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create convertion for camera 2: two direction and camera point */ 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double convRotMatr[9]; 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double convTransVect[3]; 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCreateConvertMatrVect( rotMatr1, 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect1, 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2, 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect2, 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr, 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convTransVect); 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double zeroVect[3]; 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn zeroVect[0] = zeroVect[1] = zeroVect[2] = 0.0; 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camPoint1[0] = camPoint1[1] = camPoint1[2] = 0.0; 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvConvertPointSystem(*((CvPoint3D64d*)directS3),(CvPoint3D64d*)direct3,convRotMatr,convTransVect); 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvConvertPointSystem(*((CvPoint3D64d*)directS4),(CvPoint3D64d*)direct4,convRotMatr,convTransVect); 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvConvertPointSystem(*((CvPoint3D64d*)zeroVect),(CvPoint3D64d*)camPoint2,convRotMatr,convTransVect); 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double pointB[3]; 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int postype = 0; 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Changed order */ 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute point B: xB,yB,zB */ 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCrossLines(*((CvPoint3D64d*)camPoint1),*((CvPoint3D64d*)direct2), 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *((CvPoint3D64d*)camPoint2),*((CvPoint3D64d*)direct3), 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CvPoint3D64d*)pointB); 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pointB[2] < 0 )/* If negative use other lines for cross */ 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn postype = 1; 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCrossLines(*((CvPoint3D64d*)camPoint1),*((CvPoint3D64d*)direct1), 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *((CvPoint3D64d*)camPoint2),*((CvPoint3D64d*)direct4), 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CvPoint3D64d*)pointB); 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d pointNewA; 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d pointNewC; 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointNewA.x = pointNewA.y = pointNewA.z = 0; 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointNewC.x = pointNewC.y = pointNewC.z = 0; 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( postype == 0 ) 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetSymPoint3D( *((CvPoint3D64d*)camPoint1), 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *((CvPoint3D64d*)direct1), 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *((CvPoint3D64d*)pointB), 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &pointNewA); 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetSymPoint3D( *((CvPoint3D64d*)camPoint2), 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *((CvPoint3D64d*)direct4), 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *((CvPoint3D64d*)pointB), 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &pointNewC); 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* In this case we must change cameras */ 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *needSwapCamera = 1; 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetSymPoint3D( *((CvPoint3D64d*)camPoint2), 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *((CvPoint3D64d*)direct3), 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *((CvPoint3D64d*)pointB), 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &pointNewA); 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetSymPoint3D( *((CvPoint3D64d*)camPoint1), 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *((CvPoint3D64d*)direct2), 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *((CvPoint3D64d*)pointB), 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &pointNewC); 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double gamma; 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x1,y1,z1; 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x1 = camPoint1[0]; 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y1 = camPoint1[1]; 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z1 = camPoint1[2]; 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double xA,yA,zA; 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double xB,yB,zB; 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double xC,yC,zC; 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xA = pointNewA.x; 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yA = pointNewA.y; 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn zA = pointNewA.z; 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xB = pointB[0]; 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yB = pointB[1]; 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn zB = pointB[2]; 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xC = pointNewC.x; 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yC = pointNewC.y; 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn zC = pointNewC.z; 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double len1,len2; 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn len1 = sqrt( (xA-xB)*(xA-xB) + (yA-yB)*(yA-yB) + (zA-zB)*(zA-zB) ); 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn len2 = sqrt( (xB-xC)*(xB-xC) + (yB-yC)*(yB-yC) + (zB-zC)*(zB-zC) ); 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn gamma = len2 / len1; 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeStereoLineCoeffs( pointNewA, 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *((CvPoint3D64d*)pointB), 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *((CvPoint3D64d*)camPoint1), 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn gamma, 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs); 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NO_ERR; 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*--------------------------------------------------------------------------------------*/ 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvGetDirectionForPoint( CvPoint2D64d point, 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d camMatr, 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d* direct) 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* */ 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double invMatr[9]; 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Invert matrix */ 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvInvertMatrix_64d(camMatr,3,invMatr); 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* TEST FOR ERRORS */ 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double vect[3]; 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vect[0] = point.x; 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vect[1] = point.y; 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vect[2] = 1; 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Mul matr */ 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvMulMatrix_64d( invMatr, 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3,3, 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vect, 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1,3, 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (double*)direct); 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NO_ERR; 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*--------------------------------------------------------------------------------------*/ 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvGetCrossLines(CvPoint3D64d point11,CvPoint3D64d point12, 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d point21,CvPoint3D64d point22, 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d* midPoint) 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double xM,yM,zM; 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double xN,yN,zN; 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double xA,yA,zA; 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double xB,yB,zB; 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double xC,yC,zC; 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double xD,yD,zD; 5366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xA = point11.x; 5386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yA = point11.y; 5396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn zA = point11.z; 5406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xB = point12.x; 5426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yB = point12.y; 5436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn zB = point12.z; 5446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xC = point21.x; 5466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yC = point21.y; 5476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn zC = point21.z; 5486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xD = point22.x; 5506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yD = point22.y; 5516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn zD = point22.z; 5526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a11,a12,a21,a22; 5546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double b1,b2; 5556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a11 = (xB-xA)*(xB-xA)+(yB-yA)*(yB-yA)+(zB-zA)*(zB-zA); 5576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a12 = -(xD-xC)*(xB-xA)-(yD-yC)*(yB-yA)-(zD-zC)*(zB-zA); 5586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a21 = (xB-xA)*(xD-xC)+(yB-yA)*(yD-yC)+(zB-zA)*(zD-zC); 5596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a22 = -(xD-xC)*(xD-xC)-(yD-yC)*(yD-yC)-(zD-zC)*(zD-zC); 5606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b1 = -( (xA-xC)*(xB-xA)+(yA-yC)*(yB-yA)+(zA-zC)*(zB-zA) ); 5616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b2 = -( (xA-xC)*(xD-xC)+(yA-yC)*(yD-yC)+(zA-zC)*(zD-zC) ); 5626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double delta; 5646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double deltaA,deltaB; 5656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double alpha,betta; 5666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delta = a11*a22-a12*a21; 5686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(delta) < EPS64D ) 5706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /*return ERROR;*/ 5726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn deltaA = b1*a22-b2*a12; 5756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn deltaB = a11*b2-b1*a21; 5766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn alpha = deltaA / delta; 5786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn betta = deltaB / delta; 5796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xM = xA+alpha*(xB-xA); 5816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yM = yA+alpha*(yB-yA); 5826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn zM = zA+alpha*(zB-zA); 5836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xN = xC+betta*(xD-xC); 5856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yN = yC+betta*(yD-yC); 5866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn zN = zC+betta*(zD-zC); 5876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute middle point */ 5896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn midPoint->x = (xM + xN) * 0.5; 5906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn midPoint->y = (yM + yN) * 0.5; 5916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn midPoint->z = (zM + zN) * 0.5; 5926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NO_ERR; 5946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*--------------------------------------------------------------------------------------*/ 5976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvComputeStereoLineCoeffs( CvPoint3D64d pointA, 5996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d pointB, 6006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d pointCam1, 6016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double gamma, 6026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvStereoLineCoeff* coeffs) 6036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 6046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x1,y1,z1; 6056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x1 = pointCam1.x; 6076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y1 = pointCam1.y; 6086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z1 = pointCam1.z; 6096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double xA,yA,zA; 6116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double xB,yB,zB; 6126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xA = pointA.x; 6146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yA = pointA.y; 6156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn zA = pointA.z; 6166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xB = pointB.x; 6186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yB = pointB.y; 6196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn zB = pointB.z; 6206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( gamma > 0 ) 6226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->Xcoef = -x1 + xA; 6246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->XcoefA = xB + x1 - xA; 6256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->XcoefB = -xA - gamma * x1 + gamma * xA; 6266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->XcoefAB = -xB + xA + gamma * xB - gamma * xA; 6276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->Ycoef = -y1 + yA; 6296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->YcoefA = yB + y1 - yA; 6306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->YcoefB = -yA - gamma * y1 + gamma * yA; 6316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->YcoefAB = -yB + yA + gamma * yB - gamma * yA; 6326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->Zcoef = -z1 + zA; 6346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->ZcoefA = zB + z1 - zA; 6356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->ZcoefB = -zA - gamma * z1 + gamma * zA; 6366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->ZcoefAB = -zB + zA + gamma * zB - gamma * zA; 6376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 6396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn gamma = - gamma; 6416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->Xcoef = -( -x1 + xA); 6426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->XcoefB = -( xB + x1 - xA); 6436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->XcoefA = -( -xA - gamma * x1 + gamma * xA); 6446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->XcoefAB = -( -xB + xA + gamma * xB - gamma * xA); 6456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->Ycoef = -( -y1 + yA); 6476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->YcoefB = -( yB + y1 - yA); 6486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->YcoefA = -( -yA - gamma * y1 + gamma * yA); 6496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->YcoefAB = -( -yB + yA + gamma * yB - gamma * yA); 6506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->Zcoef = -( -z1 + zA); 6526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->ZcoefB = -( zB + z1 - zA); 6536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->ZcoefA = -( -zA - gamma * z1 + gamma * zA); 6546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs->ZcoefAB = -( -zB + zA + gamma * zB - gamma * zA); 6556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NO_ERR; 6606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 6616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*--------------------------------------------------------------------------------------*/ 6626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 6656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* This function get minimum angle started at point which contains rect */ 6676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvGetAngleLine( CvPoint2D64d startPoint, CvSize imageSize,CvPoint2D64d *point1,CvPoint2D64d *point2) 6686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 6696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Get crosslines with image corners */ 6706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find four lines */ 6726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d pa,pb,pc,pd; 6746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pa.x = 0; 6766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pa.y = 0; 6776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pb.x = imageSize.width-1; 6796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pb.y = 0; 6806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pd.x = imageSize.width-1; 6826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pd.y = imageSize.height-1; 6836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pc.x = 0; 6856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pc.y = imageSize.height-1; 6866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We can compute points for angle */ 6886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for place section */ 6896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( startPoint.x < 0 ) 6916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 1,4,7 */ 6926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( startPoint.y < 0) 6936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 1 */ 6946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point1 = pb; 6956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point2 = pc; 6966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( startPoint.y > imageSize.height-1 ) 6986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 7 */ 6996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point1 = pa; 7006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point2 = pd; 7016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 7036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 4 */ 7046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point1 = pa; 7056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point2 = pc; 7066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if ( startPoint.x > imageSize.width-1 ) 7096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 3,6,9 */ 7106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( startPoint.y < 0 ) 7116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 3 */ 7126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point1 = pa; 7136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point2 = pd; 7146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if ( startPoint.y > imageSize.height-1 ) 7166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 9 */ 7176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point1 = pb; 7186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point2 = pc; 7196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 7216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 6 */ 7226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point1 = pb; 7236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point2 = pd; 7246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 7276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 2,5,8 */ 7286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( startPoint.y < 0 ) 7296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 2 */ 7306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( startPoint.x < imageSize.width/2 ) 7316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point1 = pb; 7336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point2 = pa; 7346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 7366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point1 = pa; 7386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point2 = pb; 7396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( startPoint.y > imageSize.height-1 ) 7426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 8 */ 7436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( startPoint.x < imageSize.width/2 ) 7446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point1 = pc; 7466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point2 = pd; 7476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 7496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point1 = pd; 7516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point2 = pc; 7526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 7556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 5 - point in the image */ 7566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return 2; 7576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return 0; 7606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn}/* GetAngleLine */ 7616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 7636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvGetCoefForPiece( CvPoint2D64d p_start,CvPoint2D64d p_end, 7656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double *a,double *b,double *c, 7666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* result) 7676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 7686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double det; 7696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double detA,detB,detC; 7706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 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; 7726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(det) < EPS64D)/* Error */ 7736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 0; 7756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 7766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn detA = p_start.y - p_end.y; 7796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn detB = p_end.x - p_start.x; 7806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn detC = p_start.x*p_end.y - p_end.x*p_start.y; 7816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double invDet = 1.0 / det; 7836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *a = detA * invDet; 7846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *b = detB * invDet; 7856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *c = detC * invDet; 7866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 1; 7886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 7896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 7906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 7926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Get common area of rectifying */ 7946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvGetCommonArea( CvSize imageSize, 7956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d epipole1,CvPoint3D64d epipole2, 7966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d fundMatr, 7976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvVect64d coeff11,CvVect64d coeff12, 7986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvVect64d coeff21,CvVect64d coeff22, 7996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* result) 8006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 8016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int res = 0; 8026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d point11; 8036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d point12; 8046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d point21; 8056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d point22; 8066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double corr11[3]; 8086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double corr12[3]; 8096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double corr21[3]; 8106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double corr22[3]; 8116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double pointW11[3]; 8136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double pointW12[3]; 8146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double pointW21[3]; 8156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double pointW22[3]; 8166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double transFundMatr[3*3]; 8186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute transpose of fundamental matrix */ 8196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvTransposeMatrix_64d( fundMatr, 3, 3, transFundMatr ); 8206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d epipole1_2d; 8226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d epipole2_2d; 8236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(epipole1.z) < 1e-8 ) 8256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* epipole1 in infinity */ 8266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 0; 8276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 8286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole1_2d.x = epipole1.x / epipole1.z; 8306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole1_2d.y = epipole1.y / epipole1.z; 8316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(epipole2.z) < 1e-8 ) 8336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* epipole2 in infinity */ 8346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 0; 8356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 8366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole2_2d.x = epipole2.x / epipole2.z; 8386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole2_2d.y = epipole2.y / epipole2.z; 8396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int stat = icvGetAngleLine( epipole1_2d, imageSize,&point11,&point12); 8416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( stat == 2 ) 8426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* No angle */ 8446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 0; 8456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 8466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stat = icvGetAngleLine( epipole2_2d, imageSize,&point21,&point22); 8496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( stat == 2 ) 8506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* No angle */ 8526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 0; 8536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 8546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ============= Computation for line 1 ================ */ 8576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find correspondence line for angle points11 */ 8586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* corr21 = Fund'*p1 */ 8596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointW11[0] = point11.x; 8616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointW11[1] = point11.y; 8626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointW11[2] = 1.0; 8636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvTransformVector_64d( transFundMatr, /* !!! Modified from not transposed */ 8656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointW11, 8666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corr21, 8676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3,3); 8686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find crossing of line with image 2 */ 8706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d start; 8716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d end; 8726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCrossRectDirect( imageSize, 8736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corr21[0],corr21[1],corr21[2], 8746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &start,&end, 8756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &res); 8766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( res == 0 ) 8786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* We have not cross */ 8796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We must define new angle */ 8806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointW21[0] = point21.x; 8826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointW21[1] = point21.y; 8836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointW21[2] = 1.0; 8846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find correspondence line for this angle points */ 8866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We know point and try to get corr line */ 8876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* For point21 */ 8886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* corr11 = Fund * p21 */ 8896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvTransformVector_64d( fundMatr, /* !!! Modified */ 8916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointW21, 8926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corr11, 8936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3,3); 8946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We have cross. And it's result cross for up line. Set result coefs */ 8966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Set coefs for line 1 image 1 */ 8986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeff11[0] = corr11[0]; 8996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeff11[1] = corr11[1]; 9006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeff11[2] = corr11[2]; 9016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Set coefs for line 1 image 2 */ 9036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCoefForPiece( epipole2_2d,point21, 9046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &coeff21[0],&coeff21[1],&coeff21[2], 9056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &res); 9066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( res == 0 ) 9076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 0; 9096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return;/* Error */ 9106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 9136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Line 1 cross image 2 */ 9146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Set coefs for line 1 image 1 */ 9156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCoefForPiece( epipole1_2d,point11, 9166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &coeff11[0],&coeff11[1],&coeff11[2], 9176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &res); 9186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( res == 0 ) 9196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 0; 9216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return;/* Error */ 9226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Set coefs for line 1 image 2 */ 9256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeff21[0] = corr21[0]; 9266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeff21[1] = corr21[1]; 9276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeff21[2] = corr21[2]; 9286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ============= Computation for line 2 ================ */ 9326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find correspondence line for angle points11 */ 9336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* corr22 = Fund*p2 */ 9346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointW12[0] = point12.x; 9366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointW12[1] = point12.y; 9376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointW12[2] = 1.0; 9386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvTransformVector_64d( transFundMatr, 9406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointW12, 9416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corr22, 9426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3,3); 9436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find crossing of line with image 2 */ 9456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCrossRectDirect( imageSize, 9466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corr22[0],corr22[1],corr22[2], 9476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &start,&end, 9486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &res); 9496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( res == 0 ) 9516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* We have not cross */ 9526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We must define new angle */ 9536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointW22[0] = point22.x; 9556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointW22[1] = point22.y; 9566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointW22[2] = 1.0; 9576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find correspondence line for this angle points */ 9596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We know point and try to get corr line */ 9606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* For point21 */ 9616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* corr2 = Fund' * p1 */ 9626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvTransformVector_64d( fundMatr, 9646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointW22, 9656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corr12, 9666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3,3); 9676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We have cross. And it's result cross for down line. Set result coefs */ 9706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Set coefs for line 2 image 1 */ 9726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeff12[0] = corr12[0]; 9736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeff12[1] = corr12[1]; 9746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeff12[2] = corr12[2]; 9756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Set coefs for line 1 image 2 */ 9776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCoefForPiece( epipole2_2d,point22, 9786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &coeff22[0],&coeff22[1],&coeff22[2], 9796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &res); 9806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( res == 0 ) 9816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 0; 9836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return;/* Error */ 9846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 9876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Line 2 cross image 2 */ 9886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Set coefs for line 2 image 1 */ 9896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCoefForPiece( epipole1_2d,point12, 9906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &coeff12[0],&coeff12[1],&coeff12[2], 9916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &res); 9926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( res == 0 ) 9936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 0; 9956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return;/* Error */ 9966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Set coefs for line 1 image 2 */ 9996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeff22[0] = corr22[0]; 10006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeff22[1] = corr22[1]; 10016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeff22[2] = corr22[2]; 10026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Now we know common area */ 10066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 10086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn}/* GetCommonArea */ 10106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 10126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Get cross for direction1 and direction2 */ 10146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Result = 1 - cross */ 10156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Result = 2 - parallel and not equal */ 10166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Result = 3 - parallel and equal */ 10176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvGetCrossDirectDirect( CvVect64d direct1,CvVect64d direct2, 10196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d *cross,int* result) 10206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 10216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double det = direct1[0]*direct2[1] - direct2[0]*direct1[1]; 10226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double detx = -direct1[2]*direct2[1] + direct1[1]*direct2[2]; 10236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(det) > EPS64D ) 10256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Have cross */ 10266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cross->x = detx/det; 10276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cross->y = (-direct1[0]*direct2[2] + direct2[0]*direct1[2])/det; 10286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 1; 10296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 10316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* may be parallel */ 10326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(detx) > EPS64D ) 10336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* parallel and not equal */ 10346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 2; 10356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 10376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* equals */ 10386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 3; 10396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 10436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 10446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 10466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Get cross for piece p1,p2 and direction a,b,c */ 10486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Result = 0 - no cross */ 10496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Result = 1 - cross */ 10506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Result = 2 - parallel and not equal */ 10516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Result = 3 - parallel and equal */ 10526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvGetCrossPieceDirect( CvPoint2D64d p_start,CvPoint2D64d p_end, 10546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a,double b,double c, 10556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d *cross,int* result) 10566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 10576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (a*p_start.x + b*p_start.y + c) * (a*p_end.x + b*p_end.y + c) <= 0 ) 10596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Have cross */ 10606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double det; 10616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double detxc,detyc; 10626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn det = a * (p_end.x - p_start.x) + b * (p_end.y - p_start.y); 10646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(det) < EPS64D ) 10666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* lines are parallel and may be equal or line is point */ 10676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(a*p_start.x + b*p_start.y + c) < EPS64D ) 10686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* line is point or not diff */ 10696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 3; 10706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 10716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 10736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 2; 10756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 10776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn detxc = b*(p_end.y*p_start.x - p_start.y*p_end.x) + c*(p_start.x - p_end.x); 10806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn detyc = a*(p_end.x*p_start.y - p_start.x*p_end.y) + c*(p_start.y - p_end.y); 10816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cross->x = detxc / det; 10836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cross->y = detyc / det; 10846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 1; 10856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 10886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 0; 10906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 10926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 10936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*--------------------------------------------------------------------------------------*/ 10946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvGetCrossPiecePiece( CvPoint2D64d p1_start,CvPoint2D64d p1_end, 10966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d p2_start,CvPoint2D64d p2_end, 10976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d* cross, 10986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* result) 10996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 11006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double ex1,ey1,ex2,ey2; 11016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double px1,py1,px2,py2; 11026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double del; 11036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double delA,delB,delX,delY; 11046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double alpha,betta; 11056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ex1 = p1_start.x; 11076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ey1 = p1_start.y; 11086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ex2 = p1_end.x; 11096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ey2 = p1_end.y; 11106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn px1 = p2_start.x; 11126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn py1 = p2_start.y; 11136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn px2 = p2_end.x; 11146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn py2 = p2_end.y; 11156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn del = (py1-py2)*(ex1-ex2)-(px1-px2)*(ey1-ey2); 11176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(del) <= EPS64D ) 11186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* May be they are parallel !!! */ 11196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 0; 11206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 11216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delA = (ey1-ey2)*(ex1-px1) + (ex1-ex2)*(py1-ey1); 11246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delB = (py1-py2)*(ex1-px1) + (px1-px2)*(py1-ey1); 11256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn alpha = delA / del; 11276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn betta = delB / del; 11286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0) 11306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 0; 11326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 11336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delX = (px1-px2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2))+ 11366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (ex1-ex2)*(px1*(py1-py2)-py1*(px1-px2)); 11376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delY = (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2))+ 11396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2)); 11406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cross->x = delX / del; 11426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cross->y = delY / del; 11436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 1; 11456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 11466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 11476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 11506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvGetPieceLength(CvPoint2D64d point1,CvPoint2D64d point2,double* dist) 11526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 11536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dx = point2.x - point1.x; 11546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dy = point2.y - point1.y; 11556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *dist = sqrt( dx*dx + dy*dy ); 11566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 11576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 11586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 11606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvGetPieceLength3D(CvPoint3D64d point1,CvPoint3D64d point2,double* dist) 11626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 11636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dx = point2.x - point1.x; 11646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dy = point2.y - point1.y; 11656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dz = point2.z - point1.z; 11666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *dist = sqrt( dx*dx + dy*dy + dz*dz ); 11676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 11686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 11696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 11716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Find line from epipole which cross image rect */ 11736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Find points of cross 0 or 1 or 2. Return number of points in cross */ 11746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvGetCrossRectDirect( CvSize imageSize, 11756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a,double b,double c, 11766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d *start,CvPoint2D64d *end, 11776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* result) 11786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 11796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d frameBeg; 11806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d frameEnd; 11816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d cross[4]; 11826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int haveCross[4]; 11836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[0] = 0; 11856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[1] = 0; 11866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[2] = 0; 11876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[3] = 0; 11886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.x = 0; 11906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.y = 0; 11916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.x = imageSize.width; 11926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.y = 0; 11936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[0],&haveCross[0]); 11956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.x = imageSize.width; 11976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.y = 0; 11986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.x = imageSize.width; 11996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.y = imageSize.height; 12006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[1],&haveCross[1]); 12016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.x = imageSize.width; 12036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.y = imageSize.height; 12046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.x = 0; 12056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.y = imageSize.height; 12066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[2],&haveCross[2]); 12076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.x = 0; 12096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.y = imageSize.height; 12106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.x = 0; 12116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.y = 0; 12126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCrossPieceDirect(frameBeg,frameEnd,a,b,c,&cross[3],&haveCross[3]); 12136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double maxDist; 12156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int maxI=0,maxJ=0; 12176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i,j; 12206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxDist = -1.0; 12226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double distance; 12246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ ) 12266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( haveCross[i] == 1 ) 12286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = i + 1; j < 4; j++ ) 12306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( haveCross[j] == 1) 12326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Compute dist */ 12336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetPieceLength(cross[i],cross[j],&distance); 12346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( distance > maxDist ) 12356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxI = i; 12376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxJ = j; 12386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxDist = distance; 12396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( maxDist >= 0 ) 12466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* We have cross */ 12476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *start = cross[maxI]; 12486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 1; 12496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( maxDist > 0 ) 12506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *end = cross[maxJ]; 12526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 2; 12536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 12566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 0; 12586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 12616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn}/* GetCrossRectDirect */ 12626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 12646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvProjectPointToImage( CvPoint3D64d point, 12656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d camMatr,CvMatr64d rotMatr,CvVect64d transVect, 12666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d* projPoint) 12676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 12686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tmpVect1[3]; 12706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tmpVect2[3]; 12716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvMulMatrix_64d ( rotMatr, 12736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3,3, 12746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (double*)&point, 12756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1,3, 12766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmpVect1); 12776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvAddVector_64d ( tmpVect1, transVect,tmpVect2, 3); 12796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvMulMatrix_64d ( camMatr, 12816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3,3, 12826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmpVect2, 12836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1,3, 12846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmpVect1); 12856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projPoint->x = tmpVect1[0] / tmpVect1[2]; 12876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projPoint->y = tmpVect1[1] / tmpVect1[2]; 12886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 12906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 12916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 12936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Get quads for transform images */ 12946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvGetQuadsTransform( 12956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize imageSize, 12966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d camMatr1, 12976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d rotMatr1, 12986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvVect64d transVect1, 12996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d camMatr2, 13006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d rotMatr2, 13016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvVect64d transVect2, 13026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize* warpSize, 13036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double quad1[4][2], 13046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double quad2[4][2], 13056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d fundMatr, 13066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d* epipole1, 13076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d* epipole2 13086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ) 13096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 13106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* First compute fundamental matrix and epipoles */ 13116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int res; 13126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute epipoles and fundamental matrix using new functions */ 13156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double convRotMatr[9]; 13176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double convTransVect[3]; 13186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCreateConvertMatrVect( rotMatr1, 13206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect1, 13216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2, 13226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect2, 13236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr, 13246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convTransVect); 13256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float convRotMatr_32f[9]; 13266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float convTransVect_32f[3]; 13276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_64d_32f(convRotMatr,convRotMatr_32f,9); 13296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_64d_32f(convTransVect,convTransVect_32f,3); 13306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We know R and t */ 13326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute essential matrix */ 13336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float essMatr[9]; 13346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float fundMatr_32f[9]; 13356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float camMatr1_32f[9]; 13376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float camMatr2_32f[9]; 13386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_64d_32f(camMatr1,camMatr1_32f,9); 13406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_64d_32f(camMatr2,camMatr2_32f,9); 13416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvComputeEssentialMatrix( convRotMatr_32f, 13436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convTransVect_32f, 13446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn essMatr); 13456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvertEssential2Fundamental( essMatr, 13476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fundMatr_32f, 13486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr1_32f, 13496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr2_32f); 13506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f epipole1_32f; 13526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f epipole2_32f; 13536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvComputeEpipolesFromFundMatrix( fundMatr_32f, 13556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &epipole1_32f, 13566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &epipole2_32f); 13576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* copy to 64d epipoles */ 13586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole1->x = epipole1_32f.x; 13596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole1->y = epipole1_32f.y; 13606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole1->z = epipole1_32f.z; 13616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole2->x = epipole2_32f.x; 13636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole2->y = epipole2_32f.y; 13646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole2->z = epipole2_32f.z; 13656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Convert fundamental matrix */ 13676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(fundMatr_32f,fundMatr,9); 13686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double coeff11[3]; 13716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double coeff12[3]; 13726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double coeff21[3]; 13736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double coeff22[3]; 13746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCommonArea( imageSize, 13766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *epipole1,*epipole2, 13776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fundMatr, 13786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeff11,coeff12, 13796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeff21,coeff22, 13806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &res); 13816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d point11, point12,point21, point22; 13836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double width1,width2; 13846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double height1,height2; 13856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tmpHeight1,tmpHeight2; 13866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d epipole1_2d; 13886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d epipole2_2d; 13896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- Image 1 ----- */ 13916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(epipole1->z) < 1e-8 ) 13926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 13946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole1_2d.x = epipole1->x / epipole1->z; 13966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole1_2d.y = epipole1->y / epipole1->z; 13976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCutPiece( coeff11,coeff12, 13996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole1_2d, 14006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imageSize, 14016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &point11,&point12, 14026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &point21,&point22, 14036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &res); 14046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute distance */ 14066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetPieceLength(point11,point21,&width1); 14076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetPieceLength(point11,point12,&tmpHeight1); 14086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetPieceLength(point21,point22,&tmpHeight2); 14096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn height1 = MAX(tmpHeight1,tmpHeight2); 14106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad1[0][0] = point11.x; 14126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad1[0][1] = point11.y; 14136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad1[1][0] = point21.x; 14156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad1[1][1] = point21.y; 14166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad1[2][0] = point22.x; 14186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad1[2][1] = point22.y; 14196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad1[3][0] = point12.x; 14216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad1[3][1] = point12.y; 14226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- Image 2 ----- */ 14246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(epipole2->z) < 1e-8 ) 14256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 14276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole2_2d.x = epipole2->x / epipole2->z; 14296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole2_2d.y = epipole2->y / epipole2->z; 14306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCutPiece( coeff21,coeff22, 14326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole2_2d, 14336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imageSize, 14346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &point11,&point12, 14356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &point21,&point22, 14366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &res); 14376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute distance */ 14396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetPieceLength(point11,point21,&width2); 14406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetPieceLength(point11,point12,&tmpHeight1); 14416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetPieceLength(point21,point22,&tmpHeight2); 14426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn height2 = MAX(tmpHeight1,tmpHeight2); 14436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad2[0][0] = point11.x; 14456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad2[0][1] = point11.y; 14466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad2[1][0] = point21.x; 14486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad2[1][1] = point21.y; 14496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad2[2][0] = point22.x; 14516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad2[2][1] = point22.y; 14526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad2[3][0] = point12.x; 14546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad2[3][1] = point12.y; 14556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /*=======================================================*/ 14586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* This is a new additional way to compute quads. */ 14596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We must correct quads */ 14606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double convRotMatr[9]; 14626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double convTransVect[3]; 14636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double newQuad1[4][2]; 14656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double newQuad2[4][2]; 14666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCreateConvertMatrVect( rotMatr1, 14696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect1, 14706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2, 14716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect2, 14726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr, 14736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convTransVect); 14746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* -------------Compute for first image-------------- */ 14766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f pointb1; 14776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f pointe1; 14786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f pointb2; 14806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f pointe2; 14816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointb1.x = (float)quad1[0][0]; 14836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointb1.y = (float)quad1[0][1]; 14846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointe1.x = (float)quad1[3][0]; 14866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointe1.y = (float)quad1[3][1]; 14876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeeInfiniteProject1(convRotMatr, 14896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr1, 14906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr2, 14916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointb1, 14926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &pointb2); 14936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeeInfiniteProject1(convRotMatr, 14956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr1, 14966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr2, 14976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointe1, 14986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &pointe2); 14996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* JUST TEST FOR POINT */ 15016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute distances */ 15036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dxOld,dyOld; 15046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dxNew,dyNew; 15056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double distOld,distNew; 15066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dxOld = quad2[1][0] - quad2[0][0]; 15086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dyOld = quad2[1][1] - quad2[0][1]; 15096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn distOld = dxOld*dxOld + dyOld*dyOld; 15106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dxNew = quad2[1][0] - pointb2.x; 15126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dyNew = quad2[1][1] - pointb2.y; 15136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn distNew = dxNew*dxNew + dyNew*dyNew; 15146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( distNew > distOld ) 15166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Get new points for second quad */ 15176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad2[0][0] = pointb2.x; 15186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad2[0][1] = pointb2.y; 15196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad2[3][0] = pointe2.x; 15206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad2[3][1] = pointe2.y; 15216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad1[0][0] = quad1[0][0]; 15226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad1[0][1] = quad1[0][1]; 15236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad1[3][0] = quad1[3][0]; 15246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad1[3][1] = quad1[3][1]; 15256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 15276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Get new points for first quad */ 15286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointb2.x = (float)quad2[0][0]; 15306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointb2.y = (float)quad2[0][1]; 15316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointe2.x = (float)quad2[3][0]; 15336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointe2.y = (float)quad2[3][1]; 15346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeeInfiniteProject2(convRotMatr, 15366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr1, 15376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr2, 15386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &pointb1, 15396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointb2); 15406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeeInfiniteProject2(convRotMatr, 15426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr1, 15436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr2, 15446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &pointe1, 15456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointe2); 15466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* JUST TEST FOR POINT */ 15496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad2[0][0] = quad2[0][0]; 15516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad2[0][1] = quad2[0][1]; 15526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad2[3][0] = quad2[3][0]; 15536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad2[3][1] = quad2[3][1]; 15546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad1[0][0] = pointb1.x; 15566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad1[0][1] = pointb1.y; 15576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad1[3][0] = pointe1.x; 15586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad1[3][1] = pointe1.y; 15596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* -------------Compute for second image-------------- */ 15626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointb1.x = (float)quad1[1][0]; 15636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointb1.y = (float)quad1[1][1]; 15646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointe1.x = (float)quad1[2][0]; 15666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointe1.y = (float)quad1[2][1]; 15676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeeInfiniteProject1(convRotMatr, 15696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr1, 15706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr2, 15716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointb1, 15726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &pointb2); 15736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeeInfiniteProject1(convRotMatr, 15756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr1, 15766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr2, 15776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointe1, 15786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &pointe2); 15796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute distances */ 15816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dxOld = quad2[0][0] - quad2[1][0]; 15836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dyOld = quad2[0][1] - quad2[1][1]; 15846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn distOld = dxOld*dxOld + dyOld*dyOld; 15856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dxNew = quad2[0][0] - pointb2.x; 15876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dyNew = quad2[0][1] - pointb2.y; 15886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn distNew = dxNew*dxNew + dyNew*dyNew; 15896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( distNew > distOld ) 15916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Get new points for second quad */ 15926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad2[1][0] = pointb2.x; 15936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad2[1][1] = pointb2.y; 15946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad2[2][0] = pointe2.x; 15956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad2[2][1] = pointe2.y; 15966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad1[1][0] = quad1[1][0]; 15976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad1[1][1] = quad1[1][1]; 15986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad1[2][0] = quad1[2][0]; 15996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad1[2][1] = quad1[2][1]; 16006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 16026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Get new points for first quad */ 16036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointb2.x = (float)quad2[1][0]; 16056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointb2.y = (float)quad2[1][1]; 16066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointe2.x = (float)quad2[2][0]; 16086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointe2.y = (float)quad2[2][1]; 16096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeeInfiniteProject2(convRotMatr, 16116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr1, 16126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr2, 16136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &pointb1, 16146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointb2); 16156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeeInfiniteProject2(convRotMatr, 16176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr1, 16186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr2, 16196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &pointe1, 16206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointe2); 16216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad2[1][0] = quad2[1][0]; 16236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad2[1][1] = quad2[1][1]; 16246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad2[2][0] = quad2[2][0]; 16256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad2[2][1] = quad2[2][1]; 16266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad1[1][0] = pointb1.x; 16286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad1[1][1] = pointb1.y; 16296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad1[2][0] = pointe1.x; 16306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newQuad1[2][1] = pointe1.y; 16316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*-------------------------------------------------------------------------------*/ 16366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy new quads to old quad */ 16386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 16396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 4; i++ ) 16406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad1[i][0] = newQuad1[i][0]; 16436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad1[i][1] = newQuad1[i][1]; 16446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad2[i][0] = newQuad2[i][0]; 16456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad2[i][1] = newQuad2[i][1]; 16466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /*=======================================================*/ 16506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double warpWidth,warpHeight; 16526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn warpWidth = MAX(width1,width2); 16546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn warpHeight = MAX(height1,height2); 16556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn warpSize->width = (int)warpWidth; 16576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn warpSize->height = (int)warpHeight; 16586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn warpSize->width = cvRound(warpWidth-1); 16606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn warpSize->height = cvRound(warpHeight-1); 16616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* !!! by Valery Mosyagin. this lines added just for test no warp */ 16636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn warpSize->width = imageSize.width; 16646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn warpSize->height = imageSize.height; 16656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 16676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 16686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 16716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvGetQuadsTransformNew( CvSize imageSize, 16736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f camMatr1, 16746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f camMatr2, 16756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f rotMatr1, 16766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvVect32f transVect1, 16776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize* warpSize, 16786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double quad1[4][2], 16796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double quad2[4][2], 16806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f fundMatr, 16816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f* epipole1, 16826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f* epipole2 16836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ) 16846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 16856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Convert data */ 16866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Convert camera matrix */ 16876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double camMatr1_64d[9]; 16886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double camMatr2_64d[9]; 16896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double rotMatr1_64d[9]; 16906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double transVect1_64d[3]; 16916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double rotMatr2_64d[9]; 16926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double transVect2_64d[3]; 16936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double fundMatr_64d[9]; 16946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d epipole1_64d; 16956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d epipole2_64d; 16966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(camMatr1,camMatr1_64d,9); 16986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(camMatr2,camMatr2_64d,9); 16996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(rotMatr1,rotMatr1_64d,9); 17006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(transVect1,transVect1_64d,3); 17016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create vector and matrix */ 17036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[0] = 1; 17056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[1] = 0; 17066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[2] = 0; 17076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[3] = 0; 17086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[4] = 1; 17096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[5] = 0; 17106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[6] = 0; 17116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[7] = 0; 17126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d[8] = 1; 17136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect2_64d[0] = 0; 17156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect2_64d[1] = 0; 17166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect2_64d[2] = 0; 17176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetQuadsTransform( imageSize, 17196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr1_64d, 17206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr1_64d, 17216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect1_64d, 17226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camMatr2_64d, 17236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2_64d, 17246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect2_64d, 17256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn warpSize, 17266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad1, 17276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad2, 17286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fundMatr_64d, 17296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &epipole1_64d, 17306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &epipole2_64d 17316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ); 17326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Convert epipoles */ 17346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole1->x = (float)(epipole1_64d.x); 17356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole1->y = (float)(epipole1_64d.y); 17366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole1->z = (float)(epipole1_64d.z); 17376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole2->x = (float)(epipole2_64d.x); 17396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole2->y = (float)(epipole2_64d.y); 17406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole2->z = (float)(epipole2_64d.z); 17416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Convert fundamental matrix */ 17436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_64d_32f(fundMatr_64d,fundMatr,9); 17446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 17466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 17476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 17496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvGetQuadsTransformStruct( CvStereoCamera* stereoCamera) 17506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 17516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Wrapper for icvGetQuadsTransformNew */ 17526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double quad1[4][2]; 17556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double quad2[4][2]; 17566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetQuadsTransformNew( cvSize(cvRound(stereoCamera->camera[0]->imgSize[0]),cvRound(stereoCamera->camera[0]->imgSize[1])), 17586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoCamera->camera[0]->matrix, 17596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoCamera->camera[1]->matrix, 17606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoCamera->rotMatrix, 17616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoCamera->transVector, 17626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &(stereoCamera->warpSize), 17636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad1, 17646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quad2, 17656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoCamera->fundMatr, 17666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &(stereoCamera->epipole[0]), 17676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &(stereoCamera->epipole[1]) 17686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ); 17696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 17716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 4; i++ ) 17726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoCamera->quad[0][i] = cvPoint2D32f(quad1[i][0],quad1[i][1]); 17746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoCamera->quad[1][i] = cvPoint2D32f(quad2[i][0],quad2[i][1]); 17756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 17786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 17796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 17816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeStereoParamsForCameras(CvStereoCamera* stereoCamera) 17826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 17836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* For given intrinsic and extrinsic parameters computes rest parameters 17846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ** such as fundamental matrix. warping coeffs, epipoles, ... 17856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn */ 17866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* compute rotate matrix and translate vector */ 17896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double rotMatr1[9]; 17906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double rotMatr2[9]; 17916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double transVect1[3]; 17936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double transVect2[3]; 17946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double convRotMatr[9]; 17966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double convTransVect[3]; 17976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* fill matrices */ 17996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(stereoCamera->camera[0]->rotMatr,rotMatr1,9); 18006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(stereoCamera->camera[1]->rotMatr,rotMatr2,9); 18016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(stereoCamera->camera[0]->transVect,transVect1,3); 18036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(stereoCamera->camera[1]->transVect,transVect2,3); 18046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCreateConvertMatrVect( rotMatr1, 18066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect1, 18076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatr2, 18086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVect2, 18096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr, 18106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convTransVect); 18116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* copy to stereo camera params */ 18136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_64d_32f(convRotMatr,stereoCamera->rotMatrix,9); 18146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_64d_32f(convTransVect,stereoCamera->transVector,3); 18156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetQuadsTransformStruct(stereoCamera); 18186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeRestStereoParams(stereoCamera); 18196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 18206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 18246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Get cut line for one image */ 18266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvGetCutPiece( CvVect64d areaLineCoef1,CvVect64d areaLineCoef2, 18276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d epipole, 18286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize imageSize, 18296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d* point11,CvPoint2D64d* point12, 18306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d* point21,CvPoint2D64d* point22, 18316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* result) 18326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 18336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute nearest cut line to epipole */ 18346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Get corners inside sector */ 18356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Collect all candidate point */ 18366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d candPoints[8]; 18386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d midPoint; 18396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints = 0; 18406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int res; 18416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 18426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double cutLine1[3]; 18446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double cutLine2[3]; 18456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find middle line of sector */ 18476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double midLine[3]; 18486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Different way */ 18516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d pointOnLine1; pointOnLine1.x = pointOnLine1.y = 0; 18526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d pointOnLine2; pointOnLine2.x = pointOnLine2.y = 0; 18536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d start1,end1; 18556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCrossRectDirect( imageSize, 18576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn areaLineCoef1[0],areaLineCoef1[1],areaLineCoef1[2], 18586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &start1,&end1,&res); 18596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( res > 0 ) 18606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointOnLine1 = start1; 18626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCrossRectDirect( imageSize, 18656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn areaLineCoef2[0],areaLineCoef2[1],areaLineCoef2[2], 18666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &start1,&end1,&res); 18676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( res > 0 ) 18686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointOnLine2 = start1; 18706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetMiddleAnglePoint(epipole,pointOnLine1,pointOnLine2,&midPoint); 18736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCoefForPiece(epipole,midPoint,&midLine[0],&midLine[1],&midLine[2],&res); 18756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test corner points */ 18776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d cornerPoint; 18786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d tmpPoints[2]; 18796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cornerPoint.x = 0; 18816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cornerPoint.y = 0; 18826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res); 18836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( res == 1 ) 18846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Add point */ 18856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn candPoints[numPoints] = cornerPoint; 18866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints++; 18876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cornerPoint.x = imageSize.width; 18906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cornerPoint.y = 0; 18916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res); 18926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( res == 1 ) 18936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Add point */ 18946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn candPoints[numPoints] = cornerPoint; 18956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints++; 18966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cornerPoint.x = imageSize.width; 18996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cornerPoint.y = imageSize.height; 19006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res); 19016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( res == 1 ) 19026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Add point */ 19036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn candPoints[numPoints] = cornerPoint; 19046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints++; 19056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cornerPoint.x = 0; 19086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cornerPoint.y = imageSize.height; 19096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvTestPoint( cornerPoint, areaLineCoef1, areaLineCoef2, epipole, &res); 19106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( res == 1 ) 19116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Add point */ 19126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn candPoints[numPoints] = cornerPoint; 19136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints++; 19146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find cross line 1 with image border */ 19176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCrossRectDirect( imageSize, 19186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn areaLineCoef1[0],areaLineCoef1[1],areaLineCoef1[2], 19196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &tmpPoints[0], &tmpPoints[1], 19206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &res); 19216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < res; i++ ) 19226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn candPoints[numPoints++] = tmpPoints[i]; 19246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find cross line 2 with image border */ 19276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCrossRectDirect( imageSize, 19286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn areaLineCoef2[0],areaLineCoef2[1],areaLineCoef2[2], 19296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &tmpPoints[0], &tmpPoints[1], 19306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &res); 19316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < res; i++ ) 19336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn candPoints[numPoints++] = tmpPoints[i]; 19356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 2 ) 19386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 0; 19406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return;/* Error. Not enought points */ 19416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Project all points to middle line and get max and min */ 19436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d projPoint; 19456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d minPoint; minPoint.x = minPoint.y = FLT_MAX; 19466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d maxPoint; maxPoint.x = maxPoint.y = -FLT_MAX; 19476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dist; 19506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double maxDist = 0; 19516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double minDist = 10000000; 19526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 19556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvProjectPointToDirect(candPoints[i], midLine, &projPoint); 19576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetPieceLength(epipole,projPoint,&dist); 19586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dist < minDist) 19596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minDist = dist; 19616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minPoint = projPoint; 19626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dist > maxDist) 19656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxDist = dist; 19676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxPoint = projPoint; 19686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We know maximum and minimum points. Now we can compute cut lines */ 19726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetNormalDirect(midLine,minPoint,cutLine1); 19746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetNormalDirect(midLine,maxPoint,cutLine2); 19756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for begin of line. */ 19776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d tmpPoint2; 19786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Get cross with */ 19806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCrossDirectDirect(areaLineCoef1,cutLine1,point11,&res); 19816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCrossDirectDirect(areaLineCoef2,cutLine1,point12,&res); 19826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCrossDirectDirect(areaLineCoef1,cutLine2,point21,&res); 19846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCrossDirectDirect(areaLineCoef2,cutLine2,point22,&res); 19856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( epipole.x > imageSize.width * 0.5 ) 19876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Need to change points */ 19886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmpPoint2 = *point11; 19896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point11 = *point21; 19906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point21 = tmpPoint2; 19916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmpPoint2 = *point12; 19936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point12 = *point22; 19946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *point22 = tmpPoint2; 19956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 19986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 19996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 20006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Get middle angle */ 20016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvGetMiddleAnglePoint( CvPoint2D64d basePoint, 20026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d point1,CvPoint2D64d point2, 20036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d* midPoint) 20046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{/* !!! May be need to return error */ 20056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dist1; 20076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dist2; 20086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetPieceLength(basePoint,point1,&dist1); 20096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetPieceLength(basePoint,point2,&dist2); 20106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d pointNew1; 20116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d pointNew2; 20126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double alpha = dist2/dist1; 20136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointNew1.x = basePoint.x + (1.0/alpha) * ( point2.x - basePoint.x ); 20156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointNew1.y = basePoint.y + (1.0/alpha) * ( point2.y - basePoint.y ); 20166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointNew2.x = basePoint.x + alpha * ( point1.x - basePoint.x ); 20186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointNew2.y = basePoint.y + alpha * ( point1.y - basePoint.y ); 20196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int res; 20216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetCrossPiecePiece(point1,point2,pointNew1,pointNew2,midPoint,&res); 20226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 20246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 20256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 20276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Get normal direct to direct in line */ 20286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvGetNormalDirect(CvVect64d direct,CvPoint2D64d point,CvVect64d normDirect) 20296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 20306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn normDirect[0] = direct[1]; 20316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn normDirect[1] = - direct[0]; 20326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn normDirect[2] = -(normDirect[0]*point.x + normDirect[1]*point.y); 20336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 20346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 20356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 20376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL double icvGetVect(CvPoint2D64d basePoint,CvPoint2D64d point1,CvPoint2D64d point2) 20386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 20396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return (point1.x - basePoint.x)*(point2.y - basePoint.y) - 20406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (point2.x - basePoint.x)*(point1.y - basePoint.y); 20416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 20426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 20436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Test for point in sector */ 20446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Return 0 - point not inside sector */ 20456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Return 1 - point inside sector */ 20466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvTestPoint( CvPoint2D64d testPoint, 20476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvVect64d line1,CvVect64d line2, 20486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d basePoint, 20496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* result) 20506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 20516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d point1,point2; 20526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvProjectPointToDirect(testPoint,line1,&point1); 20546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvProjectPointToDirect(testPoint,line2,&point2); 20556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double sign1 = icvGetVect(basePoint,point1,point2); 20576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double sign2 = icvGetVect(basePoint,point1,testPoint); 20586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( sign1 * sign2 > 0 ) 20596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Correct for first line */ 20606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sign1 = - sign1; 20616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sign2 = icvGetVect(basePoint,point2,testPoint); 20626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( sign1 * sign2 > 0 ) 20636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Correct for both lines */ 20646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 1; 20656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 20676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 20686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 0; 20696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 20726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 20736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *result = 0; 20746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 20776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 20786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 20806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Project point to line */ 20816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvProjectPointToDirect( CvPoint2D64d point,CvVect64d lineCoeff, 20826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d* projectPoint) 20836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 20846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a = lineCoeff[0]; 20856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double b = lineCoeff[1]; 20866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double det = 1.0 / ( a*a + b*b ); 20886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double delta = a*point.y - b*point.x; 20896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projectPoint->x = ( -a*lineCoeff[2] - b * delta ) * det; 20916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projectPoint->y = ( -b*lineCoeff[2] + a * delta ) * det ; 20926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 20946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 20956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 20976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Get distance from point to direction */ 20986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvGetDistanceFromPointToDirect( CvPoint2D64d point,CvVect64d lineCoef,double*dist) 20996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 21006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d tmpPoint; 21016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvProjectPointToDirect(point,lineCoef,&tmpPoint); 21026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dx = point.x - tmpPoint.x; 21036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dy = point.y - tmpPoint.y; 21046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *dist = sqrt(dx*dx+dy*dy); 21056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 21066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 21076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---------------------------------------------------------------------------------------*/ 21086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL IplImage* icvCreateIsometricImage( IplImage* src, IplImage* dst, 21106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int desired_depth, int desired_num_channels ) 21116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 21126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize src_size ; 21136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn src_size.width = src->width; 21146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn src_size.height = src->height; 21156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize dst_size = src_size; 21176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dst ) 21196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst_size.width = dst->width; 21216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst_size.height = dst->height; 21226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 21236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !dst || dst->depth != desired_depth || 21256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst->nChannels != desired_num_channels || 21266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst_size.width != src_size.width || 21276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst_size.height != dst_size.height ) 21286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseImage( &dst ); 21306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst = cvCreateImage( src_size, desired_depth, desired_num_channels ); 21316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvRect rect = cvRect(0,0,src_size.width,src_size.height); 21326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSetImageROI( dst, rect ); 21336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 21356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return dst; 21376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 21386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint 21406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvCvt_32f_64d( float *src, double *dst, int size ) 21416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 21426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int t; 21436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !src || !dst ) 21456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NULLPTR_ERR; 21466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( size <= 0 ) 21476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_BADRANGE_ERR; 21486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( t = 0; t < size; t++ ) 21506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst[t] = (double) (src[t]); 21526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 21536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; 21556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 21566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*======================================================================================*/ 21586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Type conversion double -> float */ 21596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint 21606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvCvt_64d_32f( double *src, float *dst, int size ) 21616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 21626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int t; 21636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !src || !dst ) 21656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NULLPTR_ERR; 21666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( size <= 0 ) 21676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_BADRANGE_ERR; 21686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( t = 0; t < size; t++ ) 21706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst[t] = (float) (src[t]); 21726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 21736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; 21756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 21766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*----------------------------------------------------------------------------------*/ 21786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Find line which cross frame by line(a,b,c) */ 21816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid FindLineForEpiline( CvSize imageSize, 21826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float a,float b,float c, 21836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f *start,CvPoint2D32f *end, 21846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* result) 21856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 21866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = result; 21876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f frameBeg; 21886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f frameEnd; 21906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f cross[4]; 21916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int haveCross[4]; 21926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float dist; 21936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[0] = 0; 21956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[1] = 0; 21966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[2] = 0; 21976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[3] = 0; 21986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.x = 0; 22006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.y = 0; 22016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.x = (float)(imageSize.width); 22026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.y = 0; 22036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[0] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[0]); 22046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.x = (float)(imageSize.width); 22066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.y = 0; 22076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.x = (float)(imageSize.width); 22086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.y = (float)(imageSize.height); 22096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[1] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[1]); 22106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.x = (float)(imageSize.width); 22126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.y = (float)(imageSize.height); 22136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.x = 0; 22146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.y = (float)(imageSize.height); 22156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[2] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[2]); 22166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.x = 0; 22186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.y = (float)(imageSize.height); 22196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.x = 0; 22206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.y = 0; 22216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[3] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[3]); 22226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int n; 22246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float minDist = (float)(INT_MAX); 22256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float maxDist = (float)(INT_MIN); 22266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int maxN = -1; 22286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int minN = -1; 22296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double midPointX = imageSize.width / 2.0; 22316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double midPointY = imageSize.height / 2.0; 22326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( n = 0; n < 4; n++ ) 22346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( haveCross[n] > 0 ) 22366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist = (float)((midPointX - cross[n].x)*(midPointX - cross[n].x) + 22386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (midPointY - cross[n].y)*(midPointY - cross[n].y)); 22396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dist < minDist ) 22416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minDist = dist; 22436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minN = n; 22446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dist > maxDist ) 22476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxDist = dist; 22496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxN = n; 22506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( minN >= 0 && maxN >= 0 && (minN != maxN) ) 22556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *start = cross[minN]; 22576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *end = cross[maxN]; 22586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 22606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn start->x = 0; 22626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn start->y = 0; 22636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn end->x = 0; 22646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn end->y = 0; 22656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 22686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 22706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*----------------------------------------------------------------------------------*/ 22736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint GetAngleLinee( CvPoint2D32f epipole, CvSize imageSize,CvPoint2D32f point1,CvPoint2D32f point2) 22756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 22766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float width = (float)(imageSize.width); 22776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float height = (float)(imageSize.height); 22786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Get crosslines with image corners */ 22806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find four lines */ 22826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f pa,pb,pc,pd; 22846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pa.x = 0; 22866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pa.y = 0; 22876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pb.x = width; 22896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pb.y = 0; 22906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pd.x = width; 22926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pd.y = height; 22936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pc.x = 0; 22956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pc.y = height; 22966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We can compute points for angle */ 22986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for place section */ 22996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x,y; 23016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = epipole.x; 23026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = epipole.y; 23036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( x < 0 ) 23056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 1,4,7 */ 23066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( y < 0) 23076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 1 */ 23086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point1 = pb; 23096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point2 = pc; 23106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( y > height ) 23126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 7 */ 23136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point1 = pa; 23146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point2 = pd; 23156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 23176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 4 */ 23186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point1 = pa; 23196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point2 = pc; 23206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if ( x > width ) 23236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 3,6,9 */ 23246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( y < 0 ) 23256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 3 */ 23266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point1 = pa; 23276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point2 = pd; 23286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if ( y > height ) 23306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 9 */ 23316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point1 = pc; 23326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point2 = pb; 23336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 23356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 6 */ 23366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point1 = pb; 23376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point2 = pd; 23386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 23416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 2,5,8 */ 23426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( y < 0 ) 23436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 2 */ 23446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point1 = pa; 23456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point2 = pb; 23466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( y > height ) 23486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 8 */ 23496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point1 = pc; 23506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point2 = pd; 23516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 23536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* 5 - point in the image */ 23546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return 2; 23556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return 0; 23626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 23636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*--------------------------------------------------------------------------------------*/ 23656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputePerspectiveCoeffs(const CvPoint2D32f srcQuad[4],const CvPoint2D32f dstQuad[4],double coeffs[3][3]) 23666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{/* Computes perspective coeffs for transformation from src to dst quad */ 23676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvComputePerspectiveCoeffs" ); 23706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 23726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double A[64]; 23746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double b[8]; 23756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double c[8]; 23766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f pt[4]; 23776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 23786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt[0] = srcQuad[0]; 23806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt[1] = srcQuad[1]; 23816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt[2] = srcQuad[2]; 23826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt[3] = srcQuad[3]; 23836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 4; i++ ) 23856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 23866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 0 23876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = dstQuad[i].x; 23886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double y = dstQuad[i].y; 23896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double X = pt[i].x; 23906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double Y = pt[i].y; 23916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#else 23926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = pt[i].x; 23936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double y = pt[i].y; 23946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double X = dstQuad[i].x; 23956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double Y = dstQuad[i].y; 23966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 23976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* a = A + i*16; 23986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[0] = x; 24006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[1] = y; 24016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[2] = 1; 24026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[3] = 0; 24036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[4] = 0; 24046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[5] = 0; 24056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[6] = -X*x; 24066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[7] = -X*y; 24076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a += 8; 24096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[0] = 0; 24116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[1] = 0; 24126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[2] = 0; 24136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[3] = x; 24146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[4] = y; 24156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[5] = 1; 24166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[6] = -Y*x; 24176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[7] = -Y*y; 24186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b[i*2] = X; 24206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b[i*2 + 1] = Y; 24216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 24226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 24246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double invA[64]; 24256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matA = cvMat( 8, 8, CV_64F, A ); 24266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matInvA = cvMat( 8, 8, CV_64F, invA ); 24276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matB = cvMat( 8, 1, CV_64F, b ); 24286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matX = cvMat( 8, 1, CV_64F, c ); 24296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvPseudoInverse( &matA, &matInvA )); 24316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvMatMulAdd( &matInvA, &matB, 0, &matX )); 24326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 24336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs[0][0] = c[0]; 24356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs[0][1] = c[1]; 24366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs[0][2] = c[2]; 24376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs[1][0] = c[3]; 24386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs[1][1] = c[4]; 24396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs[1][2] = c[5]; 24406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs[2][0] = c[6]; 24416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs[2][1] = c[7]; 24426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs[2][2] = 1.0; 24436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 24456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 24476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 24486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*--------------------------------------------------------------------------------------*/ 24506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void cvComputePerspectiveMap(const double c[3][3], CvArr* rectMapX, CvArr* rectMapY ) 24526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 24536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvComputePerspectiveMap" ); 24546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 24566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize size; 24586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubx, *mapx = (CvMat*)rectMapX; 24596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stuby, *mapy = (CvMat*)rectMapY; 24606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j; 24616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( mapx = cvGetMat( mapx, &stubx )); 24636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( mapy = cvGetMat( mapy, &stuby )); 24646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE( mapx->type ) != CV_32FC1 || CV_MAT_TYPE( mapy->type ) != CV_32FC1 ) 24666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "" ); 24676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn size = cvGetMatSize(mapx); 24696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( fabs(c[2][2] - 1.) < FLT_EPSILON ); 24706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < size.height; i++ ) 24726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 24736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* mx = (float*)(mapx->data.ptr + mapx->step*i); 24746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* my = (float*)(mapy->data.ptr + mapy->step*i); 24756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < size.width; j++ ) 24776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 24786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double w = 1./(c[2][0]*j + c[2][1]*i + 1.); 24796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = (c[0][0]*j + c[0][1]*i + c[0][2])*w; 24806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double y = (c[1][0]*j + c[1][1]*i + c[1][2])*w; 24816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mx[j] = (float)x; 24836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn my[j] = (float)y; 24846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 24856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 24866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 24886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 24896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*--------------------------------------------------------------------------------------*/ 24916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void cvInitPerspectiveTransform( CvSize size, const CvPoint2D32f quad[4], double matrix[3][3], 24936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvArr* rectMap ) 24946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 24956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Computes Perspective Transform coeffs and map if need 24966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for given image size and given result quad */ 24976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvInitPerspectiveTransform" ); 24986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 25006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double A[64]; 25026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double b[8]; 25036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double c[8]; 25046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f pt[4]; 25056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat mapstub, *map = (CvMat*)rectMap; 25066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j; 25076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( map ) 25096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( map = cvGetMat( map, &mapstub )); 25116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE( map->type ) != CV_32FC2 ) 25136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "" ); 25146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( map->width != size.width || map->height != size.height ) 25166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "" ); 25176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 25186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt[0] = cvPoint2D32f( 0, 0 ); 25206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt[1] = cvPoint2D32f( size.width, 0 ); 25216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt[2] = cvPoint2D32f( size.width, size.height ); 25226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt[3] = cvPoint2D32f( 0, size.height ); 25236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 4; i++ ) 25256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 0 25276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = quad[i].x; 25286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double y = quad[i].y; 25296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double X = pt[i].x; 25306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double Y = pt[i].y; 25316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#else 25326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = pt[i].x; 25336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double y = pt[i].y; 25346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double X = quad[i].x; 25356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double Y = quad[i].y; 25366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 25376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* a = A + i*16; 25386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[0] = x; 25406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[1] = y; 25416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[2] = 1; 25426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[3] = 0; 25436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[4] = 0; 25446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[5] = 0; 25456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[6] = -X*x; 25466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[7] = -X*y; 25476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a += 8; 25496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[0] = 0; 25516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[1] = 0; 25526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[2] = 0; 25536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[3] = x; 25546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[4] = y; 25556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[5] = 1; 25566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[6] = -Y*x; 25576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[7] = -Y*y; 25586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b[i*2] = X; 25606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b[i*2 + 1] = Y; 25616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 25626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double invA[64]; 25656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matA = cvMat( 8, 8, CV_64F, A ); 25666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matInvA = cvMat( 8, 8, CV_64F, invA ); 25676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matB = cvMat( 8, 1, CV_64F, b ); 25686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matX = cvMat( 8, 1, CV_64F, c ); 25696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvPseudoInverse( &matA, &matInvA )); 25716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvMatMulAdd( &matInvA, &matB, 0, &matX )); 25726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 25736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrix[0][0] = c[0]; 25756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrix[0][1] = c[1]; 25766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrix[0][2] = c[2]; 25776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrix[1][0] = c[3]; 25786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrix[1][1] = c[4]; 25796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrix[1][2] = c[5]; 25806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrix[2][0] = c[6]; 25816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrix[2][1] = c[7]; 25826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrix[2][2] = 1.0; 25836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( map ) 25856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < size.height; i++ ) 25876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f* maprow = (CvPoint2D32f*)(map->data.ptr + map->step*i); 25896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < size.width; j++ ) 25906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double w = 1./(c[6]*j + c[7]*i + 1.); 25926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = (c[0]*j + c[1]*i + c[2])*w; 25936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double y = (c[3]*j + c[4]*i + c[5])*w; 25946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maprow[j].x = (float)x; 25966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maprow[j].y = (float)y; 25976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 25986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 25996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 26026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 26046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 26056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*-----------------------------------------------------------------------*/ 26086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Compute projected infinite point for second image if first image point is known */ 26096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeeInfiniteProject1( CvMatr64d rotMatr, 26106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d camMatr1, 26116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d camMatr2, 26126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f point1, 26136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f* point2) 26146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 26156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double invMatr1[9]; 26166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvInvertMatrix_64d(camMatr1,3,invMatr1); 26176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double P1[3]; 26186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double p1[3]; 26196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p1[0] = (double)(point1.x); 26206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p1[1] = (double)(point1.y); 26216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p1[2] = 1; 26226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvMulMatrix_64d( invMatr1, 26246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3,3, 26256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p1, 26266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1,3, 26276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn P1); 26286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double invR[9]; 26306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvTransposeMatrix_64d( rotMatr, 3, 3, invR ); 26316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Change system 1 to system 2 */ 26336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double P2[3]; 26346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvMulMatrix_64d( invR, 26356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3,3, 26366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn P1, 26376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1,3, 26386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn P2); 26396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Now we can project this point to image 2 */ 26416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double projP[3]; 26426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvMulMatrix_64d( camMatr2, 26446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3,3, 26456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn P2, 26466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1,3, 26476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projP); 26486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point2->x = (float)(projP[0] / projP[2]); 26506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point2->y = (float)(projP[1] / projP[2]); 26516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 26536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 26546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*-----------------------------------------------------------------------*/ 26566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Compute projected infinite point for second image if first image point is known */ 26576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeeInfiniteProject2( CvMatr64d rotMatr, 26586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d camMatr1, 26596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr64d camMatr2, 26606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f* point1, 26616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f point2) 26626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 26636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double invMatr2[9]; 26646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvInvertMatrix_64d(camMatr2,3,invMatr2); 26656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double P2[3]; 26666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double p2[3]; 26676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p2[0] = (double)(point2.x); 26686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p2[1] = (double)(point2.y); 26696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p2[2] = 1; 26706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvMulMatrix_64d( invMatr2, 26726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3,3, 26736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p2, 26746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1,3, 26756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn P2); 26766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Change system 1 to system 2 */ 26786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double P1[3]; 26806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvMulMatrix_64d( rotMatr, 26816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3,3, 26826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn P2, 26836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1,3, 26846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn P1); 26856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Now we can project this point to image 2 */ 26876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double projP[3]; 26886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvMulMatrix_64d( camMatr1, 26906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3,3, 26916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn P1, 26926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1,3, 26936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projP); 26946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point1->x = (float)(projP[0] / projP[2]); 26966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point1->y = (float)(projP[1] / projP[2]); 26976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 26996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 27006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Select best R and t for given cameras, points, ... */ 27026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* For both cameras */ 27036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvSelectBestRt( int numImages, 27046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* numPoints, 27056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f* imagePoints1, 27066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f* imagePoints2, 27076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f* objectPoints, 27086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f cameraMatrix1, 27106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvVect32f distortion1, 27116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f rotMatrs1, 27126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvVect32f transVects1, 27136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f cameraMatrix2, 27156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvVect32f distortion2, 27166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f rotMatrs2, 27176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvVect32f transVects2, 27186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f bestRotMatr, 27206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvVect32f bestTransVect 27216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ) 27226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 27236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Need to convert input data 32 -> 64 */ 27256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d* objectPoints_64d; 27266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* rotMatrs1_64d; 27286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* rotMatrs2_64d; 27296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* transVects1_64d; 27316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* transVects2_64d; 27326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double cameraMatrix1_64d[9]; 27346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double cameraMatrix2_64d[9]; 27356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double distortion1_64d[4]; 27376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double distortion2_64d[4]; 27386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* allocate memory for 64d data */ 27406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int totalNum = 0; 27416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 27436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numImages; i++ ) 27446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 27456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn totalNum += numPoints[i]; 27466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 27476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn objectPoints_64d = (CvPoint3D64d*)calloc(totalNum,sizeof(CvPoint3D64d)); 27496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatrs1_64d = (double*)calloc(numImages,sizeof(double)*9); 27516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatrs2_64d = (double*)calloc(numImages,sizeof(double)*9); 27526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVects1_64d = (double*)calloc(numImages,sizeof(double)*3); 27546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVects2_64d = (double*)calloc(numImages,sizeof(double)*3); 27556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Convert input data to 64d */ 27576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d((float*)objectPoints, (double*)objectPoints_64d, totalNum*3); 27596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(rotMatrs1, rotMatrs1_64d, numImages*9); 27616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(rotMatrs2, rotMatrs2_64d, numImages*9); 27626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(transVects1, transVects1_64d, numImages*3); 27646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(transVects2, transVects2_64d, numImages*3); 27656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Convert to arrays */ 27676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(cameraMatrix1, cameraMatrix1_64d, 9); 27686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(cameraMatrix2, cameraMatrix2_64d, 9); 27696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(distortion1, distortion1_64d, 4); 27716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_32f_64d(distortion2, distortion2_64d, 4); 27726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* for each R and t compute error for image pair */ 27756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* errors; 27766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn errors = (float*)calloc(numImages*numImages,sizeof(float)); 27786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( errors == 0 ) 27796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 27806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OUTOFMEM_ERR; 27816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 27826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImagePair; 27846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currRt; 27856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currRt = 0; currRt < numImages; currRt++ ) 27866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 27876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int begPoint = 0; 27886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for(currImagePair = 0; currImagePair < numImages; currImagePair++ ) 27896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 27906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* For current R,t R,t compute relative position of cameras */ 27916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double convRotMatr[9]; 27936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double convTransVect[3]; 27946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCreateConvertMatrVect( rotMatrs1_64d + currRt*9, 27966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVects1_64d + currRt*3, 27976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatrs2_64d + currRt*9, 27986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVects2_64d + currRt*3, 27996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr, 28006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convTransVect); 28016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Project points using relative position of cameras */ 28036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double convRotMatr2[9]; 28056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double convTransVect2[3]; 28066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr2[0] = 1; 28086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr2[1] = 0; 28096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr2[2] = 0; 28106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr2[3] = 0; 28126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr2[4] = 1; 28136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr2[5] = 0; 28146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr2[6] = 0; 28166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr2[7] = 0; 28176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr2[8] = 1; 28186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convTransVect2[0] = 0; 28206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convTransVect2[1] = 0; 28216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convTransVect2[2] = 0; 28226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute error for given pair and Rt */ 28246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We must project points to image and compute error */ 28256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d* projImagePoints1; 28276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64d* projImagePoints2; 28286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d* points1; 28306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d* points2; 28316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numberPnt; 28336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numberPnt = numPoints[currImagePair]; 28346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projImagePoints1 = (CvPoint2D64d*)calloc(numberPnt,sizeof(CvPoint2D64d)); 28356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projImagePoints2 = (CvPoint2D64d*)calloc(numberPnt,sizeof(CvPoint2D64d)); 28366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points1 = (CvPoint3D64d*)calloc(numberPnt,sizeof(CvPoint3D64d)); 28386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points2 = (CvPoint3D64d*)calloc(numberPnt,sizeof(CvPoint3D64d)); 28396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Transform object points to first camera position */ 28416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 28426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numberPnt; i++ ) 28436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 28446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create second camera point */ 28456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d tmpPoint; 28466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmpPoint.x = (double)(objectPoints[i].x); 28476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmpPoint.y = (double)(objectPoints[i].y); 28486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmpPoint.z = (double)(objectPoints[i].z); 28496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvConvertPointSystem( tmpPoint, 28516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points2+i, 28526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatrs2_64d + currImagePair*9, 28536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVects2_64d + currImagePair*3); 28546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create first camera point using R, t */ 28566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvConvertPointSystem( points2[i], 28576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points1+i, 28586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr, 28596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convTransVect); 28606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64d tmpPoint2 = { 0, 0, 0 }; 28626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvConvertPointSystem( tmpPoint, 28636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &tmpPoint2, 28646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatrs1_64d + currImagePair*9, 28656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVects1_64d + currImagePair*3); 28666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double err; 28676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dx,dy,dz; 28686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dx = tmpPoint2.x - points1[i].x; 28696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dy = tmpPoint2.y - points1[i].y; 28706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dz = tmpPoint2.z - points1[i].z; 28716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err = sqrt(dx*dx + dy*dy + dz*dz); 28726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 28756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 0 28776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvProjectPointsSimple( numPoints[currImagePair], 28786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn objectPoints_64d + begPoint, 28796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatrs1_64d + currRt*9, 28806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVects1_64d + currRt*3, 28816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cameraMatrix1_64d, 28826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn distortion1_64d, 28836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projImagePoints1); 28846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvProjectPointsSimple( numPoints[currImagePair], 28866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn objectPoints_64d + begPoint, 28876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatrs2_64d + currRt*9, 28886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVects2_64d + currRt*3, 28896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cameraMatrix2_64d, 28906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn distortion2_64d, 28916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projImagePoints2); 28926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 28936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Project with no translate and no rotation */ 28956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 28966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 0 28976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 28986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double nodist[4] = {0,0,0,0}; 28996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvProjectPointsSimple( numPoints[currImagePair], 29006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points1, 29016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr2, 29026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convTransVect2, 29036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cameraMatrix1_64d, 29046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nodist, 29056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projImagePoints1); 29066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 29076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvProjectPointsSimple( numPoints[currImagePair], 29086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points2, 29096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr2, 29106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convTransVect2, 29116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cameraMatrix2_64d, 29126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nodist, 29136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projImagePoints2); 29146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 29156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 29166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 29176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 29186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvProjectPointsSimple( numPoints[currImagePair], 29196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points1, 29206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr2, 29216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convTransVect2, 29226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cameraMatrix1_64d, 29236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn distortion1_64d, 29246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projImagePoints1); 29256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 29266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvProjectPointsSimple( numPoints[currImagePair], 29276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points2, 29286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convRotMatr2, 29296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convTransVect2, 29306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cameraMatrix2_64d, 29316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn distortion2_64d, 29326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projImagePoints2); 29336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 29346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* points are projected. Compute error */ 29356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 29366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currPoint; 29376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double err1 = 0; 29386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double err2 = 0; 29396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double err; 29406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currPoint = 0; currPoint < numberPnt; currPoint++ ) 29416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 29426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double len1,len2; 29436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dx1,dy1; 29446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dx1 = imagePoints1[begPoint+currPoint].x - projImagePoints1[currPoint].x; 29456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dy1 = imagePoints1[begPoint+currPoint].y - projImagePoints1[currPoint].y; 29466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn len1 = sqrt(dx1*dx1 + dy1*dy1); 29476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err1 += len1; 29486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 29496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dx2,dy2; 29506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dx2 = imagePoints2[begPoint+currPoint].x - projImagePoints2[currPoint].x; 29516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dy2 = imagePoints2[begPoint+currPoint].y - projImagePoints2[currPoint].y; 29526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn len2 = sqrt(dx2*dx2 + dy2*dy2); 29536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err2 += len2; 29546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 29556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 29566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err1 /= (float)(numberPnt); 29576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err2 /= (float)(numberPnt); 29586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 29596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err = (err1+err2) * 0.5; 29606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn begPoint += numberPnt; 29616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 29626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Set this error to */ 29636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn errors[numImages*currImagePair+currRt] = (float)err; 29646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 29656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn free(points1); 29666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn free(points2); 29676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn free(projImagePoints1); 29686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn free(projImagePoints2); 29696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 29706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 29716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 29726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Just select R and t with minimal average error */ 29736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 29746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int bestnumRt = 0; 29756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float minError = 0;/* Just for no warnings. Uses 'first' flag. */ 29766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int first = 1; 29776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currRt = 0; currRt < numImages; currRt++ ) 29786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 29796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float avErr = 0; 29806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for(currImagePair = 0; currImagePair < numImages; currImagePair++ ) 29816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 29826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn avErr += errors[numImages*currImagePair+currRt]; 29836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 29846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn avErr /= (float)(numImages); 29856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 29866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( first ) 29876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 29886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bestnumRt = 0; 29896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minError = avErr; 29906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn first = 0; 29916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 29926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 29936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 29946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( avErr < minError ) 29956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 29966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bestnumRt = currRt; 29976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minError = avErr; 29986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 29996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 30006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 30026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double bestRotMatr_64d[9]; 30046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double bestTransVect_64d[3]; 30056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCreateConvertMatrVect( rotMatrs1_64d + bestnumRt * 9, 30076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVects1_64d + bestnumRt * 3, 30086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatrs2_64d + bestnumRt * 9, 30096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVects2_64d + bestnumRt * 3, 30106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bestRotMatr_64d, 30116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bestTransVect_64d); 30126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_64d_32f(bestRotMatr_64d,bestRotMatr,9); 30146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCvt_64d_32f(bestTransVect_64d,bestTransVect,3); 30156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn free(errors); 30186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; 30206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 30216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* ----------------- Stereo calibration functions --------------------- */ 30246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennfloat icvDefinePointPosition(CvPoint2D32f point1,CvPoint2D32f point2,CvPoint2D32f point) 30266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 30276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float ax = point2.x - point1.x; 30286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float ay = point2.y - point1.y; 30296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float bx = point.x - point1.x; 30316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float by = point.y - point1.y; 30326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return (ax*by - ay*bx); 30346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 30356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Convert function for stereo warping */ 30376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvConvertWarpCoordinates(double coeffs[3][3], 30386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f* cameraPoint, 30396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f* warpPoint, 30406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int direction) 30416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 30426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x,y; 30436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double det; 30446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( direction == CV_WARP_TO_CAMERA ) 30456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* convert from camera image to warped image coordinates */ 30466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = warpPoint->x; 30476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = warpPoint->y; 30486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn det = (coeffs[2][0] * x + coeffs[2][1] * y + coeffs[2][2]); 30506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(det) > 1e-8 ) 30516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 30526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cameraPoint->x = (float)((coeffs[0][0] * x + coeffs[0][1] * y + coeffs[0][2]) / det); 30536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cameraPoint->y = (float)((coeffs[1][0] * x + coeffs[1][1] * y + coeffs[1][2]) / det); 30546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; 30556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 30566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 30576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( direction == CV_CAMERA_TO_WARP ) 30586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* convert from warped image to camera image coordinates */ 30596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = cameraPoint->x; 30606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = cameraPoint->y; 30616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 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]); 30636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(det) > 1e-8 ) 30656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 30666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 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); 30676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 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); 30686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; 30696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 30706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 30716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_BADFACTOR_ERR; 30736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 30746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Compute stereo params using some camera params */ 30766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* by Valery Mosyagin. int ComputeRestStereoParams(StereoParams *stereoparams) */ 30776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvComputeRestStereoParams(CvStereoCamera *stereoparams) 30786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 30796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetQuadsTransformStruct(stereoparams); 30826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvInitPerspectiveTransform( stereoparams->warpSize, 30846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->quad[0], 30856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->coeffs[0], 30866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0); 30876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvInitPerspectiveTransform( stereoparams->warpSize, 30896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->quad[1], 30906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->coeffs[1], 30916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0); 30926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create border for warped images */ 30946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f corns[4]; 30956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corns[0].x = 0; 30966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corns[0].y = 0; 30976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 30986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corns[1].x = (float)(stereoparams->camera[0]->imgSize[0]-1); 30996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corns[1].y = 0; 31006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corns[2].x = (float)(stereoparams->camera[0]->imgSize[0]-1); 31026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corns[2].y = (float)(stereoparams->camera[0]->imgSize[1]-1); 31036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corns[3].x = 0; 31056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corns[3].y = (float)(stereoparams->camera[0]->imgSize[1]-1); 31066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 31086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 4; i++ ) 31096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 31106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* For first camera */ 31116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvConvertWarpCoordinates( stereoparams->coeffs[0], 31126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corns+i, 31136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->border[0]+i, 31146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CAMERA_TO_WARP); 31156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* For second camera */ 31176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvConvertWarpCoordinates( stereoparams->coeffs[1], 31186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corns+i, 31196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->border[1]+i, 31206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CAMERA_TO_WARP); 31216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 31226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test compute */ 31246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 31256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f warpPoints[4]; 31266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn warpPoints[0] = cvPoint2D32f(0,0); 31276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn warpPoints[1] = cvPoint2D32f(stereoparams->warpSize.width-1,0); 31286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn warpPoints[2] = cvPoint2D32f(stereoparams->warpSize.width-1,stereoparams->warpSize.height-1); 31296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn warpPoints[3] = cvPoint2D32f(0,stereoparams->warpSize.height-1); 31306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f camPoints1[4]; 31326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f camPoints2[4]; 31336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int i = 0; i < 4; i++ ) 31356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 31366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvConvertWarpCoordinates(stereoparams->coeffs[0], 31376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camPoints1+i, 31386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn warpPoints+i, 31396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_WARP_TO_CAMERA); 31406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvConvertWarpCoordinates(stereoparams->coeffs[1], 31426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn camPoints2+i, 31436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn warpPoints+i, 31446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_WARP_TO_CAMERA); 31456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 31466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 31476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Allocate memory for scanlines coeffs */ 31506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->lineCoeffs = (CvStereoLineCoeff*)calloc(stereoparams->warpSize.height,sizeof(CvStereoLineCoeff)); 31526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute coeffs for epilines */ 31546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeCoeffForStereo( stereoparams); 31566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* all coeffs are known */ 31586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; 31596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 31606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*-------------------------------------------------------------------------------------------*/ 31626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvStereoCalibration( int numImages, 31646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* nums, 31656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize imageSize, 31666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f* imagePoints1, 31676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f* imagePoints2, 31686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f* objectPoints, 31696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvStereoCamera* stereoparams 31706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ) 31716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 31726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Firstly we must calibrate both cameras */ 31736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Alocate memory for data */ 31746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Allocate for translate vectors */ 31756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* transVects1; 31766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* transVects2; 31776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* rotMatrs1; 31786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* rotMatrs2; 31796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVects1 = (float*)calloc(numImages,sizeof(float)*3); 31816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVects2 = (float*)calloc(numImages,sizeof(float)*3); 31826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatrs1 = (float*)calloc(numImages,sizeof(float)*9); 31846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatrs2 = (float*)calloc(numImages,sizeof(float)*9); 31856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Calibrate first camera */ 31876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCalibrateCamera( numImages, 31886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nums, 31896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imageSize, 31906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imagePoints1, 31916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn objectPoints, 31926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->camera[0]->distortion, 31936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->camera[0]->matrix, 31946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVects1, 31956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatrs1, 31966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1); 31976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 31986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Calibrate second camera */ 31996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCalibrateCamera( numImages, 32006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nums, 32016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imageSize, 32026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imagePoints2, 32036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn objectPoints, 32046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->camera[1]->distortion, 32056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->camera[1]->matrix, 32066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVects2, 32076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatrs2, 32086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1); 32096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 32106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Cameras are calibrated */ 32116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 32126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->camera[0]->imgSize[0] = (float)imageSize.width; 32136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->camera[0]->imgSize[1] = (float)imageSize.height; 32146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 32156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->camera[1]->imgSize[0] = (float)imageSize.width; 32166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->camera[1]->imgSize[1] = (float)imageSize.height; 32176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 32186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvSelectBestRt( numImages, 32196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nums, 32206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imagePoints1, 32216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imagePoints2, 32226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn objectPoints, 32236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->camera[0]->matrix, 32246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->camera[0]->distortion, 32256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatrs1, 32266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVects1, 32276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->camera[1]->matrix, 32286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->camera[1]->distortion, 32296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rotMatrs2, 32306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transVects2, 32316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->rotMatrix, 32326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stereoparams->transVector 32336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ); 32346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 32356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Free memory */ 32366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn free(transVects1); 32376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn free(transVects2); 32386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn free(rotMatrs1); 32396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn free(rotMatrs2); 32406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 32416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeRestStereoParams(stereoparams); 32426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 32436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NO_ERR; 32446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 32456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 32466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Find line from epipole */ 32476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid FindLine(CvPoint2D32f epipole,CvSize imageSize,CvPoint2D32f point,CvPoint2D32f *start,CvPoint2D32f *end) 32486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 32496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f frameBeg; 32506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f frameEnd; 32516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f cross[4]; 32526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int haveCross[4]; 32536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float dist; 32546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 32556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[0] = 0; 32566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[1] = 0; 32576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[2] = 0; 32586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[3] = 0; 32596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 32606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.x = 0; 32616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.y = 0; 32626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.x = (float)(imageSize.width); 32636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.y = 0; 32646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[0] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[0]); 32656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 32666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.x = (float)(imageSize.width); 32676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.y = 0; 32686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.x = (float)(imageSize.width); 32696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.y = (float)(imageSize.height); 32706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[1] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[1]); 32716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 32726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.x = (float)(imageSize.width); 32736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.y = (float)(imageSize.height); 32746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.x = 0; 32756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.y = (float)(imageSize.height); 32766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[2] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[2]); 32776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 32786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.x = 0; 32796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.y = (float)(imageSize.height); 32806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.x = 0; 32816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.y = 0; 32826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[3] = icvGetCrossPieceVector(frameBeg,frameEnd,epipole,point,&cross[3]); 32836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 32846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int n; 32856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float minDist = (float)(INT_MAX); 32866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float maxDist = (float)(INT_MIN); 32876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 32886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int maxN = -1; 32896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int minN = -1; 32906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 32916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( n = 0; n < 4; n++ ) 32926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 32936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( haveCross[n] > 0 ) 32946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 32956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist = (epipole.x - cross[n].x)*(epipole.x - cross[n].x) + 32966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (epipole.y - cross[n].y)*(epipole.y - cross[n].y); 32976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 32986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dist < minDist ) 32996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 33006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minDist = dist; 33016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minN = n; 33026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 33036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 33046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dist > maxDist ) 33056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 33066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxDist = dist; 33076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxN = n; 33086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 33096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 33106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 33116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 33126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( minN >= 0 && maxN >= 0 && (minN != maxN) ) 33136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 33146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *start = cross[minN]; 33156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *end = cross[maxN]; 33166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 33176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 33186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 33196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn start->x = 0; 33206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn start->y = 0; 33216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn end->x = 0; 33226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn end->y = 0; 33236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 33246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 33256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 33266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 33276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 33286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 33296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Find line which cross frame by line(a,b,c) */ 33306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid FindLineForEpiline(CvSize imageSize,float a,float b,float c,CvPoint2D32f *start,CvPoint2D32f *end) 33316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 33326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f frameBeg; 33336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f frameEnd; 33346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f cross[4]; 33356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int haveCross[4]; 33366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float dist; 33376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 33386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[0] = 0; 33396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[1] = 0; 33406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[2] = 0; 33416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[3] = 0; 33426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 33436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.x = 0; 33446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.y = 0; 33456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.x = (float)(imageSize.width); 33466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.y = 0; 33476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[0] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[0]); 33486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 33496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.x = (float)(imageSize.width); 33506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.y = 0; 33516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.x = (float)(imageSize.width); 33526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.y = (float)(imageSize.height); 33536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[1] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[1]); 33546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 33556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.x = (float)(imageSize.width); 33566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.y = (float)(imageSize.height); 33576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.x = 0; 33586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.y = (float)(imageSize.height); 33596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[2] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[2]); 33606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 33616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.x = 0; 33626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameBeg.y = (float)(imageSize.height); 33636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.x = 0; 33646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn frameEnd.y = 0; 33656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveCross[3] = icvGetCrossLineDirect(frameBeg,frameEnd,a,b,c,&cross[3]); 33666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 33676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int n; 33686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float minDist = (float)(INT_MAX); 33696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float maxDist = (float)(INT_MIN); 33706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 33716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int maxN = -1; 33726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int minN = -1; 33736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 33746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double midPointX = imageSize.width / 2.0; 33756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double midPointY = imageSize.height / 2.0; 33766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 33776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( n = 0; n < 4; n++ ) 33786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 33796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( haveCross[n] > 0 ) 33806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 33816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist = (float)((midPointX - cross[n].x)*(midPointX - cross[n].x) + 33826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (midPointY - cross[n].y)*(midPointY - cross[n].y)); 33836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 33846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dist < minDist ) 33856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 33866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minDist = dist; 33876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minN = n; 33886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 33896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 33906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dist > maxDist ) 33916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 33926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxDist = dist; 33936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxN = n; 33946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 33956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 33966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 33976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 33986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( minN >= 0 && maxN >= 0 && (minN != maxN) ) 33996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 34006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *start = cross[minN]; 34016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *end = cross[maxN]; 34026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 34036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 34046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 34056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn start->x = 0; 34066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn start->y = 0; 34076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn end->x = 0; 34086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn end->y = 0; 34096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 34106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 34126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 34146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Cross lines */ 34166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint GetCrossLines(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f p2_start,CvPoint2D32f p2_end,CvPoint2D32f *cross) 34176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 34186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double ex1,ey1,ex2,ey2; 34196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double px1,py1,px2,py2; 34206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double del; 34216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double delA,delB,delX,delY; 34226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double alpha,betta; 34236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ex1 = p1_start.x; 34256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ey1 = p1_start.y; 34266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ex2 = p1_end.x; 34276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ey2 = p1_end.y; 34286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn px1 = p2_start.x; 34306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn py1 = p2_start.y; 34316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn px2 = p2_end.x; 34326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn py2 = p2_end.y; 34336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn del = (ex1-ex2)*(py2-py1)+(ey2-ey1)*(px2-px1); 34356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( del == 0) 34366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 34376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return -1; 34386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 34396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delA = (px1-ex1)*(py1-py2) + (ey1-py1)*(px1-px2); 34416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delB = (ex1-px1)*(ey1-ey2) + (py1-ey1)*(ex1-ex2); 34426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn alpha = delA / del; 34446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn betta = -delB / del; 34456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( alpha < 0 || alpha > 1.0 || betta < 0 || betta > 1.0) 34476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 34486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return -1; 34496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 34506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delX = (ex1-ex2)*(py1*(px1-px2)-px1*(py1-py2))+ 34526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (px1-px2)*(ex1*(ey1-ey2)-ey1*(ex1-ex2)); 34536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delY = (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2))+ 34556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2)); 34566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cross->x = (float)( delX / del); 34586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cross->y = (float)(-delY / del); 34596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return 1; 34606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 34616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvGetCrossPieceVector(CvPoint2D32f p1_start,CvPoint2D32f p1_end,CvPoint2D32f v2_start,CvPoint2D32f v2_end,CvPoint2D32f *cross) 34646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 34656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double ex1,ey1,ex2,ey2; 34666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double px1,py1,px2,py2; 34676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double del; 34686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double delA,delB,delX,delY; 34696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double alpha,betta; 34706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ex1 = p1_start.x; 34726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ey1 = p1_start.y; 34736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ex2 = p1_end.x; 34746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ey2 = p1_end.y; 34756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn px1 = v2_start.x; 34776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn py1 = v2_start.y; 34786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn px2 = v2_end.x; 34796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn py2 = v2_end.y; 34806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn del = (ex1-ex2)*(py2-py1)+(ey2-ey1)*(px2-px1); 34826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( del == 0) 34836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 34846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return -1; 34856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 34866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delA = (px1-ex1)*(py1-py2) + (ey1-py1)*(px1-px2); 34886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delB = (ex1-px1)*(ey1-ey2) + (py1-ey1)*(ex1-ex2); 34896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn alpha = delA / del; 34916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn betta = -delB / del; 34926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( alpha < 0 || alpha > 1.0 ) 34946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 34956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return -1; 34966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 34976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 34986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delX = (ex1-ex2)*(py1*(px1-px2)-px1*(py1-py2))+ 34996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (px1-px2)*(ex1*(ey1-ey2)-ey1*(ex1-ex2)); 35006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delY = (ey1-ey2)*(px1*(py1-py2)-py1*(px1-px2))+ 35026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (py1-py2)*(ey1*(ex1-ex2)-ex1*(ey1-ey2)); 35036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cross->x = (float)( delX / del); 35056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cross->y = (float)(-delY / del); 35066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return 1; 35076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 35086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvGetCrossLineDirect(CvPoint2D32f p1,CvPoint2D32f p2,float a,float b,float c,CvPoint2D32f* cross) 35116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 35126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double del; 35136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double delX,delY,delA; 35146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double px1,px2,py1,py2; 35166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double X,Y,alpha; 35176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn px1 = p1.x; 35196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn py1 = p1.y; 35206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn px2 = p2.x; 35226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn py2 = p2.y; 35236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn del = a * (px2 - px1) + b * (py2-py1); 35256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( del == 0 ) 35266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 35276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return -1; 35286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 35296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delA = - c - a*px1 - b*py1; 35316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn alpha = delA / del; 35326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( alpha < 0 || alpha > 1.0 ) 35346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 35356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return -1;/* no cross */ 35366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 35376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delX = b * (py1*(px1-px2) - px1*(py1-py2)) + c * (px1-px2); 35396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn delY = a * (px1*(py1-py2) - py1*(px1-px2)) + c * (py1-py2); 35406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X = delX / del; 35426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Y = delY / del; 35436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cross->x = (float)X; 35456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cross->y = (float)Y; 35466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return 1; 35486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 35496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint cvComputeEpipoles( CvMatr32f camMatr1, CvMatr32f camMatr2, 35516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f rotMatr1, CvMatr32f rotMatr2, 35526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvVect32f transVect1,CvVect32f transVect2, 35536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvVect32f epipole1, 35546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvVect32f epipole2) 35556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 35566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy matrix */ 35586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat ccamMatr1 = cvMat(3,3,CV_MAT32F,camMatr1); 35606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat ccamMatr2 = cvMat(3,3,CV_MAT32F,camMatr2); 35616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat crotMatr1 = cvMat(3,3,CV_MAT32F,rotMatr1); 35626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat crotMatr2 = cvMat(3,3,CV_MAT32F,rotMatr2); 35636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat ctransVect1 = cvMat(3,1,CV_MAT32F,transVect1); 35646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat ctransVect2 = cvMat(3,1,CV_MAT32F,transVect2); 35656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat cepipole1 = cvMat(3,1,CV_MAT32F,epipole1); 35666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat cepipole2 = cvMat(3,1,CV_MAT32F,epipole2); 35676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat cmatrP1 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrP1); 35706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat cmatrP2 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrP2); 35716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat cvectp1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&cvectp1); 35726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat cvectp2 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&cvectp2); 35736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat ctmpF1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpF1); 35746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat ctmpM1 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpM1); 35756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat ctmpM2 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpM2); 35766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat cinvP1 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cinvP1); 35776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat cinvP2 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cinvP2); 35786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat ctmpMatr = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpMatr); 35796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat ctmpVect1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpVect1); 35806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat ctmpVect2 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpVect2); 35816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat cmatrF1 = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&cmatrF1); 35826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat ctmpF = cvMat(3,3,CV_MAT32F,0); cvmAlloc(&ctmpF); 35836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat ctmpE1 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpE1); 35846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat ctmpE2 = cvMat(3,1,CV_MAT32F,0); cvmAlloc(&ctmpE2); 35856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute first */ 35876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul( &ccamMatr1, &crotMatr1, &cmatrP1); 35886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmInvert( &cmatrP1,&cinvP1 ); 35896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul( &ccamMatr1, &ctransVect1, &cvectp1 ); 35906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute second */ 35926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul( &ccamMatr2, &crotMatr2, &cmatrP2 ); 35936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmInvert( &cmatrP2,&cinvP2 ); 35946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul( &ccamMatr2, &ctransVect2, &cvectp2 ); 35956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 35966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul( &cmatrP1, &cinvP2, &ctmpM1); 35976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul( &ctmpM1, &cvectp2, &ctmpVect1); 35986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSub( &cvectp1,&ctmpVect1,&ctmpE1); 35996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul( &cmatrP2, &cinvP1, &ctmpM2); 36016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul( &ctmpM2, &cvectp1, &ctmpVect2); 36026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSub( &cvectp2, &ctmpVect2, &ctmpE2); 36036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Need scale */ 36066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmScale(&ctmpE1,&cepipole1,1.0); 36086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmScale(&ctmpE2,&cepipole2,1.0); 36096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmFree(&cmatrP1); 36116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmFree(&cmatrP1); 36126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmFree(&cvectp1); 36136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmFree(&cvectp2); 36146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmFree(&ctmpF1); 36156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmFree(&ctmpM1); 36166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmFree(&ctmpM2); 36176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmFree(&cinvP1); 36186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmFree(&cinvP2); 36196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmFree(&ctmpMatr); 36206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmFree(&ctmpVect1); 36216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmFree(&ctmpVect2); 36226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmFree(&cmatrF1); 36236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmFree(&ctmpF); 36246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmFree(&ctmpE1); 36256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmFree(&ctmpE2); 36266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NO_ERR; 36286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn}/* cvComputeEpipoles */ 36296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Compute epipoles for fundamental matrix */ 36326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint cvComputeEpipolesFromFundMatrix(CvMatr32f fundMatr, 36336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f* epipole1, 36346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f* epipole2) 36356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 36366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Decompose fundamental matrix using SVD ( A = U W V') */ 36376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat fundMatrC = cvMat(3,3,CV_MAT32F,fundMatr); 36386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* matrW = cvCreateMat(3,3,CV_MAT32F); 36406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* matrU = cvCreateMat(3,3,CV_MAT32F); 36416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* matrV = cvCreateMat(3,3,CV_MAT32F); 36426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* From svd we need just last vector of U and V or last row from U' and V' */ 36446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We get transposed matrixes U and V */ 36456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD(&fundMatrC,matrW,matrU,matrV,CV_SVD_V_T|CV_SVD_U_T); 36466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Get last row from U' and compute epipole1 */ 36486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole1->x = matrU->data.fl[6]; 36496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole1->y = matrU->data.fl[7]; 36506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole1->z = matrU->data.fl[8]; 36516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Get last row from V' and compute epipole2 */ 36536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole2->x = matrV->data.fl[6]; 36546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole2->y = matrV->data.fl[7]; 36556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn epipole2->z = matrV->data.fl[8]; 36566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&matrW); 36586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&matrU); 36596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&matrV); 36606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; 36616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 36626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint cvConvertEssential2Fundamental( CvMatr32f essMatr, 36646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f fundMatr, 36656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f cameraMatr1, 36666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f cameraMatr2) 36676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{/* Fund = inv(CM1') * Ess * inv(CM2) */ 36686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat essMatrC = cvMat(3,3,CV_MAT32F,essMatr); 36706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat fundMatrC = cvMat(3,3,CV_MAT32F,fundMatr); 36716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat cameraMatr1C = cvMat(3,3,CV_MAT32F,cameraMatr1); 36726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat cameraMatr2C = cvMat(3,3,CV_MAT32F,cameraMatr2); 36736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* invCM2 = cvCreateMat(3,3,CV_MAT32F); 36756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* tmpMatr = cvCreateMat(3,3,CV_MAT32F); 36766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* invCM1T = cvCreateMat(3,3,CV_MAT32F); 36776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvTranspose(&cameraMatr1C,tmpMatr); 36796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvInvert(tmpMatr,invCM1T); 36806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul(invCM1T,&essMatrC,tmpMatr); 36816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvInvert(&cameraMatr2C,invCM2); 36826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul(tmpMatr,invCM2,&fundMatrC); 36836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Scale fundamental matrix */ 36856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double scale; 36866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale = 1.0/fundMatrC.data.fl[8]; 36876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvertScale(&fundMatrC,&fundMatrC,scale); 36886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&invCM2); 36906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&tmpMatr); 36916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&invCM1T); 36926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; 36946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 36956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Compute essential matrix */ 36976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 36986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint cvComputeEssentialMatrix( CvMatr32f rotMatr, 36996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f transVect, 37006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMatr32f essMatr) 37016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 37026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float transMatr[9]; 37036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 37046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Make antisymmetric matrix from transpose vector */ 37056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transMatr[0] = 0; 37066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transMatr[1] = - transVect[2]; 37076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transMatr[2] = transVect[1]; 37086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 37096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transMatr[3] = transVect[2]; 37106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transMatr[4] = 0; 37116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transMatr[5] = - transVect[0]; 37126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 37136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transMatr[6] = - transVect[1]; 37146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transMatr[7] = transVect[0]; 37156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transMatr[8] = 0; 37166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 37176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvMulMatrix_32f(transMatr,3,3,rotMatr,3,3,essMatr); 37186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 37196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; 37206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 37216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 37226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3723