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#include "_cvaux.h" 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//#include "cvtypes.h" 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <float.h> 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <limits.h> 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//#include "cv.h" 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//#include "windows.h" 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <stdio.h> 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Valery Mosyagin */ 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Function defenitions */ 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* ----------------- */ 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid cvOptimizeLevenbergMarquardtBundle( CvMat** projMatrs, CvMat** observProjPoints, 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat** pointsPres, int numImages, 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat** resultProjMatrs, CvMat* resultPoints4D,int maxIter,double epsilon ); 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvComputeProjectMatrices6Points( CvMat* points1,CvMat* points2,CvMat* points3, 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3); 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvFindBaseTransform(CvMat* points,CvMat* resultT); 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid GetGeneratorReduceFundSolution(CvMat* points1,CvMat* points2,CvMat* fundReduceCoef1,CvMat* fundReduceCoef2); 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint GetGoodReduceFundamMatrFromTwo(CvMat* fundReduceCoef1,CvMat* fundReduceCoef2,CvMat* resFundReduceCoef); 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid GetProjMatrFromReducedFundamental(CvMat* fundReduceCoefs,CvMat* projMatrCoefs); 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeProjectMatrix(CvMat* objPoints,CvMat* projPoints,CvMat* projMatr); 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeTransform4D(CvMat* points1,CvMat* points2,CvMat* transMatr); 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvComputeProjectMatricesNPoints( CvMat* points1,CvMat* points2,CvMat* points3, 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3, 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double threshold,/* Threshold for good point */ 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double p,/* Probability of good result. */ 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* status, 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* points4D); 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvComputeProjectMatricesNPoints( CvMat* points1,CvMat* points2,CvMat* points3, 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3, 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double threshold,/* Threshold for good point */ 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double p,/* Probability of good result. */ 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* status, 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* points4D); 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvReconstructPointsFor3View( CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3, 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projPoints1,CvMat* projPoints2,CvMat* projPoints3, 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* points4D); 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvReconstructPointsFor3View( CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3, 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projPoints1,CvMat* projPoints2,CvMat* projPoints3, 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* points4D); 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Functions for calculation the tensor */ 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 1 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid fprintMatrix(FILE* file,CvMat* matrix) 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i,j; 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n"); 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i=0;i<matrix->rows;i++ ) 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for(j=0;j<matrix->cols;j++) 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"%10.7lf ",cvmGet(matrix,i,j)); 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n"); 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvNormalizePoints( CvMat* points, CvMat* normPoints,CvMat* cameraMatr ) 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Normalize image points using camera matrix */ 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvNormalizePoints" ); 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for null pointers */ 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points == 0 || normPoints == 0 || cameraMatr == 0 ) 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(points) || !CV_IS_MAT(normPoints) || !CV_IS_MAT(cameraMatr) ) 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = points->cols; 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints <= 0 || numPoints != normPoints->cols ) 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be the same and more than 0" ); 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( normPoints->rows != 2 || normPoints->rows != points->rows ) 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Points must have 2 coordinates" ); 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if(cameraMatr->rows != 3 || cameraMatr->cols != 3) 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of camera matrix must be 3x3" ); 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double fx,fy,cx,cy; 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fx = cvmGet(cameraMatr,0,0); 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fy = cvmGet(cameraMatr,1,1); 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cx = cvmGet(cameraMatr,0,2); 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cy = cvmGet(cameraMatr,1,2); 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(normPoints, 0, i, (cvmGet(points,0,i) - cx) / fx ); 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(normPoints, 1, i, (cvmGet(points,1,i) - cy) / fy ); 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*=====================================================================================*/ 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennComputes projection matrices for given 6 points on 3 images 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennMay returns 3 results. */ 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvComputeProjectMatrices6Points( CvMat* points1,CvMat* points2,CvMat* points3, 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3/*, 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* points4D*/) 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test input data correctness */ 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numSol = 0; 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvComputeProjectMatrices6Points" ); 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for null pointers */ 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points1 == 0 || points2 == 0 || points3 == 0 || 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatr1 == 0 || projMatr2 == 0 || projMatr3 == 0 ) 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) || !CV_IS_MAT(points3) || 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) || !CV_IS_MAT(projMatr3) ) 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (points1->cols != points2->cols) || (points1->cols != points3->cols) || (points1->cols != 6) /* || (points4D->cols !=6) */) 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be same and == 6" ); 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points1->rows != 2 || points2->rows != 2 || points3->rows != 2 ) 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of points coordinates must be 2" ); 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatr1->cols != 4 || projMatr2->cols != 4 || projMatr3->cols != 4 || 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !(projMatr1->rows == 3 && projMatr2->rows == 3 && projMatr3->rows == 3) && 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !(projMatr1->rows == 9 && projMatr2->rows == 9 && projMatr3->rows == 9) ) 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of project matrix must be 3x4 or 9x4 (for 3 matrices)" ); 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 0 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points4D->row != 4 ) 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of points4D must be 4" ); 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find transform matrix for each camera */ 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* points[3]; 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points[0] = points1; 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points[1] = points2; 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points[2] = points3; 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projMatrs[3]; 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrs[0] = projMatr1; 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrs[1] = projMatr2; 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrs[2] = projMatr3; 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat transMatr; 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double transMatr_dat[9]; 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn transMatr = cvMat(3,3,CV_64F,transMatr_dat); 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat corrPoints1; 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat corrPoints2; 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double corrPoints_dat[3*3*2];/* 3-point(images) by 3-coordinates by 2-correspondence*/ 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corrPoints1 = cvMat(3,3,CV_64F,corrPoints_dat); /* 3-coordinates for each of 3-points(3-image) */ 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corrPoints2 = cvMat(3,3,CV_64F,corrPoints_dat+9);/* 3-coordinates for each of 3-points(3-image) */ 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ )/* for each image */ 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Get last 4 points for computing transformation */ 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat tmpPoints; 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* find base points transform for last four points on i-th image */ 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect(points[i],&tmpPoints,cvRect(2,0,4,2)); 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFindBaseTransform(&tmpPoints,&transMatr); 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* We have base transform. Compute error scales for three first points */ 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat trPoint; 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double trPoint_dat[3*3]; 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn trPoint = cvMat(3,3,CV_64F,trPoint_dat); 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* fill points */ 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int kk = 0; kk < 3; kk++ ) 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&trPoint,0,kk,cvmGet(points[i],0,kk+2)); 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&trPoint,1,kk,cvmGet(points[i],1,kk+2)); 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&trPoint,2,kk,1); 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Transform points */ 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat resPnts; 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double resPnts_dat[9]; 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resPnts = cvMat(3,3,CV_64F,resPnts_dat); 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul(&transMatr,&trPoint,&resPnts); 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Transform two first points */ 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int j = 0; j < 2; j++ ) 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat pnt; 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double pnt_dat[3]; 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pnt = cvMat(3,1,CV_64F,pnt_dat); 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pnt_dat[0] = cvmGet(points[i],0,j); 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pnt_dat[1] = cvmGet(points[i],1,j); 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pnt_dat[2] = 1.0; 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat trPnt; 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double trPnt_dat[3]; 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn trPnt = cvMat(3,1,CV_64F,trPnt_dat); 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul(&transMatr,&pnt,&trPnt); 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Collect transformed points */ 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corrPoints_dat[j * 9 + 0 * 3 + i] = trPnt_dat[0];/* x */ 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corrPoints_dat[j * 9 + 1 * 3 + i] = trPnt_dat[1];/* y */ 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corrPoints_dat[j * 9 + 2 * 3 + i] = trPnt_dat[2];/* w */ 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We have computed corr points. Now we can compute generators for reduced fundamental matrix */ 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute generators for reduced fundamental matrix from 3 pair of collect points */ 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat fundReduceCoef1; 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat fundReduceCoef2; 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double fundReduceCoef1_dat[5]; 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double fundReduceCoef2_dat[5]; 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fundReduceCoef1 = cvMat(1,5,CV_64F,fundReduceCoef1_dat); 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fundReduceCoef2 = cvMat(1,5,CV_64F,fundReduceCoef2_dat); 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GetGeneratorReduceFundSolution(&corrPoints1, &corrPoints2, &fundReduceCoef1, &fundReduceCoef2); 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Choose best solutions for two generators. We can get 3 solutions */ 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat resFundReduceCoef; 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double resFundReduceCoef_dat[3*5]; 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resFundReduceCoef = cvMat(3,5,CV_64F,resFundReduceCoef_dat); 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numSol = GetGoodReduceFundamMatrFromTwo(&fundReduceCoef1, &fundReduceCoef2,&resFundReduceCoef); 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int maxSol; 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxSol = projMatrs[0]->rows / 3; 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currSol; 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currSol = 0; (currSol < numSol && currSol < maxSol); currSol++ ) 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* For current solution compute projection matrix */ 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat fundCoefs; 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect(&resFundReduceCoef, &fundCoefs, cvRect(0,currSol,5,1)); 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat projMatrCoefs; 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double projMatrCoefs_dat[4]; 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrCoefs = cvMat(1,4,CV_64F,projMatrCoefs_dat); 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GetProjMatrFromReducedFundamental(&fundCoefs,&projMatrCoefs); 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* we have computed coeffs for reduced project matrix */ 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat objPoints; 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double objPoints_dat[4*6]; 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn objPoints = cvMat(4,6,CV_64F,objPoints_dat); 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero(&objPoints); 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* fill object points */ 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i =0; i < 4; i++ ) 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn objPoints_dat[i*6] = 1; 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn objPoints_dat[i*6+1] = projMatrCoefs_dat[i]; 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn objPoints_dat[i*7+2] = 1; 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currCamera; 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currCamera = 0; currCamera < 3; currCamera++ ) 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat projPoints; 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double projPoints_dat[3*6]; 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projPoints = cvMat(3,6,CV_64F,projPoints_dat); 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* fill projected points for current camera */ 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 6; i++ )/* for each points for current camera */ 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projPoints_dat[6*0+i] = cvmGet(points[currCamera],0,i);/* x */ 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projPoints_dat[6*1+i] = cvmGet(points[currCamera],1,i);/* y */ 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projPoints_dat[6*2+i] = 1;/* w */ 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* compute project matrix for current camera */ 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat projMatrix; 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double projMatrix_dat[3*4]; 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrix = cvMat(3,4,CV_64F,projMatrix_dat); 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeProjectMatrix(&objPoints,&projPoints,&projMatrix); 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Add this matrix to result */ 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat tmpSubRes; 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect(projMatrs[currCamera],&tmpSubRes,cvRect(0,currSol*3,4,3)); 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert(&projMatrix,&tmpSubRes); 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We know project matrices. And we can reconstruct 6 3D-points if need */ 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 0 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points4D ) 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( currSol < points4D->rows / 4 ) 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat tmpPoints4D; 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tmpPoints4D_dat[4*6]; 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmpPoints4D = cvMat(4,6,CV_64F,tmpPoints4D_dat); 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvReconstructPointsFor3View( &wProjMatr[0], &wProjMatr[1], &wProjMatr[2], 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points1, points2, points3, 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &tmpPoints4D); 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat tmpSubRes; 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect(points4D,tmpSubRes,cvRect(0,currSol*4,6,4)); 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert(tmpPoints4D,points4D); 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }/* for all sollutions */ 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return numSol; 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvGetRandNumbers(int range,int count,int* arr) 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Generate random numbers [0,range-1] */ 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvGetRandNumbers" ); 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test input data */ 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( arr == 0 ) 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Parameter 'arr' is a NULL pointer" ); 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for errors input data */ 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( range < count || range <= 0 ) 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Can't generate such numbers. Count must be <= range and range must be > 0" ); 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i,j; 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int newRand; 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int haveRep = 0;/* firstly we have not repeats */ 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn do 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* generate new number */ 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newRand = rand()%range; 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveRep = 0; 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for repeats in previous numbers */ 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < i; j++ ) 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( arr[j] == newRand ) 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn haveRep = 1; 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } while(haveRep); 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We have good random number */ 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr[i] = newRand; 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return 1; 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvSelectColsByNumbers(CvMat* srcMatr, CvMat* dstMatr, int* indexes,int number) 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvSelectColsByNumbers" ); 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test input data */ 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( srcMatr == 0 || dstMatr == 0 || indexes == 0) 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(srcMatr) || !CV_IS_MAT(dstMatr) ) 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "srcMatr and dstMatr must be a matrices" ); 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int srcSize; 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numRows; 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numRows = srcMatr->rows; 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn srcSize = srcMatr->cols; 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numRows != dstMatr->rows ) 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of rows of matrices must be the same" ); 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dst; 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( dst = 0; dst < number; dst++ ) 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int src = indexes[dst]; 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( src >=0 && src < srcSize ) 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy each elements in column */ 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numRows; i++ ) 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(dstMatr,i,dst,cvmGet(srcMatr,i,src)); 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvProject4DPoints(CvMat* points4D,CvMat* projMatr, CvMat* projPoints) 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* tmpProjPoints = 0; 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvProject4DPoints" ); 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points4D == 0 || projMatr == 0 || projPoints == 0) 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(points4D) || !CV_IS_MAT(projMatr) || !CV_IS_MAT(projPoints) ) 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = points4D->cols; 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 1 ) 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" ); 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints != projPoints->cols ) 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be the same"); 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projPoints->rows != 2 ) 5366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of projected points must be 2"); 5386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points4D->rows != 4 ) 5416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR(CV_StsUnmatchedSizes, "Number of coordinates of 4D points must be 4"); 5436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatr->cols != 4 || projMatr->rows != 3 ) 5466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of projection matrix must be 3x4"); 5486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( tmpProjPoints = cvCreateMat(3,numPoints,CV_64F) ); 5526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul(projMatr,points4D,tmpProjPoints); 5546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Scale points */ 5566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 5576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 5586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double scale,x,y; 5606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale = cvmGet(tmpProjPoints,2,i); 5626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = cvmGet(tmpProjPoints,0,i); 5636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = cvmGet(tmpProjPoints,1,i); 5646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(scale) > 1e-7 ) 5666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x /= scale; 5686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y /= scale; 5696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 5716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = 1e8; 5736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = 1e8; 5746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(projPoints,0,i,x); 5776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(projPoints,1,i,y); 5786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 5816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&tmpProjPoints); 5836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 5856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 5876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvCompute3ProjectMatricesNPointsStatus( CvMat** points,/* 3 arrays of points on image */ 5886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat** projMatrs,/* array of 3 prejection matrices */ 5896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat** statuses,/* 3 arrays of status of points */ 5906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double threshold,/* Threshold for good point */ 5916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double p,/* Probability of good result. */ 5926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* resStatus, 5936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* points4D) 5946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numProjMatrs = 0; 5966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn unsigned char *comStat = 0; 5976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *triPoints[3] = {0,0,0}; 5986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *status = 0; 5996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *triPoints4D = 0; 6006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvCompute3ProjectMatricesNPointsStatus" ); 6026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 6036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for errors */ 6056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points == 0 || projMatrs == 0 || statuses == 0 || resStatus == 0 ) 6066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 6086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 6116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < 3; currImage++ ) 6126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for null pointers */ 6146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points[currImage] == 0 ) 6156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of points arrays is a NULL pointer" ); 6176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatrs[currImage] == 0 ) 6206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of projMatr is a NULL pointer" ); 6226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( statuses[currImage] == 0 ) 6256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of status arrays is a NULL pointer" ); 6276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for matrices */ 6306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(points[currImage]) ) 6316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of points arrays is not a matrix" ); 6336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(projMatrs[currImage]) ) 6366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of projMatr is not a matrix" ); 6386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MASK_ARR(statuses[currImage]) ) 6416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of status arrays is not a mask array" ); 6436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 6476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = points[0]->cols; 6486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 6 ) 6496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number points must be more than 6" ); 6516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < 3; currImage++ ) 6546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points[currImage]->cols != numPoints || statuses[currImage]->cols != numPoints ) 6566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of points and statuses must be the same" ); 6586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points[currImage]->rows != 2 ) 6616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of points coordinates must be == 2" ); 6636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( statuses[currImage]->rows != 1 ) 6666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Each of status must be matrix 1xN" ); 6686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatrs[currImage]->rows != 3 || projMatrs[currImage]->cols != 4 ) 6716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Each of projection matrix must be 3x4" ); 6736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create common status for all points */ 6786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 6806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( comStat = (unsigned char*)cvAlloc(sizeof(unsigned char)*numPoints) ); 6826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn unsigned char *stats[3]; 6846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stats[0] = statuses[0]->data.ptr; 6866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stats[1] = statuses[1]->data.ptr; 6876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stats[2] = statuses[2]->data.ptr; 6886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numTripl; 6906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numTripl = 0; 6916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 6926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn comStat[i] = (unsigned char)(stats[0][i] * stats[1][i] * stats[2][i]); 6946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numTripl += comStat[i]; 6956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numTripl > 0 ) 6986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create new arrays with points */ 7006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( triPoints[0] = cvCreateMat(2,numTripl,CV_64F) ); 7016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( triPoints[1] = cvCreateMat(2,numTripl,CV_64F) ); 7026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( triPoints[2] = cvCreateMat(2,numTripl,CV_64F) ); 7036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points4D ) 7046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( triPoints4D = cvCreateMat(4,numTripl,CV_64F) ); 7066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create status array */ 7096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( status = cvCreateMat(1,numTripl,CV_64F) ); 7106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy points to new arrays */ 7126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currPnt = 0; 7136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 7146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( comStat[i] ) 7166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < 3; currImage++ ) 7186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(triPoints[currImage],0,currPnt,cvmGet(points[currImage],0,i)); 7206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(triPoints[currImage],1,currPnt,cvmGet(points[currImage],1,i)); 7216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currPnt++; 7236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Call function */ 7276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numProjMatrs = icvComputeProjectMatricesNPoints( triPoints[0],triPoints[1],triPoints[2], 7286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrs[0],projMatrs[1],projMatrs[2], 7296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn threshold,/* Threshold for good point */ 7306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p,/* Probability of good result. */ 7316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status, 7326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn triPoints4D); 7336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Get computed status and set to result */ 7356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero(resStatus); 7366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currPnt = 0; 7376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 7386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( comStat[i] ) 7406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(status,0,currPnt) > 0 ) 7426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resStatus->data.ptr[i] = 1; 7446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currPnt++; 7466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( triPoints4D ) 7506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy copmuted 4D points */ 7526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero(points4D); 7536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currPnt = 0; 7546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 7556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( comStat[i] ) 7576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(status,0,currPnt) > 0 ) 7596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet( points4D, 0, i, cvmGet( triPoints4D , 0, currPnt) ); 7616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet( points4D, 1, i, cvmGet( triPoints4D , 1, currPnt) ); 7626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet( points4D, 2, i, cvmGet( triPoints4D , 2, currPnt) ); 7636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet( points4D, 3, i, cvmGet( triPoints4D , 3, currPnt) ); 7646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currPnt++; 7666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 7726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Free allocated memory */ 7746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&status); 7756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &comStat); 7766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&status); 7776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&triPoints[0]); 7796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&triPoints[1]); 7806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&triPoints[2]); 7816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&triPoints4D); 7826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return numProjMatrs; 7846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 7866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 7886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvComputeProjectMatricesNPoints( CvMat* points1,CvMat* points2,CvMat* points3, 7896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3, 7906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double threshold,/* Threshold for good point */ 7916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double p,/* Probability of good result. */ 7926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* status, 7936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* points4D) 7946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 7956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Returns status for each point, Good or bad */ 7966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute projection matrices using N points */ 7986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn char* flags = 0; 8006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn char* bestFlags = 0; 8016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numProjMatrs = 0; 8036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* tmpProjPoints[3]={0,0,0}; 8056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* recPoints4D = 0; 8066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *reconPoints4D = 0; 8076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvComputeProjectMatricesNPoints" ); 8106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 8116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* points[3]; 8136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points[0] = points1; 8146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points[1] = points2; 8156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points[2] = points3; 8166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for errors */ 8186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points1 == 0 || points2 == 0 || points3 == 0 || 8196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatr1 == 0 || projMatr2 == 0 || projMatr3 == 0 || 8206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status == 0) 8216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 8236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) || !CV_IS_MAT(points3) || 8266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) || !CV_IS_MAT(projMatr3) || 8276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_IS_MAT(status) ) 8286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); 8306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 8336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = points1->cols; 8346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 6 ) 8366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number points must be more than 6" ); 8386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints != points2->cols || numPoints != points3->cols ) 8416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "number of points must be the same" ); 8436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( p < 0 || p > 1.0 ) 8466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Probability must be >=0 and <=1" ); 8486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( threshold < 0 ) 8516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Threshold for good points must be at least >= 0" ); 8536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projMatrs[3]; 8566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrs[0] = projMatr1; 8586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrs[1] = projMatr2; 8596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrs[2] = projMatr3; 8606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 8626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ ) 8636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatrs[i]->cols != 4 || projMatrs[i]->rows != 3 ) 8656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of projection matrices must be 3x4" ); 8676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ ) 8716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points[i]->rows != 2) 8736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of points must be 2" ); 8756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* use RANSAC algorithm to compute projection matrices */ 8796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( recPoints4D = cvCreateMat(4,numPoints,CV_64F) ); 8816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( tmpProjPoints[0] = cvCreateMat(2,numPoints,CV_64F) ); 8826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( tmpProjPoints[1] = cvCreateMat(2,numPoints,CV_64F) ); 8836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( tmpProjPoints[2] = cvCreateMat(2,numPoints,CV_64F) ); 8846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( flags = (char*)cvAlloc(sizeof(char)*numPoints) ); 8866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( bestFlags = (char*)cvAlloc(sizeof(char)*numPoints) ); 8876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int NumSamples = 500;/* just init number of samples */ 8906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int wasCount = 0; /* count of choosing samples */ 8916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int maxGoodPoints = 0; 8926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numGoodPoints = 0; 8936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double bestProjMatrs_dat[36]; 8956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat bestProjMatrs[3]; 8966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bestProjMatrs[0] = cvMat(3,4,CV_64F,bestProjMatrs_dat); 8976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bestProjMatrs[1] = cvMat(3,4,CV_64F,bestProjMatrs_dat+12); 8986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bestProjMatrs[2] = cvMat(3,4,CV_64F,bestProjMatrs_dat+24); 8996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tmpProjMatr_dat[36*3]; 9016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat tmpProjMatr[3]; 9026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmpProjMatr[0] = cvMat(9,4,CV_64F,tmpProjMatr_dat); 9036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmpProjMatr[1] = cvMat(9,4,CV_64F,tmpProjMatr_dat+36); 9046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmpProjMatr[2] = cvMat(9,4,CV_64F,tmpProjMatr_dat+72); 9056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* choosen points */ 9076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn while( wasCount < NumSamples ) 9096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* select samples */ 9116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int randNumbs[6]; 9126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetRandNumbers(numPoints,6,randNumbs); 9136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* random numbers of points was generated */ 9156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* select points */ 9166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double selPoints_dat[2*6*3]; 9186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat selPoints[3]; 9196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn selPoints[0] = cvMat(2,6,CV_64F,selPoints_dat); 9206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn selPoints[1] = cvMat(2,6,CV_64F,selPoints_dat+12); 9216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn selPoints[2] = cvMat(2,6,CV_64F,selPoints_dat+24); 9226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy 6 point for random indexes */ 9246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvSelectColsByNumbers( points[0], &selPoints[0], randNumbs,6); 9256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvSelectColsByNumbers( points[1], &selPoints[1], randNumbs,6); 9266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvSelectColsByNumbers( points[2], &selPoints[2], randNumbs,6); 9276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute projection matrices for this points */ 9296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numProj = icvComputeProjectMatrices6Points( &selPoints[0],&selPoints[1],&selPoints[2], 9306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &tmpProjMatr[0],&tmpProjMatr[1],&tmpProjMatr[2]); 9316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute number of good points for each matrix */ 9336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat proj6[3]; 9346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int currProj = 0; currProj < numProj; currProj++ ) 9356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubArr(&tmpProjMatr[0],&proj6[0],cvRect(0,currProj*3,4,3)); 9376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubArr(&tmpProjMatr[1],&proj6[1],cvRect(0,currProj*3,4,3)); 9386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubArr(&tmpProjMatr[2],&proj6[2],cvRect(0,currProj*3,4,3)); 9396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Reconstruct points for projection matrices */ 9416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvReconstructPointsFor3View( &proj6[0],&proj6[1],&proj6[2], 9426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points[0], points[1], points[2], 9436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn recPoints4D); 9446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Project points to images using projection matrices */ 9466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvProject4DPoints(recPoints4D,&proj6[0],tmpProjPoints[0]); 9476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvProject4DPoints(recPoints4D,&proj6[1],tmpProjPoints[1]); 9486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvProject4DPoints(recPoints4D,&proj6[2],tmpProjPoints[2]); 9496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute distances and number of good points (inliers) */ 9516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 9526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 9536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numGoodPoints = 0; 9546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 9556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dist=-1; 9576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist = 0; 9586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Choose max distance for each of three points */ 9596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < 3; currImage++ ) 9606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x1,y1,x2,y2; 9626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x1 = cvmGet(tmpProjPoints[currImage],0,i); 9636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y1 = cvmGet(tmpProjPoints[currImage],1,i); 9646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x2 = cvmGet(points[currImage],0,i); 9656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y2 = cvmGet(points[currImage],1,i); 9666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dx,dy; 9686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dx = x1-x2; 9696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dy = y1-y2; 9706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 1 9716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double newDist = dx*dx+dy*dy; 9726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( newDist > dist ) 9736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist = newDist; 9756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#else 9776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist += sqrt(dx*dx+dy*dy)/3.0; 9786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 9796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist = sqrt(dist); 9816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn flags[i] = (char)(dist > threshold ? 0 : 1); 9826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numGoodPoints += flags[i]; 9836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numGoodPoints > maxGoodPoints ) 9886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Copy current projection matrices as best */ 9896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(&proj6[0],&bestProjMatrs[0]); 9916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(&proj6[1],&bestProjMatrs[1]); 9926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(&proj6[2],&bestProjMatrs[2]); 9936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxGoodPoints = numGoodPoints; 9956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* copy best flags */ 9966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy(bestFlags,flags,sizeof(flags[0])*numPoints); 9976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Adaptive number of samples to count*/ 9996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double ep = 1 - (double)numGoodPoints / (double)numPoints; 10006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ep == 1 ) 10016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ep = 0.5;/* if there is not good points set ration of outliers to 50% */ 10036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double newNumSamples = (log(1-p) / log(1-pow(1-ep,6))); 10066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( newNumSamples < double(NumSamples) ) 10076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn NumSamples = cvRound(newNumSamples); 10096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn wasCount++; 10146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 0 10166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn char str[300]; 10176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sprintf(str,"Initial numPoints = %d\nmaxGoodPoints=%d\nRANSAC made %d steps", 10186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints, 10196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxGoodPoints, 10206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRound(wasCount)); 10216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MessageBox(0,str,"Info",MB_OK|MB_TASKMODAL); 10226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 10236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* we may have best 6-point projection matrices. */ 10256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* and best points */ 10266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* use these points to improve matrices */ 10276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( maxGoodPoints < 6 ) 10296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* matrix not found */ 10316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numProjMatrs = 0; 10326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 10346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We may Improove matrices using ---- method */ 10366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We may try to use Levenberg-Marquardt optimization */ 10376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //int currIter = 0; 10386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int finalGoodPoints = 0; 10396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn char *goodFlags = 0; 10406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn goodFlags = (char*)cvAlloc(numPoints*sizeof(char)); 10416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int needRepeat; 10436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn do 10446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 0 10466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Version without using status for Levenberg-Marquardt minimization */ 10476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *optStatus; 10496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn optStatus = cvCreateMat(1,numPoints,CV_64F); 10506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int testNumber = 0; 10516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i=0;i<numPoints;i++ ) 10526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(optStatus,0,i,(double)bestFlags[i]); 10546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn testNumber += bestFlags[i]; 10556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn char str2[200]; 10586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sprintf(str2,"test good num=%d\nmaxGoodPoints=%d",testNumber,maxGoodPoints); 10596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MessageBox(0,str2,"Info",MB_OK|MB_TASKMODAL); 10606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *gPresPoints; 10626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn gPresPoints = cvCreateMat(1,maxGoodPoints,CV_64F); 10636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < maxGoodPoints; i++) 10646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(gPresPoints,0,i,1.0); 10666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create array of points pres */ 10696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *pointsPres[3]; 10706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointsPres[0] = gPresPoints; 10716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointsPres[1] = gPresPoints; 10726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointsPres[2] = gPresPoints; 10736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create just good points 2D */ 10756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *gPoints[3]; 10766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCreateGoodPoints(points[0],&gPoints[0],optStatus); 10776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCreateGoodPoints(points[1],&gPoints[1],optStatus); 10786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCreateGoodPoints(points[2],&gPoints[2],optStatus); 10796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create 4D points array for good points */ 10816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *resPoints4D; 10826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resPoints4D = cvCreateMat(4,maxGoodPoints,CV_64F); 10836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projMs[3]; 10856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMs[0] = &bestProjMatrs[0]; 10876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMs[1] = &bestProjMatrs[1]; 10886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMs[2] = &bestProjMatrs[2]; 10896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat resProjMatrs[3]; 10926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double resProjMatrs_dat[36]; 10936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resProjMatrs[0] = cvMat(3,4,CV_64F,resProjMatrs_dat); 10946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resProjMatrs[1] = cvMat(3,4,CV_64F,resProjMatrs_dat+12); 10956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resProjMatrs[2] = cvMat(3,4,CV_64F,resProjMatrs_dat+24); 10966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* resMatrs[3]; 10986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resMatrs[0] = &resProjMatrs[0]; 10996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resMatrs[1] = &resProjMatrs[1]; 11006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resMatrs[2] = &resProjMatrs[2]; 11016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvOptimizeLevenbergMarquardtBundle( projMs,//projMs, 11036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn gPoints,//points,//points2D, 11046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointsPres,//pointsPres, 11056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3, 11066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resMatrs,//resProjMatrs, 11076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resPoints4D,//resPoints4D, 11086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 100, 1e-9 ); 11096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We found optimized projection matrices */ 11116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *reconPoints4D; 11136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn reconPoints4D = cvCreateMat(4,numPoints,CV_64F); 11146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Reconstruct all points using found projection matrices */ 11166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvReconstructPointsFor3View( &resProjMatrs[0],&resProjMatrs[1],&resProjMatrs[2], 11176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points[0], points[1], points[2], 11186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn reconPoints4D); 11196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Project points to images using projection matrices */ 11216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvProject4DPoints(reconPoints4D,&resProjMatrs[0],tmpProjPoints[0]); 11226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvProject4DPoints(reconPoints4D,&resProjMatrs[1],tmpProjPoints[1]); 11236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvProject4DPoints(reconPoints4D,&resProjMatrs[2],tmpProjPoints[2]); 11246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute error for each point and select good */ 11276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 11296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn finalGoodPoints = 0; 11306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 11316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dist=-1; 11336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Choose max distance for each of three points */ 11346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < 3; currImage++ ) 11356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x1,y1,x2,y2; 11376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x1 = cvmGet(tmpProjPoints[currImage],0,i); 11386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y1 = cvmGet(tmpProjPoints[currImage],1,i); 11396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x2 = cvmGet(points[currImage],0,i); 11406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y2 = cvmGet(points[currImage],1,i); 11416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dx,dy; 11436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dx = x1-x2; 11446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dy = y1-y2; 11456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double newDist = dx*dx+dy*dy; 11476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( newDist > dist ) 11486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist = newDist; 11506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist = sqrt(dist); 11536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn goodFlags[i] = (char)(dist > threshold ? 0 : 1); 11546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn finalGoodPoints += goodFlags[i]; 11556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn char str[200]; 11586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sprintf(str,"Was num = %d\nNew num=%d",maxGoodPoints,finalGoodPoints); 11596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MessageBox(0,str,"Info",MB_OK|MB_TASKMODAL); 11606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( finalGoodPoints > maxGoodPoints ) 11616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy new version of projection matrices */ 11636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(&resProjMatrs[0],&bestProjMatrs[0]); 11646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(&resProjMatrs[1],&bestProjMatrs[1]); 11656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(&resProjMatrs[2],&bestProjMatrs[2]); 11666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy(bestFlags,goodFlags,numPoints*sizeof(char)); 11676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxGoodPoints = finalGoodPoints; 11686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&optStatus); 11716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&resPoints4D); 11726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#else 11736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Version with using status for Levenberd-Marquardt minimization */ 11746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create status */ 11766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *optStatus; 11776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn optStatus = cvCreateMat(1,numPoints,CV_64F); 11786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i=0;i<numPoints;i++ ) 11796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(optStatus,0,i,(double)bestFlags[i]); 11816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *pointsPres[3]; 11846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointsPres[0] = optStatus; 11856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointsPres[1] = optStatus; 11866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointsPres[2] = optStatus; 11876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create 4D points array for good points */ 11896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *resPoints4D; 11906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resPoints4D = cvCreateMat(4,numPoints,CV_64F); 11916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projMs[3]; 11936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMs[0] = &bestProjMatrs[0]; 11956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMs[1] = &bestProjMatrs[1]; 11966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMs[2] = &bestProjMatrs[2]; 11976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat resProjMatrs[3]; 11996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double resProjMatrs_dat[36]; 12006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resProjMatrs[0] = cvMat(3,4,CV_64F,resProjMatrs_dat); 12016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resProjMatrs[1] = cvMat(3,4,CV_64F,resProjMatrs_dat+12); 12026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resProjMatrs[2] = cvMat(3,4,CV_64F,resProjMatrs_dat+24); 12036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* resMatrs[3]; 12056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resMatrs[0] = &resProjMatrs[0]; 12066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resMatrs[1] = &resProjMatrs[1]; 12076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resMatrs[2] = &resProjMatrs[2]; 12086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvOptimizeLevenbergMarquardtBundle( projMs,//projMs, 12106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points,//points2D, 12116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointsPres,//pointsPres, 12126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3, 12136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resMatrs,//resProjMatrs, 12146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resPoints4D,//resPoints4D, 12156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 100, 1e-9 ); 12166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We found optimized projection matrices */ 12186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn reconPoints4D = cvCreateMat(4,numPoints,CV_64F); 12206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Reconstruct all points using found projection matrices */ 12226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvReconstructPointsFor3View( &resProjMatrs[0],&resProjMatrs[1],&resProjMatrs[2], 12236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points[0], points[1], points[2], 12246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn reconPoints4D); 12256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Project points to images using projection matrices */ 12276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvProject4DPoints(reconPoints4D,&resProjMatrs[0],tmpProjPoints[0]); 12286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvProject4DPoints(reconPoints4D,&resProjMatrs[1],tmpProjPoints[1]); 12296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvProject4DPoints(reconPoints4D,&resProjMatrs[2],tmpProjPoints[2]); 12306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute error for each point and select good */ 12336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 12356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn finalGoodPoints = 0; 12366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 12376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dist=-1; 12396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Choose max distance for each of three points */ 12406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < 3; currImage++ ) 12416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x1,y1,x2,y2; 12436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x1 = cvmGet(tmpProjPoints[currImage],0,i); 12446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y1 = cvmGet(tmpProjPoints[currImage],1,i); 12456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x2 = cvmGet(points[currImage],0,i); 12466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y2 = cvmGet(points[currImage],1,i); 12476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dx,dy; 12496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dx = x1-x2; 12506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dy = y1-y2; 12516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double newDist = dx*dx+dy*dy; 12536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( newDist > dist ) 12546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist = newDist; 12566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist = sqrt(dist); 12596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn goodFlags[i] = (char)(dist > threshold ? 0 : 1); 12606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn finalGoodPoints += goodFlags[i]; 12616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /*char str[200]; 12646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sprintf(str,"Was num = %d\nNew num=%d",maxGoodPoints,finalGoodPoints); 12656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MessageBox(0,str,"Info",MB_OK|MB_TASKMODAL);*/ 12666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn needRepeat = 0; 12686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( finalGoodPoints > maxGoodPoints ) 12696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy new version of projection matrices */ 12716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(&resProjMatrs[0],&bestProjMatrs[0]); 12726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(&resProjMatrs[1],&bestProjMatrs[1]); 12736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(&resProjMatrs[2],&bestProjMatrs[2]); 12746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy(bestFlags,goodFlags,numPoints*sizeof(char)); 12756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxGoodPoints = finalGoodPoints; 12766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn needRepeat = 1; 12776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&optStatus); 12806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&resPoints4D); 12816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 12846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } while ( needRepeat ); 12856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &goodFlags); 12876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numProjMatrs = 1; 12926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy projection matrices */ 12946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert(&bestProjMatrs[0],projMatr1); 12956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert(&bestProjMatrs[1],projMatr2); 12966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert(&bestProjMatrs[2],projMatr3); 12976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( status ) 12996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* copy status for each points if need */ 13016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int i = 0; i < numPoints; i++) 13026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(status,0,i,(double)bestFlags[i]); 13046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points4D ) 13106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Fill reconstructed points */ 13116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero(points4D); 13136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvReconstructPointsFor3View( projMatr1,projMatr2,projMatr3, 13146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points[0], points[1], points[2], 13156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points4D); 13166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 13216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &flags); 13236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &bestFlags); 13246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&recPoints4D); 13266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&tmpProjPoints[0]); 13276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&tmpProjPoints[1]); 13286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&tmpProjPoints[2]); 13296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return numProjMatrs; 13316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 13326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 13346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvFindBaseTransform(CvMat* points,CvMat* resultT) 13366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 13376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvFindBaseTransform" ); 13396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 13406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points == 0 || resultT == 0 ) 13426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 13446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(points) || !CV_IS_MAT(resultT) ) 13476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "points and resultT must be a matrices" ); 13496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points->rows != 2 || points->cols != 4 ) 13526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be 4. And they must have 2 coordinates" ); 13546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( resultT->rows != 3 || resultT->cols != 3 ) 13576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "size of matrix resultT must be 3x3" ); 13596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Function gets four points and compute transformation to e1=(100) e2=(010) e3=(001) e4=(111) */ 13626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* !!! test each three points not collinear. Need to test */ 13646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create matrices */ 13666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrA; 13676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat vectB; 13686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrA_dat[3*3]; 13696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double vectB_dat[3]; 13706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA = cvMat(3,3,CV_64F,matrA_dat); 13716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectB = cvMat(3,1,CV_64F,vectB_dat); 13726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* fill matrices */ 13746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 13756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ ) 13766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&matrA,0,i,cvmGet(points,0,i)); 13786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&matrA,1,i,cvmGet(points,1,i)); 13796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&matrA,2,i,1); 13806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill vector B */ 13836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&vectB,0,0,cvmGet(points,0,3)); 13846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&vectB,1,0,cvmGet(points,1,3)); 13856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&vectB,2,0,1); 13866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* result scale */ 13886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat scale; 13896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double scale_dat[3]; 13906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale = cvMat(3,1,CV_64F,scale_dat); 13916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSolve(&matrA,&vectB,&scale,CV_SVD); 13936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* multiply by scale */ 13956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int j; 13966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 3; j++ ) 13976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double sc = scale_dat[j]; 13996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ ) 14006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[i*3+j] *= sc; 14026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Convert inverse matrix */ 14066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat tmpRes; 14076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tmpRes_dat[9]; 14086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmpRes = cvMat(3,3,CV_64F,tmpRes_dat); 14096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvInvert(&matrA,&tmpRes); 14106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert(&tmpRes,resultT); 14126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 14146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 14166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 14176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 14206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid GetGeneratorReduceFundSolution(CvMat* points1,CvMat* points2,CvMat* fundReduceCoef1,CvMat* fundReduceCoef2) 14216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 14226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "GetGeneratorReduceFundSolution" ); 14246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 14256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test input data for errors */ 14276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points1 == 0 || points2 == 0 || fundReduceCoef1 == 0 || fundReduceCoef2 == 0) 14296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 14316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) || !CV_IS_MAT(fundReduceCoef1) || !CV_IS_MAT(fundReduceCoef2) ) 14346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); 14366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points1->rows != 3 || points1->cols != 3 ) 14416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of points1 must be 3 and and have 3 coordinates" ); 14436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points2->rows != 3 || points2->cols != 3 ) 14466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of points2 must be 3 and and have 3 coordinates" ); 14486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fundReduceCoef1->rows != 1 || fundReduceCoef1->cols != 5 ) 14516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of fundReduceCoef1 must be 1x5" ); 14536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fundReduceCoef2->rows != 1 || fundReduceCoef2->cols != 5 ) 14566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of fundReduceCoef2 must be 1x5" ); 14586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Using 3 corr. points compute reduce */ 14616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create matrix */ 14636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrA; 14646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrA_dat[3*5]; 14656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA = cvMat(3,5,CV_64F,matrA_dat); 14666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 14676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ ) 14686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x1,y1,w1,x2,y2,w2; 14706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x1 = cvmGet(points1,0,i); 14716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y1 = cvmGet(points1,1,i); 14726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w1 = cvmGet(points1,2,i); 14736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x2 = cvmGet(points2,0,i); 14756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y2 = cvmGet(points2,1,i); 14766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w2 = cvmGet(points2,2,i); 14776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&matrA,i,0,y1*x2-y1*w2); 14796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&matrA,i,1,w1*x2-y1*w2); 14806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&matrA,i,2,x1*y2-y1*w2); 14816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&matrA,i,3,w1*y2-y1*w2); 14826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&matrA,i,4,x1*w2-y1*w2); 14836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* solve system using svd */ 14866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrU; 14876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrW; 14886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrV; 14896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrU_dat[3*3]; 14916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrW_dat[3*5]; 14926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrV_dat[5*5]; 14936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrU = cvMat(3,3,CV_64F,matrU_dat); 14956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrW = cvMat(3,5,CV_64F,matrW_dat); 14966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrV = cvMat(5,5,CV_64F,matrV_dat); 14976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* From svd we need just two last vectors of V or two last row V' */ 14996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We get transposed matrixes U and V */ 15006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T); 15026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* copy results to fundamental matrices */ 15046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for(i=0;i<5;i++) 15056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(fundReduceCoef1,0,i,cvmGet(&matrV,3,i)); 15076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(fundReduceCoef2,0,i,cvmGet(&matrV,4,i)); 15086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 15116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 15126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 15146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 15166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint GetGoodReduceFundamMatrFromTwo(CvMat* fundReduceCoef1,CvMat* fundReduceCoef2,CvMat* resFundReduceCoef) 15186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 15196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numRoots = 0; 15206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "GetGoodReduceFundamMatrFromTwo" ); 15226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 15236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fundReduceCoef1 == 0 || fundReduceCoef2 == 0 || resFundReduceCoef == 0 ) 15256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 15276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(fundReduceCoef1) || !CV_IS_MAT(fundReduceCoef2) || !CV_IS_MAT(resFundReduceCoef) ) 15306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); 15326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* using two fundamental matrix comute matrixes for det(F)=0 */ 15356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* May compute 1 or 3 matrices. Returns number of solutions */ 15366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Here we will use case F=a*F1+(1-a)*F2 instead of F=m*F1+l*F2 */ 15376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for errors */ 15396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fundReduceCoef1->rows != 1 || fundReduceCoef1->cols != 5 ) 15406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of fundReduceCoef1 must be 1x5" ); 15426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fundReduceCoef2->rows != 1 || fundReduceCoef2->cols != 5 ) 15456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of fundReduceCoef2 must be 1x5" ); 15476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (resFundReduceCoef->rows != 1 && resFundReduceCoef->rows != 3) || resFundReduceCoef->cols != 5 ) 15506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of resFundReduceCoef must be 1x5" ); 15526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double p1,q1,r1,s1,t1; 15556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double p2,q2,r2,s2,t2; 15566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p1 = cvmGet(fundReduceCoef1,0,0); 15576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn q1 = cvmGet(fundReduceCoef1,0,1); 15586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r1 = cvmGet(fundReduceCoef1,0,2); 15596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s1 = cvmGet(fundReduceCoef1,0,3); 15606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = cvmGet(fundReduceCoef1,0,4); 15616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p2 = cvmGet(fundReduceCoef2,0,0); 15636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn q2 = cvmGet(fundReduceCoef2,0,1); 15646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r2 = cvmGet(fundReduceCoef2,0,2); 15656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s2 = cvmGet(fundReduceCoef2,0,3); 15666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t2 = cvmGet(fundReduceCoef2,0,4); 15676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* solve equation */ 15696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat result; 15706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat coeffs; 15716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double result_dat[2*3]; 15726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double coeffs_dat[4]; 15736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = cvMat(2,3,CV_64F,result_dat); 15746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs = cvMat(1,4,CV_64F,coeffs_dat); 15756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs_dat[0] = ((r1-r2)*(-p1-q1-r1-s1-t1+p2+q2+r2+s2+t2)*(q1-q2)+(p1-p2)*(s1-s2)*(t1-t2));/* *a^3 */ 15776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs_dat[1] = ((r2*(-p1-q1-r1-s1-t1+p2+q2+r2+s2+t2)+(r1-r2)*(-p2-q2-r2-s2-t2))*(q1-q2)+(r1-r2)*(-p1-q1-r1-s1-t1+p2+q2+r2+s2+t2)*q2+(p2*(s1-s2)+(p1-p2)*s2)*(t1-t2)+(p1-p2)*(s1-s2)*t2);/* *a^2 */ 15786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs_dat[2] = (r2*(-p2-q2-r2-s2-t2)*(q1-q2)+(r2*(-p1-q1-r1-s1-t1+p2+q2+r2+s2+t2)+(r1-r2)*(-p2-q2-r2-s2-t2))*q2+p2*s2*(t1-t2)+(p2*(s1-s2)+(p1-p2)*s2)*t2);/* *a */ 15796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn coeffs_dat[3] = r2*(-p2-q2-r2-s2-t2)*q2+p2*s2*t2;/* 1 */ 15806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int num; 15826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn num = cvSolveCubic(&coeffs,&result); 15836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* test number of solutions and test for real solutions */ 15866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 15876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < num; i++ ) 15886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(cvmGet(&result,1,i)) < 1e-8 ) 15906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double alpha = cvmGet(&result,0,i); 15926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int j; 15936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 5; j++ ) 15946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(resFundReduceCoef,numRoots,j, 15966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn alpha * cvmGet(fundReduceCoef1,0,j) + (1-alpha) * cvmGet(fundReduceCoef2,0,j) ); 15976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numRoots++; 15996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 16036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return numRoots; 16046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 16056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 16076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid GetProjMatrFromReducedFundamental(CvMat* fundReduceCoefs,CvMat* projMatrCoefs) 16096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 16106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "GetProjMatrFromReducedFundamental" ); 16116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 16126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for errors */ 16146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fundReduceCoefs == 0 || projMatrCoefs == 0 ) 16156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 16176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(fundReduceCoefs) || !CV_IS_MAT(projMatrCoefs) ) 16206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); 16226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fundReduceCoefs->rows != 1 || fundReduceCoefs->cols != 5 ) 16266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of fundReduceCoefs must be 1x5" ); 16286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatrCoefs->rows != 1 || projMatrCoefs->cols != 4 ) 16316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of projMatrCoefs must be 1x4" ); 16336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Computes project matrix from given reduced matrix */ 16366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* we have p,q,r,s,t and need get a,b,c,d */ 16376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill matrix to compute ratio a:b:c as A:B:C */ 16386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrA; 16406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrA_dat[3*3]; 16416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA = cvMat(3,3,CV_64F,matrA_dat); 16426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double p,q,r,s,t; 16446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p = cvmGet(fundReduceCoefs,0,0); 16456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn q = cvmGet(fundReduceCoefs,0,1); 16466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r = cvmGet(fundReduceCoefs,0,2); 16476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s = cvmGet(fundReduceCoefs,0,3); 16486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t = cvmGet(fundReduceCoefs,0,4); 16496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[0] = p; 16516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[1] = r; 16526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[2] = 0; 16536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[3] = q; 16556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[4] = 0; 16566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[5] = t; 16576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[6] = 0; 16596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[7] = s; 16606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[8] = -(p+q+r+s+t); 16616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrU; 16636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrW; 16646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrV; 16656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrU_dat[3*3]; 16676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrW_dat[3*3]; 16686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrV_dat[3*3]; 16696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrU = cvMat(3,3,CV_64F,matrU_dat); 16716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrW = cvMat(3,3,CV_64F,matrW_dat); 16726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrV = cvMat(3,3,CV_64F,matrV_dat); 16736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* From svd we need just last vector of V or last row V' */ 16756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We get transposed matrixes U and V */ 16766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T); 16786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double A1,B1,C1; 16806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A1 = matrV_dat[6]; 16816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn B1 = matrV_dat[7]; 16826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn C1 = matrV_dat[8]; 16836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Get second coeffs */ 16856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[0] = 0; 16866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[1] = r; 16876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[2] = t; 16886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[3] = p; 16906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[4] = 0; 16916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[5] = -(p+q+r+s+t); 16926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[6] = q; 16946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[7] = s; 16956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[8] = 0; 16966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T); 16986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double A2,B2,C2; 17006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A2 = matrV_dat[6]; 17016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn B2 = matrV_dat[7]; 17026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn C2 = matrV_dat[8]; 17036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a,b,c,d; 17056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrK; 17076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrK_dat[36]; 17086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrK = cvMat(6,6,CV_64F,matrK_dat); 17096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero(&matrK); 17106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrK_dat[0] = 1; 17126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrK_dat[7] = 1; 17136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrK_dat[14] = 1; 17146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrK_dat[18] = -1; 17166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrK_dat[25] = -1; 17176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrK_dat[32] = -1; 17186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrK_dat[21] = 1; 17206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrK_dat[27] = 1; 17216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrK_dat[33] = 1; 17226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrK_dat[0*6+4] = -A1; 17246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrK_dat[1*6+4] = -B1; 17256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrK_dat[2*6+4] = -C1; 17266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrK_dat[3*6+5] = -A2; 17286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrK_dat[4*6+5] = -B2; 17296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrK_dat[5*6+5] = -C2; 17306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrU; 17326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrW; 17336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrV; 17346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrU_dat[36]; 17366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrW_dat[36]; 17376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrV_dat[36]; 17386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrU = cvMat(6,6,CV_64F,matrU_dat); 17406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrW = cvMat(6,6,CV_64F,matrW_dat); 17416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrV = cvMat(6,6,CV_64F,matrV_dat); 17426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* From svd we need just last vector of V or last row V' */ 17446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We get transposed matrixes U and V */ 17456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD(&matrK,&matrW,0,&matrV,CV_SVD_V_T); 17476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a = matrV_dat[6*5+0]; 17496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b = matrV_dat[6*5+1]; 17506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c = matrV_dat[6*5+2]; 17516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = matrV_dat[6*5+3]; 17526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* we don't need last two coefficients. Because it just a k1,k2 */ 17536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(projMatrCoefs,0,0,a); 17556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(projMatrCoefs,0,1,b); 17566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(projMatrCoefs,0,2,c); 17576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(projMatrCoefs,0,3,d); 17586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 17626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 17636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 17646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 17666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeProjectMatrix(CvMat* objPoints,CvMat* projPoints,CvMat* projMatr) 17686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{/* Using SVD method */ 17696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Reconstruct points using object points and projected points */ 17716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Number of points must be >=6 */ 17726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrV; 17746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* matrA = 0; 17756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* matrW = 0; 17766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* workProjPoints = 0; 17776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* tmpProjPoints = 0; 17786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvComputeProjectMatrix" ); 17806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 17816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for errors */ 17836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( objPoints == 0 || projPoints == 0 || projMatr == 0) 17846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 17866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(objPoints) || !CV_IS_MAT(projPoints) || !CV_IS_MAT(projMatr) ) 17896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); 17916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatr->rows != 3 || projMatr->cols != 4 ) 17946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of projMatr must be 3x4" ); 17966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 17996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = projPoints->cols; 18006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 6 ) 18016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of points must be at least 6" ); 18036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints != objPoints->cols ) 18066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be same" ); 18086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( objPoints->rows != 4 ) 18116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Object points must have 4 coordinates" ); 18136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projPoints->rows != 3 && projPoints->rows != 2 ) 18166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Projected points must have 2 or 3 coordinates" ); 18186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create and fill matrix A */ 18216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( matrA = cvCreateMat(numPoints*3, 12, CV_64F) ); 18226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( matrW = cvCreateMat(numPoints*3, 12, CV_64F) ); 18236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projPoints->rows == 2 ) 18256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( tmpProjPoints = cvCreateMat(3,numPoints,CV_64F) ); 18276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMake3DPoints(projPoints,tmpProjPoints); 18286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn workProjPoints = tmpProjPoints; 18296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 18316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn workProjPoints = projPoints; 18336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrV_dat[144]; 18366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrV = cvMat(12,12,CV_64F,matrV_dat); 18376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 18386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn char* dat; 18406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dat = (char*)(matrA->data.db); 18416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 1 18436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE *file; 18446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen("d:\\test\\recProjMatr.txt","w"); 18456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 18476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0;i < numPoints; i++ ) 18486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x,y,w; 18506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double X,Y,Z,W; 18516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* matrDat = (double*)dat; 18526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = cvmGet(workProjPoints,0,i); 18546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = cvmGet(workProjPoints,1,i); 18556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w = cvmGet(workProjPoints,2,i); 18566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X = cvmGet(objPoints,0,i); 18596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Y = cvmGet(objPoints,1,i); 18606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Z = cvmGet(objPoints,2,i); 18616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn W = cvmGet(objPoints,3,i); 18626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 1 18646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"%d (%lf %lf %lf %lf) - (%lf %lf %lf)\n",i,X,Y,Z,W,x,y,w ); 18656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 18666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---*/ 18686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[ 0] = 0; 18696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[ 1] = 0; 18706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[ 2] = 0; 18716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[ 3] = 0; 18726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[ 4] = -w*X; 18746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[ 5] = -w*Y; 18756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[ 6] = -w*Z; 18766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[ 7] = -w*W; 18776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[ 8] = y*X; 18796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[ 9] = y*Y; 18806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[10] = y*Z; 18816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[11] = y*W; 18826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---*/ 18836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[12] = w*X; 18846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[13] = w*Y; 18856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[14] = w*Z; 18866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[15] = w*W; 18876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[16] = 0; 18896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[17] = 0; 18906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[18] = 0; 18916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[19] = 0; 18926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[20] = -x*X; 18946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[21] = -x*Y; 18956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[22] = -x*Z; 18966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[23] = -x*W; 18976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---*/ 18986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[24] = -y*X; 18996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[25] = -y*Y; 19006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[26] = -y*Z; 19016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[27] = -y*W; 19026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[28] = x*X; 19046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[29] = x*Y; 19056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[30] = x*Z; 19066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[31] = x*W; 19076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[32] = 0; 19096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[33] = 0; 19106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[34] = 0; 19116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrDat[35] = 0; 19126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*---*/ 19136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dat += (matrA->step)*3; 19146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 1 19166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 19176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 19196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Solve this system */ 19216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* From svd we need just last vector of V or last row V' */ 19236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We get transposed matrix V */ 19246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD(matrA,matrW,0,&matrV,CV_SVD_V_T); 19266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* projected matrix was computed */ 19286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 12; i++ ) 19296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(projMatr,i/4,i%4,cvmGet(&matrV,11,i)); 19316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&matrA); 19346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&matrW); 19356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&tmpProjPoints); 19366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 19376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 19386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 19416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* May be useless function */ 19426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeTransform4D(CvMat* points1,CvMat* points2,CvMat* transMatr) 19436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 19446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* matrA = 0; 19456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* matrW = 0; 19466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrV_dat[256]; 19486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrV = cvMat(16,16,CV_64F,matrV_dat); 19496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvComputeTransform4D" ); 19516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 19526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points1 == 0 || points2 == 0 || transMatr == 0) 19546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 19566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) || !CV_IS_MAT(transMatr) ) 19596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); 19616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Computes transformation matrix (4x4) for points1 -> points2 */ 19646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* p2=H*p1 */ 19656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for errors */ 19676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 19686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = points1->cols; 19696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* we must have at least 5 points */ 19716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 5 ) 19726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be at least 5" ); 19746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints != points2->cols ) 19776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be the same" ); 19796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( transMatr->rows != 4 || transMatr->cols != 4 ) 19826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of transMatr must be 4x4" ); 19846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points1->rows != 4 || points2->rows != 4 ) 19876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of points must be 4" ); 19896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create matrix */ 19926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( matrA = cvCreateMat(6*numPoints,16,CV_64F) ); 19936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( matrW = cvCreateMat(6*numPoints,16,CV_64F) ); 19946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero(matrA); 19966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill matrices */ 19986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 19996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ )/* For each point */ 20006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 20016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double X1,Y1,Z1,W1; 20026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double P[4]; 20036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn P[0] = cvmGet(points1,0,i); 20056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn P[1] = cvmGet(points1,1,i); 20066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn P[2] = cvmGet(points1,2,i); 20076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn P[3] = cvmGet(points1,3,i); 20086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X1 = cvmGet(points2,0,i); 20106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Y1 = cvmGet(points2,1,i); 20116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Z1 = cvmGet(points2,2,i); 20126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn W1 = cvmGet(points2,3,i); 20136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill matrA */ 20156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int j = 0; j < 4; j++ )/* For each coordinate */ 20166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 20176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x,y,z,w; 20186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = X1*P[j]; 20206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = Y1*P[j]; 20216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z = Z1*P[j]; 20226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w = W1*P[j]; 20236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrA,6*i+0,4*0+j,y); 20256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrA,6*i+0,4*1+j,-x); 20266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrA,6*i+1,4*0+j,z); 20286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrA,6*i+1,4*2+j,-x); 20296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrA,6*i+2,4*0+j,w); 20316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrA,6*i+2,4*3+j,-x); 20326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrA,6*i+3,4*1+j,-z); 20346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrA,6*i+3,4*2+j,y); 20356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrA,6*i+4,4*1+j,-w); 20376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrA,6*i+4,4*3+j,y); 20386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrA,6*i+5,4*2+j,-w); 20406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrA,6*i+5,4*3+j,z); 20416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* From svd we need just two last vectors of V or two last row V' */ 20456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We get transposed matrixes U and V */ 20466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD(matrA,matrW,0,&matrV,CV_SVD_V_T); 20486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy result to result matrix */ 20506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 16; i++ ) 20516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 20526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(transMatr,i/4,i%4,cvmGet(&matrV,15,i)); 20536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&matrA); 20566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&matrW); 20576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 20596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 20606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 20616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 20636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvReconstructPointsFor3View( CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3, 20656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projPoints1,CvMat* projPoints2,CvMat* projPoints3, 20666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* points4D) 20676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 20686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvReconstructPointsFor3View" ); 20696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 20706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatr1 == 0 || projMatr2 == 0 || projMatr3 == 0 || 20726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projPoints1 == 0 || projPoints2 == 0 || projPoints3 == 0 || 20736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points4D == 0) 20746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 20756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 20766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) || !CV_IS_MAT(projMatr3) || 20796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_IS_MAT(projPoints1) || !CV_IS_MAT(projPoints2) || !CV_IS_MAT(projPoints3) || 20806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_IS_MAT(points4D) ) 20816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 20826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); 20836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 20866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = projPoints1->cols; 20876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 1 ) 20896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 20906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of points must be more than zero" ); 20916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projPoints2->cols != numPoints || projPoints3->cols != numPoints || points4D->cols != numPoints ) 20946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 20956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be the same" ); 20966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projPoints1->rows != 2 || projPoints2->rows != 2 || projPoints3->rows != 2) 20996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of proj points coordinates must be == 2" ); 21016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 21026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points4D->rows != 4 ) 21046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of world points coordinates must be == 4" ); 21066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 21076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatr1->cols != 4 || projMatr1->rows != 3 || 21096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatr2->cols != 4 || projMatr2->rows != 3 || 21106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatr3->cols != 4 || projMatr3->rows != 3) 21116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of projection matrices must be 3x4" ); 21136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 21146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrA; 21166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrA_dat[36]; 21176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA = cvMat(9,4,CV_64F,matrA_dat); 21186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //CvMat matrU; 21206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrW; 21216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrV; 21226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //double matrU_dat[9*9]; 21236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrW_dat[9*4]; 21246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrV_dat[4*4]; 21256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //matrU = cvMat(9,9,CV_64F,matrU_dat); 21276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrW = cvMat(9,4,CV_64F,matrW_dat); 21286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrV = cvMat(4,4,CV_64F,matrV_dat); 21296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projPoints[3]; 21316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projMatrs[3]; 21326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projPoints[0] = projPoints1; 21346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projPoints[1] = projPoints2; 21356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projPoints[2] = projPoints3; 21366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrs[0] = projMatr1; 21386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrs[1] = projMatr2; 21396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrs[2] = projMatr3; 21406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Solve system for each point */ 21426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i,j; 21436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ )/* For each point */ 21446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill matrix for current point */ 21466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 3; j++ )/* For each view */ 21476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x,y; 21496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = cvmGet(projPoints[j],0,i); 21506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = cvmGet(projPoints[j],1,i); 21516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int k = 0; k < 4; k++ ) 21526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&matrA, j*3+0, k, x * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],0,k) ); 21546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&matrA, j*3+1, k, y * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],1,k) ); 21556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&matrA, j*3+2, k, x * cvmGet(projMatrs[j],1,k) - y * cvmGet(projMatrs[j],0,k) ); 21566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 21576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 21586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Solve system for current point */ 21596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T); 21616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy computed point */ 21636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points4D,0,i,cvmGet(&matrV,3,0));/* X */ 21646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points4D,1,i,cvmGet(&matrV,3,1));/* Y */ 21656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points4D,2,i,cvmGet(&matrV,3,2));/* Z */ 21666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points4D,3,i,cvmGet(&matrV,3,3));/* W */ 21676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 21686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 21696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Points was reconstructed. Try to reproject points */ 21716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We can compute reprojection error if need */ 21726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 21746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat point3D; 21756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double point3D_dat[4]; 21766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D = cvMat(4,1,CV_64F,point3D_dat); 21776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat point2D; 21796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double point2D_dat[3]; 21806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point2D = cvMat(3,1,CV_64F,point2D_dat); 21816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 21836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double W = cvmGet(points4D,3,i); 21856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D_dat[0] = cvmGet(points4D,0,i)/W; 21876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D_dat[1] = cvmGet(points4D,1,i)/W; 21886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D_dat[2] = cvmGet(points4D,2,i)/W; 21896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D_dat[3] = 1; 21906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* !!! Project this point for each camera */ 21926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int currCamera = 0; currCamera < 3; currCamera++ ) 21936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul(projMatrs[currCamera], &point3D, &point2D); 21956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x,y; 21976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float xr,yr,wr; 21986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = (float)cvmGet(projPoints[currCamera],0,i); 21996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = (float)cvmGet(projPoints[currCamera],1,i); 22006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn wr = (float)point2D_dat[2]; 22026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xr = (float)(point2D_dat[0]/wr); 22036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yr = (float)(point2D_dat[1]/wr); 22046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float deltaX,deltaY; 22066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn deltaX = (float)fabs(x-xr); 22076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn deltaY = (float)fabs(y-yr); 22086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 22136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 22146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 22156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 0 22206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid ReconstructPointsFor3View_bySolve( CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3, 22216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projPoints1,CvMat* projPoints2,CvMat* projPoints3, 22226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* points3D) 22236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 22246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "ReconstructPointsFor3View" ); 22256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 22266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 22296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = projPoints1->cols; 22306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projPoints2->cols != numPoints || projPoints3->cols != numPoints || points3D->cols != numPoints ) 22316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be the same" ); 22336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projPoints1->rows != 2 || projPoints2->rows != 2 || projPoints3->rows != 2) 22366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of proj points coordinates must be == 2" ); 22386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points3D->rows != 4 ) 22416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of world points coordinates must be == 4" ); 22436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatr1->cols != 4 || projMatr1->rows != 3 || 22466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatr2->cols != 4 || projMatr2->rows != 3 || 22476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatr3->cols != 4 || projMatr3->rows != 3) 22486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of proj matrix must be 3x4" ); 22506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrA; 22536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrA_dat[3*3*3]; 22546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA = cvMat(3*3,3,CV_64F,matrA_dat); 22556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat vectB; 22576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double vectB_dat[9]; 22586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectB = cvMat(9,1,CV_64F,vectB_dat); 22596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat result; 22616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double result_dat[3]; 22626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = cvMat(3,1,CV_64F,result_dat); 22636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projPoints[3]; 22656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projMatrs[3]; 22666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projPoints[0] = projPoints1; 22686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projPoints[1] = projPoints2; 22696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projPoints[2] = projPoints3; 22706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrs[0] = projMatr1; 22726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrs[1] = projMatr2; 22736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrs[2] = projMatr3; 22746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Solve system for each point */ 22766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i,j; 22776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ )/* For each point */ 22786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill matrix for current point */ 22806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 3; j++ )/* For each view */ 22816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x,y; 22836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = cvmGet(projPoints[j],0,i); 22846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = cvmGet(projPoints[j],1,i); 22856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&vectB,j*3+0,0,x-cvmGet(projMatrs[j],0,3)); 22876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&vectB,j*3+1,0,y-cvmGet(projMatrs[j],1,3)); 22886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&vectB,j*3+2,0,1-cvmGet(projMatrs[j],2,3)); 22896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int t = 0; t < 3; t++ ) 22916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int k = 0; k < 3; k++ ) 22936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&matrA, j*3+t, k, cvmGet(projMatrs[j],t,k) ); 22956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Solve system for current point */ 23016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSolve(&matrA,&vectB,&result,CV_SVD); 23026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points3D,0,i,result_dat[0]);/* X */ 23046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points3D,1,i,result_dat[1]);/* Y */ 23056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points3D,2,i,result_dat[2]);/* Z */ 23066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points3D,3,i,1);/* W */ 23076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Points was reconstructed. Try to reproject points */ 23116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 23126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 23136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat point3D; 23146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double point3D_dat[4]; 23156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D = cvMat(4,1,CV_64F,point3D_dat); 23166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat point2D; 23186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double point2D_dat[3]; 23196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point2D = cvMat(3,1,CV_64F,point2D_dat); 23206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 23226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 23236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double W = cvmGet(points3D,3,i); 23246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D_dat[0] = cvmGet(points3D,0,i)/W; 23266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D_dat[1] = cvmGet(points3D,1,i)/W; 23276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D_dat[2] = cvmGet(points3D,2,i)/W; 23286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D_dat[3] = 1; 23296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Project this point for each camera */ 23316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int currCamera = 0; currCamera < 3; currCamera++ ) 23326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 23336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul(projMatrs[currCamera], &point3D, &point2D); 23346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x,y; 23356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float xr,yr,wr; 23366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = (float)cvmGet(projPoints[currCamera],0,i); 23376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = (float)cvmGet(projPoints[currCamera],1,i); 23386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn wr = (float)point2D_dat[2]; 23406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xr = (float)(point2D_dat[0]/wr); 23416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yr = (float)(point2D_dat[1]/wr); 23426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 23486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 23496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 23506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 23516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 23536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeCameraExrinnsicByPosition(CvMat* camPos, CvMat* rotMatr, CvMat* transVect) 23556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 23566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We know position of camera. we must to compute rotate matrix and translate vector */ 23576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvComputeCameraExrinnsicByPosition" ); 23596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 23606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test input paramaters */ 23626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( camPos == 0 || rotMatr == 0 || transVect == 0 ) 23636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 23646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 23656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(camPos) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(transVect) ) 23686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 23696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); 23706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( camPos->cols != 1 || camPos->rows != 3 ) 23736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 23746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of camera position must be 3x1 vector" ); 23756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( rotMatr->cols != 3 || rotMatr->rows != 3 ) 23786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 23796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Rotate matrix must be 3x3" ); 23806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( transVect->cols != 1 || transVect->rows != 3 ) 23836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 23846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Translate vector must be 3x1" ); 23856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x,y,z; 23886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = cvmGet(camPos,0,0); 23896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = cvmGet(camPos,1,0); 23906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z = cvmGet(camPos,2,0); 23916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Set translate vector. It same as camea position */ 23936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(transVect,0,0,x); 23946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(transVect,1,0,y); 23956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(transVect,2,0,z); 23966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute rotate matrix. Compute each unit transformed vector */ 23986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* normalize flat direction x,y */ 24006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double vectorX[3]; 24016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double vectorY[3]; 24026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double vectorZ[3]; 24036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectorX[0] = -z; 24056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectorX[1] = 0; 24066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectorX[2] = x; 24076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectorY[0] = x*y; 24096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectorY[1] = x*x+z*z; 24106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectorY[2] = z*y; 24116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectorZ[0] = -x; 24136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectorZ[1] = -y; 24146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectorZ[2] = -z; 24156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* normaize vectors */ 24176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double norm; 24186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 24196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Norm X */ 24216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm = 0; 24226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ ) 24236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm += vectorX[i]*vectorX[i]; 24246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm = sqrt(norm); 24256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ ) 24266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectorX[i] /= norm; 24276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Norm Y */ 24296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm = 0; 24306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ ) 24316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm += vectorY[i]*vectorY[i]; 24326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm = sqrt(norm); 24336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ ) 24346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectorY[i] /= norm; 24356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Norm Z */ 24376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm = 0; 24386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ ) 24396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm += vectorZ[i]*vectorZ[i]; 24406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm = sqrt(norm); 24416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ ) 24426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectorZ[i] /= norm; 24436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Set output results */ 24456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ ) 24476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 24486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(rotMatr,i,0,vectorX[i]); 24496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(rotMatr,i,1,vectorY[i]); 24506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(rotMatr,i,2,vectorZ[i]); 24516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 24526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Try to inverse rotate matrix */ 24546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat tmpInvRot; 24556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tmpInvRot_dat[9]; 24566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmpInvRot = cvMat(3,3,CV_64F,tmpInvRot_dat); 24576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvInvert(rotMatr,&tmpInvRot,CV_SVD); 24586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert(&tmpInvRot,rotMatr); 24596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 24636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 24656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 24676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 24686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 24706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid FindTransformForProjectMatrices(CvMat* projMatr1,CvMat* projMatr2,CvMat* rotMatr,CvMat* transVect) 24726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 24736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Computes homography for project matrix be "canonical" form */ 24746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "computeProjMatrHomography" ); 24756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 24766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test input paramaters */ 24786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatr1 == 0 || projMatr2 == 0 || rotMatr == 0 || transVect == 0 ) 24796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 24806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 24816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 24826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(transVect) ) 24846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 24856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); 24866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 24876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatr1->cols != 4 || projMatr1->rows != 3 ) 24896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 24906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of project matrix 1 must be 3x4" ); 24916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 24926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatr2->cols != 4 || projMatr2->rows != 3 ) 24946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 24956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of project matrix 2 must be 3x4" ); 24966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 24976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( rotMatr->cols != 3 || rotMatr->rows != 3 ) 24996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of rotation matrix must be 3x3" ); 25016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 25026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( transVect->cols != 1 || transVect->rows != 3 ) 25046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of translation vector must be 3x1" ); 25066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 25076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrA; 25096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrA_dat[12*12]; 25106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA = cvMat(12,12,CV_64F,matrA_dat); 25116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat vectB; 25126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double vectB_dat[12]; 25136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectB = cvMat(12,1,CV_64F,vectB_dat); 25146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero(&matrA); 25166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero(&vectB); 25176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i,j; 25186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 12; i++ ) 25196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 12; j++ ) 25216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&matrA,i,j,cvmGet(projMatr1,i/4,j%4)); 25236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 25246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill vector B */ 25256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double val = cvmGet(projMatr2,i/4,i%4); 25276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (i+1)%4 == 0 ) 25286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn val -= cvmGet(projMatr1,i/4,3); 25306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 25326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&vectB,i,0,val); 25336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 25346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Solve system */ 25366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat resVect; 25376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double resVect_dat[12]; 25386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resVect = cvMat(12,1,CV_64F,resVect_dat); 25396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int sing; 25416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sing = cvSolve(&matrA,&vectB,&resVect); 25426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill rotation matrix */ 25446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 12; i++ ) 25456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double val = cvmGet(&resVect,i,0); 25476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( i < 9 ) 25486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(rotMatr,i%3,i/3,val); 25496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 25506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(transVect,i-9,0,val); 25516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 25526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 25546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 25566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 25576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 25596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 0 25606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeQknowPrincipalPoint(int numImages, CvMat **projMatrs,CvMat *matrQ, double cx,double cy) 25616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 25626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Computes matrix Q */ 25636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* focal x and y eqauls () */ 25646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* we know principal point for camera */ 25656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* focal may differ from image to image */ 25666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* image skew is 0 */ 25676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numImages < 10 ) 25696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 25716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //Error. Number of images too few 25726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 25736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create */ 25756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 25786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 25796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 25806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 25826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 25846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 25856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 25866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*==========================================================================================*/ 25876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Part with metric reconstruction */ 25886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 1 25906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeQ(int numMatr, CvMat** projMatr, CvMat** cameraMatr, CvMat* matrQ) 25916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 25926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* K*K' = P*Q*P' */ 25936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* try to solve Q by linear method */ 25946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* matrA = 0; 25966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* vectB = 0; 25976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "ComputeQ" ); 25996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 26006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Define number of projection matrices */ 26026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numMatr < 2 ) 26036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of projection matrices must be at least 2" ); 26056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* test matrices sizes */ 26096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( matrQ->cols != 4 || matrQ->rows != 4 ) 26106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of matrix Q must be 3x3" ); 26126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currMatr; 26156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currMatr = 0; currMatr < numMatr; currMatr++ ) 26166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cameraMatr[currMatr]->cols != 3 || cameraMatr[currMatr]->rows != 3 ) 26196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of each camera matrix must be 3x3" ); 26216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatr[currMatr]->cols != 4 || projMatr[currMatr]->rows != 3 ) 26246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of each camera matrix must be 3x3" ); 26266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrw; 26306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrw_dat[9]; 26316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrw = cvMat(3,3,CV_64F,matrw_dat); 26326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrKt; 26346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrKt_dat[9]; 26356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrKt = cvMat(3,3,CV_64F,matrKt_dat); 26366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create matrix A and vector B */ 26396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( matrA = cvCreateMat(9*numMatr,10,CV_64F) ); 26406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( vectB = cvCreateMat(9*numMatr,1,CV_64F) ); 26416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dataQ[16]; 26436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currMatr = 0; currMatr < numMatr; currMatr++ ) 26456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int ord10[10] = {0,1,2,3,5,6,7,10,11,15}; 26476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill atrix A by data from matrices */ 26486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute matrix w for current camera matrix */ 26506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvTranspose(cameraMatr[currMatr],&matrKt); 26516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul(cameraMatr[currMatr],&matrKt,&matrw); 26526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill matrix A and vector B */ 26546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currWi,currWj; 26566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currMatr; 26576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currMatr = 0; currMatr < numMatr; currMatr++ ) 26586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currWi = 0; currWi < 3; currWi++ ) 26606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currWj = 0; currWj < 3; currWj++ ) 26626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i,j; 26646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 4; i++ ) 26656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 4; j++ ) 26676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* get elements from current projection matrix */ 26696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dataQ[i*4+j] = cvmGet(projMatr[currMatr],currWi,j) * 26706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmGet(projMatr[currMatr],currWj,i); 26716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* we know 16 elements in dataQ move them to matrQ 10 */ 26756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dataQ[1] += dataQ[4]; 26766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dataQ[2] += dataQ[8]; 26776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dataQ[3] += dataQ[12]; 26786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dataQ[6] += dataQ[9]; 26796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dataQ[7] += dataQ[13]; 26806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dataQ[11] += dataQ[14]; 26816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Now first 10 elements has coeffs */ 26826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* copy to matrix A */ 26846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 10; i++ ) 26856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrA,currMatr*9 + currWi*3+currWj,i,dataQ[ord10[i]]); 26876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill vector B */ 26926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int i = 0; i < 9; i++ ) 26936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(vectB,currMatr*9+i,0,matrw_dat[i]); 26956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Matrix A and vector B filled and we can solve system */ 27006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Solve system */ 27026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat resQ; 27036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double resQ_dat[10]; 27046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resQ = cvMat(10,1,CV_64F,resQ_dat); 27056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSolve(matrA,vectB,&resQ,CV_SVD); 27076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* System was solved. We know matrix Q. But we must have condition det Q=0 */ 27096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Just copy result matrix Q */ 27106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 27116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int curr = 0; 27126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int ord16[16] = {0,1,2,3,1,4,5,6,2,5,7,8,3,6,8,9}; 27136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int i = 0; i < 4; i++ ) 27156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 27166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int j = 0; j < 4; j++ ) 27176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 27186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrQ,i,j,resQ_dat[ord16[curr++]]); 27196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 27206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 27216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 27226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 27256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Free allocated memory */ 27276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&matrA); 27286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&vectB); 27296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 27316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 27326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 27336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*-----------------------------------------------------------------------------------------------------*/ 27346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvDecomposeQ(CvMat* /*matrQ*/,CvMat* /*matrH*/) 27366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 27376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 0 27386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Use SVD to decompose matrix Q=H*I*H' */ 27396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* test input data */ 27406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrW; 27426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrU; 27436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// CvMat matrV; 27446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrW_dat[16]; 27456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrU_dat[16]; 27466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// double matrV_dat[16]; 27476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrW = cvMat(4,4,CV_64F,matrW_dat); 27496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrU = cvMat(4,4,CV_64F,matrU_dat); 27506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// matrV = cvMat(4,4,CV_64F,matrV_dat); 27516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD(matrQ,&matrW,&matrU,0); 27536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double eig[3]; 27556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn eig[0] = fsqrt(cvmGet(&matrW,0,0)); 27566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn eig[1] = fsqrt(cvmGet(&matrW,1,1)); 27576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn eig[2] = fsqrt(cvmGet(&matrW,2,2)); 27586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrIS; 27606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrIS_dat[16]; 27616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrIS = 27626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* det for matrix Q with q1-q10 */ 27676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* 27686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn+ q1*q5*q8*q10 27696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn- q1*q5*q9*q9 27706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn- q1*q6*q6*q10 27716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn+ 2*q1*q6*q7*q9 27726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn- q1*q7*q7*q8 27736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn- q2*q2*q8*q10 27746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn+ q2*q2*q9*q9 27756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn+ 2*q2*q6*q3*q10 27766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn- 2*q2*q6*q4*q9 27776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn- 2*q2*q7*q3*q9 27786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn+ 2*q2*q7*q4*q8 27796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn- q5*q3*q3*q10 27806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn+ 2*q3*q5*q4*q9 27816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn+ q3*q3*q7*q7 27826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn- 2*q3*q7*q4*q6 27836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn- q5*q4*q4*q8 27846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn+ q4*q4*q6*q6 27856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn*/ 27866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (1-a)^4 = 1 - 4 * a + 6 * a * a - 4 * a * a * a + a * a * a * a; 27886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 27906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 27916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 27926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2793