modules/legacy/src/levmarprojbandle.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42
  43 #include "precomp.hpp"
  44 #include <float.h>
  45 #include <limits.h>
  46 #include <stdio.h>
  47
  48 void icvReconstructPoints4DStatus(CvMat** projPoints, CvMat **projMatrs, CvMat** presPoints, CvMat *points4D,int numImages,CvMat **projError=0);
  49
  50 /* Valery Mosyagin */
  51
  52 /* If you want to save internal debug info to files uncomment next lines and set paths to files if need */
  53 /* Note these file may be very large */
  54 /*
  55 #define TRACK_BUNDLE
  56 #define TRACK_BUNDLE_FILE            "d:\\test\\bundle.txt"
  57 #define TRACK_BUNDLE_FILE_JAC        "d:\\test\\bundle.txt"
  58 #define TRACK_BUNDLE_FILE_JACERRPROJ "d:\\test\\JacErrProj.txt"
  59 #define TRACK_BUNDLE_FILE_JACERRPNT  "d:\\test\\JacErrPoint.txt"
  60 #define TRACK_BUNDLE_FILE_MATRW      "d:\\test\\matrWt.txt"
  61 #define TRACK_BUNDLE_FILE_DELTAP     "d:\\test\\deltaP.txt"
  62 */
  63 #define TRACK_BUNDLE_FILE            "d:\\test\\bundle.txt"
  64
  65 void cvOptimizeLevenbergMarquardtBundle( CvMat** projMatrs, CvMat** observProjPoints,
  66                                        CvMat** pointsPres, int numImages,
  67                                        CvMat** resultProjMatrs, CvMat* resultPoints4D,int maxIter,double epsilon );
  68
  69
  70 /* ============== Bundle adjustment optimization ================= */
  71 static void icvComputeDerivateProj(CvMat *points4D,CvMat *projMatr, CvMat *status, CvMat *derivProj)
  72 {
  73     /* Compute derivate for given projection matrix points and status of points */
  74
  75     CV_FUNCNAME( "icvComputeDerivateProj" );
  76     __BEGIN__;
  77
  78
  79     /* ----- Test input params for errors ----- */
  80     if( points4D == 0 || projMatr == 0 || status == 0 || derivProj == 0)
  81     {
  82         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
  83     }
  84
  85     if( !CV_IS_MAT(points4D) )
  86     {
  87         CV_ERROR( CV_StsUnsupportedFormat, "points4D must be a matrix 4xN" );
  88     }
  89
  90     /* Compute number of points */
  91     int numPoints;
  92     numPoints = points4D->cols;
  93
  94     if( numPoints < 1 )
  95     {
  96         CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" );
  97     }
  98
  99     if( points4D->rows != 4 )
 100     {
 101         CV_ERROR( CV_StsOutOfRange, "Number of coordinates of points4D must be 4" );
 102     }
 103
 104     if( !CV_IS_MAT(projMatr) )
 105     {
 106         CV_ERROR( CV_StsUnsupportedFormat, "projMatr must be a matrix 3x4" );
 107     }
 108
 109     if( projMatr->rows != 3 || projMatr->cols != 4 )
 110     {
 111         CV_ERROR( CV_StsOutOfRange, "Size of projection matrix (projMatr) must be 3x4" );
 112     }
 113
 114     if( !CV_IS_MAT(status) )
 115     {
 116         CV_ERROR( CV_StsUnsupportedFormat, "Status must be a matrix 1xN" );
 117     }
 118
 119     if( status->rows != 1 || status->cols != numPoints )
 120     {
 121         CV_ERROR( CV_StsOutOfRange, "Size of status of points must be 1xN" );
 122     }
 123
 124     if( !CV_IS_MAT(derivProj) )
 125     {
 126         CV_ERROR( CV_StsUnsupportedFormat, "derivProj must be a matrix VisN x 12" );
 127     }
 128
 129     if( derivProj->cols != 12 )
 130     {
 131         CV_ERROR( CV_StsOutOfRange, "derivProj must be a matrix VisN x 12" );
 132     }
 133     /* ----- End test ----- */
 134
 135     /* Allocate memory for derivates */
 136
 137     double p[12];
 138     /* Copy projection matrix */
 139     for(int i = 0; i < 12; i++ )
 140     {
 141         p[i] = cvmGet(projMatr,i/4,i%4);
 142     }
 143
 144     /* Fill deriv matrix */
 145     int currVisPoint;
 146     int currPoint;
 147
 148     currVisPoint = 0;
 149     for( currPoint = 0; currPoint < numPoints; currPoint++ )
 150     {
 151         if( cvmGet(status,0,currPoint) > 0 )
 152         {
 153             double X[4];
 154             X[0] = cvmGet(points4D,0,currVisPoint);
 155             X[1] = cvmGet(points4D,1,currVisPoint);
 156             X[2] = cvmGet(points4D,2,currVisPoint);
 157             X[3] = cvmGet(points4D,3,currVisPoint);
 158
 159             /* Compute derivate for this point */
 160
 161             double piX[3];
 162             piX[0] = X[0]*p[0] + X[1]*p[1] + X[2]*p[2]  + X[3]*p[3];
 163             piX[1] = X[0]*p[4] + X[1]*p[5] + X[2]*p[6]  + X[3]*p[7];
 164             piX[2] = X[0]*p[8] + X[1]*p[9] + X[2]*p[10] + X[3]*p[11];
 165
 166             /* fill derivate by point */
 167
 168             double tmp3 = 1/(piX[2]*piX[2]);
 169
 170             double tmp1 = -piX[0]*tmp3;
 171             double tmp2 = -piX[1]*tmp3;
 172
 173             /* fill derivate by projection matrix */
 174             for(int i = 0; i < 4; i++ )
 175             {
 176                 /* derivate for x */
 177                 cvmSet(derivProj,currVisPoint*2,i,X[i]/piX[2]);//x' p1i
 178                 cvmSet(derivProj,currVisPoint*2,4+i,0);//x' p1i
 179                 cvmSet(derivProj,currVisPoint*2,8+i,X[i]*tmp1);//x' p3i
 180
 181                 /* derivate for y */
 182                 cvmSet(derivProj,currVisPoint*2+1,i,0);//y' p2i
 183                 cvmSet(derivProj,currVisPoint*2+1,4+i,X[i]/piX[2]);//y' p2i
 184                 cvmSet(derivProj,currVisPoint*2+1,8+i,X[i]*tmp2);//y' p3i
 185             }
 186
 187             currVisPoint++;
 188         }
 189     }
 190
 191     if( derivProj->rows != currVisPoint * 2 )
 192     {
 193         CV_ERROR( CV_StsOutOfRange, "derivProj must be a matrix 2VisN x 12" );
 194     }
 195
 196
 197     __END__;
 198     return;
 199 }
 200 /*======================================================================================*/
 201
 202 static void icvComputeDerivateProjAll(CvMat *points4D, CvMat **projMatrs, CvMat **pointPres, int numImages,CvMat **projDerives)
 203 {
 204     CV_FUNCNAME( "icvComputeDerivateProjAll" );
 205     __BEGIN__;
 206
 207     /* ----- Test input params for errors ----- */
 208     if( numImages < 1 )
 209     {
 210         CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
 211     }
 212     if( projMatrs == 0 || pointPres == 0 || projDerives == 0 )
 213     {
 214         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 215     }
 216     /* ----- End test ----- */
 217
 218     int currImage;
 219     for( currImage = 0; currImage < numImages; currImage++ )
 220     {
 221         icvComputeDerivateProj(points4D,projMatrs[currImage], pointPres[currImage], projDerives[currImage]);
 222     }
 223
 224     __END__;
 225     return;
 226 }
 227 /*======================================================================================*/
 228
 229 static void icvComputeDerivatePoints(CvMat *points4D,CvMat *projMatr, CvMat *presPoints, CvMat *derivPoint)
 230 {
 231
 232     CV_FUNCNAME( "icvComputeDerivatePoints" );
 233     __BEGIN__;
 234
 235     /* ----- Test input params for errors ----- */
 236     if( points4D == 0 || projMatr == 0 || presPoints == 0 || derivPoint == 0)
 237     {
 238         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 239     }
 240
 241     if( !CV_IS_MAT(points4D) )
 242     {
 243         CV_ERROR( CV_StsUnsupportedFormat, "points4D must be a matrix N x 4" );
 244     }
 245
 246     int numPoints;
 247     numPoints = presPoints->cols;
 248
 249     if( numPoints < 1 )
 250     {
 251         CV_ERROR( CV_StsOutOfRange, "Number of points must be more than zero" );
 252     }
 253
 254     if( points4D->rows != 4 )
 255     {
 256         CV_ERROR( CV_StsOutOfRange, "points4D must be a matrix N x 4" );
 257     }
 258
 259     if( !CV_IS_MAT(projMatr) )
 260     {
 261         CV_ERROR( CV_StsUnsupportedFormat, "projMatr must be a matrix 3x4" );
 262     }
 263
 264     if( projMatr->rows != 3 || projMatr->cols != 4 )
 265     {
 266         CV_ERROR( CV_StsOutOfRange, "Size of projection matrix (projMatr) must be 3x4" );
 267     }
 268
 269     if( !CV_IS_MAT(presPoints) )
 270     {
 271         CV_ERROR( CV_StsUnsupportedFormat, "Status must be a matrix 1xN" );
 272     }
 273
 274     if( presPoints->rows != 1 || presPoints->cols != numPoints )
 275     {
 276         CV_ERROR( CV_StsOutOfRange, "Size of presPoints status must be 1xN" );
 277     }
 278
 279     if( !CV_IS_MAT(derivPoint) )
 280     {
 281         CV_ERROR( CV_StsUnsupportedFormat, "derivPoint must be a matrix 2 x 4VisNum" );
 282     }
 283     /* ----- End test ----- */
 284
 285     /* Compute derivates by points */
 286
 287     double p[12];
 288     for(int i = 0; i < 12; i++ )
 289     {
 290         p[i] = cvmGet(projMatr,i/4,i%4);
 291     }
 292
 293     int currVisPoint;
 294     int currProjPoint;
 295
 296     currVisPoint = 0;
 297     for( currProjPoint = 0; currProjPoint < numPoints; currProjPoint++ )
 298     {
 299         if( cvmGet(presPoints,0,currProjPoint) > 0 )
 300         {
 301             double X[4];
 302             X[0] = cvmGet(points4D,0,currProjPoint);
 303             X[1] = cvmGet(points4D,1,currProjPoint);
 304             X[2] = cvmGet(points4D,2,currProjPoint);
 305             X[3] = cvmGet(points4D,3,currProjPoint);
 306
 307             double piX[3];
 308             piX[0] = X[0]*p[0] + X[1]*p[1] + X[2]*p[2]  + X[3]*p[3];
 309             piX[1] = X[0]*p[4] + X[1]*p[5] + X[2]*p[6]  + X[3]*p[7];
 310             piX[2] = X[0]*p[8] + X[1]*p[9] + X[2]*p[10] + X[3]*p[11];
 311
 312             double tmp3 = 1/(piX[2]*piX[2]);
 313
 314             for(int j = 0; j < 2; j++ )//for x and y
 315             {
 316                 for(int i = 0; i < 4; i++ )// for X,Y,Z,W
 317                 {
 318                     cvmSet( derivPoint,
 319                             j, currVisPoint*4+i,
 320                             (p[j*4+i]*piX[2]-p[8+i]*piX[j]) * tmp3  );
 321                 }
 322             }
 323             currVisPoint++;
 324         }
 325     }
 326
 327     if( derivPoint->rows != 2 || derivPoint->cols != currVisPoint*4 )
 328     {
 329         CV_ERROR( CV_StsUnsupportedFormat, "derivPoint must be a matrix 2 x 4VisNum" );
 330     }
 331
 332     __END__;
 333     return;
 334 }
 335
 336 /*======================================================================================*/
 337 static void icvComputeDerivatePointsAll(CvMat *points4D, CvMat **projMatrs, CvMat **pointPres, int numImages,CvMat **pointDerives)
 338 {
 339     CV_FUNCNAME( "icvComputeDerivatePointsAll" );
 340     __BEGIN__;
 341
 342     /* ----- Test input params for errors ----- */
 343     if( numImages < 1 )
 344     {
 345         CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
 346     }
 347     if( projMatrs == 0 || pointPres == 0 || pointDerives == 0 )
 348     {
 349         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 350     }
 351     /* ----- End test ----- */
 352
 353     int currImage;
 354     for( currImage = 0; currImage < numImages; currImage++ )
 355     {
 356         icvComputeDerivatePoints(points4D, projMatrs[currImage], pointPres[currImage], pointDerives[currImage]);
 357     }
 358
 359     __END__;
 360     return;
 361 }
 362 /*======================================================================================*/
 363 static void icvComputeMatrixVAll(int numImages,CvMat **pointDeriv,CvMat **presPoints, CvMat **matrV)
 364 {
 365     int *shifts = 0;
 366
 367     CV_FUNCNAME( "icvComputeMatrixVAll" );
 368     __BEGIN__;
 369
 370     /* ----- Test input params for errors ----- */
 371     if( numImages < 1 )
 372     {
 373         CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
 374     }
 375     if( pointDeriv == 0 || presPoints == 0 || matrV == 0 )
 376     {
 377         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 378     }
 379     /*  !!! not tested all parameters */
 380     /* ----- End test ----- */
 381
 382     /* Compute all matrices U */
 383     int currImage;
 384     int currPoint;
 385     int numPoints;
 386     numPoints = presPoints[0]->cols;
 387     CV_CALL(shifts = (int*)cvAlloc(sizeof(int)*numImages));
 388     memset(shifts,0,sizeof(int)*numImages);
 389
 390     for( currPoint = 0; currPoint < numPoints; currPoint++ )//For each point (matrix V)
 391     {
 392         int i,j;
 393
 394         for( i = 0; i < 4; i++ )
 395         {
 396             for( j = 0; j < 4; j++ )
 397             {
 398                 double sum = 0;
 399                 for( currImage = 0; currImage < numImages; currImage++ )
 400                 {
 401                     if( cvmGet(presPoints[currImage],0,currPoint) > 0 )
 402                     {
 403                         sum += cvmGet(pointDeriv[currImage],0,shifts[currImage]*4+i) *
 404                                cvmGet(pointDeriv[currImage],0,shifts[currImage]*4+j);
 405
 406                         sum += cvmGet(pointDeriv[currImage],1,shifts[currImage]*4+i) *
 407                                cvmGet(pointDeriv[currImage],1,shifts[currImage]*4+j);
 408                     }
 409                 }
 410
 411                 cvmSet(matrV[currPoint],i,j,sum);
 412             }
 413         }
 414
 415
 416         /* shift position of visible points */
 417         for( currImage = 0; currImage < numImages; currImage++ )
 418         {
 419             if( cvmGet(presPoints[currImage],0,currPoint) > 0 )
 420             {
 421                 shifts[currImage]++;
 422             }
 423         }
 424     }
 425
 426     __END__;
 427     cvFree( &shifts);
 428
 429     return;
 430 }
 431 /*======================================================================================*/
 432 static void icvComputeMatrixUAll(int numImages,CvMat **projDeriv,CvMat** matrU)
 433 {
 434     CV_FUNCNAME( "icvComputeMatrixVAll" );
 435     __BEGIN__;
 436     /* ----- Test input params for errors ----- */
 437     if( numImages < 1 )
 438     {
 439         CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
 440     }
 441     if( projDeriv == 0 || matrU == 0 )
 442     {
 443         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 444     }
 445     /* !!! Not tested all input parameters */
 446     /* ----- End test ----- */
 447
 448     /* Compute matrices V */
 449     int currImage;
 450     for( currImage = 0; currImage < numImages; currImage++ )
 451     {
 452         cvMulTransposed(projDeriv[currImage],matrU[currImage],1);
 453     }
 454
 455     __END__;
 456     return;
 457 }
 458 /*======================================================================================*/
 459 static void icvComputeMatrixW(int numImages, CvMat **projDeriv, CvMat **pointDeriv, CvMat **presPoints, CvMat *matrW)
 460 {
 461     CV_FUNCNAME( "icvComputeMatrixW" );
 462     __BEGIN__;
 463
 464     /* ----- Test input params for errors ----- */
 465     if( numImages < 1 )
 466     {
 467         CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
 468     }
 469     if( projDeriv == 0 || pointDeriv == 0 || presPoints == 0 || matrW == 0 )
 470     {
 471         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 472     }
 473     int numPoints;
 474     numPoints = presPoints[0]->cols;
 475     if( numPoints < 1 )
 476     {
 477         CV_ERROR( CV_StsOutOfRange, "Number of points must more than zero" );
 478     }
 479     if( !CV_IS_MAT(matrW) )
 480     {
 481         CV_ERROR( CV_StsUnsupportedFormat, "matrW must be a matrix 12NumIm x 4NumPnt" );
 482     }
 483     if( matrW->rows != numImages*12 || matrW->cols != numPoints*4 )
 484     {
 485         CV_ERROR( CV_StsOutOfRange, "matrW must be a matrix 12NumIm x 4NumPnt" );
 486     }
 487     /* !!! Not tested all input parameters */
 488     /* ----- End test ----- */
 489
 490     /* Compute number of points */
 491     /* Compute matrix W using derivate proj and points */
 492
 493     int currImage;
 494
 495     for( currImage = 0; currImage < numImages; currImage++ )
 496     {
 497         for( int currLine = 0; currLine < 12; currLine++ )
 498         {
 499             int currVis = 0;
 500             for( int currPoint = 0; currPoint < numPoints; currPoint++ )
 501             {
 502                 if( cvmGet(presPoints[currImage],0,currPoint) > 0 )
 503                 {
 504
 505                     for( int currCol = 0; currCol < 4; currCol++ )
 506                     {
 507                         double sum;
 508                         sum = cvmGet(projDeriv[currImage],currVis*2+0,currLine) *
 509                               cvmGet(pointDeriv[currImage],0,currVis*4+currCol);
 510
 511                         sum += cvmGet(projDeriv[currImage],currVis*2+1,currLine) *
 512                               cvmGet(pointDeriv[currImage],1,currVis*4+currCol);
 513
 514                         cvmSet(matrW,currImage*12+currLine,currPoint*4+currCol,sum);
 515                     }
 516                     currVis++;
 517                 }
 518                 else
 519                 {/* set all sub elements to zero */
 520                     for( int currCol = 0; currCol < 4; currCol++ )
 521                     {
 522                         cvmSet(matrW,currImage*12+currLine,currPoint*4+currCol,0);
 523                     }
 524                 }
 525             }
 526         }
 527     }
 528
 529 #ifdef TRACK_BUNDLE
 530     {
 531         FILE *file;
 532         file = fopen( TRACK_BUNDLE_FILE_MATRW ,"w");
 533         int currPoint,currImage;
 534         for( currPoint = 0; currPoint < numPoints; currPoint++ )
 535         {
 536             fprintf(file,"\nPoint=%d\n",currPoint);
 537             int currRow;
 538             for( currRow = 0; currRow < 4; currRow++  )
 539             {
 540                 for( currImage = 0; currImage< numImages; currImage++ )
 541                 {
 542                     int i;
 543                     for( i = 0; i < 12; i++ )
 544                     {
 545                         double val = cvmGet(matrW, currImage * 12 + i, currPoint * 4 + currRow);
 546                         fprintf(file,"%lf ",val);
 547                     }
 548                 }
 549                 fprintf(file,"\n");
 550             }
 551         }
 552         fclose(file);
 553     }
 554 #endif
 555
 556     __END__;
 557     return;
 558 }
 559
 560 /*======================================================================================*/
 561 /* Compute jacobian mult projection matrices error */
 562 static void icvComputeJacErrorProj(int numImages,CvMat **projDeriv,CvMat **projErrors,CvMat *jacProjErr )
 563 {
 564     CV_FUNCNAME( "icvComputeJacErrorProj" );
 565     __BEGIN__;
 566
 567     /* ----- Test input params for errors ----- */
 568     if( numImages < 1 )
 569     {
 570         CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
 571     }
 572     if( projDeriv == 0 || projErrors == 0 || jacProjErr == 0 )
 573     {
 574         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 575     }
 576     if( !CV_IS_MAT(jacProjErr) )
 577     {
 578         CV_ERROR( CV_StsUnsupportedFormat, "jacProjErr must be a matrix 12NumIm x 1" );
 579     }
 580     if( jacProjErr->rows != numImages*12 || jacProjErr->cols != 1 )
 581     {
 582         CV_ERROR( CV_StsOutOfRange, "jacProjErr must be a matrix 12NumIm x 1" );
 583     }
 584     /* !!! Not tested all input parameters */
 585     /* ----- End test ----- */
 586
 587     int currImage;
 588     for( currImage = 0; currImage < numImages; currImage++ )
 589     {
 590         for( int currCol = 0; currCol < 12; currCol++ )
 591         {
 592             int num = projDeriv[currImage]->rows;
 593             double sum = 0;
 594             for( int i = 0; i < num; i++ )
 595             {
 596                 sum += cvmGet(projDeriv[currImage],i,currCol) *
 597                        cvmGet(projErrors[currImage],i%2,i/2);
 598             }
 599             cvmSet(jacProjErr,currImage*12+currCol,0,sum);
 600         }
 601     }
 602
 603 #ifdef TRACK_BUNDLE
 604     {
 605         FILE *file;
 606         file = fopen( TRACK_BUNDLE_FILE_JACERRPROJ ,"w");
 607         int currImage;
 608         for( currImage = 0; currImage < numImages; currImage++ )
 609         {
 610             fprintf(file,"\nImage=%d\n",currImage);
 611             int currRow;
 612             for( currRow = 0; currRow < 12; currRow++  )
 613             {
 614                 double val = cvmGet(jacProjErr, currImage * 12 + currRow, 0);
 615                 fprintf(file,"%lf\n",val);
 616             }
 617             fprintf(file,"\n");
 618         }
 619         fclose(file);
 620     }
 621 #endif
 622
 623
 624     __END__;
 625     return;
 626 }
 627
 628 /*======================================================================================*/
 629 /* Compute jacobian mult points error */
 630 static void icvComputeJacErrorPoint(int numImages,CvMat **pointDeriv,CvMat **projErrors, CvMat **presPoints,CvMat *jacPointErr )
 631 {
 632     int *shifts = 0;
 633
 634     CV_FUNCNAME( "icvComputeJacErrorPoint" );
 635     __BEGIN__;
 636
 637     /* ----- Test input params for errors ----- */
 638     if( numImages < 1 )
 639     {
 640         CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
 641     }
 642
 643     if( pointDeriv == 0 || projErrors == 0 || presPoints == 0 || jacPointErr == 0 )
 644     {
 645         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 646     }
 647
 648     int numPoints;
 649     numPoints = presPoints[0]->cols;
 650     if( numPoints < 1 )
 651     {
 652         CV_ERROR( CV_StsOutOfRange, "Number of points must more than zero" );
 653     }
 654
 655     if( !CV_IS_MAT(jacPointErr) )
 656     {
 657         CV_ERROR( CV_StsUnsupportedFormat, "jacPointErr must be a matrix 4NumPnt x 1" );
 658     }
 659
 660     if( jacPointErr->rows != numPoints*4 || jacPointErr->cols != 1 )
 661     {
 662         CV_ERROR( CV_StsOutOfRange, "jacPointErr must be a matrix 4NumPnt x 1" );
 663     }
 664     /* !!! Not tested all input parameters */
 665     /* ----- End test ----- */
 666
 667     int currImage;
 668     int currPoint;
 669     CV_CALL(shifts = (int*)cvAlloc(sizeof(int)*numImages));
 670     memset(shifts,0,sizeof(int)*numImages);
 671     for( currPoint = 0; currPoint < numPoints; currPoint++ )
 672     {
 673         for( int currCoord = 0; currCoord < 4; currCoord++ )
 674         {
 675             double sum = 0;
 676             {
 677                 int currVis = 0;
 678                 for( currImage = 0; currImage < numImages; currImage++ )
 679                 {
 680                     if( cvmGet(presPoints[currImage],0,currPoint) > 0 )
 681                     {
 682                         sum += cvmGet(pointDeriv[currImage],0,shifts[currImage]*4+currCoord) *
 683                                cvmGet(projErrors[currImage],0,shifts[currImage]);//currVis);
 684
 685                         sum += cvmGet(pointDeriv[currImage],1,shifts[currImage]*4+currCoord) *
 686                                cvmGet(projErrors[currImage],1,shifts[currImage]);//currVis);
 687
 688                         currVis++;
 689                     }
 690                 }
 691             }
 692
 693             cvmSet(jacPointErr,currPoint*4+currCoord,0,sum);
 694         }
 695
 696         /* Increase shifts */
 697         for( currImage = 0; currImage < numImages; currImage++ )
 698         {
 699             if( cvmGet(presPoints[currImage],0,currPoint) > 0 )
 700             {
 701                 shifts[currImage]++;
 702             }
 703         }
 704     }
 705
 706
 707 #ifdef TRACK_BUNDLE
 708     {
 709         FILE *file;
 710         file = fopen(TRACK_BUNDLE_FILE_JACERRPNT,"w");
 711         int currPoint;
 712         for( currPoint = 0; currPoint < numPoints; currPoint++ )
 713         {
 714             fprintf(file,"\nPoint=%d\n",currPoint);
 715             int currRow;
 716             for( currRow = 0; currRow < 4; currRow++  )
 717             {
 718                 double val = cvmGet(jacPointErr, currPoint * 4 + currRow, 0);
 719                 fprintf(file,"%lf\n",val);
 720             }
 721             fprintf(file,"\n");
 722         }
 723         fclose(file);
 724     }
 725 #endif
 726
 727
 728
 729     __END__;
 730     cvFree( &shifts);
 731
 732 }
 733 /*======================================================================================*/
 734
 735
 736 /* Reconstruct 4D points using status */
 737 void icvReconstructPoints4DStatus(CvMat** projPoints, CvMat **projMatrs, CvMat** presPoints,
 738                                   CvMat *points4D,int numImages,CvMat **projError)
 739 {
 740
 741     double* matrA_dat = 0;
 742     double* matrW_dat = 0;
 743
 744     CV_FUNCNAME( "icvReconstructPoints4DStatus" );
 745
 746     __BEGIN__;
 747
 748     /* ----- Test input params for errors ----- */
 749     if( numImages < 2 )
 750     {
 751         CV_ERROR( CV_StsOutOfRange, "Number of images must be more than one" );
 752     }
 753
 754     if( projPoints == 0 || projMatrs == 0 || presPoints == 0 || points4D == 0 )
 755     {
 756         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 757     }
 758
 759     int numPoints;
 760     numPoints = points4D->cols;
 761     if( numPoints < 1 )
 762     {
 763         CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" );
 764     }
 765
 766     if( points4D->rows != 4 )
 767     {
 768         CV_ERROR( CV_StsOutOfRange, "Points must have 4 cordinates" );
 769     }
 770
 771
 772     /* !!! Not tested all input parameters */
 773     /* ----- End test ----- */
 774
 775     int currImage;
 776     int currPoint;
 777
 778     /* Allocate maximum data */
 779
 780     {
 781         double matrV_dat[4*4];
 782         CvMat matrV = cvMat(4,4,CV_64F,matrV_dat);
 783
 784         CV_CALL(matrA_dat = (double*)cvAlloc(3*numImages * 4 * sizeof(double)));
 785         CV_CALL(matrW_dat = (double*)cvAlloc(3*numImages * 4 * sizeof(double)));
 786
 787         /* reconstruct each point */
 788         for( currPoint = 0; currPoint < numPoints; currPoint++ )
 789         {
 790             /* Reconstruct current point */
 791             /* Define number of visible projections */
 792             int numVisProj = 0;
 793             for( currImage = 0; currImage < numImages; currImage++ )
 794             {
 795                 if( cvmGet(presPoints[currImage],0,currPoint) > 0 )
 796                 {
 797                     numVisProj++;
 798                 }
 799             }
 800
 801             if( numVisProj < 2 )
 802             {
 803                 /* This point can't be reconstructed */
 804                 continue;
 805             }
 806
 807             /* Allocate memory and create matrices */
 808             CvMat matrA;
 809             matrA = cvMat(3*numVisProj,4,CV_64F,matrA_dat);
 810
 811             CvMat matrW;
 812             matrW = cvMat(3*numVisProj,4,CV_64F,matrW_dat);
 813
 814             int currVisProj = 0;
 815             for( currImage = 0; currImage < numImages; currImage++ )/* For each view */
 816             {
 817                 if( cvmGet(presPoints[currImage],0,currPoint) > 0 )
 818                 {
 819                     double x,y;
 820                     x = cvmGet(projPoints[currImage],0,currPoint);
 821                     y = cvmGet(projPoints[currImage],1,currPoint);
 822                     for( int k = 0; k < 4; k++ )
 823                     {
 824                         matrA_dat[currVisProj*12   + k] =
 825                                x * cvmGet(projMatrs[currImage],2,k) -     cvmGet(projMatrs[currImage],0,k);
 826
 827                         matrA_dat[currVisProj*12+4 + k] =
 828                                y * cvmGet(projMatrs[currImage],2,k) -     cvmGet(projMatrs[currImage],1,k);
 829
 830                         matrA_dat[currVisProj*12+8 + k] =
 831                                x * cvmGet(projMatrs[currImage],1,k) - y * cvmGet(projMatrs[currImage],0,k);
 832                     }
 833                     currVisProj++;
 834                 }
 835             }
 836
 837             /* Solve system for current point */
 838             {
 839                 cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T);
 840
 841                 /* Copy computed point */
 842                 cvmSet(points4D,0,currPoint,cvmGet(&matrV,3,0));//X
 843                 cvmSet(points4D,1,currPoint,cvmGet(&matrV,3,1));//Y
 844                 cvmSet(points4D,2,currPoint,cvmGet(&matrV,3,2));//Z
 845                 cvmSet(points4D,3,currPoint,cvmGet(&matrV,3,3));//W
 846             }
 847
 848         }
 849     }
 850
 851     {/* Compute projection error */
 852         for( currImage = 0; currImage < numImages; currImage++ )
 853         {
 854             CvMat point4D;
 855             CvMat point3D;
 856             double point3D_dat[3];
 857             point3D = cvMat(3,1,CV_64F,point3D_dat);
 858
 859             int numVis = 0;
 860             double totalError = 0;
 861             for(int curPoint = 0; curPoint < numPoints; curPoint++ )
 862             {
 863                 if( cvmGet(presPoints[currImage],0,curPoint) > 0)
 864                 {
 865                     double  dx,dy;
 866                     cvGetCol(points4D,&point4D,curPoint);
 867                     cvmMul(projMatrs[currImage],&point4D,&point3D);
 868                     double w = point3D_dat[2];
 869                     double x = point3D_dat[0] / w;
 870                     double y = point3D_dat[1] / w;
 871
 872                     dx = cvmGet(projPoints[currImage],0,curPoint) - x;
 873                     dy = cvmGet(projPoints[currImage],1,curPoint) - y;
 874                     if( projError )
 875                     {
 876                         cvmSet(projError[currImage],0,curPoint,dx);
 877                         cvmSet(projError[currImage],1,curPoint,dy);
 878                     }
 879                     totalError += sqrt(dx*dx+dy*dy);
 880                     numVis++;
 881                 }
 882             }
 883
 884             //double meanError = totalError / (double)numVis;
 885
 886         }
 887
 888     }
 889
 890     __END__;
 891
 892     cvFree( &matrA_dat);
 893     cvFree( &matrW_dat);
 894
 895     return;
 896 }
 897
 898 /*======================================================================================*/
 899
 900 static void icvProjPointsStatusFunc( int numImages, CvMat *points4D, CvMat **projMatrs, CvMat **pointsPres, CvMat **projPoints)
 901 {
 902     CV_FUNCNAME( "icvProjPointsStatusFunc" );
 903     __BEGIN__;
 904
 905     /* ----- Test input params for errors ----- */
 906     if( numImages < 1 )
 907     {
 908         CV_ERROR( CV_StsOutOfRange, "Number of images must be more than zero" );
 909     }
 910
 911     if( points4D == 0 || projMatrs == 0 || pointsPres == 0 || projPoints == 0 )
 912     {
 913         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 914     }
 915     {
 916     int numPoints;
 917     numPoints = points4D->cols;
 918     if( numPoints < 1 )
 919     {
 920         CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" );
 921     }
 922
 923     if( points4D->rows != 4 )
 924     {
 925         CV_ERROR( CV_StsOutOfRange, "Points must have 4 cordinates" );
 926     }
 927
 928     /* !!! Not tested all input parameters */
 929     /* ----- End test ----- */
 930
 931     CvMat point4D;
 932     CvMat point3D;
 933     double point4D_dat[4];
 934     double point3D_dat[3];
 935     point4D = cvMat(4,1,CV_64F,point4D_dat);
 936     point3D = cvMat(3,1,CV_64F,point3D_dat);
 937
 938 #ifdef TRACK_BUNDLE
 939         {
 940             FILE *file;
 941             file = fopen( TRACK_BUNDLE_FILE ,"a");
 942             fprintf(file,"\n----- test 14.01 icvProjPointsStatusFunc -----\n");
 943             fclose(file);
 944         }
 945 #endif
 946
 947     int currImage;
 948     for( currImage = 0; currImage < numImages; currImage++ )
 949     {
 950         /* Get project matrix */
 951         /* project visible points using current projection matrix */
 952         int currVisPoint = 0;
 953         for( int currPoint = 0; currPoint < numPoints; currPoint++ )
 954         {
 955             if( cvmGet(pointsPres[currImage],0,currPoint) > 0 )
 956             {
 957                 /* project current point */
 958                 cvGetSubRect(points4D,&point4D,cvRect(currPoint,0,1,4));
 959
 960 #ifdef TRACK_BUNDLE
 961                 {
 962                     FILE *file;
 963                     file = fopen( TRACK_BUNDLE_FILE ,"a");
 964                     fprintf(file,"\n----- test 14.02 point4D (%lf, %lf, %lf, %lf) -----\n",
 965                                  cvmGet(&point4D,0,0),
 966                                  cvmGet(&point4D,1,0),
 967                                  cvmGet(&point4D,2,0),
 968                                  cvmGet(&point4D,3,0));
 969                     fclose(file);
 970                 }
 971 #endif
 972
 973                 cvmMul(projMatrs[currImage],&point4D,&point3D);
 974                 double w = point3D_dat[2];
 975                 cvmSet(projPoints[currImage],0,currVisPoint,point3D_dat[0]/w);
 976                 cvmSet(projPoints[currImage],1,currVisPoint,point3D_dat[1]/w);
 977
 978 #ifdef TRACK_BUNDLE
 979                 {
 980                     FILE *file;
 981                     file = fopen( TRACK_BUNDLE_FILE ,"a");
 982                     fprintf(file,"\n----- test 14.03 (%lf, %lf, %lf) -> (%lf, %lf)-----\n",
 983                                  point3D_dat[0],
 984                                  point3D_dat[1],
 985                                  point3D_dat[2],
 986                                  point3D_dat[0]/w,
 987                                  point3D_dat[1]/w
 988                                  );
 989                     fclose(file);
 990                 }
 991 #endif
 992                 currVisPoint++;
 993             }
 994         }
 995     }
 996     }
 997     __END__;
 998 }
 999
1000 /*======================================================================================*/
1001 static void icvFreeMatrixArray(CvMat ***matrArray,int numMatr)
1002 {
1003     /* Free each matrix */
1004     int currMatr;
1005
1006     if( *matrArray != 0 )
1007     {/* Need delete */
1008         for( currMatr = 0; currMatr < numMatr; currMatr++ )
1009         {
1010             cvReleaseMat((*matrArray)+currMatr);
1011         }
1012         cvFree( matrArray);
1013     }
1014     return;
1015 }
1016
1017 /*======================================================================================*/
1018 static void *icvClearAlloc(int size)
1019 {
1020     void *ptr = 0;
1021
1022     CV_FUNCNAME( "icvClearAlloc" );
1023     __BEGIN__;
1024
1025     if( size > 0 )
1026     {
1027         CV_CALL(ptr = cvAlloc(size));
1028         memset(ptr,0,size);
1029     }
1030
1031     __END__;
1032     return ptr;
1033 }
1034
1035 /*======================================================================================*/
1036 #if 0
1037 void cvOptimizeLevenbergMarquardtBundleWraper( CvMat** projMatrs, CvMat** observProjPoints,
1038                                        CvMat** pointsPres, int numImages,
1039                                        CvMat** resultProjMatrs, CvMat* resultPoints4D,int maxIter,double epsilon )
1040 {
1041     /* Delete al sparse points */
1042
1043 int icvDeleteSparsInPoints(  int numImages,
1044                              CvMat **points,
1045                              CvMat **status,
1046                              CvMat *wasStatus)/* status of previous configuration */
1047
1048 }
1049 #endif
1050
1051 /*======================================================================================*/
1052 /* !!! may be useful to return norm of error */
1053 /* !!! may be does not work correct with not all visible 4D points */
1054 void cvOptimizeLevenbergMarquardtBundle( CvMat** projMatrs, CvMat** observProjPoints,
1055                                        CvMat** pointsPres, int numImages,
1056                                        CvMat** resultProjMatrs, CvMat* resultPoints4D,int maxIter,double epsilon )
1057 {
1058
1059     CvMat  *vectorX_points4D = 0;
1060     CvMat **vectorX_projMatrs = 0;
1061
1062     CvMat  *newVectorX_points4D = 0;
1063     CvMat **newVectorX_projMatrs = 0;
1064
1065     CvMat  *changeVectorX_points4D = 0;
1066     CvMat  *changeVectorX_projMatrs = 0;
1067
1068     CvMat **observVisPoints = 0;
1069     CvMat **projVisPoints = 0;
1070     CvMat **errorProjPoints = 0;
1071     CvMat **DerivProj = 0;
1072     CvMat **DerivPoint = 0;
1073     CvMat *matrW = 0;
1074     CvMat **matrsUk = 0;
1075     CvMat **workMatrsUk = 0;
1076     CvMat **matrsVi = 0;
1077     CvMat *workMatrVi = 0;
1078     CvMat **workMatrsInvVi = 0;
1079     CvMat *jacProjErr = 0;
1080     CvMat *jacPointErr = 0;
1081
1082     CvMat *matrTmpSys1 = 0;
1083     CvMat *matrSysDeltaP = 0;
1084     CvMat *vectTmpSys3 = 0;
1085     CvMat *vectSysDeltaP = 0;
1086     CvMat *deltaP = 0;
1087     CvMat *deltaM = 0;
1088     CvMat *vectTmpSysM = 0;
1089
1090     int numPoints = 0;
1091
1092
1093     CV_FUNCNAME( "cvOptimizeLevenbergMarquardtBundle" );
1094     __BEGIN__;
1095
1096     /* ----- Test input params for errors ----- */
1097     if( numImages < 1 )
1098     {
1099         CV_ERROR( CV_StsOutOfRange, "Number of images must be more than zero" );
1100     }
1101
1102     if( maxIter < 1 || maxIter > 2000 )
1103     {
1104         CV_ERROR( CV_StsOutOfRange, "Maximum number of iteration must be in [1..1000]" );
1105     }
1106
1107     if( epsilon < 0  )
1108     {
1109         CV_ERROR( CV_StsOutOfRange, "Epsilon parameter must be >= 0" );
1110     }
1111
1112     if( !CV_IS_MAT(resultPoints4D) )
1113     {
1114         CV_ERROR( CV_StsUnsupportedFormat, "resultPoints4D must be a matrix 4 x NumPnt" );
1115     }
1116
1117     numPoints = resultPoints4D->cols;
1118     if( numPoints < 1 )
1119     {
1120         CV_ERROR( CV_StsOutOfRange, "Number of points must be more than zero" );//!!!
1121     }
1122
1123     if( resultPoints4D->rows != 4 )
1124     {
1125         CV_ERROR( CV_StsOutOfRange, "resultPoints4D must have 4 cordinates" );
1126     }
1127
1128     /* ----- End test ----- */
1129
1130     /* Optimization using bundle adjustment */
1131     /* work with non visible points */
1132
1133     CV_CALL( vectorX_points4D  = cvCreateMat(4,numPoints,CV_64F));
1134     CV_CALL( vectorX_projMatrs = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages));
1135
1136     CV_CALL( newVectorX_points4D  = cvCreateMat(4,numPoints,CV_64F));
1137     CV_CALL( newVectorX_projMatrs = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages));
1138
1139     CV_CALL( changeVectorX_points4D  = cvCreateMat(4,numPoints,CV_64F));
1140     CV_CALL( changeVectorX_projMatrs = cvCreateMat(3,4,CV_64F));
1141
1142     /* ----- Test input params ----- */
1143     for(int currImage = 0; currImage < numImages; currImage++ )
1144     {
1145         /* Test size of input initial and result projection matrices */
1146         if( !CV_IS_MAT(projMatrs[currImage]) )
1147         {
1148             CV_ERROR( CV_StsUnsupportedFormat, "each of initial projMatrs must be a matrix 3 x 4" );
1149         }
1150         if( projMatrs[currImage]->rows != 3 || projMatrs[currImage]->cols != 4 )
1151         {
1152             CV_ERROR( CV_StsOutOfRange, "each of initial projMatrs must be a matrix 3 x 4" );
1153         }
1154
1155
1156         if( !CV_IS_MAT(observProjPoints[currImage]) )
1157         {
1158             CV_ERROR( CV_StsUnsupportedFormat, "each of observProjPoints must be a matrix 2 x NumPnts" );
1159         }
1160         if( observProjPoints[currImage]->rows != 2 || observProjPoints[currImage]->cols != numPoints )
1161         {
1162             CV_ERROR( CV_StsOutOfRange, "each of observProjPoints must be a matrix 2 x NumPnts" );
1163         }
1164
1165         if( !CV_IS_MAT(pointsPres[currImage]) )
1166         {
1167             CV_ERROR( CV_StsUnsupportedFormat, "each of pointsPres must be a matrix 1 x NumPnt" );
1168         }
1169         if( pointsPres[currImage]->rows != 1 || pointsPres[currImage]->cols != numPoints )
1170         {
1171             CV_ERROR( CV_StsOutOfRange, "each of pointsPres must be a matrix 1 x NumPnt" );
1172         }
1173
1174         if( !CV_IS_MAT(resultProjMatrs[currImage]) )
1175         {
1176             CV_ERROR( CV_StsUnsupportedFormat, "each of resultProjMatrs must be a matrix 3 x 4" );
1177         }
1178         if( resultProjMatrs[currImage]->rows != 3 || resultProjMatrs[currImage]->cols != 4 )
1179         {
1180             CV_ERROR( CV_StsOutOfRange, "each of resultProjMatrs must be a matrix 3 x 4" );
1181         }
1182
1183     }
1184     /* ----- End test ----- */
1185
1186     /* Copy projection matrices to vectorX0 */
1187     for(int currImage = 0; currImage < numImages; currImage++ )
1188     {
1189         CV_CALL( vectorX_projMatrs[currImage] = cvCreateMat(3,4,CV_64F));
1190         CV_CALL( newVectorX_projMatrs[currImage] = cvCreateMat(3,4,CV_64F));
1191         cvCopy(projMatrs[currImage],vectorX_projMatrs[currImage]);
1192     }
1193
1194     /* Reconstruct points4D using projection matrices and status information */
1195     icvReconstructPoints4DStatus(observProjPoints, projMatrs, pointsPres, vectorX_points4D, numImages);
1196
1197     /* ----- Allocate memory for work matrices ----- */
1198     /* Compute number of good points on each images */
1199
1200     CV_CALL( observVisPoints = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) );
1201     CV_CALL( projVisPoints   = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) );
1202     CV_CALL( errorProjPoints = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) );
1203     CV_CALL( DerivProj       = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) );
1204     CV_CALL( DerivPoint      = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) );
1205     CV_CALL( matrW           = cvCreateMat(12*numImages,4*numPoints,CV_64F) );
1206     CV_CALL( matrsUk         = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) );
1207     CV_CALL( workMatrsUk     = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) );
1208     CV_CALL( matrsVi         = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numPoints) );
1209     CV_CALL( workMatrVi      = cvCreateMat(4,4,CV_64F) );
1210     CV_CALL( workMatrsInvVi  = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numPoints) );
1211
1212     CV_CALL( jacProjErr      = cvCreateMat(12*numImages,1,CV_64F) );
1213     CV_CALL( jacPointErr     = cvCreateMat(4*numPoints,1,CV_64F) );
1214
1215
1216     int i;
1217     for( i = 0; i < numPoints; i++ )
1218     {
1219         CV_CALL( matrsVi[i]        = cvCreateMat(4,4,CV_64F) );
1220         CV_CALL( workMatrsInvVi[i] = cvCreateMat(4,4,CV_64F) );
1221     }
1222
1223     for(int currImage = 0; currImage < numImages; currImage++ )
1224     {
1225         CV_CALL( matrsUk[currImage]     = cvCreateMat(12,12,CV_64F) );
1226         CV_CALL( workMatrsUk[currImage] = cvCreateMat(12,12,CV_64F) );
1227
1228         int currVisPoint = 0, currPoint, numVisPoint;
1229         for( currPoint = 0; currPoint < numPoints; currPoint++ )
1230         {
1231             if( cvmGet(pointsPres[currImage],0,currPoint) > 0 )
1232             {
1233                 currVisPoint++;
1234             }
1235         }
1236
1237         numVisPoint = currVisPoint;
1238
1239         /* Allocate memory for current image data */
1240         CV_CALL( observVisPoints[currImage]  = cvCreateMat(2,numVisPoint,CV_64F) );
1241         CV_CALL( projVisPoints[currImage]    = cvCreateMat(2,numVisPoint,CV_64F) );
1242
1243         /* create error matrix */
1244         CV_CALL( errorProjPoints[currImage] = cvCreateMat(2,numVisPoint,CV_64F) );
1245
1246         /* Create derivate matrices */
1247         CV_CALL( DerivProj[currImage]  = cvCreateMat(2*numVisPoint,12,CV_64F) );
1248         CV_CALL( DerivPoint[currImage] = cvCreateMat(2,numVisPoint*4,CV_64F) );
1249
1250         /* Copy observed projected visible points */
1251         currVisPoint = 0;
1252         for( currPoint = 0; currPoint < numPoints; currPoint++ )
1253         {
1254             if( cvmGet(pointsPres[currImage],0,currPoint) > 0 )
1255             {
1256                 cvmSet(observVisPoints[currImage],0,currVisPoint,cvmGet(observProjPoints[currImage],0,currPoint));
1257                 cvmSet(observVisPoints[currImage],1,currVisPoint,cvmGet(observProjPoints[currImage],1,currPoint));
1258                 currVisPoint++;
1259             }
1260         }
1261     }
1262     /*---------------------------------------------------*/
1263
1264     CV_CALL( matrTmpSys1   = cvCreateMat(numPoints*4, numImages*12, CV_64F) );
1265     CV_CALL( matrSysDeltaP = cvCreateMat(numImages*12, numImages*12, CV_64F) );
1266     CV_CALL( vectTmpSys3   = cvCreateMat(numPoints*4,1,CV_64F) );
1267     CV_CALL( vectSysDeltaP = cvCreateMat(numImages*12,1,CV_64F) );
1268     CV_CALL( deltaP        = cvCreateMat(numImages*12,1,CV_64F) );
1269     CV_CALL( deltaM        = cvCreateMat(numPoints*4,1,CV_64F) );
1270     CV_CALL( vectTmpSysM   = cvCreateMat(numPoints*4,1,CV_64F) );
1271
1272
1273 //#ifdef TRACK_BUNDLE
1274 #if 1
1275     {
1276         /* Create file to track */
1277         FILE* file;
1278         file = fopen( TRACK_BUNDLE_FILE ,"w");
1279         fprintf(file,"begin\n");
1280         fclose(file);
1281     }
1282 #endif
1283
1284     /* ============= main optimization loop ============== */
1285
1286     /* project all points using current vector X */
1287     icvProjPointsStatusFunc(numImages, vectorX_points4D, vectorX_projMatrs, pointsPres, projVisPoints);
1288
1289     /* Compute error with observed value and computed projection */
1290     double prevError;
1291     prevError = 0;
1292     for(int currImage = 0; currImage < numImages; currImage++ )
1293     {
1294         cvSub(observVisPoints[currImage],projVisPoints[currImage],errorProjPoints[currImage]);
1295         double currNorm = cvNorm(errorProjPoints[currImage]);
1296         prevError += currNorm * currNorm;
1297     }
1298     prevError = sqrt(prevError);
1299
1300     int currIter;
1301     double change;
1302     double alpha;
1303
1304 //#ifdef TRACK_BUNDLE
1305 #if 1
1306     {
1307         /* Create file to track */
1308         FILE* file;
1309         file = fopen( TRACK_BUNDLE_FILE ,"a");
1310         fprintf(file,"\n========================================\n");;
1311         fprintf(file,"Iter=0\n");
1312         fprintf(file,"Error = %20.15lf\n",prevError);
1313         fprintf(file,"Change = %20.15lf\n",1.0);
1314
1315         fprintf(file,"projection errors\n");
1316
1317         /* Print all proejction errors */
1318         for(int currImage = 0; currImage < numImages; currImage++)
1319         {
1320             fprintf(file,"\nImage=%d\n",currImage);
1321             int numPn = errorProjPoints[currImage]->cols;
1322             for( int currPoint = 0; currPoint < numPn; currPoint++ )
1323             {
1324                 double ex,ey;
1325                 ex = cvmGet(errorProjPoints[currImage],0,currPoint);
1326                 ey = cvmGet(errorProjPoints[currImage],1,currPoint);
1327                 fprintf(file,"%40.35lf, %40.35lf\n",ex,ey);
1328             }
1329         }
1330         fclose(file);
1331     }
1332 #endif
1333
1334     currIter = 0;
1335     change = 1;
1336     alpha = 0.001;
1337
1338
1339     do
1340     {
1341
1342 #ifdef TRACK_BUNDLE
1343         {
1344             FILE *file;
1345             file = fopen( TRACK_BUNDLE_FILE ,"a");
1346             fprintf(file,"\n----- test 6 do main -----\n");
1347
1348             double norm = cvNorm(vectorX_points4D);
1349             fprintf(file,"        test 6.01 prev normPnts=%lf\n",norm);
1350
1351             for( int i=0;i<numImages;i++ )
1352             {
1353                 double norm = cvNorm(vectorX_projMatrs[i]);
1354                 fprintf(file,"        test 6.01 prev normProj=%lf\n",norm);
1355             }
1356
1357             fclose(file);
1358         }
1359 #endif
1360
1361         /* Compute derivates by projectinon matrices */
1362         icvComputeDerivateProjAll(vectorX_points4D,vectorX_projMatrs,pointsPres,numImages,DerivProj);
1363
1364         /* Compute derivates by 4D points */
1365         icvComputeDerivatePointsAll(vectorX_points4D,vectorX_projMatrs,pointsPres,numImages,DerivPoint);
1366
1367         /* Compute matrces Uk */
1368         icvComputeMatrixUAll(numImages,DerivProj,matrsUk);
1369         icvComputeMatrixVAll(numImages,DerivPoint,pointsPres,matrsVi);
1370         icvComputeMatrixW(numImages,DerivProj,DerivPoint,pointsPres,matrW);
1371
1372
1373 #ifdef TRACK_BUNDLE
1374         {
1375             FILE *file;
1376             file = fopen( TRACK_BUNDLE_FILE ,"a");
1377             fprintf(file,"\n----- test 6.03 do matrs U V -----\n");
1378
1379             int i;
1380             for( i = 0; i < numImages; i++ )
1381             {
1382                 double norm = cvNorm(matrsUk[i]);
1383                 fprintf(file,"        test 6.01 prev matrsUk=%lf\n",norm);
1384             }
1385
1386             for( i = 0; i < numPoints; i++ )
1387             {
1388                 double norm = cvNorm(matrsVi[i]);
1389                 fprintf(file,"        test 6.01 prev matrsVi=%lf\n",norm);
1390             }
1391
1392             fclose(file);
1393         }
1394 #endif
1395
1396         /* Compute jac errors */
1397         icvComputeJacErrorProj(numImages, DerivProj, errorProjPoints, jacProjErr);
1398         icvComputeJacErrorPoint(numImages, DerivPoint, errorProjPoints, pointsPres, jacPointErr);
1399
1400 #ifdef TRACK_BUNDLE
1401         {
1402             FILE *file;
1403             file = fopen( TRACK_BUNDLE_FILE ,"a");
1404             fprintf(file,"\n----- test 6 do main -----\n");
1405             double norm1 = cvNorm(vectorX_points4D);
1406             fprintf(file,"        test 6.02 post normPnts=%lf\n",norm1);
1407             fclose(file);
1408         }
1409 #endif
1410         /* Copy matrices Uk to work matrices Uk */
1411         for(int currImage = 0; currImage < numImages; currImage++ )
1412         {
1413             cvCopy(matrsUk[currImage],workMatrsUk[currImage]);
1414         }
1415
1416 #ifdef TRACK_BUNDLE
1417         {
1418             FILE *file;
1419             file = fopen( TRACK_BUNDLE_FILE ,"a");
1420             fprintf(file,"\n----- test 60.3 do matrs U V -----\n");
1421
1422             int i;
1423             for( i = 0; i < numImages; i++ )
1424             {
1425                 double norm = cvNorm(matrsUk[i]);
1426                 fprintf(file,"        test 6.01 post1 matrsUk=%lf\n",norm);
1427             }
1428
1429             for( i = 0; i < numPoints; i++ )
1430             {
1431                 double norm = cvNorm(matrsVi[i]);
1432                 fprintf(file,"        test 6.01 post1 matrsVi=%lf\n",norm);
1433             }
1434
1435             fclose(file);
1436         }
1437 #endif
1438
1439         /* ========== Solve normal equation for given alpha and Jacobian ============ */
1440
1441         do
1442         {
1443             /* ---- Add alpha to matrices --- */
1444             /* Add alpha to matrInvVi and make workMatrsInvVi */
1445
1446             int currV;
1447             for( currV = 0; currV < numPoints; currV++ )
1448             {
1449                 cvCopy(matrsVi[currV],workMatrVi);
1450
1451                 for( i = 0; i < 4; i++ )
1452                 {
1453                     cvmSet(workMatrVi,i,i,cvmGet(matrsVi[currV],i,i)*(1+alpha) );
1454                 }
1455
1456                 cvInvert(workMatrVi,workMatrsInvVi[currV],CV_LU/*,&currV*/);
1457             }
1458
1459             /* Add alpha to matrUk and make matrix workMatrsUk */
1460             for(int currImage = 0; currImage< numImages; currImage++ )
1461             {
1462
1463                 for( i = 0; i < 12; i++ )
1464                 {
1465                     cvmSet(workMatrsUk[currImage],i,i,cvmGet(matrsUk[currImage],i,i)*(1+alpha));
1466                 }
1467
1468
1469             }
1470
1471             /* Fill matrix to make system for computing delta P (matrTmpSys1 = inv(V)*tr(W) )*/
1472             for( currV = 0; currV < numPoints; currV++ )
1473             {
1474                 int currRowV;
1475                 for( currRowV = 0; currRowV < 4; currRowV++ )
1476                 {
1477                     for(int currImage = 0; currImage < numImages; currImage++ )
1478                     {
1479                         for( int currCol = 0; currCol < 12; currCol++ )/* For each column of transposed matrix W */
1480                         {
1481                             double sum = 0;
1482                             for( i = 0; i < 4; i++ )
1483                             {
1484                                 sum += cvmGet(workMatrsInvVi[currV],currRowV,i) *
1485                                        cvmGet(matrW,currImage*12+currCol,currV*4+i);
1486                             }
1487                             cvmSet(matrTmpSys1,currV*4+currRowV,currImage*12+currCol,sum);
1488                         }
1489                     }
1490                 }
1491             }
1492
1493
1494             /* Fill matrix to make system for computing delta P (matrTmpSys2 = W * matrTmpSys ) */
1495             cvmMul(matrW,matrTmpSys1,matrSysDeltaP);
1496
1497             /* need to compute U-matrTmpSys2. But we compute matTmpSys2-U */
1498             for(int currImage = 0; currImage < numImages; currImage++ )
1499             {
1500                 CvMat subMatr;
1501                 cvGetSubRect(matrSysDeltaP,&subMatr,cvRect(currImage*12,currImage*12,12,12));
1502                 cvSub(&subMatr,workMatrsUk[currImage],&subMatr);
1503             }
1504
1505             /* Compute right side of normal equation  */
1506             for( currV = 0; currV < numPoints; currV++ )
1507             {
1508                 CvMat subMatrErPnts;
1509                 CvMat subMatr;
1510                 cvGetSubRect(jacPointErr,&subMatrErPnts,cvRect(0,currV*4,1,4));
1511                 cvGetSubRect(vectTmpSys3,&subMatr,cvRect(0,currV*4,1,4));
1512                 cvmMul(workMatrsInvVi[currV],&subMatrErPnts,&subMatr);
1513             }
1514
1515             cvmMul(matrW,vectTmpSys3,vectSysDeltaP);
1516             cvSub(vectSysDeltaP,jacProjErr,vectSysDeltaP);
1517
1518             /* Now we can compute part of normal system - deltaP */
1519             cvSolve(matrSysDeltaP ,vectSysDeltaP, deltaP, CV_SVD);
1520
1521             /* Print deltaP to file */
1522
1523 #ifdef TRACK_BUNDLE
1524             {
1525                 FILE* file;
1526                 file = fopen( TRACK_BUNDLE_FILE_DELTAP ,"w");
1527
1528                 for(int currImage = 0; currImage < numImages; currImage++ )
1529                 {
1530                     fprintf(file,"\nImage=%d\n",currImage);
1531                     int i;
1532                     for( i = 0; i < 12; i++ )
1533                     {
1534                         double val;
1535                         val = cvmGet(deltaP,currImage*12+i,0);
1536                         fprintf(file,"%lf\n",val);
1537                     }
1538                     fprintf(file,"\n");
1539                 }
1540                 fclose(file);
1541             }
1542 #endif
1543             /* We know deltaP and now we can compute system for deltaM */
1544             for( i = 0; i < numPoints * 4; i++ )
1545             {
1546                 double sum = 0;
1547                 for( int j = 0; j < numImages * 12; j++ )
1548                 {
1549                     sum += cvmGet(matrW,j,i) * cvmGet(deltaP,j,0);
1550                 }
1551                 cvmSet(vectTmpSysM,i,0,cvmGet(jacPointErr,i,0)-sum);
1552             }
1553
1554             /* Compute deltaM */
1555             for( currV = 0; currV < numPoints; currV++ )
1556             {
1557                 CvMat subMatr;
1558                 CvMat subMatrM;
1559                 cvGetSubRect(vectTmpSysM,&subMatr,cvRect(0,currV*4,1,4));
1560                 cvGetSubRect(deltaM,&subMatrM,cvRect(0,currV*4,1,4));
1561                 cvmMul(workMatrsInvVi[currV],&subMatr,&subMatrM);
1562             }
1563
1564             /* We know delta and compute new value of vector X: nextVectX = vectX + deltas */
1565
1566             /* Compute new P */
1567             for(int currImage = 0; currImage < numImages; currImage++ )
1568             {
1569                 for( i = 0; i < 3; i++ )
1570                 {
1571                     for( int j = 0; j < 4; j++ )
1572                     {
1573                         cvmSet(newVectorX_projMatrs[currImage],i,j,
1574                                 cvmGet(vectorX_projMatrs[currImage],i,j) + cvmGet(deltaP,currImage*12+i*4+j,0));
1575                     }
1576                 }
1577             }
1578
1579             /* Compute new M */
1580             int currPoint;
1581             for( currPoint = 0; currPoint < numPoints; currPoint++ )
1582             {
1583                 for( i = 0; i < 4; i++ )
1584                 {
1585                     cvmSet(newVectorX_points4D,i,currPoint,
1586                         cvmGet(vectorX_points4D,i,currPoint) + cvmGet(deltaM,currPoint*4+i,0));
1587                 }
1588             }
1589
1590             /* ----- Compute errors for new vectorX ----- */
1591             /* Project points using new vectorX and status of each point */
1592             icvProjPointsStatusFunc(numImages, newVectorX_points4D, newVectorX_projMatrs, pointsPres, projVisPoints);
1593             /* Compute error with observed value and computed projection */
1594             double newError = 0;
1595             for(int currImage = 0; currImage < numImages; currImage++ )
1596             {
1597                 cvSub(observVisPoints[currImage],projVisPoints[currImage],errorProjPoints[currImage]);
1598                 double currNorm = cvNorm(errorProjPoints[currImage]);
1599
1600 //#ifdef TRACK_BUNDLE
1601 #if 1
1602                 {
1603                     FILE *file;
1604                     file = fopen( TRACK_BUNDLE_FILE ,"a");
1605                     fprintf(file,"\n----- test 13,01 currImage=%d currNorm=%lf -----\n",currImage,currNorm);
1606                     fclose(file);
1607                 }
1608 #endif
1609                 newError += currNorm * currNorm;
1610             }
1611             newError = sqrt(newError);
1612
1613             currIter++;
1614
1615
1616
1617
1618 //#ifdef TRACK_BUNDLE
1619 #if 1
1620             {
1621                 /* Create file to track */
1622                 FILE* file;
1623                 file = fopen( TRACK_BUNDLE_FILE ,"a");
1624                 fprintf(file,"\n========================================\n");
1625                 fprintf(file,"numPoints=%d\n",numPoints);
1626                 fprintf(file,"Iter=%d\n",currIter);
1627                 fprintf(file,"Error = %20.15lf\n",newError);
1628                 fprintf(file,"Change = %20.15lf\n",change);
1629
1630
1631                 /* Print all projection errors */
1632 #if 0
1633                 fprintf(file,"projection errors\n");
1634                 for(int currImage = 0; currImage < numImages; currImage++)
1635                 {
1636                     fprintf(file,"\nImage=%d\n",currImage);
1637                     int numPn = errorProjPoints[currImage]->cols;
1638                     for( int currPoint = 0; currPoint < numPn; currPoint++ )
1639                     {
1640                         double ex,ey;
1641                         ex = cvmGet(errorProjPoints[currImage],0,currPoint);
1642                         ey = cvmGet(errorProjPoints[currImage],1,currPoint);
1643                         fprintf(file,"%lf,%lf\n",ex,ey);
1644                     }
1645                 }
1646                 fprintf(file,"\n---- test 0 -----\n");
1647 #endif
1648
1649                 fclose(file);
1650             }
1651 #endif
1652
1653
1654
1655             /* Compare new error and last error */
1656             if( newError < prevError )
1657             {/* accept new value */
1658                 prevError = newError;
1659                 /* Compute relative change of required parameter vectorX. change = norm(curr-prev) / norm(curr) )  */
1660                 {
1661                     double normAll1 = 0;
1662                     double normAll2 = 0;
1663                     double currNorm1 = 0;
1664                     double currNorm2 = 0;
1665                     /* compute norm for projection matrices */
1666                     for(int currImage = 0; currImage < numImages; currImage++ )
1667                     {
1668                         currNorm1 = cvNorm(newVectorX_projMatrs[currImage],vectorX_projMatrs[currImage]);
1669                         currNorm2 = cvNorm(newVectorX_projMatrs[currImage]);
1670
1671                         normAll1 += currNorm1 * currNorm1;
1672                         normAll2 += currNorm2 * currNorm2;
1673                     }
1674
1675                     /* compute norm for points */
1676                     currNorm1 = cvNorm(newVectorX_points4D,vectorX_points4D);
1677                     currNorm2 = cvNorm(newVectorX_points4D);
1678
1679                     normAll1 += currNorm1 * currNorm1;
1680                     normAll2 += currNorm2 * currNorm2;
1681
1682                     /* compute change */
1683                     change = sqrt(normAll1) / sqrt(normAll2);
1684
1685
1686 //#ifdef TRACK_BUNDLE
1687 #if 1
1688                     {
1689                         /* Create file to track */
1690                         FILE* file;
1691                         file = fopen( TRACK_BUNDLE_FILE ,"a");
1692                         fprintf(file,"\nChange inside newVal change = %20.15lf\n",change);
1693                         fprintf(file,"   normAll1= %20.15lf\n",sqrt(normAll1));
1694                         fprintf(file,"   normAll2= %20.15lf\n",sqrt(normAll2));
1695
1696                         fclose(file);
1697                     }
1698 #endif
1699
1700                 }
1701
1702                 alpha /= 10;
1703                 for(int currImage = 0; currImage < numImages; currImage++ )
1704                 {
1705                     cvCopy(newVectorX_projMatrs[currImage],vectorX_projMatrs[currImage]);
1706                 }
1707                 cvCopy(newVectorX_points4D,vectorX_points4D);
1708
1709                 break;
1710             }
1711             else
1712             {
1713                 alpha *= 10;
1714             }
1715
1716         } while( change > epsilon && currIter < maxIter );/* solve normal equation using current alpha */
1717
1718 //#ifdef TRACK_BUNDLE
1719 #if 1
1720         {
1721             FILE* file;
1722             file = fopen( TRACK_BUNDLE_FILE ,"a");
1723             fprintf(file,"\nBest error = %40.35lf\n",prevError);
1724             fclose(file);
1725         }
1726
1727 #endif
1728
1729
1730     } while( change > epsilon && currIter < maxIter );
1731
1732     /*--------------------------------------------*/
1733     /* Optimization complete copy computed params */
1734     /* Copy projection matrices */
1735     for(int currImage = 0; currImage < numImages; currImage++ )
1736     {
1737         cvCopy(newVectorX_projMatrs[currImage],resultProjMatrs[currImage]);
1738     }
1739     /* Copy 4D points */
1740     cvCopy(newVectorX_points4D,resultPoints4D);
1741
1742 //    free(memory);
1743
1744     __END__;
1745
1746     /* Free allocated memory */
1747
1748     /* Free simple matrices */
1749     cvFree(&vectorX_points4D);
1750     cvFree(&newVectorX_points4D);
1751     cvFree(&changeVectorX_points4D);
1752     cvFree(&changeVectorX_projMatrs);
1753     cvFree(&matrW);
1754     cvFree(&workMatrVi);
1755     cvFree(&jacProjErr);
1756     cvFree(&jacPointErr);
1757     cvFree(&matrTmpSys1);
1758     cvFree(&matrSysDeltaP);
1759     cvFree(&vectTmpSys3);
1760     cvFree(&vectSysDeltaP);
1761     cvFree(&deltaP);
1762     cvFree(&deltaM);
1763     cvFree(&vectTmpSysM);
1764
1765     /* Free arrays of matrices */
1766     icvFreeMatrixArray(&vectorX_projMatrs,numImages);
1767     icvFreeMatrixArray(&newVectorX_projMatrs,numImages);
1768     icvFreeMatrixArray(&observVisPoints,numImages);
1769     icvFreeMatrixArray(&projVisPoints,numImages);
1770     icvFreeMatrixArray(&errorProjPoints,numImages);
1771     icvFreeMatrixArray(&DerivProj,numImages);
1772     icvFreeMatrixArray(&DerivPoint,numImages);
1773     icvFreeMatrixArray(&matrsUk,numImages);
1774     icvFreeMatrixArray(&workMatrsUk,numImages);
1775     icvFreeMatrixArray(&matrsVi,numPoints);
1776     icvFreeMatrixArray(&workMatrsInvVi,numPoints);
1777
1778     return;
1779 }