X-Git-Url: http://git.maemo.org/git/?a=blobdiff_plain;ds=sidebyside;f=src%2Fcvaux%2Fcvlevmartrif.cpp;fp=src%2Fcvaux%2Fcvlevmartrif.cpp;h=1d0b13d7e9cffa02946c59e7ff60559325716330;hb=e4c14cdbdf2fe805e79cd96ded236f57e7b89060;hp=0000000000000000000000000000000000000000;hpb=454138ff8a20f6edb9b65a910101403d8b520643;p=opencv diff --git a/src/cvaux/cvlevmartrif.cpp b/src/cvaux/cvlevmartrif.cpp new file mode 100644 index 0000000..1d0b13d --- /dev/null +++ b/src/cvaux/cvlevmartrif.cpp @@ -0,0 +1,497 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// Intel License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000, Intel Corporation, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of Intel Corporation may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + +#include "_cvaux.h" + +#include "cvtypes.h" +#include +#include +#include "cv.h" + +/* Valery Mosyagin */ + +typedef void (*pointer_LMJac)( const CvMat* src, CvMat* dst ); +typedef void (*pointer_LMFunc)( const CvMat* src, CvMat* dst ); + +void cvLevenbergMarquardtOptimization(pointer_LMJac JacobianFunction, + pointer_LMFunc function, + /*pointer_Err error_function,*/ + CvMat *X0,CvMat *observRes,CvMat *resultX, + int maxIter,double epsilon); + +void icvReconstructPointsFor3View( CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3, + CvMat* projPoints1,CvMat* projPoints2,CvMat* projPoints3, + CvMat* points4D); + + +/* Jacobian computation for trifocal case */ +void icvJacobianFunction_ProjTrifocal(const CvMat *vectX,CvMat *Jacobian) +{ + CV_FUNCNAME( "icvJacobianFunction_ProjTrifocal" ); + __BEGIN__; + + /* Test data for errors */ + if( vectX == 0 || Jacobian == 0 ) + { + CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); + } + + if( !CV_IS_MAT(vectX) || !CV_IS_MAT(Jacobian) ) + { + CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); + } + + int numPoints; + numPoints = (vectX->rows - 36)/4; + + if( numPoints < 1 )//!!! Need to correct this minimal number of points + { + CV_ERROR( CV_StsUnmatchedSizes, "number of points must be more than 0" ); + } + + if( Jacobian->rows == numPoints*6 || Jacobian->cols != 36+numPoints*4 ) + { + CV_ERROR( CV_StsUnmatchedSizes, "Size of Jacobian is not correct it must be 6*numPoints x (36+numPoints*4)" ); + } + + /* Computed Jacobian in a given point */ + /* This is for function with 3 projection matrices */ + /* vector X consists of projection matrices and points3D */ + /* each 3D points has X,Y,Z,W */ + /* each projection matrices has 3x4 coeffs */ + /* For N points 4D we have Jacobian 2N x (12*3+4N) */ + + /* Will store derivates as */ + /* Fill Jacobian matrix */ + int currProjPoint; + int currMatr; + + cvZero(Jacobian); + for( currMatr = 0; currMatr < 3; currMatr++ ) + { + double p[12]; + for( int i=0;i<12;i++ ) + { + p[i] = cvmGet(vectX,currMatr*12+i,0); + } + + int currVal = 36; + for( currProjPoint = 0; currProjPoint < numPoints; currProjPoint++ ) + { + /* Compute */ + double X[4]; + X[0] = cvmGet(vectX,currVal++,0); + X[1] = cvmGet(vectX,currVal++,0); + X[2] = cvmGet(vectX,currVal++,0); + X[3] = cvmGet(vectX,currVal++,0); + + double piX[3]; + piX[0] = X[0]*p[0] + X[1]*p[1] + X[2]*p[2] + X[3]*p[3]; + piX[1] = X[0]*p[4] + X[1]*p[5] + X[2]*p[6] + X[3]*p[7]; + piX[2] = X[0]*p[8] + X[1]*p[9] + X[2]*p[10] + X[3]*p[11]; + + int i,j; + /* fill derivate by point */ + + double tmp3 = 1/(piX[2]*piX[2]); + + double tmp1 = -piX[0]*tmp3; + double tmp2 = -piX[1]*tmp3; + for( j = 0; j < 2; j++ )//for x and y + { + for( i = 0; i < 4; i++ )// for X,Y,Z,W + { + cvmSet( Jacobian, + currMatr*numPoints*2+currProjPoint*2+j, 36+currProjPoint*4+i, + (p[j*4+i]*piX[2]-p[8+i]*piX[j]) * tmp3 ); + } + } + /* fill derivate by projection matrix */ + for( i = 0; i < 4; i++ ) + { + /* derivate for x */ + cvmSet(Jacobian,currMatr*numPoints*2+currProjPoint*2,currMatr*12+i,X[i]/piX[2]);//x' p1i + cvmSet(Jacobian,currMatr*numPoints*2+currProjPoint*2,currMatr*12+8+i,X[i]*tmp1);//x' p3i + + /* derivate for y */ + cvmSet(Jacobian,currMatr*numPoints*2+currProjPoint*2+1,currMatr*12+4+i,X[i]/piX[2]);//y' p2i + cvmSet(Jacobian,currMatr*numPoints*2+currProjPoint*2+1,currMatr*12+8+i,X[i]*tmp2);//y' p3i + } + + } + } + + __END__; + return; +} + +void icvFunc_ProjTrifocal(const CvMat *vectX, CvMat *resFunc) +{ + /* Computes function in a given point */ + /* Computers project points using 3 projection matrices and points 3D */ + + /* vector X consists of projection matrices and points3D */ + /* each projection matrices has 3x4 coeffs */ + /* each 3D points has X,Y,Z,W(?) */ + + /* result of function is projection of N 3D points using 3 projection matrices */ + /* projected points store as (projection by matrix P1),(projection by matrix P2),(projection by matrix P3) */ + /* each projection is x1,y1,x2,y2,x3,y3,x4,y4 */ + + /* Compute projection of points */ + + /* Fill projection matrices */ + + CV_FUNCNAME( "icvFunc_ProjTrifocal" ); + __BEGIN__; + + /* Test data for errors */ + if( vectX == 0 || resFunc == 0 ) + { + CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); + } + + if( !CV_IS_MAT(vectX) || !CV_IS_MAT(resFunc) ) + { + CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); + } + + int numPoints; + numPoints = (vectX->rows - 36)/4; + + if( numPoints < 1 )//!!! Need to correct this minimal number of points + { + CV_ERROR( CV_StsUnmatchedSizes, "number of points must be more than 0" ); + } + + if( resFunc->rows == 2*numPoints*3 || resFunc->cols != 1 ) + { + CV_ERROR( CV_StsUnmatchedSizes, "Size of resFunc is not correct it must be 2*numPoints*3 x 1"); + } + + + CvMat projMatrs[3]; + double projMatrs_dat[36]; + projMatrs[0] = cvMat(3,4,CV_64F,projMatrs_dat); + projMatrs[1] = cvMat(3,4,CV_64F,projMatrs_dat+12); + projMatrs[2] = cvMat(3,4,CV_64F,projMatrs_dat+24); + + CvMat point3D; + double point3D_dat[3]; + point3D = cvMat(3,1,CV_64F,point3D_dat); + + int currMatr; + int currV; + int i,j; + + currV=0; + for( currMatr = 0; currMatr < 3; currMatr++ ) + { + for( i = 0; i < 3; i++ ) + { + for( j = 0;j < 4; j++ ) + { + double val = cvmGet(vectX,currV,0); + cvmSet(&projMatrs[currMatr],i,j,val); + currV++; + } + } + } + + /* Project points */ + int currPoint; + CvMat point4D; + double point4D_dat[4]; + point4D = cvMat(4,1,CV_64F,point4D_dat); + for( currPoint = 0; currPoint < numPoints; currPoint++ ) + { + /* get curr point */ + point4D_dat[0] = cvmGet(vectX,currV++,0); + point4D_dat[1] = cvmGet(vectX,currV++,0); + point4D_dat[2] = cvmGet(vectX,currV++,0); + point4D_dat[3] = cvmGet(vectX,currV++,0); + + for( currMatr = 0; currMatr < 3; currMatr++ ) + { + /* Compute projection for current point */ + cvmMul(&projMatrs[currMatr],&point4D,&point3D); + double z = point3D_dat[2]; + cvmSet(resFunc,currMatr*numPoints*2 + currPoint*2, 0,point3D_dat[0]/z); + cvmSet(resFunc,currMatr*numPoints*2 + currPoint*2+1,0,point3D_dat[1]/z); + } + } + + __END__; + return; +} + + +/*----------------------------------------------------------------------------------------*/ + +void icvOptimizeProjectionTrifocal(CvMat **projMatrs,CvMat **projPoints, + CvMat **resultProjMatrs, CvMat *resultPoints4D) +{ + + CvMat *optimX = 0; + CvMat *points4D = 0; + CvMat *vectorX0 = 0; + CvMat *observRes = 0; + //CvMat *error = 0; + + CV_FUNCNAME( "icvOptimizeProjectionTrifocal" ); + __BEGIN__; + + /* Test data for errors */ + if( projMatrs == 0 || projPoints == 0 || resultProjMatrs == 0 || resultPoints4D == 0) + { + CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); + } + + if( !CV_IS_MAT(resultPoints4D) ) + { + CV_ERROR( CV_StsUnsupportedFormat, "resultPoints4D must be a matrix" ); + } + + int numPoints; + numPoints = resultPoints4D->cols; + if( numPoints < 1 ) + { + CV_ERROR( CV_StsOutOfRange, "Number points of resultPoints4D must be more than 0" ); + } + + if( resultPoints4D->rows != 4 ) + { + CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of points4D must be 4" ); + } + + int i; + for( i = 0; i < 3; i++ ) + { + if( projMatrs[i] == 0 ) + { + CV_ERROR( CV_StsNullPtr, "Some of projMatrs is a NULL pointer" ); + } + + if( projPoints[i] == 0 ) + { + CV_ERROR( CV_StsNullPtr, "Some of projPoints is a NULL pointer" ); + } + + if( resultProjMatrs[i] == 0 ) + { + CV_ERROR( CV_StsNullPtr, "Some of resultProjMatrs is a NULL pointer" ); + } + + /* ----------- test for matrix ------------- */ + if( !CV_IS_MAT(projMatrs[i]) ) + { + CV_ERROR( CV_StsUnsupportedFormat, "Each of projMatrs must be a matrix" ); + } + + if( !CV_IS_MAT(projPoints[i]) ) + { + CV_ERROR( CV_StsUnsupportedFormat, "Each of projPoints must be a matrix" ); + } + + if( !CV_IS_MAT(resultProjMatrs[i]) ) + { + CV_ERROR( CV_StsUnsupportedFormat, "Each of resultProjMatrs must be a matrix" ); + } + + /* ------------- Test sizes --------------- */ + if( projMatrs[i]->rows != 3 || projMatrs[i]->cols != 4 ) + { + CV_ERROR( CV_StsUnmatchedSizes, "Size of projMatr must be 3x4" ); + } + + if( projPoints[i]->rows != 2 || projPoints[i]->cols != numPoints ) + { + CV_ERROR( CV_StsUnmatchedSizes, "Size of resultProjMatrs must be 3x4" ); + } + + if( resultProjMatrs[i]->rows != 3 || resultProjMatrs[i]->cols != 4 ) + { + CV_ERROR( CV_StsUnmatchedSizes, "Size of resultProjMatrs must be 3x4" ); + } + } + + + /* Allocate memory for points 4D */ + CV_CALL( points4D = cvCreateMat(4,numPoints,CV_64F) ); + CV_CALL( vectorX0 = cvCreateMat(36 + numPoints*4,1,CV_64F) ); + CV_CALL( observRes = cvCreateMat(2*numPoints*3,1,CV_64F) ); + CV_CALL( optimX = cvCreateMat(36+numPoints*4,1,CV_64F) ); + //CV_CALL( error = cvCreateMat(numPoints*2*3,1,CV_64F) ); + + + /* Reconstruct points 4D using projected points and projection matrices */ + icvReconstructPointsFor3View( projMatrs[0],projMatrs[1],projMatrs[2], + projPoints[0],projPoints[1],projPoints[2], + points4D); + + + + /* Fill observed points on images */ + /* result of function is projection of N 3D points using 3 projection matrices */ + /* projected points store as (projection by matrix P1),(projection by matrix P2),(projection by matrix P3) */ + /* each projection is x1,y1,x2,y2,x3,y3,x4,y4 */ + int currMatr; + for( currMatr = 0; currMatr < 3; currMatr++ ) + { + for( i = 0; i < numPoints; i++ ) + { + cvmSet(observRes,currMatr*numPoints*2+i*2 ,0,cvmGet(projPoints[currMatr],0,i) );/* x */ + cvmSet(observRes,currMatr*numPoints*2+i*2+1,0,cvmGet(projPoints[currMatr],1,i) );/* y */ + } + } + + /* Fill with projection matrices */ + for( currMatr = 0; currMatr < 3; currMatr++ ) + { + int i; + for( i = 0; i < 12; i++ ) + { + cvmSet(vectorX0,currMatr*12+i,0,cvmGet(projMatrs[currMatr],i/4,i%4)); + } + } + + /* Fill with 4D points */ + + int currPoint; + for( currPoint = 0; currPoint < numPoints; currPoint++ ) + { + cvmSet(vectorX0,36 + currPoint*4 + 0,0,cvmGet(points4D,0,currPoint)); + cvmSet(vectorX0,36 + currPoint*4 + 1,0,cvmGet(points4D,1,currPoint)); + cvmSet(vectorX0,36 + currPoint*4 + 2,0,cvmGet(points4D,2,currPoint)); + cvmSet(vectorX0,36 + currPoint*4 + 3,0,cvmGet(points4D,3,currPoint)); + } + + + /* Allocate memory for result */ + cvLevenbergMarquardtOptimization( icvJacobianFunction_ProjTrifocal, icvFunc_ProjTrifocal, + vectorX0,observRes,optimX,100,1e-6); + + /* Copy results */ + for( currMatr = 0; currMatr < 3; currMatr++ ) + { + /* Copy projection matrices */ + for(int i=0;i<12;i++) + { + cvmSet(resultProjMatrs[currMatr],i/4,i%4,cvmGet(optimX,currMatr*12+i,0)); + } + } + + /* Copy 4D points */ + for( currPoint = 0; currPoint < numPoints; currPoint++ ) + { + cvmSet(resultPoints4D,0,currPoint,cvmGet(optimX,36 + currPoint*4,0)); + cvmSet(resultPoints4D,1,currPoint,cvmGet(optimX,36 + currPoint*4+1,0)); + cvmSet(resultPoints4D,2,currPoint,cvmGet(optimX,36 + currPoint*4+2,0)); + cvmSet(resultPoints4D,3,currPoint,cvmGet(optimX,36 + currPoint*4+3,0)); + } + + __END__; + + /* Free allocated memory */ + cvReleaseMat(&optimX); + cvReleaseMat(&points4D); + cvReleaseMat(&vectorX0); + cvReleaseMat(&observRes); + + return; + + +} + +/*------------------------------------------------------------------------------*/ +/* Create good points using status information */ +void icvCreateGoodPoints(CvMat *points,CvMat **goodPoints, CvMat *status) +{ + *goodPoints = 0; + + CV_FUNCNAME( "icvCreateGoodPoints" ); + __BEGIN__; + + int numPoints; + numPoints = points->cols; + + if( numPoints < 1 ) + { + CV_ERROR( CV_StsOutOfRange, "Number of points must be more than 0" ); + } + + int numCoord; + numCoord = points->rows; + if( numCoord < 1 ) + { + CV_ERROR( CV_StsOutOfRange, "Number of points coordinates must be more than 0" ); + } + + /* Define number of good points */ + int goodNum; + int i,j; + + goodNum = 0; + for( i = 0; i < numPoints; i++) + { + if( cvmGet(status,0,i) > 0 ) + goodNum++; + } + + /* Allocate memory for good points */ + CV_CALL( *goodPoints = cvCreateMat(numCoord,goodNum,CV_64F) ); + + for( i = 0; i < numCoord; i++ ) + { + int currPoint = 0; + for( j = 0; j < numPoints; j++) + { + if( cvmGet(status,0,j) > 0 ) + { + cvmSet(*goodPoints,i,currPoint,cvmGet(points,i,j)); + currPoint++; + } + } + } + __END__; + return; +} +