small updates to bundle adjustment implementation

modules/contrib/include/opencv2/contrib/contrib.hpp

@@ -431,8 +431,10 @@ namespace cv
             DEFAULT_NUM_DISTANCE_BUCKETS = 7 };
     };
     
-    class CV_EXPORTS LevMarqSparse
-    {
+
+  typedef bool (*BundleAdjustCallback)(int iteration, double norm_error, void* user_data);
+
+  class LevMarqSparse {
   public:
     LevMarqSparse();
     LevMarqSparse(int npoints, // number of points
@@ -453,8 +455,10 @@ namespace cv
 		  // callback for estimation of backprojection errors
 		  void (CV_CDECL * func)(int i, int j, Mat& point_params,
 					 Mat& cam_params, Mat& estim, void* data),
-                      void* data // user-specific data passed to the callbacks
+		  void* data, // user-specific data passed to the callbacks
+		  BundleAdjustCallback cb, void* user_data
 		  );
+
     virtual ~LevMarqSparse();
     
     virtual void run( int npoints, // number of points
@@ -480,24 +484,25 @@ namespace cv
 
     virtual void clear();
     
-        // useful function to do simple bundle adjastment tasks
-        static void bundleAdjust(vector<Point3d>& points, //positions of points in global coordinate system (input and output)
-                                 const vector<vector<Point2d> >& imagePoints, //projections of 3d points for every camera
-                                 const vector<vector<int> >& visibility, //visibility of 3d points for every camera
-                                 vector<Mat>& cameraMatrix, //intrinsic matrices of all cameras (input and output)
-                                 vector<Mat>& R, //rotation matrices of all cameras (input and output)
-                                 vector<Mat>& T, //translation vector of all cameras (input and output)
-                                 vector<Mat>& distCoeffs, //distortion coefficients of all cameras (input and output)
+    // useful function to do simple bundle adjustment tasks
+    static void bundleAdjust(vector<Point3d>& points, // positions of points in global coordinate system (input and output)
+			     const vector<vector<Point2d> >& imagePoints, // projections of 3d points for every camera
+			     const vector<vector<int> >& visibility, // visibility of 3d points for every camera 
+			     vector<Mat>& cameraMatrix, // intrinsic matrices of all cameras (input and output)
+			     vector<Mat>& R, // rotation matrices of all cameras (input and output)
+			     vector<Mat>& T, // translation vector of all cameras (input and output)
+			     vector<Mat>& distCoeffs, // distortion coefficients of all cameras (input and output)
 			     const TermCriteria& criteria=
-                                 TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, DBL_EPSILON));
+			     TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, DBL_EPSILON),
+			     BundleAdjustCallback cb = 0, void* user_data = 0);
     
-    protected:
-        virtual void optimize(); //main function that runs minimization
+  public:
+    virtual void optimize(CvMat &_vis); //main function that runs minimization
     
     //iteratively asks for measurement for visible camera-point pairs
-        void ask_for_proj();
+    void ask_for_proj(CvMat &_vis,bool once=false);
     //iteratively asks for Jacobians for every camera_point pair
-        void ask_for_projac();
+    void ask_for_projac(CvMat &_vis);
         
     CvMat* err; //error X-hX
     double prevErrNorm, errNorm;
@@ -509,9 +514,9 @@ namespace cv
     CvMat** V; //size of array is equal to number of points
     CvMat** inv_V_star; //inverse of V*
 
-        CvMat* A;
-        CvMat* B;
-        CvMat* W;
+    CvMat** A;
+    CvMat** B;
+    CvMat** W;
 
     CvMat* X; //measurement 
     CvMat* hX; //current measurement extimation given new parameter vector 
@@ -543,11 +548,13 @@ namespace cv
 
     //target function and jacobian pointers, which needs to be initialized 
     void (*fjac)(int i, int j, Mat& point_params, Mat& cam_params, Mat& A, Mat& B, void* data);
-        void (*func)(int i, int j, Mat& point_params, Mat& cam_params, Mat& estim, void* data );
+    void (*func)(int i, int j, Mat& point_params, Mat& cam_params, Mat& estim, void* data);
 
     void* data;
-    };
 
+    BundleAdjustCallback cb;
+    void* user_data;
+  };   
     
     CV_EXPORTS int chamerMatching( Mat& img, Mat& templ,
                                    vector<vector<Point> >& results, vector<float>& cost,

modules/contrib/src/ba.cpp

@@ -41,19 +41,19 @@
 
 #include "precomp.hpp"
 #include "opencv2/calib3d/calib3d.hpp"
+#include <iostream>
 
-namespace cv {  
+using namespace cv;
 
-LevMarqSparse::LevMarqSparse()
-{
-    A = B = W = Vis_index = X = prevP = P = deltaP = err = JtJ_diag = S = hX = NULL;
+LevMarqSparse::LevMarqSparse() {
+  Vis_index = X = prevP = P = deltaP = err = JtJ_diag = S = hX = NULL;
   U = ea = V = inv_V_star = eb = Yj = NULL;
-    num_points = 0;
-    num_cams = 0;
+  num_cams = 0,   num_points = 0,   num_err_param = 0;
+  num_cam_param = 0,  num_point_param = 0;
+  A = B = W = NULL;
 }
 
-LevMarqSparse::~LevMarqSparse()
-{
+LevMarqSparse::~LevMarqSparse() {
   clear();
 } 
 
@@ -75,69 +75,68 @@ LevMarqSparse::LevMarqSparse(int npoints, // number of points
 			     // callback for estimation of backprojection errors
 			     void (CV_CDECL * func)(int i, int j, Mat& point_params,
 						    Mat& cam_params, Mat& estim, void* data),
-        void* data // user-specific data passed to the callbacks
-        )
-{
-    A = B = W = Vis_index = X = prevP = P = deltaP = err = JtJ_diag = S = hX = NULL;
+			     void* data, // user-specific data passed to the callbacks
+			     BundleAdjustCallback _cb, void* _user_data
+			     ) {
+  Vis_index = X = prevP = P = deltaP = err = JtJ_diag = S = hX = NULL;
   U = ea = V = inv_V_star = eb = Yj = NULL;
+  A = B = W = NULL;
+
+  cb = _cb;
+  user_data = _user_data;
     
   run(npoints, ncameras, nPointParams, nCameraParams, nErrParams, visibility,
       P0, X_, criteria, fjac, func, data);
 }
 
-void LevMarqSparse::clear()
-{
-     for( int i = 0; i < num_points; i++ )
-     {
-         for(int j = 0; j < num_cams; j++ )
-         {  
-                 CvMat* tmp = ((CvMat**)(A->data.ptr + i * A->step))[j];
-                 if( tmp )
+void LevMarqSparse::clear() {
+  for( int i = 0; i < num_points; i++ ) {
+    for(int j = 0; j < num_cams; j++ ) {
+      //CvMat* tmp = ((CvMat**)(A->data.ptr + i * A->step))[j];
+      CvMat* tmp = A[j+i*num_cams];
+      if (tmp)
 	cvReleaseMat( &tmp );
 
-                 tmp = ((CvMat**)(B->data.ptr + i * B->step))[j];
-                 if( tmp )
+      //tmp = ((CvMat**)(B->data.ptr + i * B->step))[j];
+      tmp  = B[j+i*num_cams];
+      if (tmp)
 	cvReleaseMat( &tmp );
                  
-                 tmp = ((CvMat**)(W->data.ptr + j * W->step))[i];
-                 if( tmp )
+      //tmp = ((CvMat**)(W->data.ptr + j * W->step))[i];
+      tmp  = W[j+i*num_cams];
+      if (tmp)
 	cvReleaseMat( &tmp ); 
     }
   }   
-     cvReleaseMat( &A );
-     cvReleaseMat( &B );
-     cvReleaseMat( &W );       
+  delete A; //cvReleaseMat(&A);
+  delete B;//cvReleaseMat(&B);
+  delete W;//cvReleaseMat(&W);
   cvReleaseMat( &Vis_index);
 
-     for( int j = 0; j < num_cams; j++ )
-     {
+  for( int j = 0; j < num_cams; j++ ) {
     cvReleaseMat( &U[j] );
   }
   delete U;
 
-     for( int j = 0; j < num_cams; j++ )
-     {
+  for( int j = 0; j < num_cams; j++ ) {
     cvReleaseMat( &ea[j] );
   }
   delete ea;
      
   //allocate V and inv_V_star
-     for( int i = 0; i < num_points; i++ )
-     {
+  for( int i = 0; i < num_points; i++ ) {
     cvReleaseMat(&V[i]);
     cvReleaseMat(&inv_V_star[i]);
   }
   delete V;
   delete inv_V_star;
 
-     for( int i = 0; i < num_points; i++ )
-     {
+  for( int i = 0; i < num_points; i++ ) {
     cvReleaseMat(&eb[i]);
   }
   delete eb;
 
-     for( int i = 0; i < num_points; i++ )
-     {
+  for( int i = 0; i < num_points; i++ ) {
     cvReleaseMat(&Yj[i]);
   }   
   delete Yj;
@@ -178,9 +177,8 @@ void LevMarqSparse::run( int num_points_, //number of points
 			 void (*fjac_)(int i, int j, Mat& point_params, Mat& cam_params, Mat& A, Mat& B, void* data),
 			 void (*func_)(int i, int j, Mat& point_params, Mat& cam_params, Mat& estim, void* data),
 			 void* data_
-             ) //termination criteria
-{
-    clear();
+			 ) { //termination criteria
+  //clear();
     
   func = func_; //assign evaluation function
   fjac = fjac_; //assign jacobian
@@ -211,80 +209,94 @@ void LevMarqSparse::run( int num_points_, //number of points
     
   //Allocate matrix A whose elements are nointers to Aij
   //if Aij is zero (point i is not visible in camera j) then A(i,j) contains NULL
-    A = cvCreateMat( num_points, num_cams, CV_32S /*pointer is stored here*/ );
-    B = cvCreateMat( num_points, num_cams, CV_32S /*pointer is stored here*/ );
-    W = cvCreateMat( num_cams, num_points, CV_32S /*pointer is stored here*/ );
+  //A = cvCreateMat( num_points, num_cams, CV_32S /*pointer is stored here*/ );
+  //B = cvCreateMat( num_points, num_cams, CV_32S /*pointer is stored here*/ );
+  //W = cvCreateMat( num_cams, num_points, CV_32S /*pointer is stored here*/ );
+
+  A = new CvMat* [num_points * num_cams];
+  B = new CvMat* [num_points * num_cams];
+  W = new CvMat* [num_cams * num_points];
   Vis_index = cvCreateMat( num_points, num_cams, CV_32S /*integer index is stored here*/ );
-    cvSetZero( A );
-    cvSetZero( B );
-    cvSetZero( W );
+  //cvSetZero( A );
+  //cvSetZero( B );
+  //cvSetZero( W );
   cvSet( Vis_index, cvScalar(-1) );
     
   //fill matrices A and B based on visibility
   CvMat _vis = visibility;
   int index = 0;
-    for( int i = 0; i < num_points; i++ )
-    {
-        for(int j = 0; j < num_cams; j++ )
-        {  
-            if( ((int*)(_vis.data.ptr+ i * _vis.step))[j] )
-            {
+  for (int i = 0; i < num_points; i++ ) {
+    for (int j = 0; j < num_cams; j++ ) {
+      if (((int*)(_vis.data.ptr+ i * _vis.step))[j] ) {
 	((int*)(Vis_index->data.ptr + i * Vis_index->step))[j] = index;
 	index += num_err_param;
     
 	//create matrices Aij, Bij
-                CvMat* tmp = cvCreateMat( Aij_height, Aij_width, CV_64F );
-                ((CvMat**)(A->data.ptr + i * A->step))[j] = tmp;
+	CvMat* tmp = cvCreateMat(Aij_height, Aij_width, CV_64F );
+	//((CvMat**)(A->data.ptr + i * A->step))[j] = tmp;
+	cvSet(tmp,cvScalar(1.0,1.0,1.0,1.0));
+	A[j+i*num_cams] = tmp;
 
 	tmp = cvCreateMat( Bij_height, Bij_width, CV_64F );
-                ((CvMat**)(B->data.ptr + i * B->step))[j] = tmp;
+	//((CvMat**)(B->data.ptr + i * B->step))[j] = tmp;
+	cvSet(tmp,cvScalar(1.0,1.0,1.0,1.0));
+	B[j+i*num_cams] = tmp;
     
 	tmp = cvCreateMat( Wij_height, Wij_width, CV_64F );
-                ((CvMat**)(W->data.ptr + j * W->step))[i] = tmp;  //note indices i and j swapped
+	//((CvMat**)(W->data.ptr + j * W->step))[i] = tmp;  //note indices i and j swapped
+	cvSet(tmp,cvScalar(1.0,1.0,1.0,1.0));
+	W[j+i*num_cams] = tmp;
+      } else{
+	A[j+i*num_cams] = NULL;
+	B[j+i*num_cams] = NULL;
+	W[j+i*num_cams] = NULL;
       }
     }                
   }
     
   //allocate U
   U = new CvMat* [num_cams];
-    for( int j = 0; j < num_cams; j++ )
-    {
+  for (int j = 0; j < num_cams; j++ ) {
     U[j] = cvCreateMat( U_size, U_size, CV_64F );
+    cvSetZero(U[j]);
+
   }
   //allocate ea
   ea = new CvMat* [num_cams];
-    for( int j = 0; j < num_cams; j++ )
-    {
+  for (int j = 0; j < num_cams; j++ ) {
     ea[j] = cvCreateMat( U_size, 1, CV_64F );
+    cvSetZero(ea[j]);
   }
     
   //allocate V and inv_V_star
   V = new CvMat* [num_points];
   inv_V_star = new CvMat* [num_points];
-    for( int i = 0; i < num_points; i++ )
-    {
+  for (int i = 0; i < num_points; i++ ) {
     V[i] = cvCreateMat( V_size, V_size, CV_64F );
     inv_V_star[i] = cvCreateMat( V_size, V_size, CV_64F );
+    cvSetZero(V[i]);
+    cvSetZero(inv_V_star[i]);
   }
     
   //allocate eb
   eb = new CvMat* [num_points];
-    for( int i = 0; i < num_points; i++ )
-    {
+  for (int i = 0; i < num_points; i++ ) {
     eb[i] = cvCreateMat( V_size, 1, CV_64F );
+    cvSetZero(eb[i]);
   }   
     
   //allocate Yj
   Yj = new CvMat* [num_points];
-    for( int i = 0; i < num_points; i++ )
-    {
+  for (int i = 0; i < num_points; i++ ) {
     Yj[i] = cvCreateMat( Wij_height, Wij_width, CV_64F );  //Yij has the same size as Wij
+    cvSetZero(Yj[i]);
   }        
     
   //allocate matrix S
   S = cvCreateMat( num_cams * num_cam_param, num_cams * num_cam_param, CV_64F);
-    
+  cvSetZero(S);
   JtJ_diag = cvCreateMat( num_cams * num_cam_param + num_points * num_point_param, 1, CV_64F );
+  cvSetZero(JtJ_diag);
     
   //set starting parameters
   CvMat _tmp_ = CvMat(P0); 
@@ -297,72 +309,90 @@ void LevMarqSparse::run( int num_points_, //number of points
   X = cvCloneMat( &_tmp_ );  
   //create vector for estimated measurements
   hX = cvCreateMat( X->rows, X->cols, CV_64F );
+  cvSetZero(hX);
   //create error vector
   err = cvCreateMat( X->rows, X->cols, CV_64F );
-                              
-    ask_for_proj();
-         
+  cvSetZero(err);
+  ask_for_proj(_vis);
   //compute initial error
-    cvSub(  X, hX, err );
+  cvSub(X, hX, err );
+
+  /*
+    assert(X->rows == hX->rows);
+    std::cerr<<"X size = "<<X->rows<<" "<<X->cols<<std::endl;
+    std::cerr<<"hX size = "<<hX->rows<<" "<<hX->cols<<std::endl;
+    for (int j=0;j<X->rows;j+=2) {
+    double Xj1 = *(double*)(X->data.ptr + j * X->step);
+    double hXj1 = *(double*)(hX->data.ptr + j * hX->step);
+    double err1 = *(double*)(err->data.ptr + j * err->step);
+    double Xj2 = *(double*)(X->data.ptr + (j+1) * X->step);
+    double hXj2 = *(double*)(hX->data.ptr + (j+1) * hX->step);
+    double err2 = *(double*)(err->data.ptr + (j+1) * err->step);
+    std::cerr<<"("<<Xj1<<","<<Xj2<<") -> ("<<hXj1<<","<<hXj2<<"). err = ("<<err1<<","<<err2<<")"<<std::endl;
+    }
+  */
 
   prevErrNorm = cvNorm( err, 0,  CV_L2 );
+  //    std::cerr<<"prevErrNorm = "<<prevErrNorm<<std::endl;
   iters = 0; 
   criteria = criteria_init;
     
-    optimize();
+  optimize(_vis);
+
+  ask_for_proj(_vis,true);
+  cvSub(X, hX, err );
+  errNorm = cvNorm( err, 0,  CV_L2 );
 }
 
-void LevMarqSparse::ask_for_proj()
-{
+void LevMarqSparse::ask_for_proj(CvMat &_vis,bool once) {
   //given parameter P, compute measurement hX
   int ind = 0;
-    for( int i = 0; i < num_points; i++ )
-    {
+  for (int i = 0; i < num_points; i++ ) {
     CvMat point_mat;
     cvGetSubRect( P, &point_mat, cvRect( 0, num_cams * num_cam_param + num_point_param * i, 1, num_point_param ));
-    
-        for( int j = 0; j < num_cams; j++ )
-        {
-            CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
-            if( Aij ) //visible
-            {
+    for (int j = 0; j < num_cams; j++ ) {
+      //CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
+      CvMat* Aij = A[j+i*num_cams];
+      if (Aij ) { //visible
 	CvMat cam_mat;
 	cvGetSubRect( P, &cam_mat, cvRect( 0, j * num_cam_param, 1, num_cam_param ));
 	CvMat measur_mat;
 	cvGetSubRect( hX, &measur_mat, cvRect( 0, ind * num_err_param, 1, num_err_param ));
 	Mat _point_mat(&point_mat), _cam_mat(&cam_mat), _measur_mat(&measur_mat);
-                func( i, j, _point_mat, _cam_mat, _measur_mat, data );
-                
+	func( i, j, _point_mat, _cam_mat, _measur_mat, data);
 	assert( ind*num_err_param == ((int*)(Vis_index->data.ptr + i * Vis_index->step))[j]);
-    
 	ind+=1;
-                
       }  
     } 
   }
 }
+
 //iteratively asks for Jacobians for every camera_point pair
-void LevMarqSparse::ask_for_projac()  //should be evaluated at point prevP 
-{
+void LevMarqSparse::ask_for_projac(CvMat &_vis) {  //should be evaluated at point prevP
   // compute jacobians Aij and Bij
-    for( int i = 0; i < A->height; i++ )
-    {
+  for (int i = 0; i < num_points; i++ ) {
     CvMat point_mat;
     cvGetSubRect( prevP, &point_mat, cvRect( 0, num_cams * num_cam_param + num_point_param * i, 1, num_point_param ));
 
+    //CvMat** A_line = (CvMat**)(A->data.ptr + A->step * i);
+    //CvMat** B_line = (CvMat**)(B->data.ptr + B->step * i);
+    for( int j = 0; j < num_cams; j++ ) {
+      //CvMat* Aij = A_line[j];
+      //if( Aij ) //Aij is not zero
+      CvMat* Aij = A[j+i*num_cams];
+      CvMat* Bij = B[j+i*num_cams];
+      if(Aij) {
+	//CvMat** A_line = (CvMat**)(A->data.ptr + A->step * i);
+	//CvMat** B_line = (CvMat**)(B->data.ptr + B->step * i);
 
-        CvMat** A_line = (CvMat**)(A->data.ptr + A->step * i);
-        CvMat** B_line = (CvMat**)(B->data.ptr + B->step * i);
+	//CvMat* Aij = A_line[j];
+	//CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
 
-        for( int j = 0; j < A->width; j++ )
-        {   
-            CvMat* Aij = A_line[j];
-            if( Aij ) //Aij is not zero
-            {
 	CvMat cam_mat;
 	cvGetSubRect( prevP, &cam_mat, cvRect( 0, j * num_cam_param, 1, num_cam_param ));
                 
-                CvMat* Bij = B_line[j];
+	//CvMat* Bij = B_line[j];
+	//CvMat* Bij = ((CvMat**)(B->data.ptr + B->step * i))[j];
 	Mat _point_mat(&point_mat), _cam_mat(&cam_mat), _Aij(Aij), _Bij(Bij);
 	(*fjac)(i, j, _point_mat, _cam_mat, _Aij, _Bij, data);
       }
@@ -370,58 +400,60 @@ void LevMarqSparse::ask_for_projac()  //should be evaluated at point prevP
   }
 }  
 
-void LevMarqSparse::optimize() //main function that runs minimization
-{   
+void LevMarqSparse::optimize(CvMat &_vis) { //main function that runs minimization
   bool done = false;
     
   CvMat* YWt = cvCreateMat( num_cam_param, num_cam_param, CV_64F ); //this matrix used to store Yij*Wik' 
   CvMat* E = cvCreateMat( S->height, 1 , CV_64F ); //this is right part of system with S       
+  cvSetZero(YWt);
+  cvSetZero(E);
 
-    while(!done)
-    { 
+  while(!done) {
     // compute jacobians Aij and Bij
-        ask_for_projac();
-        
+    ask_for_projac(_vis);
+    int invisible_count=0;
     //compute U_j  and  ea_j
-        for( int j = 0; j < num_cams; j++ )
-        {
+    for (int j = 0; j < num_cams; j++ ) {
       cvSetZero(U[j]); 
       cvSetZero(ea[j]);
       //summ by i (number of points)
-            for( int i = 0; i < num_points; i++ )
-            {
+      for (int i = 0; i < num_points; i++ ) {
 	//get Aij
-                CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];                
-                if( Aij )
-                {
+	//CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
+	CvMat* Aij = A[j+i*num_cams];
+	if (Aij ) {
 	  //Uj+= AijT*Aij
 	  cvGEMM( Aij, Aij, 1, U[j], 1, U[j], CV_GEMM_A_T );
-
 	  //ea_j += AijT * e_ij
 	  CvMat eij;
 
 	  int index = ((int*)(Vis_index->data.ptr + i * Vis_index->step))[j];
 
-                    cvGetSubRect( err, &eij, cvRect( 0, index, 1, Aij->height /*width of transposed Aij*/ ) );
+	  cvGetSubRect( err, &eij, cvRect( 0, index, 1, Aij->height  ) ); //width of transposed Aij
 	  cvGEMM( Aij, &eij, 1, ea[j], 1, ea[j], CV_GEMM_A_T );
 	}
+	else
+	  invisible_count++;
       }
     } //U_j and ea_j computed for all j
 
+    //    if (!(iters%100))
+    int nviz = X->rows / num_err_param;
+    double e2 = prevErrNorm*prevErrNorm, e2n = e2 / nviz;
+    std::cerr<<"Iteration: "<<iters<<", normError: "<<e2<<" ("<<e2n<<")"<<std::endl;
+    if (cb)
+      cb(iters, prevErrNorm, user_data);
     //compute V_i  and  eb_i
-        for( int i = 0; i < num_points; i++ )
-        {
+    for (int i = 0; i < num_points; i++ ) {
       cvSetZero(V[i]); 
       cvSetZero(eb[i]);
             
       //summ by i (number of points)
-            for( int j = 0; j < num_cams; j++ )
-            {
+      for( int j = 0; j < num_cams; j++ ) {
 	//get Bij
-                CvMat* Bij = ((CvMat**)(B->data.ptr + B->step * i))[j];
-                
-                if( Bij )
-                {
+	//CvMat* Bij = ((CvMat**)(B->data.ptr + B->step * i))[j];
+	CvMat* Bij = B[j+i*num_cams];
+	if (Bij ) {
 	  //Vi+= BijT*Bij
 	  cvGEMM( Bij, Bij, 1, V[i], 1, V[i], CV_GEMM_A_T );
 
@@ -429,22 +461,22 @@ void LevMarqSparse::optimize() //main function that runs minimization
 	  int index = ((int*)(Vis_index->data.ptr + i * Vis_index->step))[j];
 
 	  CvMat eij;
-                    cvGetSubRect( err, &eij, cvRect( 0, index, 1, Bij->height /*width of transposed Bij*/ ) );
+	  cvGetSubRect( err, &eij, cvRect( 0, index, 1, Bij->height  ) ); //width of transposed Bij
 	  cvGEMM( Bij, &eij, 1, eb[i], 1, eb[i], CV_GEMM_A_T );
 	}
       }
     } //V_i and eb_i computed for all i
 
       //compute W_ij
-        for( int i = 0; i < num_points; i++ )
-        {
-            for( int j = 0; j < num_cams; j++ )
-            {
-                 CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
-                 if( Aij ) //visible
-                 {
-                     CvMat* Bij = ((CvMat**)(B->data.ptr + B->step * i))[j];
-                     CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
+    for( int i = 0; i < num_points; i++ ) {
+      for( int j = 0; j < num_cams; j++ ) {
+	//CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
+	CvMat* Aij = A[j+i*num_cams];
+	if( Aij ) { //visible
+	  //CvMat* Bij = ((CvMat**)(B->data.ptr + B->step * i))[j];
+	  CvMat* Bij = B[j+i*num_cams];
+	  //CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
+	  CvMat* Wij = W[j+i*num_cams];
 
 	  //multiply
 	  cvGEMM( Aij, Bij, 1, NULL, 0, Wij, CV_GEMM_A_T );                     
@@ -456,15 +488,13 @@ void LevMarqSparse::optimize() //main function that runs minimization
     {               
       CvMat dia;
       CvMat subr;
-            for( int j = 0; j < num_cams; j++ )
-            {                          
+      for( int j = 0; j < num_cams; j++ ) {
 	cvGetDiag(U[j], &dia);
 	cvGetSubRect(JtJ_diag, &subr, 
 		     cvRect(0, j*num_cam_param, 1, num_cam_param ));
 	cvCopy( &dia, &subr );
       } 
-            for( int i = 0; i < num_points; i++ )
-            {
+      for( int i = 0; i < num_points; i++ ) {
 	cvGetDiag(V[i], &dia);
 	cvGetSubRect(JtJ_diag, &subr, 
 		     cvRect(0, num_cams*num_cam_param + i * num_point_param, 1, num_point_param ));
@@ -472,29 +502,26 @@ void LevMarqSparse::optimize() //main function that runs minimization
       }   
     } 
 
-        if( iters == 0 )
-        {
+    if( iters == 0 ) {
       //initialize lambda. It is set to 1e-3 * average diagonal element in JtJ
       double average_diag = 0;
-            for( int j = 0; j < num_cams; j++ )
-            {
+      for( int j = 0; j < num_cams; j++ ) {
 	average_diag += cvTrace( U[j] ).val[0];
       }
-            for( int i = 0; i < num_points; i++ )
-            {
+      for( int i = 0; i < num_points; i++ ) {
 	average_diag += cvTrace( V[i] ).val[0];
       }
       average_diag /= (num_cams*num_cam_param + num_points * num_point_param );
                         
+      //      lambda = 1e-3 * average_diag;        
       lambda = 1e-3 * average_diag;        
+      lambda = 0.245560;
     }
        
     //now we are going to find good step and make it
-        for(;;)
-        {
+    for(;;) {
       //augmentation of diagonal
-            for(int j = 0; j < num_cams; j++ )
-            {
+      for(int j = 0; j < num_cams; j++ ) {
 	CvMat diag;
 	cvGetDiag( U[j], &diag );
 #if 1
@@ -503,8 +530,7 @@ void LevMarqSparse::optimize() //main function that runs minimization
 	cvScale( &diag, &diag, 1 + lambda );
 #endif
       }
-            for(int i = 0; i < num_points; i++ )
-            {
+      for(int i = 0; i < num_points; i++ ) {
 	CvMat diag;
 	cvGetDiag( V[i], &diag );
 #if 1
@@ -516,48 +542,43 @@ void LevMarqSparse::optimize() //main function that runs minimization
       bool error = false;
       //compute inv(V*)
       bool inverted_ok = true;
-            for(int i = 0; i < num_points; i++ )
-            {
+      for(int i = 0; i < num_points; i++ ) {
 	double det = cvInvert( V[i], inv_V_star[i] );
 
-                if( fabs(det) <= FLT_EPSILON ) 
-                {                       
+	if( fabs(det) <= FLT_EPSILON )  {
 	  inverted_ok = false;
+	  std::cerr<<"V["<<i<<"] failed"<<std::endl;
 	  break;
 	} //means we did wrong augmentation, try to choose different lambda
       }
 
-            if( inverted_ok )
-            {
+      if( inverted_ok ) {
 	cvSetZero( E ); 
 	//loop through cameras, compute upper diagonal blocks of matrix S 
-                for( int j = 0; j < num_cams; j++ )
-                {  
+	for( int j = 0; j < num_cams; j++ ) {
 	  //compute Yij = Wij (V*_i)^-1  for all i   (if Wij exists/nonzero)
-                    for( int i = 0; i < num_points; i++ )
-                    {   
+	  for( int i = 0; i < num_points; i++ ) {
 	    //
-                        CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
-                        if( Wij )
-                        {
+	    //CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
+	    CvMat* Wij = W[j+i*num_cams];
+	    if( Wij ) {
 	      cvMatMul( Wij, inv_V_star[i], Yj[i] );
 	    }
 	  }
 
 	  //compute Sjk   for k>=j  (because Sjk = Skj)
-                    for( int k = j; k < num_cams; k++ )
-                    {
+	  for( int k = j; k < num_cams; k++ ) {
 	    cvSetZero( YWt );
-                        for( int i = 0; i < num_points; i++ )
-                        {
+	    for( int i = 0; i < num_points; i++ ) {
 	      //check that both Wij and Wik exist
-                            CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
-                            CvMat* Wik = ((CvMat**)(W->data.ptr + W->step * k))[i];
+	      // CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
+	      CvMat* Wij = W[j+i*num_cams];
+	      //CvMat* Wik = ((CvMat**)(W->data.ptr + W->step * k))[i];
+	      CvMat* Wik = W[k+i*num_cams];
 
-                            if( Wij && Wik )
-                            {
+	      if( Wij && Wik ) {
 		//multiply YWt += Yj[i]*Wik'
-                                cvGEMM( Yj[i], Wik, 1, YWt, 1, YWt, CV_GEMM_B_T /*transpose Wik*/ ); 
+		cvGEMM( Yj[i], Wik, 1, YWt, 1, YWt, CV_GEMM_B_T  ); ///*transpose Wik
 	      }
 	    }
 
@@ -569,13 +590,10 @@ void LevMarqSparse::optimize() //main function that runs minimization
                         
 
 	    //if j==k, add diagonal
-                        if( j != k )
-                        {
+	    if( j != k ) {
 	      //just copy with minus
 	      cvScale( YWt, &Sjk, -1 ); //if we set initial S to zero then we can use cvSub( Sjk, YWt, Sjk);
-                        }
-                        else
-                        {
+	    } else {
 	      //add diagonal value
 
 	      //subtract YWt from augmented Uj
@@ -591,9 +609,9 @@ void LevMarqSparse::optimize() //main function that runs minimization
 	    //select submat
 	    cvGetSubRect( E, &e_j, cvRect( 0, j * num_cam_param, 1, num_cam_param ) ); 
                     
-                    for( int i = 0; i < num_points; i++ )
-                    {
-                        CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
+	    for( int i = 0; i < num_points; i++ ) {
+	      //CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
+	      CvMat* Wij = W[j+i*num_cams];
 	      if( Wij )
 		cvMatMulAdd( Yj[i], eb[i], &e_j, &e_j );
 	    }
@@ -603,33 +621,29 @@ void LevMarqSparse::optimize() //main function that runs minimization
 
 	} 
 	//fill below diagonal elements of matrix S
-                cvCompleteSymm( S,  0 /*from upper to low*/ ); //operation may be done by nonzero blocks or during upper diagonal computation 
+	cvCompleteSymm( S,  0  ); ///*from upper to low //operation may be done by nonzero blocks or during upper diagonal computation
                 
 	//Solve linear system  S * deltaP_a = E
 	CvMat dpa;
 	cvGetSubRect( deltaP, &dpa, cvRect(0, 0, 1, S->width ) );
-                int res = cvSolve( S, E, &dpa );
+	int res = cvSolve( S, E, &dpa, CV_CHOLESKY );
             
-                if( res ) //system solved ok
-                {   
+	if( res ) { //system solved ok
 	  //compute db_i
-                    for( int i = 0; i < num_points; i++ )
-                    {
+	  for( int i = 0; i < num_points; i++ ) {
 	    CvMat dbi;
 	    cvGetSubRect( deltaP, &dbi, cvRect( 0, dpa.height + i * num_point_param, 1, num_point_param ) );   
 
-                        /* compute \sum_j W_ij^T da_j */
-                        for( int j = 0; j < num_cams; j++ )
-                        {
+	    // compute \sum_j W_ij^T da_j
+	    for( int j = 0; j < num_cams; j++ ) {
 	      //get Wij
-                            CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
-
-                            if( Wij )
-                            {
+	      //CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
+	      CvMat* Wij = W[j+i*num_cams];
+	      if( Wij ) {
 		//get da_j
 		CvMat daj;
 		cvGetSubRect( &dpa, &daj, cvRect( 0, j * num_cam_param, 1, num_cam_param ));  
-                                cvGEMM( Wij, &daj, 1, &dbi, 1, &dbi, CV_GEMM_A_T /* transpose Wij */ ); 
+		cvGEMM( Wij, &daj, 1, &dbi, 1, &dbi, CV_GEMM_A_T  ); ///* transpose Wij
 	      }  
 	    }
 	    //finalize dbi
@@ -641,53 +655,49 @@ void LevMarqSparse::optimize() //main function that runs minimization
 	  cvAdd( prevP, deltaP, P );
                                         
 	  //evaluate  function with new parameters
-                    ask_for_proj(); // func( P, hX );
+	  ask_for_proj(_vis); // func( P, hX );
 
 	  //compute error
 	  errNorm = cvNorm( X, hX, CV_L2 );
                                         
-                }
-                else
-                {
+	} else {
 	  error = true;
 	}                
-            }
-            else
-            {
+      } else {
 	error = true;
       }
       //check solution
-            if( error || /* singularities somewhere */ 
-                errNorm > prevErrNorm )  //step was not accepted
-            {
+      if( error || ///* singularities somewhere
+	  errNorm > prevErrNorm )  { //step was not accepted
 	//increase lambda and reject change 
 	lambda *= 10;
+	int nviz = X->rows / num_err_param;
+	double e2 = errNorm*errNorm, e2_prev = prevErrNorm*prevErrNorm;
+	double e2n = e2/nviz, e2n_prev = e2_prev/nviz;
+	std::cerr<<"move failed: lambda = "<<lambda<<", e2 = "<<e2<<" ("<<e2n<<") > "<<e2_prev<<" ("<<e2n_prev<<")"<<std::endl;
 
 	//restore diagonal from backup
 	{               
 	  CvMat dia;
 	  CvMat subr;
-                    for( int j = 0; j < num_cams; j++ )
-                    {                          
+	  for( int j = 0; j < num_cams; j++ ) {
 	    cvGetDiag(U[j], &dia);
 	    cvGetSubRect(JtJ_diag, &subr, 
 			 cvRect(0, j*num_cam_param, 1, num_cam_param ));
 	    cvCopy( &subr, &dia );
 	  } 
-                    for( int i = 0; i < num_points; i++ )
-                    {
+	  for( int i = 0; i < num_points; i++ ) {
 	    cvGetDiag(V[i], &dia);
 	    cvGetSubRect(JtJ_diag, &subr, 
 			 cvRect(0, num_cams*num_cam_param + i * num_point_param, 1, num_point_param ));
 	    cvCopy( &subr, &dia );
 	  }   
 	}                  
-            }
-            else  //all is ok
-            {
+      } else {  //all is ok
 	//accept change and decrease lambda
 	lambda /= 10;
 	lambda = MAX(lambda, 1e-16);
+	std::cerr<<"decreasing lambda to "<<lambda<<std::endl;
 	prevErrNorm = errNorm;
 
 	//compute new projection error vector
@@ -700,13 +710,11 @@ void LevMarqSparse::optimize() //main function that runs minimization
     double param_change_norm = cvNorm(P, prevP, CV_RELATIVE_L2);
     //check termination criteria
     if( (criteria.type&CV_TERMCRIT_ITER && iters > criteria.max_iter ) || 
-            (criteria.type&CV_TERMCRIT_EPS && param_change_norm < criteria.epsilon) )
-        {
+	(criteria.type&CV_TERMCRIT_EPS && param_change_norm < criteria.epsilon) ) {
+      //      std::cerr<<"relative norm change "<<param_change_norm<<" lower than eps "<<criteria.epsilon<<", stopping"<<std::endl;
       done = true;
       break;
-        }  
-        else
-        {
+    } else {
       //copy new params and continue iterations
       cvCopy( P, prevP );
     }
@@ -717,15 +725,14 @@ void LevMarqSparse::optimize() //main function that runs minimization
 
 //Utilities
 
-void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A, CvMat* B, void* /*data*/) 
-{
+void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A, CvMat* B, void* /*data*/) {
   //compute jacobian per camera parameters (i.e. Aij)
   //take i-th point 3D current coordinates
     
   CvMat _Mi;
   cvReshape(point_params, &_Mi, 3, 1 );
 
-    CvMat* _mp = cvCreateMat(1, 2, CV_64F ); //projection of the point
+  CvMat* _mp = cvCreateMat(1, 1, CV_64FC2 ); //projection of the point
 
   //split camera params into different matrices
   CvMat _ri, _ti, _k;
@@ -738,7 +745,7 @@ void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A
   intr_data[2] = cam_params->data.db[8];
   intr_data[5] = cam_params->data.db[9];
 
-    CvMat matA = cvMat(3,3, CV_64F, intr_data ); 
+  CvMat _A = cvMat(3,3, CV_64F, intr_data ); 
 
   CvMat _dpdr, _dpdt, _dpdf, _dpdc, _dpdk;
     
@@ -749,12 +756,11 @@ void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A
   cvGetCols( A, &_dpdf, 6, 8 );
   cvGetCols( A, &_dpdc, 8, 10 );
     
-    if( have_dk )
-    {
+  if( have_dk ) {
     cvGetRows( cam_params, &_k, 10, cam_params->height );
     cvGetCols( A, &_dpdk, 10, A->width );
   }
-    cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, have_dk ? &_k : NULL, _mp, &_dpdr, &_dpdt,
+  cvProjectPoints2(&_Mi, &_ri, &_ti, &_A, have_dk ? &_k : NULL, _mp, &_dpdr, &_dpdt,
 		   &_dpdf, &_dpdc, have_dk ? &_dpdk : NULL, 0);   
 
   cvReleaseMat( &_mp );                                 
@@ -805,8 +811,8 @@ void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A
 
   //get rotation matrix
   double R[9], t[3], fx = intr_data[0], fy = intr_data[4];
-    CvMat matR = cvMat( 3, 3, CV_64F, R );
-    cvRodrigues2(&_ri, &matR);
+  CvMat _R = cvMat( 3, 3, CV_64F, R );
+  cvRodrigues2(&_ri, &_R);
 
   double X,Y,Z;
   X = point_params->data.db[0];
@@ -836,11 +842,10 @@ void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A
   CvMat coeffmat = cvMat( 2, 3, CV_64F, coeff );
 
   CvMat* dstrike_dbig = cvCreateMat(2,3,CV_64F);
-    cvMatMul(&coeffmat, &matR, dstrike_dbig);
+  cvMatMul(&coeffmat, &_R, dstrike_dbig);
   cvScale(dstrike_dbig, dstrike_dbig, 1/(z*z) );      
     
-    if( have_dk )
-    {
+  if( have_dk ) {
     double strike_[2] = {x_strike, y_strike};
     CvMat strike = cvMat(1, 2, CV_64F, strike_);       
         
@@ -860,8 +865,7 @@ void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A
     double& p2 = _k.data.db[3];          
     double k3 = 0;
 
-        if( _k.cols*_k.rows == 5 )
-        {   
+    if( _k.cols*_k.rows == 5 ) {
       k3 = _k.data.db[4];
     }    
     //compute dg/dbig
@@ -898,9 +902,7 @@ void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A
     cvReleaseMat(&tmp);
     cvReleaseMat(&dstrike2_dbig);
     cvReleaseMat(&tmp);  
-    } 
-    else
-    {
+  } else {
     cvCopy(dstrike_dbig, B);
   }
   //multiply by fx, fy
@@ -928,13 +930,13 @@ void fjac(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* A
 #endif
     
 };
-void func(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* estim, void* /*data*/) 
-{
+void func(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* estim, void* /*data*/) {
   //just do projections
   CvMat _Mi;
   cvReshape( point_params, &_Mi, 3, 1 );
 
-    CvMat* _mp = cvCreateMat(1, 2, CV_64F ); //projection of the point
+  CvMat* _mp = cvCreateMat(1, 1, CV_64FC2 ); //projection of the point
+  CvMat* _mp2 = cvCreateMat(1, 2, CV_64F ); //projection of the point
 
   //split camera params into different matrices
   CvMat _ri, _ti, _k;
@@ -948,30 +950,32 @@ void func(int /*i*/, int /*j*/, CvMat *point_params, CvMat* cam_params, CvMat* e
   intr_data[2] = cam_params->data.db[8];
   intr_data[5] = cam_params->data.db[9];
 
-    CvMat matA = cvMat(3,3, CV_64F, intr_data ); 
+  CvMat _A = cvMat(3,3, CV_64F, intr_data ); 
 
   //int cn = CV_MAT_CN(_Mi.type);
 
   bool have_dk = cam_params->height - 10 ? true : false;
            
-    if( have_dk )
-    {
+  if( have_dk ) {
     cvGetRows( cam_params, &_k, 10, cam_params->height );        
   }  
-    cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, have_dk ? &_k : NULL, _mp, NULL, NULL,
+  cvProjectPoints2( &_Mi, &_ri, &_ti, &_A, have_dk ? &_k : NULL, _mp, NULL, NULL,
 		    NULL, NULL, NULL, 0);   
-    cvTranspose( _mp, estim );
+  //    std::cerr<<"_mp = "<<_mp->data.db[0]<<","<<_mp->data.db[1]<<std::endl;
+  //    
+  _mp2->data.db[0] = _mp->data.db[0];
+  _mp2->data.db[1] = _mp->data.db[1];
+  cvTranspose( _mp2, estim );
   cvReleaseMat( &_mp );
+  cvReleaseMat( &_mp2 );
 };
 
-void fjac_new(int i, int j, Mat& point_params, Mat& cam_params, Mat& A, Mat& B, void* data)
-{
-    CvMat _point_params = point_params, _cam_params = cam_params, matA = A, matB = B;
-    fjac(i,j, &_point_params, &_cam_params, &matA, &matB, data);
+void fjac_new(int i, int j, Mat& point_params, Mat& cam_params, Mat& A, Mat& B, void* data) {
+  CvMat _point_params = point_params, _cam_params = cam_params, _A = A, _B = B;
+  fjac(i,j, &_point_params, &_cam_params, &_A, &_B, data);
 };
 
-void func_new(int i, int j, Mat& point_params, Mat& cam_params, Mat& estim, void* data) 
-{
+void func_new(int i, int j, Mat& point_params, Mat& cam_params, Mat& estim, void* data)  {
   CvMat _point_params = point_params, _cam_params = cam_params, _estim = estim;
   func(i,j,&_point_params,&_cam_params,&_estim,data);
 };                                                 
@@ -983,11 +987,11 @@ void LevMarqSparse::bundleAdjust( vector<Point3d>& points, //positions of points
 				  vector<Mat>& R, //rotation matrices of all cameras (input and output)
 				  vector<Mat>& T, //translation vector of all cameras (input and output)
 				  vector<Mat>& distCoeffs, //distortion coefficients of all cameras (input and output)
-                  const TermCriteria& criteria)
+				  const TermCriteria& criteria,
+				  BundleAdjustCallback cb, void* user_data) {
   //,enum{MOTION_AND_STRUCTURE,MOTION,STRUCTURE})
-{     
-    int num_points = (int)points.size();
-    int num_cameras = (int)cameraMatrix.size();
+  int num_points = points.size();
+  int num_cameras = cameraMatrix.size();
 
   CV_Assert( imagePoints.size() == (size_t)num_cameras && 
 	     visibility.size() == (size_t)num_cameras && 
@@ -1006,8 +1010,7 @@ void LevMarqSparse::bundleAdjust( vector<Point3d>& points, //positions of points
   Mat params( num_cameras * num_cam_param + num_points * num_point_param, 1, CV_64F );
 
   //fill camera params
-    for( int i = 0; i < num_cameras; i++ )
-    {   
+  for( int i = 0; i < num_cameras; i++ ) {
     //rotation
     Mat rot_vec; Rodrigues( R[i], rot_vec );
     Mat dst = params.rowRange(i*num_cam_param, i*num_cam_param+3);
@@ -1028,8 +1031,7 @@ void LevMarqSparse::bundleAdjust( vector<Point3d>& points, //positions of points
     intr[3] = intr_data[5];  //cy  
 
     //add distortion if exists
-        if( distCoeffs.size() )
-        {
+    if( distCoeffs.size() ) {
       dst = params.rowRange(i*num_cam_param + 10, i*num_cam_param+10+numdist);
       distCoeffs[i].copyTo(dst); 
     }
@@ -1043,8 +1045,7 @@ void LevMarqSparse::bundleAdjust( vector<Point3d>& points, //positions of points
 
   //convert visibility vectors to visibility matrix
   Mat vismat(num_points, num_cameras, CV_32S);
-    for( int i = 0; i < num_cameras; i++ )
-    {
+  for( int i = 0; i < num_cameras; i++ ) {
     //get row
     Mat col = vismat.col(i);
     Mat((int)visibility[i].size(), 1, vismat.type(), (void*)&visibility[i][0]).copyTo( col );
@@ -1056,34 +1057,41 @@ void LevMarqSparse::bundleAdjust( vector<Point3d>& points, //positions of points
   Mat X(num_proj*2,1,CV_64F); //measurement vector      
     
   int counter = 0;
-    for(int i = 0; i < num_points; i++ )
-    {
-        for(int j = 0; j < num_cameras; j++ )
-        {
+  for(int i = 0; i < num_points; i++ ) {
+    for(int j = 0; j < num_cameras; j++ ) {
       //check visibility
-            if( visibility[j][i] )
-            {
+      if( visibility[j][i] ) {
 	//extract point and put tu vector
 	Point2d p = imagePoints[j][i];
 	((double*)(X.data))[counter] = p.x;
 	((double*)(X.data))[counter+1] = p.y;
+	assert(p.x != -1 || p.y != -1);
 	counter+=2;
       }             
     }   
   }
 
   LevMarqSparse levmar( num_points, num_cameras, num_point_param, num_cam_param, 2, vismat, params, X,
-                          TermCriteria(criteria), fjac_new, func_new, NULL );
+			TermCriteria(criteria), fjac_new, func_new, NULL,
+			cb, user_data);
   //extract results
   //fill point params
-    Mat final_points(num_points, 1, CV_64FC3,
+  /*Mat final_points(num_points, 1, CV_64FC3,
     levmar.P->data.db + num_cameras*num_cam_param *levmar.P->step);
     CV_Assert(_points.size() == final_points.size() && _points.type() == final_points.type());
-    final_points.copyTo(_points);
+    final_points.copyTo(_points);*/
 
+  points.clear();
+  for( int i = 0; i < num_points; i++ ) {
+    CvMat point_mat;
+    cvGetSubRect( levmar.P, &point_mat, cvRect( 0, levmar.num_cams * levmar.num_cam_param+ levmar.num_point_param * i, 1, levmar.num_point_param ));
+    CvScalar x = cvGet2D(&point_mat,0,0); CvScalar y = cvGet2D(&point_mat,1,0); CvScalar z = cvGet2D(&point_mat,2,0);
+    points.push_back(Point3f(x.val[0],y.val[0],z.val[0]));
+    //std::cerr<<"point"<<points[points.size()-1].x<<","<<points[points.size()-1].y<<","<<points[points.size()-1].z<<std::endl;
+  }
   //fill camera params
-    for( int i = 0; i < num_cameras; i++ )
-    {   
+  //R.clear();T.clear();cameraMatrix.clear();
+  for( int i = 0; i < num_cameras; i++ ) {
     //rotation
     Mat rot_vec = Mat(levmar.P).rowRange(i*num_cam_param, i*num_cam_param+3);
     Rodrigues( rot_vec, R[i] );
@@ -1101,11 +1109,8 @@ void LevMarqSparse::bundleAdjust( vector<Point3d>& points, //positions of points
     intr_data[5] = intr[3];  //cy  
 
     //add distortion if exists
-        if( distCoeffs.size() )
-        {
-            params.rowRange(i*num_cam_param + 10, i*num_cam_param+10+numdist).copyTo(distCoeffs[i]);
+    if( distCoeffs.size() ) {
+      Mat(levmar.P).rowRange(i*num_cam_param + 10, i*num_cam_param+10+numdist).copyTo(distCoeffs[i]);
     }
   } 
 }    
-
-}// end of namespace cv