git.maemo.org Git - opencv/blob - src/cvaux/cvba.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 //                         License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2009, PhaseSpace Inc., 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 names of the copyright holders 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 #include "_cvaux.h"
  43
  44 namespace cv {
  45
  46 LevMarqSparse::LevMarqSparse()
  47 {
  48     A = B = W = Vis_index = X = prevP = P = deltaP = err = JtJ_diag = S = hX = NULL;
  49     U = ea = V = inv_V_star = eb = Yj = NULL;
  50 }
  51
  52 LevMarqSparse::~LevMarqSparse()
  53 {
  54     clear();
  55 }
  56
  57 LevMarqSparse::LevMarqSparse(int npoints, // number of points
  58         int ncameras, // number of cameras
  59         int nPointParams, // number of params per one point  (3 in case of 3D points)
  60         int nCameraParams, // number of parameters per one camera
  61         int nErrParams, // number of parameters in measurement vector
  62                         // for 1 point at one camera (2 in case of 2D projections)
  63         Mat& visibility, // visibility matrix. rows correspond to points, columns correspond to cameras
  64                          // 1 - point is visible for the camera, 0 - invisible
  65         Mat& P0, // starting vector of parameters, first cameras then points
  66         Mat& X_, // measurements, in order of visibility. non visible cases are skipped
  67         TermCriteria criteria, // termination criteria
  68
  69         // callback for estimation of Jacobian matrices
  70         void (CV_CDECL * fjac)(int i, int j, Mat& point_params,
  71                                Mat& cam_params, Mat& A, Mat& B, void* data),
  72         // callback for estimation of backprojection errors
  73         void (CV_CDECL * func)(int i, int j, Mat& point_params,
  74                                Mat& cam_params, Mat& estim, void* data),
  75         void* data // user-specific data passed to the callbacks
  76         )
  77 {
  78     A = B = W = Vis_index = X = prevP = P = deltaP = err = JtJ_diag = S = hX = NULL;
  79     U = ea = V = inv_V_star = eb = Yj = NULL;
  80
  81     run(npoints, ncameras, nPointParams, nCameraParams, nErrParams, visibility,
  82         P0, X_, criteria, fjac, func, data);
  83 }
  84
  85 void LevMarqSparse::clear()
  86 {
  87      for( int i = 0; i < num_points; i++ )
  88      {
  89          for(int j = 0; j < num_cams; j++ )
  90          {
  91                  CvMat* tmp = ((CvMat**)(A->data.ptr + i * A->step))[j];
  92                  if( tmp )
  93                      cvReleaseMat( &tmp );
  94
  95                  tmp = ((CvMat**)(B->data.ptr + i * B->step))[j];
  96                  if( tmp )
  97                      cvReleaseMat( &tmp );
  98
  99                  tmp = ((CvMat**)(W->data.ptr + j * W->step))[i];
 100                  if( tmp )
 101                      cvReleaseMat( &tmp );
 102          }
 103      }
 104      cvReleaseMat( &A );
 105      cvReleaseMat( &B );
 106      cvReleaseMat( &W );
 107      cvReleaseMat( &Vis_index);
 108
 109      for( int j = 0; j < num_cams; j++ )
 110      {
 111          cvReleaseMat( &U[j] );
 112      }
 113      delete U;
 114
 115      for( int j = 0; j < num_cams; j++ )
 116      {
 117          cvReleaseMat( &ea[j] );
 118      }
 119      delete ea;
 120
 121      //allocate V and inv_V_star
 122      for( int i = 0; i < num_points; i++ )
 123      {
 124          cvReleaseMat(&V[i]);
 125          cvReleaseMat(&inv_V_star[i]);
 126      }
 127      delete V;
 128      delete inv_V_star;
 129
 130      for( int i = 0; i < num_points; i++ )
 131      {
 132          cvReleaseMat(&eb[i]);
 133      }
 134      delete eb;
 135
 136      for( int i = 0; i < num_points; i++ )
 137      {
 138          cvReleaseMat(&Yj[i]);
 139      }
 140      delete Yj;
 141
 142     cvReleaseMat(&X);
 143     cvReleaseMat(&prevP);
 144     cvReleaseMat(&P);
 145     cvReleaseMat(&deltaP);
 146
 147     cvReleaseMat(&err);
 148
 149     cvReleaseMat(&JtJ_diag);
 150     cvReleaseMat(&S);
 151     cvReleaseMat(&hX);
 152 }
 153
 154 //A params correspond to  Cameras
 155 //B params correspont to  Points
 156
 157 //num_cameras  - total number of cameras
 158 //num_points   - total number of points
 159
 160 //num_par_per_camera - number of parameters per camera
 161 //num_par_per_point - number of parameters per point
 162
 163 //num_errors - number of measurements.
 164
 165 void LevMarqSparse::run( int num_points_, //number of points
 166             int num_cams_, //number of cameras
 167             int num_point_param_, //number of params per one point  (3 in case of 3D points)
 168             int num_cam_param_, //number of parameters per one camera
 169             int num_err_param_, //number of parameters in measurement vector for 1 point at one camera (2 in case of 2D projections)
 170             Mat& visibility,   //visibility matrix . rows correspond to points, columns correspond to cameras
 171                                // 0 - point is visible for the camera, 0 - invisible
 172             Mat& P0, //starting vector of parameters, first cameras then points
 173             Mat& X_init, //measurements, in order of visibility. non visible cases are skipped
 174             TermCriteria criteria_init,
 175             void (*fjac_)(int i, int j, Mat& point_params, Mat& cam_params, Mat& A, Mat& B, void* data),
 176             void (*func_)(int i, int j, Mat& point_params, Mat& cam_params, Mat& estim, void* data),
 177             void* data_
 178              ) //termination criteria
 179 {
 180     clear();
 181
 182     func = func_; //assign evaluation function
 183     fjac = fjac_; //assign jacobian
 184     data = data_;
 185
 186     num_cams = num_cams_;
 187     num_points = num_points_;
 188     num_err_param = num_err_param_;
 189     num_cam_param = num_cam_param_;
 190     num_point_param = num_point_param_;
 191
 192     //compute all sizes
 193     int Aij_width = num_cam_param;
 194     int Aij_height = num_err_param;
 195
 196     int Bij_width = num_point_param;
 197     int Bij_height = num_err_param;
 198
 199     int U_size = Aij_width;
 200     int V_size = Bij_width;
 201
 202     int Wij_height = Aij_width;
 203     int Wij_width = Bij_width;
 204
 205     //allocate memory for all Aij, Bij, U, V, W
 206
 207     //allocate num_points*num_cams matrices A
 208
 209     //Allocate matrix A whose elements are nointers to Aij
 210     //if Aij is zero (point i is not visible in camera j) then A(i,j) contains NULL
 211     A = cvCreateMat( num_points, num_cams, CV_32S /*pointer is stored here*/ );
 212     B = cvCreateMat( num_points, num_cams, CV_32S /*pointer is stored here*/ );
 213     W = cvCreateMat( num_cams, num_points, CV_32S /*pointer is stored here*/ );
 214     Vis_index = cvCreateMat( num_points, num_cams, CV_32S /*integer index is stored here*/ );
 215     cvSetZero( A );
 216     cvSetZero( B );
 217     cvSetZero( W );
 218     cvSet( Vis_index, cvScalar(-1) );
 219
 220     //fill matrices A and B based on visibility
 221     CvMat _vis = visibility;
 222     int index = 0;
 223     for( int i = 0; i < num_points; i++ )
 224     {
 225         for(int j = 0; j < num_cams; j++ )
 226         {
 227             if( ((int*)(_vis.data.ptr+ i * _vis.step))[j] )
 228             {
 229                 ((int*)(Vis_index->data.ptr + i * Vis_index->step))[j] = index;
 230                 index += num_err_param;
 231
 232                 //create matrices Aij, Bij
 233                 CvMat* tmp = cvCreateMat( Aij_height, Aij_width, CV_64F );
 234                 ((CvMat**)(A->data.ptr + i * A->step))[j] = tmp;
 235
 236                 tmp = cvCreateMat( Bij_height, Bij_width, CV_64F );
 237                 ((CvMat**)(B->data.ptr + i * B->step))[j] = tmp;
 238
 239                 tmp = cvCreateMat( Wij_height, Wij_width, CV_64F );
 240                 ((CvMat**)(W->data.ptr + j * W->step))[i] = tmp;  //note indices i and j swapped
 241             }
 242         }
 243     }
 244
 245     //allocate U
 246     U = new CvMat* [num_cams];
 247     for( int j = 0; j < num_cams; j++ )
 248     {
 249         U[j] = cvCreateMat( U_size, U_size, CV_64F );
 250     }
 251     //allocate ea
 252     ea = new CvMat* [num_cams];
 253     for( int j = 0; j < num_cams; j++ )
 254     {
 255         ea[j] = cvCreateMat( U_size, 1, CV_64F );
 256     }
 257
 258     //allocate V and inv_V_star
 259     V = new CvMat* [num_points];
 260     inv_V_star = new CvMat* [num_points];
 261     for( int i = 0; i < num_points; i++ )
 262     {
 263         V[i] = cvCreateMat( V_size, V_size, CV_64F );
 264         inv_V_star[i] = cvCreateMat( V_size, V_size, CV_64F );
 265     }
 266
 267     //allocate eb
 268     eb = new CvMat* [num_points];
 269     for( int i = 0; i < num_points; i++ )
 270     {
 271         eb[i] = cvCreateMat( V_size, 1, CV_64F );
 272     }
 273
 274     //allocate Yj
 275     Yj = new CvMat* [num_points];
 276     for( int i = 0; i < num_points; i++ )
 277     {
 278         Yj[i] = cvCreateMat( Wij_height, Wij_width, CV_64F );  //Yij has the same size as Wij
 279     }
 280
 281     //allocate matrix S
 282     S = cvCreateMat( num_cams * num_cam_param, num_cams * num_cam_param, CV_64F);
 283
 284     JtJ_diag = cvCreateMat( num_cams * num_cam_param + num_points * num_point_param, 1, CV_64F );
 285
 286     //set starting parameters
 287     CvMat _tmp_ = CvMat(P0);
 288     prevP = cvCloneMat( &_tmp_ );
 289     P = cvCloneMat( &_tmp_ );
 290     deltaP = cvCloneMat( &_tmp_ );
 291
 292     //set measurements
 293     _tmp_ = CvMat(X_init);
 294     X = cvCloneMat( &_tmp_ );
 295     //create vector for estimated measurements
 296     hX = cvCreateMat( X->rows, X->cols, CV_64F );
 297     //create error vector
 298     err = cvCreateMat( X->rows, X->cols, CV_64F );
 299
 300     ask_for_proj();
 301
 302     //compute initial error
 303     cvSub(  X, hX, err );
 304
 305     prevErrNorm = cvNorm( err, 0,  CV_L2 );
 306     iters = 0;
 307     criteria = criteria_init;
 308
 309     optimize();
 310 }
 311
 312 void LevMarqSparse::ask_for_proj()
 313 {
 314     //given parameter P, compute measurement hX
 315     int ind = 0;
 316     for( int i = 0; i < num_points; i++ )
 317     {
 318         CvMat point_mat;
 319         cvGetSubRect( P, &point_mat, cvRect( 0, num_cams * num_cam_param + num_point_param * i, 1, num_point_param ));
 320
 321         for( int j = 0; j < num_cams; j++ )
 322         {
 323             CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
 324             if( Aij ) //visible
 325             {
 326                 CvMat cam_mat;
 327                 cvGetSubRect( P, &cam_mat, cvRect( 0, j * num_cam_param, 1, num_cam_param ));
 328                 CvMat measur_mat;
 329                 cvGetSubRect( hX, &measur_mat, cvRect( 0, ind * num_err_param, 1, num_err_param ));
 330                 Mat _point_mat(&point_mat), _cam_mat(&cam_mat), _measur_mat(&measur_mat);
 331                 func( i, j, _point_mat, _cam_mat, _measur_mat, data );
 332
 333                 assert( ind*num_err_param == ((int*)(Vis_index->data.ptr + i * Vis_index->step))[j]);
 334
 335                 ind+=1;
 336
 337             }
 338         }
 339     }
 340 }
 341 //iteratively asks for Jacobians for every camera_point pair
 342 void LevMarqSparse::ask_for_projac()  //should be evaluated at point prevP
 343 {
 344     // compute jacobians Aij and Bij
 345     for( int i = 0; i < A->height; i++ )
 346     {
 347         CvMat point_mat;
 348         cvGetSubRect( prevP, &point_mat, cvRect( 0, num_cams * num_cam_param + num_point_param * i, 1, num_point_param ));
 349
 350
 351         CvMat** A_line = (CvMat**)(A->data.ptr + A->step * i);
 352         CvMat** B_line = (CvMat**)(B->data.ptr + B->step * i);
 353
 354         for( int j = 0; j < A->width; j++ )
 355         {
 356             CvMat* Aij = A_line[j];
 357             if( Aij ) //Aij is not zero
 358             {
 359                 CvMat cam_mat;
 360                 cvGetSubRect( prevP, &cam_mat, cvRect( 0, j * num_cam_param, 1, num_cam_param ));
 361
 362                 CvMat* Bij = B_line[j];
 363                 Mat _point_mat(&point_mat), _cam_mat(&cam_mat), _Aij(Aij), _Bij(Bij);
 364                 (*fjac)(i, j, _point_mat, _cam_mat, _Aij, _Bij, data);
 365             }
 366         }
 367     }
 368 }
 369
 370 void LevMarqSparse::optimize() //main function that runs minimization
 371 {
 372     bool done = false;
 373
 374     CvMat* YWt = cvCreateMat( num_cam_param, num_cam_param, CV_64F ); //this matrix used to store Yij*Wik'
 375     CvMat* E = cvCreateMat( S->height, 1 , CV_64F ); //this is right part of system with S
 376
 377     while(!done)
 378     {
 379         // compute jacobians Aij and Bij
 380         ask_for_projac();
 381
 382         //compute U_j  and  ea_j
 383         for( int j = 0; j < num_cams; j++ )
 384         {
 385             cvSetZero(U[j]);
 386             cvSetZero(ea[j]);
 387             //summ by i (number of points)
 388             for( int i = 0; i < num_points; i++ )
 389             {
 390                 //get Aij
 391                 CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
 392                 if( Aij )
 393                 {
 394                     //Uj+= AijT*Aij
 395                     cvGEMM( Aij, Aij, 1, U[j], 1, U[j], CV_GEMM_A_T );
 396
 397                     //ea_j += AijT * e_ij
 398                     CvMat eij;
 399
 400                     int index = ((int*)(Vis_index->data.ptr + i * Vis_index->step))[j];
 401
 402                     cvGetSubRect( err, &eij, cvRect( 0, index, 1, Aij->height /*width of transposed Aij*/ ) );
 403                     cvGEMM( Aij, &eij, 1, ea[j], 1, ea[j], CV_GEMM_A_T );
 404                 }
 405             }
 406         } //U_j and ea_j computed for all j
 407
 408         //compute V_i  and  eb_i
 409         for( int i = 0; i < num_points; i++ )
 410         {
 411             cvSetZero(V[i]);
 412             cvSetZero(eb[i]);
 413
 414             //summ by i (number of points)
 415             for( int j = 0; j < num_cams; j++ )
 416             {
 417                 //get Bij
 418                 CvMat* Bij = ((CvMat**)(B->data.ptr + B->step * i))[j];
 419
 420                 if( Bij )
 421                 {
 422                     //Vi+= BijT*Bij
 423                     cvGEMM( Bij, Bij, 1, V[i], 1, V[i], CV_GEMM_A_T );
 424
 425                     //eb_i += BijT * e_ij
 426                     int index = ((int*)(Vis_index->data.ptr + i * Vis_index->step))[j];
 427
 428                     CvMat eij;
 429                     cvGetSubRect( err, &eij, cvRect( 0, index, 1, Bij->height /*width of transposed Bij*/ ) );
 430                     cvGEMM( Bij, &eij, 1, eb[i], 1, eb[i], CV_GEMM_A_T );
 431                 }
 432             }
 433         } //V_i and eb_i computed for all i
 434
 435         //compute W_ij
 436         for( int i = 0; i < num_points; i++ )
 437         {
 438             for( int j = 0; j < num_cams; j++ )
 439             {
 440                  CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
 441                  if( Aij ) //visible
 442                  {
 443                      CvMat* Bij = ((CvMat**)(B->data.ptr + B->step * i))[j];
 444                      CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
 445
 446                      //multiply
 447                      cvGEMM( Aij, Bij, 1, NULL, 0, Wij, CV_GEMM_A_T );
 448                  }
 449             }
 450         } //Wij computed
 451
 452         //backup diagonal of JtJ before we start augmenting it
 453         {
 454             CvMat dia;
 455             CvMat subr;
 456             for( int j = 0; j < num_cams; j++ )
 457             {
 458                 cvGetDiag(U[j], &dia);
 459                 cvGetSubRect(JtJ_diag, &subr,
 460                              cvRect(0, j*num_cam_param, 1, num_cam_param ));
 461                 cvCopy( &dia, &subr );
 462             }
 463             for( int i = 0; i < num_points; i++ )
 464             {
 465                 cvGetDiag(V[i], &dia);
 466                 cvGetSubRect(JtJ_diag, &subr,
 467                              cvRect(0, num_cams*num_cam_param + i * num_point_param, 1, num_point_param ));
 468                 cvCopy( &dia, &subr );
 469             }
 470         }
 471
 472         if( iters == 0 )
 473         {
 474             //initialize lambda. It is set to 1e-3 * average diagonal element in JtJ
 475             double average_diag = 0;
 476             for( int j = 0; j < num_cams; j++ )
 477             {
 478                 average_diag += cvTrace( U[j] ).val[0];
 479             }
 480             for( int i = 0; i < num_points; i++ )
 481             {
 482                 average_diag += cvTrace( V[i] ).val[0];
 483             }
 484             average_diag /= (num_cams*num_cam_param + num_points * num_point_param );
 485
 486             lambda = 1e-3 * average_diag;
 487         }
 488
 489         //now we are going to find good step and make it
 490         for(;;)
 491         {
 492             //augmentation of diagonal
 493             for(int j = 0; j < num_cams; j++ )
 494             {
 495                 CvMat diag;
 496                 cvGetDiag( U[j], &diag );
 497 #if 1
 498                 cvAddS( &diag, cvScalar( lambda ), &diag );
 499 #else
 500                 cvScale( &diag, &diag, 1 + lambda );
 501 #endif
 502             }
 503             for(int i = 0; i < num_points; i++ )
 504             {
 505                 CvMat diag;
 506                 cvGetDiag( V[i], &diag );
 507 #if 1
 508                 cvAddS( &diag, cvScalar( lambda ), &diag );
 509 #else
 510                 cvScale( &diag, &diag, 1 + lambda );
 511 #endif
 512             }
 513             bool error = false;
 514             //compute inv(V*)
 515             bool inverted_ok = true;
 516             for(int i = 0; i < num_points; i++ )
 517             {
 518                 double det = cvInvert( V[i], inv_V_star[i] );
 519
 520                 if( fabs(det) <= FLT_EPSILON )
 521                 {
 522                     inverted_ok = false;
 523                     break;
 524                 } //means we did wrong augmentation, try to choose different lambda
 525             }
 526
 527             if( inverted_ok )
 528             {
 529                 cvSetZero( E );
 530                 //loop through cameras, compute upper diagonal blocks of matrix S
 531                 for( int j = 0; j < num_cams; j++ )
 532                 {
 533                     //compute Yij = Wij (V*_i)^-1  for all i   (if Wij exists/nonzero)
 534                     for( int i = 0; i < num_points; i++ )
 535                     {
 536                         //
 537                         CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
 538                         if( Wij )
 539                         {
 540                             cvMatMul( Wij, inv_V_star[i], Yj[i] );
 541                         }
 542                     }
 543
 544                     //compute Sjk   for k>=j  (because Sjk = Skj)
 545                     for( int k = j; k < num_cams; k++ )
 546                     {
 547                         cvSetZero( YWt );
 548                         for( int i = 0; i < num_points; i++ )
 549                         {
 550                             //check that both Wij and Wik exist
 551                             CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
 552                             CvMat* Wik = ((CvMat**)(W->data.ptr + W->step * k))[i];
 553
 554                             if( Wij && Wik )
 555                             {
 556                                 //multiply YWt += Yj[i]*Wik'
 557                                 cvGEMM( Yj[i], Wik, 1, YWt, 1, YWt, CV_GEMM_B_T /*transpose Wik*/ );
 558                             }
 559                         }
 560
 561                         //copy result to matrix S
 562
 563                         CvMat Sjk;
 564                         //extract submat
 565                         cvGetSubRect( S, &Sjk, cvRect( k * num_cam_param, j * num_cam_param, num_cam_param, num_cam_param ));
 566
 567
 568                         //if j==k, add diagonal
 569                         if( j != k )
 570                         {
 571                             //just copy with minus
 572                             cvScale( YWt, &Sjk, -1 ); //if we set initial S to zero then we can use cvSub( Sjk, YWt, Sjk);
 573                         }
 574                         else
 575                         {
 576                             //add diagonal value
 577
 578                             //subtract YWt from augmented Uj
 579                             cvSub( U[j], YWt, &Sjk );
 580                         }
 581                     }
 582
 583                     //compute right part of equation involving matrix S
 584                     // e_j=ea_j - \sum_i Y_ij eb_i
 585                     {
 586                     CvMat e_j;
 587
 588                     //select submat
 589                     cvGetSubRect( E, &e_j, cvRect( 0, j * num_cam_param, 1, num_cam_param ) );
 590
 591                     for( int i = 0; i < num_points; i++ )
 592                     {
 593                         CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
 594                         if( Wij )
 595                             cvMatMulAdd( Yj[i], eb[i], &e_j, &e_j );
 596                     }
 597
 598                     cvSub( ea[j], &e_j, &e_j );
 599                     }
 600
 601                 }
 602                 //fill below diagonal elements of matrix S
 603                 cvCompleteSymm( S,  0 /*from upper to low*/ ); //operation may be done by nonzero blocks or during upper diagonal computation
 604
 605                 //Solve linear system  S * deltaP_a = E
 606                 CvMat dpa;
 607                 cvGetSubRect( deltaP, &dpa, cvRect(0, 0, 1, S->width ) );
 608                 int res = cvSolve( S, E, &dpa );
 609
 610                 if( res ) //system solved ok
 611                 {
 612                     //compute db_i
 613                     for( int i = 0; i < num_points; i++ )
 614                     {
 615                         CvMat dbi;
 616                         cvGetSubRect( deltaP, &dbi, cvRect( 0, dpa.height + i * num_point_param, 1, num_point_param ) );
 617
 618                         /* compute \sum_j W_ij^T da_j */
 619                         for( int j = 0; j < num_cams; j++ )
 620                         {
 621                             //get Wij
 622                             CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
 623
 624                             if( Wij )
 625                             {
 626                                 //get da_j
 627                                 CvMat daj;
 628                                 cvGetSubRect( &dpa, &daj, cvRect( 0, j * num_cam_param, 1, num_cam_param ));
 629                                 cvGEMM( Wij, &daj, 1, &dbi, 1, &dbi, CV_GEMM_A_T /* transpose Wij */ );
 630                             }
 631                         }
 632                         //finalize dbi
 633                         cvSub( eb[i], &dbi, &dbi );
 634                         cvMatMul(inv_V_star[i], &dbi, &dbi );  //here we get final dbi
 635                     }  //now we computed whole deltaP
 636
 637                     //add deltaP to delta
 638                     cvAdd( prevP, deltaP, P );
 639
 640                     //evaluate  function with new parameters
 641                     ask_for_proj(); // func( P, hX );
 642
 643                     //compute error
 644                     errNorm = cvNorm( X, hX, CV_L2 );
 645
 646                 }
 647                 else
 648                 {
 649                     error = true;
 650                 }
 651             }
 652             else
 653             {
 654                 error = true;
 655             }
 656             //check solution
 657             if( error || /* singularities somewhere */
 658                 errNorm > prevErrNorm )  //step was not accepted
 659             {
 660                 //increase lambda and reject change
 661                 lambda *= 10;
 662
 663                 //restore diagonal from backup
 664                 {
 665                     CvMat dia;
 666                     CvMat subr;
 667                     for( int j = 0; j < num_cams; j++ )
 668                     {
 669                         cvGetDiag(U[j], &dia);
 670                         cvGetSubRect(JtJ_diag, &subr,
 671                                      cvRect(0, j*num_cam_param, 1, num_cam_param ));
 672                         cvCopy( &subr, &dia );
 673                     }
 674                     for( int i = 0; i < num_points; i++ )
 675                     {
 676                         cvGetDiag(V[i], &dia);
 677                         cvGetSubRect(JtJ_diag, &subr,
 678                                      cvRect(0, num_cams*num_cam_param + i * num_point_param, 1, num_point_param ));
 679                         cvCopy( &subr, &dia );
 680                     }
 681                 }
 682             }
 683             else  //all is ok
 684             {
 685                 //accept change and decrease lambda
 686                 lambda /= 10;
 687                 lambda = MAX(lambda, 1e-16);
 688                 prevErrNorm = errNorm;
 689
 690                 //compute new projection error vector
 691                 cvSub(  X, hX, err );
 692                 break;
 693             }
 694         }
 695         iters++;
 696
 697         double param_change_norm = cvNorm(P, prevP, CV_RELATIVE_L2);
 698         //check termination criteria
 699         if( (criteria.type&CV_TERMCRIT_ITER && iters > criteria.max_iter ) ||
 700             (criteria.type&CV_TERMCRIT_EPS && param_change_norm < criteria.epsilon) )
 701         {
 702             done = true;
 703             break;
 704         }
 705         else
 706         {
 707             //copy new params and continue iterations
 708             cvCopy( P, prevP );
 709         }
 710     }
 711     cvReleaseMat(&YWt);
 712     cvReleaseMat(&E);
 713 }
 714
 715 //Utilities
 716
 717 void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A, CvMat* B, void* /*data*/)
 718 {
 719     //compute jacobian per camera parameters (i.e. Aij)
 720     //take i-th point 3D current coordinates
 721
 722     CvMat _Mi;
 723     cvReshape(point_params, &_Mi, 3, 1 );
 724
 725     CvMat* _mp = cvCreateMat(1, 2, CV_64F ); //projection of the point
 726
 727     //split camera params into different matrices
 728     CvMat _ri, _ti, _k;
 729     cvGetRows( cam_params, &_ri, 0, 3 );
 730     cvGetRows( cam_params, &_ti, 3, 6 );
 731
 732     double intr_data[9] = {0, 0, 0, 0, 0, 0, 0, 0, 1};
 733     intr_data[0] = cam_params->data.db[6];
 734     intr_data[4] = cam_params->data.db[7];
 735     intr_data[2] = cam_params->data.db[8];
 736     intr_data[5] = cam_params->data.db[9];
 737
 738     CvMat _A = cvMat(3,3, CV_64F, intr_data );
 739
 740     CvMat _dpdr, _dpdt, _dpdf, _dpdc, _dpdk;
 741
 742     bool have_dk = cam_params->height - 10 ? true : false;
 743
 744     cvGetCols( A, &_dpdr, 0, 3 );
 745     cvGetCols( A, &_dpdt, 3, 6 );
 746     cvGetCols( A, &_dpdf, 6, 8 );
 747     cvGetCols( A, &_dpdc, 8, 10 );
 748
 749     if( have_dk )
 750     {
 751         cvGetRows( cam_params, &_k, 10, cam_params->height );
 752         cvGetCols( A, &_dpdk, 10, A->width );
 753     }
 754     cvProjectPoints2( &_Mi, &_ri, &_ti, &_A, have_dk ? &_k : NULL, _mp, &_dpdr, &_dpdt,
 755         &_dpdf, &_dpdc, have_dk ? &_dpdk : NULL, 0);
 756
 757     cvReleaseMat( &_mp );
 758
 759     //compute jacobian for point params
 760     //compute dMeasure/dPoint3D
 761
 762     // x = (r11 * X + r12 * Y + r13 * Z + t1)
 763     // y = (r21 * X + r22 * Y + r23 * Z + t2)
 764     // z = (r31 * X + r32 * Y + r33 * Z + t3)
 765
 766     // x' = x/z
 767     // y' = y/z
 768
 769     //d(x') = ( dx*z - x*dz)/(z*z)
 770     //d(y') = ( dy*z - y*dz)/(z*z)
 771
 772     //g = 1 + k1*r_2 + k2*r_4 + k3*r_6
 773     //r_2 = x'*x' + y'*y'
 774
 775     //d(r_2) = 2*x'*dx' + 2*y'*dy'
 776
 777     //dg = k1* d(r_2) + k2*2*r_2*d(r_2) + k3*3*r_2*r_2*d(r_2)
 778
 779     //x" = x'*g + 2*p1*x'*y' + p2(r_2+2*x'_2)
 780     //y" = y'*g + p1(r_2+2*y'_2) + 2*p2*x'*y'
 781
 782     //d(x") = d(x') * g + x' * d(g) + 2*p1*( d(x')*y' + x'*dy) + p2*(d(r_2) + 2*2*x'* dx')
 783     //d(y") = d(y') * g + y' * d(g) + 2*p2*( d(x')*y' + x'*dy) + p1*(d(r_2) + 2*2*y'* dy')
 784
 785     // u = fx*( x") + cx
 786     // v = fy*( y") + cy
 787
 788     // du = fx * d(x")  = fx * ( dx*z - x*dz)/ (z*z)
 789     // dv = fy * d(y")  = fy * ( dy*z - y*dz)/ (z*z)
 790
 791     // dx/dX = r11,  dx/dY = r12, dx/dZ = r13
 792     // dy/dX = r21,  dy/dY = r22, dy/dZ = r23
 793     // dz/dX = r31,  dz/dY = r32, dz/dZ = r33
 794
 795     // du/dX = fx*(r11*z-x*r31)/(z*z)
 796     // du/dY = fx*(r12*z-x*r32)/(z*z)
 797     // du/dZ = fx*(r13*z-x*r33)/(z*z)
 798
 799     // dv/dX = fy*(r21*z-y*r31)/(z*z)
 800     // dv/dY = fy*(r22*z-y*r32)/(z*z)
 801     // dv/dZ = fy*(r23*z-y*r33)/(z*z)
 802
 803     //get rotation matrix
 804     double R[9], t[3], fx = intr_data[0], fy = intr_data[4];
 805     CvMat _R = cvMat( 3, 3, CV_64F, R );
 806     cvRodrigues2(&_ri, &_R);
 807
 808     double X,Y,Z;
 809     X = point_params->data.db[0];
 810     Y = point_params->data.db[1];
 811     Z = point_params->data.db[2];
 812
 813     t[0] = _ti.data.db[0];
 814     t[1] = _ti.data.db[1];
 815     t[2] = _ti.data.db[2];
 816
 817     //compute x,y,z
 818     double x = R[0] * X + R[1] * Y + R[2] * Z + t[0];
 819     double y = R[3] * X + R[4] * Y + R[5] * Z + t[1];
 820     double z = R[6] * X + R[7] * Y + R[8] * Z + t[2];
 821
 822 #if 1
 823     //compute x',y'
 824     double x_strike = x/z;
 825     double y_strike = y/z;
 826     //compute dx',dy'  matrix
 827     //
 828     //    dx'/dX  dx'/dY dx'/dZ    =
 829     //    dy'/dX  dy'/dY dy'/dZ
 830
 831     double coeff[6] = { z, 0, -x,
 832                         0, z, -y };
 833     CvMat coeffmat = cvMat( 2, 3, CV_64F, coeff );
 834
 835     CvMat* dstrike_dbig = cvCreateMat(2,3,CV_64F);
 836     cvMatMul(&coeffmat, &_R, dstrike_dbig);
 837     cvScale(dstrike_dbig, dstrike_dbig, 1/(z*z) );
 838
 839     if( have_dk )
 840     {
 841         double strike_[2] = {x_strike, y_strike};
 842         CvMat strike = cvMat(1, 2, CV_64F, strike_);
 843
 844         //compute r_2
 845         double r_2 = x_strike*x_strike + y_strike*y_strike;
 846         double r_4 = r_2*r_2;
 847         double r_6 = r_4*r_2;
 848
 849         //compute d(r_2)/dbig
 850         CvMat* dr2_dbig = cvCreateMat(1,3,CV_64F);
 851         cvMatMul( &strike, dstrike_dbig, dr2_dbig);
 852         cvScale( dr2_dbig, dr2_dbig, 2 );
 853
 854         double& k1 = _k.data.db[0];
 855         double& k2 = _k.data.db[1];
 856         double& p1 = _k.data.db[2];
 857         double& p2 = _k.data.db[3];
 858         double k3 = 0;
 859
 860         if( _k.cols*_k.rows == 5 )
 861         {
 862             k3 = _k.data.db[4];
 863         }
 864         //compute dg/dbig
 865         double dg_dr2 = k1 + k2*2*r_2 + k3*3*r_4;
 866         double g = 1+k1*r_2+k2*r_4+k3*r_6;
 867
 868         CvMat* dg_dbig = cvCreateMat(1,3,CV_64F);
 869         cvScale( dr2_dbig, dg_dbig, dg_dr2 );
 870
 871         CvMat* tmp = cvCreateMat( 2, 3, CV_64F );
 872         CvMat* dstrike2_dbig = cvCreateMat( 2, 3, CV_64F );
 873
 874         double c[4] = { g+2*p1*y_strike+4*p2*x_strike,       2*p1*x_strike,
 875                         2*p2*y_strike,                 g+2*p2*x_strike + 4*p1*y_strike };
 876
 877         CvMat coeffmat = cvMat(2,2,CV_64F, c );
 878
 879         cvMatMul(&coeffmat, dstrike_dbig, dstrike2_dbig );
 880
 881         cvGEMM( &strike, dg_dbig, 1, NULL, 0, tmp, CV_GEMM_A_T );
 882         cvAdd( dstrike2_dbig, tmp, dstrike2_dbig );
 883
 884         double p[2] = { p2, p1 };
 885         CvMat pmat = cvMat(2, 1, CV_64F, p );
 886
 887         cvMatMul( &pmat, dr2_dbig ,tmp);
 888         cvAdd( dstrike2_dbig, tmp, dstrike2_dbig );
 889
 890         cvCopy( dstrike2_dbig, B );
 891
 892         cvReleaseMat(&dr2_dbig);
 893         cvReleaseMat(&dg_dbig);
 894
 895         cvReleaseMat(&tmp);
 896         cvReleaseMat(&dstrike2_dbig);
 897         cvReleaseMat(&tmp);
 898     }
 899     else
 900     {
 901         cvCopy(dstrike_dbig, B);
 902     }
 903     //multiply by fx, fy
 904     CvMat row;
 905     cvGetRows( B, &row, 0, 1 );
 906     cvScale( &row, &row, fx );
 907
 908     cvGetRows( B, &row, 1, 2 );
 909     cvScale( &row, &row, fy );
 910
 911 #else
 912
 913     double k = fx/(z*z);
 914
 915     cvmSet( B, 0, 0, k*(R[0]*z-x*R[6]));
 916     cvmSet( B, 0, 1, k*(R[1]*z-x*R[7]));
 917     cvmSet( B, 0, 2, k*(R[2]*z-x*R[8]));
 918
 919     k = fy/(z*z);
 920
 921     cvmSet( B, 1, 0, k*(R[3]*z-y*R[6]));
 922     cvmSet( B, 1, 1, k*(R[4]*z-y*R[7]));
 923     cvmSet( B, 1, 2, k*(R[5]*z-y*R[8]));
 924
 925 #endif
 926
 927 };
 928 void func(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* estim, void* /*data*/)
 929 {
 930     //just do projections
 931     CvMat _Mi;
 932     cvReshape( point_params, &_Mi, 3, 1 );
 933
 934     CvMat* _mp = cvCreateMat(1, 2, CV_64F ); //projection of the point
 935
 936     //split camera params into different matrices
 937     CvMat _ri, _ti, _k;
 938
 939     cvGetRows( cam_params, &_ri, 0, 3 );
 940     cvGetRows( cam_params, &_ti, 3, 6 );
 941
 942     double intr_data[9] = {0, 0, 0, 0, 0, 0, 0, 0, 1};
 943     intr_data[0] = cam_params->data.db[6];
 944     intr_data[4] = cam_params->data.db[7];
 945     intr_data[2] = cam_params->data.db[8];
 946     intr_data[5] = cam_params->data.db[9];
 947
 948     CvMat _A = cvMat(3,3, CV_64F, intr_data );
 949
 950     //int cn = CV_MAT_CN(_Mi.type);
 951
 952     bool have_dk = cam_params->height - 10 ? true : false;
 953
 954     if( have_dk )
 955     {
 956         cvGetRows( cam_params, &_k, 10, cam_params->height );
 957     }
 958     cvProjectPoints2( &_Mi, &_ri, &_ti, &_A, have_dk ? &_k : NULL, _mp, NULL, NULL,
 959                                                               NULL, NULL, NULL, 0);
 960     cvTranspose( _mp, estim );
 961     cvReleaseMat( &_mp );
 962 };
 963
 964 void fjac_new(int i, int j, Mat& point_params, Mat& cam_params, Mat& A, Mat& B, void* data)
 965 {
 966     CvMat _point_params = point_params, _cam_params = cam_params, _A = A, _B = B;
 967     fjac(i,j, &_point_params, &_cam_params, &_A, &_B, data);
 968 };
 969
 970 void func_new(int i, int j, Mat& point_params, Mat& cam_params, Mat& estim, void* data)
 971 {
 972     CvMat _point_params = point_params, _cam_params = cam_params, _estim = estim;
 973     func(i,j,&_point_params,&_cam_params,&_estim,data);
 974 };
 975
 976 void LevMarqSparse::bundleAdjust( vector<Point3d>& points, //positions of points in global coordinate system (input and output)
 977                   const vector<vector<Point2d> >& imagePoints, //projections of 3d points for every camera
 978                   const vector<vector<int> >& visibility, //visibility of 3d points for every camera
 979                   vector<Mat>& cameraMatrix, //intrinsic matrices of all cameras (input and output)
 980                   vector<Mat>& R, //rotation matrices of all cameras (input and output)
 981                   vector<Mat>& T, //translation vector of all cameras (input and output)
 982                   vector<Mat>& distCoeffs, //distortion coefficients of all cameras (input and output)
 983                   const TermCriteria& criteria)
 984                   //,enum{MOTION_AND_STRUCTURE,MOTION,STRUCTURE})
 985 {
 986     int num_points = points.size();
 987     int num_cameras = cameraMatrix.size();
 988
 989     CV_Assert( imagePoints.size() == (size_t)num_cameras &&
 990                visibility.size() == (size_t)num_cameras &&
 991                R.size() == (size_t)num_cameras &&
 992                T.size() == (size_t)num_cameras &&
 993                (distCoeffs.size() == (size_t)num_cameras || distCoeffs.size() == 0) );
 994
 995     int numdist = distCoeffs.size() ? (distCoeffs[0].rows * distCoeffs[0].cols) : 0;
 996
 997     int num_cam_param = 3 /* rotation vector */ + 3 /* translation vector */
 998                         + 2 /* fx, fy */ + 2 /* cx, cy */ + numdist;
 999
1000     int num_point_param = 3;
1001
1002     //collect camera parameters into vector
1003     Mat params( num_cameras * num_cam_param + num_points * num_point_param, 1, CV_64F );
1004
1005     //fill camera params
1006     for( int i = 0; i < num_cameras; i++ )
1007     {
1008         //rotation
1009         Mat rot_vec; Rodrigues( R[i], rot_vec );
1010         Mat dst = params.rowRange(i*num_cam_param, i*num_cam_param+3);
1011         rot_vec.copyTo(dst);
1012
1013         //translation
1014         dst = params.rowRange(i*num_cam_param + 3, i*num_cam_param+6);
1015         T[i].copyTo(dst);
1016
1017         //intrinsic camera matrix
1018         double* intr_data = (double*)cameraMatrix[i].data;
1019         double* intr = (double*)(params.data + params.step * (i*num_cam_param+6));
1020         //focals
1021         intr[0] = intr_data[0];  //fx
1022         intr[1] = intr_data[4];  //fy
1023         //center of projection
1024         intr[2] = intr_data[2];  //cx
1025         intr[3] = intr_data[5];  //cy
1026
1027         //add distortion if exists
1028         if( distCoeffs.size() )
1029         {
1030             dst = params.rowRange(i*num_cam_param + 10, i*num_cam_param+10+numdist);
1031             distCoeffs[i].copyTo(dst);
1032         }
1033     }
1034
1035     //fill point params
1036     Mat ptparams(num_points, 1, CV_64FC3, params.data + num_cameras*num_cam_param*params.step);
1037     Mat _points(points);
1038     CV_Assert(_points.size() == ptparams.size() && _points.type() == ptparams.type());
1039     _points.copyTo(ptparams);
1040
1041     //convert visibility vectors to visibility matrix
1042     Mat vismat(num_points, num_cameras, CV_32S);
1043     for( int i = 0; i < num_cameras; i++ )
1044     {
1045         //get row
1046         Mat col = vismat.col(i);
1047         Mat((int)visibility[i].size(), 1, vismat.type(), (void*)&visibility[i][0]).copyTo( col );
1048     }
1049
1050     int num_proj = countNonZero(vismat); //total number of points projections
1051
1052     //collect measurements
1053     Mat X(num_proj*2,1,CV_64F); //measurement vector
1054
1055     int counter = 0;
1056     for(int i = 0; i < num_points; i++ )
1057     {
1058         for(int j = 0; j < num_cameras; j++ )
1059         {
1060             //check visibility
1061             if( visibility[j][i] )
1062             {
1063                 //extract point and put tu vector
1064                 Point2d p = imagePoints[j][i];
1065                 ((double*)(X.data))[counter] = p.x;
1066                 ((double*)(X.data))[counter+1] = p.y;
1067                 counter+=2;
1068             }
1069         }
1070     }
1071
1072     LevMarqSparse levmar( num_points, num_cameras, num_point_param, num_cam_param, 2, vismat, params, X,
1073                           TermCriteria(criteria), fjac_new, func_new, NULL );
1074     //extract results
1075     //fill point params
1076     Mat final_points(num_points, 1, CV_64FC3,
1077         levmar.P->data.db + num_cameras*num_cam_param *levmar.P->step);
1078     CV_Assert(_points.size() == final_points.size() && _points.type() == final_points.type());
1079     final_points.copyTo(_points);
1080
1081     //fill camera params
1082     for( int i = 0; i < num_cameras; i++ )
1083     {
1084         //rotation
1085         Mat rot_vec = Mat(levmar.P).rowRange(i*num_cam_param, i*num_cam_param+3);
1086         Rodrigues( rot_vec, R[i] );
1087         //translation
1088         T[i] = Mat(levmar.P).rowRange(i*num_cam_param + 3, i*num_cam_param+6);
1089
1090         //intrinsic camera matrix
1091         double* intr_data = (double*)cameraMatrix[i].data;
1092         double* intr = (double*)(Mat(levmar.P).data + Mat(levmar.P).step * (i*num_cam_param+6));
1093         //focals
1094         intr_data[0] = intr[0];  //fx
1095         intr_data[4] = intr[1];  //fy
1096         //center of projection
1097         intr_data[2] = intr[2];  //cx
1098         intr_data[5] = intr[3];  //cy
1099
1100         //add distortion if exists
1101         if( distCoeffs.size() )
1102         {
1103             params.rowRange(i*num_cam_param + 10, i*num_cam_param+10+numdist).copyTo(distCoeffs[i]);
1104         }
1105     }
1106 }
1107
1108 }// end of namespace cv