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 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Valery Mosyagin */ 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntypedef void (*pointer_LMJac)( const CvMat* src, CvMat* dst ); 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntypedef void (*pointer_LMFunc)( const CvMat* src, CvMat* dst ); 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid cvLevenbergMarquardtOptimization(pointer_LMJac JacobianFunction, 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointer_LMFunc function, 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /*pointer_Err error_function,*/ 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *X0,CvMat *observRes,CvMat *resultX, 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int maxIter,double epsilon); 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvReconstructPointsFor3View( CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3, 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* projPoints1,CvMat* projPoints2,CvMat* projPoints3, 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* points4D); 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Jacobian computation for trifocal case */ 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvJacobianFunction_ProjTrifocal(const CvMat *vectX,CvMat *Jacobian) 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvJacobianFunction_ProjTrifocal" ); 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test data for errors */ 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( vectX == 0 || Jacobian == 0 ) 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(vectX) || !CV_IS_MAT(Jacobian) ) 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = (vectX->rows - 36)/4; 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 1 )//!!! Need to correct this minimal number of points 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "number of points must be more than 0" ); 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( Jacobian->rows == numPoints*6 || Jacobian->cols != 36+numPoints*4 ) 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of Jacobian is not correct it must be 6*numPoints x (36+numPoints*4)" ); 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Computed Jacobian in a given point */ 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* This is for function with 3 projection matrices */ 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* vector X consists of projection matrices and points3D */ 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* each 3D points has X,Y,Z,W */ 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* each projection matrices has 3x4 coeffs */ 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* For N points 4D we have Jacobian 2N x (12*3+4N) */ 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Will store derivates as */ 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill Jacobian matrix */ 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currProjPoint; 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currMatr; 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero(Jacobian); 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currMatr = 0; currMatr < 3; currMatr++ ) 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double p[12]; 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int i=0;i<12;i++ ) 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p[i] = cvmGet(vectX,currMatr*12+i,0); 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currVal = 36; 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currProjPoint = 0; currProjPoint < numPoints; currProjPoint++ ) 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute */ 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double X[4]; 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X[0] = cvmGet(vectX,currVal++,0); 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X[1] = cvmGet(vectX,currVal++,0); 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X[2] = cvmGet(vectX,currVal++,0); 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X[3] = cvmGet(vectX,currVal++,0); 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double piX[3]; 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn piX[0] = X[0]*p[0] + X[1]*p[1] + X[2]*p[2] + X[3]*p[3]; 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn piX[1] = X[0]*p[4] + X[1]*p[5] + X[2]*p[6] + X[3]*p[7]; 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn piX[2] = X[0]*p[8] + X[1]*p[9] + X[2]*p[10] + X[3]*p[11]; 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i,j; 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* fill derivate by point */ 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tmp3 = 1/(piX[2]*piX[2]); 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tmp1 = -piX[0]*tmp3; 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tmp2 = -piX[1]*tmp3; 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 2; j++ )//for x and y 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 4; i++ )// for X,Y,Z,W 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet( Jacobian, 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currMatr*numPoints*2+currProjPoint*2+j, 36+currProjPoint*4+i, 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (p[j*4+i]*piX[2]-p[8+i]*piX[j]) * tmp3 ); 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* fill derivate by projection matrix */ 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 4; i++ ) 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* derivate for x */ 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(Jacobian,currMatr*numPoints*2+currProjPoint*2,currMatr*12+i,X[i]/piX[2]);//x' p1i 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(Jacobian,currMatr*numPoints*2+currProjPoint*2,currMatr*12+8+i,X[i]*tmp1);//x' p3i 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* derivate for y */ 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(Jacobian,currMatr*numPoints*2+currProjPoint*2+1,currMatr*12+4+i,X[i]/piX[2]);//y' p2i 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(Jacobian,currMatr*numPoints*2+currProjPoint*2+1,currMatr*12+8+i,X[i]*tmp2);//y' p3i 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvFunc_ProjTrifocal(const CvMat *vectX, CvMat *resFunc) 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Computes function in a given point */ 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Computers project points using 3 projection matrices and points 3D */ 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* vector X consists of projection matrices and points3D */ 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* each projection matrices has 3x4 coeffs */ 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* each 3D points has X,Y,Z,W(?) */ 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* result of function is projection of N 3D points using 3 projection matrices */ 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* projected points store as (projection by matrix P1),(projection by matrix P2),(projection by matrix P3) */ 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* each projection is x1,y1,x2,y2,x3,y3,x4,y4 */ 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute projection of points */ 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill projection matrices */ 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvFunc_ProjTrifocal" ); 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test data for errors */ 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( vectX == 0 || resFunc == 0 ) 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(vectX) || !CV_IS_MAT(resFunc) ) 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = (vectX->rows - 36)/4; 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 1 )//!!! Need to correct this minimal number of points 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "number of points must be more than 0" ); 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( resFunc->rows == 2*numPoints*3 || resFunc->cols != 1 ) 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of resFunc is not correct it must be 2*numPoints*3 x 1"); 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat projMatrs[3]; 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double projMatrs_dat[36]; 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrs[0] = cvMat(3,4,CV_64F,projMatrs_dat); 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrs[1] = cvMat(3,4,CV_64F,projMatrs_dat+12); 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatrs[2] = cvMat(3,4,CV_64F,projMatrs_dat+24); 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat point3D; 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double point3D_dat[3]; 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D = cvMat(3,1,CV_64F,point3D_dat); 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currMatr; 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currV; 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i,j; 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currV=0; 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currMatr = 0; currMatr < 3; currMatr++ ) 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ ) 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0;j < 4; j++ ) 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double val = cvmGet(vectX,currV,0); 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(&projMatrs[currMatr],i,j,val); 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currV++; 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Project points */ 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currPoint; 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat point4D; 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double point4D_dat[4]; 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point4D = cvMat(4,1,CV_64F,point4D_dat); 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currPoint = 0; currPoint < numPoints; currPoint++ ) 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* get curr point */ 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point4D_dat[0] = cvmGet(vectX,currV++,0); 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point4D_dat[1] = cvmGet(vectX,currV++,0); 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point4D_dat[2] = cvmGet(vectX,currV++,0); 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point4D_dat[3] = cvmGet(vectX,currV++,0); 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currMatr = 0; currMatr < 3; currMatr++ ) 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute projection for current point */ 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul(&projMatrs[currMatr],&point4D,&point3D); 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double z = point3D_dat[2]; 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(resFunc,currMatr*numPoints*2 + currPoint*2, 0,point3D_dat[0]/z); 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(resFunc,currMatr*numPoints*2 + currPoint*2+1,0,point3D_dat[1]/z); 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*----------------------------------------------------------------------------------------*/ 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvOptimizeProjectionTrifocal(CvMat **projMatrs,CvMat **projPoints, 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat **resultProjMatrs, CvMat *resultPoints4D) 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *optimX = 0; 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *points4D = 0; 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *vectorX0 = 0; 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *observRes = 0; 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //CvMat *error = 0; 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvOptimizeProjectionTrifocal" ); 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test data for errors */ 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatrs == 0 || projPoints == 0 || resultProjMatrs == 0 || resultPoints4D == 0) 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(resultPoints4D) ) 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "resultPoints4D must be a matrix" ); 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = resultPoints4D->cols; 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 1 ) 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number points of resultPoints4D must be more than 0" ); 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( resultPoints4D->rows != 4 ) 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of points4D must be 4" ); 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ ) 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatrs[i] == 0 ) 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of projMatrs is a NULL pointer" ); 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projPoints[i] == 0 ) 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of projPoints is a NULL pointer" ); 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( resultProjMatrs[i] == 0 ) 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of resultProjMatrs is a NULL pointer" ); 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----------- test for matrix ------------- */ 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(projMatrs[i]) ) 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Each of projMatrs must be a matrix" ); 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(projPoints[i]) ) 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Each of projPoints must be a matrix" ); 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(resultProjMatrs[i]) ) 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Each of resultProjMatrs must be a matrix" ); 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ------------- Test sizes --------------- */ 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatrs[i]->rows != 3 || projMatrs[i]->cols != 4 ) 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of projMatr must be 3x4" ); 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projPoints[i]->rows != 2 || projPoints[i]->cols != numPoints ) 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of resultProjMatrs must be 3x4" ); 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( resultProjMatrs[i]->rows != 3 || resultProjMatrs[i]->cols != 4 ) 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of resultProjMatrs must be 3x4" ); 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Allocate memory for points 4D */ 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( points4D = cvCreateMat(4,numPoints,CV_64F) ); 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( vectorX0 = cvCreateMat(36 + numPoints*4,1,CV_64F) ); 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( observRes = cvCreateMat(2*numPoints*3,1,CV_64F) ); 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( optimX = cvCreateMat(36+numPoints*4,1,CV_64F) ); 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //CV_CALL( error = cvCreateMat(numPoints*2*3,1,CV_64F) ); 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Reconstruct points 4D using projected points and projection matrices */ 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvReconstructPointsFor3View( projMatrs[0],projMatrs[1],projMatrs[2], 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projPoints[0],projPoints[1],projPoints[2], 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points4D); 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill observed points on images */ 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* result of function is projection of N 3D points using 3 projection matrices */ 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* projected points store as (projection by matrix P1),(projection by matrix P2),(projection by matrix P3) */ 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* each projection is x1,y1,x2,y2,x3,y3,x4,y4 */ 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currMatr; 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currMatr = 0; currMatr < 3; currMatr++ ) 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(observRes,currMatr*numPoints*2+i*2 ,0,cvmGet(projPoints[currMatr],0,i) );/* x */ 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(observRes,currMatr*numPoints*2+i*2+1,0,cvmGet(projPoints[currMatr],1,i) );/* y */ 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill with projection matrices */ 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currMatr = 0; currMatr < 3; currMatr++ ) 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 12; i++ ) 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(vectorX0,currMatr*12+i,0,cvmGet(projMatrs[currMatr],i/4,i%4)); 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill with 4D points */ 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currPoint; 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currPoint = 0; currPoint < numPoints; currPoint++ ) 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(vectorX0,36 + currPoint*4 + 0,0,cvmGet(points4D,0,currPoint)); 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(vectorX0,36 + currPoint*4 + 1,0,cvmGet(points4D,1,currPoint)); 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(vectorX0,36 + currPoint*4 + 2,0,cvmGet(points4D,2,currPoint)); 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(vectorX0,36 + currPoint*4 + 3,0,cvmGet(points4D,3,currPoint)); 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Allocate memory for result */ 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvLevenbergMarquardtOptimization( icvJacobianFunction_ProjTrifocal, icvFunc_ProjTrifocal, 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectorX0,observRes,optimX,100,1e-6); 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy results */ 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currMatr = 0; currMatr < 3; currMatr++ ) 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy projection matrices */ 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for(int i=0;i<12;i++) 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(resultProjMatrs[currMatr],i/4,i%4,cvmGet(optimX,currMatr*12+i,0)); 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy 4D points */ 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currPoint = 0; currPoint < numPoints; currPoint++ ) 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(resultPoints4D,0,currPoint,cvmGet(optimX,36 + currPoint*4,0)); 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(resultPoints4D,1,currPoint,cvmGet(optimX,36 + currPoint*4+1,0)); 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(resultPoints4D,2,currPoint,cvmGet(optimX,36 + currPoint*4+2,0)); 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(resultPoints4D,3,currPoint,cvmGet(optimX,36 + currPoint*4+3,0)); 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Free allocated memory */ 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&optimX); 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&points4D); 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&vectorX0); 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&observRes); 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*------------------------------------------------------------------------------*/ 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Create good points using status information */ 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvCreateGoodPoints(CvMat *points,CvMat **goodPoints, CvMat *status) 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *goodPoints = 0; 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvCreateGoodPoints" ); 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = points->cols; 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 1 ) 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of points must be more than 0" ); 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numCoord; 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numCoord = points->rows; 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numCoord < 1 ) 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of points coordinates must be more than 0" ); 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Define number of good points */ 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int goodNum; 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i,j; 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn goodNum = 0; 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++) 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(status,0,i) > 0 ) 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn goodNum++; 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Allocate memory for good points */ 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( *goodPoints = cvCreateMat(numCoord,goodNum,CV_64F) ); 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numCoord; i++ ) 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currPoint = 0; 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < numPoints; j++) 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(status,0,j) > 0 ) 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(*goodPoints,i,currPoint,cvmGet(points,i,j)); 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currPoint++; 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 498