git.maemo.org Git - opencv/blob - cvaux/src/cvlevmarprojbandle.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 "_cvaux.h"
  44 //#include "cvtypes.h"
  45 #include <float.h>
  46 #include <limits.h>
  47 //#include "cv.h"
  48
  49 #include <stdio.h>
  50
  51 void icvReconstructPoints4DStatus(CvMat** projPoints, CvMat **projMatrs, CvMat** presPoints, CvMat *points4D,int numImages,CvMat **projError=0);
  52
  53 /* Valery Mosyagin */
  54
  55 /* If you want to save internal debug info to files uncomment next lines and set paths to files if need */
  56 /* Note these file may be very large */
  57 /*
  58 #define TRACK_BUNDLE
  59 #define TRACK_BUNDLE_FILE            "d:\\test\\bundle.txt"
  60 #define TRACK_BUNDLE_FILE_JAC        "d:\\test\\bundle.txt"
  61 #define TRACK_BUNDLE_FILE_JACERRPROJ "d:\\test\\JacErrProj.txt"
  62 #define TRACK_BUNDLE_FILE_JACERRPNT  "d:\\test\\JacErrPoint.txt"
  63 #define TRACK_BUNDLE_FILE_MATRW      "d:\\test\\matrWt.txt"
  64 #define TRACK_BUNDLE_FILE_DELTAP     "d:\\test\\deltaP.txt"
  65 */
  66 #define TRACK_BUNDLE_FILE            "d:\\test\\bundle.txt"
  67
  68
  69 /* ============== Bundle adjustment optimization ================= */
  70 void icvComputeDerivateProj(CvMat *points4D,CvMat *projMatr, CvMat *status, CvMat *derivProj)
  71 {
  72     /* Compute derivate for given projection matrix points and status of points */
  73
  74     CV_FUNCNAME( "icvComputeDerivateProj" );
  75     __BEGIN__;
  76
  77
  78     /* ----- Test input params for errors ----- */
  79     if( points4D == 0 || projMatr == 0 || status == 0 || derivProj == 0)
  80     {
  81         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
  82     }
  83
  84     if( !CV_IS_MAT(points4D) )
  85     {
  86         CV_ERROR( CV_StsUnsupportedFormat, "points4D must be a matrix 4xN" );
  87     }
  88
  89     /* Compute number of points */
  90     int numPoints;
  91     numPoints = points4D->cols;
  92
  93     if( numPoints < 1 )
  94     {
  95         CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" );
  96     }
  97
  98     if( points4D->rows != 4 )
  99     {
 100         CV_ERROR( CV_StsOutOfRange, "Number of coordinates of points4D must be 4" );
 101     }
 102
 103     if( !CV_IS_MAT(projMatr) )
 104     {
 105         CV_ERROR( CV_StsUnsupportedFormat, "projMatr must be a matrix 3x4" );
 106     }
 107
 108     if( projMatr->rows != 3 || projMatr->cols != 4 )
 109     {
 110         CV_ERROR( CV_StsOutOfRange, "Size of projection matrix (projMatr) must be 3x4" );
 111     }
 112
 113     if( !CV_IS_MAT(status) )
 114     {
 115         CV_ERROR( CV_StsUnsupportedFormat, "Status must be a matrix 1xN" );
 116     }
 117
 118     if( status->rows != 1 || status->cols != numPoints )
 119     {
 120         CV_ERROR( CV_StsOutOfRange, "Size of status of points must be 1xN" );
 121     }
 122
 123     if( !CV_IS_MAT(derivProj) )
 124     {
 125         CV_ERROR( CV_StsUnsupportedFormat, "derivProj must be a matrix VisN x 12" );
 126     }
 127
 128     if( derivProj->cols != 12 )
 129     {
 130         CV_ERROR( CV_StsOutOfRange, "derivProj must be a matrix VisN x 12" );
 131     }
 132     /* ----- End test ----- */
 133
 134     int i;
 135
 136     /* Allocate memory for derivates */
 137
 138     double p[12];
 139     /* Copy projection matrix */
 140     for( i = 0; i < 12; i++ )
 141     {
 142         p[i] = cvmGet(projMatr,i/4,i%4);
 143     }
 144
 145     /* Fill deriv matrix */
 146     int currVisPoint;
 147     int currPoint;
 148
 149     currVisPoint = 0;
 150     for( currPoint = 0; currPoint < numPoints; currPoint++ )
 151     {
 152         if( cvmGet(status,0,currPoint) > 0 )
 153         {
 154             double X[4];
 155             X[0] = cvmGet(points4D,0,currVisPoint);
 156             X[1] = cvmGet(points4D,1,currVisPoint);
 157             X[2] = cvmGet(points4D,2,currVisPoint);
 158             X[3] = cvmGet(points4D,3,currVisPoint);
 159
 160             /* Compute derivate for this point */
 161
 162             double piX[3];
 163             piX[0] = X[0]*p[0] + X[1]*p[1] + X[2]*p[2]  + X[3]*p[3];
 164             piX[1] = X[0]*p[4] + X[1]*p[5] + X[2]*p[6]  + X[3]*p[7];
 165             piX[2] = X[0]*p[8] + X[1]*p[9] + X[2]*p[10] + X[3]*p[11];
 166
 167             int i;
 168             /* fill derivate by point */
 169
 170             double tmp3 = 1/(piX[2]*piX[2]);
 171
 172             double tmp1 = -piX[0]*tmp3;
 173             double tmp2 = -piX[1]*tmp3;
 174
 175             /* fill derivate by projection matrix */
 176             for( i = 0; i < 4; i++ )
 177             {
 178                 /* derivate for x */
 179                 cvmSet(derivProj,currVisPoint*2,i,X[i]/piX[2]);//x' p1i
 180                 cvmSet(derivProj,currVisPoint*2,4+i,0);//x' p1i
 181                 cvmSet(derivProj,currVisPoint*2,8+i,X[i]*tmp1);//x' p3i
 182
 183                 /* derivate for y */
 184                 cvmSet(derivProj,currVisPoint*2+1,i,0);//y' p2i
 185                 cvmSet(derivProj,currVisPoint*2+1,4+i,X[i]/piX[2]);//y' p2i
 186                 cvmSet(derivProj,currVisPoint*2+1,8+i,X[i]*tmp2);//y' p3i
 187             }
 188
 189             currVisPoint++;
 190         }
 191     }
 192
 193     if( derivProj->rows != currVisPoint * 2 )
 194     {
 195         CV_ERROR( CV_StsOutOfRange, "derivProj must be a matrix 2VisN x 12" );
 196     }
 197
 198
 199     __END__;
 200     return;
 201 }
 202 /*======================================================================================*/
 203
 204 void icvComputeDerivateProjAll(CvMat *points4D, CvMat **projMatrs, CvMat **pointPres, int numImages,CvMat **projDerives)
 205 {
 206     CV_FUNCNAME( "icvComputeDerivateProjAll" );
 207     __BEGIN__;
 208
 209     /* ----- Test input params for errors ----- */
 210     if( numImages < 1 )
 211     {
 212         CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
 213     }
 214     if( projMatrs == 0 || pointPres == 0 || projDerives == 0 )
 215     {
 216         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 217     }
 218     /* ----- End test ----- */
 219
 220     int currImage;
 221     for( currImage = 0; currImage < numImages; currImage++ )
 222     {
 223         icvComputeDerivateProj(points4D,projMatrs[currImage], pointPres[currImage], projDerives[currImage]);
 224     }
 225
 226     __END__;
 227     return;
 228 }
 229 /*======================================================================================*/
 230
 231 void icvComputeDerivatePoints(CvMat *points4D,CvMat *projMatr, CvMat *presPoints, CvMat *derivPoint)
 232 {
 233
 234     CV_FUNCNAME( "icvComputeDerivatePoints" );
 235     __BEGIN__;
 236
 237     /* ----- Test input params for errors ----- */
 238     if( points4D == 0 || projMatr == 0 || presPoints == 0 || derivPoint == 0)
 239     {
 240         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 241     }
 242
 243     if( !CV_IS_MAT(points4D) )
 244     {
 245         CV_ERROR( CV_StsUnsupportedFormat, "points4D must be a matrix N x 4" );
 246     }
 247
 248     int numPoints;
 249     numPoints = presPoints->cols;
 250
 251     if( numPoints < 1 )
 252     {
 253         CV_ERROR( CV_StsOutOfRange, "Number of points must be more than zero" );
 254     }
 255
 256     if( points4D->rows != 4 )
 257     {
 258         CV_ERROR( CV_StsOutOfRange, "points4D must be a matrix N x 4" );
 259     }
 260
 261     if( !CV_IS_MAT(projMatr) )
 262     {
 263         CV_ERROR( CV_StsUnsupportedFormat, "projMatr must be a matrix 3x4" );
 264     }
 265
 266     if( projMatr->rows != 3 || projMatr->cols != 4 )
 267     {
 268         CV_ERROR( CV_StsOutOfRange, "Size of projection matrix (projMatr) must be 3x4" );
 269     }
 270
 271     if( !CV_IS_MAT(presPoints) )
 272     {
 273         CV_ERROR( CV_StsUnsupportedFormat, "Status must be a matrix 1xN" );
 274     }
 275
 276     if( presPoints->rows != 1 || presPoints->cols != numPoints )
 277     {
 278         CV_ERROR( CV_StsOutOfRange, "Size of presPoints status must be 1xN" );
 279     }
 280
 281     if( !CV_IS_MAT(derivPoint) )
 282     {
 283         CV_ERROR( CV_StsUnsupportedFormat, "derivPoint must be a matrix 2 x 4VisNum" );
 284     }
 285     /* ----- End test ----- */
 286
 287     /* Compute derivates by points */
 288
 289     double p[12];
 290     int i;
 291     for( i = 0; i < 12; i++ )
 292     {
 293         p[i] = cvmGet(projMatr,i/4,i%4);
 294     }
 295
 296     int currVisPoint;
 297     int currProjPoint;
 298
 299     currVisPoint = 0;
 300     for( currProjPoint = 0; currProjPoint < numPoints; currProjPoint++ )
 301     {
 302         if( cvmGet(presPoints,0,currProjPoint) > 0 )
 303         {
 304             double X[4];
 305             X[0] = cvmGet(points4D,0,currProjPoint);
 306             X[1] = cvmGet(points4D,1,currProjPoint);
 307             X[2] = cvmGet(points4D,2,currProjPoint);
 308             X[3] = cvmGet(points4D,3,currProjPoint);
 309
 310             double piX[3];
 311             piX[0] = X[0]*p[0] + X[1]*p[1] + X[2]*p[2]  + X[3]*p[3];
 312             piX[1] = X[0]*p[4] + X[1]*p[5] + X[2]*p[6]  + X[3]*p[7];
 313             piX[2] = X[0]*p[8] + X[1]*p[9] + X[2]*p[10] + X[3]*p[11];
 314
 315             int i,j;
 316
 317             double tmp3 = 1/(piX[2]*piX[2]);
 318
 319             for( j = 0; j < 2; j++ )//for x and y
 320             {
 321                 for( i = 0; i < 4; i++ )// for X,Y,Z,W
 322                 {
 323                     cvmSet( derivPoint,
 324                             j, currVisPoint*4+i,
 325                             (p[j*4+i]*piX[2]-p[8+i]*piX[j]) * tmp3  );
 326                 }
 327             }
 328             currVisPoint++;
 329         }
 330     }
 331
 332     if( derivPoint->rows != 2 || derivPoint->cols != currVisPoint*4 )
 333     {
 334         CV_ERROR( CV_StsUnsupportedFormat, "derivPoint must be a matrix 2 x 4VisNum" );
 335     }
 336
 337     __END__;
 338     return;
 339 }
 340 /*======================================================================================*/
 341 void icvComputeDerivatePointsAll(CvMat *points4D, CvMat **projMatrs, CvMat **pointPres, int numImages,CvMat **pointDerives)
 342 {
 343     CV_FUNCNAME( "icvComputeDerivatePointsAll" );
 344     __BEGIN__;
 345
 346     /* ----- Test input params for errors ----- */
 347     if( numImages < 1 )
 348     {
 349         CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
 350     }
 351     if( projMatrs == 0 || pointPres == 0 || pointDerives == 0 )
 352     {
 353         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 354     }
 355     /* ----- End test ----- */
 356
 357     int currImage;
 358     for( currImage = 0; currImage < numImages; currImage++ )
 359     {
 360         icvComputeDerivatePoints(points4D, projMatrs[currImage], pointPres[currImage], pointDerives[currImage]);
 361     }
 362
 363     __END__;
 364     return;
 365 }
 366 /*======================================================================================*/
 367 void icvComputeMatrixVAll(int numImages,CvMat **pointDeriv,CvMat **presPoints, CvMat **matrV)
 368 {
 369     int *shifts = 0;
 370
 371     CV_FUNCNAME( "icvComputeMatrixVAll" );
 372     __BEGIN__;
 373
 374     /* ----- Test input params for errors ----- */
 375     if( numImages < 1 )
 376     {
 377         CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
 378     }
 379     if( pointDeriv == 0 || presPoints == 0 || matrV == 0 )
 380     {
 381         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 382     }
 383     /*  !!! not tested all parameters */
 384     /* ----- End test ----- */
 385
 386     /* Compute all matrices U */
 387     int currImage;
 388     int currPoint;
 389     int numPoints;
 390     numPoints = presPoints[0]->cols;
 391     CV_CALL(shifts = (int*)cvAlloc(sizeof(int)*numImages));
 392     memset(shifts,0,sizeof(int)*numImages);
 393
 394     for( currPoint = 0; currPoint < numPoints; currPoint++ )//For each point (matrix V)
 395     {
 396         int i,j;
 397
 398         for( i = 0; i < 4; i++ )
 399         {
 400             for( j = 0; j < 4; j++ )
 401             {
 402                 double sum = 0;
 403                 for( currImage = 0; currImage < numImages; currImage++ )
 404                 {
 405                     if( cvmGet(presPoints[currImage],0,currPoint) > 0 )
 406                     {
 407                         sum += cvmGet(pointDeriv[currImage],0,shifts[currImage]*4+i) *
 408                                cvmGet(pointDeriv[currImage],0,shifts[currImage]*4+j);
 409
 410                         sum += cvmGet(pointDeriv[currImage],1,shifts[currImage]*4+i) *
 411                                cvmGet(pointDeriv[currImage],1,shifts[currImage]*4+j);
 412                     }
 413                 }
 414
 415                 cvmSet(matrV[currPoint],i,j,sum);
 416             }
 417         }
 418
 419
 420         /* shift position of visible points */
 421         for( currImage = 0; currImage < numImages; currImage++ )
 422         {
 423             if( cvmGet(presPoints[currImage],0,currPoint) > 0 )
 424             {
 425                 shifts[currImage]++;
 426             }
 427         }
 428     }
 429
 430     __END__;
 431     cvFree( &shifts);
 432
 433     return;
 434 }
 435 /*======================================================================================*/
 436 void icvComputeMatrixUAll(int numImages,CvMat **projDeriv,CvMat** matrU)
 437 {
 438     CV_FUNCNAME( "icvComputeMatrixVAll" );
 439     __BEGIN__;
 440     /* ----- Test input params for errors ----- */
 441     if( numImages < 1 )
 442     {
 443         CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
 444     }
 445     if( projDeriv == 0 || matrU == 0 )
 446     {
 447         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 448     }
 449     /* !!! Not tested all input parameters */
 450     /* ----- End test ----- */
 451
 452     /* Compute matrices V */
 453     int currImage;
 454     for( currImage = 0; currImage < numImages; currImage++ )
 455     {
 456         cvMulTransposed(projDeriv[currImage],matrU[currImage],1);
 457     }
 458
 459     __END__;
 460     return;
 461 }
 462 /*======================================================================================*/
 463 void icvComputeMatrixW(int numImages, CvMat **projDeriv, CvMat **pointDeriv, CvMat **presPoints, CvMat *matrW)
 464 {
 465     CV_FUNCNAME( "icvComputeMatrixW" );
 466     __BEGIN__;
 467
 468     /* ----- Test input params for errors ----- */
 469     if( numImages < 1 )
 470     {
 471         CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
 472     }
 473     if( projDeriv == 0 || pointDeriv == 0 || presPoints == 0 || matrW == 0 )
 474     {
 475         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 476     }
 477     int numPoints;
 478     numPoints = presPoints[0]->cols;
 479     if( numPoints < 1 )
 480     {
 481         CV_ERROR( CV_StsOutOfRange, "Number of points must more than zero" );
 482     }
 483     if( !CV_IS_MAT(matrW) )
 484     {
 485         CV_ERROR( CV_StsUnsupportedFormat, "matrW must be a matrix 12NumIm x 4NumPnt" );
 486     }
 487     if( matrW->rows != numImages*12 || matrW->cols != numPoints*4 )
 488     {
 489         CV_ERROR( CV_StsOutOfRange, "matrW must be a matrix 12NumIm x 4NumPnt" );
 490     }
 491     /* !!! Not tested all input parameters */
 492     /* ----- End test ----- */
 493
 494     /* Compute number of points */
 495     /* Compute matrix W using derivate proj and points */
 496
 497     int currImage;
 498
 499     for( currImage = 0; currImage < numImages; currImage++ )
 500     {
 501         for( int currLine = 0; currLine < 12; currLine++ )
 502         {
 503             int currVis = 0;
 504             for( int currPoint = 0; currPoint < numPoints; currPoint++ )
 505             {
 506                 if( cvmGet(presPoints[currImage],0,currPoint) > 0 )
 507                 {
 508
 509                     for( int currCol = 0; currCol < 4; currCol++ )
 510                     {
 511                         double sum;
 512                         sum = cvmGet(projDeriv[currImage],currVis*2+0,currLine) *
 513                               cvmGet(pointDeriv[currImage],0,currVis*4+currCol);
 514
 515                         sum += cvmGet(projDeriv[currImage],currVis*2+1,currLine) *
 516                               cvmGet(pointDeriv[currImage],1,currVis*4+currCol);
 517
 518                         cvmSet(matrW,currImage*12+currLine,currPoint*4+currCol,sum);
 519                     }
 520                     currVis++;
 521                 }
 522                 else
 523                 {/* set all sub elements to zero */
 524                     for( int currCol = 0; currCol < 4; currCol++ )
 525                     {
 526                         cvmSet(matrW,currImage*12+currLine,currPoint*4+currCol,0);
 527                     }
 528                 }
 529             }
 530         }
 531     }
 532
 533 #ifdef TRACK_BUNDLE
 534     {
 535         FILE *file;
 536         file = fopen( TRACK_BUNDLE_FILE_MATRW ,"w");
 537         int currPoint,currImage;
 538         for( currPoint = 0; currPoint < numPoints; currPoint++ )
 539         {
 540             fprintf(file,"\nPoint=%d\n",currPoint);
 541             int currRow;
 542             for( currRow = 0; currRow < 4; currRow++  )
 543             {
 544                 for( currImage = 0; currImage< numImages; currImage++ )
 545                 {
 546                     int i;
 547                     for( i = 0; i < 12; i++ )
 548                     {
 549                         double val = cvmGet(matrW, currImage * 12 + i, currPoint * 4 + currRow);
 550                         fprintf(file,"%lf ",val);
 551                     }
 552                 }
 553                 fprintf(file,"\n");
 554             }
 555         }
 556         fclose(file);
 557     }
 558 #endif
 559
 560     __END__;
 561     return;
 562 }
 563 /*======================================================================================*/
 564 /* Compute jacobian mult projection matrices error */
 565 void icvComputeJacErrorProj(int numImages,CvMat **projDeriv,CvMat **projErrors,CvMat *jacProjErr )
 566 {
 567     CV_FUNCNAME( "icvComputeJacErrorProj" );
 568     __BEGIN__;
 569
 570     /* ----- Test input params for errors ----- */
 571     if( numImages < 1 )
 572     {
 573         CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
 574     }
 575     if( projDeriv == 0 || projErrors == 0 || jacProjErr == 0 )
 576     {
 577         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 578     }
 579     if( !CV_IS_MAT(jacProjErr) )
 580     {
 581         CV_ERROR( CV_StsUnsupportedFormat, "jacProjErr must be a matrix 12NumIm x 1" );
 582     }
 583     if( jacProjErr->rows != numImages*12 || jacProjErr->cols != 1 )
 584     {
 585         CV_ERROR( CV_StsOutOfRange, "jacProjErr must be a matrix 12NumIm x 1" );
 586     }
 587     /* !!! Not tested all input parameters */
 588     /* ----- End test ----- */
 589
 590     int currImage;
 591     for( currImage = 0; currImage < numImages; currImage++ )
 592     {
 593         for( int currCol = 0; currCol < 12; currCol++ )
 594         {
 595             int num = projDeriv[currImage]->rows;
 596             double sum = 0;
 597             for( int i = 0; i < num; i++ )
 598             {
 599                 sum += cvmGet(projDeriv[currImage],i,currCol) *
 600                        cvmGet(projErrors[currImage],i%2,i/2);
 601             }
 602             cvmSet(jacProjErr,currImage*12+currCol,0,sum);
 603         }
 604     }
 605
 606 #ifdef TRACK_BUNDLE
 607     {
 608         FILE *file;
 609         file = fopen( TRACK_BUNDLE_FILE_JACERRPROJ ,"w");
 610         int currImage;
 611         for( currImage = 0; currImage < numImages; currImage++ )
 612         {
 613             fprintf(file,"\nImage=%d\n",currImage);
 614             int currRow;
 615             for( currRow = 0; currRow < 12; currRow++  )
 616             {
 617                 double val = cvmGet(jacProjErr, currImage * 12 + currRow, 0);
 618                 fprintf(file,"%lf\n",val);
 619             }
 620             fprintf(file,"\n");
 621         }
 622         fclose(file);
 623     }
 624 #endif
 625
 626
 627     __END__;
 628     return;
 629 }
 630 /*======================================================================================*/
 631 /* Compute jacobian mult points error */
 632 void icvComputeJacErrorPoint(int numImages,CvMat **pointDeriv,CvMat **projErrors, CvMat **presPoints,CvMat *jacPointErr )
 633 {
 634     int *shifts = 0;
 635
 636     CV_FUNCNAME( "icvComputeJacErrorPoint" );
 637     __BEGIN__;
 638
 639     /* ----- Test input params for errors ----- */
 640     if( numImages < 1 )
 641     {
 642         CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" );
 643     }
 644
 645     if( pointDeriv == 0 || projErrors == 0 || presPoints == 0 || jacPointErr == 0 )
 646     {
 647         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
 648     }
 649
 650     int numPoints;
 651     numPoints = presPoints[0]->cols;
 652     if( numPoints < 1 )
 653     {
 654         CV_ERROR( CV_StsOutOfRange, "Number of points must more than zero" );
 655     }
 656
 657     if( !CV_IS_MAT(jacPointErr) )
 658     {
 659         CV_ERROR( CV_StsUnsupportedFormat, "jacPointErr must be a matrix 4NumPnt x 1" );
 660     }
 661
 662     if( jacPointErr->rows != numPoints*4 || jacPointErr->cols != 1 )
 663     {
 664         CV_ERROR( CV_StsOutOfRange, "jacPointErr must be a matrix 4NumPnt x 1" );
 665     }
 666     /* !!! Not tested all input parameters */
 667     /* ----- End test ----- */
 668
 669     int currImage;
 670     int currPoint;
 671     CV_CALL(shifts = (int*)cvAlloc(sizeof(int)*numImages));
 672     memset(shifts,0,sizeof(int)*numImages);
 673     for( currPoint = 0; currPoint < numPoints; currPoint++ )
 674     {
 675         for( int currCoord = 0; currCoord < 4; currCoord++ )
 676         {
 677             double sum = 0;
 678             {
 679                 int currVis = 0;
 680                 for( currImage = 0; currImage < numImages; currImage++ )
 681                 {
 682                     if( cvmGet(presPoints[currImage],0,currPoint) > 0 )
 683                     {
 684                         sum += cvmGet(pointDeriv[currImage],0,shifts[currImage]*4+currCoord) *
 685                                cvmGet(projErrors[currImage],0,shifts[currImage]);//currVis);
 686
 687                         sum += cvmGet(pointDeriv[currImage],1,shifts[currImage]*4+currCoord) *
 688                                cvmGet(projErrors[currImage],1,shifts[currImage]);//currVis);
 689
 690                         currVis++;
 691                     }
 692                 }
 693             }
 694
 695             cvmSet(jacPointErr,currPoint*4+currCoord,0,sum);
 696         }
 697
 698         /* Increase shifts */
 699         for( currImage = 0; currImage < numImages; currImage++ )
 700         {
 701             if( cvmGet(presPoints[currImage],0,currPoint) > 0 )
 702             {
 703                 shifts[currImage]++;
 704             }
 705         }
 706     }
 707
 708
 709 #ifdef TRACK_BUNDLE
 710     {
 711         FILE *file;
 712         file = fopen(TRACK_BUNDLE_FILE_JACERRPNT,"w");
 713         int currPoint;
 714         for( currPoint = 0; currPoint < numPoints; currPoint++ )
 715         {
 716             fprintf(file,"\nPoint=%d\n",currPoint);
 717             int currRow;
 718             for( currRow = 0; currRow < 4; currRow++  )
 719             {
 720                 double val = cvmGet(jacPointErr, currPoint * 4 + currRow, 0);
 721                 fprintf(file,"%lf\n",val);
 722             }
 723             fprintf(file,"\n");
 724         }
 725         fclose(file);
 726     }
 727 #endif
 728
 729
 730
 731     __END__;
 732     cvFree( &shifts);
 733
 734 }
 735 /*======================================================================================*/
 736
 737 /* Reconstruct 4D points using status */
 738 void icvReconstructPoints4DStatus(CvMat** projPoints, CvMat **projMatrs, CvMat** presPoints,
 739                                   CvMat *points4D,int numImages,CvMat **projError)
 740 {
 741
 742     double* matrA_dat = 0;
 743     double* matrW_dat = 0;
 744
 745     CV_FUNCNAME( "icvReconstructPoints4DStatus" );
 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     /* !!! Not tested all input parameters */
 772     /* ----- End test ----- */
 773
 774     int currImage;
 775     int currPoint;
 776
 777     /* Allocate maximum data */
 778
 779
 780     CvMat matrV;
 781     double matrV_dat[4*4];
 782     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     {/* Compute projection error */
 851         for( currImage = 0; currImage < numImages; currImage++ )
 852         {
 853             CvMat point4D;
 854             CvMat point3D;
 855             double point3D_dat[3];
 856             point3D = cvMat(3,1,CV_64F,point3D_dat);
 857
 858             int currPoint;
 859             int numVis = 0;
 860             double totalError = 0;
 861             for( currPoint = 0; currPoint < numPoints; currPoint++ )
 862             {
 863                 if( cvmGet(presPoints[currImage],0,currPoint) > 0)
 864                 {
 865                     double  dx,dy;
 866                     cvGetCol(points4D,&point4D,currPoint);
 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,currPoint) - x;
 873                     dy = cvmGet(projPoints[currImage],1,currPoint) - y;
 874                     if( projError )
 875                     {
 876                         cvmSet(projError[currImage],0,currPoint,dx);
 877                         cvmSet(projError[currImage],1,currPoint,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 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 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 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 /* !!! may be useful to return norm of error */
1052 /* !!! may be does not work correct with not all visible 4D points */
1053 void cvOptimizeLevenbergMarquardtBundle( CvMat** projMatrs, CvMat** observProjPoints,
1054                                        CvMat** pointsPres, int numImages,
1055                                        CvMat** resultProjMatrs, CvMat* resultPoints4D,int maxIter,double epsilon )
1056 {
1057
1058     CvMat  *vectorX_points4D = 0;
1059     CvMat **vectorX_projMatrs = 0;
1060
1061     CvMat  *newVectorX_points4D = 0;
1062     CvMat **newVectorX_projMatrs = 0;
1063
1064     CvMat  *changeVectorX_points4D = 0;
1065     CvMat  *changeVectorX_projMatrs = 0;
1066
1067     CvMat **observVisPoints = 0;
1068     CvMat **projVisPoints = 0;
1069     CvMat **errorProjPoints = 0;
1070     CvMat **DerivProj = 0;
1071     CvMat **DerivPoint = 0;
1072     CvMat *matrW = 0;
1073     CvMat **matrsUk = 0;
1074     CvMat **workMatrsUk = 0;
1075     CvMat **matrsVi = 0;
1076     CvMat *workMatrVi = 0;
1077     CvMat **workMatrsInvVi = 0;
1078     CvMat *jacProjErr = 0;
1079     CvMat *jacPointErr = 0;
1080
1081     CvMat *matrTmpSys1 = 0;
1082     CvMat *matrSysDeltaP = 0;
1083     CvMat *vectTmpSys3 = 0;
1084     CvMat *vectSysDeltaP = 0;
1085     CvMat *deltaP = 0;
1086     CvMat *deltaM = 0;
1087     CvMat *vectTmpSysM = 0;
1088
1089     int numPoints = 0;
1090
1091
1092     CV_FUNCNAME( "cvOptimizeLevenbergMarquardtBundle" );
1093     __BEGIN__;
1094
1095     /* ----- Test input params for errors ----- */
1096     if( numImages < 1 )
1097     {
1098         CV_ERROR( CV_StsOutOfRange, "Number of images must be more than zero" );
1099     }
1100
1101     if( maxIter < 1 || maxIter > 2000 )
1102     {
1103         CV_ERROR( CV_StsOutOfRange, "Maximum number of iteration must be in [1..1000]" );
1104     }
1105
1106     if( epsilon < 0  )
1107     {
1108         CV_ERROR( CV_StsOutOfRange, "Epsilon parameter must be >= 0" );
1109     }
1110
1111     if( !CV_IS_MAT(resultPoints4D) )
1112     {
1113         CV_ERROR( CV_StsUnsupportedFormat, "resultPoints4D must be a matrix 4 x NumPnt" );
1114     }
1115
1116     numPoints = resultPoints4D->cols;
1117     if( numPoints < 1 )
1118     {
1119         CV_ERROR( CV_StsOutOfRange, "Number of points must be more than zero" );//!!!
1120     }
1121
1122     if( resultPoints4D->rows != 4 )
1123     {
1124         CV_ERROR( CV_StsOutOfRange, "resultPoints4D must have 4 cordinates" );
1125     }
1126
1127     /* ----- End test ----- */
1128
1129     /* Optimization using bundle adjustment */
1130     /* work with non visible points */
1131
1132     CV_CALL( vectorX_points4D  = cvCreateMat(4,numPoints,CV_64F));
1133     CV_CALL( vectorX_projMatrs = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages));
1134
1135     CV_CALL( newVectorX_points4D  = cvCreateMat(4,numPoints,CV_64F));
1136     CV_CALL( newVectorX_projMatrs = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages));
1137
1138     CV_CALL( changeVectorX_points4D  = cvCreateMat(4,numPoints,CV_64F));
1139     CV_CALL( changeVectorX_projMatrs = cvCreateMat(3,4,CV_64F));
1140
1141     int currImage;
1142
1143     /* ----- Test input params ----- */
1144     for( currImage = 0; currImage < numImages; currImage++ )
1145     {
1146         /* Test size of input initial and result projection matrices */
1147         if( !CV_IS_MAT(projMatrs[currImage]) )
1148         {
1149             CV_ERROR( CV_StsUnsupportedFormat, "each of initial projMatrs must be a matrix 3 x 4" );
1150         }
1151         if( projMatrs[currImage]->rows != 3 || projMatrs[currImage]->cols != 4 )
1152         {
1153             CV_ERROR( CV_StsOutOfRange, "each of initial projMatrs must be a matrix 3 x 4" );
1154         }
1155
1156
1157         if( !CV_IS_MAT(observProjPoints[currImage]) )
1158         {
1159             CV_ERROR( CV_StsUnsupportedFormat, "each of observProjPoints must be a matrix 2 x NumPnts" );
1160         }
1161         if( observProjPoints[currImage]->rows != 2 || observProjPoints[currImage]->cols != numPoints )
1162         {
1163             CV_ERROR( CV_StsOutOfRange, "each of observProjPoints must be a matrix 2 x NumPnts" );
1164         }
1165
1166         if( !CV_IS_MAT(pointsPres[currImage]) )
1167         {
1168             CV_ERROR( CV_StsUnsupportedFormat, "each of pointsPres must be a matrix 1 x NumPnt" );
1169         }
1170         if( pointsPres[currImage]->rows != 1 || pointsPres[currImage]->cols != numPoints )
1171         {
1172             CV_ERROR( CV_StsOutOfRange, "each of pointsPres must be a matrix 1 x NumPnt" );
1173         }
1174
1175         if( !CV_IS_MAT(resultProjMatrs[currImage]) )
1176         {
1177             CV_ERROR( CV_StsUnsupportedFormat, "each of resultProjMatrs must be a matrix 3 x 4" );
1178         }
1179         if( resultProjMatrs[currImage]->rows != 3 || resultProjMatrs[currImage]->cols != 4 )
1180         {
1181             CV_ERROR( CV_StsOutOfRange, "each of resultProjMatrs must be a matrix 3 x 4" );
1182         }
1183
1184     }
1185     /* ----- End test ----- */
1186
1187     /* Copy projection matrices to vectorX0 */
1188     for( currImage = 0; currImage < numImages; currImage++ )
1189     {
1190         CV_CALL( vectorX_projMatrs[currImage] = cvCreateMat(3,4,CV_64F));
1191         CV_CALL( newVectorX_projMatrs[currImage] = cvCreateMat(3,4,CV_64F));
1192         cvCopy(projMatrs[currImage],vectorX_projMatrs[currImage]);
1193     }
1194
1195     /* Reconstruct points4D using projection matrices and status information */
1196     icvReconstructPoints4DStatus(observProjPoints, projMatrs, pointsPres, vectorX_points4D, numImages);
1197
1198     /* ----- Allocate memory for work matrices ----- */
1199     /* Compute number of good points on each images */
1200
1201     CV_CALL( observVisPoints = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) );
1202     CV_CALL( projVisPoints   = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) );
1203     CV_CALL( errorProjPoints = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) );
1204     CV_CALL( DerivProj       = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) );
1205     CV_CALL( DerivPoint      = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) );
1206     CV_CALL( matrW           = cvCreateMat(12*numImages,4*numPoints,CV_64F) );
1207     CV_CALL( matrsUk         = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) );
1208     CV_CALL( workMatrsUk     = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) );
1209     CV_CALL( matrsVi         = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numPoints) );
1210     CV_CALL( workMatrVi      = cvCreateMat(4,4,CV_64F) );
1211     CV_CALL( workMatrsInvVi  = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numPoints) );
1212
1213     CV_CALL( jacProjErr      = cvCreateMat(12*numImages,1,CV_64F) );
1214     CV_CALL( jacPointErr     = cvCreateMat(4*numPoints,1,CV_64F) );
1215
1216
1217     int i;
1218     for( i = 0; i < numPoints; i++ )
1219     {
1220         CV_CALL( matrsVi[i]        = cvCreateMat(4,4,CV_64F) );
1221         CV_CALL( workMatrsInvVi[i] = cvCreateMat(4,4,CV_64F) );
1222     }
1223
1224     for( currImage = 0; currImage < numImages; currImage++ )
1225     {
1226         CV_CALL( matrsUk[currImage]     = cvCreateMat(12,12,CV_64F) );
1227         CV_CALL( workMatrsUk[currImage] = cvCreateMat(12,12,CV_64F) );
1228
1229         int currVisPoint = 0, currPoint, numVisPoint;
1230         for( currPoint = 0; currPoint < numPoints; currPoint++ )
1231         {
1232             if( cvmGet(pointsPres[currImage],0,currPoint) > 0 )
1233             {
1234                 currVisPoint++;
1235             }
1236         }
1237
1238         numVisPoint = currVisPoint;
1239
1240         /* Allocate memory for current image data */
1241         CV_CALL( observVisPoints[currImage]  = cvCreateMat(2,numVisPoint,CV_64F) );
1242         CV_CALL( projVisPoints[currImage]    = cvCreateMat(2,numVisPoint,CV_64F) );
1243
1244         /* create error matrix */
1245         CV_CALL( errorProjPoints[currImage] = cvCreateMat(2,numVisPoint,CV_64F) );
1246
1247         /* Create derivate matrices */
1248         CV_CALL( DerivProj[currImage]  = cvCreateMat(2*numVisPoint,12,CV_64F) );
1249         CV_CALL( DerivPoint[currImage] = cvCreateMat(2,numVisPoint*4,CV_64F) );
1250
1251         /* Copy observed projected visible points */
1252         currVisPoint = 0;
1253         for( currPoint = 0; currPoint < numPoints; currPoint++ )
1254         {
1255             if( cvmGet(pointsPres[currImage],0,currPoint) > 0 )
1256             {
1257                 cvmSet(observVisPoints[currImage],0,currVisPoint,cvmGet(observProjPoints[currImage],0,currPoint));
1258                 cvmSet(observVisPoints[currImage],1,currVisPoint,cvmGet(observProjPoints[currImage],1,currPoint));
1259                 currVisPoint++;
1260             }
1261         }
1262     }
1263     /*---------------------------------------------------*/
1264
1265     CV_CALL( matrTmpSys1   = cvCreateMat(numPoints*4, numImages*12, CV_64F) );
1266     CV_CALL( matrSysDeltaP = cvCreateMat(numImages*12, numImages*12, CV_64F) );
1267     CV_CALL( vectTmpSys3   = cvCreateMat(numPoints*4,1,CV_64F) );
1268     CV_CALL( vectSysDeltaP = cvCreateMat(numImages*12,1,CV_64F) );
1269     CV_CALL( deltaP        = cvCreateMat(numImages*12,1,CV_64F) );
1270     CV_CALL( deltaM        = cvCreateMat(numPoints*4,1,CV_64F) );
1271     CV_CALL( vectTmpSysM   = cvCreateMat(numPoints*4,1,CV_64F) );
1272
1273
1274 //#ifdef TRACK_BUNDLE
1275 #if 1
1276     {
1277         /* Create file to track */
1278         FILE* file;
1279         file = fopen( TRACK_BUNDLE_FILE ,"w");
1280         fprintf(file,"begin\n");
1281         fclose(file);
1282     }
1283 #endif
1284
1285     /* ============= main optimization loop ============== */
1286
1287     /* project all points using current vector X */
1288     icvProjPointsStatusFunc(numImages, vectorX_points4D, vectorX_projMatrs, pointsPres, projVisPoints);
1289
1290     /* Compute error with observed value and computed projection */
1291     double prevError;
1292     prevError = 0;
1293     for( currImage = 0; currImage < numImages; currImage++ )
1294     {
1295         cvSub(observVisPoints[currImage],projVisPoints[currImage],errorProjPoints[currImage]);
1296         double currNorm = cvNorm(errorProjPoints[currImage]);
1297         prevError += currNorm * currNorm;
1298     }
1299     prevError = sqrt(prevError);
1300
1301     int currIter;
1302     double change;
1303     double alpha;
1304
1305 //#ifdef TRACK_BUNDLE
1306 #if 1
1307     {
1308         /* Create file to track */
1309         FILE* file;
1310         file = fopen( TRACK_BUNDLE_FILE ,"a");
1311         fprintf(file,"\n========================================\n");;
1312         fprintf(file,"Iter=0\n");
1313         fprintf(file,"Error = %20.15lf\n",prevError);
1314         fprintf(file,"Change = %20.15lf\n",1.0);
1315
1316         fprintf(file,"projection errors\n");
1317
1318         /* Print all proejction errors */
1319         int currImage;
1320         for( currImage = 0; currImage < numImages; currImage++)
1321         {
1322             fprintf(file,"\nImage=%d\n",currImage);
1323             int numPn = errorProjPoints[currImage]->cols;
1324             for( int currPoint = 0; currPoint < numPn; currPoint++ )
1325             {
1326                 double ex,ey;
1327                 ex = cvmGet(errorProjPoints[currImage],0,currPoint);
1328                 ey = cvmGet(errorProjPoints[currImage],1,currPoint);
1329                 fprintf(file,"%40.35lf, %40.35lf\n",ex,ey);
1330             }
1331         }
1332         fclose(file);
1333     }
1334 #endif
1335
1336     currIter = 0;
1337     change = 1;
1338     alpha = 0.001;
1339
1340
1341     do
1342     {
1343
1344 #ifdef TRACK_BUNDLE
1345         {
1346             FILE *file;
1347             file = fopen( TRACK_BUNDLE_FILE ,"a");
1348             fprintf(file,"\n----- test 6 do main -----\n");
1349
1350             double norm = cvNorm(vectorX_points4D);
1351             fprintf(file,"        test 6.01 prev normPnts=%lf\n",norm);
1352
1353             for( int i=0;i<numImages;i++ )
1354             {
1355                 double norm = cvNorm(vectorX_projMatrs[i]);
1356                 fprintf(file,"        test 6.01 prev normProj=%lf\n",norm);
1357             }
1358
1359             fclose(file);
1360         }
1361 #endif
1362
1363         /* Compute derivates by projectinon matrices */
1364         icvComputeDerivateProjAll(vectorX_points4D,vectorX_projMatrs,pointsPres,numImages,DerivProj);
1365
1366         /* Compute derivates by 4D points */
1367         icvComputeDerivatePointsAll(vectorX_points4D,vectorX_projMatrs,pointsPres,numImages,DerivPoint);
1368
1369         /* Compute matrces Uk */
1370         icvComputeMatrixUAll(numImages,DerivProj,matrsUk);
1371         icvComputeMatrixVAll(numImages,DerivPoint,pointsPres,matrsVi);
1372         icvComputeMatrixW(numImages,DerivProj,DerivPoint,pointsPres,matrW);
1373
1374
1375 #ifdef TRACK_BUNDLE
1376         {
1377             FILE *file;
1378             file = fopen( TRACK_BUNDLE_FILE ,"a");
1379             fprintf(file,"\n----- test 6.03 do matrs U V -----\n");
1380
1381             int i;
1382             for( i = 0; i < numImages; i++ )
1383             {
1384                 double norm = cvNorm(matrsUk[i]);
1385                 fprintf(file,"        test 6.01 prev matrsUk=%lf\n",norm);
1386             }
1387
1388             for( i = 0; i < numPoints; i++ )
1389             {
1390                 double norm = cvNorm(matrsVi[i]);
1391                 fprintf(file,"        test 6.01 prev matrsVi=%lf\n",norm);
1392             }
1393
1394             fclose(file);
1395         }
1396 #endif
1397
1398         /* Compute jac errors */
1399         icvComputeJacErrorProj(numImages, DerivProj, errorProjPoints, jacProjErr);
1400         icvComputeJacErrorPoint(numImages, DerivPoint, errorProjPoints, pointsPres, jacPointErr);
1401
1402 #ifdef TRACK_BUNDLE
1403         {
1404             FILE *file;
1405             file = fopen( TRACK_BUNDLE_FILE ,"a");
1406             fprintf(file,"\n----- test 6 do main -----\n");
1407             double norm1 = cvNorm(vectorX_points4D);
1408             fprintf(file,"        test 6.02 post normPnts=%lf\n",norm1);
1409             fclose(file);
1410         }
1411 #endif
1412         /* Copy matrices Uk to work matrices Uk */
1413         for( currImage = 0; currImage < numImages; currImage++ )
1414         {
1415             cvCopy(matrsUk[currImage],workMatrsUk[currImage]);
1416         }
1417
1418 #ifdef TRACK_BUNDLE
1419         {
1420             FILE *file;
1421             file = fopen( TRACK_BUNDLE_FILE ,"a");
1422             fprintf(file,"\n----- test 60.3 do matrs U V -----\n");
1423
1424             int i;
1425             for( i = 0; i < numImages; i++ )
1426             {
1427                 double norm = cvNorm(matrsUk[i]);
1428                 fprintf(file,"        test 6.01 post1 matrsUk=%lf\n",norm);
1429             }
1430
1431             for( i = 0; i < numPoints; i++ )
1432             {
1433                 double norm = cvNorm(matrsVi[i]);
1434                 fprintf(file,"        test 6.01 post1 matrsVi=%lf\n",norm);
1435             }
1436
1437             fclose(file);
1438         }
1439 #endif
1440
1441         /* ========== Solve normal equation for given alpha and Jacobian ============ */
1442
1443         do
1444         {
1445             /* ---- Add alpha to matrices --- */
1446             /* Add alpha to matrInvVi and make workMatrsInvVi */
1447
1448             int currV;
1449             for( currV = 0; currV < numPoints; currV++ )
1450             {
1451                 cvCopy(matrsVi[currV],workMatrVi);
1452
1453                 for( int i = 0; i < 4; i++ )
1454                 {
1455                     cvmSet(workMatrVi,i,i,cvmGet(matrsVi[currV],i,i)*(1+alpha) );
1456                 }
1457
1458                 cvInvert(workMatrVi,workMatrsInvVi[currV],CV_LU/*,&currV*/);
1459             }
1460
1461             /* Add alpha to matrUk and make matrix workMatrsUk */
1462             for( currImage = 0; currImage< numImages; currImage++ )
1463             {
1464
1465                 for( i = 0; i < 12; i++ )
1466                 {
1467                     cvmSet(workMatrsUk[currImage],i,i,cvmGet(matrsUk[currImage],i,i)*(1+alpha));
1468                 }
1469
1470
1471             }
1472
1473             /* Fill matrix to make system for computing delta P (matrTmpSys1 = inv(V)*tr(W) )*/
1474             for( currV = 0; currV < numPoints; currV++ )
1475             {
1476                 int currRowV;
1477                 for( currRowV = 0; currRowV < 4; currRowV++ )
1478                 {
1479                     for( currImage = 0; currImage < numImages; currImage++ )
1480                     {
1481                         for( int currCol = 0; currCol < 12; currCol++ )/* For each column of transposed matrix W */
1482                         {
1483                             double sum = 0;
1484                             for( i = 0; i < 4; i++ )
1485                             {
1486                                 sum += cvmGet(workMatrsInvVi[currV],currRowV,i) *
1487                                        cvmGet(matrW,currImage*12+currCol,currV*4+i);
1488                             }
1489                             cvmSet(matrTmpSys1,currV*4+currRowV,currImage*12+currCol,sum);
1490                         }
1491                     }
1492                 }
1493             }
1494
1495
1496             /* Fill matrix to make system for computing delta P (matrTmpSys2 = W * matrTmpSys ) */
1497             cvmMul(matrW,matrTmpSys1,matrSysDeltaP);
1498
1499             /* need to compute U-matrTmpSys2. But we compute matTmpSys2-U */
1500             for( currImage = 0; currImage < numImages; currImage++ )
1501             {
1502                 CvMat subMatr;
1503                 cvGetSubRect(matrSysDeltaP,&subMatr,cvRect(currImage*12,currImage*12,12,12));
1504                 cvSub(&subMatr,workMatrsUk[currImage],&subMatr);
1505             }
1506
1507             /* Compute right side of normal equation  */
1508             for( currV = 0; currV < numPoints; currV++ )
1509             {
1510                 CvMat subMatrErPnts;
1511                 CvMat subMatr;
1512                 cvGetSubRect(jacPointErr,&subMatrErPnts,cvRect(0,currV*4,1,4));
1513                 cvGetSubRect(vectTmpSys3,&subMatr,cvRect(0,currV*4,1,4));
1514                 cvmMul(workMatrsInvVi[currV],&subMatrErPnts,&subMatr);
1515             }
1516
1517             cvmMul(matrW,vectTmpSys3,vectSysDeltaP);
1518             cvSub(vectSysDeltaP,jacProjErr,vectSysDeltaP);
1519
1520             /* Now we can compute part of normal system - deltaP */
1521             cvSolve(matrSysDeltaP ,vectSysDeltaP, deltaP, CV_SVD);
1522
1523             /* Print deltaP to file */
1524
1525 #ifdef TRACK_BUNDLE
1526             {
1527                 FILE* file;
1528                 file = fopen( TRACK_BUNDLE_FILE_DELTAP ,"w");
1529
1530                 int currImage;
1531                 for( currImage = 0; currImage < numImages; currImage++ )
1532                 {
1533                     fprintf(file,"\nImage=%d\n",currImage);
1534                     int i;
1535                     for( i = 0; i < 12; i++ )
1536                     {
1537                         double val;
1538                         val = cvmGet(deltaP,currImage*12+i,0);
1539                         fprintf(file,"%lf\n",val);
1540                     }
1541                     fprintf(file,"\n");
1542                 }
1543                 fclose(file);
1544             }
1545 #endif
1546             /* We know deltaP and now we can compute system for deltaM */
1547             for( i = 0; i < numPoints * 4; i++ )
1548             {
1549                 double sum = 0;
1550                 for( int j = 0; j < numImages * 12; j++ )
1551                 {
1552                     sum += cvmGet(matrW,j,i) * cvmGet(deltaP,j,0);
1553                 }
1554                 cvmSet(vectTmpSysM,i,0,cvmGet(jacPointErr,i,0)-sum);
1555             }
1556
1557             /* Compute deltaM */
1558             for( currV = 0; currV < numPoints; currV++ )
1559             {
1560                 CvMat subMatr;
1561                 CvMat subMatrM;
1562                 cvGetSubRect(vectTmpSysM,&subMatr,cvRect(0,currV*4,1,4));
1563                 cvGetSubRect(deltaM,&subMatrM,cvRect(0,currV*4,1,4));
1564                 cvmMul(workMatrsInvVi[currV],&subMatr,&subMatrM);
1565             }
1566
1567             /* We know delta and compute new value of vector X: nextVectX = vectX + deltas */
1568
1569             /* Compute new P */
1570             for( currImage = 0; currImage < numImages; currImage++ )
1571             {
1572                 for( i = 0; i < 3; i++ )
1573                 {
1574                     for( int j = 0; j < 4; j++ )
1575                     {
1576                         cvmSet(newVectorX_projMatrs[currImage],i,j,
1577                                 cvmGet(vectorX_projMatrs[currImage],i,j) + cvmGet(deltaP,currImage*12+i*4+j,0));
1578                     }
1579                 }
1580             }
1581
1582             /* Compute new M */
1583             int currPoint;
1584             for( currPoint = 0; currPoint < numPoints; currPoint++ )
1585             {
1586                 for( i = 0; i < 4; i++ )
1587                 {
1588                     cvmSet(newVectorX_points4D,i,currPoint,
1589                         cvmGet(vectorX_points4D,i,currPoint) + cvmGet(deltaM,currPoint*4+i,0));
1590                 }
1591             }
1592
1593             /* ----- Compute errors for new vectorX ----- */
1594             /* Project points using new vectorX and status of each point */
1595             icvProjPointsStatusFunc(numImages, newVectorX_points4D, newVectorX_projMatrs, pointsPres, projVisPoints);
1596             /* Compute error with observed value and computed projection */
1597             double newError = 0;
1598             for( currImage = 0; currImage < numImages; currImage++ )
1599             {
1600                 cvSub(observVisPoints[currImage],projVisPoints[currImage],errorProjPoints[currImage]);
1601                 double currNorm = cvNorm(errorProjPoints[currImage]);
1602
1603 //#ifdef TRACK_BUNDLE
1604 #if 1
1605                 {
1606                     FILE *file;
1607                     file = fopen( TRACK_BUNDLE_FILE ,"a");
1608                     fprintf(file,"\n----- test 13,01 currImage=%d currNorm=%lf -----\n",currImage,currNorm);
1609                     fclose(file);
1610                 }
1611 #endif
1612                 newError += currNorm * currNorm;
1613             }
1614             newError = sqrt(newError);
1615
1616             currIter++;
1617
1618
1619
1620
1621 //#ifdef TRACK_BUNDLE
1622 #if 1
1623             {
1624                 /* Create file to track */
1625                 FILE* file;
1626                 file = fopen( TRACK_BUNDLE_FILE ,"a");
1627                 fprintf(file,"\n========================================\n");
1628                 fprintf(file,"numPoints=%d\n",numPoints);
1629                 fprintf(file,"Iter=%d\n",currIter);
1630                 fprintf(file,"Error = %20.15lf\n",newError);
1631                 fprintf(file,"Change = %20.15lf\n",change);
1632
1633
1634                 /* Print all projection errors */
1635 #if 0
1636                 fprintf(file,"projection errors\n");
1637                 int currImage;
1638                 for( currImage = 0; currImage < numImages; currImage++)
1639                 {
1640                     fprintf(file,"\nImage=%d\n",currImage);
1641                     int numPn = errorProjPoints[currImage]->cols;
1642                     for( int currPoint = 0; currPoint < numPn; currPoint++ )
1643                     {
1644                         double ex,ey;
1645                         ex = cvmGet(errorProjPoints[currImage],0,currPoint);
1646                         ey = cvmGet(errorProjPoints[currImage],1,currPoint);
1647                         fprintf(file,"%lf,%lf\n",ex,ey);
1648                     }
1649                 }
1650                 fprintf(file,"\n---- test 0 -----\n");
1651 #endif
1652
1653                 fclose(file);
1654             }
1655 #endif
1656
1657
1658
1659             /* Compare new error and last error */
1660             if( newError < prevError )
1661             {/* accept new value */
1662                 prevError = newError;
1663                 /* Compute relative change of required parameter vectorX. change = norm(curr-prev) / norm(curr) )  */
1664                 {
1665                     double normAll1 = 0;
1666                     double normAll2 = 0;
1667                     double currNorm1 = 0;
1668                     double currNorm2 = 0;
1669                     /* compute norm for projection matrices */
1670                     for( currImage = 0; currImage < numImages; currImage++ )
1671                     {
1672                         currNorm1 = cvNorm(newVectorX_projMatrs[currImage],vectorX_projMatrs[currImage]);
1673                         currNorm2 = cvNorm(newVectorX_projMatrs[currImage]);
1674
1675                         normAll1 += currNorm1 * currNorm1;
1676                         normAll2 += currNorm2 * currNorm2;
1677                     }
1678
1679                     /* compute norm for points */
1680                     currNorm1 = cvNorm(newVectorX_points4D,vectorX_points4D);
1681                     currNorm2 = cvNorm(newVectorX_points4D);
1682
1683                     normAll1 += currNorm1 * currNorm1;
1684                     normAll2 += currNorm2 * currNorm2;
1685
1686                     /* compute change */
1687                     change = sqrt(normAll1) / sqrt(normAll2);
1688
1689
1690 //#ifdef TRACK_BUNDLE
1691 #if 1
1692                     {
1693                         /* Create file to track */
1694                         FILE* file;
1695                         file = fopen( TRACK_BUNDLE_FILE ,"a");
1696                         fprintf(file,"\nChange inside newVal change = %20.15lf\n",change);
1697                         fprintf(file,"   normAll1= %20.15lf\n",sqrt(normAll1));
1698                         fprintf(file,"   normAll2= %20.15lf\n",sqrt(normAll2));
1699
1700                         fclose(file);
1701                     }
1702 #endif
1703
1704                 }
1705
1706                 alpha /= 10;
1707                 for( currImage = 0; currImage < numImages; currImage++ )
1708                 {
1709                     cvCopy(newVectorX_projMatrs[currImage],vectorX_projMatrs[currImage]);
1710                 }
1711                 cvCopy(newVectorX_points4D,vectorX_points4D);
1712
1713                 break;
1714             }
1715             else
1716             {
1717                 alpha *= 10;
1718             }
1719
1720         } while( change > epsilon && currIter < maxIter );/* solve normal equation using current alpha */
1721
1722 //#ifdef TRACK_BUNDLE
1723 #if 1
1724         {
1725             FILE* file;
1726             file = fopen( TRACK_BUNDLE_FILE ,"a");
1727             fprintf(file,"\nBest error = %40.35lf\n",prevError);
1728             fclose(file);
1729         }
1730
1731 #endif
1732
1733
1734     } while( change > epsilon && currIter < maxIter );
1735
1736     /*--------------------------------------------*/
1737     /* Optimization complete copy computed params */
1738     /* Copy projection matrices */
1739     for( currImage = 0; currImage < numImages; currImage++ )
1740     {
1741         cvCopy(newVectorX_projMatrs[currImage],resultProjMatrs[currImage]);
1742     }
1743     /* Copy 4D points */
1744     cvCopy(newVectorX_points4D,resultPoints4D);
1745
1746 //    free(memory);
1747
1748     __END__;
1749
1750     /* Free allocated memory */
1751
1752     /* Free simple matrices */
1753     cvFree(&vectorX_points4D);
1754     cvFree(&newVectorX_points4D);
1755     cvFree(&changeVectorX_points4D);
1756     cvFree(&changeVectorX_projMatrs);
1757     cvFree(&matrW);
1758     cvFree(&workMatrVi);
1759     cvFree(&jacProjErr);
1760     cvFree(&jacPointErr);
1761     cvFree(&matrTmpSys1);
1762     cvFree(&matrSysDeltaP);
1763     cvFree(&vectTmpSys3);
1764     cvFree(&vectSysDeltaP);
1765     cvFree(&deltaP);
1766     cvFree(&deltaM);
1767     cvFree(&vectTmpSysM);
1768
1769     /* Free arrays of matrices */
1770     icvFreeMatrixArray(&vectorX_projMatrs,numImages);
1771     icvFreeMatrixArray(&newVectorX_projMatrs,numImages);
1772     icvFreeMatrixArray(&observVisPoints,numImages);
1773     icvFreeMatrixArray(&projVisPoints,numImages);
1774     icvFreeMatrixArray(&errorProjPoints,numImages);
1775     icvFreeMatrixArray(&DerivProj,numImages);
1776     icvFreeMatrixArray(&DerivPoint,numImages);
1777     icvFreeMatrixArray(&matrsUk,numImages);
1778     icvFreeMatrixArray(&workMatrsUk,numImages);
1779     icvFreeMatrixArray(&matrsVi,numPoints);
1780     icvFreeMatrixArray(&workMatrsInvVi,numPoints);
1781
1782     return;
1783 }