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