| /*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 <float.h> |
| #include <limits.h> |
| //#include "cv.h" |
| |
| #include <stdio.h> |
| |
| void icvReconstructPoints4DStatus(CvMat** projPoints, CvMat **projMatrs, CvMat** presPoints, CvMat *points4D,int numImages,CvMat **projError=0); |
| |
| /* Valery Mosyagin */ |
| |
| /* If you want to save internal debug info to files uncomment next lines and set paths to files if need */ |
| /* Note these file may be very large */ |
| /* |
| #define TRACK_BUNDLE |
| #define TRACK_BUNDLE_FILE "d:\\test\\bundle.txt" |
| #define TRACK_BUNDLE_FILE_JAC "d:\\test\\bundle.txt" |
| #define TRACK_BUNDLE_FILE_JACERRPROJ "d:\\test\\JacErrProj.txt" |
| #define TRACK_BUNDLE_FILE_JACERRPNT "d:\\test\\JacErrPoint.txt" |
| #define TRACK_BUNDLE_FILE_MATRW "d:\\test\\matrWt.txt" |
| #define TRACK_BUNDLE_FILE_DELTAP "d:\\test\\deltaP.txt" |
| */ |
| #define TRACK_BUNDLE_FILE "d:\\test\\bundle.txt" |
| |
| |
| /* ============== Bundle adjustment optimization ================= */ |
| void icvComputeDerivateProj(CvMat *points4D,CvMat *projMatr, CvMat *status, CvMat *derivProj) |
| { |
| /* Compute derivate for given projection matrix points and status of points */ |
| |
| CV_FUNCNAME( "icvComputeDerivateProj" ); |
| __BEGIN__; |
| |
| |
| /* ----- Test input params for errors ----- */ |
| if( points4D == 0 || projMatr == 0 || status == 0 || derivProj == 0) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| |
| if( !CV_IS_MAT(points4D) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "points4D must be a matrix 4xN" ); |
| } |
| |
| /* Compute number of points */ |
| int numPoints; |
| numPoints = points4D->cols; |
| |
| if( numPoints < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" ); |
| } |
| |
| if( points4D->rows != 4 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of coordinates of points4D must be 4" ); |
| } |
| |
| if( !CV_IS_MAT(projMatr) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "projMatr must be a matrix 3x4" ); |
| } |
| |
| if( projMatr->rows != 3 || projMatr->cols != 4 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Size of projection matrix (projMatr) must be 3x4" ); |
| } |
| |
| if( !CV_IS_MAT(status) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "Status must be a matrix 1xN" ); |
| } |
| |
| if( status->rows != 1 || status->cols != numPoints ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Size of status of points must be 1xN" ); |
| } |
| |
| if( !CV_IS_MAT(derivProj) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "derivProj must be a matrix VisN x 12" ); |
| } |
| |
| if( derivProj->cols != 12 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "derivProj must be a matrix VisN x 12" ); |
| } |
| /* ----- End test ----- */ |
| |
| int i; |
| |
| /* Allocate memory for derivates */ |
| |
| double p[12]; |
| /* Copy projection matrix */ |
| for( i = 0; i < 12; i++ ) |
| { |
| p[i] = cvmGet(projMatr,i/4,i%4); |
| } |
| |
| /* Fill deriv matrix */ |
| int currVisPoint; |
| int currPoint; |
| |
| currVisPoint = 0; |
| for( currPoint = 0; currPoint < numPoints; currPoint++ ) |
| { |
| if( cvmGet(status,0,currPoint) > 0 ) |
| { |
| double X[4]; |
| X[0] = cvmGet(points4D,0,currVisPoint); |
| X[1] = cvmGet(points4D,1,currVisPoint); |
| X[2] = cvmGet(points4D,2,currVisPoint); |
| X[3] = cvmGet(points4D,3,currVisPoint); |
| |
| /* Compute derivate for this point */ |
| |
| 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; |
| /* fill derivate by point */ |
| |
| double tmp3 = 1/(piX[2]*piX[2]); |
| |
| double tmp1 = -piX[0]*tmp3; |
| double tmp2 = -piX[1]*tmp3; |
| |
| /* fill derivate by projection matrix */ |
| for( i = 0; i < 4; i++ ) |
| { |
| /* derivate for x */ |
| cvmSet(derivProj,currVisPoint*2,i,X[i]/piX[2]);//x' p1i |
| cvmSet(derivProj,currVisPoint*2,4+i,0);//x' p1i |
| cvmSet(derivProj,currVisPoint*2,8+i,X[i]*tmp1);//x' p3i |
| |
| /* derivate for y */ |
| cvmSet(derivProj,currVisPoint*2+1,i,0);//y' p2i |
| cvmSet(derivProj,currVisPoint*2+1,4+i,X[i]/piX[2]);//y' p2i |
| cvmSet(derivProj,currVisPoint*2+1,8+i,X[i]*tmp2);//y' p3i |
| } |
| |
| currVisPoint++; |
| } |
| } |
| |
| if( derivProj->rows != currVisPoint * 2 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "derivProj must be a matrix 2VisN x 12" ); |
| } |
| |
| |
| __END__; |
| return; |
| } |
| /*======================================================================================*/ |
| |
| void icvComputeDerivateProjAll(CvMat *points4D, CvMat **projMatrs, CvMat **pointPres, int numImages,CvMat **projDerives) |
| { |
| CV_FUNCNAME( "icvComputeDerivateProjAll" ); |
| __BEGIN__; |
| |
| /* ----- Test input params for errors ----- */ |
| if( numImages < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); |
| } |
| if( projMatrs == 0 || pointPres == 0 || projDerives == 0 ) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| /* ----- End test ----- */ |
| |
| int currImage; |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| icvComputeDerivateProj(points4D,projMatrs[currImage], pointPres[currImage], projDerives[currImage]); |
| } |
| |
| __END__; |
| return; |
| } |
| /*======================================================================================*/ |
| |
| void icvComputeDerivatePoints(CvMat *points4D,CvMat *projMatr, CvMat *presPoints, CvMat *derivPoint) |
| { |
| |
| CV_FUNCNAME( "icvComputeDerivatePoints" ); |
| __BEGIN__; |
| |
| /* ----- Test input params for errors ----- */ |
| if( points4D == 0 || projMatr == 0 || presPoints == 0 || derivPoint == 0) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| |
| if( !CV_IS_MAT(points4D) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "points4D must be a matrix N x 4" ); |
| } |
| |
| int numPoints; |
| numPoints = presPoints->cols; |
| |
| if( numPoints < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of points must be more than zero" ); |
| } |
| |
| if( points4D->rows != 4 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "points4D must be a matrix N x 4" ); |
| } |
| |
| if( !CV_IS_MAT(projMatr) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "projMatr must be a matrix 3x4" ); |
| } |
| |
| if( projMatr->rows != 3 || projMatr->cols != 4 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Size of projection matrix (projMatr) must be 3x4" ); |
| } |
| |
| if( !CV_IS_MAT(presPoints) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "Status must be a matrix 1xN" ); |
| } |
| |
| if( presPoints->rows != 1 || presPoints->cols != numPoints ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Size of presPoints status must be 1xN" ); |
| } |
| |
| if( !CV_IS_MAT(derivPoint) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "derivPoint must be a matrix 2 x 4VisNum" ); |
| } |
| /* ----- End test ----- */ |
| |
| /* Compute derivates by points */ |
| |
| double p[12]; |
| int i; |
| for( i = 0; i < 12; i++ ) |
| { |
| p[i] = cvmGet(projMatr,i/4,i%4); |
| } |
| |
| int currVisPoint; |
| int currProjPoint; |
| |
| currVisPoint = 0; |
| for( currProjPoint = 0; currProjPoint < numPoints; currProjPoint++ ) |
| { |
| if( cvmGet(presPoints,0,currProjPoint) > 0 ) |
| { |
| double X[4]; |
| X[0] = cvmGet(points4D,0,currProjPoint); |
| X[1] = cvmGet(points4D,1,currProjPoint); |
| X[2] = cvmGet(points4D,2,currProjPoint); |
| X[3] = cvmGet(points4D,3,currProjPoint); |
| |
| 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; |
| |
| double tmp3 = 1/(piX[2]*piX[2]); |
| |
| for( j = 0; j < 2; j++ )//for x and y |
| { |
| for( i = 0; i < 4; i++ )// for X,Y,Z,W |
| { |
| cvmSet( derivPoint, |
| j, currVisPoint*4+i, |
| (p[j*4+i]*piX[2]-p[8+i]*piX[j]) * tmp3 ); |
| } |
| } |
| currVisPoint++; |
| } |
| } |
| |
| if( derivPoint->rows != 2 || derivPoint->cols != currVisPoint*4 ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "derivPoint must be a matrix 2 x 4VisNum" ); |
| } |
| |
| __END__; |
| return; |
| } |
| /*======================================================================================*/ |
| void icvComputeDerivatePointsAll(CvMat *points4D, CvMat **projMatrs, CvMat **pointPres, int numImages,CvMat **pointDerives) |
| { |
| CV_FUNCNAME( "icvComputeDerivatePointsAll" ); |
| __BEGIN__; |
| |
| /* ----- Test input params for errors ----- */ |
| if( numImages < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); |
| } |
| if( projMatrs == 0 || pointPres == 0 || pointDerives == 0 ) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| /* ----- End test ----- */ |
| |
| int currImage; |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| icvComputeDerivatePoints(points4D, projMatrs[currImage], pointPres[currImage], pointDerives[currImage]); |
| } |
| |
| __END__; |
| return; |
| } |
| /*======================================================================================*/ |
| void icvComputeMatrixVAll(int numImages,CvMat **pointDeriv,CvMat **presPoints, CvMat **matrV) |
| { |
| int *shifts = 0; |
| |
| CV_FUNCNAME( "icvComputeMatrixVAll" ); |
| __BEGIN__; |
| |
| /* ----- Test input params for errors ----- */ |
| if( numImages < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); |
| } |
| if( pointDeriv == 0 || presPoints == 0 || matrV == 0 ) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| /* !!! not tested all parameters */ |
| /* ----- End test ----- */ |
| |
| /* Compute all matrices U */ |
| int currImage; |
| int currPoint; |
| int numPoints; |
| numPoints = presPoints[0]->cols; |
| CV_CALL(shifts = (int*)cvAlloc(sizeof(int)*numImages)); |
| memset(shifts,0,sizeof(int)*numImages); |
| |
| for( currPoint = 0; currPoint < numPoints; currPoint++ )//For each point (matrix V) |
| { |
| int i,j; |
| |
| for( i = 0; i < 4; i++ ) |
| { |
| for( j = 0; j < 4; j++ ) |
| { |
| double sum = 0; |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) |
| { |
| sum += cvmGet(pointDeriv[currImage],0,shifts[currImage]*4+i) * |
| cvmGet(pointDeriv[currImage],0,shifts[currImage]*4+j); |
| |
| sum += cvmGet(pointDeriv[currImage],1,shifts[currImage]*4+i) * |
| cvmGet(pointDeriv[currImage],1,shifts[currImage]*4+j); |
| } |
| } |
| |
| cvmSet(matrV[currPoint],i,j,sum); |
| } |
| } |
| |
| |
| /* shift position of visible points */ |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) |
| { |
| shifts[currImage]++; |
| } |
| } |
| } |
| |
| __END__; |
| cvFree( &shifts); |
| |
| return; |
| } |
| /*======================================================================================*/ |
| void icvComputeMatrixUAll(int numImages,CvMat **projDeriv,CvMat** matrU) |
| { |
| CV_FUNCNAME( "icvComputeMatrixVAll" ); |
| __BEGIN__; |
| /* ----- Test input params for errors ----- */ |
| if( numImages < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); |
| } |
| if( projDeriv == 0 || matrU == 0 ) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| /* !!! Not tested all input parameters */ |
| /* ----- End test ----- */ |
| |
| /* Compute matrices V */ |
| int currImage; |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| cvMulTransposed(projDeriv[currImage],matrU[currImage],1); |
| } |
| |
| __END__; |
| return; |
| } |
| /*======================================================================================*/ |
| void icvComputeMatrixW(int numImages, CvMat **projDeriv, CvMat **pointDeriv, CvMat **presPoints, CvMat *matrW) |
| { |
| CV_FUNCNAME( "icvComputeMatrixW" ); |
| __BEGIN__; |
| |
| /* ----- Test input params for errors ----- */ |
| if( numImages < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); |
| } |
| if( projDeriv == 0 || pointDeriv == 0 || presPoints == 0 || matrW == 0 ) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| int numPoints; |
| numPoints = presPoints[0]->cols; |
| if( numPoints < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of points must more than zero" ); |
| } |
| if( !CV_IS_MAT(matrW) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "matrW must be a matrix 12NumIm x 4NumPnt" ); |
| } |
| if( matrW->rows != numImages*12 || matrW->cols != numPoints*4 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "matrW must be a matrix 12NumIm x 4NumPnt" ); |
| } |
| /* !!! Not tested all input parameters */ |
| /* ----- End test ----- */ |
| |
| /* Compute number of points */ |
| /* Compute matrix W using derivate proj and points */ |
| |
| int currImage; |
| |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| for( int currLine = 0; currLine < 12; currLine++ ) |
| { |
| int currVis = 0; |
| for( int currPoint = 0; currPoint < numPoints; currPoint++ ) |
| { |
| if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) |
| { |
| |
| for( int currCol = 0; currCol < 4; currCol++ ) |
| { |
| double sum; |
| sum = cvmGet(projDeriv[currImage],currVis*2+0,currLine) * |
| cvmGet(pointDeriv[currImage],0,currVis*4+currCol); |
| |
| sum += cvmGet(projDeriv[currImage],currVis*2+1,currLine) * |
| cvmGet(pointDeriv[currImage],1,currVis*4+currCol); |
| |
| cvmSet(matrW,currImage*12+currLine,currPoint*4+currCol,sum); |
| } |
| currVis++; |
| } |
| else |
| {/* set all sub elements to zero */ |
| for( int currCol = 0; currCol < 4; currCol++ ) |
| { |
| cvmSet(matrW,currImage*12+currLine,currPoint*4+currCol,0); |
| } |
| } |
| } |
| } |
| } |
| |
| #ifdef TRACK_BUNDLE |
| { |
| FILE *file; |
| file = fopen( TRACK_BUNDLE_FILE_MATRW ,"w"); |
| int currPoint,currImage; |
| for( currPoint = 0; currPoint < numPoints; currPoint++ ) |
| { |
| fprintf(file,"\nPoint=%d\n",currPoint); |
| int currRow; |
| for( currRow = 0; currRow < 4; currRow++ ) |
| { |
| for( currImage = 0; currImage< numImages; currImage++ ) |
| { |
| int i; |
| for( i = 0; i < 12; i++ ) |
| { |
| double val = cvmGet(matrW, currImage * 12 + i, currPoint * 4 + currRow); |
| fprintf(file,"%lf ",val); |
| } |
| } |
| fprintf(file,"\n"); |
| } |
| } |
| fclose(file); |
| } |
| #endif |
| |
| __END__; |
| return; |
| } |
| /*======================================================================================*/ |
| /* Compute jacobian mult projection matrices error */ |
| void icvComputeJacErrorProj(int numImages,CvMat **projDeriv,CvMat **projErrors,CvMat *jacProjErr ) |
| { |
| CV_FUNCNAME( "icvComputeJacErrorProj" ); |
| __BEGIN__; |
| |
| /* ----- Test input params for errors ----- */ |
| if( numImages < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); |
| } |
| if( projDeriv == 0 || projErrors == 0 || jacProjErr == 0 ) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| if( !CV_IS_MAT(jacProjErr) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "jacProjErr must be a matrix 12NumIm x 1" ); |
| } |
| if( jacProjErr->rows != numImages*12 || jacProjErr->cols != 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "jacProjErr must be a matrix 12NumIm x 1" ); |
| } |
| /* !!! Not tested all input parameters */ |
| /* ----- End test ----- */ |
| |
| int currImage; |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| for( int currCol = 0; currCol < 12; currCol++ ) |
| { |
| int num = projDeriv[currImage]->rows; |
| double sum = 0; |
| for( int i = 0; i < num; i++ ) |
| { |
| sum += cvmGet(projDeriv[currImage],i,currCol) * |
| cvmGet(projErrors[currImage],i%2,i/2); |
| } |
| cvmSet(jacProjErr,currImage*12+currCol,0,sum); |
| } |
| } |
| |
| #ifdef TRACK_BUNDLE |
| { |
| FILE *file; |
| file = fopen( TRACK_BUNDLE_FILE_JACERRPROJ ,"w"); |
| int currImage; |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| fprintf(file,"\nImage=%d\n",currImage); |
| int currRow; |
| for( currRow = 0; currRow < 12; currRow++ ) |
| { |
| double val = cvmGet(jacProjErr, currImage * 12 + currRow, 0); |
| fprintf(file,"%lf\n",val); |
| } |
| fprintf(file,"\n"); |
| } |
| fclose(file); |
| } |
| #endif |
| |
| |
| __END__; |
| return; |
| } |
| /*======================================================================================*/ |
| /* Compute jacobian mult points error */ |
| void icvComputeJacErrorPoint(int numImages,CvMat **pointDeriv,CvMat **projErrors, CvMat **presPoints,CvMat *jacPointErr ) |
| { |
| int *shifts = 0; |
| |
| CV_FUNCNAME( "icvComputeJacErrorPoint" ); |
| __BEGIN__; |
| |
| /* ----- Test input params for errors ----- */ |
| if( numImages < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); |
| } |
| |
| if( pointDeriv == 0 || projErrors == 0 || presPoints == 0 || jacPointErr == 0 ) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| |
| int numPoints; |
| numPoints = presPoints[0]->cols; |
| if( numPoints < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of points must more than zero" ); |
| } |
| |
| if( !CV_IS_MAT(jacPointErr) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "jacPointErr must be a matrix 4NumPnt x 1" ); |
| } |
| |
| if( jacPointErr->rows != numPoints*4 || jacPointErr->cols != 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "jacPointErr must be a matrix 4NumPnt x 1" ); |
| } |
| /* !!! Not tested all input parameters */ |
| /* ----- End test ----- */ |
| |
| int currImage; |
| int currPoint; |
| CV_CALL(shifts = (int*)cvAlloc(sizeof(int)*numImages)); |
| memset(shifts,0,sizeof(int)*numImages); |
| for( currPoint = 0; currPoint < numPoints; currPoint++ ) |
| { |
| for( int currCoord = 0; currCoord < 4; currCoord++ ) |
| { |
| double sum = 0; |
| { |
| int currVis = 0; |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) |
| { |
| sum += cvmGet(pointDeriv[currImage],0,shifts[currImage]*4+currCoord) * |
| cvmGet(projErrors[currImage],0,shifts[currImage]);//currVis); |
| |
| sum += cvmGet(pointDeriv[currImage],1,shifts[currImage]*4+currCoord) * |
| cvmGet(projErrors[currImage],1,shifts[currImage]);//currVis); |
| |
| currVis++; |
| } |
| } |
| } |
| |
| cvmSet(jacPointErr,currPoint*4+currCoord,0,sum); |
| } |
| |
| /* Increase shifts */ |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) |
| { |
| shifts[currImage]++; |
| } |
| } |
| } |
| |
| |
| #ifdef TRACK_BUNDLE |
| { |
| FILE *file; |
| file = fopen(TRACK_BUNDLE_FILE_JACERRPNT,"w"); |
| int currPoint; |
| for( currPoint = 0; currPoint < numPoints; currPoint++ ) |
| { |
| fprintf(file,"\nPoint=%d\n",currPoint); |
| int currRow; |
| for( currRow = 0; currRow < 4; currRow++ ) |
| { |
| double val = cvmGet(jacPointErr, currPoint * 4 + currRow, 0); |
| fprintf(file,"%lf\n",val); |
| } |
| fprintf(file,"\n"); |
| } |
| fclose(file); |
| } |
| #endif |
| |
| |
| |
| __END__; |
| cvFree( &shifts); |
| |
| } |
| /*======================================================================================*/ |
| |
| /* Reconstruct 4D points using status */ |
| void icvReconstructPoints4DStatus(CvMat** projPoints, CvMat **projMatrs, CvMat** presPoints, |
| CvMat *points4D,int numImages,CvMat **projError) |
| { |
| |
| double* matrA_dat = 0; |
| double* matrW_dat = 0; |
| |
| CV_FUNCNAME( "icvReconstructPoints4DStatus" ); |
| __BEGIN__; |
| |
| /* ----- Test input params for errors ----- */ |
| if( numImages < 2 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of images must be more than one" ); |
| } |
| |
| if( projPoints == 0 || projMatrs == 0 || presPoints == 0 || points4D == 0 ) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| |
| int numPoints; |
| numPoints = points4D->cols; |
| if( numPoints < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" ); |
| } |
| |
| if( points4D->rows != 4 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Points must have 4 cordinates" ); |
| } |
| |
| /* !!! Not tested all input parameters */ |
| /* ----- End test ----- */ |
| |
| int currImage; |
| int currPoint; |
| |
| /* Allocate maximum data */ |
| |
| |
| CvMat matrV; |
| double matrV_dat[4*4]; |
| matrV = cvMat(4,4,CV_64F,matrV_dat); |
| |
| CV_CALL(matrA_dat = (double*)cvAlloc(3*numImages * 4 * sizeof(double))); |
| CV_CALL(matrW_dat = (double*)cvAlloc(3*numImages * 4 * sizeof(double))); |
| |
| /* reconstruct each point */ |
| for( currPoint = 0; currPoint < numPoints; currPoint++ ) |
| { |
| /* Reconstruct current point */ |
| /* Define number of visible projections */ |
| int numVisProj = 0; |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) |
| { |
| numVisProj++; |
| } |
| } |
| |
| if( numVisProj < 2 ) |
| { |
| /* This point can't be reconstructed */ |
| continue; |
| } |
| |
| /* Allocate memory and create matrices */ |
| CvMat matrA; |
| matrA = cvMat(3*numVisProj,4,CV_64F,matrA_dat); |
| |
| CvMat matrW; |
| matrW = cvMat(3*numVisProj,4,CV_64F,matrW_dat); |
| |
| int currVisProj = 0; |
| for( currImage = 0; currImage < numImages; currImage++ )/* For each view */ |
| { |
| if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) |
| { |
| double x,y; |
| x = cvmGet(projPoints[currImage],0,currPoint); |
| y = cvmGet(projPoints[currImage],1,currPoint); |
| for( int k = 0; k < 4; k++ ) |
| { |
| matrA_dat[currVisProj*12 + k] = |
| x * cvmGet(projMatrs[currImage],2,k) - cvmGet(projMatrs[currImage],0,k); |
| |
| matrA_dat[currVisProj*12+4 + k] = |
| y * cvmGet(projMatrs[currImage],2,k) - cvmGet(projMatrs[currImage],1,k); |
| |
| matrA_dat[currVisProj*12+8 + k] = |
| x * cvmGet(projMatrs[currImage],1,k) - y * cvmGet(projMatrs[currImage],0,k); |
| } |
| currVisProj++; |
| } |
| } |
| |
| /* Solve system for current point */ |
| { |
| cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T); |
| |
| /* Copy computed point */ |
| cvmSet(points4D,0,currPoint,cvmGet(&matrV,3,0));//X |
| cvmSet(points4D,1,currPoint,cvmGet(&matrV,3,1));//Y |
| cvmSet(points4D,2,currPoint,cvmGet(&matrV,3,2));//Z |
| cvmSet(points4D,3,currPoint,cvmGet(&matrV,3,3));//W |
| } |
| |
| } |
| |
| {/* Compute projection error */ |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| CvMat point4D; |
| CvMat point3D; |
| double point3D_dat[3]; |
| point3D = cvMat(3,1,CV_64F,point3D_dat); |
| |
| int currPoint; |
| int numVis = 0; |
| double totalError = 0; |
| for( currPoint = 0; currPoint < numPoints; currPoint++ ) |
| { |
| if( cvmGet(presPoints[currImage],0,currPoint) > 0) |
| { |
| double dx,dy; |
| cvGetCol(points4D,&point4D,currPoint); |
| cvmMul(projMatrs[currImage],&point4D,&point3D); |
| double w = point3D_dat[2]; |
| double x = point3D_dat[0] / w; |
| double y = point3D_dat[1] / w; |
| |
| dx = cvmGet(projPoints[currImage],0,currPoint) - x; |
| dy = cvmGet(projPoints[currImage],1,currPoint) - y; |
| if( projError ) |
| { |
| cvmSet(projError[currImage],0,currPoint,dx); |
| cvmSet(projError[currImage],1,currPoint,dy); |
| } |
| totalError += sqrt(dx*dx+dy*dy); |
| numVis++; |
| } |
| } |
| |
| //double meanError = totalError / (double)numVis; |
| |
| } |
| |
| } |
| |
| __END__; |
| |
| cvFree( &matrA_dat); |
| cvFree( &matrW_dat); |
| |
| return; |
| } |
| |
| /*======================================================================================*/ |
| |
| void icvProjPointsStatusFunc( int numImages, CvMat *points4D, CvMat **projMatrs, CvMat **pointsPres, CvMat **projPoints) |
| { |
| CV_FUNCNAME( "icvProjPointsStatusFunc" ); |
| __BEGIN__; |
| |
| /* ----- Test input params for errors ----- */ |
| if( numImages < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of images must be more than zero" ); |
| } |
| |
| if( points4D == 0 || projMatrs == 0 || pointsPres == 0 || projPoints == 0 ) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| |
| int numPoints; |
| numPoints = points4D->cols; |
| if( numPoints < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" ); |
| } |
| |
| if( points4D->rows != 4 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Points must have 4 cordinates" ); |
| } |
| |
| /* !!! Not tested all input parameters */ |
| /* ----- End test ----- */ |
| |
| CvMat point4D; |
| CvMat point3D; |
| double point4D_dat[4]; |
| double point3D_dat[3]; |
| point4D = cvMat(4,1,CV_64F,point4D_dat); |
| point3D = cvMat(3,1,CV_64F,point3D_dat); |
| |
| #ifdef TRACK_BUNDLE |
| { |
| FILE *file; |
| file = fopen( TRACK_BUNDLE_FILE ,"a"); |
| fprintf(file,"\n----- test 14.01 icvProjPointsStatusFunc -----\n"); |
| fclose(file); |
| } |
| #endif |
| |
| int currImage; |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| /* Get project matrix */ |
| /* project visible points using current projection matrix */ |
| int currVisPoint = 0; |
| for( int currPoint = 0; currPoint < numPoints; currPoint++ ) |
| { |
| if( cvmGet(pointsPres[currImage],0,currPoint) > 0 ) |
| { |
| /* project current point */ |
| cvGetSubRect(points4D,&point4D,cvRect(currPoint,0,1,4)); |
| |
| #ifdef TRACK_BUNDLE |
| { |
| FILE *file; |
| file = fopen( TRACK_BUNDLE_FILE ,"a"); |
| fprintf(file,"\n----- test 14.02 point4D (%lf, %lf, %lf, %lf) -----\n", |
| cvmGet(&point4D,0,0), |
| cvmGet(&point4D,1,0), |
| cvmGet(&point4D,2,0), |
| cvmGet(&point4D,3,0)); |
| fclose(file); |
| } |
| #endif |
| |
| cvmMul(projMatrs[currImage],&point4D,&point3D); |
| double w = point3D_dat[2]; |
| cvmSet(projPoints[currImage],0,currVisPoint,point3D_dat[0]/w); |
| cvmSet(projPoints[currImage],1,currVisPoint,point3D_dat[1]/w); |
| |
| #ifdef TRACK_BUNDLE |
| { |
| FILE *file; |
| file = fopen( TRACK_BUNDLE_FILE ,"a"); |
| fprintf(file,"\n----- test 14.03 (%lf, %lf, %lf) -> (%lf, %lf)-----\n", |
| point3D_dat[0], |
| point3D_dat[1], |
| point3D_dat[2], |
| point3D_dat[0]/w, |
| point3D_dat[1]/w |
| ); |
| fclose(file); |
| } |
| #endif |
| currVisPoint++; |
| } |
| } |
| } |
| |
| __END__; |
| } |
| |
| /*======================================================================================*/ |
| void icvFreeMatrixArray(CvMat ***matrArray,int numMatr) |
| { |
| /* Free each matrix */ |
| int currMatr; |
| |
| if( *matrArray != 0 ) |
| {/* Need delete */ |
| for( currMatr = 0; currMatr < numMatr; currMatr++ ) |
| { |
| cvReleaseMat((*matrArray)+currMatr); |
| } |
| cvFree( matrArray); |
| } |
| return; |
| } |
| |
| /*======================================================================================*/ |
| void *icvClearAlloc(int size) |
| { |
| void *ptr = 0; |
| |
| CV_FUNCNAME( "icvClearAlloc" ); |
| __BEGIN__; |
| |
| if( size > 0 ) |
| { |
| CV_CALL(ptr = cvAlloc(size)); |
| memset(ptr,0,size); |
| } |
| |
| __END__; |
| return ptr; |
| } |
| |
| /*======================================================================================*/ |
| #if 0 |
| void cvOptimizeLevenbergMarquardtBundleWraper( CvMat** projMatrs, CvMat** observProjPoints, |
| CvMat** pointsPres, int numImages, |
| CvMat** resultProjMatrs, CvMat* resultPoints4D,int maxIter,double epsilon ) |
| { |
| /* Delete al sparse points */ |
| |
| int icvDeleteSparsInPoints( int numImages, |
| CvMat **points, |
| CvMat **status, |
| CvMat *wasStatus)/* status of previous configuration */ |
| |
| } |
| #endif |
| /*======================================================================================*/ |
| /* !!! may be useful to return norm of error */ |
| /* !!! may be does not work correct with not all visible 4D points */ |
| void cvOptimizeLevenbergMarquardtBundle( CvMat** projMatrs, CvMat** observProjPoints, |
| CvMat** pointsPres, int numImages, |
| CvMat** resultProjMatrs, CvMat* resultPoints4D,int maxIter,double epsilon ) |
| { |
| |
| CvMat *vectorX_points4D = 0; |
| CvMat **vectorX_projMatrs = 0; |
| |
| CvMat *newVectorX_points4D = 0; |
| CvMat **newVectorX_projMatrs = 0; |
| |
| CvMat *changeVectorX_points4D = 0; |
| CvMat *changeVectorX_projMatrs = 0; |
| |
| CvMat **observVisPoints = 0; |
| CvMat **projVisPoints = 0; |
| CvMat **errorProjPoints = 0; |
| CvMat **DerivProj = 0; |
| CvMat **DerivPoint = 0; |
| CvMat *matrW = 0; |
| CvMat **matrsUk = 0; |
| CvMat **workMatrsUk = 0; |
| CvMat **matrsVi = 0; |
| CvMat *workMatrVi = 0; |
| CvMat **workMatrsInvVi = 0; |
| CvMat *jacProjErr = 0; |
| CvMat *jacPointErr = 0; |
| |
| CvMat *matrTmpSys1 = 0; |
| CvMat *matrSysDeltaP = 0; |
| CvMat *vectTmpSys3 = 0; |
| CvMat *vectSysDeltaP = 0; |
| CvMat *deltaP = 0; |
| CvMat *deltaM = 0; |
| CvMat *vectTmpSysM = 0; |
| |
| int numPoints = 0; |
| |
| |
| CV_FUNCNAME( "cvOptimizeLevenbergMarquardtBundle" ); |
| __BEGIN__; |
| |
| /* ----- Test input params for errors ----- */ |
| if( numImages < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of images must be more than zero" ); |
| } |
| |
| if( maxIter < 1 || maxIter > 2000 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Maximum number of iteration must be in [1..1000]" ); |
| } |
| |
| if( epsilon < 0 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Epsilon parameter must be >= 0" ); |
| } |
| |
| if( !CV_IS_MAT(resultPoints4D) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "resultPoints4D must be a matrix 4 x NumPnt" ); |
| } |
| |
| numPoints = resultPoints4D->cols; |
| if( numPoints < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of points must be more than zero" );//!!! |
| } |
| |
| if( resultPoints4D->rows != 4 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "resultPoints4D must have 4 cordinates" ); |
| } |
| |
| /* ----- End test ----- */ |
| |
| /* Optimization using bundle adjustment */ |
| /* work with non visible points */ |
| |
| CV_CALL( vectorX_points4D = cvCreateMat(4,numPoints,CV_64F)); |
| CV_CALL( vectorX_projMatrs = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages)); |
| |
| CV_CALL( newVectorX_points4D = cvCreateMat(4,numPoints,CV_64F)); |
| CV_CALL( newVectorX_projMatrs = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages)); |
| |
| CV_CALL( changeVectorX_points4D = cvCreateMat(4,numPoints,CV_64F)); |
| CV_CALL( changeVectorX_projMatrs = cvCreateMat(3,4,CV_64F)); |
| |
| int currImage; |
| |
| /* ----- Test input params ----- */ |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| /* Test size of input initial and result projection matrices */ |
| if( !CV_IS_MAT(projMatrs[currImage]) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "each of initial projMatrs must be a matrix 3 x 4" ); |
| } |
| if( projMatrs[currImage]->rows != 3 || projMatrs[currImage]->cols != 4 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "each of initial projMatrs must be a matrix 3 x 4" ); |
| } |
| |
| |
| if( !CV_IS_MAT(observProjPoints[currImage]) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "each of observProjPoints must be a matrix 2 x NumPnts" ); |
| } |
| if( observProjPoints[currImage]->rows != 2 || observProjPoints[currImage]->cols != numPoints ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "each of observProjPoints must be a matrix 2 x NumPnts" ); |
| } |
| |
| if( !CV_IS_MAT(pointsPres[currImage]) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "each of pointsPres must be a matrix 1 x NumPnt" ); |
| } |
| if( pointsPres[currImage]->rows != 1 || pointsPres[currImage]->cols != numPoints ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "each of pointsPres must be a matrix 1 x NumPnt" ); |
| } |
| |
| if( !CV_IS_MAT(resultProjMatrs[currImage]) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "each of resultProjMatrs must be a matrix 3 x 4" ); |
| } |
| if( resultProjMatrs[currImage]->rows != 3 || resultProjMatrs[currImage]->cols != 4 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "each of resultProjMatrs must be a matrix 3 x 4" ); |
| } |
| |
| } |
| /* ----- End test ----- */ |
| |
| /* Copy projection matrices to vectorX0 */ |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| CV_CALL( vectorX_projMatrs[currImage] = cvCreateMat(3,4,CV_64F)); |
| CV_CALL( newVectorX_projMatrs[currImage] = cvCreateMat(3,4,CV_64F)); |
| cvCopy(projMatrs[currImage],vectorX_projMatrs[currImage]); |
| } |
| |
| /* Reconstruct points4D using projection matrices and status information */ |
| icvReconstructPoints4DStatus(observProjPoints, projMatrs, pointsPres, vectorX_points4D, numImages); |
| |
| /* ----- Allocate memory for work matrices ----- */ |
| /* Compute number of good points on each images */ |
| |
| CV_CALL( observVisPoints = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); |
| CV_CALL( projVisPoints = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); |
| CV_CALL( errorProjPoints = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); |
| CV_CALL( DerivProj = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); |
| CV_CALL( DerivPoint = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); |
| CV_CALL( matrW = cvCreateMat(12*numImages,4*numPoints,CV_64F) ); |
| CV_CALL( matrsUk = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); |
| CV_CALL( workMatrsUk = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); |
| CV_CALL( matrsVi = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numPoints) ); |
| CV_CALL( workMatrVi = cvCreateMat(4,4,CV_64F) ); |
| CV_CALL( workMatrsInvVi = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numPoints) ); |
| |
| CV_CALL( jacProjErr = cvCreateMat(12*numImages,1,CV_64F) ); |
| CV_CALL( jacPointErr = cvCreateMat(4*numPoints,1,CV_64F) ); |
| |
| |
| int i; |
| for( i = 0; i < numPoints; i++ ) |
| { |
| CV_CALL( matrsVi[i] = cvCreateMat(4,4,CV_64F) ); |
| CV_CALL( workMatrsInvVi[i] = cvCreateMat(4,4,CV_64F) ); |
| } |
| |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| CV_CALL( matrsUk[currImage] = cvCreateMat(12,12,CV_64F) ); |
| CV_CALL( workMatrsUk[currImage] = cvCreateMat(12,12,CV_64F) ); |
| |
| int currVisPoint = 0, currPoint, numVisPoint; |
| for( currPoint = 0; currPoint < numPoints; currPoint++ ) |
| { |
| if( cvmGet(pointsPres[currImage],0,currPoint) > 0 ) |
| { |
| currVisPoint++; |
| } |
| } |
| |
| numVisPoint = currVisPoint; |
| |
| /* Allocate memory for current image data */ |
| CV_CALL( observVisPoints[currImage] = cvCreateMat(2,numVisPoint,CV_64F) ); |
| CV_CALL( projVisPoints[currImage] = cvCreateMat(2,numVisPoint,CV_64F) ); |
| |
| /* create error matrix */ |
| CV_CALL( errorProjPoints[currImage] = cvCreateMat(2,numVisPoint,CV_64F) ); |
| |
| /* Create derivate matrices */ |
| CV_CALL( DerivProj[currImage] = cvCreateMat(2*numVisPoint,12,CV_64F) ); |
| CV_CALL( DerivPoint[currImage] = cvCreateMat(2,numVisPoint*4,CV_64F) ); |
| |
| /* Copy observed projected visible points */ |
| currVisPoint = 0; |
| for( currPoint = 0; currPoint < numPoints; currPoint++ ) |
| { |
| if( cvmGet(pointsPres[currImage],0,currPoint) > 0 ) |
| { |
| cvmSet(observVisPoints[currImage],0,currVisPoint,cvmGet(observProjPoints[currImage],0,currPoint)); |
| cvmSet(observVisPoints[currImage],1,currVisPoint,cvmGet(observProjPoints[currImage],1,currPoint)); |
| currVisPoint++; |
| } |
| } |
| } |
| /*---------------------------------------------------*/ |
| |
| CV_CALL( matrTmpSys1 = cvCreateMat(numPoints*4, numImages*12, CV_64F) ); |
| CV_CALL( matrSysDeltaP = cvCreateMat(numImages*12, numImages*12, CV_64F) ); |
| CV_CALL( vectTmpSys3 = cvCreateMat(numPoints*4,1,CV_64F) ); |
| CV_CALL( vectSysDeltaP = cvCreateMat(numImages*12,1,CV_64F) ); |
| CV_CALL( deltaP = cvCreateMat(numImages*12,1,CV_64F) ); |
| CV_CALL( deltaM = cvCreateMat(numPoints*4,1,CV_64F) ); |
| CV_CALL( vectTmpSysM = cvCreateMat(numPoints*4,1,CV_64F) ); |
| |
| |
| //#ifdef TRACK_BUNDLE |
| #if 1 |
| { |
| /* Create file to track */ |
| FILE* file; |
| file = fopen( TRACK_BUNDLE_FILE ,"w"); |
| fprintf(file,"begin\n"); |
| fclose(file); |
| } |
| #endif |
| |
| /* ============= main optimization loop ============== */ |
| |
| /* project all points using current vector X */ |
| icvProjPointsStatusFunc(numImages, vectorX_points4D, vectorX_projMatrs, pointsPres, projVisPoints); |
| |
| /* Compute error with observed value and computed projection */ |
| double prevError; |
| prevError = 0; |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| cvSub(observVisPoints[currImage],projVisPoints[currImage],errorProjPoints[currImage]); |
| double currNorm = cvNorm(errorProjPoints[currImage]); |
| prevError += currNorm * currNorm; |
| } |
| prevError = sqrt(prevError); |
| |
| int currIter; |
| double change; |
| double alpha; |
| |
| //#ifdef TRACK_BUNDLE |
| #if 1 |
| { |
| /* Create file to track */ |
| FILE* file; |
| file = fopen( TRACK_BUNDLE_FILE ,"a"); |
| fprintf(file,"\n========================================\n");; |
| fprintf(file,"Iter=0\n"); |
| fprintf(file,"Error = %20.15lf\n",prevError); |
| fprintf(file,"Change = %20.15lf\n",1.0); |
| |
| fprintf(file,"projection errors\n"); |
| |
| /* Print all proejction errors */ |
| int currImage; |
| for( currImage = 0; currImage < numImages; currImage++) |
| { |
| fprintf(file,"\nImage=%d\n",currImage); |
| int numPn = errorProjPoints[currImage]->cols; |
| for( int currPoint = 0; currPoint < numPn; currPoint++ ) |
| { |
| double ex,ey; |
| ex = cvmGet(errorProjPoints[currImage],0,currPoint); |
| ey = cvmGet(errorProjPoints[currImage],1,currPoint); |
| fprintf(file,"%40.35lf, %40.35lf\n",ex,ey); |
| } |
| } |
| fclose(file); |
| } |
| #endif |
| |
| currIter = 0; |
| change = 1; |
| alpha = 0.001; |
| |
| |
| do |
| { |
| |
| #ifdef TRACK_BUNDLE |
| { |
| FILE *file; |
| file = fopen( TRACK_BUNDLE_FILE ,"a"); |
| fprintf(file,"\n----- test 6 do main -----\n"); |
| |
| double norm = cvNorm(vectorX_points4D); |
| fprintf(file," test 6.01 prev normPnts=%lf\n",norm); |
| |
| for( int i=0;i<numImages;i++ ) |
| { |
| double norm = cvNorm(vectorX_projMatrs[i]); |
| fprintf(file," test 6.01 prev normProj=%lf\n",norm); |
| } |
| |
| fclose(file); |
| } |
| #endif |
| |
| /* Compute derivates by projectinon matrices */ |
| icvComputeDerivateProjAll(vectorX_points4D,vectorX_projMatrs,pointsPres,numImages,DerivProj); |
| |
| /* Compute derivates by 4D points */ |
| icvComputeDerivatePointsAll(vectorX_points4D,vectorX_projMatrs,pointsPres,numImages,DerivPoint); |
| |
| /* Compute matrces Uk */ |
| icvComputeMatrixUAll(numImages,DerivProj,matrsUk); |
| icvComputeMatrixVAll(numImages,DerivPoint,pointsPres,matrsVi); |
| icvComputeMatrixW(numImages,DerivProj,DerivPoint,pointsPres,matrW); |
| |
| |
| #ifdef TRACK_BUNDLE |
| { |
| FILE *file; |
| file = fopen( TRACK_BUNDLE_FILE ,"a"); |
| fprintf(file,"\n----- test 6.03 do matrs U V -----\n"); |
| |
| int i; |
| for( i = 0; i < numImages; i++ ) |
| { |
| double norm = cvNorm(matrsUk[i]); |
| fprintf(file," test 6.01 prev matrsUk=%lf\n",norm); |
| } |
| |
| for( i = 0; i < numPoints; i++ ) |
| { |
| double norm = cvNorm(matrsVi[i]); |
| fprintf(file," test 6.01 prev matrsVi=%lf\n",norm); |
| } |
| |
| fclose(file); |
| } |
| #endif |
| |
| /* Compute jac errors */ |
| icvComputeJacErrorProj(numImages, DerivProj, errorProjPoints, jacProjErr); |
| icvComputeJacErrorPoint(numImages, DerivPoint, errorProjPoints, pointsPres, jacPointErr); |
| |
| #ifdef TRACK_BUNDLE |
| { |
| FILE *file; |
| file = fopen( TRACK_BUNDLE_FILE ,"a"); |
| fprintf(file,"\n----- test 6 do main -----\n"); |
| double norm1 = cvNorm(vectorX_points4D); |
| fprintf(file," test 6.02 post normPnts=%lf\n",norm1); |
| fclose(file); |
| } |
| #endif |
| /* Copy matrices Uk to work matrices Uk */ |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| cvCopy(matrsUk[currImage],workMatrsUk[currImage]); |
| } |
| |
| #ifdef TRACK_BUNDLE |
| { |
| FILE *file; |
| file = fopen( TRACK_BUNDLE_FILE ,"a"); |
| fprintf(file,"\n----- test 60.3 do matrs U V -----\n"); |
| |
| int i; |
| for( i = 0; i < numImages; i++ ) |
| { |
| double norm = cvNorm(matrsUk[i]); |
| fprintf(file," test 6.01 post1 matrsUk=%lf\n",norm); |
| } |
| |
| for( i = 0; i < numPoints; i++ ) |
| { |
| double norm = cvNorm(matrsVi[i]); |
| fprintf(file," test 6.01 post1 matrsVi=%lf\n",norm); |
| } |
| |
| fclose(file); |
| } |
| #endif |
| |
| /* ========== Solve normal equation for given alpha and Jacobian ============ */ |
| |
| do |
| { |
| /* ---- Add alpha to matrices --- */ |
| /* Add alpha to matrInvVi and make workMatrsInvVi */ |
| |
| int currV; |
| for( currV = 0; currV < numPoints; currV++ ) |
| { |
| cvCopy(matrsVi[currV],workMatrVi); |
| |
| for( int i = 0; i < 4; i++ ) |
| { |
| cvmSet(workMatrVi,i,i,cvmGet(matrsVi[currV],i,i)*(1+alpha) ); |
| } |
| |
| cvInvert(workMatrVi,workMatrsInvVi[currV],CV_LU/*,&currV*/); |
| } |
| |
| /* Add alpha to matrUk and make matrix workMatrsUk */ |
| for( currImage = 0; currImage< numImages; currImage++ ) |
| { |
| |
| for( i = 0; i < 12; i++ ) |
| { |
| cvmSet(workMatrsUk[currImage],i,i,cvmGet(matrsUk[currImage],i,i)*(1+alpha)); |
| } |
| |
| |
| } |
| |
| /* Fill matrix to make system for computing delta P (matrTmpSys1 = inv(V)*tr(W) )*/ |
| for( currV = 0; currV < numPoints; currV++ ) |
| { |
| int currRowV; |
| for( currRowV = 0; currRowV < 4; currRowV++ ) |
| { |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| for( int currCol = 0; currCol < 12; currCol++ )/* For each column of transposed matrix W */ |
| { |
| double sum = 0; |
| for( i = 0; i < 4; i++ ) |
| { |
| sum += cvmGet(workMatrsInvVi[currV],currRowV,i) * |
| cvmGet(matrW,currImage*12+currCol,currV*4+i); |
| } |
| cvmSet(matrTmpSys1,currV*4+currRowV,currImage*12+currCol,sum); |
| } |
| } |
| } |
| } |
| |
| |
| /* Fill matrix to make system for computing delta P (matrTmpSys2 = W * matrTmpSys ) */ |
| cvmMul(matrW,matrTmpSys1,matrSysDeltaP); |
| |
| /* need to compute U-matrTmpSys2. But we compute matTmpSys2-U */ |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| CvMat subMatr; |
| cvGetSubRect(matrSysDeltaP,&subMatr,cvRect(currImage*12,currImage*12,12,12)); |
| cvSub(&subMatr,workMatrsUk[currImage],&subMatr); |
| } |
| |
| /* Compute right side of normal equation */ |
| for( currV = 0; currV < numPoints; currV++ ) |
| { |
| CvMat subMatrErPnts; |
| CvMat subMatr; |
| cvGetSubRect(jacPointErr,&subMatrErPnts,cvRect(0,currV*4,1,4)); |
| cvGetSubRect(vectTmpSys3,&subMatr,cvRect(0,currV*4,1,4)); |
| cvmMul(workMatrsInvVi[currV],&subMatrErPnts,&subMatr); |
| } |
| |
| cvmMul(matrW,vectTmpSys3,vectSysDeltaP); |
| cvSub(vectSysDeltaP,jacProjErr,vectSysDeltaP); |
| |
| /* Now we can compute part of normal system - deltaP */ |
| cvSolve(matrSysDeltaP ,vectSysDeltaP, deltaP, CV_SVD); |
| |
| /* Print deltaP to file */ |
| |
| #ifdef TRACK_BUNDLE |
| { |
| FILE* file; |
| file = fopen( TRACK_BUNDLE_FILE_DELTAP ,"w"); |
| |
| int currImage; |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| fprintf(file,"\nImage=%d\n",currImage); |
| int i; |
| for( i = 0; i < 12; i++ ) |
| { |
| double val; |
| val = cvmGet(deltaP,currImage*12+i,0); |
| fprintf(file,"%lf\n",val); |
| } |
| fprintf(file,"\n"); |
| } |
| fclose(file); |
| } |
| #endif |
| /* We know deltaP and now we can compute system for deltaM */ |
| for( i = 0; i < numPoints * 4; i++ ) |
| { |
| double sum = 0; |
| for( int j = 0; j < numImages * 12; j++ ) |
| { |
| sum += cvmGet(matrW,j,i) * cvmGet(deltaP,j,0); |
| } |
| cvmSet(vectTmpSysM,i,0,cvmGet(jacPointErr,i,0)-sum); |
| } |
| |
| /* Compute deltaM */ |
| for( currV = 0; currV < numPoints; currV++ ) |
| { |
| CvMat subMatr; |
| CvMat subMatrM; |
| cvGetSubRect(vectTmpSysM,&subMatr,cvRect(0,currV*4,1,4)); |
| cvGetSubRect(deltaM,&subMatrM,cvRect(0,currV*4,1,4)); |
| cvmMul(workMatrsInvVi[currV],&subMatr,&subMatrM); |
| } |
| |
| /* We know delta and compute new value of vector X: nextVectX = vectX + deltas */ |
| |
| /* Compute new P */ |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| for( i = 0; i < 3; i++ ) |
| { |
| for( int j = 0; j < 4; j++ ) |
| { |
| cvmSet(newVectorX_projMatrs[currImage],i,j, |
| cvmGet(vectorX_projMatrs[currImage],i,j) + cvmGet(deltaP,currImage*12+i*4+j,0)); |
| } |
| } |
| } |
| |
| /* Compute new M */ |
| int currPoint; |
| for( currPoint = 0; currPoint < numPoints; currPoint++ ) |
| { |
| for( i = 0; i < 4; i++ ) |
| { |
| cvmSet(newVectorX_points4D,i,currPoint, |
| cvmGet(vectorX_points4D,i,currPoint) + cvmGet(deltaM,currPoint*4+i,0)); |
| } |
| } |
| |
| /* ----- Compute errors for new vectorX ----- */ |
| /* Project points using new vectorX and status of each point */ |
| icvProjPointsStatusFunc(numImages, newVectorX_points4D, newVectorX_projMatrs, pointsPres, projVisPoints); |
| /* Compute error with observed value and computed projection */ |
| double newError = 0; |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| cvSub(observVisPoints[currImage],projVisPoints[currImage],errorProjPoints[currImage]); |
| double currNorm = cvNorm(errorProjPoints[currImage]); |
| |
| //#ifdef TRACK_BUNDLE |
| #if 1 |
| { |
| FILE *file; |
| file = fopen( TRACK_BUNDLE_FILE ,"a"); |
| fprintf(file,"\n----- test 13,01 currImage=%d currNorm=%lf -----\n",currImage,currNorm); |
| fclose(file); |
| } |
| #endif |
| newError += currNorm * currNorm; |
| } |
| newError = sqrt(newError); |
| |
| currIter++; |
| |
| |
| |
| |
| //#ifdef TRACK_BUNDLE |
| #if 1 |
| { |
| /* Create file to track */ |
| FILE* file; |
| file = fopen( TRACK_BUNDLE_FILE ,"a"); |
| fprintf(file,"\n========================================\n"); |
| fprintf(file,"numPoints=%d\n",numPoints); |
| fprintf(file,"Iter=%d\n",currIter); |
| fprintf(file,"Error = %20.15lf\n",newError); |
| fprintf(file,"Change = %20.15lf\n",change); |
| |
| |
| /* Print all projection errors */ |
| #if 0 |
| fprintf(file,"projection errors\n"); |
| int currImage; |
| for( currImage = 0; currImage < numImages; currImage++) |
| { |
| fprintf(file,"\nImage=%d\n",currImage); |
| int numPn = errorProjPoints[currImage]->cols; |
| for( int currPoint = 0; currPoint < numPn; currPoint++ ) |
| { |
| double ex,ey; |
| ex = cvmGet(errorProjPoints[currImage],0,currPoint); |
| ey = cvmGet(errorProjPoints[currImage],1,currPoint); |
| fprintf(file,"%lf,%lf\n",ex,ey); |
| } |
| } |
| fprintf(file,"\n---- test 0 -----\n"); |
| #endif |
| |
| fclose(file); |
| } |
| #endif |
| |
| |
| |
| /* Compare new error and last error */ |
| if( newError < prevError ) |
| {/* accept new value */ |
| prevError = newError; |
| /* Compute relative change of required parameter vectorX. change = norm(curr-prev) / norm(curr) ) */ |
| { |
| double normAll1 = 0; |
| double normAll2 = 0; |
| double currNorm1 = 0; |
| double currNorm2 = 0; |
| /* compute norm for projection matrices */ |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| currNorm1 = cvNorm(newVectorX_projMatrs[currImage],vectorX_projMatrs[currImage]); |
| currNorm2 = cvNorm(newVectorX_projMatrs[currImage]); |
| |
| normAll1 += currNorm1 * currNorm1; |
| normAll2 += currNorm2 * currNorm2; |
| } |
| |
| /* compute norm for points */ |
| currNorm1 = cvNorm(newVectorX_points4D,vectorX_points4D); |
| currNorm2 = cvNorm(newVectorX_points4D); |
| |
| normAll1 += currNorm1 * currNorm1; |
| normAll2 += currNorm2 * currNorm2; |
| |
| /* compute change */ |
| change = sqrt(normAll1) / sqrt(normAll2); |
| |
| |
| //#ifdef TRACK_BUNDLE |
| #if 1 |
| { |
| /* Create file to track */ |
| FILE* file; |
| file = fopen( TRACK_BUNDLE_FILE ,"a"); |
| fprintf(file,"\nChange inside newVal change = %20.15lf\n",change); |
| fprintf(file," normAll1= %20.15lf\n",sqrt(normAll1)); |
| fprintf(file," normAll2= %20.15lf\n",sqrt(normAll2)); |
| |
| fclose(file); |
| } |
| #endif |
| |
| } |
| |
| alpha /= 10; |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| cvCopy(newVectorX_projMatrs[currImage],vectorX_projMatrs[currImage]); |
| } |
| cvCopy(newVectorX_points4D,vectorX_points4D); |
| |
| break; |
| } |
| else |
| { |
| alpha *= 10; |
| } |
| |
| } while( change > epsilon && currIter < maxIter );/* solve normal equation using current alpha */ |
| |
| //#ifdef TRACK_BUNDLE |
| #if 1 |
| { |
| FILE* file; |
| file = fopen( TRACK_BUNDLE_FILE ,"a"); |
| fprintf(file,"\nBest error = %40.35lf\n",prevError); |
| fclose(file); |
| } |
| |
| #endif |
| |
| |
| } while( change > epsilon && currIter < maxIter ); |
| |
| /*--------------------------------------------*/ |
| /* Optimization complete copy computed params */ |
| /* Copy projection matrices */ |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| cvCopy(newVectorX_projMatrs[currImage],resultProjMatrs[currImage]); |
| } |
| /* Copy 4D points */ |
| cvCopy(newVectorX_points4D,resultPoints4D); |
| |
| // free(memory); |
| |
| __END__; |
| |
| /* Free allocated memory */ |
| |
| /* Free simple matrices */ |
| cvFree(&vectorX_points4D); |
| cvFree(&newVectorX_points4D); |
| cvFree(&changeVectorX_points4D); |
| cvFree(&changeVectorX_projMatrs); |
| cvFree(&matrW); |
| cvFree(&workMatrVi); |
| cvFree(&jacProjErr); |
| cvFree(&jacPointErr); |
| cvFree(&matrTmpSys1); |
| cvFree(&matrSysDeltaP); |
| cvFree(&vectTmpSys3); |
| cvFree(&vectSysDeltaP); |
| cvFree(&deltaP); |
| cvFree(&deltaM); |
| cvFree(&vectTmpSysM); |
| |
| /* Free arrays of matrices */ |
| icvFreeMatrixArray(&vectorX_projMatrs,numImages); |
| icvFreeMatrixArray(&newVectorX_projMatrs,numImages); |
| icvFreeMatrixArray(&observVisPoints,numImages); |
| icvFreeMatrixArray(&projVisPoints,numImages); |
| icvFreeMatrixArray(&errorProjPoints,numImages); |
| icvFreeMatrixArray(&DerivProj,numImages); |
| icvFreeMatrixArray(&DerivPoint,numImages); |
| icvFreeMatrixArray(&matrsUk,numImages); |
| icvFreeMatrixArray(&workMatrsUk,numImages); |
| icvFreeMatrixArray(&matrsVi,numPoints); |
| icvFreeMatrixArray(&workMatrsInvVi,numPoints); |
| |
| return; |
| } |
