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