X-Git-Url: https://vcs.maemo.org/git/?a=blobdiff_plain;f=src%2Fcvaux%2Fcvba.cpp;fp=src%2Fcvaux%2Fcvba.cpp;h=a1dbb52712bdf5e2152c55d13193e552dc0248b9;hb=e4c14cdbdf2fe805e79cd96ded236f57e7b89060;hp=0000000000000000000000000000000000000000;hpb=454138ff8a20f6edb9b65a910101403d8b520643;p=opencv diff --git a/src/cvaux/cvba.cpp b/src/cvaux/cvba.cpp new file mode 100755 index 0000000..a1dbb52 --- /dev/null +++ b/src/cvaux/cvba.cpp @@ -0,0 +1,1108 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2009, PhaseSpace Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The names of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + +#include "_cvaux.h" + +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; +} + +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 + + // 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; + + 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 ); + + tmp = ((CvMat**)(B->data.ptr + i * B->step))[j]; + 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); + + 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; + + //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(&Yj[i]); + } + delete Yj; + + cvReleaseMat(&X); + cvReleaseMat(&prevP); + cvReleaseMat(&P); + cvReleaseMat(&deltaP); + + cvReleaseMat(&err); + + cvReleaseMat(&JtJ_diag); + cvReleaseMat(&S); + cvReleaseMat(&hX); +} + +//A params correspond to Cameras +//B params correspont to Points + +//num_cameras - total number of cameras +//num_points - total number of points + +//num_par_per_camera - number of parameters per camera +//num_par_per_point - number of parameters per point + +//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(); + + 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_; + + //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 U_size = Aij_width; + int V_size = Bij_width; + + int Wij_height = Aij_width; + int Wij_width = Bij_width; + + //allocate memory for all Aij, Bij, U, V, W + + //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) ); + + //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; + + 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 + } + } + } + + //allocate U + U = new CvMat* [num_cams]; + for( int j = 0; j < num_cams; j++ ) + { + U[j] = cvCreateMat( U_size, U_size, CV_64F ); + } + //allocate ea + ea = new CvMat* [num_cams]; + for( int j = 0; j < num_cams; j++ ) + { + ea[j] = cvCreateMat( U_size, 1, CV_64F ); + } + + //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 ); + } + + //allocate eb + eb = new CvMat* [num_points]; + for( int i = 0; i < num_points; i++ ) + { + eb[i] = cvCreateMat( V_size, 1, CV_64F ); + } + + //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 + } + + //allocate matrix S + S = cvCreateMat( num_cams * num_cam_param, num_cams * num_cam_param, CV_64F); + + JtJ_diag = cvCreateMat( num_cams * num_cam_param + num_points * num_point_param, 1, CV_64F ); + + //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 ); + //create error vector + err = cvCreateMat( X->rows, X->cols, CV_64F ); + + ask_for_proj(); + + //compute initial error + cvSub( X, hX, err ); + + prevErrNorm = cvNorm( err, 0, CV_L2 ); + iters = 0; + criteria = criteria_init; + + optimize(); +} + +void LevMarqSparse::ask_for_proj() +{ + //given parameter P, compute measurement hX + int ind = 0; + 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 + { + 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 ); + + 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 +{ + // compute jacobians Aij and Bij + for( int i = 0; i < A->height; 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 < 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]; + Mat _point_mat(&point_mat), _cam_mat(&cam_mat), _Aij(Aij), _Bij(Bij); + (*fjac)(i, j, _point_mat, _cam_mat, _Aij, _Bij, data); + } + } + } +} + +void LevMarqSparse::optimize() //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 + + while(!done) + { + // compute jacobians Aij and Bij + ask_for_projac(); + + //compute U_j and ea_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++ ) + { + //get Aij + CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j]; + 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*/ ) ); + cvGEMM( Aij, &eij, 1, ea[j], 1, ea[j], CV_GEMM_A_T ); + } + } + } //U_j and ea_j computed for all j + + //compute V_i and eb_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++ ) + { + //get Bij + CvMat* Bij = ((CvMat**)(B->data.ptr + B->step * i))[j]; + + 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]; + + 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]; + + //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 ); + } + } + + 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; + } + + //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 ); +#else + cvScale( &diag, &diag, 1 + lambda ); +#endif + } + for(int i = 0; i < num_points; i++ ) + { + CvMat diag; + cvGetDiag( V[i], &diag ); +#if 1 + cvAddS( &diag, cvScalar( lambda ), &diag ); +#else + 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] ); + + if( fabs(det) <= FLT_EPSILON ) + { + inverted_ok = false; + break; + } //means we did wrong augmentation, try to choose different lambda + } + + 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]; + 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]; + + 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 + + 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 + + //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; + + //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 ); + } + + 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 + + //Solve linear system S * deltaP_a = E + CvMat dpa; + cvGetSubRect( deltaP, &dpa, cvRect(0, 0, 1, S->width ) ); + int res = cvSolve( S, E, &dpa ); + + 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]; + + 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 ); + + //evaluate function with new parameters + ask_for_proj(); // func( P, hX ); + + //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; + + //restore diagonal from backup + { + 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( &subr, &dia ); + } + 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 + { + //accept change and decrease lambda + lambda /= 10; + lambda = MAX(lambda, 1e-16); + prevErrNorm = errNorm; + + //compute new projection error vector + cvSub( X, hX, err ); + break; + } + } + iters++; + + 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) ) + { + done = true; + break; + } + else + { + //copy new params and continue iterations + cvCopy( P, prevP ); + } + } + cvReleaseMat(&YWt); + cvReleaseMat(&E); +} + +//Utilities + +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 + + //split camera params into different matrices + CvMat _ri, _ti, _k; + 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]; + + CvMat _A = cvMat(3,3, CV_64F, intr_data ); + + CvMat _dpdr, _dpdt, _dpdf, _dpdc, _dpdk; + + 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 ); + + 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 ); + + //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' = x/z + // y' = y/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' + + //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) + + //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') + + // 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) + + // 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) + + // 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 _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]; + + 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]; + +#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 + + 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, &_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_); + + //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 ); + + 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; + + 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 ); + + 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 ); + + cvMatMul(&coeffmat, dstrike_dbig, 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 ); + + cvMatMul( &pmat, dr2_dbig ,tmp); + cvAdd( dstrike2_dbig, tmp, dstrike2_dbig ); + + cvCopy( dstrike2_dbig, B ); + + 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 ); + + cvGetRows( B, &row, 1, 2 ); + cvScale( &row, &row, fy ); + +#else + + 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])); + + 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])); + +#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 ); + + CvMat* _mp = cvCreateMat(1, 2, CV_64F ); //projection of the point + + //split camera params into different matrices + CvMat _ri, _ti, _k; + + 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]; + + 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 ) + { + cvGetRows( cam_params, &_k, 10, cam_params->height ); + } + cvProjectPoints2( &_Mi, &_ri, &_ti, &_A, have_dk ? &_k : NULL, _mp, NULL, NULL, + NULL, NULL, NULL, 0); + cvTranspose( _mp, estim ); + cvReleaseMat( &_mp ); +}; + +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 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 = 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) ); + + 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_point_param = 3; + + //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); + + //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 + + //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 ); + } + + 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; + } + } + } + + 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); + 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); + + //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 + + //add distortion if exists + if( distCoeffs.size() ) + { + params.rowRange(i*num_cam_param + 10, i*num_cam_param+10+numdist).copyTo(distCoeffs[i]); + } + } +} + +}// end of namespace cv