16acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*M/////////////////////////////////////////////////////////////////////////////////////// 26acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 36acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 46acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 56acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// By downloading, copying, installing or using the software you agree to this license. 66acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// If you do not agree to this license, do not download, install, 76acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// copy or use the software. 86acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 96acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Intel License Agreement 116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// For Open Source Computer Vision Library 126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Copyright (C) 2000, Intel Corporation, all rights reserved. 146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Third party copyrights are property of their respective owners. 156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Redistribution and use in source and binary forms, with or without modification, 176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// are permitted provided that the following conditions are met: 186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's of source code must retain the above copyright notice, 206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer. 216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's in binary form must reproduce the above copyright notice, 236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer in the documentation 246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and/or other materials provided with the distribution. 256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * The name of Intel Corporation may not be used to endorse or promote products 276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// derived from this software without specific prior written permission. 286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// This software is provided by the copyright holders and contributors "as is" and 306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// any express or implied warranties, including, but not limited to, the implied 316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// warranties of merchantability and fitness for a particular purpose are disclaimed. 326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// In no event shall the Intel Corporation or contributors be liable for any direct, 336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// indirect, incidental, special, exemplary, or consequential damages 346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (including, but not limited to, procurement of substitute goods or services; 356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// loss of use, data, or profits; or business interruption) however caused 366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and on any theory of liability, whether in contract, strict liability, 376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// or tort (including negligence or otherwise) arising in any way out of 386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the use of this software, even if advised of the possibility of such damage. 396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*/ 416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cvaux.h" 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//#include "cvtypes.h" 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <float.h> 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <limits.h> 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//#include "cv.h" 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <stdio.h> 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvReconstructPoints4DStatus(CvMat** projPoints, CvMat **projMatrs, CvMat** presPoints, CvMat *points4D,int numImages,CvMat **projError=0); 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Valery Mosyagin */ 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* If you want to save internal debug info to files uncomment next lines and set paths to files if need */ 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Note these file may be very large */ 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define TRACK_BUNDLE 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define TRACK_BUNDLE_FILE "d:\\test\\bundle.txt" 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define TRACK_BUNDLE_FILE_JAC "d:\\test\\bundle.txt" 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define TRACK_BUNDLE_FILE_JACERRPROJ "d:\\test\\JacErrProj.txt" 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define TRACK_BUNDLE_FILE_JACERRPNT "d:\\test\\JacErrPoint.txt" 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define TRACK_BUNDLE_FILE_MATRW "d:\\test\\matrWt.txt" 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define TRACK_BUNDLE_FILE_DELTAP "d:\\test\\deltaP.txt" 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn*/ 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define TRACK_BUNDLE_FILE "d:\\test\\bundle.txt" 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* ============== Bundle adjustment optimization ================= */ 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeDerivateProj(CvMat *points4D,CvMat *projMatr, CvMat *status, CvMat *derivProj) 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute derivate for given projection matrix points and status of points */ 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvComputeDerivateProj" ); 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- Test input params for errors ----- */ 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points4D == 0 || projMatr == 0 || status == 0 || derivProj == 0) 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(points4D) ) 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "points4D must be a matrix 4xN" ); 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute number of points */ 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = points4D->cols; 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 1 ) 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" ); 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points4D->rows != 4 ) 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of coordinates of points4D must be 4" ); 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(projMatr) ) 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "projMatr must be a matrix 3x4" ); 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatr->rows != 3 || projMatr->cols != 4 ) 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Size of projection matrix (projMatr) must be 3x4" ); 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(status) ) 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Status must be a matrix 1xN" ); 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( status->rows != 1 || status->cols != numPoints ) 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Size of status of points must be 1xN" ); 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(derivProj) ) 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "derivProj must be a matrix VisN x 12" ); 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( derivProj->cols != 12 ) 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "derivProj must be a matrix VisN x 12" ); 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- End test ----- */ 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Allocate memory for derivates */ 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double p[12]; 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy projection matrix */ 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 12; i++ ) 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p[i] = cvmGet(projMatr,i/4,i%4); 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill deriv matrix */ 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currVisPoint; 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currPoint; 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currVisPoint = 0; 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currPoint = 0; currPoint < numPoints; currPoint++ ) 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(status,0,currPoint) > 0 ) 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double X[4]; 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X[0] = cvmGet(points4D,0,currVisPoint); 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X[1] = cvmGet(points4D,1,currVisPoint); 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X[2] = cvmGet(points4D,2,currVisPoint); 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X[3] = cvmGet(points4D,3,currVisPoint); 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute derivate for this point */ 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double piX[3]; 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn piX[0] = X[0]*p[0] + X[1]*p[1] + X[2]*p[2] + X[3]*p[3]; 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn piX[1] = X[0]*p[4] + X[1]*p[5] + X[2]*p[6] + X[3]*p[7]; 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn piX[2] = X[0]*p[8] + X[1]*p[9] + X[2]*p[10] + X[3]*p[11]; 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* fill derivate by point */ 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tmp3 = 1/(piX[2]*piX[2]); 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tmp1 = -piX[0]*tmp3; 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tmp2 = -piX[1]*tmp3; 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* fill derivate by projection matrix */ 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 4; i++ ) 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* derivate for x */ 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(derivProj,currVisPoint*2,i,X[i]/piX[2]);//x' p1i 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(derivProj,currVisPoint*2,4+i,0);//x' p1i 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(derivProj,currVisPoint*2,8+i,X[i]*tmp1);//x' p3i 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* derivate for y */ 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(derivProj,currVisPoint*2+1,i,0);//y' p2i 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(derivProj,currVisPoint*2+1,4+i,X[i]/piX[2]);//y' p2i 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(derivProj,currVisPoint*2+1,8+i,X[i]*tmp2);//y' p3i 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currVisPoint++; 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( derivProj->rows != currVisPoint * 2 ) 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "derivProj must be a matrix 2VisN x 12" ); 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*======================================================================================*/ 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeDerivateProjAll(CvMat *points4D, CvMat **projMatrs, CvMat **pointPres, int numImages,CvMat **projDerives) 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvComputeDerivateProjAll" ); 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- Test input params for errors ----- */ 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numImages < 1 ) 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatrs == 0 || pointPres == 0 || projDerives == 0 ) 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- End test ----- */ 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeDerivateProj(points4D,projMatrs[currImage], pointPres[currImage], projDerives[currImage]); 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*======================================================================================*/ 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeDerivatePoints(CvMat *points4D,CvMat *projMatr, CvMat *presPoints, CvMat *derivPoint) 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvComputeDerivatePoints" ); 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- Test input params for errors ----- */ 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points4D == 0 || projMatr == 0 || presPoints == 0 || derivPoint == 0) 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(points4D) ) 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "points4D must be a matrix N x 4" ); 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = presPoints->cols; 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 1 ) 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of points must be more than zero" ); 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points4D->rows != 4 ) 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "points4D must be a matrix N x 4" ); 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(projMatr) ) 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "projMatr must be a matrix 3x4" ); 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatr->rows != 3 || projMatr->cols != 4 ) 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Size of projection matrix (projMatr) must be 3x4" ); 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(presPoints) ) 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Status must be a matrix 1xN" ); 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( presPoints->rows != 1 || presPoints->cols != numPoints ) 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Size of presPoints status must be 1xN" ); 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(derivPoint) ) 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "derivPoint must be a matrix 2 x 4VisNum" ); 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- End test ----- */ 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute derivates by points */ 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double p[12]; 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 12; i++ ) 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p[i] = cvmGet(projMatr,i/4,i%4); 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currVisPoint; 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currProjPoint; 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currVisPoint = 0; 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currProjPoint = 0; currProjPoint < numPoints; currProjPoint++ ) 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(presPoints,0,currProjPoint) > 0 ) 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double X[4]; 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X[0] = cvmGet(points4D,0,currProjPoint); 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X[1] = cvmGet(points4D,1,currProjPoint); 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X[2] = cvmGet(points4D,2,currProjPoint); 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X[3] = cvmGet(points4D,3,currProjPoint); 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double piX[3]; 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn piX[0] = X[0]*p[0] + X[1]*p[1] + X[2]*p[2] + X[3]*p[3]; 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn piX[1] = X[0]*p[4] + X[1]*p[5] + X[2]*p[6] + X[3]*p[7]; 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn piX[2] = X[0]*p[8] + X[1]*p[9] + X[2]*p[10] + X[3]*p[11]; 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i,j; 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tmp3 = 1/(piX[2]*piX[2]); 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 2; j++ )//for x and y 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 4; i++ )// for X,Y,Z,W 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet( derivPoint, 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn j, currVisPoint*4+i, 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (p[j*4+i]*piX[2]-p[8+i]*piX[j]) * tmp3 ); 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currVisPoint++; 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( derivPoint->rows != 2 || derivPoint->cols != currVisPoint*4 ) 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "derivPoint must be a matrix 2 x 4VisNum" ); 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*======================================================================================*/ 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeDerivatePointsAll(CvMat *points4D, CvMat **projMatrs, CvMat **pointPres, int numImages,CvMat **pointDerives) 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvComputeDerivatePointsAll" ); 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- Test input params for errors ----- */ 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numImages < 1 ) 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatrs == 0 || pointPres == 0 || pointDerives == 0 ) 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- End test ----- */ 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeDerivatePoints(points4D, projMatrs[currImage], pointPres[currImage], pointDerives[currImage]); 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*======================================================================================*/ 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeMatrixVAll(int numImages,CvMat **pointDeriv,CvMat **presPoints, CvMat **matrV) 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int *shifts = 0; 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvComputeMatrixVAll" ); 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- Test input params for errors ----- */ 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numImages < 1 ) 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pointDeriv == 0 || presPoints == 0 || matrV == 0 ) 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* !!! not tested all parameters */ 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- End test ----- */ 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute all matrices U */ 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currPoint; 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = presPoints[0]->cols; 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL(shifts = (int*)cvAlloc(sizeof(int)*numImages)); 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset(shifts,0,sizeof(int)*numImages); 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currPoint = 0; currPoint < numPoints; currPoint++ )//For each point (matrix V) 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i,j; 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 4; i++ ) 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 4; j++ ) 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double sum = 0; 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum += cvmGet(pointDeriv[currImage],0,shifts[currImage]*4+i) * 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmGet(pointDeriv[currImage],0,shifts[currImage]*4+j); 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum += cvmGet(pointDeriv[currImage],1,shifts[currImage]*4+i) * 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmGet(pointDeriv[currImage],1,shifts[currImage]*4+j); 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrV[currPoint],i,j,sum); 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* shift position of visible points */ 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn shifts[currImage]++; 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &shifts); 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*======================================================================================*/ 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeMatrixUAll(int numImages,CvMat **projDeriv,CvMat** matrU) 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvComputeMatrixVAll" ); 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- Test input params for errors ----- */ 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numImages < 1 ) 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projDeriv == 0 || matrU == 0 ) 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* !!! Not tested all input parameters */ 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- End test ----- */ 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute matrices V */ 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMulTransposed(projDeriv[currImage],matrU[currImage],1); 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*======================================================================================*/ 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeMatrixW(int numImages, CvMat **projDeriv, CvMat **pointDeriv, CvMat **presPoints, CvMat *matrW) 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvComputeMatrixW" ); 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- Test input params for errors ----- */ 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numImages < 1 ) 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projDeriv == 0 || pointDeriv == 0 || presPoints == 0 || matrW == 0 ) 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = presPoints[0]->cols; 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 1 ) 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of points must more than zero" ); 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(matrW) ) 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "matrW must be a matrix 12NumIm x 4NumPnt" ); 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( matrW->rows != numImages*12 || matrW->cols != numPoints*4 ) 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "matrW must be a matrix 12NumIm x 4NumPnt" ); 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* !!! Not tested all input parameters */ 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- End test ----- */ 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute number of points */ 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute matrix W using derivate proj and points */ 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int currLine = 0; currLine < 12; currLine++ ) 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currVis = 0; 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int currPoint = 0; currPoint < numPoints; currPoint++ ) 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int currCol = 0; currCol < 4; currCol++ ) 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double sum; 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum = cvmGet(projDeriv[currImage],currVis*2+0,currLine) * 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmGet(pointDeriv[currImage],0,currVis*4+currCol); 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum += cvmGet(projDeriv[currImage],currVis*2+1,currLine) * 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmGet(pointDeriv[currImage],1,currVis*4+currCol); 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrW,currImage*12+currLine,currPoint*4+currCol,sum); 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currVis++; 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* set all sub elements to zero */ 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int currCol = 0; currCol < 4; currCol++ ) 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrW,currImage*12+currLine,currPoint*4+currCol,0); 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#ifdef TRACK_BUNDLE 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE *file; 5366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen( TRACK_BUNDLE_FILE_MATRW ,"w"); 5376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currPoint,currImage; 5386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currPoint = 0; currPoint < numPoints; currPoint++ ) 5396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\nPoint=%d\n",currPoint); 5416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currRow; 5426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currRow = 0; currRow < 4; currRow++ ) 5436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage< numImages; currImage++ ) 5456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 5476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 12; i++ ) 5486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double val = cvmGet(matrW, currImage * 12 + i, currPoint * 4 + currRow); 5506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"%lf ",val); 5516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n"); 5546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 5576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 5596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 5616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 5626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*======================================================================================*/ 5646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Compute jacobian mult projection matrices error */ 5656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeJacErrorProj(int numImages,CvMat **projDeriv,CvMat **projErrors,CvMat *jacProjErr ) 5666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvComputeJacErrorProj" ); 5686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 5696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- Test input params for errors ----- */ 5716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numImages < 1 ) 5726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); 5746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projDeriv == 0 || projErrors == 0 || jacProjErr == 0 ) 5766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 5786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(jacProjErr) ) 5806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "jacProjErr must be a matrix 12NumIm x 1" ); 5826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( jacProjErr->rows != numImages*12 || jacProjErr->cols != 1 ) 5846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "jacProjErr must be a matrix 12NumIm x 1" ); 5866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* !!! Not tested all input parameters */ 5886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- End test ----- */ 5896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 5916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 5926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int currCol = 0; currCol < 12; currCol++ ) 5946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int num = projDeriv[currImage]->rows; 5966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double sum = 0; 5976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int i = 0; i < num; i++ ) 5986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum += cvmGet(projDeriv[currImage],i,currCol) * 6006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmGet(projErrors[currImage],i%2,i/2); 6016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(jacProjErr,currImage*12+currCol,0,sum); 6036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#ifdef TRACK_BUNDLE 6076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE *file; 6096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen( TRACK_BUNDLE_FILE_JACERRPROJ ,"w"); 6106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 6116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 6126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\nImage=%d\n",currImage); 6146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currRow; 6156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currRow = 0; currRow < 12; currRow++ ) 6166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double val = cvmGet(jacProjErr, currImage * 12 + currRow, 0); 6186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"%lf\n",val); 6196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n"); 6216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 6236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 6256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 6286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 6296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 6306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*======================================================================================*/ 6316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Compute jacobian mult points error */ 6326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeJacErrorPoint(int numImages,CvMat **pointDeriv,CvMat **projErrors, CvMat **presPoints,CvMat *jacPointErr ) 6336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 6346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int *shifts = 0; 6356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvComputeJacErrorPoint" ); 6376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 6386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- Test input params for errors ----- */ 6406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numImages < 1 ) 6416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); 6436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pointDeriv == 0 || projErrors == 0 || presPoints == 0 || jacPointErr == 0 ) 6466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 6486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 6516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = presPoints[0]->cols; 6526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 1 ) 6536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of points must more than zero" ); 6556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(jacPointErr) ) 6586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "jacPointErr must be a matrix 4NumPnt x 1" ); 6606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( jacPointErr->rows != numPoints*4 || jacPointErr->cols != 1 ) 6636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "jacPointErr must be a matrix 4NumPnt x 1" ); 6656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* !!! Not tested all input parameters */ 6676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- End test ----- */ 6686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 6706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currPoint; 6716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL(shifts = (int*)cvAlloc(sizeof(int)*numImages)); 6726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset(shifts,0,sizeof(int)*numImages); 6736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currPoint = 0; currPoint < numPoints; currPoint++ ) 6746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int currCoord = 0; currCoord < 4; currCoord++ ) 6766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double sum = 0; 6786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currVis = 0; 6806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 6816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) 6836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum += cvmGet(pointDeriv[currImage],0,shifts[currImage]*4+currCoord) * 6856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmGet(projErrors[currImage],0,shifts[currImage]);//currVis); 6866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum += cvmGet(pointDeriv[currImage],1,shifts[currImage]*4+currCoord) * 6886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmGet(projErrors[currImage],1,shifts[currImage]);//currVis); 6896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currVis++; 6916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(jacPointErr,currPoint*4+currCoord,0,sum); 6966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Increase shifts */ 6996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 7006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) 7026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn shifts[currImage]++; 7046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#ifdef TRACK_BUNDLE 7106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE *file; 7126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen(TRACK_BUNDLE_FILE_JACERRPNT,"w"); 7136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currPoint; 7146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currPoint = 0; currPoint < numPoints; currPoint++ ) 7156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\nPoint=%d\n",currPoint); 7176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currRow; 7186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currRow = 0; currRow < 4; currRow++ ) 7196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double val = cvmGet(jacPointErr, currPoint * 4 + currRow, 0); 7216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"%lf\n",val); 7226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n"); 7246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 7266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 7286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 7326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &shifts); 7336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 7356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*======================================================================================*/ 7366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Reconstruct 4D points using status */ 7386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvReconstructPoints4DStatus(CvMat** projPoints, CvMat **projMatrs, CvMat** presPoints, 7396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *points4D,int numImages,CvMat **projError) 7406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 7416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* matrA_dat = 0; 7436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* matrW_dat = 0; 7446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvReconstructPoints4DStatus" ); 7466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 7476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- Test input params for errors ----- */ 7496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numImages < 2 ) 7506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of images must be more than one" ); 7526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projPoints == 0 || projMatrs == 0 || presPoints == 0 || points4D == 0 ) 7556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 7576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 7606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = points4D->cols; 7616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 1 ) 7626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" ); 7646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points4D->rows != 4 ) 7676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Points must have 4 cordinates" ); 7696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* !!! Not tested all input parameters */ 7726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- End test ----- */ 7736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 7756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currPoint; 7766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Allocate maximum data */ 7786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrV; 7816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double matrV_dat[4*4]; 7826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrV = cvMat(4,4,CV_64F,matrV_dat); 7836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL(matrA_dat = (double*)cvAlloc(3*numImages * 4 * sizeof(double))); 7856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL(matrW_dat = (double*)cvAlloc(3*numImages * 4 * sizeof(double))); 7866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* reconstruct each point */ 7886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currPoint = 0; currPoint < numPoints; currPoint++ ) 7896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Reconstruct current point */ 7916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Define number of visible projections */ 7926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numVisProj = 0; 7936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 7946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) 7966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numVisProj++; 7986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numVisProj < 2 ) 8026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* This point can't be reconstructed */ 8046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 8056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Allocate memory and create matrices */ 8086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrA; 8096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA = cvMat(3*numVisProj,4,CV_64F,matrA_dat); 8106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat matrW; 8126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrW = cvMat(3*numVisProj,4,CV_64F,matrW_dat); 8136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currVisProj = 0; 8156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ )/* For each view */ 8166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) 8186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x,y; 8206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = cvmGet(projPoints[currImage],0,currPoint); 8216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = cvmGet(projPoints[currImage],1,currPoint); 8226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int k = 0; k < 4; k++ ) 8236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[currVisProj*12 + k] = 8256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x * cvmGet(projMatrs[currImage],2,k) - cvmGet(projMatrs[currImage],0,k); 8266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[currVisProj*12+4 + k] = 8286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y * cvmGet(projMatrs[currImage],2,k) - cvmGet(projMatrs[currImage],1,k); 8296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrA_dat[currVisProj*12+8 + k] = 8316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x * cvmGet(projMatrs[currImage],1,k) - y * cvmGet(projMatrs[currImage],0,k); 8326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currVisProj++; 8346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Solve system for current point */ 8386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T); 8406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy computed point */ 8426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points4D,0,currPoint,cvmGet(&matrV,3,0));//X 8436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points4D,1,currPoint,cvmGet(&matrV,3,1));//Y 8446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points4D,2,currPoint,cvmGet(&matrV,3,2));//Z 8456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points4D,3,currPoint,cvmGet(&matrV,3,3));//W 8466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Compute projection error */ 8516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 8526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat point4D; 8546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat point3D; 8556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double point3D_dat[3]; 8566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D = cvMat(3,1,CV_64F,point3D_dat); 8576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currPoint; 8596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numVis = 0; 8606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double totalError = 0; 8616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currPoint = 0; currPoint < numPoints; currPoint++ ) 8626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(presPoints[currImage],0,currPoint) > 0) 8646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dx,dy; 8666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCol(points4D,&point4D,currPoint); 8676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul(projMatrs[currImage],&point4D,&point3D); 8686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double w = point3D_dat[2]; 8696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = point3D_dat[0] / w; 8706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double y = point3D_dat[1] / w; 8716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dx = cvmGet(projPoints[currImage],0,currPoint) - x; 8736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dy = cvmGet(projPoints[currImage],1,currPoint) - y; 8746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projError ) 8756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(projError[currImage],0,currPoint,dx); 8776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(projError[currImage],1,currPoint,dy); 8786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn totalError += sqrt(dx*dx+dy*dy); 8806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numVis++; 8816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //double meanError = totalError / (double)numVis; 8856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 8916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &matrA_dat); 8936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &matrW_dat); 8946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 8966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 8976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*======================================================================================*/ 8996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvProjPointsStatusFunc( int numImages, CvMat *points4D, CvMat **projMatrs, CvMat **pointsPres, CvMat **projPoints) 9016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 9026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvProjPointsStatusFunc" ); 9036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 9046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- Test input params for errors ----- */ 9066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numImages < 1 ) 9076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of images must be more than zero" ); 9096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points4D == 0 || projMatrs == 0 || pointsPres == 0 || projPoints == 0 ) 9126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 9146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 9176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = points4D->cols; 9186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 1 ) 9196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" ); 9216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points4D->rows != 4 ) 9246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Points must have 4 cordinates" ); 9266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* !!! Not tested all input parameters */ 9296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- End test ----- */ 9306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat point4D; 9326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat point3D; 9336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double point4D_dat[4]; 9346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double point3D_dat[3]; 9356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point4D = cvMat(4,1,CV_64F,point4D_dat); 9366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D = cvMat(3,1,CV_64F,point3D_dat); 9376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#ifdef TRACK_BUNDLE 9396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE *file; 9416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen( TRACK_BUNDLE_FILE ,"a"); 9426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n----- test 14.01 icvProjPointsStatusFunc -----\n"); 9436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 9446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 9466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 9486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 9496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Get project matrix */ 9516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* project visible points using current projection matrix */ 9526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currVisPoint = 0; 9536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int currPoint = 0; currPoint < numPoints; currPoint++ ) 9546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(pointsPres[currImage],0,currPoint) > 0 ) 9566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* project current point */ 9586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect(points4D,&point4D,cvRect(currPoint,0,1,4)); 9596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#ifdef TRACK_BUNDLE 9616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE *file; 9636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen( TRACK_BUNDLE_FILE ,"a"); 9646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n----- test 14.02 point4D (%lf, %lf, %lf, %lf) -----\n", 9656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmGet(&point4D,0,0), 9666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmGet(&point4D,1,0), 9676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmGet(&point4D,2,0), 9686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmGet(&point4D,3,0)); 9696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 9706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 9726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul(projMatrs[currImage],&point4D,&point3D); 9746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double w = point3D_dat[2]; 9756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(projPoints[currImage],0,currVisPoint,point3D_dat[0]/w); 9766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(projPoints[currImage],1,currVisPoint,point3D_dat[1]/w); 9776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#ifdef TRACK_BUNDLE 9796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE *file; 9816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen( TRACK_BUNDLE_FILE ,"a"); 9826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n----- test 14.03 (%lf, %lf, %lf) -> (%lf, %lf)-----\n", 9836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D_dat[0], 9846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D_dat[1], 9856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D_dat[2], 9866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D_dat[0]/w, 9876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point3D_dat[1]/w 9886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ); 9896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 9906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 9926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currVisPoint++; 9936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 9986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 9996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*======================================================================================*/ 10016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvFreeMatrixArray(CvMat ***matrArray,int numMatr) 10026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 10036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Free each matrix */ 10046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currMatr; 10056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( *matrArray != 0 ) 10076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* Need delete */ 10086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currMatr = 0; currMatr < numMatr; currMatr++ ) 10096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat((*matrArray)+currMatr); 10116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( matrArray); 10136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 10156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 10166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*======================================================================================*/ 10186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid *icvClearAlloc(int size) 10196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 10206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void *ptr = 0; 10216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvClearAlloc" ); 10236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 10246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( size > 0 ) 10266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL(ptr = cvAlloc(size)); 10286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset(ptr,0,size); 10296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 10326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return ptr; 10336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 10346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*======================================================================================*/ 10366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 0 10376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid cvOptimizeLevenbergMarquardtBundleWraper( CvMat** projMatrs, CvMat** observProjPoints, 10386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat** pointsPres, int numImages, 10396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat** resultProjMatrs, CvMat* resultPoints4D,int maxIter,double epsilon ) 10406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 10416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Delete al sparse points */ 10426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvDeleteSparsInPoints( int numImages, 10446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat **points, 10456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat **status, 10466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *wasStatus)/* status of previous configuration */ 10476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 10496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 10506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*======================================================================================*/ 10516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* !!! may be useful to return norm of error */ 10526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* !!! may be does not work correct with not all visible 4D points */ 10536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid cvOptimizeLevenbergMarquardtBundle( CvMat** projMatrs, CvMat** observProjPoints, 10546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat** pointsPres, int numImages, 10556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat** resultProjMatrs, CvMat* resultPoints4D,int maxIter,double epsilon ) 10566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 10576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *vectorX_points4D = 0; 10596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat **vectorX_projMatrs = 0; 10606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newVectorX_points4D = 0; 10626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat **newVectorX_projMatrs = 0; 10636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *changeVectorX_points4D = 0; 10656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *changeVectorX_projMatrs = 0; 10666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat **observVisPoints = 0; 10686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat **projVisPoints = 0; 10696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat **errorProjPoints = 0; 10706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat **DerivProj = 0; 10716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat **DerivPoint = 0; 10726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *matrW = 0; 10736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat **matrsUk = 0; 10746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat **workMatrsUk = 0; 10756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat **matrsVi = 0; 10766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *workMatrVi = 0; 10776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat **workMatrsInvVi = 0; 10786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *jacProjErr = 0; 10796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *jacPointErr = 0; 10806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *matrTmpSys1 = 0; 10826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *matrSysDeltaP = 0; 10836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *vectTmpSys3 = 0; 10846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *vectSysDeltaP = 0; 10856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *deltaP = 0; 10866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *deltaM = 0; 10876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *vectTmpSysM = 0; 10886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints = 0; 10906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvOptimizeLevenbergMarquardtBundle" ); 10936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 10946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- Test input params for errors ----- */ 10966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numImages < 1 ) 10976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of images must be more than zero" ); 10996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( maxIter < 1 || maxIter > 2000 ) 11026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Maximum number of iteration must be in [1..1000]" ); 11046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( epsilon < 0 ) 11076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Epsilon parameter must be >= 0" ); 11096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(resultPoints4D) ) 11126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "resultPoints4D must be a matrix 4 x NumPnt" ); 11146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = resultPoints4D->cols; 11176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints < 1 ) 11186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of points must be more than zero" );//!!! 11206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( resultPoints4D->rows != 4 ) 11236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "resultPoints4D must have 4 cordinates" ); 11256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- End test ----- */ 11286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Optimization using bundle adjustment */ 11306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* work with non visible points */ 11316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( vectorX_points4D = cvCreateMat(4,numPoints,CV_64F)); 11336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( vectorX_projMatrs = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages)); 11346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( newVectorX_points4D = cvCreateMat(4,numPoints,CV_64F)); 11366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( newVectorX_projMatrs = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages)); 11376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( changeVectorX_points4D = cvCreateMat(4,numPoints,CV_64F)); 11396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( changeVectorX_projMatrs = cvCreateMat(3,4,CV_64F)); 11406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 11426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- Test input params ----- */ 11446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 11456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test size of input initial and result projection matrices */ 11476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(projMatrs[currImage]) ) 11486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "each of initial projMatrs must be a matrix 3 x 4" ); 11506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( projMatrs[currImage]->rows != 3 || projMatrs[currImage]->cols != 4 ) 11526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "each of initial projMatrs must be a matrix 3 x 4" ); 11546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(observProjPoints[currImage]) ) 11586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "each of observProjPoints must be a matrix 2 x NumPnts" ); 11606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( observProjPoints[currImage]->rows != 2 || observProjPoints[currImage]->cols != numPoints ) 11626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "each of observProjPoints must be a matrix 2 x NumPnts" ); 11646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(pointsPres[currImage]) ) 11676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "each of pointsPres must be a matrix 1 x NumPnt" ); 11696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pointsPres[currImage]->rows != 1 || pointsPres[currImage]->cols != numPoints ) 11716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "each of pointsPres must be a matrix 1 x NumPnt" ); 11736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(resultProjMatrs[currImage]) ) 11766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "each of resultProjMatrs must be a matrix 3 x 4" ); 11786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( resultProjMatrs[currImage]->rows != 3 || resultProjMatrs[currImage]->cols != 4 ) 11806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "each of resultProjMatrs must be a matrix 3 x 4" ); 11826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- End test ----- */ 11866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy projection matrices to vectorX0 */ 11886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 11896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( vectorX_projMatrs[currImage] = cvCreateMat(3,4,CV_64F)); 11916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( newVectorX_projMatrs[currImage] = cvCreateMat(3,4,CV_64F)); 11926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(projMatrs[currImage],vectorX_projMatrs[currImage]); 11936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Reconstruct points4D using projection matrices and status information */ 11966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvReconstructPoints4DStatus(observProjPoints, projMatrs, pointsPres, vectorX_points4D, numImages); 11976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- Allocate memory for work matrices ----- */ 11996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute number of good points on each images */ 12006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( observVisPoints = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); 12026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( projVisPoints = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); 12036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( errorProjPoints = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); 12046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( DerivProj = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); 12056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( DerivPoint = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); 12066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( matrW = cvCreateMat(12*numImages,4*numPoints,CV_64F) ); 12076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( matrsUk = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); 12086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( workMatrsUk = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); 12096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( matrsVi = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numPoints) ); 12106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( workMatrVi = cvCreateMat(4,4,CV_64F) ); 12116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( workMatrsInvVi = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numPoints) ); 12126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( jacProjErr = cvCreateMat(12*numImages,1,CV_64F) ); 12146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( jacPointErr = cvCreateMat(4*numPoints,1,CV_64F) ); 12156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 12186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 12196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( matrsVi[i] = cvCreateMat(4,4,CV_64F) ); 12216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( workMatrsInvVi[i] = cvCreateMat(4,4,CV_64F) ); 12226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 12256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( matrsUk[currImage] = cvCreateMat(12,12,CV_64F) ); 12276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( workMatrsUk[currImage] = cvCreateMat(12,12,CV_64F) ); 12286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currVisPoint = 0, currPoint, numVisPoint; 12306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currPoint = 0; currPoint < numPoints; currPoint++ ) 12316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(pointsPres[currImage],0,currPoint) > 0 ) 12336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currVisPoint++; 12356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numVisPoint = currVisPoint; 12396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Allocate memory for current image data */ 12416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( observVisPoints[currImage] = cvCreateMat(2,numVisPoint,CV_64F) ); 12426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( projVisPoints[currImage] = cvCreateMat(2,numVisPoint,CV_64F) ); 12436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* create error matrix */ 12456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( errorProjPoints[currImage] = cvCreateMat(2,numVisPoint,CV_64F) ); 12466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create derivate matrices */ 12486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( DerivProj[currImage] = cvCreateMat(2*numVisPoint,12,CV_64F) ); 12496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( DerivPoint[currImage] = cvCreateMat(2,numVisPoint*4,CV_64F) ); 12506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy observed projected visible points */ 12526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currVisPoint = 0; 12536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currPoint = 0; currPoint < numPoints; currPoint++ ) 12546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(pointsPres[currImage],0,currPoint) > 0 ) 12566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(observVisPoints[currImage],0,currVisPoint,cvmGet(observProjPoints[currImage],0,currPoint)); 12586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(observVisPoints[currImage],1,currVisPoint,cvmGet(observProjPoints[currImage],1,currPoint)); 12596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currVisPoint++; 12606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /*---------------------------------------------------*/ 12646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( matrTmpSys1 = cvCreateMat(numPoints*4, numImages*12, CV_64F) ); 12666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( matrSysDeltaP = cvCreateMat(numImages*12, numImages*12, CV_64F) ); 12676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( vectTmpSys3 = cvCreateMat(numPoints*4,1,CV_64F) ); 12686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( vectSysDeltaP = cvCreateMat(numImages*12,1,CV_64F) ); 12696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( deltaP = cvCreateMat(numImages*12,1,CV_64F) ); 12706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( deltaM = cvCreateMat(numPoints*4,1,CV_64F) ); 12716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( vectTmpSysM = cvCreateMat(numPoints*4,1,CV_64F) ); 12726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//#ifdef TRACK_BUNDLE 12756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 1 12766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create file to track */ 12786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE* file; 12796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen( TRACK_BUNDLE_FILE ,"w"); 12806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"begin\n"); 12816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 12826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 12846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ============= main optimization loop ============== */ 12866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* project all points using current vector X */ 12886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvProjPointsStatusFunc(numImages, vectorX_points4D, vectorX_projMatrs, pointsPres, projVisPoints); 12896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute error with observed value and computed projection */ 12916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double prevError; 12926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prevError = 0; 12936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 12946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSub(observVisPoints[currImage],projVisPoints[currImage],errorProjPoints[currImage]); 12966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double currNorm = cvNorm(errorProjPoints[currImage]); 12976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prevError += currNorm * currNorm; 12986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prevError = sqrt(prevError); 13006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currIter; 13026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double change; 13036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double alpha; 13046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//#ifdef TRACK_BUNDLE 13066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 1 13076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create file to track */ 13096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE* file; 13106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen( TRACK_BUNDLE_FILE ,"a"); 13116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n========================================\n");; 13126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"Iter=0\n"); 13136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"Error = %20.15lf\n",prevError); 13146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"Change = %20.15lf\n",1.0); 13156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"projection errors\n"); 13176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Print all proejction errors */ 13196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 13206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++) 13216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\nImage=%d\n",currImage); 13236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPn = errorProjPoints[currImage]->cols; 13246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int currPoint = 0; currPoint < numPn; currPoint++ ) 13256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double ex,ey; 13276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ex = cvmGet(errorProjPoints[currImage],0,currPoint); 13286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ey = cvmGet(errorProjPoints[currImage],1,currPoint); 13296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"%40.35lf, %40.35lf\n",ex,ey); 13306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 13336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 13356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currIter = 0; 13376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn change = 1; 13386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn alpha = 0.001; 13396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn do 13426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#ifdef TRACK_BUNDLE 13456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE *file; 13476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen( TRACK_BUNDLE_FILE ,"a"); 13486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n----- test 6 do main -----\n"); 13496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double norm = cvNorm(vectorX_points4D); 13516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file," test 6.01 prev normPnts=%lf\n",norm); 13526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int i=0;i<numImages;i++ ) 13546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double norm = cvNorm(vectorX_projMatrs[i]); 13566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file," test 6.01 prev normProj=%lf\n",norm); 13576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 13606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 13626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute derivates by projectinon matrices */ 13646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeDerivateProjAll(vectorX_points4D,vectorX_projMatrs,pointsPres,numImages,DerivProj); 13656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute derivates by 4D points */ 13676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeDerivatePointsAll(vectorX_points4D,vectorX_projMatrs,pointsPres,numImages,DerivPoint); 13686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute matrces Uk */ 13706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeMatrixUAll(numImages,DerivProj,matrsUk); 13716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeMatrixVAll(numImages,DerivPoint,pointsPres,matrsVi); 13726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeMatrixW(numImages,DerivProj,DerivPoint,pointsPres,matrW); 13736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#ifdef TRACK_BUNDLE 13766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE *file; 13786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen( TRACK_BUNDLE_FILE ,"a"); 13796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n----- test 6.03 do matrs U V -----\n"); 13806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 13826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numImages; i++ ) 13836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double norm = cvNorm(matrsUk[i]); 13856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file," test 6.01 prev matrsUk=%lf\n",norm); 13866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 13896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double norm = cvNorm(matrsVi[i]); 13916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file," test 6.01 prev matrsVi=%lf\n",norm); 13926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 13956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 13976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute jac errors */ 13996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeJacErrorProj(numImages, DerivProj, errorProjPoints, jacProjErr); 14006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeJacErrorPoint(numImages, DerivPoint, errorProjPoints, pointsPres, jacPointErr); 14016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#ifdef TRACK_BUNDLE 14036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE *file; 14056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen( TRACK_BUNDLE_FILE ,"a"); 14066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n----- test 6 do main -----\n"); 14076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double norm1 = cvNorm(vectorX_points4D); 14086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file," test 6.02 post normPnts=%lf\n",norm1); 14096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 14106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 14126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy matrices Uk to work matrices Uk */ 14136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 14146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(matrsUk[currImage],workMatrsUk[currImage]); 14166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#ifdef TRACK_BUNDLE 14196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE *file; 14216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen( TRACK_BUNDLE_FILE ,"a"); 14226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n----- test 60.3 do matrs U V -----\n"); 14236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 14256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numImages; i++ ) 14266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double norm = cvNorm(matrsUk[i]); 14286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file," test 6.01 post1 matrsUk=%lf\n",norm); 14296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 14326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double norm = cvNorm(matrsVi[i]); 14346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file," test 6.01 post1 matrsVi=%lf\n",norm); 14356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 14386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 14406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ========== Solve normal equation for given alpha and Jacobian ============ */ 14426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn do 14446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ---- Add alpha to matrices --- */ 14466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Add alpha to matrInvVi and make workMatrsInvVi */ 14476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currV; 14496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currV = 0; currV < numPoints; currV++ ) 14506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(matrsVi[currV],workMatrVi); 14526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int i = 0; i < 4; i++ ) 14546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(workMatrVi,i,i,cvmGet(matrsVi[currV],i,i)*(1+alpha) ); 14566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvInvert(workMatrVi,workMatrsInvVi[currV],CV_LU/*,&currV*/); 14596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Add alpha to matrUk and make matrix workMatrsUk */ 14626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage< numImages; currImage++ ) 14636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 12; i++ ) 14666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(workMatrsUk[currImage],i,i,cvmGet(matrsUk[currImage],i,i)*(1+alpha)); 14686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill matrix to make system for computing delta P (matrTmpSys1 = inv(V)*tr(W) )*/ 14746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currV = 0; currV < numPoints; currV++ ) 14756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currRowV; 14776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currRowV = 0; currRowV < 4; currRowV++ ) 14786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 14806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int currCol = 0; currCol < 12; currCol++ )/* For each column of transposed matrix W */ 14826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double sum = 0; 14846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 4; i++ ) 14856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum += cvmGet(workMatrsInvVi[currV],currRowV,i) * 14876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmGet(matrW,currImage*12+currCol,currV*4+i); 14886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrTmpSys1,currV*4+currRowV,currImage*12+currCol,sum); 14906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Fill matrix to make system for computing delta P (matrTmpSys2 = W * matrTmpSys ) */ 14976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul(matrW,matrTmpSys1,matrSysDeltaP); 14986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* need to compute U-matrTmpSys2. But we compute matTmpSys2-U */ 15006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 15016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat subMatr; 15036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect(matrSysDeltaP,&subMatr,cvRect(currImage*12,currImage*12,12,12)); 15046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSub(&subMatr,workMatrsUk[currImage],&subMatr); 15056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute right side of normal equation */ 15086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currV = 0; currV < numPoints; currV++ ) 15096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat subMatrErPnts; 15116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat subMatr; 15126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect(jacPointErr,&subMatrErPnts,cvRect(0,currV*4,1,4)); 15136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect(vectTmpSys3,&subMatr,cvRect(0,currV*4,1,4)); 15146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul(workMatrsInvVi[currV],&subMatrErPnts,&subMatr); 15156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul(matrW,vectTmpSys3,vectSysDeltaP); 15186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSub(vectSysDeltaP,jacProjErr,vectSysDeltaP); 15196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Now we can compute part of normal system - deltaP */ 15216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSolve(matrSysDeltaP ,vectSysDeltaP, deltaP, CV_SVD); 15226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Print deltaP to file */ 15246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#ifdef TRACK_BUNDLE 15266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE* file; 15286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen( TRACK_BUNDLE_FILE_DELTAP ,"w"); 15296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 15316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 15326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\nImage=%d\n",currImage); 15346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 15356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 12; i++ ) 15366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double val; 15386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn val = cvmGet(deltaP,currImage*12+i,0); 15396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"%lf\n",val); 15406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n"); 15426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 15446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 15466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We know deltaP and now we can compute system for deltaM */ 15476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints * 4; i++ ) 15486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double sum = 0; 15506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int j = 0; j < numImages * 12; j++ ) 15516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum += cvmGet(matrW,j,i) * cvmGet(deltaP,j,0); 15536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(vectTmpSysM,i,0,cvmGet(jacPointErr,i,0)-sum); 15556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute deltaM */ 15586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currV = 0; currV < numPoints; currV++ ) 15596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat subMatr; 15616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat subMatrM; 15626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect(vectTmpSysM,&subMatr,cvRect(0,currV*4,1,4)); 15636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect(deltaM,&subMatrM,cvRect(0,currV*4,1,4)); 15646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul(workMatrsInvVi[currV],&subMatr,&subMatrM); 15656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We know delta and compute new value of vector X: nextVectX = vectX + deltas */ 15686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute new P */ 15706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 15716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ ) 15736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int j = 0; j < 4; j++ ) 15756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(newVectorX_projMatrs[currImage],i,j, 15776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmGet(vectorX_projMatrs[currImage],i,j) + cvmGet(deltaP,currImage*12+i*4+j,0)); 15786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute new M */ 15836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currPoint; 15846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currPoint = 0; currPoint < numPoints; currPoint++ ) 15856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 4; i++ ) 15876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(newVectorX_points4D,i,currPoint, 15896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmGet(vectorX_points4D,i,currPoint) + cvmGet(deltaM,currPoint*4+i,0)); 15906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- Compute errors for new vectorX ----- */ 15946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Project points using new vectorX and status of each point */ 15956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvProjPointsStatusFunc(numImages, newVectorX_points4D, newVectorX_projMatrs, pointsPres, projVisPoints); 15966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute error with observed value and computed projection */ 15976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double newError = 0; 15986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 15996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSub(observVisPoints[currImage],projVisPoints[currImage],errorProjPoints[currImage]); 16016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double currNorm = cvNorm(errorProjPoints[currImage]); 16026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//#ifdef TRACK_BUNDLE 16046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 1 16056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE *file; 16076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen( TRACK_BUNDLE_FILE ,"a"); 16086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n----- test 13,01 currImage=%d currNorm=%lf -----\n",currImage,currNorm); 16096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 16106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 16126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newError += currNorm * currNorm; 16136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newError = sqrt(newError); 16156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currIter++; 16176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//#ifdef TRACK_BUNDLE 16226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 1 16236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create file to track */ 16256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE* file; 16266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen( TRACK_BUNDLE_FILE ,"a"); 16276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n========================================\n"); 16286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"numPoints=%d\n",numPoints); 16296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"Iter=%d\n",currIter); 16306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"Error = %20.15lf\n",newError); 16316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"Change = %20.15lf\n",change); 16326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Print all projection errors */ 16356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 0 16366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"projection errors\n"); 16376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 16386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++) 16396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\nImage=%d\n",currImage); 16416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPn = errorProjPoints[currImage]->cols; 16426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int currPoint = 0; currPoint < numPn; currPoint++ ) 16436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double ex,ey; 16456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ex = cvmGet(errorProjPoints[currImage],0,currPoint); 16466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ey = cvmGet(errorProjPoints[currImage],1,currPoint); 16476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"%lf,%lf\n",ex,ey); 16486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n---- test 0 -----\n"); 16516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 16526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 16546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 16566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compare new error and last error */ 16606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( newError < prevError ) 16616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* accept new value */ 16626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prevError = newError; 16636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute relative change of required parameter vectorX. change = norm(curr-prev) / norm(curr) ) */ 16646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double normAll1 = 0; 16666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double normAll2 = 0; 16676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double currNorm1 = 0; 16686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double currNorm2 = 0; 16696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* compute norm for projection matrices */ 16706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 16716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currNorm1 = cvNorm(newVectorX_projMatrs[currImage],vectorX_projMatrs[currImage]); 16736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currNorm2 = cvNorm(newVectorX_projMatrs[currImage]); 16746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn normAll1 += currNorm1 * currNorm1; 16766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn normAll2 += currNorm2 * currNorm2; 16776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* compute norm for points */ 16806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currNorm1 = cvNorm(newVectorX_points4D,vectorX_points4D); 16816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currNorm2 = cvNorm(newVectorX_points4D); 16826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn normAll1 += currNorm1 * currNorm1; 16846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn normAll2 += currNorm2 * currNorm2; 16856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* compute change */ 16876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn change = sqrt(normAll1) / sqrt(normAll2); 16886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//#ifdef TRACK_BUNDLE 16916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 1 16926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create file to track */ 16946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE* file; 16956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen( TRACK_BUNDLE_FILE ,"a"); 16966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\nChange inside newVal change = %20.15lf\n",change); 16976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file," normAll1= %20.15lf\n",sqrt(normAll1)); 16986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file," normAll2= %20.15lf\n",sqrt(normAll2)); 16996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 17016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 17036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn alpha /= 10; 17076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 17086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(newVectorX_projMatrs[currImage],vectorX_projMatrs[currImage]); 17106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(newVectorX_points4D,vectorX_points4D); 17126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 17146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 17166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn alpha *= 10; 17186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } while( change > epsilon && currIter < maxIter );/* solve normal equation using current alpha */ 17216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//#ifdef TRACK_BUNDLE 17236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 1 17246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE* file; 17266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen( TRACK_BUNDLE_FILE ,"a"); 17276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\nBest error = %40.35lf\n",prevError); 17286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 17296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 17326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } while( change > epsilon && currIter < maxIter ); 17356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /*--------------------------------------------*/ 17376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Optimization complete copy computed params */ 17386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy projection matrices */ 17396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 17406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(newVectorX_projMatrs[currImage],resultProjMatrs[currImage]); 17426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy 4D points */ 17446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(newVectorX_points4D,resultPoints4D); 17456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// free(memory); 17476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 17496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Free allocated memory */ 17516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Free simple matrices */ 17536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&vectorX_points4D); 17546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&newVectorX_points4D); 17556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&changeVectorX_points4D); 17566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&changeVectorX_projMatrs); 17576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&matrW); 17586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&workMatrVi); 17596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&jacProjErr); 17606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&jacPointErr); 17616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&matrTmpSys1); 17626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&matrSysDeltaP); 17636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&vectTmpSys3); 17646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&vectSysDeltaP); 17656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&deltaP); 17666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&deltaM); 17676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&vectTmpSysM); 17686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Free arrays of matrices */ 17706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFreeMatrixArray(&vectorX_projMatrs,numImages); 17716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFreeMatrixArray(&newVectorX_projMatrs,numImages); 17726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFreeMatrixArray(&observVisPoints,numImages); 17736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFreeMatrixArray(&projVisPoints,numImages); 17746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFreeMatrixArray(&errorProjPoints,numImages); 17756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFreeMatrixArray(&DerivProj,numImages); 17766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFreeMatrixArray(&DerivPoint,numImages); 17776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFreeMatrixArray(&matrsUk,numImages); 17786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFreeMatrixArray(&workMatrsUk,numImages); 17796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFreeMatrixArray(&matrsVi,numPoints); 17806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFreeMatrixArray(&workMatrsInvVi,numPoints); 17816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 17836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1784