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,