diff --git a/modules/contrib/include/opencv2/contrib/contrib.hpp b/modules/contrib/include/opencv2/contrib/contrib.hpp index 4f56c3f09..901d96733 100644 --- a/modules/contrib/include/opencv2/contrib/contrib.hpp +++ b/modules/contrib/include/opencv2/contrib/contrib.hpp @@ -431,123 +431,130 @@ namespace cv DEFAULT_NUM_DISTANCE_BUCKETS = 7 }; }; - class CV_EXPORTS LevMarqSparse - { - public: - LevMarqSparse(); - LevMarqSparse(int npoints, // number of points - int ncameras, // number of cameras - int nPointParams, // number of params per one point (3 in case of 3D points) - int nCameraParams, // number of parameters per one camera - int nErrParams, // number of parameters in measurement vector - // for 1 point at one camera (2 in case of 2D projections) - Mat& visibility, // visibility matrix. rows correspond to points, columns correspond to cameras - // 1 - point is visible for the camera, 0 - invisible - Mat& P0, // starting vector of parameters, first cameras then points - Mat& X, // measurements, in order of visibility. non visible cases are skipped - TermCriteria criteria, // termination criteria - - // callback for estimation of Jacobian matrices - void (CV_CDECL * fjac)(int i, int j, Mat& point_params, - Mat& cam_params, Mat& A, Mat& B, void* data), - // 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 - ); - virtual ~LevMarqSparse(); - - virtual void run( int npoints, // number of points - int ncameras, // number of cameras - int nPointParams, // number of params per one point (3 in case of 3D points) - int nCameraParams, // number of parameters per one camera - int nErrParams, // number of parameters in measurement vector - // for 1 point at one camera (2 in case of 2D projections) - Mat& visibility, // visibility matrix. rows correspond to points, columns correspond to cameras - // 1 - point is visible for the camera, 0 - invisible - Mat& P0, // starting vector of parameters, first cameras then points - Mat& X, // measurements, in order of visibility. non visible cases are skipped - TermCriteria criteria, // termination criteria - - // callback for estimation of Jacobian matrices - void (CV_CDECL * fjac)(int i, int j, Mat& point_params, - Mat& cam_params, Mat& A, Mat& B, void* data), - // 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 - ); - - virtual void clear(); - - // useful function to do simple bundle adjastment tasks - static void bundleAdjust(vector& points, //positions of points in global coordinate system (input and output) - const vector >& imagePoints, //projections of 3d points for every camera - const vector >& visibility, //visibility of 3d points for every camera - vector& cameraMatrix, //intrinsic matrices of all cameras (input and output) - vector& R, //rotation matrices of all cameras (input and output) - vector& T, //translation vector of all cameras (input and output) - vector& distCoeffs, //distortion coefficients of all cameras (input and output) - const TermCriteria& criteria= - TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, DBL_EPSILON)); - - protected: - virtual void optimize(); //main function that runs minimization - - //iteratively asks for measurement for visible camera-point pairs - void ask_for_proj(); - //iteratively asks for Jacobians for every camera_point pair - void ask_for_projac(); - - CvMat* err; //error X-hX - double prevErrNorm, errNorm; - double lambda; - CvTermCriteria criteria; - int iters; - - CvMat** U; //size of array is equal to number of cameras - 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* X; //measurement - CvMat* hX; //current measurement extimation given new parameter vector - - CvMat* prevP; //current already accepted parameter. - CvMat* P; // parameters used to evaluate function with new params - // this parameters may be rejected - - CvMat* deltaP; //computed increase of parameters (result of normal system solution ) - - CvMat** ea; // sum_i AijT * e_ij , used as right part of normal equation - // length of array is j = number of cameras - CvMat** eb; // sum_j BijT * e_ij , used as right part of normal equation - // length of array is i = number of points - - CvMat** Yj; //length of array is i = num_points - - CvMat* S; //big matrix of block Sjk , each block has size num_cam_params x num_cam_params - - CvMat* JtJ_diag; //diagonal of JtJ, used to backup diagonal elements before augmentation - - CvMat* Vis_index; // matrix which element is index of measurement for point i and camera j - - int num_cams; - int num_points; - int num_err_param; - int num_cam_param; - int num_point_param; - - //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* data; - }; + + typedef bool (*BundleAdjustCallback)(int iteration, double norm_error, void* user_data); + + class LevMarqSparse { + public: + LevMarqSparse(); + LevMarqSparse(int npoints, // number of points + int ncameras, // number of cameras + int nPointParams, // number of params per one point (3 in case of 3D points) + int nCameraParams, // number of parameters per one camera + int nErrParams, // number of parameters in measurement vector + // for 1 point at one camera (2 in case of 2D projections) + Mat& visibility, // visibility matrix. rows correspond to points, columns correspond to cameras + // 1 - point is visible for the camera, 0 - invisible + Mat& P0, // starting vector of parameters, first cameras then points + Mat& X, // measurements, in order of visibility. non visible cases are skipped + TermCriteria criteria, // termination criteria + + // callback for estimation of Jacobian matrices + void (CV_CDECL * fjac)(int i, int j, Mat& point_params, + Mat& cam_params, Mat& A, Mat& B, void* data), + // 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 + BundleAdjustCallback cb, void* user_data + ); + + virtual ~LevMarqSparse(); + virtual void run( int npoints, // number of points + int ncameras, // number of cameras + int nPointParams, // number of params per one point (3 in case of 3D points) + int nCameraParams, // number of parameters per one camera + int nErrParams, // number of parameters in measurement vector + // for 1 point at one camera (2 in case of 2D projections) + Mat& visibility, // visibility matrix. rows correspond to points, columns correspond to cameras + // 1 - point is visible for the camera, 0 - invisible + Mat& P0, // starting vector of parameters, first cameras then points + Mat& X, // measurements, in order of visibility. non visible cases are skipped + TermCriteria criteria, // termination criteria + + // callback for estimation of Jacobian matrices + void (CV_CDECL * fjac)(int i, int j, Mat& point_params, + Mat& cam_params, Mat& A, Mat& B, void* data), + // 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 + ); + + virtual void clear(); + + // useful function to do simple bundle adjustment tasks + static void bundleAdjust(vector& points, // positions of points in global coordinate system (input and output) + const vector >& imagePoints, // projections of 3d points for every camera + const vector >& visibility, // visibility of 3d points for every camera + vector& cameraMatrix, // intrinsic matrices of all cameras (input and output) + vector& R, // rotation matrices of all cameras (input and output) + vector& T, // translation vector of all cameras (input and output) + vector& distCoeffs, // distortion coefficients of all cameras (input and output) + const TermCriteria& criteria= + TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, DBL_EPSILON), + BundleAdjustCallback cb = 0, void* user_data = 0); + + public: + virtual void optimize(CvMat &_vis); //main function that runs minimization + + //iteratively asks for measurement for visible camera-point pairs + void ask_for_proj(CvMat &_vis,bool once=false); + //iteratively asks for Jacobians for every camera_point pair + void ask_for_projac(CvMat &_vis); + + CvMat* err; //error X-hX + double prevErrNorm, errNorm; + double lambda; + CvTermCriteria criteria; + int iters; + + CvMat** U; //size of array is equal to number of cameras + 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* X; //measurement + CvMat* hX; //current measurement extimation given new parameter vector + + CvMat* prevP; //current already accepted parameter. + CvMat* P; // parameters used to evaluate function with new params + // this parameters may be rejected + + CvMat* deltaP; //computed increase of parameters (result of normal system solution ) + + CvMat** ea; // sum_i AijT * e_ij , used as right part of normal equation + // length of array is j = number of cameras + CvMat** eb; // sum_j BijT * e_ij , used as right part of normal equation + // length of array is i = number of points + + CvMat** Yj; //length of array is i = num_points + + CvMat* S; //big matrix of block Sjk , each block has size num_cam_params x num_cam_params + + CvMat* JtJ_diag; //diagonal of JtJ, used to backup diagonal elements before augmentation + + CvMat* Vis_index; // matrix which element is index of measurement for point i and camera j + + int num_cams; + int num_points; + int num_err_param; + int num_cam_param; + int num_point_param; + + //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* data; + + BundleAdjustCallback cb; + void* user_data; + }; CV_EXPORTS int chamerMatching( Mat& img, Mat& templ, vector >& results, vector& cost, diff --git a/modules/contrib/src/ba.cpp b/modules/contrib/src/ba.cpp index c8679aa17..81e1548ae 100644 --- a/modules/contrib/src/ba.cpp +++ b/modules/contrib/src/ba.cpp @@ -41,117 +41,116 @@ #include "precomp.hpp" #include "opencv2/calib3d/calib3d.hpp" +#include -namespace cv { +using namespace cv; -LevMarqSparse::LevMarqSparse() -{ - A = B = W = 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; +LevMarqSparse::LevMarqSparse() { + Vis_index = X = prevP = P = deltaP = err = JtJ_diag = S = hX = NULL; + U = ea = V = inv_V_star = eb = Yj = NULL; + num_cams = 0, num_points = 0, num_err_param = 0; + num_cam_param = 0, num_point_param = 0; + A = B = W = NULL; } -LevMarqSparse::~LevMarqSparse() -{ - clear(); +LevMarqSparse::~LevMarqSparse() { + clear(); } LevMarqSparse::LevMarqSparse(int npoints, // number of points - int ncameras, // number of cameras - int nPointParams, // number of params per one point (3 in case of 3D points) - int nCameraParams, // number of parameters per one camera - int nErrParams, // number of parameters in measurement vector - // for 1 point at one camera (2 in case of 2D projections) - Mat& visibility, // visibility matrix. rows correspond to points, columns correspond to cameras - // 1 - point is visible for the camera, 0 - invisible - Mat& P0, // starting vector of parameters, first cameras then points - Mat& X_, // measurements, in order of visibility. non visible cases are skipped - TermCriteria criteria, // termination criteria + int ncameras, // number of cameras + int nPointParams, // number of params per one point (3 in case of 3D points) + int nCameraParams, // number of parameters per one camera + int nErrParams, // number of parameters in measurement vector + // for 1 point at one camera (2 in case of 2D projections) + Mat& visibility, // visibility matrix. rows correspond to points, columns correspond to cameras + // 1 - point is visible for the camera, 0 - invisible + Mat& P0, // starting vector of parameters, first cameras then points + Mat& X_, // measurements, in order of visibility. non visible cases are skipped + TermCriteria criteria, // termination criteria - // callback for estimation of Jacobian matrices - void (CV_CDECL * fjac)(int i, int j, Mat& point_params, - Mat& cam_params, Mat& A, Mat& B, void* data), - // 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; - U = ea = V = inv_V_star = eb = Yj = NULL; + // callback for estimation of Jacobian matrices + void (CV_CDECL * fjac)(int i, int j, Mat& point_params, + Mat& cam_params, Mat& A, Mat& B, void* data), + // 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 + 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); + 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 ) - cvReleaseMat( &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 ) - cvReleaseMat( &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 ) - cvReleaseMat( &tmp ); - } - } - cvReleaseMat( &A ); - cvReleaseMat( &B ); - cvReleaseMat( &W ); - cvReleaseMat( &Vis_index); + //tmp = ((CvMat**)(W->data.ptr + j * W->step))[i]; + tmp = W[j+i*num_cams]; + if (tmp) + cvReleaseMat( &tmp ); + } + } + delete A; //cvReleaseMat(&A); + delete B;//cvReleaseMat(&B); + delete W;//cvReleaseMat(&W); + cvReleaseMat( &Vis_index); - for( int j = 0; j < num_cams; j++ ) - { - cvReleaseMat( &U[j] ); - } - delete U; + for( int j = 0; j < num_cams; j++ ) { + cvReleaseMat( &U[j] ); + } + delete U; - for( int j = 0; j < num_cams; j++ ) - { - cvReleaseMat( &ea[j] ); - } - delete ea; + 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++ ) - { - cvReleaseMat(&V[i]); - cvReleaseMat(&inv_V_star[i]); - } - delete V; - delete inv_V_star; + //allocate V and inv_V_star + 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++ ) - { - cvReleaseMat(&eb[i]); - } - delete eb; + for( int i = 0; i < num_points; i++ ) { + cvReleaseMat(&eb[i]); + } + delete eb; - for( int i = 0; i < num_points; i++ ) - { - cvReleaseMat(&Yj[i]); - } - delete Yj; + for( int i = 0; i < num_points; i++ ) { + cvReleaseMat(&Yj[i]); + } + delete Yj; - cvReleaseMat(&X); - cvReleaseMat(&prevP); - cvReleaseMat(&P); - cvReleaseMat(&deltaP); + cvReleaseMat(&X); + cvReleaseMat(&prevP); + cvReleaseMat(&P); + cvReleaseMat(&deltaP); - cvReleaseMat(&err); + cvReleaseMat(&err); - cvReleaseMat(&JtJ_diag); - cvReleaseMat(&S); - cvReleaseMat(&hX); + cvReleaseMat(&JtJ_diag); + cvReleaseMat(&S); + cvReleaseMat(&hX); } //A params correspond to Cameras @@ -166,946 +165,952 @@ void LevMarqSparse::clear() //num_errors - number of measurements. void LevMarqSparse::run( int num_points_, //number of points - int num_cams_, //number of cameras - int num_point_param_, //number of params per one point (3 in case of 3D points) - int num_cam_param_, //number of parameters per one camera - int num_err_param_, //number of parameters in measurement vector for 1 point at one camera (2 in case of 2D projections) - Mat& visibility, //visibility matrix . rows correspond to points, columns correspond to cameras - // 0 - point is visible for the camera, 0 - invisible - Mat& P0, //starting vector of parameters, first cameras then points - Mat& X_init, //measurements, in order of visibility. non visible cases are skipped - TermCriteria criteria_init, - 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(); + int num_cams_, //number of cameras + int num_point_param_, //number of params per one point (3 in case of 3D points) + int num_cam_param_, //number of parameters per one camera + int num_err_param_, //number of parameters in measurement vector for 1 point at one camera (2 in case of 2D projections) + Mat& visibility, //visibility matrix . rows correspond to points, columns correspond to cameras + // 0 - point is visible for the camera, 0 - invisible + Mat& P0, //starting vector of parameters, first cameras then points + Mat& X_init, //measurements, in order of visibility. non visible cases are skipped + TermCriteria criteria_init, + 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(); - func = func_; //assign evaluation function - fjac = fjac_; //assign jacobian - data = data_; + func = func_; //assign evaluation function + fjac = fjac_; //assign jacobian + data = data_; - num_cams = num_cams_; - num_points = num_points_; - num_err_param = num_err_param_; - num_cam_param = num_cam_param_; - num_point_param = num_point_param_; + num_cams = num_cams_; + num_points = num_points_; + num_err_param = num_err_param_; + num_cam_param = num_cam_param_; + num_point_param = num_point_param_; - //compute all sizes - int Aij_width = num_cam_param; - int Aij_height = num_err_param; + //compute all sizes + int Aij_width = num_cam_param; + int Aij_height = num_err_param; - int Bij_width = num_point_param; - int Bij_height = num_err_param; + int Bij_width = num_point_param; + int Bij_height = num_err_param; - int U_size = Aij_width; - int V_size = Bij_width; + int U_size = Aij_width; + int V_size = Bij_width; - int Wij_height = Aij_width; - int Wij_width = Bij_width; + int Wij_height = Aij_width; + int Wij_width = Bij_width; - //allocate memory for all Aij, Bij, U, V, W + //allocate memory for all Aij, Bij, U, V, W - //allocate num_points*num_cams matrices A + //allocate num_points*num_cams matrices A - //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*/ ); - Vis_index = cvCreateMat( num_points, num_cams, CV_32S /*integer index is stored here*/ ); - cvSetZero( A ); - cvSetZero( B ); - cvSetZero( W ); - cvSet( Vis_index, cvScalar(-1) ); + //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 = 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 ); + 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] ) - { - ((int*)(Vis_index->data.ptr + i * Vis_index->step))[j] = index; - index += num_err_param; + //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] ) { + ((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; + //create matrices Aij, Bij + 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; + cvSet(tmp,cvScalar(1.0,1.0,1.0,1.0)); + B[j+i*num_cams] = tmp; - tmp = cvCreateMat( Bij_height, Bij_width, CV_64F ); - ((CvMat**)(B->data.ptr + i * B->step))[j] = tmp; + tmp = cvCreateMat( Wij_height, Wij_width, CV_64F ); + //((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; + } + } + } - tmp = cvCreateMat( Wij_height, Wij_width, CV_64F ); - ((CvMat**)(W->data.ptr + j * W->step))[i] = tmp; //note indices i and j swapped - } - } + //allocate U + U = new CvMat* [num_cams]; + 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++ ) { + 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++ ) { + 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++ ) { + 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++ ) { + 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); + prevP = cvCloneMat( &_tmp_ ); + P = cvCloneMat( &_tmp_ ); + deltaP = cvCloneMat( &_tmp_ ); + + //set measurements + _tmp_ = CvMat(X_init); + 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 ); + cvSetZero(err); + ask_for_proj(_vis); + //compute initial error + cvSub(X, hX, err ); + + /* + assert(X->rows == hX->rows); + std::cerr<<"X size = "<rows<<" "<cols<cols<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<<"("< ("<data.ptr + B->step * i))[j]; - - if( Bij ) - { - //Vi+= BijT*Bij - cvGEMM( Bij, Bij, 1, V[i], 1, V[i], CV_GEMM_A_T ); + //summ by i (number of points) + for( int j = 0; j < num_cams; j++ ) { + //get 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 ); - //eb_i += BijT * e_ij - int index = ((int*)(Vis_index->data.ptr + i * Vis_index->step))[j]; + //eb_i += BijT * e_ij + 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*/ ) ); - cvGEMM( Bij, &eij, 1, eb[i], 1, eb[i], CV_GEMM_A_T ); - } - } - } //V_i and eb_i computed for all i + CvMat eij; + 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]; + //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]; + 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 ); - } - } - } //Wij computed + //multiply + cvGEMM( Aij, Bij, 1, NULL, 0, Wij, CV_GEMM_A_T ); + } + } + } //Wij computed - //backup diagonal of JtJ before we start augmenting it - { - CvMat dia; - CvMat subr; - 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++ ) - { - cvGetDiag(V[i], &dia); - cvGetSubRect(JtJ_diag, &subr, - cvRect(0, num_cams*num_cam_param + i * num_point_param, 1, num_point_param )); - cvCopy( &dia, &subr ); - } - } + //backup diagonal of JtJ before we start augmenting it + { + CvMat dia; + CvMat subr; + 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++ ) { + cvGetDiag(V[i], &dia); + cvGetSubRect(JtJ_diag, &subr, + cvRect(0, num_cams*num_cam_param + i * num_point_param, 1, num_point_param )); + cvCopy( &dia, &subr ); + } + } - 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++ ) - { - average_diag += cvTrace( U[j] ).val[0]; - } - 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 ); + 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++ ) { + average_diag += cvTrace( U[j] ).val[0]; + } + 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 = 1e-3 * average_diag; + lambda = 0.245560; + } - //now we are going to find good step and make it - for(;;) - { - //augmentation of diagonal - for(int j = 0; j < num_cams; j++ ) - { - CvMat diag; - cvGetDiag( U[j], &diag ); + //now we are going to find good step and make it + for(;;) { + //augmentation of diagonal + for(int j = 0; j < num_cams; j++ ) { + CvMat diag; + cvGetDiag( U[j], &diag ); #if 1 - cvAddS( &diag, cvScalar( lambda ), &diag ); + cvAddS( &diag, cvScalar( lambda ), &diag ); #else - cvScale( &diag, &diag, 1 + lambda ); + cvScale( &diag, &diag, 1 + lambda ); #endif - } - for(int i = 0; i < num_points; i++ ) - { - CvMat diag; - cvGetDiag( V[i], &diag ); + } + for(int i = 0; i < num_points; i++ ) { + CvMat diag; + cvGetDiag( V[i], &diag ); #if 1 - cvAddS( &diag, cvScalar( lambda ), &diag ); + cvAddS( &diag, cvScalar( lambda ), &diag ); #else - cvScale( &diag, &diag, 1 + lambda ); + cvScale( &diag, &diag, 1 + lambda ); #endif - } - bool error = false; - //compute inv(V*) - bool inverted_ok = true; - for(int i = 0; i < num_points; i++ ) - { - double det = cvInvert( V[i], inv_V_star[i] ); + } + bool error = false; + //compute inv(V*) + bool inverted_ok = true; + for(int i = 0; i < num_points; i++ ) { + double det = cvInvert( V[i], inv_V_star[i] ); - if( fabs(det) <= FLT_EPSILON ) - { - inverted_ok = false; - break; - } //means we did wrong augmentation, try to choose different lambda - } + if( fabs(det) <= FLT_EPSILON ) { + inverted_ok = false; + std::cerr<<"V["<data.ptr + W->step * j))[i]; - if( Wij ) - { - cvMatMul( Wij, inv_V_star[i], Yj[i] ); - } - } + if( inverted_ok ) { + cvSetZero( E ); + //loop through cameras, compute upper diagonal blocks of matrix S + 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++ ) { + // + //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++ ) - { - cvSetZero( YWt ); - 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]; + //compute Sjk for k>=j (because Sjk = Skj) + for( int k = j; k < num_cams; k++ ) { + cvSetZero( YWt ); + 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* 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 ) - { - //multiply YWt += Yj[i]*Wik' - cvGEMM( Yj[i], Wik, 1, YWt, 1, YWt, CV_GEMM_B_T /*transpose Wik*/ ); - } - } + if( Wij && Wik ) { + //multiply YWt += Yj[i]*Wik' + cvGEMM( Yj[i], Wik, 1, YWt, 1, YWt, CV_GEMM_B_T ); ///*transpose Wik + } + } - //copy result to matrix S + //copy result to matrix S - CvMat Sjk; - //extract submat - cvGetSubRect( S, &Sjk, cvRect( k * num_cam_param, j * num_cam_param, num_cam_param, num_cam_param )); + CvMat Sjk; + //extract submat + cvGetSubRect( S, &Sjk, cvRect( k * num_cam_param, j * num_cam_param, num_cam_param, num_cam_param )); - //if j==k, add diagonal - 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 - { - //add diagonal value + //if j==k, add diagonal + 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 { + //add diagonal value - //subtract YWt from augmented Uj - cvSub( U[j], YWt, &Sjk ); - } - } + //subtract YWt from augmented Uj + cvSub( U[j], YWt, &Sjk ); + } + } - //compute right part of equation involving matrix S - // e_j=ea_j - \sum_i Y_ij eb_i - { - CvMat e_j; + //compute right part of equation involving matrix S + // e_j=ea_j - \sum_i Y_ij eb_i + { + CvMat e_j; - //select submat - cvGetSubRect( E, &e_j, cvRect( 0, j * num_cam_param, 1, num_cam_param ) ); + //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]; - if( Wij ) - cvMatMulAdd( Yj[i], eb[i], &e_j, &e_j ); - } + 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 ); + } - cvSub( ea[j], &e_j, &e_j ); - } + cvSub( ea[j], &e_j, &e_j ); + } - } - //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 + } + //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 - //Solve linear system S * deltaP_a = E - CvMat dpa; - cvGetSubRect( deltaP, &dpa, cvRect(0, 0, 1, S->width ) ); - int res = cvSolve( S, E, &dpa ); + //Solve linear system S * deltaP_a = E + CvMat dpa; + cvGetSubRect( deltaP, &dpa, cvRect(0, 0, 1, S->width ) ); + int res = cvSolve( S, E, &dpa, CV_CHOLESKY ); - if( res ) //system solved ok - { - //compute db_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 ) ); + if( res ) { //system solved ok + //compute db_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++ ) - { - //get Wij - CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i]; + // 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]; + 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 + } + } + //finalize dbi + cvSub( eb[i], &dbi, &dbi ); + cvMatMul(inv_V_star[i], &dbi, &dbi ); //here we get final dbi + } //now we computed whole deltaP - 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 */ ); - } - } - //finalize dbi - cvSub( eb[i], &dbi, &dbi ); - cvMatMul(inv_V_star[i], &dbi, &dbi ); //here we get final dbi - } //now we computed whole deltaP - - //add deltaP to delta - cvAdd( prevP, deltaP, P ); + //add deltaP to delta + cvAdd( prevP, deltaP, P ); - //evaluate function with new parameters - ask_for_proj(); // func( P, hX ); + //evaluate function with new parameters + ask_for_proj(_vis); // func( P, hX ); - //compute error - errNorm = cvNorm( X, hX, CV_L2 ); + //compute error + errNorm = cvNorm( X, hX, CV_L2 ); - } - else - { - error = true; - } - } - else - { - error = true; - } - //check solution - if( error || /* singularities somewhere */ - errNorm > prevErrNorm ) //step was not accepted - { - //increase lambda and reject change - lambda *= 10; + } else { + error = true; + } + } else { + error = true; + } + //check solution + 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 = "< "< criteria.max_iter ) || - (criteria.type&CV_TERMCRIT_EPS && param_change_norm < criteria.epsilon) ) - { - done = true; - break; - } - else - { - //copy new params and continue iterations - cvCopy( P, prevP ); - } - } - cvReleaseMat(&YWt); - cvReleaseMat(&E); + 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) ) { + // std::cerr<<"relative norm change "<data.db[6]; - intr_data[4] = cam_params->data.db[7]; - intr_data[2] = cam_params->data.db[8]; - intr_data[5] = cam_params->data.db[9]; + double intr_data[9] = {0, 0, 0, 0, 0, 0, 0, 0, 1}; + intr_data[0] = cam_params->data.db[6]; + intr_data[4] = cam_params->data.db[7]; + 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; + CvMat _dpdr, _dpdt, _dpdf, _dpdc, _dpdk; - bool have_dk = cam_params->height - 10 ? true : false; + bool have_dk = cam_params->height - 10 ? true : false; - cvGetCols( A, &_dpdr, 0, 3 ); - cvGetCols( A, &_dpdt, 3, 6 ); - cvGetCols( A, &_dpdf, 6, 8 ); - cvGetCols( A, &_dpdc, 8, 10 ); + cvGetCols( A, &_dpdr, 0, 3 ); + cvGetCols( A, &_dpdt, 3, 6 ); + cvGetCols( A, &_dpdf, 6, 8 ); + cvGetCols( A, &_dpdc, 8, 10 ); - 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, - &_dpdf, &_dpdc, have_dk ? &_dpdk : NULL, 0); + if( have_dk ) { + cvGetRows( cam_params, &_k, 10, cam_params->height ); + cvGetCols( A, &_dpdk, 10, A->width ); + } + cvProjectPoints2(&_Mi, &_ri, &_ti, &_A, have_dk ? &_k : NULL, _mp, &_dpdr, &_dpdt, + &_dpdf, &_dpdc, have_dk ? &_dpdk : NULL, 0); - cvReleaseMat( &_mp ); + cvReleaseMat( &_mp ); - //compute jacobian for point params - //compute dMeasure/dPoint3D + //compute jacobian for point params + //compute dMeasure/dPoint3D - // x = (r11 * X + r12 * Y + r13 * Z + t1) - // y = (r21 * X + r22 * Y + r23 * Z + t2) - // z = (r31 * X + r32 * Y + r33 * Z + t3) + // x = (r11 * X + r12 * Y + r13 * Z + t1) + // y = (r21 * X + r22 * Y + r23 * Z + t2) + // z = (r31 * X + r32 * Y + r33 * Z + t3) - // x' = x/z - // y' = y/z + // x' = x/z + // y' = y/z - //d(x') = ( dx*z - x*dz)/(z*z) - //d(y') = ( dy*z - y*dz)/(z*z) + //d(x') = ( dx*z - x*dz)/(z*z) + //d(y') = ( dy*z - y*dz)/(z*z) - //g = 1 + k1*r_2 + k2*r_4 + k3*r_6 - //r_2 = x'*x' + y'*y' + //g = 1 + k1*r_2 + k2*r_4 + k3*r_6 + //r_2 = x'*x' + y'*y' - //d(r_2) = 2*x'*dx' + 2*y'*dy' + //d(r_2) = 2*x'*dx' + 2*y'*dy' - //dg = k1* d(r_2) + k2*2*r_2*d(r_2) + k3*3*r_2*r_2*d(r_2) + //dg = k1* d(r_2) + k2*2*r_2*d(r_2) + k3*3*r_2*r_2*d(r_2) - //x" = x'*g + 2*p1*x'*y' + p2(r_2+2*x'_2) - //y" = y'*g + p1(r_2+2*y'_2) + 2*p2*x'*y' + //x" = x'*g + 2*p1*x'*y' + p2(r_2+2*x'_2) + //y" = y'*g + p1(r_2+2*y'_2) + 2*p2*x'*y' - //d(x") = d(x') * g + x' * d(g) + 2*p1*( d(x')*y' + x'*dy) + p2*(d(r_2) + 2*2*x'* dx') - //d(y") = d(y') * g + y' * d(g) + 2*p2*( d(x')*y' + x'*dy) + p1*(d(r_2) + 2*2*y'* dy') + //d(x") = d(x') * g + x' * d(g) + 2*p1*( d(x')*y' + x'*dy) + p2*(d(r_2) + 2*2*x'* dx') + //d(y") = d(y') * g + y' * d(g) + 2*p2*( d(x')*y' + x'*dy) + p1*(d(r_2) + 2*2*y'* dy') - // u = fx*( x") + cx - // v = fy*( y") + cy + // u = fx*( x") + cx + // v = fy*( y") + cy - // du = fx * d(x") = fx * ( dx*z - x*dz)/ (z*z) - // dv = fy * d(y") = fy * ( dy*z - y*dz)/ (z*z) + // du = fx * d(x") = fx * ( dx*z - x*dz)/ (z*z) + // dv = fy * d(y") = fy * ( dy*z - y*dz)/ (z*z) - // dx/dX = r11, dx/dY = r12, dx/dZ = r13 - // dy/dX = r21, dy/dY = r22, dy/dZ = r23 - // dz/dX = r31, dz/dY = r32, dz/dZ = r33 + // dx/dX = r11, dx/dY = r12, dx/dZ = r13 + // dy/dX = r21, dy/dY = r22, dy/dZ = r23 + // dz/dX = r31, dz/dY = r32, dz/dZ = r33 - // du/dX = fx*(r11*z-x*r31)/(z*z) - // du/dY = fx*(r12*z-x*r32)/(z*z) - // du/dZ = fx*(r13*z-x*r33)/(z*z) + // du/dX = fx*(r11*z-x*r31)/(z*z) + // du/dY = fx*(r12*z-x*r32)/(z*z) + // du/dZ = fx*(r13*z-x*r33)/(z*z) - // dv/dX = fy*(r21*z-y*r31)/(z*z) - // dv/dY = fy*(r22*z-y*r32)/(z*z) - // dv/dZ = fy*(r23*z-y*r33)/(z*z) + // dv/dX = fy*(r21*z-y*r31)/(z*z) + // dv/dY = fy*(r22*z-y*r32)/(z*z) + // dv/dZ = fy*(r23*z-y*r33)/(z*z) - //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); + //get rotation matrix + double R[9], t[3], fx = intr_data[0], fy = intr_data[4]; + CvMat _R = cvMat( 3, 3, CV_64F, R ); + cvRodrigues2(&_ri, &_R); - double X,Y,Z; - X = point_params->data.db[0]; - Y = point_params->data.db[1]; - Z = point_params->data.db[2]; + double X,Y,Z; + X = point_params->data.db[0]; + Y = point_params->data.db[1]; + Z = point_params->data.db[2]; - t[0] = _ti.data.db[0]; - t[1] = _ti.data.db[1]; - t[2] = _ti.data.db[2]; + t[0] = _ti.data.db[0]; + t[1] = _ti.data.db[1]; + t[2] = _ti.data.db[2]; - //compute x,y,z - double x = R[0] * X + R[1] * Y + R[2] * Z + t[0]; - double y = R[3] * X + R[4] * Y + R[5] * Z + t[1]; - double z = R[6] * X + R[7] * Y + R[8] * Z + t[2]; + //compute x,y,z + double x = R[0] * X + R[1] * Y + R[2] * Z + t[0]; + double y = R[3] * X + R[4] * Y + R[5] * Z + t[1]; + double z = R[6] * X + R[7] * Y + R[8] * Z + t[2]; #if 1 - //compute x',y' - double x_strike = x/z; - double y_strike = y/z; - //compute dx',dy' matrix - // - // dx'/dX dx'/dY dx'/dZ = - // dy'/dX dy'/dY dy'/dZ + //compute x',y' + double x_strike = x/z; + double y_strike = y/z; + //compute dx',dy' matrix + // + // dx'/dX dx'/dY dx'/dZ = + // dy'/dX dy'/dY dy'/dZ - double coeff[6] = { z, 0, -x, - 0, z, -y }; - CvMat coeffmat = cvMat( 2, 3, CV_64F, coeff ); + double coeff[6] = { z, 0, -x, + 0, z, -y }; + CvMat coeffmat = cvMat( 2, 3, CV_64F, coeff ); - CvMat* dstrike_dbig = cvCreateMat(2,3,CV_64F); - cvMatMul(&coeffmat, &matR, dstrike_dbig); - cvScale(dstrike_dbig, dstrike_dbig, 1/(z*z) ); + CvMat* dstrike_dbig = cvCreateMat(2,3,CV_64F); + cvMatMul(&coeffmat, &_R, dstrike_dbig); + cvScale(dstrike_dbig, dstrike_dbig, 1/(z*z) ); - if( have_dk ) - { - double strike_[2] = {x_strike, y_strike}; - CvMat strike = cvMat(1, 2, CV_64F, strike_); + if( have_dk ) { + double strike_[2] = {x_strike, y_strike}; + CvMat strike = cvMat(1, 2, CV_64F, strike_); - //compute r_2 - double r_2 = x_strike*x_strike + y_strike*y_strike; - double r_4 = r_2*r_2; - double r_6 = r_4*r_2; + //compute r_2 + double r_2 = x_strike*x_strike + y_strike*y_strike; + double r_4 = r_2*r_2; + double r_6 = r_4*r_2; - //compute d(r_2)/dbig - CvMat* dr2_dbig = cvCreateMat(1,3,CV_64F); - cvMatMul( &strike, dstrike_dbig, dr2_dbig); - cvScale( dr2_dbig, dr2_dbig, 2 ); + //compute d(r_2)/dbig + CvMat* dr2_dbig = cvCreateMat(1,3,CV_64F); + cvMatMul( &strike, dstrike_dbig, dr2_dbig); + cvScale( dr2_dbig, dr2_dbig, 2 ); - double& k1 = _k.data.db[0]; - double& k2 = _k.data.db[1]; - double& p1 = _k.data.db[2]; - double& p2 = _k.data.db[3]; - double k3 = 0; + double& k1 = _k.data.db[0]; + double& k2 = _k.data.db[1]; + double& p1 = _k.data.db[2]; + double& p2 = _k.data.db[3]; + double k3 = 0; - if( _k.cols*_k.rows == 5 ) - { - k3 = _k.data.db[4]; - } - //compute dg/dbig - double dg_dr2 = k1 + k2*2*r_2 + k3*3*r_4; - double g = 1+k1*r_2+k2*r_4+k3*r_6; + if( _k.cols*_k.rows == 5 ) { + k3 = _k.data.db[4]; + } + //compute dg/dbig + double dg_dr2 = k1 + k2*2*r_2 + k3*3*r_4; + double g = 1+k1*r_2+k2*r_4+k3*r_6; - CvMat* dg_dbig = cvCreateMat(1,3,CV_64F); - cvScale( dr2_dbig, dg_dbig, dg_dr2 ); + CvMat* dg_dbig = cvCreateMat(1,3,CV_64F); + cvScale( dr2_dbig, dg_dbig, dg_dr2 ); - CvMat* tmp = cvCreateMat( 2, 3, CV_64F ); - CvMat* dstrike2_dbig = cvCreateMat( 2, 3, CV_64F ); + CvMat* tmp = cvCreateMat( 2, 3, CV_64F ); + CvMat* dstrike2_dbig = cvCreateMat( 2, 3, CV_64F ); - double c[4] = { g+2*p1*y_strike+4*p2*x_strike, 2*p1*x_strike, - 2*p2*y_strike, g+2*p2*x_strike + 4*p1*y_strike }; + double c[4] = { g+2*p1*y_strike+4*p2*x_strike, 2*p1*x_strike, + 2*p2*y_strike, g+2*p2*x_strike + 4*p1*y_strike }; - CvMat coeffmat = cvMat(2,2,CV_64F, c ); + CvMat coeffmat = cvMat(2,2,CV_64F, c ); - cvMatMul(&coeffmat, dstrike_dbig, dstrike2_dbig ); + cvMatMul(&coeffmat, dstrike_dbig, dstrike2_dbig ); - cvGEMM( &strike, dg_dbig, 1, NULL, 0, tmp, CV_GEMM_A_T ); - cvAdd( dstrike2_dbig, tmp, dstrike2_dbig ); + cvGEMM( &strike, dg_dbig, 1, NULL, 0, tmp, CV_GEMM_A_T ); + cvAdd( dstrike2_dbig, tmp, dstrike2_dbig ); - double p[2] = { p2, p1 }; - CvMat pmat = cvMat(2, 1, CV_64F, p ); + double p[2] = { p2, p1 }; + CvMat pmat = cvMat(2, 1, CV_64F, p ); - cvMatMul( &pmat, dr2_dbig ,tmp); - cvAdd( dstrike2_dbig, tmp, dstrike2_dbig ); + cvMatMul( &pmat, dr2_dbig ,tmp); + cvAdd( dstrike2_dbig, tmp, dstrike2_dbig ); - cvCopy( dstrike2_dbig, B ); + cvCopy( dstrike2_dbig, B ); - cvReleaseMat(&dr2_dbig); - cvReleaseMat(&dg_dbig); + cvReleaseMat(&dr2_dbig); + cvReleaseMat(&dg_dbig); - cvReleaseMat(&tmp); - cvReleaseMat(&dstrike2_dbig); - cvReleaseMat(&tmp); - } - else - { - cvCopy(dstrike_dbig, B); - } - //multiply by fx, fy - CvMat row; - cvGetRows( B, &row, 0, 1 ); - cvScale( &row, &row, fx ); + cvReleaseMat(&tmp); + cvReleaseMat(&dstrike2_dbig); + cvReleaseMat(&tmp); + } else { + cvCopy(dstrike_dbig, B); + } + //multiply by fx, fy + CvMat row; + cvGetRows( B, &row, 0, 1 ); + cvScale( &row, &row, fx ); - cvGetRows( B, &row, 1, 2 ); - cvScale( &row, &row, fy ); + cvGetRows( B, &row, 1, 2 ); + cvScale( &row, &row, fy ); #else - double k = fx/(z*z); + double k = fx/(z*z); - cvmSet( B, 0, 0, k*(R[0]*z-x*R[6])); - cvmSet( B, 0, 1, k*(R[1]*z-x*R[7])); - cvmSet( B, 0, 2, k*(R[2]*z-x*R[8])); + cvmSet( B, 0, 0, k*(R[0]*z-x*R[6])); + cvmSet( B, 0, 1, k*(R[1]*z-x*R[7])); + cvmSet( B, 0, 2, k*(R[2]*z-x*R[8])); - k = fy/(z*z); + k = fy/(z*z); - cvmSet( B, 1, 0, k*(R[3]*z-y*R[6])); - cvmSet( B, 1, 1, k*(R[4]*z-y*R[7])); - cvmSet( B, 1, 2, k*(R[5]*z-y*R[8])); + cvmSet( B, 1, 0, k*(R[3]*z-y*R[6])); + cvmSet( B, 1, 1, k*(R[4]*z-y*R[7])); + cvmSet( B, 1, 2, k*(R[5]*z-y*R[8])); #endif }; -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 ); +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; + //split camera params into different matrices + CvMat _ri, _ti, _k; - cvGetRows( cam_params, &_ri, 0, 3 ); - cvGetRows( cam_params, &_ti, 3, 6 ); + cvGetRows( cam_params, &_ri, 0, 3 ); + cvGetRows( cam_params, &_ti, 3, 6 ); - double intr_data[9] = {0, 0, 0, 0, 0, 0, 0, 0, 1}; - intr_data[0] = cam_params->data.db[6]; - intr_data[4] = cam_params->data.db[7]; - intr_data[2] = cam_params->data.db[8]; - intr_data[5] = cam_params->data.db[9]; + double intr_data[9] = {0, 0, 0, 0, 0, 0, 0, 0, 1}; + intr_data[0] = cam_params->data.db[6]; + intr_data[4] = cam_params->data.db[7]; + 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); + //int cn = CV_MAT_CN(_Mi.type); - bool have_dk = cam_params->height - 10 ? true : false; + bool have_dk = cam_params->height - 10 ? true : false; - if( have_dk ) - { - cvGetRows( cam_params, &_k, 10, cam_params->height ); - } - cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, have_dk ? &_k : NULL, _mp, NULL, NULL, - NULL, NULL, NULL, 0); - cvTranspose( _mp, estim ); - cvReleaseMat( &_mp ); + if( have_dk ) { + cvGetRows( cam_params, &_k, 10, cam_params->height ); + } + cvProjectPoints2( &_Mi, &_ri, &_ti, &_A, have_dk ? &_k : NULL, _mp, NULL, NULL, + NULL, NULL, NULL, 0); + // std::cerr<<"_mp = "<<_mp->data.db[0]<<","<<_mp->data.db[1]<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) -{ - CvMat _point_params = point_params, _cam_params = cam_params, _estim = estim; - func(i,j,&_point_params,&_cam_params,&_estim,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); }; void LevMarqSparse::bundleAdjust( vector& points, //positions of points in global coordinate system (input and output) - const vector >& imagePoints, //projections of 3d points for every camera - const vector >& visibility, //visibility of 3d points for every camera - vector& cameraMatrix, //intrinsic matrices of all cameras (input and output) - vector& R, //rotation matrices of all cameras (input and output) - vector& T, //translation vector of all cameras (input and output) - vector& distCoeffs, //distortion coefficients of all cameras (input and output) - const TermCriteria& criteria) - //,enum{MOTION_AND_STRUCTURE,MOTION,STRUCTURE}) -{ - int num_points = (int)points.size(); - int num_cameras = (int)cameraMatrix.size(); + const vector >& imagePoints, //projections of 3d points for every camera + const vector >& visibility, //visibility of 3d points for every camera + vector& cameraMatrix, //intrinsic matrices of all cameras (input and output) + vector& R, //rotation matrices of all cameras (input and output) + vector& T, //translation vector of all cameras (input and output) + vector& distCoeffs, //distortion coefficients of all cameras (input and output) + const TermCriteria& criteria, + BundleAdjustCallback cb, void* user_data) { + //,enum{MOTION_AND_STRUCTURE,MOTION,STRUCTURE}) + int num_points = points.size(); + int num_cameras = cameraMatrix.size(); - CV_Assert( imagePoints.size() == (size_t)num_cameras && - visibility.size() == (size_t)num_cameras && - R.size() == (size_t)num_cameras && - T.size() == (size_t)num_cameras && - (distCoeffs.size() == (size_t)num_cameras || distCoeffs.size() == 0) ); + CV_Assert( imagePoints.size() == (size_t)num_cameras && + visibility.size() == (size_t)num_cameras && + R.size() == (size_t)num_cameras && + T.size() == (size_t)num_cameras && + (distCoeffs.size() == (size_t)num_cameras || distCoeffs.size() == 0) ); - int numdist = distCoeffs.size() ? (distCoeffs[0].rows * distCoeffs[0].cols) : 0; + int numdist = distCoeffs.size() ? (distCoeffs[0].rows * distCoeffs[0].cols) : 0; - int num_cam_param = 3 /* rotation vector */ + 3 /* translation vector */ - + 2 /* fx, fy */ + 2 /* cx, cy */ + numdist; + int num_cam_param = 3 /* rotation vector */ + 3 /* translation vector */ + + 2 /* fx, fy */ + 2 /* cx, cy */ + numdist; - int num_point_param = 3; + int num_point_param = 3; - //collect camera parameters into vector - Mat params( num_cameras * num_cam_param + num_points * num_point_param, 1, CV_64F ); + //collect camera parameters into vector + 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++ ) - { - //rotation - Mat rot_vec; Rodrigues( R[i], rot_vec ); - Mat dst = params.rowRange(i*num_cam_param, i*num_cam_param+3); - rot_vec.copyTo(dst); + //fill camera params + 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); + rot_vec.copyTo(dst); - //translation - dst = params.rowRange(i*num_cam_param + 3, i*num_cam_param+6); - T[i].copyTo(dst); + //translation + dst = params.rowRange(i*num_cam_param + 3, i*num_cam_param+6); + T[i].copyTo(dst); - //intrinsic camera matrix - double* intr_data = (double*)cameraMatrix[i].data; - double* intr = (double*)(params.data + params.step * (i*num_cam_param+6)); - //focals - intr[0] = intr_data[0]; //fx - intr[1] = intr_data[4]; //fy - //center of projection - intr[2] = intr_data[2]; //cx - intr[3] = intr_data[5]; //cy + //intrinsic camera matrix + double* intr_data = (double*)cameraMatrix[i].data; + double* intr = (double*)(params.data + params.step * (i*num_cam_param+6)); + //focals + intr[0] = intr_data[0]; //fx + intr[1] = intr_data[4]; //fy + //center of projection + intr[2] = intr_data[2]; //cx + intr[3] = intr_data[5]; //cy - //add distortion if exists - if( distCoeffs.size() ) - { - dst = params.rowRange(i*num_cam_param + 10, i*num_cam_param+10+numdist); - distCoeffs[i].copyTo(dst); - } - } - - //fill point params - Mat ptparams(num_points, 1, CV_64FC3, params.data + num_cameras*num_cam_param*params.step); - Mat _points(points); - CV_Assert(_points.size() == ptparams.size() && _points.type() == ptparams.type()); - _points.copyTo(ptparams); - - //convert visibility vectors to visibility matrix - Mat vismat(num_points, num_cameras, CV_32S); - 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 ); + //add distortion if exists + if( distCoeffs.size() ) { + dst = params.rowRange(i*num_cam_param + 10, i*num_cam_param+10+numdist); + distCoeffs[i].copyTo(dst); } + } - int num_proj = countNonZero(vismat); //total number of points projections + //fill point params + Mat ptparams(num_points, 1, CV_64FC3, params.data + num_cameras*num_cam_param*params.step); + Mat _points(points); + CV_Assert(_points.size() == ptparams.size() && _points.type() == ptparams.type()); + _points.copyTo(ptparams); - //collect measurements - Mat X(num_proj*2,1,CV_64F); //measurement vector + //convert visibility vectors to visibility matrix + Mat vismat(num_points, num_cameras, CV_32S); + 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 ); + } + + int num_proj = countNonZero(vismat); //total number of points projections + + //collect measurements + 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++ ) - { - //check visibility - 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; - counter+=2; - } - } - } + int counter = 0; + for(int i = 0; i < num_points; i++ ) { + for(int j = 0; j < num_cameras; j++ ) { + //check visibility + 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 ); - //extract results - //fill point params - Mat final_points(num_points, 1, CV_64FC3, - levmar.P->data.db + num_cameras*num_cam_param *levmar.P->step); + LevMarqSparse levmar( num_points, num_cameras, num_point_param, num_cam_param, 2, vismat, params, X, + TermCriteria(criteria), fjac_new, func_new, NULL, + cb, user_data); + //extract results + //fill point params + /*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); - - //fill camera params - 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] ); - //translation - T[i] = Mat(levmar.P).rowRange(i*num_cam_param + 3, i*num_cam_param+6); + final_points.copyTo(_points);*/ - //intrinsic camera matrix - double* intr_data = (double*)cameraMatrix[i].data; - double* intr = (double*)(Mat(levmar.P).data + Mat(levmar.P).step * (i*num_cam_param+6)); - //focals - intr_data[0] = intr[0]; //fx - intr_data[4] = intr[1]; //fy - //center of projection - intr_data[2] = intr[2]; //cx - intr_data[5] = intr[3]; //cy + 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"<