diff --git a/modules/3d/doc/3d.bib b/modules/3d/doc/3d.bib index 5a7f653766..f577858fc7 100644 --- a/modules/3d/doc/3d.bib +++ b/modules/3d/doc/3d.bib @@ -20,26 +20,6 @@ publisher={IEEE} } -@inproceedings{hesch2011direct, - title={A direct least-squares (DLS) method for PnP}, - author={Hesch, Joel and Roumeliotis, Stergios and others}, - booktitle={Computer Vision (ICCV), 2011 IEEE International Conference on}, - pages={383--390}, - year={2011}, - organization={IEEE} -} - -@article{penate2013exhaustive, - title={Exhaustive linearization for robust camera pose and focal length estimation}, - author={Penate-Sanchez, Adrian and Andrade-Cetto, Juan and Moreno-Noguer, Francesc}, - journal={Pattern Analysis and Machine Intelligence, IEEE Transactions on}, - volume={35}, - number={10}, - pages={2387--2400}, - year={2013}, - publisher={IEEE} -} - @inproceedings{Terzakis2020SQPnP, title={A Consistently Fast and Globally Optimal Solution to the Perspective-n-Point Problem}, author={George Terzakis and Manolis Lourakis}, diff --git a/modules/3d/doc/solvePnP.markdown b/modules/3d/doc/solvePnP.markdown index dd4fbaa15d..cda9f7a116 100644 --- a/modules/3d/doc/solvePnP.markdown +++ b/modules/3d/doc/solvePnP.markdown @@ -112,15 +112,6 @@ In this case the function requires exactly four object and image points. In this case the function requires exactly four object and image points. - cv::SOLVEPNP_EPNP Method has been introduced by F. Moreno-Noguer, V. Lepetit and P. Fua in the paper "EPnP: Efficient Perspective-n-Point Camera Pose Estimation" (@cite lepetit2009epnp). -- cv::SOLVEPNP_DLS **Broken implementation. Using this flag will fallback to EPnP.** \n -Method is based on the paper of J. Hesch and S. Roumeliotis. -"A Direct Least-Squares (DLS) Method for PnP" (@cite hesch2011direct). -- cv::SOLVEPNP_UPNP **Broken implementation. Using this flag will fallback to EPnP.** \n -Method is based on the paper of A. Penate-Sanchez, J. Andrade-Cetto, -F. Moreno-Noguer. "Exhaustive Linearization for Robust Camera Pose and Focal Length -Estimation" (@cite penate2013exhaustive). In this case the function also estimates the parameters \f$f_x\f$ and \f$f_y\f$ -assuming that both have the same value. Then the cameraMatrix is updated with the estimated -focal length. - cv::SOLVEPNP_IPPE Method is based on the paper of T. Collins and A. Bartoli. "Infinitesimal Plane-Based Pose Estimation" (@cite Collins14). This method requires coplanar object points. - cv::SOLVEPNP_IPPE_SQUARE Method is based on the paper of Toby Collins and Adrien Bartoli. diff --git a/modules/3d/include/opencv2/3d.hpp b/modules/3d/include/opencv2/3d.hpp index 71d3da9127..07b9b1144d 100644 --- a/modules/3d/include/opencv2/3d.hpp +++ b/modules/3d/include/opencv2/3d.hpp @@ -392,21 +392,17 @@ enum SolvePnPMethod { //!< Initial solution for planar "objectPoints" needs at least 4 points and uses pose from homography decomposition. SOLVEPNP_EPNP = 1, //!< EPnP: Efficient Perspective-n-Point Camera Pose Estimation @cite lepetit2009epnp SOLVEPNP_P3P = 2, //!< Complete Solution Classification for the Perspective-Three-Point Problem @cite gao2003complete - SOLVEPNP_DLS = 3, //!< **Broken implementation. Using this flag will fallback to EPnP.** \n - //!< A Direct Least-Squares (DLS) Method for PnP @cite hesch2011direct - SOLVEPNP_UPNP = 4, //!< **Broken implementation. Using this flag will fallback to EPnP.** \n - //!< Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation @cite penate2013exhaustive - SOLVEPNP_AP3P = 5, //!< An Efficient Algebraic Solution to the Perspective-Three-Point Problem @cite Ke17 - SOLVEPNP_IPPE = 6, //!< Infinitesimal Plane-Based Pose Estimation @cite Collins14 \n + SOLVEPNP_AP3P = 3, //!< An Efficient Algebraic Solution to the Perspective-Three-Point Problem @cite Ke17 + SOLVEPNP_IPPE = 4, //!< Infinitesimal Plane-Based Pose Estimation @cite Collins14 \n //!< Object points must be coplanar. - SOLVEPNP_IPPE_SQUARE = 7, //!< Infinitesimal Plane-Based Pose Estimation @cite Collins14 \n + SOLVEPNP_IPPE_SQUARE = 5, //!< Infinitesimal Plane-Based Pose Estimation @cite Collins14 \n //!< This is a special case suitable for marker pose estimation.\n //!< 4 coplanar object points must be defined in the following order: //!< - point 0: [-squareLength / 2, squareLength / 2, 0] //!< - point 1: [ squareLength / 2, squareLength / 2, 0] //!< - point 2: [ squareLength / 2, -squareLength / 2, 0] //!< - point 3: [-squareLength / 2, -squareLength / 2, 0] - SOLVEPNP_SQPNP = 8, //!< SQPnP: A Consistently Fast and Globally OptimalSolution to the Perspective-n-Point Problem @cite Terzakis2020SQPnP + SOLVEPNP_SQPNP = 6, //!< SQPnP: A Consistently Fast and Globally OptimalSolution to the Perspective-n-Point Problem @cite Terzakis2020SQPnP #ifndef CV_DOXYGEN SOLVEPNP_MAX_COUNT //!< Used for count #endif @@ -1020,9 +1016,6 @@ More information about Perspective-n-Points is described in @ref calib3d_solvePn - Thus, given some data D = np.array(...) where D.shape = (N,M), in order to use a subset of it as, e.g., imagePoints, one must effectively copy it into a new array: imagePoints = np.ascontiguousarray(D[:,:2]).reshape((N,1,2)) - - The methods @ref SOLVEPNP_DLS and @ref SOLVEPNP_UPNP cannot be used as the current implementations are - unstable and sometimes give completely wrong results. If you pass one of these two - flags, @ref SOLVEPNP_EPNP method will be used instead. - The minimum number of points is 4 in the general case. In the case of @ref SOLVEPNP_P3P and @ref SOLVEPNP_AP3P methods, it is required to use exactly 4 points (the first 3 points are used to estimate all the solutions of the P3P problem, the last one is used to retain the best solution that minimizes the reprojection error). @@ -1252,9 +1245,6 @@ More information is described in @ref calib3d_solvePnP - Thus, given some data D = np.array(...) where D.shape = (N,M), in order to use a subset of it as, e.g., imagePoints, one must effectively copy it into a new array: imagePoints = np.ascontiguousarray(D[:,:2]).reshape((N,1,2)) - - The methods @ref SOLVEPNP_DLS and @ref SOLVEPNP_UPNP cannot be used as the current implementations are - unstable and sometimes give completely wrong results. If you pass one of these two - flags, @ref SOLVEPNP_EPNP method will be used instead. - The minimum number of points is 4 in the general case. In the case of @ref SOLVEPNP_P3P and @ref SOLVEPNP_AP3P methods, it is required to use exactly 4 points (the first 3 points are used to estimate all the solutions of the P3P problem, the last one is used to retain the best solution that minimizes the reprojection error). diff --git a/modules/3d/perf/perf_pnp.cpp b/modules/3d/perf/perf_pnp.cpp index 3a5522310d..f41e95efba 100644 --- a/modules/3d/perf/perf_pnp.cpp +++ b/modules/3d/perf/perf_pnp.cpp @@ -4,7 +4,7 @@ namespace opencv_test { using namespace perf; -CV_ENUM(pnpAlgo, SOLVEPNP_ITERATIVE, SOLVEPNP_EPNP, SOLVEPNP_P3P, SOLVEPNP_DLS, SOLVEPNP_UPNP) +CV_ENUM(pnpAlgo, SOLVEPNP_ITERATIVE, SOLVEPNP_EPNP, SOLVEPNP_P3P) typedef tuple PointsNum_Algo_t; typedef perf::TestBaseWithParam PointsNum_Algo; @@ -14,7 +14,7 @@ typedef perf::TestBaseWithParam PointsNum; PERF_TEST_P(PointsNum_Algo, solvePnP, testing::Combine( //When non planar, DLT needs at least 6 points for SOLVEPNP_ITERATIVE flag testing::Values(6, 3*9, 7*13), //TODO: find why results on 4 points are too unstable - testing::Values((int)SOLVEPNP_ITERATIVE, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_UPNP, (int)SOLVEPNP_DLS) + testing::Values((int)SOLVEPNP_ITERATIVE, (int)SOLVEPNP_EPNP) ) ) { @@ -61,7 +61,7 @@ PERF_TEST_P(PointsNum_Algo, solvePnP, PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints, testing::Combine( testing::Values(5), - testing::Values((int)SOLVEPNP_P3P, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_DLS, (int)SOLVEPNP_UPNP) + testing::Values((int)SOLVEPNP_P3P, (int)SOLVEPNP_EPNP) ) ) { diff --git a/modules/3d/src/dls.cpp b/modules/3d/src/dls.cpp deleted file mode 100644 index ed2e37f558..0000000000 --- a/modules/3d/src/dls.cpp +++ /dev/null @@ -1,660 +0,0 @@ -#include "precomp.hpp" -#include "dls.h" - -#include - -#ifdef HAVE_EIGEN -# if defined __GNUC__ && defined __APPLE__ -# pragma GCC diagnostic ignored "-Wshadow" -# endif -# if defined(_MSC_VER) -# pragma warning(push) -# pragma warning(disable:4701) // potentially uninitialized local variable -# pragma warning(disable:4702) // unreachable code -# pragma warning(disable:4714) // const marked as __forceinline not inlined -# endif -# include -# include -# if defined(_MSC_VER) -# pragma warning(pop) -# endif -# include "opencv2/core/eigen.hpp" -#endif - -namespace cv { - -dls::dls(const Mat& opoints, const Mat& ipoints) -{ - N = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); - p = Mat(3, N, CV_64F); - z = Mat(3, N, CV_64F); - mn = Mat::zeros(3, 1, CV_64F); - - cost__ = 9999; - - f1coeff.resize(21); - f2coeff.resize(21); - f3coeff.resize(21); - - if (opoints.depth() == ipoints.depth()) - { - if (opoints.depth() == CV_32F) - init_points(opoints, ipoints); - else - init_points(opoints, ipoints); - } - else if (opoints.depth() == CV_32F) - init_points(opoints, ipoints); - else - init_points(opoints, ipoints); -} - -dls::~dls() -{ - // TODO Auto-generated destructor stub -} - -bool dls::compute_pose(Mat& R, Mat& t) -{ - - std::vector R_; - R_.push_back(rotx(CV_PI/2)); - R_.push_back(roty(CV_PI/2)); - R_.push_back(rotz(CV_PI/2)); - - // version that calls dls 3 times, to avoid Cayley singularity - for (int i = 0; i < 3; ++i) - { - // Make a random rotation - Mat pp = R_[i] * ( p - repeat(mn, 1, p.cols) ); - - // clear for new data - C_est_.clear(); - t_est_.clear(); - cost_.clear(); - - this->run_kernel(pp); // run dls_pnp() - - // find global minimum - for (unsigned int j = 0; j < cost_.size(); ++j) - { - if( cost_[j] < cost__ ) - { - t_est__ = t_est_[j] - C_est_[j] * R_[i] * mn; - C_est__ = C_est_[j] * R_[i]; - cost__ = cost_[j]; - } - } - - } - - if(C_est__.cols > 0 && C_est__.rows > 0) - { - C_est__.copyTo(R); - t_est__.copyTo(t); - return true; - } - - return false; -} - -void dls::run_kernel(const Mat& pp) -{ - Mat Mtilde(27, 27, CV_64F); - Mat D = Mat::zeros(9, 9, CV_64F); - build_coeff_matrix(pp, Mtilde, D); - - Mat eigenval_r, eigenval_i, eigenvec_r, eigenvec_i; - compute_eigenvec(Mtilde, eigenval_r, eigenval_i, eigenvec_r, eigenvec_i); - - /* - * Now check the solutions - */ - - // extract the optimal solutions from the eigen decomposition of the - // Multiplication matrix - - Mat sols = Mat::zeros(3, 27, CV_64F); - std::vector cost; - int count = 0; - for (int k = 0; k < 27; ++k) - { - // V(:,k) = V(:,k)/V(1,k); - Mat V_kA = eigenvec_r.col(k); // 27x1 - Mat V_kB = Mat(1, 1, z.depth(), V_kA.at(0)); // 1x1 - Mat V_k; solve(V_kB.t(), V_kA.t(), V_k); // A/B = B'\A' - Mat( V_k.t()).copyTo( eigenvec_r.col(k) ); - - //if (imag(V(2,k)) == 0) -#ifdef HAVE_EIGEN - const double epsilon = 1e-4; - if( eigenval_i.at(k,0) >= -epsilon && eigenval_i.at(k,0) <= epsilon ) -#endif - { - - double stmp[3]; - stmp[0] = eigenvec_r.at(9, k); - stmp[1] = eigenvec_r.at(3, k); - stmp[2] = eigenvec_r.at(1, k); - - Mat H = Hessian(stmp); - - Mat eigenvalues, eigenvectors; - eigen(H, eigenvalues, eigenvectors); - - if(positive_eigenvalues(&eigenvalues)) - { - - // sols(:,i) = stmp; - Mat stmp_mat(3, 1, CV_64F, &stmp); - - stmp_mat.copyTo( sols.col(count) ); - - Mat Cbar = cayley2rotbar(stmp_mat); - Mat Cbarvec = Cbar.reshape(1,1).t(); - - // cost(i) = CbarVec' * D * CbarVec; - Mat cost_mat = Cbarvec.t() * D * Cbarvec; - cost.push_back( cost_mat.at(0) ); - - count++; - } - } - } - - // extract solutions - sols = sols.clone().colRange(0, count); - - std::vector C_est, t_est; - for (int j = 0; j < sols.cols; ++j) - { - // recover the optimal orientation - // C_est(:,:,j) = 1/(1 + sols(:,j)' * sols(:,j)) * cayley2rotbar(sols(:,j)); - - Mat sols_j = sols.col(j); - double sols_mult = 1./(1.+Mat( sols_j.t() * sols_j ).at(0)); - Mat C_est_j = cayley2rotbar(sols_j).mul(sols_mult); - C_est.push_back( C_est_j ); - - Mat A2 = Mat::zeros(3, 3, CV_64F); - Mat b2 = Mat::zeros(3, 1, CV_64F); - for (int i = 0; i < N; ++i) - { - Mat eye = Mat::eye(3, 3, CV_64F); - Mat z_mul = z.col(i)*z.col(i).t(); - - A2 += eye - z_mul; - b2 += (z_mul - eye) * C_est_j * pp.col(i); - } - - // recover the optimal translation - Mat X2; solve(A2, b2, X2); // A\B - t_est.push_back(X2); - - } - - // check that the points are infront of the center of perspectivity - for (int k = 0; k < sols.cols; ++k) - { - Mat cam_points = C_est[k] * pp + repeat(t_est[k], 1, pp.cols); - Mat cam_points_k = cam_points.row(2); - - if(is_empty(&cam_points_k)) - { - Mat C_valid = C_est[k], t_valid = t_est[k]; - double cost_valid = cost[k]; - - C_est_.push_back(C_valid); - t_est_.push_back(t_valid); - cost_.push_back(cost_valid); - } - } - -} - -void dls::build_coeff_matrix(const Mat& pp, Mat& Mtilde, Mat& D) -{ - CV_Assert(!pp.empty() && N > 0); - Mat eye = Mat::eye(3, 3, CV_64F); - - // build coeff matrix - // An intermediate matrix, the inverse of what is called "H" in the paper - // (see eq. 25) - - Mat H = Mat::zeros(3, 3, CV_64F); - Mat A = Mat::zeros(3, 9, CV_64F); - Mat pp_i(3, 1, CV_64F); - - Mat z_i(3, 1, CV_64F); - for (int i = 0; i < N; ++i) - { - z.col(i).copyTo(z_i); - A += ( z_i*z_i.t() - eye ) * LeftMultVec(pp.col(i)); - } - - H = eye.mul(N) - z * z.t(); - - // A\B - solve(H, A, A, DECOMP_NORMAL); - H.release(); - - Mat ppi_A(3, 1, CV_64F); - for (int i = 0; i < N; ++i) - { - z.col(i).copyTo(z_i); - ppi_A = LeftMultVec(pp.col(i)) + A; - D += ppi_A.t() * ( eye - z_i*z_i.t() ) * ppi_A; - } - A.release(); - - // fill the coefficients - fill_coeff(&D); - - // generate random samples - std::vector u(5); - randn(u, 0, 200); - - Mat M2 = cayley_LS_M(f1coeff, f2coeff, f3coeff, u); - - Mat M2_1 = M2(Range(0,27), Range(0,27)); - Mat M2_2 = M2(Range(0,27), Range(27,120)); - Mat M2_3 = M2(Range(27,120), Range(27,120)); - Mat M2_4 = M2(Range(27,120), Range(0,27)); - M2.release(); - - // A/B = B'\A' - Mat M2_5; solve(M2_3.t(), M2_2.t(), M2_5); - M2_2.release(); M2_3.release(); - - // construct the multiplication matrix via schur compliment of the Macaulay - // matrix - Mtilde = M2_1 - M2_5.t()*M2_4; - -} - -void dls::compute_eigenvec(const Mat& Mtilde, Mat& eigenval_real, Mat& eigenval_imag, - Mat& eigenvec_real, Mat& eigenvec_imag) -{ -#ifdef HAVE_EIGEN - Eigen::MatrixXd Mtilde_eig, zeros_eig; - cv2eigen(Mtilde, Mtilde_eig); - cv2eigen(Mat::zeros(27, 27, CV_64F), zeros_eig); - - Eigen::MatrixXcd Mtilde_eig_cmplx(27, 27); - Mtilde_eig_cmplx.real() = Mtilde_eig; - Mtilde_eig_cmplx.imag() = zeros_eig; - - Eigen::ComplexEigenSolver ces; - ces.compute(Mtilde_eig_cmplx); - - Eigen::MatrixXd eigval_real = ces.eigenvalues().real(); - Eigen::MatrixXd eigval_imag = ces.eigenvalues().imag(); - Eigen::MatrixXd eigvec_real = ces.eigenvectors().real(); - Eigen::MatrixXd eigvec_imag = ces.eigenvectors().imag(); - - eigen2cv(eigval_real, eigenval_real); - eigen2cv(eigval_imag, eigenval_imag); - eigen2cv(eigvec_real, eigenvec_real); - eigen2cv(eigvec_imag, eigenvec_imag); -#else - EigenvalueDecomposition es(Mtilde); - eigenval_real = es.eigenvalues(); - eigenvec_real = es.eigenvectors(); - eigenval_imag = eigenvec_imag = Mat(); -#endif - -} - -void dls::fill_coeff(const Mat * D_mat) -{ - // TODO: shift D and coefficients one position to left - - double D[10][10]; // put D_mat into array - - for (int i = 0; i < D_mat->rows; ++i) - { - const double* Di = D_mat->ptr(i); - for (int j = 0; j < D_mat->cols; ++j) - { - D[i+1][j+1] = Di[j]; - } - } - - // F1 COEFFICIENT - - f1coeff[1] = 2*D[1][6] - 2*D[1][8] + 2*D[5][6] - 2*D[5][8] + 2*D[6][1] + 2*D[6][5] + 2*D[6][9] - 2*D[8][1] - 2*D[8][5] - 2*D[8][9] + 2*D[9][6] - 2*D[9][8]; // constant term - f1coeff[2] = 6*D[1][2] + 6*D[1][4] + 6*D[2][1] - 6*D[2][5] - 6*D[2][9] + 6*D[4][1] - 6*D[4][5] - 6*D[4][9] - 6*D[5][2] - 6*D[5][4] - 6*D[9][2] - 6*D[9][4]; // s1^2 * s2 - f1coeff[3] = 4*D[1][7] - 4*D[1][3] + 8*D[2][6] - 8*D[2][8] - 4*D[3][1] + 4*D[3][5] + 4*D[3][9] + 8*D[4][6] - 8*D[4][8] + 4*D[5][3] - 4*D[5][7] + 8*D[6][2] + 8*D[6][4] + 4*D[7][1] - 4*D[7][5] - 4*D[7][9] - 8*D[8][2] - 8*D[8][4] + 4*D[9][3] - 4*D[9][7]; // s1 * s2 - f1coeff[4] = 4*D[1][2] - 4*D[1][4] + 4*D[2][1] - 4*D[2][5] - 4*D[2][9] + 8*D[3][6] - 8*D[3][8] - 4*D[4][1] + 4*D[4][5] + 4*D[4][9] - 4*D[5][2] + 4*D[5][4] + 8*D[6][3] + 8*D[6][7] + 8*D[7][6] - 8*D[7][8] - 8*D[8][3] - 8*D[8][7] - 4*D[9][2] + 4*D[9][4]; //s1 * s3 - f1coeff[5] = 8*D[2][2] - 8*D[3][3] - 8*D[4][4] + 8*D[6][6] + 8*D[7][7] - 8*D[8][8]; // s2 * s3 - f1coeff[6] = 4*D[2][6] - 2*D[1][7] - 2*D[1][3] + 4*D[2][8] - 2*D[3][1] + 2*D[3][5] - 2*D[3][9] + 4*D[4][6] + 4*D[4][8] + 2*D[5][3] + 2*D[5][7] + 4*D[6][2] + 4*D[6][4] - 2*D[7][1] + 2*D[7][5] - 2*D[7][9] + 4*D[8][2] + 4*D[8][4] - 2*D[9][3] - 2*D[9][7]; // s2^2 * s3 - f1coeff[7] = 2*D[2][5] - 2*D[1][4] - 2*D[2][1] - 2*D[1][2] - 2*D[2][9] - 2*D[4][1] + 2*D[4][5] - 2*D[4][9] + 2*D[5][2] + 2*D[5][4] - 2*D[9][2] - 2*D[9][4]; //s2^3 - f1coeff[8] = 4*D[1][9] - 4*D[1][1] + 8*D[3][3] + 8*D[3][7] + 4*D[5][5] + 8*D[7][3] + 8*D[7][7] + 4*D[9][1] - 4*D[9][9]; // s1 * s3^2 - f1coeff[9] = 4*D[1][1] - 4*D[5][5] - 4*D[5][9] + 8*D[6][6] - 8*D[6][8] - 8*D[8][6] + 8*D[8][8] - 4*D[9][5] - 4*D[9][9]; // s1 - f1coeff[10] = 2*D[1][3] + 2*D[1][7] + 4*D[2][6] - 4*D[2][8] + 2*D[3][1] + 2*D[3][5] + 2*D[3][9] - 4*D[4][6] + 4*D[4][8] + 2*D[5][3] + 2*D[5][7] + 4*D[6][2] - 4*D[6][4] + 2*D[7][1] + 2*D[7][5] + 2*D[7][9] - 4*D[8][2] + 4*D[8][4] + 2*D[9][3] + 2*D[9][7]; // s3 - f1coeff[11] = 2*D[1][2] + 2*D[1][4] + 2*D[2][1] + 2*D[2][5] + 2*D[2][9] - 4*D[3][6] + 4*D[3][8] + 2*D[4][1] + 2*D[4][5] + 2*D[4][9] + 2*D[5][2] + 2*D[5][4] - 4*D[6][3] + 4*D[6][7] + 4*D[7][6] - 4*D[7][8] + 4*D[8][3] - 4*D[8][7] + 2*D[9][2] + 2*D[9][4]; // s2 - f1coeff[12] = 2*D[2][9] - 2*D[1][4] - 2*D[2][1] - 2*D[2][5] - 2*D[1][2] + 4*D[3][6] + 4*D[3][8] - 2*D[4][1] - 2*D[4][5] + 2*D[4][9] - 2*D[5][2] - 2*D[5][4] + 4*D[6][3] + 4*D[6][7] + 4*D[7][6] + 4*D[7][8] + 4*D[8][3] + 4*D[8][7] + 2*D[9][2] + 2*D[9][4]; // s2 * s3^2 - f1coeff[13] = 6*D[1][6] - 6*D[1][8] - 6*D[5][6] + 6*D[5][8] + 6*D[6][1] - 6*D[6][5] - 6*D[6][9] - 6*D[8][1] + 6*D[8][5] + 6*D[8][9] - 6*D[9][6] + 6*D[9][8]; // s1^2 - f1coeff[14] = 2*D[1][8] - 2*D[1][6] + 4*D[2][3] + 4*D[2][7] + 4*D[3][2] - 4*D[3][4] - 4*D[4][3] - 4*D[4][7] - 2*D[5][6] + 2*D[5][8] - 2*D[6][1] - 2*D[6][5] + 2*D[6][9] + 4*D[7][2] - 4*D[7][4] + 2*D[8][1] + 2*D[8][5] - 2*D[8][9] + 2*D[9][6] - 2*D[9][8]; // s3^2 - f1coeff[15] = 2*D[1][8] - 2*D[1][6] - 4*D[2][3] + 4*D[2][7] - 4*D[3][2] - 4*D[3][4] - 4*D[4][3] + 4*D[4][7] + 2*D[5][6] - 2*D[5][8] - 2*D[6][1] + 2*D[6][5] - 2*D[6][9] + 4*D[7][2] + 4*D[7][4] + 2*D[8][1] - 2*D[8][5] + 2*D[8][9] - 2*D[9][6] + 2*D[9][8]; // s2^2 - f1coeff[16] = 2*D[3][9] - 2*D[1][7] - 2*D[3][1] - 2*D[3][5] - 2*D[1][3] - 2*D[5][3] - 2*D[5][7] - 2*D[7][1] - 2*D[7][5] + 2*D[7][9] + 2*D[9][3] + 2*D[9][7]; // s3^3 - f1coeff[17] = 4*D[1][6] + 4*D[1][8] + 8*D[2][3] + 8*D[2][7] + 8*D[3][2] + 8*D[3][4] + 8*D[4][3] + 8*D[4][7] - 4*D[5][6] - 4*D[5][8] + 4*D[6][1] - 4*D[6][5] - 4*D[6][9] + 8*D[7][2] + 8*D[7][4] + 4*D[8][1] - 4*D[8][5] - 4*D[8][9] - 4*D[9][6] - 4*D[9][8]; // s1 * s2 * s3 - f1coeff[18] = 4*D[1][5] - 4*D[1][1] + 8*D[2][2] + 8*D[2][4] + 8*D[4][2] + 8*D[4][4] + 4*D[5][1] - 4*D[5][5] + 4*D[9][9]; // s1 * s2^2 - f1coeff[19] = 6*D[1][3] + 6*D[1][7] + 6*D[3][1] - 6*D[3][5] - 6*D[3][9] - 6*D[5][3] - 6*D[5][7] + 6*D[7][1] - 6*D[7][5] - 6*D[7][9] - 6*D[9][3] - 6*D[9][7]; // s1^2 * s3 - f1coeff[20] = 4*D[1][1] - 4*D[1][5] - 4*D[1][9] - 4*D[5][1] + 4*D[5][5] + 4*D[5][9] - 4*D[9][1] + 4*D[9][5] + 4*D[9][9]; // s1^3 - - - // F2 COEFFICIENT - - f2coeff[1] = - 2*D[1][3] + 2*D[1][7] - 2*D[3][1] - 2*D[3][5] - 2*D[3][9] - 2*D[5][3] + 2*D[5][7] + 2*D[7][1] + 2*D[7][5] + 2*D[7][9] - 2*D[9][3] + 2*D[9][7]; // constant term - f2coeff[2] = 4*D[1][5] - 4*D[1][1] + 8*D[2][2] + 8*D[2][4] + 8*D[4][2] + 8*D[4][4] + 4*D[5][1] - 4*D[5][5] + 4*D[9][9]; // s1^2 * s2 - f2coeff[3] = 4*D[1][8] - 4*D[1][6] - 8*D[2][3] + 8*D[2][7] - 8*D[3][2] - 8*D[3][4] - 8*D[4][3] + 8*D[4][7] + 4*D[5][6] - 4*D[5][8] - 4*D[6][1] + 4*D[6][5] - 4*D[6][9] + 8*D[7][2] + 8*D[7][4] + 4*D[8][1] - 4*D[8][5] + 4*D[8][9] - 4*D[9][6] + 4*D[9][8]; // s1 * s2 - f2coeff[4] = 8*D[2][2] - 8*D[3][3] - 8*D[4][4] + 8*D[6][6] + 8*D[7][7] - 8*D[8][8]; // s1 * s3 - f2coeff[5] = 4*D[1][4] - 4*D[1][2] - 4*D[2][1] + 4*D[2][5] - 4*D[2][9] - 8*D[3][6] - 8*D[3][8] + 4*D[4][1] - 4*D[4][5] + 4*D[4][9] + 4*D[5][2] - 4*D[5][4] - 8*D[6][3] + 8*D[6][7] + 8*D[7][6] + 8*D[7][8] - 8*D[8][3] + 8*D[8][7] - 4*D[9][2] + 4*D[9][4]; // s2 * s3 - f2coeff[6] = 6*D[5][6] - 6*D[1][8] - 6*D[1][6] + 6*D[5][8] - 6*D[6][1] + 6*D[6][5] - 6*D[6][9] - 6*D[8][1] + 6*D[8][5] - 6*D[8][9] - 6*D[9][6] - 6*D[9][8]; // s2^2 * s3 - f2coeff[7] = 4*D[1][1] - 4*D[1][5] + 4*D[1][9] - 4*D[5][1] + 4*D[5][5] - 4*D[5][9] + 4*D[9][1] - 4*D[9][5] + 4*D[9][9]; // s2^3 - f2coeff[8] = 2*D[2][9] - 2*D[1][4] - 2*D[2][1] - 2*D[2][5] - 2*D[1][2] + 4*D[3][6] + 4*D[3][8] - 2*D[4][1] - 2*D[4][5] + 2*D[4][9] - 2*D[5][2] - 2*D[5][4] + 4*D[6][3] + 4*D[6][7] + 4*D[7][6] + 4*D[7][8] + 4*D[8][3] + 4*D[8][7] + 2*D[9][2] + 2*D[9][4]; // s1 * s3^2 - f2coeff[9] = 2*D[1][2] + 2*D[1][4] + 2*D[2][1] + 2*D[2][5] + 2*D[2][9] - 4*D[3][6] + 4*D[3][8] + 2*D[4][1] + 2*D[4][5] + 2*D[4][9] + 2*D[5][2] + 2*D[5][4] - 4*D[6][3] + 4*D[6][7] + 4*D[7][6] - 4*D[7][8] + 4*D[8][3] - 4*D[8][7] + 2*D[9][2] + 2*D[9][4]; // s1 - f2coeff[10] = 2*D[1][6] + 2*D[1][8] - 4*D[2][3] + 4*D[2][7] - 4*D[3][2] + 4*D[3][4] + 4*D[4][3] - 4*D[4][7] + 2*D[5][6] + 2*D[5][8] + 2*D[6][1] + 2*D[6][5] + 2*D[6][9] + 4*D[7][2] - 4*D[7][4] + 2*D[8][1] + 2*D[8][5] + 2*D[8][9] + 2*D[9][6] + 2*D[9][8]; // s3 - f2coeff[11] = 8*D[3][3] - 4*D[1][9] - 4*D[1][1] - 8*D[3][7] + 4*D[5][5] - 8*D[7][3] + 8*D[7][7] - 4*D[9][1] - 4*D[9][9]; // s2 - f2coeff[12] = 4*D[1][1] - 4*D[5][5] + 4*D[5][9] + 8*D[6][6] + 8*D[6][8] + 8*D[8][6] + 8*D[8][8] + 4*D[9][5] - 4*D[9][9]; // s2 * s3^2 - f2coeff[13] = 2*D[1][7] - 2*D[1][3] + 4*D[2][6] - 4*D[2][8] - 2*D[3][1] + 2*D[3][5] + 2*D[3][9] + 4*D[4][6] - 4*D[4][8] + 2*D[5][3] - 2*D[5][7] + 4*D[6][2] + 4*D[6][4] + 2*D[7][1] - 2*D[7][5] - 2*D[7][9] - 4*D[8][2] - 4*D[8][4] + 2*D[9][3] - 2*D[9][7]; // s1^2 - f2coeff[14] = 2*D[1][3] - 2*D[1][7] + 4*D[2][6] + 4*D[2][8] + 2*D[3][1] + 2*D[3][5] - 2*D[3][9] - 4*D[4][6] - 4*D[4][8] + 2*D[5][3] - 2*D[5][7] + 4*D[6][2] - 4*D[6][4] - 2*D[7][1] - 2*D[7][5] + 2*D[7][9] + 4*D[8][2] - 4*D[8][4] - 2*D[9][3] + 2*D[9][7]; // s3^2 - f2coeff[15] = 6*D[1][3] - 6*D[1][7] + 6*D[3][1] - 6*D[3][5] + 6*D[3][9] - 6*D[5][3] + 6*D[5][7] - 6*D[7][1] + 6*D[7][5] - 6*D[7][9] + 6*D[9][3] - 6*D[9][7]; // s2^2 - f2coeff[16] = 2*D[6][9] - 2*D[1][8] - 2*D[5][6] - 2*D[5][8] - 2*D[6][1] - 2*D[6][5] - 2*D[1][6] - 2*D[8][1] - 2*D[8][5] + 2*D[8][9] + 2*D[9][6] + 2*D[9][8]; // s3^3 - f2coeff[17] = 8*D[2][6] - 4*D[1][7] - 4*D[1][3] + 8*D[2][8] - 4*D[3][1] + 4*D[3][5] - 4*D[3][9] + 8*D[4][6] + 8*D[4][8] + 4*D[5][3] + 4*D[5][7] + 8*D[6][2] + 8*D[6][4] - 4*D[7][1] + 4*D[7][5] - 4*D[7][9] + 8*D[8][2] + 8*D[8][4] - 4*D[9][3] - 4*D[9][7]; // s1 * s2 * s3 - f2coeff[18] = 6*D[2][5] - 6*D[1][4] - 6*D[2][1] - 6*D[1][2] - 6*D[2][9] - 6*D[4][1] + 6*D[4][5] - 6*D[4][9] + 6*D[5][2] + 6*D[5][4] - 6*D[9][2] - 6*D[9][4]; // s1 * s2^2 - f2coeff[19] = 2*D[1][6] + 2*D[1][8] + 4*D[2][3] + 4*D[2][7] + 4*D[3][2] + 4*D[3][4] + 4*D[4][3] + 4*D[4][7] - 2*D[5][6] - 2*D[5][8] + 2*D[6][1] - 2*D[6][5] - 2*D[6][9] + 4*D[7][2] + 4*D[7][4] + 2*D[8][1] - 2*D[8][5] - 2*D[8][9] - 2*D[9][6] - 2*D[9][8]; // s1^2 * s3 - f2coeff[20] = 2*D[1][2] + 2*D[1][4] + 2*D[2][1] - 2*D[2][5] - 2*D[2][9] + 2*D[4][1] - 2*D[4][5] - 2*D[4][9] - 2*D[5][2] - 2*D[5][4] - 2*D[9][2] - 2*D[9][4]; // s1^3 - - - // F3 COEFFICIENT - - f3coeff[1] = 2*D[1][2] - 2*D[1][4] + 2*D[2][1] + 2*D[2][5] + 2*D[2][9] - 2*D[4][1] - 2*D[4][5] - 2*D[4][9] + 2*D[5][2] - 2*D[5][4] + 2*D[9][2] - 2*D[9][4]; // constant term - f3coeff[2] = 2*D[1][6] + 2*D[1][8] + 4*D[2][3] + 4*D[2][7] + 4*D[3][2] + 4*D[3][4] + 4*D[4][3] + 4*D[4][7] - 2*D[5][6] - 2*D[5][8] + 2*D[6][1] - 2*D[6][5] - 2*D[6][9] + 4*D[7][2] + 4*D[7][4] + 2*D[8][1] - 2*D[8][5] - 2*D[8][9] - 2*D[9][6] - 2*D[9][8]; // s1^2 * s2 - f3coeff[3] = 8*D[2][2] - 8*D[3][3] - 8*D[4][4] + 8*D[6][6] + 8*D[7][7] - 8*D[8][8]; // s1 * s2 - f3coeff[4] = 4*D[1][8] - 4*D[1][6] + 8*D[2][3] + 8*D[2][7] + 8*D[3][2] - 8*D[3][4] - 8*D[4][3] - 8*D[4][7] - 4*D[5][6] + 4*D[5][8] - 4*D[6][1] - 4*D[6][5] + 4*D[6][9] + 8*D[7][2] - 8*D[7][4] + 4*D[8][1] + 4*D[8][5] - 4*D[8][9] + 4*D[9][6] - 4*D[9][8]; // s1 * s3 - f3coeff[5] = 4*D[1][3] - 4*D[1][7] + 8*D[2][6] + 8*D[2][8] + 4*D[3][1] + 4*D[3][5] - 4*D[3][9] - 8*D[4][6] - 8*D[4][8] + 4*D[5][3] - 4*D[5][7] + 8*D[6][2] - 8*D[6][4] - 4*D[7][1] - 4*D[7][5] + 4*D[7][9] + 8*D[8][2] - 8*D[8][4] - 4*D[9][3] + 4*D[9][7]; // s2 * s3 - f3coeff[6] = 4*D[1][1] - 4*D[5][5] + 4*D[5][9] + 8*D[6][6] + 8*D[6][8] + 8*D[8][6] + 8*D[8][8] + 4*D[9][5] - 4*D[9][9]; // s2^2 * s3 - f3coeff[7] = 2*D[5][6] - 2*D[1][8] - 2*D[1][6] + 2*D[5][8] - 2*D[6][1] + 2*D[6][5] - 2*D[6][9] - 2*D[8][1] + 2*D[8][5] - 2*D[8][9] - 2*D[9][6] - 2*D[9][8]; // s2^3 - f3coeff[8] = 6*D[3][9] - 6*D[1][7] - 6*D[3][1] - 6*D[3][5] - 6*D[1][3] - 6*D[5][3] - 6*D[5][7] - 6*D[7][1] - 6*D[7][5] + 6*D[7][9] + 6*D[9][3] + 6*D[9][7]; // s1 * s3^2 - f3coeff[9] = 2*D[1][3] + 2*D[1][7] + 4*D[2][6] - 4*D[2][8] + 2*D[3][1] + 2*D[3][5] + 2*D[3][9] - 4*D[4][6] + 4*D[4][8] + 2*D[5][3] + 2*D[5][7] + 4*D[6][2] - 4*D[6][4] + 2*D[7][1] + 2*D[7][5] + 2*D[7][9] - 4*D[8][2] + 4*D[8][4] + 2*D[9][3] + 2*D[9][7]; // s1 - f3coeff[10] = 8*D[2][2] - 4*D[1][5] - 4*D[1][1] - 8*D[2][4] - 8*D[4][2] + 8*D[4][4] - 4*D[5][1] - 4*D[5][5] + 4*D[9][9]; // s3 - f3coeff[11] = 2*D[1][6] + 2*D[1][8] - 4*D[2][3] + 4*D[2][7] - 4*D[3][2] + 4*D[3][4] + 4*D[4][3] - 4*D[4][7] + 2*D[5][6] + 2*D[5][8] + 2*D[6][1] + 2*D[6][5] + 2*D[6][9] + 4*D[7][2] - 4*D[7][4] + 2*D[8][1] + 2*D[8][5] + 2*D[8][9] + 2*D[9][6] + 2*D[9][8]; // s2 - f3coeff[12] = 6*D[6][9] - 6*D[1][8] - 6*D[5][6] - 6*D[5][8] - 6*D[6][1] - 6*D[6][5] - 6*D[1][6] - 6*D[8][1] - 6*D[8][5] + 6*D[8][9] + 6*D[9][6] + 6*D[9][8]; // s2 * s3^2 - f3coeff[13] = 2*D[1][2] - 2*D[1][4] + 2*D[2][1] - 2*D[2][5] - 2*D[2][9] + 4*D[3][6] - 4*D[3][8] - 2*D[4][1] + 2*D[4][5] + 2*D[4][9] - 2*D[5][2] + 2*D[5][4] + 4*D[6][3] + 4*D[6][7] + 4*D[7][6] - 4*D[7][8] - 4*D[8][3] - 4*D[8][7] - 2*D[9][2] + 2*D[9][4]; // s1^2 - f3coeff[14] = 6*D[1][4] - 6*D[1][2] - 6*D[2][1] - 6*D[2][5] + 6*D[2][9] + 6*D[4][1] + 6*D[4][5] - 6*D[4][9] - 6*D[5][2] + 6*D[5][4] + 6*D[9][2] - 6*D[9][4]; // s3^2 - f3coeff[15] = 2*D[1][4] - 2*D[1][2] - 2*D[2][1] + 2*D[2][5] - 2*D[2][9] - 4*D[3][6] - 4*D[3][8] + 2*D[4][1] - 2*D[4][5] + 2*D[4][9] + 2*D[5][2] - 2*D[5][4] - 4*D[6][3] + 4*D[6][7] + 4*D[7][6] + 4*D[7][8] - 4*D[8][3] + 4*D[8][7] - 2*D[9][2] + 2*D[9][4]; // s2^2 - f3coeff[16] = 4*D[1][1] + 4*D[1][5] - 4*D[1][9] + 4*D[5][1] + 4*D[5][5] - 4*D[5][9] - 4*D[9][1] - 4*D[9][5] + 4*D[9][9]; // s3^3 - f3coeff[17] = 4*D[2][9] - 4*D[1][4] - 4*D[2][1] - 4*D[2][5] - 4*D[1][2] + 8*D[3][6] + 8*D[3][8] - 4*D[4][1] - 4*D[4][5] + 4*D[4][9] - 4*D[5][2] - 4*D[5][4] + 8*D[6][3] + 8*D[6][7] + 8*D[7][6] + 8*D[7][8] + 8*D[8][3] + 8*D[8][7] + 4*D[9][2] + 4*D[9][4]; // s1 * s2 * s3 - f3coeff[18] = 4*D[2][6] - 2*D[1][7] - 2*D[1][3] + 4*D[2][8] - 2*D[3][1] + 2*D[3][5] - 2*D[3][9] + 4*D[4][6] + 4*D[4][8] + 2*D[5][3] + 2*D[5][7] + 4*D[6][2] + 4*D[6][4] - 2*D[7][1] + 2*D[7][5] - 2*D[7][9] + 4*D[8][2] + 4*D[8][4] - 2*D[9][3] - 2*D[9][7]; // s1 * s2^2 - f3coeff[19] = 4*D[1][9] - 4*D[1][1] + 8*D[3][3] + 8*D[3][7] + 4*D[5][5] + 8*D[7][3] + 8*D[7][7] + 4*D[9][1] - 4*D[9][9]; // s1^2 * s3 - f3coeff[20] = 2*D[1][3] + 2*D[1][7] + 2*D[3][1] - 2*D[3][5] - 2*D[3][9] - 2*D[5][3] - 2*D[5][7] + 2*D[7][1] - 2*D[7][5] - 2*D[7][9] - 2*D[9][3] - 2*D[9][7]; // s1^3 - -} - -Mat dls::LeftMultVec(const Mat& v) -{ - Mat mat_ = Mat::zeros(3, 9, CV_64F); - - for (int i = 0; i < 3; ++i) - { - mat_.at(i, 3*i + 0) = v.at(0); - mat_.at(i, 3*i + 1) = v.at(1); - mat_.at(i, 3*i + 2) = v.at(2); - } - return mat_; -} - -Mat dls::cayley_LS_M(const std::vector& a, const std::vector& b, const std::vector& c, const std::vector& u) -{ - // TODO: input matrix pointer - // TODO: shift coefficients one position to left - - Mat M = Mat::zeros(120, 120, CV_64F); - - M.at(0,0)=u[1]; M.at(0,35)=a[1]; M.at(0,83)=b[1]; M.at(0,118)=c[1]; - M.at(1,0)=u[4]; M.at(1,1)=u[1]; M.at(1,34)=a[1]; M.at(1,35)=a[10]; M.at(1,54)=b[1]; M.at(1,83)=b[10]; M.at(1,99)=c[1]; M.at(1,118)=c[10]; - M.at(2,1)=u[4]; M.at(2,2)=u[1]; M.at(2,34)=a[10]; M.at(2,35)=a[14]; M.at(2,51)=a[1]; M.at(2,54)=b[10]; M.at(2,65)=b[1]; M.at(2,83)=b[14]; M.at(2,89)=c[1]; M.at(2,99)=c[10]; M.at(2,118)=c[14]; - M.at(3,0)=u[3]; M.at(3,3)=u[1]; M.at(3,35)=a[11]; M.at(3,49)=a[1]; M.at(3,76)=b[1]; M.at(3,83)=b[11]; M.at(3,118)=c[11]; M.at(3,119)=c[1]; - M.at(4,1)=u[3]; M.at(4,3)=u[4]; M.at(4,4)=u[1]; M.at(4,34)=a[11]; M.at(4,35)=a[5]; M.at(4,43)=a[1]; M.at(4,49)=a[10]; M.at(4,54)=b[11]; M.at(4,71)=b[1]; M.at(4,76)=b[10]; M.at(4,83)=b[5]; M.at(4,99)=c[11]; M.at(4,100)=c[1]; M.at(4,118)=c[5]; M.at(4,119)=c[10]; - M.at(5,2)=u[3]; M.at(5,4)=u[4]; M.at(5,5)=u[1]; M.at(5,34)=a[5]; M.at(5,35)=a[12]; M.at(5,41)=a[1]; M.at(5,43)=a[10]; M.at(5,49)=a[14]; M.at(5,51)=a[11]; M.at(5,54)=b[5]; M.at(5,62)=b[1]; M.at(5,65)=b[11]; M.at(5,71)=b[10]; M.at(5,76)=b[14]; M.at(5,83)=b[12]; M.at(5,89)=c[11]; M.at(5,99)=c[5]; M.at(5,100)=c[10]; M.at(5,111)=c[1]; M.at(5,118)=c[12]; M.at(5,119)=c[14]; - M.at(6,3)=u[3]; M.at(6,6)=u[1]; M.at(6,30)=a[1]; M.at(6,35)=a[15]; M.at(6,49)=a[11]; M.at(6,75)=b[1]; M.at(6,76)=b[11]; M.at(6,83)=b[15]; M.at(6,107)=c[1]; M.at(6,118)=c[15]; M.at(6,119)=c[11]; - M.at(7,4)=u[3]; M.at(7,6)=u[4]; M.at(7,7)=u[1]; M.at(7,30)=a[10]; M.at(7,34)=a[15]; M.at(7,35)=a[6]; M.at(7,43)=a[11]; M.at(7,45)=a[1]; M.at(7,49)=a[5]; M.at(7,54)=b[15]; M.at(7,63)=b[1]; M.at(7,71)=b[11]; M.at(7,75)=b[10]; M.at(7,76)=b[5]; M.at(7,83)=b[6]; M.at(7,99)=c[15]; M.at(7,100)=c[11]; M.at(7,107)=c[10]; M.at(7,112)=c[1]; M.at(7,118)=c[6]; M.at(7,119)=c[5]; - M.at(8,5)=u[3]; M.at(8,7)=u[4]; M.at(8,8)=u[1]; M.at(8,30)=a[14]; M.at(8,34)=a[6]; M.at(8,41)=a[11]; M.at(8,43)=a[5]; M.at(8,45)=a[10]; M.at(8,46)=a[1]; M.at(8,49)=a[12]; M.at(8,51)=a[15]; M.at(8,54)=b[6]; M.at(8,62)=b[11]; M.at(8,63)=b[10]; M.at(8,65)=b[15]; M.at(8,66)=b[1]; M.at(8,71)=b[5]; M.at(8,75)=b[14]; M.at(8,76)=b[12]; M.at(8,89)=c[15]; M.at(8,99)=c[6]; M.at(8,100)=c[5]; M.at(8,102)=c[1]; M.at(8,107)=c[14]; M.at(8,111)=c[11]; M.at(8,112)=c[10]; M.at(8,119)=c[12]; - M.at(9,0)=u[2]; M.at(9,9)=u[1]; M.at(9,35)=a[9]; M.at(9,36)=a[1]; M.at(9,83)=b[9]; M.at(9,84)=b[1]; M.at(9,88)=c[1]; M.at(9,118)=c[9]; - M.at(10,1)=u[2]; M.at(10,9)=u[4]; M.at(10,10)=u[1]; M.at(10,33)=a[1]; M.at(10,34)=a[9]; M.at(10,35)=a[4]; M.at(10,36)=a[10]; M.at(10,54)=b[9]; M.at(10,59)=b[1]; M.at(10,83)=b[4]; M.at(10,84)=b[10]; M.at(10,88)=c[10]; M.at(10,99)=c[9]; M.at(10,117)=c[1]; M.at(10,118)=c[4]; - M.at(11,2)=u[2]; M.at(11,10)=u[4]; M.at(11,11)=u[1]; M.at(11,28)=a[1]; M.at(11,33)=a[10]; M.at(11,34)=a[4]; M.at(11,35)=a[8]; M.at(11,36)=a[14]; M.at(11,51)=a[9]; M.at(11,54)=b[4]; M.at(11,57)=b[1]; M.at(11,59)=b[10]; M.at(11,65)=b[9]; M.at(11,83)=b[8]; M.at(11,84)=b[14]; M.at(11,88)=c[14]; M.at(11,89)=c[9]; M.at(11,99)=c[4]; M.at(11,114)=c[1]; M.at(11,117)=c[10]; M.at(11,118)=c[8]; - M.at(12,3)=u[2]; M.at(12,9)=u[3]; M.at(12,12)=u[1]; M.at(12,35)=a[3]; M.at(12,36)=a[11]; M.at(12,39)=a[1]; M.at(12,49)=a[9]; M.at(12,76)=b[9]; M.at(12,79)=b[1]; M.at(12,83)=b[3]; M.at(12,84)=b[11]; M.at(12,88)=c[11]; M.at(12,96)=c[1]; M.at(12,118)=c[3]; M.at(12,119)=c[9]; - M.at(13,4)=u[2]; M.at(13,10)=u[3]; M.at(13,12)=u[4]; M.at(13,13)=u[1]; M.at(13,33)=a[11]; M.at(13,34)=a[3]; M.at(13,35)=a[17]; M.at(13,36)=a[5]; M.at(13,39)=a[10]; M.at(13,43)=a[9]; M.at(13,47)=a[1]; M.at(13,49)=a[4]; M.at(13,54)=b[3]; M.at(13,59)=b[11]; M.at(13,60)=b[1]; M.at(13,71)=b[9]; M.at(13,76)=b[4]; M.at(13,79)=b[10]; M.at(13,83)=b[17]; M.at(13,84)=b[5]; M.at(13,88)=c[5]; M.at(13,90)=c[1]; M.at(13,96)=c[10]; M.at(13,99)=c[3]; M.at(13,100)=c[9]; M.at(13,117)=c[11]; M.at(13,118)=c[17]; M.at(13,119)=c[4]; - M.at(14,5)=u[2]; M.at(14,11)=u[3]; M.at(14,13)=u[4]; M.at(14,14)=u[1]; M.at(14,28)=a[11]; M.at(14,33)=a[5]; M.at(14,34)=a[17]; M.at(14,36)=a[12]; M.at(14,39)=a[14]; M.at(14,41)=a[9]; M.at(14,42)=a[1]; M.at(14,43)=a[4]; M.at(14,47)=a[10]; M.at(14,49)=a[8]; M.at(14,51)=a[3]; M.at(14,54)=b[17]; M.at(14,56)=b[1]; M.at(14,57)=b[11]; M.at(14,59)=b[5]; M.at(14,60)=b[10]; M.at(14,62)=b[9]; M.at(14,65)=b[3]; M.at(14,71)=b[4]; M.at(14,76)=b[8]; M.at(14,79)=b[14]; M.at(14,84)=b[12]; M.at(14,88)=c[12]; M.at(14,89)=c[3]; M.at(14,90)=c[10]; M.at(14,96)=c[14]; M.at(14,99)=c[17]; M.at(14,100)=c[4]; M.at(14,106)=c[1]; M.at(14,111)=c[9]; M.at(14,114)=c[11]; M.at(14,117)=c[5]; M.at(14,119)=c[8]; - M.at(15,6)=u[2]; M.at(15,12)=u[3]; M.at(15,15)=u[1]; M.at(15,29)=a[1]; M.at(15,30)=a[9]; M.at(15,35)=a[18]; M.at(15,36)=a[15]; M.at(15,39)=a[11]; M.at(15,49)=a[3]; M.at(15,74)=b[1]; M.at(15,75)=b[9]; M.at(15,76)=b[3]; M.at(15,79)=b[11]; M.at(15,83)=b[18]; M.at(15,84)=b[15]; M.at(15,88)=c[15]; M.at(15,94)=c[1]; M.at(15,96)=c[11]; M.at(15,107)=c[9]; M.at(15,118)=c[18]; M.at(15,119)=c[3]; - M.at(16,7)=u[2]; M.at(16,13)=u[3]; M.at(16,15)=u[4]; M.at(16,16)=u[1]; M.at(16,29)=a[10]; M.at(16,30)=a[4]; M.at(16,33)=a[15]; M.at(16,34)=a[18]; M.at(16,36)=a[6]; M.at(16,39)=a[5]; M.at(16,43)=a[3]; M.at(16,44)=a[1]; M.at(16,45)=a[9]; M.at(16,47)=a[11]; M.at(16,49)=a[17]; M.at(16,54)=b[18]; M.at(16,59)=b[15]; M.at(16,60)=b[11]; M.at(16,63)=b[9]; M.at(16,68)=b[1]; M.at(16,71)=b[3]; M.at(16,74)=b[10]; M.at(16,75)=b[4]; M.at(16,76)=b[17]; M.at(16,79)=b[5]; M.at(16,84)=b[6]; M.at(16,88)=c[6]; M.at(16,90)=c[11]; M.at(16,94)=c[10]; M.at(16,96)=c[5]; M.at(16,97)=c[1]; M.at(16,99)=c[18]; M.at(16,100)=c[3]; M.at(16,107)=c[4]; M.at(16,112)=c[9]; M.at(16,117)=c[15]; M.at(16,119)=c[17]; - M.at(17,8)=u[2]; M.at(17,14)=u[3]; M.at(17,16)=u[4]; M.at(17,17)=u[1]; M.at(17,28)=a[15]; M.at(17,29)=a[14]; M.at(17,30)=a[8]; M.at(17,33)=a[6]; M.at(17,39)=a[12]; M.at(17,41)=a[3]; M.at(17,42)=a[11]; M.at(17,43)=a[17]; M.at(17,44)=a[10]; M.at(17,45)=a[4]; M.at(17,46)=a[9]; M.at(17,47)=a[5]; M.at(17,51)=a[18]; M.at(17,56)=b[11]; M.at(17,57)=b[15]; M.at(17,59)=b[6]; M.at(17,60)=b[5]; M.at(17,62)=b[3]; M.at(17,63)=b[4]; M.at(17,65)=b[18]; M.at(17,66)=b[9]; M.at(17,68)=b[10]; M.at(17,71)=b[17]; M.at(17,74)=b[14]; M.at(17,75)=b[8]; M.at(17,79)=b[12]; M.at(17,89)=c[18]; M.at(17,90)=c[5]; M.at(17,94)=c[14]; M.at(17,96)=c[12]; M.at(17,97)=c[10]; M.at(17,100)=c[17]; M.at(17,102)=c[9]; M.at(17,106)=c[11]; M.at(17,107)=c[8]; M.at(17,111)=c[3]; M.at(17,112)=c[4]; M.at(17,114)=c[15]; M.at(17,117)=c[6]; - M.at(18,9)=u[2]; M.at(18,18)=u[1]; M.at(18,35)=a[13]; M.at(18,36)=a[9]; M.at(18,53)=a[1]; M.at(18,82)=b[1]; M.at(18,83)=b[13]; M.at(18,84)=b[9]; M.at(18,87)=c[1]; M.at(18,88)=c[9]; M.at(18,118)=c[13]; - M.at(19,10)=u[2]; M.at(19,18)=u[4]; M.at(19,19)=u[1]; M.at(19,32)=a[1]; M.at(19,33)=a[9]; M.at(19,34)=a[13]; M.at(19,35)=a[19]; M.at(19,36)=a[4]; M.at(19,53)=a[10]; M.at(19,54)=b[13]; M.at(19,59)=b[9]; M.at(19,61)=b[1]; M.at(19,82)=b[10]; M.at(19,83)=b[19]; M.at(19,84)=b[4]; M.at(19,87)=c[10]; M.at(19,88)=c[4]; M.at(19,99)=c[13]; M.at(19,116)=c[1]; M.at(19,117)=c[9]; M.at(19,118)=c[19]; - M.at(20,11)=u[2]; M.at(20,19)=u[4]; M.at(20,20)=u[1]; M.at(20,27)=a[1]; M.at(20,28)=a[9]; M.at(20,32)=a[10]; M.at(20,33)=a[4]; M.at(20,34)=a[19]; M.at(20,36)=a[8]; M.at(20,51)=a[13]; M.at(20,53)=a[14]; M.at(20,54)=b[19]; M.at(20,55)=b[1]; M.at(20,57)=b[9]; M.at(20,59)=b[4]; M.at(20,61)=b[10]; M.at(20,65)=b[13]; M.at(20,82)=b[14]; M.at(20,84)=b[8]; M.at(20,87)=c[14]; M.at(20,88)=c[8]; M.at(20,89)=c[13]; M.at(20,99)=c[19]; M.at(20,113)=c[1]; M.at(20,114)=c[9]; M.at(20,116)=c[10]; M.at(20,117)=c[4]; - M.at(21,12)=u[2]; M.at(21,18)=u[3]; M.at(21,21)=u[1]; M.at(21,35)=a[2]; M.at(21,36)=a[3]; M.at(21,38)=a[1]; M.at(21,39)=a[9]; M.at(21,49)=a[13]; M.at(21,53)=a[11]; M.at(21,76)=b[13]; M.at(21,78)=b[1]; M.at(21,79)=b[9]; M.at(21,82)=b[11]; M.at(21,83)=b[2]; M.at(21,84)=b[3]; M.at(21,87)=c[11]; M.at(21,88)=c[3]; M.at(21,92)=c[1]; M.at(21,96)=c[9]; M.at(21,118)=c[2]; M.at(21,119)=c[13]; - M.at(22,13)=u[2]; M.at(22,19)=u[3]; M.at(22,21)=u[4]; M.at(22,22)=u[1]; M.at(22,32)=a[11]; M.at(22,33)=a[3]; M.at(22,34)=a[2]; M.at(22,36)=a[17]; M.at(22,38)=a[10]; M.at(22,39)=a[4]; M.at(22,40)=a[1]; M.at(22,43)=a[13]; M.at(22,47)=a[9]; M.at(22,49)=a[19]; M.at(22,53)=a[5]; M.at(22,54)=b[2]; M.at(22,59)=b[3]; M.at(22,60)=b[9]; M.at(22,61)=b[11]; M.at(22,71)=b[13]; M.at(22,72)=b[1]; M.at(22,76)=b[19]; M.at(22,78)=b[10]; M.at(22,79)=b[4]; M.at(22,82)=b[5]; M.at(22,84)=b[17]; M.at(22,87)=c[5]; M.at(22,88)=c[17]; M.at(22,90)=c[9]; M.at(22,92)=c[10]; M.at(22,95)=c[1]; M.at(22,96)=c[4]; M.at(22,99)=c[2]; M.at(22,100)=c[13]; M.at(22,116)=c[11]; M.at(22,117)=c[3]; M.at(22,119)=c[19]; - M.at(23,14)=u[2]; M.at(23,20)=u[3]; M.at(23,22)=u[4]; M.at(23,23)=u[1]; M.at(23,27)=a[11]; M.at(23,28)=a[3]; M.at(23,32)=a[5]; M.at(23,33)=a[17]; M.at(23,38)=a[14]; M.at(23,39)=a[8]; M.at(23,40)=a[10]; M.at(23,41)=a[13]; M.at(23,42)=a[9]; M.at(23,43)=a[19]; M.at(23,47)=a[4]; M.at(23,51)=a[2]; M.at(23,53)=a[12]; M.at(23,55)=b[11]; M.at(23,56)=b[9]; M.at(23,57)=b[3]; M.at(23,59)=b[17]; M.at(23,60)=b[4]; M.at(23,61)=b[5]; M.at(23,62)=b[13]; M.at(23,65)=b[2]; M.at(23,71)=b[19]; M.at(23,72)=b[10]; M.at(23,78)=b[14]; M.at(23,79)=b[8]; M.at(23,82)=b[12]; M.at(23,87)=c[12]; M.at(23,89)=c[2]; M.at(23,90)=c[4]; M.at(23,92)=c[14]; M.at(23,95)=c[10]; M.at(23,96)=c[8]; M.at(23,100)=c[19]; M.at(23,106)=c[9]; M.at(23,111)=c[13]; M.at(23,113)=c[11]; M.at(23,114)=c[3]; M.at(23,116)=c[5]; M.at(23,117)=c[17]; - M.at(24,15)=u[2]; M.at(24,21)=u[3]; M.at(24,24)=u[1]; M.at(24,29)=a[9]; M.at(24,30)=a[13]; M.at(24,36)=a[18]; M.at(24,38)=a[11]; M.at(24,39)=a[3]; M.at(24,49)=a[2]; M.at(24,52)=a[1]; M.at(24,53)=a[15]; M.at(24,73)=b[1]; M.at(24,74)=b[9]; M.at(24,75)=b[13]; M.at(24,76)=b[2]; M.at(24,78)=b[11]; M.at(24,79)=b[3]; M.at(24,82)=b[15]; M.at(24,84)=b[18]; M.at(24,87)=c[15]; M.at(24,88)=c[18]; M.at(24,92)=c[11]; M.at(24,93)=c[1]; M.at(24,94)=c[9]; M.at(24,96)=c[3]; M.at(24,107)=c[13]; M.at(24,119)=c[2]; - M.at(25,16)=u[2]; M.at(25,22)=u[3]; M.at(25,24)=u[4]; M.at(25,25)=u[1]; M.at(25,29)=a[4]; M.at(25,30)=a[19]; M.at(25,32)=a[15]; M.at(25,33)=a[18]; M.at(25,38)=a[5]; M.at(25,39)=a[17]; M.at(25,40)=a[11]; M.at(25,43)=a[2]; M.at(25,44)=a[9]; M.at(25,45)=a[13]; M.at(25,47)=a[3]; M.at(25,52)=a[10]; M.at(25,53)=a[6]; M.at(25,59)=b[18]; M.at(25,60)=b[3]; M.at(25,61)=b[15]; M.at(25,63)=b[13]; M.at(25,68)=b[9]; M.at(25,71)=b[2]; M.at(25,72)=b[11]; M.at(25,73)=b[10]; M.at(25,74)=b[4]; M.at(25,75)=b[19]; M.at(25,78)=b[5]; M.at(25,79)=b[17]; M.at(25,82)=b[6]; M.at(25,87)=c[6]; M.at(25,90)=c[3]; M.at(25,92)=c[5]; M.at(25,93)=c[10]; M.at(25,94)=c[4]; M.at(25,95)=c[11]; M.at(25,96)=c[17]; M.at(25,97)=c[9]; M.at(25,100)=c[2]; M.at(25,107)=c[19]; M.at(25,112)=c[13]; M.at(25,116)=c[15]; M.at(25,117)=c[18]; - M.at(26,17)=u[2]; M.at(26,23)=u[3]; M.at(26,25)=u[4]; M.at(26,26)=u[1]; M.at(26,27)=a[15]; M.at(26,28)=a[18]; M.at(26,29)=a[8]; M.at(26,32)=a[6]; M.at(26,38)=a[12]; M.at(26,40)=a[5]; M.at(26,41)=a[2]; M.at(26,42)=a[3]; M.at(26,44)=a[4]; M.at(26,45)=a[19]; M.at(26,46)=a[13]; M.at(26,47)=a[17]; M.at(26,52)=a[14]; M.at(26,55)=b[15]; M.at(26,56)=b[3]; M.at(26,57)=b[18]; M.at(26,60)=b[17]; M.at(26,61)=b[6]; M.at(26,62)=b[2]; M.at(26,63)=b[19]; M.at(26,66)=b[13]; M.at(26,68)=b[4]; M.at(26,72)=b[5]; M.at(26,73)=b[14]; M.at(26,74)=b[8]; M.at(26,78)=b[12]; M.at(26,90)=c[17]; M.at(26,92)=c[12]; M.at(26,93)=c[14]; M.at(26,94)=c[8]; M.at(26,95)=c[5]; M.at(26,97)=c[4]; M.at(26,102)=c[13]; M.at(26,106)=c[3]; M.at(26,111)=c[2]; M.at(26,112)=c[19]; M.at(26,113)=c[15]; M.at(26,114)=c[18]; M.at(26,116)=c[6]; - M.at(27,15)=u[3]; M.at(27,29)=a[11]; M.at(27,30)=a[3]; M.at(27,36)=a[7]; M.at(27,39)=a[15]; M.at(27,49)=a[18]; M.at(27,69)=b[9]; M.at(27,70)=b[1]; M.at(27,74)=b[11]; M.at(27,75)=b[3]; M.at(27,76)=b[18]; M.at(27,79)=b[15]; M.at(27,84)=b[7]; M.at(27,88)=c[7]; M.at(27,91)=c[1]; M.at(27,94)=c[11]; M.at(27,96)=c[15]; M.at(27,107)=c[3]; M.at(27,110)=c[9]; M.at(27,119)=c[18]; - M.at(28,6)=u[3]; M.at(28,30)=a[11]; M.at(28,35)=a[7]; M.at(28,49)=a[15]; M.at(28,69)=b[1]; M.at(28,75)=b[11]; M.at(28,76)=b[15]; M.at(28,83)=b[7]; M.at(28,107)=c[11]; M.at(28,110)=c[1]; M.at(28,118)=c[7]; M.at(28,119)=c[15]; - M.at(29,24)=u[3]; M.at(29,29)=a[3]; M.at(29,30)=a[2]; M.at(29,38)=a[15]; M.at(29,39)=a[18]; M.at(29,52)=a[11]; M.at(29,53)=a[7]; M.at(29,69)=b[13]; M.at(29,70)=b[9]; M.at(29,73)=b[11]; M.at(29,74)=b[3]; M.at(29,75)=b[2]; M.at(29,78)=b[15]; M.at(29,79)=b[18]; M.at(29,82)=b[7]; M.at(29,87)=c[7]; M.at(29,91)=c[9]; M.at(29,92)=c[15]; M.at(29,93)=c[11]; M.at(29,94)=c[3]; M.at(29,96)=c[18]; M.at(29,107)=c[2]; M.at(29,110)=c[13]; - M.at(30,37)=a[18]; M.at(30,48)=a[7]; M.at(30,52)=a[2]; M.at(30,70)=b[20]; M.at(30,73)=b[2]; M.at(30,77)=b[18]; M.at(30,81)=b[7]; M.at(30,85)=c[7]; M.at(30,91)=c[20]; M.at(30,93)=c[2]; M.at(30,98)=c[18]; - M.at(31,29)=a[2]; M.at(31,37)=a[15]; M.at(31,38)=a[18]; M.at(31,50)=a[7]; M.at(31,52)=a[3]; M.at(31,69)=b[20]; M.at(31,70)=b[13]; M.at(31,73)=b[3]; M.at(31,74)=b[2]; M.at(31,77)=b[15]; M.at(31,78)=b[18]; M.at(31,80)=b[7]; M.at(31,86)=c[7]; M.at(31,91)=c[13]; M.at(31,92)=c[18]; M.at(31,93)=c[3]; M.at(31,94)=c[2]; M.at(31,98)=c[15]; M.at(31,110)=c[20]; - M.at(32,48)=a[9]; M.at(32,50)=a[13]; M.at(32,53)=a[20]; M.at(32,80)=b[13]; M.at(32,81)=b[9]; M.at(32,82)=b[20]; M.at(32,85)=c[9]; M.at(32,86)=c[13]; M.at(32,87)=c[20]; - M.at(33,29)=a[15]; M.at(33,30)=a[18]; M.at(33,39)=a[7]; M.at(33,64)=b[9]; M.at(33,69)=b[3]; M.at(33,70)=b[11]; M.at(33,74)=b[15]; M.at(33,75)=b[18]; M.at(33,79)=b[7]; M.at(33,91)=c[11]; M.at(33,94)=c[15]; M.at(33,96)=c[7]; M.at(33,103)=c[9]; M.at(33,107)=c[18]; M.at(33,110)=c[3]; - M.at(34,29)=a[18]; M.at(34,38)=a[7]; M.at(34,52)=a[15]; M.at(34,64)=b[13]; M.at(34,69)=b[2]; M.at(34,70)=b[3]; M.at(34,73)=b[15]; M.at(34,74)=b[18]; M.at(34,78)=b[7]; M.at(34,91)=c[3]; M.at(34,92)=c[7]; M.at(34,93)=c[15]; M.at(34,94)=c[18]; M.at(34,103)=c[13]; M.at(34,110)=c[2]; - M.at(35,37)=a[7]; M.at(35,52)=a[18]; M.at(35,64)=b[20]; M.at(35,70)=b[2]; M.at(35,73)=b[18]; M.at(35,77)=b[7]; M.at(35,91)=c[2]; M.at(35,93)=c[18]; M.at(35,98)=c[7]; M.at(35,103)=c[20]; - M.at(36,5)=u[4]; M.at(36,34)=a[12]; M.at(36,41)=a[10]; M.at(36,43)=a[14]; M.at(36,49)=a[16]; M.at(36,51)=a[5]; M.at(36,54)=b[12]; M.at(36,62)=b[10]; M.at(36,65)=b[5]; M.at(36,71)=b[14]; M.at(36,76)=b[16]; M.at(36,89)=c[5]; M.at(36,99)=c[12]; M.at(36,100)=c[14]; M.at(36,101)=c[1]; M.at(36,109)=c[11]; M.at(36,111)=c[10]; M.at(36,119)=c[16]; - M.at(37,2)=u[4]; M.at(37,34)=a[14]; M.at(37,35)=a[16]; M.at(37,51)=a[10]; M.at(37,54)=b[14]; M.at(37,65)=b[10]; M.at(37,83)=b[16]; M.at(37,89)=c[10]; M.at(37,99)=c[14]; M.at(37,109)=c[1]; M.at(37,118)=c[16]; - M.at(38,30)=a[15]; M.at(38,49)=a[7]; M.at(38,64)=b[1]; M.at(38,69)=b[11]; M.at(38,75)=b[15]; M.at(38,76)=b[7]; M.at(38,103)=c[1]; M.at(38,107)=c[15]; M.at(38,110)=c[11]; M.at(38,119)=c[7]; - M.at(39,28)=a[14]; M.at(39,33)=a[16]; M.at(39,51)=a[8]; M.at(39,57)=b[14]; M.at(39,59)=b[16]; M.at(39,65)=b[8]; M.at(39,89)=c[8]; M.at(39,105)=c[9]; M.at(39,108)=c[10]; M.at(39,109)=c[4]; M.at(39,114)=c[14]; M.at(39,117)=c[16]; - M.at(40,27)=a[14]; M.at(40,28)=a[8]; M.at(40,32)=a[16]; M.at(40,55)=b[14]; M.at(40,57)=b[8]; M.at(40,61)=b[16]; M.at(40,105)=c[13]; M.at(40,108)=c[4]; M.at(40,109)=c[19]; M.at(40,113)=c[14]; M.at(40,114)=c[8]; M.at(40,116)=c[16]; - M.at(41,30)=a[7]; M.at(41,64)=b[11]; M.at(41,69)=b[15]; M.at(41,75)=b[7]; M.at(41,103)=c[11]; M.at(41,107)=c[7]; M.at(41,110)=c[15]; - M.at(42,27)=a[8]; M.at(42,31)=a[16]; M.at(42,55)=b[8]; M.at(42,58)=b[16]; M.at(42,105)=c[20]; M.at(42,108)=c[19]; M.at(42,113)=c[8]; M.at(42,115)=c[16]; - M.at(43,29)=a[7]; M.at(43,64)=b[3]; M.at(43,69)=b[18]; M.at(43,70)=b[15]; M.at(43,74)=b[7]; M.at(43,91)=c[15]; M.at(43,94)=c[7]; M.at(43,103)=c[3]; M.at(43,110)=c[18]; - M.at(44,28)=a[16]; M.at(44,57)=b[16]; M.at(44,105)=c[4]; M.at(44,108)=c[14]; M.at(44,109)=c[8]; M.at(44,114)=c[16]; - M.at(45,27)=a[16]; M.at(45,55)=b[16]; M.at(45,105)=c[19]; M.at(45,108)=c[8]; M.at(45,113)=c[16]; - M.at(46,52)=a[7]; M.at(46,64)=b[2]; M.at(46,70)=b[18]; M.at(46,73)=b[7]; M.at(46,91)=c[18]; M.at(46,93)=c[7]; M.at(46,103)=c[2]; - M.at(47,40)=a[7]; M.at(47,44)=a[18]; M.at(47,52)=a[6]; M.at(47,64)=b[19]; M.at(47,67)=b[2]; M.at(47,68)=b[18]; M.at(47,70)=b[17]; M.at(47,72)=b[7]; M.at(47,73)=b[6]; M.at(47,91)=c[17]; M.at(47,93)=c[6]; M.at(47,95)=c[7]; M.at(47,97)=c[18]; M.at(47,103)=c[19]; M.at(47,104)=c[2]; - M.at(48,30)=a[6]; M.at(48,43)=a[7]; M.at(48,45)=a[15]; M.at(48,63)=b[15]; M.at(48,64)=b[10]; M.at(48,67)=b[11]; M.at(48,69)=b[5]; M.at(48,71)=b[7]; M.at(48,75)=b[6]; M.at(48,100)=c[7]; M.at(48,103)=c[10]; M.at(48,104)=c[11]; M.at(48,107)=c[6]; M.at(48,110)=c[5]; M.at(48,112)=c[15]; - M.at(49,41)=a[12]; M.at(49,45)=a[16]; M.at(49,46)=a[14]; M.at(49,62)=b[12]; M.at(49,63)=b[16]; M.at(49,66)=b[14]; M.at(49,101)=c[5]; M.at(49,102)=c[14]; M.at(49,105)=c[15]; M.at(49,109)=c[6]; M.at(49,111)=c[12]; M.at(49,112)=c[16]; - M.at(50,41)=a[16]; M.at(50,62)=b[16]; M.at(50,101)=c[14]; M.at(50,105)=c[5]; M.at(50,109)=c[12]; M.at(50,111)=c[16]; - M.at(51,64)=b[18]; M.at(51,70)=b[7]; M.at(51,91)=c[7]; M.at(51,103)=c[18]; - M.at(52,41)=a[6]; M.at(52,45)=a[12]; M.at(52,46)=a[5]; M.at(52,62)=b[6]; M.at(52,63)=b[12]; M.at(52,66)=b[5]; M.at(52,67)=b[14]; M.at(52,69)=b[16]; M.at(52,101)=c[15]; M.at(52,102)=c[5]; M.at(52,104)=c[14]; M.at(52,109)=c[7]; M.at(52,110)=c[16]; M.at(52,111)=c[6]; M.at(52,112)=c[12]; - M.at(53,64)=b[15]; M.at(53,69)=b[7]; M.at(53,103)=c[15]; M.at(53,110)=c[7]; - M.at(54,105)=c[14]; M.at(54,109)=c[16]; - M.at(55,44)=a[7]; M.at(55,64)=b[17]; M.at(55,67)=b[18]; M.at(55,68)=b[7]; M.at(55,70)=b[6]; M.at(55,91)=c[6]; M.at(55,97)=c[7]; M.at(55,103)=c[17]; M.at(55,104)=c[18]; - M.at(56,105)=c[8]; M.at(56,108)=c[16]; - M.at(57,64)=b[6]; M.at(57,67)=b[7]; M.at(57,103)=c[6]; M.at(57,104)=c[7]; - M.at(58,46)=a[7]; M.at(58,64)=b[12]; M.at(58,66)=b[7]; M.at(58,67)=b[6]; M.at(58,102)=c[7]; M.at(58,103)=c[12]; M.at(58,104)=c[6]; - M.at(59,8)=u[4]; M.at(59,30)=a[16]; M.at(59,41)=a[5]; M.at(59,43)=a[12]; M.at(59,45)=a[14]; M.at(59,46)=a[10]; M.at(59,51)=a[6]; M.at(59,62)=b[5]; M.at(59,63)=b[14]; M.at(59,65)=b[6]; M.at(59,66)=b[10]; M.at(59,71)=b[12]; M.at(59,75)=b[16]; M.at(59,89)=c[6]; M.at(59,100)=c[12]; M.at(59,101)=c[11]; M.at(59,102)=c[10]; M.at(59,107)=c[16]; M.at(59,109)=c[15]; M.at(59,111)=c[5]; M.at(59,112)=c[14]; - M.at(60,8)=u[3]; M.at(60,30)=a[12]; M.at(60,41)=a[15]; M.at(60,43)=a[6]; M.at(60,45)=a[5]; M.at(60,46)=a[11]; M.at(60,51)=a[7]; M.at(60,62)=b[15]; M.at(60,63)=b[5]; M.at(60,65)=b[7]; M.at(60,66)=b[11]; M.at(60,67)=b[10]; M.at(60,69)=b[14]; M.at(60,71)=b[6]; M.at(60,75)=b[12]; M.at(60,89)=c[7]; M.at(60,100)=c[6]; M.at(60,102)=c[11]; M.at(60,104)=c[10]; M.at(60,107)=c[12]; M.at(60,110)=c[14]; M.at(60,111)=c[15]; M.at(60,112)=c[5]; - M.at(61,42)=a[16]; M.at(61,56)=b[16]; M.at(61,101)=c[8]; M.at(61,105)=c[17]; M.at(61,106)=c[16]; M.at(61,108)=c[12]; - M.at(62,64)=b[7]; M.at(62,103)=c[7]; - M.at(63,105)=c[16]; - M.at(64,46)=a[12]; M.at(64,66)=b[12]; M.at(64,67)=b[16]; M.at(64,101)=c[6]; M.at(64,102)=c[12]; M.at(64,104)=c[16]; M.at(64,105)=c[7]; - M.at(65,46)=a[6]; M.at(65,64)=b[16]; M.at(65,66)=b[6]; M.at(65,67)=b[12]; M.at(65,101)=c[7]; M.at(65,102)=c[6]; M.at(65,103)=c[16]; M.at(65,104)=c[12]; - M.at(66,46)=a[16]; M.at(66,66)=b[16]; M.at(66,101)=c[12]; M.at(66,102)=c[16]; M.at(66,105)=c[6]; - M.at(67,101)=c[16]; M.at(67,105)=c[12]; - M.at(68,41)=a[14]; M.at(68,43)=a[16]; M.at(68,51)=a[12]; M.at(68,62)=b[14]; M.at(68,65)=b[12]; M.at(68,71)=b[16]; M.at(68,89)=c[12]; M.at(68,100)=c[16]; M.at(68,101)=c[10]; M.at(68,105)=c[11]; M.at(68,109)=c[5]; M.at(68,111)=c[14]; - M.at(69,37)=a[2]; M.at(69,48)=a[18]; M.at(69,52)=a[20]; M.at(69,73)=b[20]; M.at(69,77)=b[2]; M.at(69,81)=b[18]; M.at(69,85)=c[18]; M.at(69,93)=c[20]; M.at(69,98)=c[2]; - M.at(70,20)=u[2]; M.at(70,27)=a[9]; M.at(70,28)=a[13]; M.at(70,31)=a[10]; M.at(70,32)=a[4]; M.at(70,33)=a[19]; M.at(70,50)=a[14]; M.at(70,51)=a[20]; M.at(70,53)=a[8]; M.at(70,55)=b[9]; M.at(70,57)=b[13]; M.at(70,58)=b[10]; M.at(70,59)=b[19]; M.at(70,61)=b[4]; M.at(70,65)=b[20]; M.at(70,80)=b[14]; M.at(70,82)=b[8]; M.at(70,86)=c[14]; M.at(70,87)=c[8]; M.at(70,89)=c[20]; M.at(70,113)=c[9]; M.at(70,114)=c[13]; M.at(70,115)=c[10]; M.at(70,116)=c[4]; M.at(70,117)=c[19]; - M.at(71,45)=a[7]; M.at(71,63)=b[7]; M.at(71,64)=b[5]; M.at(71,67)=b[15]; M.at(71,69)=b[6]; M.at(71,103)=c[5]; M.at(71,104)=c[15]; M.at(71,110)=c[6]; M.at(71,112)=c[7]; - M.at(72,41)=a[7]; M.at(72,45)=a[6]; M.at(72,46)=a[15]; M.at(72,62)=b[7]; M.at(72,63)=b[6]; M.at(72,64)=b[14]; M.at(72,66)=b[15]; M.at(72,67)=b[5]; M.at(72,69)=b[12]; M.at(72,102)=c[15]; M.at(72,103)=c[14]; M.at(72,104)=c[5]; M.at(72,110)=c[12]; M.at(72,111)=c[7]; M.at(72,112)=c[6]; - M.at(73,48)=a[13]; M.at(73,50)=a[20]; M.at(73,80)=b[20]; M.at(73,81)=b[13]; M.at(73,85)=c[13]; M.at(73,86)=c[20]; - M.at(74,25)=u[3]; M.at(74,29)=a[17]; M.at(74,32)=a[7]; M.at(74,38)=a[6]; M.at(74,40)=a[15]; M.at(74,44)=a[3]; M.at(74,45)=a[2]; M.at(74,47)=a[18]; M.at(74,52)=a[5]; M.at(74,60)=b[18]; M.at(74,61)=b[7]; M.at(74,63)=b[2]; M.at(74,67)=b[13]; M.at(74,68)=b[3]; M.at(74,69)=b[19]; M.at(74,70)=b[4]; M.at(74,72)=b[15]; M.at(74,73)=b[5]; M.at(74,74)=b[17]; M.at(74,78)=b[6]; M.at(74,90)=c[18]; M.at(74,91)=c[4]; M.at(74,92)=c[6]; M.at(74,93)=c[5]; M.at(74,94)=c[17]; M.at(74,95)=c[15]; M.at(74,97)=c[3]; M.at(74,104)=c[13]; M.at(74,110)=c[19]; M.at(74,112)=c[2]; M.at(74,116)=c[7]; - M.at(75,21)=u[2]; M.at(75,36)=a[2]; M.at(75,37)=a[1]; M.at(75,38)=a[9]; M.at(75,39)=a[13]; M.at(75,49)=a[20]; M.at(75,50)=a[11]; M.at(75,53)=a[3]; M.at(75,76)=b[20]; M.at(75,77)=b[1]; M.at(75,78)=b[9]; M.at(75,79)=b[13]; M.at(75,80)=b[11]; M.at(75,82)=b[3]; M.at(75,84)=b[2]; M.at(75,86)=c[11]; M.at(75,87)=c[3]; M.at(75,88)=c[2]; M.at(75,92)=c[9]; M.at(75,96)=c[13]; M.at(75,98)=c[1]; M.at(75,119)=c[20]; - M.at(76,48)=a[20]; M.at(76,81)=b[20]; M.at(76,85)=c[20]; - M.at(77,34)=a[16]; M.at(77,51)=a[14]; M.at(77,54)=b[16]; M.at(77,65)=b[14]; M.at(77,89)=c[14]; M.at(77,99)=c[16]; M.at(77,105)=c[1]; M.at(77,109)=c[10]; - M.at(78,27)=a[17]; M.at(78,31)=a[12]; M.at(78,37)=a[16]; M.at(78,40)=a[8]; M.at(78,42)=a[19]; M.at(78,55)=b[17]; M.at(78,56)=b[19]; M.at(78,58)=b[12]; M.at(78,72)=b[8]; M.at(78,77)=b[16]; M.at(78,95)=c[8]; M.at(78,98)=c[16]; M.at(78,101)=c[20]; M.at(78,106)=c[19]; M.at(78,108)=c[2]; M.at(78,113)=c[17]; M.at(78,115)=c[12]; - M.at(79,42)=a[12]; M.at(79,44)=a[16]; M.at(79,46)=a[8]; M.at(79,56)=b[12]; M.at(79,66)=b[8]; M.at(79,68)=b[16]; M.at(79,97)=c[16]; M.at(79,101)=c[17]; M.at(79,102)=c[8]; M.at(79,105)=c[18]; M.at(79,106)=c[12]; M.at(79,108)=c[6]; - M.at(80,14)=u[4]; M.at(80,28)=a[5]; M.at(80,33)=a[12]; M.at(80,39)=a[16]; M.at(80,41)=a[4]; M.at(80,42)=a[10]; M.at(80,43)=a[8]; M.at(80,47)=a[14]; M.at(80,51)=a[17]; M.at(80,56)=b[10]; M.at(80,57)=b[5]; M.at(80,59)=b[12]; M.at(80,60)=b[14]; M.at(80,62)=b[4]; M.at(80,65)=b[17]; M.at(80,71)=b[8]; M.at(80,79)=b[16]; M.at(80,89)=c[17]; M.at(80,90)=c[14]; M.at(80,96)=c[16]; M.at(80,100)=c[8]; M.at(80,101)=c[9]; M.at(80,106)=c[10]; M.at(80,108)=c[11]; M.at(80,109)=c[3]; M.at(80,111)=c[4]; M.at(80,114)=c[5]; M.at(80,117)=c[12]; - M.at(81,31)=a[3]; M.at(81,32)=a[2]; M.at(81,37)=a[4]; M.at(81,38)=a[19]; M.at(81,40)=a[13]; M.at(81,47)=a[20]; M.at(81,48)=a[5]; M.at(81,50)=a[17]; M.at(81,58)=b[3]; M.at(81,60)=b[20]; M.at(81,61)=b[2]; M.at(81,72)=b[13]; M.at(81,77)=b[4]; M.at(81,78)=b[19]; M.at(81,80)=b[17]; M.at(81,81)=b[5]; M.at(81,85)=c[5]; M.at(81,86)=c[17]; M.at(81,90)=c[20]; M.at(81,92)=c[19]; M.at(81,95)=c[13]; M.at(81,98)=c[4]; M.at(81,115)=c[3]; M.at(81,116)=c[2]; - M.at(82,29)=a[6]; M.at(82,44)=a[15]; M.at(82,45)=a[18]; M.at(82,47)=a[7]; M.at(82,60)=b[7]; M.at(82,63)=b[18]; M.at(82,64)=b[4]; M.at(82,67)=b[3]; M.at(82,68)=b[15]; M.at(82,69)=b[17]; M.at(82,70)=b[5]; M.at(82,74)=b[6]; M.at(82,90)=c[7]; M.at(82,91)=c[5]; M.at(82,94)=c[6]; M.at(82,97)=c[15]; M.at(82,103)=c[4]; M.at(82,104)=c[3]; M.at(82,110)=c[17]; M.at(82,112)=c[18]; - M.at(83,26)=u[2]; M.at(83,27)=a[18]; M.at(83,31)=a[6]; M.at(83,37)=a[12]; M.at(83,40)=a[17]; M.at(83,42)=a[2]; M.at(83,44)=a[19]; M.at(83,46)=a[20]; M.at(83,52)=a[8]; M.at(83,55)=b[18]; M.at(83,56)=b[2]; M.at(83,58)=b[6]; M.at(83,66)=b[20]; M.at(83,68)=b[19]; M.at(83,72)=b[17]; M.at(83,73)=b[8]; M.at(83,77)=b[12]; M.at(83,93)=c[8]; M.at(83,95)=c[17]; M.at(83,97)=c[19]; M.at(83,98)=c[12]; M.at(83,102)=c[20]; M.at(83,106)=c[2]; M.at(83,113)=c[18]; M.at(83,115)=c[6]; - M.at(84,16)=u[3]; M.at(84,29)=a[5]; M.at(84,30)=a[17]; M.at(84,33)=a[7]; M.at(84,39)=a[6]; M.at(84,43)=a[18]; M.at(84,44)=a[11]; M.at(84,45)=a[3]; M.at(84,47)=a[15]; M.at(84,59)=b[7]; M.at(84,60)=b[15]; M.at(84,63)=b[3]; M.at(84,67)=b[9]; M.at(84,68)=b[11]; M.at(84,69)=b[4]; M.at(84,70)=b[10]; M.at(84,71)=b[18]; M.at(84,74)=b[5]; M.at(84,75)=b[17]; M.at(84,79)=b[6]; M.at(84,90)=c[15]; M.at(84,91)=c[10]; M.at(84,94)=c[5]; M.at(84,96)=c[6]; M.at(84,97)=c[11]; M.at(84,100)=c[18]; M.at(84,104)=c[9]; M.at(84,107)=c[17]; M.at(84,110)=c[4]; M.at(84,112)=c[3]; M.at(84,117)=c[7]; - M.at(85,25)=u[2]; M.at(85,29)=a[19]; M.at(85,31)=a[15]; M.at(85,32)=a[18]; M.at(85,37)=a[5]; M.at(85,38)=a[17]; M.at(85,40)=a[3]; M.at(85,44)=a[13]; M.at(85,45)=a[20]; M.at(85,47)=a[2]; M.at(85,50)=a[6]; M.at(85,52)=a[4]; M.at(85,58)=b[15]; M.at(85,60)=b[2]; M.at(85,61)=b[18]; M.at(85,63)=b[20]; M.at(85,68)=b[13]; M.at(85,72)=b[3]; M.at(85,73)=b[4]; M.at(85,74)=b[19]; M.at(85,77)=b[5]; M.at(85,78)=b[17]; M.at(85,80)=b[6]; M.at(85,86)=c[6]; M.at(85,90)=c[2]; M.at(85,92)=c[17]; M.at(85,93)=c[4]; M.at(85,94)=c[19]; M.at(85,95)=c[3]; M.at(85,97)=c[13]; M.at(85,98)=c[5]; M.at(85,112)=c[20]; M.at(85,115)=c[15]; M.at(85,116)=c[18]; - M.at(86,31)=a[18]; M.at(86,37)=a[17]; M.at(86,40)=a[2]; M.at(86,44)=a[20]; M.at(86,48)=a[6]; M.at(86,52)=a[19]; M.at(86,58)=b[18]; M.at(86,68)=b[20]; M.at(86,72)=b[2]; M.at(86,73)=b[19]; M.at(86,77)=b[17]; M.at(86,81)=b[6]; M.at(86,85)=c[6]; M.at(86,93)=c[19]; M.at(86,95)=c[2]; M.at(86,97)=c[20]; M.at(86,98)=c[17]; M.at(86,115)=c[18]; - M.at(87,22)=u[2]; M.at(87,31)=a[11]; M.at(87,32)=a[3]; M.at(87,33)=a[2]; M.at(87,37)=a[10]; M.at(87,38)=a[4]; M.at(87,39)=a[19]; M.at(87,40)=a[9]; M.at(87,43)=a[20]; M.at(87,47)=a[13]; M.at(87,50)=a[5]; M.at(87,53)=a[17]; M.at(87,58)=b[11]; M.at(87,59)=b[2]; M.at(87,60)=b[13]; M.at(87,61)=b[3]; M.at(87,71)=b[20]; M.at(87,72)=b[9]; M.at(87,77)=b[10]; M.at(87,78)=b[4]; M.at(87,79)=b[19]; M.at(87,80)=b[5]; M.at(87,82)=b[17]; M.at(87,86)=c[5]; M.at(87,87)=c[17]; M.at(87,90)=c[13]; M.at(87,92)=c[4]; M.at(87,95)=c[9]; M.at(87,96)=c[19]; M.at(87,98)=c[10]; M.at(87,100)=c[20]; M.at(87,115)=c[11]; M.at(87,116)=c[3]; M.at(87,117)=c[2]; - M.at(88,27)=a[2]; M.at(88,31)=a[17]; M.at(88,37)=a[8]; M.at(88,40)=a[19]; M.at(88,42)=a[20]; M.at(88,48)=a[12]; M.at(88,55)=b[2]; M.at(88,56)=b[20]; M.at(88,58)=b[17]; M.at(88,72)=b[19]; M.at(88,77)=b[8]; M.at(88,81)=b[12]; M.at(88,85)=c[12]; M.at(88,95)=c[19]; M.at(88,98)=c[8]; M.at(88,106)=c[20]; M.at(88,113)=c[2]; M.at(88,115)=c[17]; - M.at(89,31)=a[7]; M.at(89,37)=a[6]; M.at(89,40)=a[18]; M.at(89,44)=a[2]; M.at(89,52)=a[17]; M.at(89,58)=b[7]; M.at(89,67)=b[20]; M.at(89,68)=b[2]; M.at(89,70)=b[19]; M.at(89,72)=b[18]; M.at(89,73)=b[17]; M.at(89,77)=b[6]; M.at(89,91)=c[19]; M.at(89,93)=c[17]; M.at(89,95)=c[18]; M.at(89,97)=c[2]; M.at(89,98)=c[6]; M.at(89,104)=c[20]; M.at(89,115)=c[7]; - M.at(90,27)=a[12]; M.at(90,40)=a[16]; M.at(90,42)=a[8]; M.at(90,55)=b[12]; M.at(90,56)=b[8]; M.at(90,72)=b[16]; M.at(90,95)=c[16]; M.at(90,101)=c[19]; M.at(90,105)=c[2]; M.at(90,106)=c[8]; M.at(90,108)=c[17]; M.at(90,113)=c[12]; - M.at(91,23)=u[2]; M.at(91,27)=a[3]; M.at(91,28)=a[2]; M.at(91,31)=a[5]; M.at(91,32)=a[17]; M.at(91,37)=a[14]; M.at(91,38)=a[8]; M.at(91,40)=a[4]; M.at(91,41)=a[20]; M.at(91,42)=a[13]; M.at(91,47)=a[19]; M.at(91,50)=a[12]; M.at(91,55)=b[3]; M.at(91,56)=b[13]; M.at(91,57)=b[2]; M.at(91,58)=b[5]; M.at(91,60)=b[19]; M.at(91,61)=b[17]; M.at(91,62)=b[20]; M.at(91,72)=b[4]; M.at(91,77)=b[14]; M.at(91,78)=b[8]; M.at(91,80)=b[12]; M.at(91,86)=c[12]; M.at(91,90)=c[19]; M.at(91,92)=c[8]; M.at(91,95)=c[4]; M.at(91,98)=c[14]; M.at(91,106)=c[13]; M.at(91,111)=c[20]; M.at(91,113)=c[3]; M.at(91,114)=c[2]; M.at(91,115)=c[5]; M.at(91,116)=c[17]; - M.at(92,17)=u[4]; M.at(92,28)=a[6]; M.at(92,29)=a[16]; M.at(92,41)=a[17]; M.at(92,42)=a[5]; M.at(92,44)=a[14]; M.at(92,45)=a[8]; M.at(92,46)=a[4]; M.at(92,47)=a[12]; M.at(92,56)=b[5]; M.at(92,57)=b[6]; M.at(92,60)=b[12]; M.at(92,62)=b[17]; M.at(92,63)=b[8]; M.at(92,66)=b[4]; M.at(92,68)=b[14]; M.at(92,74)=b[16]; M.at(92,90)=c[12]; M.at(92,94)=c[16]; M.at(92,97)=c[14]; M.at(92,101)=c[3]; M.at(92,102)=c[4]; M.at(92,106)=c[5]; M.at(92,108)=c[15]; M.at(92,109)=c[18]; M.at(92,111)=c[17]; M.at(92,112)=c[8]; M.at(92,114)=c[6]; - M.at(93,17)=u[3]; M.at(93,28)=a[7]; M.at(93,29)=a[12]; M.at(93,41)=a[18]; M.at(93,42)=a[15]; M.at(93,44)=a[5]; M.at(93,45)=a[17]; M.at(93,46)=a[3]; M.at(93,47)=a[6]; M.at(93,56)=b[15]; M.at(93,57)=b[7]; M.at(93,60)=b[6]; M.at(93,62)=b[18]; M.at(93,63)=b[17]; M.at(93,66)=b[3]; M.at(93,67)=b[4]; M.at(93,68)=b[5]; M.at(93,69)=b[8]; M.at(93,70)=b[14]; M.at(93,74)=b[12]; M.at(93,90)=c[6]; M.at(93,91)=c[14]; M.at(93,94)=c[12]; M.at(93,97)=c[5]; M.at(93,102)=c[3]; M.at(93,104)=c[4]; M.at(93,106)=c[15]; M.at(93,110)=c[8]; M.at(93,111)=c[18]; M.at(93,112)=c[17]; M.at(93,114)=c[7]; - M.at(94,31)=a[2]; M.at(94,37)=a[19]; M.at(94,40)=a[20]; M.at(94,48)=a[17]; M.at(94,58)=b[2]; M.at(94,72)=b[20]; M.at(94,77)=b[19]; M.at(94,81)=b[17]; M.at(94,85)=c[17]; M.at(94,95)=c[20]; M.at(94,98)=c[19]; M.at(94,115)=c[2]; - M.at(95,26)=u[4]; M.at(95,27)=a[6]; M.at(95,40)=a[12]; M.at(95,42)=a[17]; M.at(95,44)=a[8]; M.at(95,46)=a[19]; M.at(95,52)=a[16]; M.at(95,55)=b[6]; M.at(95,56)=b[17]; M.at(95,66)=b[19]; M.at(95,68)=b[8]; M.at(95,72)=b[12]; M.at(95,73)=b[16]; M.at(95,93)=c[16]; M.at(95,95)=c[12]; M.at(95,97)=c[8]; M.at(95,101)=c[2]; M.at(95,102)=c[19]; M.at(95,106)=c[17]; M.at(95,108)=c[18]; M.at(95,113)=c[6]; - M.at(96,23)=u[4]; M.at(96,27)=a[5]; M.at(96,28)=a[17]; M.at(96,32)=a[12]; M.at(96,38)=a[16]; M.at(96,40)=a[14]; M.at(96,41)=a[19]; M.at(96,42)=a[4]; M.at(96,47)=a[8]; M.at(96,55)=b[5]; M.at(96,56)=b[4]; M.at(96,57)=b[17]; M.at(96,60)=b[8]; M.at(96,61)=b[12]; M.at(96,62)=b[19]; M.at(96,72)=b[14]; M.at(96,78)=b[16]; M.at(96,90)=c[8]; M.at(96,92)=c[16]; M.at(96,95)=c[14]; M.at(96,101)=c[13]; M.at(96,106)=c[4]; M.at(96,108)=c[3]; M.at(96,109)=c[2]; M.at(96,111)=c[19]; M.at(96,113)=c[5]; M.at(96,114)=c[17]; M.at(96,116)=c[12]; - M.at(97,42)=a[6]; M.at(97,44)=a[12]; M.at(97,46)=a[17]; M.at(97,56)=b[6]; M.at(97,66)=b[17]; M.at(97,67)=b[8]; M.at(97,68)=b[12]; M.at(97,70)=b[16]; M.at(97,91)=c[16]; M.at(97,97)=c[12]; M.at(97,101)=c[18]; M.at(97,102)=c[17]; M.at(97,104)=c[8]; M.at(97,106)=c[6]; M.at(97,108)=c[7]; - M.at(98,28)=a[12]; M.at(98,41)=a[8]; M.at(98,42)=a[14]; M.at(98,47)=a[16]; M.at(98,56)=b[14]; M.at(98,57)=b[12]; M.at(98,60)=b[16]; M.at(98,62)=b[8]; M.at(98,90)=c[16]; M.at(98,101)=c[4]; M.at(98,105)=c[3]; M.at(98,106)=c[14]; M.at(98,108)=c[5]; M.at(98,109)=c[17]; M.at(98,111)=c[8]; M.at(98,114)=c[12]; - M.at(99,42)=a[7]; M.at(99,44)=a[6]; M.at(99,46)=a[18]; M.at(99,56)=b[7]; M.at(99,64)=b[8]; M.at(99,66)=b[18]; M.at(99,67)=b[17]; M.at(99,68)=b[6]; M.at(99,70)=b[12]; M.at(99,91)=c[12]; M.at(99,97)=c[6]; M.at(99,102)=c[18]; M.at(99,103)=c[8]; M.at(99,104)=c[17]; M.at(99,106)=c[7]; - M.at(100,51)=a[16]; M.at(100,65)=b[16]; M.at(100,89)=c[16]; M.at(100,105)=c[10]; M.at(100,109)=c[14]; - M.at(101,37)=a[9]; M.at(101,38)=a[13]; M.at(101,39)=a[20]; M.at(101,48)=a[11]; M.at(101,50)=a[3]; M.at(101,53)=a[2]; M.at(101,77)=b[9]; M.at(101,78)=b[13]; M.at(101,79)=b[20]; M.at(101,80)=b[3]; M.at(101,81)=b[11]; M.at(101,82)=b[2]; M.at(101,85)=c[11]; M.at(101,86)=c[3]; M.at(101,87)=c[2]; M.at(101,92)=c[13]; M.at(101,96)=c[20]; M.at(101,98)=c[9]; - M.at(102,37)=a[13]; M.at(102,38)=a[20]; M.at(102,48)=a[3]; M.at(102,50)=a[2]; M.at(102,77)=b[13]; M.at(102,78)=b[20]; M.at(102,80)=b[2]; M.at(102,81)=b[3]; M.at(102,85)=c[3]; M.at(102,86)=c[2]; M.at(102,92)=c[20]; M.at(102,98)=c[13]; - M.at(103,37)=a[20]; M.at(103,48)=a[2]; M.at(103,77)=b[20]; M.at(103,81)=b[2]; M.at(103,85)=c[2]; M.at(103,98)=c[20]; - M.at(104,11)=u[4]; M.at(104,28)=a[10]; M.at(104,33)=a[14]; M.at(104,34)=a[8]; M.at(104,36)=a[16]; M.at(104,51)=a[4]; M.at(104,54)=b[8]; M.at(104,57)=b[10]; M.at(104,59)=b[14]; M.at(104,65)=b[4]; M.at(104,84)=b[16]; M.at(104,88)=c[16]; M.at(104,89)=c[4]; M.at(104,99)=c[8]; M.at(104,108)=c[1]; M.at(104,109)=c[9]; M.at(104,114)=c[10]; M.at(104,117)=c[14]; - M.at(105,20)=u[4]; M.at(105,27)=a[10]; M.at(105,28)=a[4]; M.at(105,32)=a[14]; M.at(105,33)=a[8]; M.at(105,51)=a[19]; M.at(105,53)=a[16]; M.at(105,55)=b[10]; M.at(105,57)=b[4]; M.at(105,59)=b[8]; M.at(105,61)=b[14]; M.at(105,65)=b[19]; M.at(105,82)=b[16]; M.at(105,87)=c[16]; M.at(105,89)=c[19]; M.at(105,108)=c[9]; M.at(105,109)=c[13]; M.at(105,113)=c[10]; M.at(105,114)=c[4]; M.at(105,116)=c[14]; M.at(105,117)=c[8]; - M.at(106,27)=a[4]; M.at(106,28)=a[19]; M.at(106,31)=a[14]; M.at(106,32)=a[8]; M.at(106,50)=a[16]; M.at(106,55)=b[4]; M.at(106,57)=b[19]; M.at(106,58)=b[14]; M.at(106,61)=b[8]; M.at(106,80)=b[16]; M.at(106,86)=c[16]; M.at(106,108)=c[13]; M.at(106,109)=c[20]; M.at(106,113)=c[4]; M.at(106,114)=c[19]; M.at(106,115)=c[14]; M.at(106,116)=c[8]; - M.at(107,27)=a[19]; M.at(107,31)=a[8]; M.at(107,48)=a[16]; M.at(107,55)=b[19]; M.at(107,58)=b[8]; M.at(107,81)=b[16]; M.at(107,85)=c[16]; M.at(107,108)=c[20]; M.at(107,113)=c[19]; M.at(107,115)=c[8]; - M.at(108,36)=a[20]; M.at(108,48)=a[1]; M.at(108,50)=a[9]; M.at(108,53)=a[13]; M.at(108,80)=b[9]; M.at(108,81)=b[1]; M.at(108,82)=b[13]; M.at(108,84)=b[20]; M.at(108,85)=c[1]; M.at(108,86)=c[9]; M.at(108,87)=c[13]; M.at(108,88)=c[20]; - M.at(109,26)=u[3]; M.at(109,27)=a[7]; M.at(109,40)=a[6]; M.at(109,42)=a[18]; M.at(109,44)=a[17]; M.at(109,46)=a[2]; M.at(109,52)=a[12]; M.at(109,55)=b[7]; M.at(109,56)=b[18]; M.at(109,66)=b[2]; M.at(109,67)=b[19]; M.at(109,68)=b[17]; M.at(109,70)=b[8]; M.at(109,72)=b[6]; M.at(109,73)=b[12]; M.at(109,91)=c[8]; M.at(109,93)=c[12]; M.at(109,95)=c[6]; M.at(109,97)=c[17]; M.at(109,102)=c[2]; M.at(109,104)=c[19]; M.at(109,106)=c[18]; M.at(109,113)=c[7]; - M.at(110,7)=u[3]; M.at(110,30)=a[5]; M.at(110,34)=a[7]; M.at(110,43)=a[15]; M.at(110,45)=a[11]; M.at(110,49)=a[6]; M.at(110,54)=b[7]; M.at(110,63)=b[11]; M.at(110,67)=b[1]; M.at(110,69)=b[10]; M.at(110,71)=b[15]; M.at(110,75)=b[5]; M.at(110,76)=b[6]; M.at(110,99)=c[7]; M.at(110,100)=c[15]; M.at(110,104)=c[1]; M.at(110,107)=c[5]; M.at(110,110)=c[10]; M.at(110,112)=c[11]; M.at(110,119)=c[6]; - M.at(111,18)=u[2]; M.at(111,35)=a[20]; M.at(111,36)=a[13]; M.at(111,50)=a[1]; M.at(111,53)=a[9]; M.at(111,80)=b[1]; M.at(111,82)=b[9]; M.at(111,83)=b[20]; M.at(111,84)=b[13]; M.at(111,86)=c[1]; M.at(111,87)=c[9]; M.at(111,88)=c[13]; M.at(111,118)=c[20]; - M.at(112,19)=u[2]; M.at(112,31)=a[1]; M.at(112,32)=a[9]; M.at(112,33)=a[13]; M.at(112,34)=a[20]; M.at(112,36)=a[19]; M.at(112,50)=a[10]; M.at(112,53)=a[4]; M.at(112,54)=b[20]; M.at(112,58)=b[1]; M.at(112,59)=b[13]; M.at(112,61)=b[9]; M.at(112,80)=b[10]; M.at(112,82)=b[4]; M.at(112,84)=b[19]; M.at(112,86)=c[10]; M.at(112,87)=c[4]; M.at(112,88)=c[19]; M.at(112,99)=c[20]; M.at(112,115)=c[1]; M.at(112,116)=c[9]; M.at(112,117)=c[13]; - M.at(113,31)=a[9]; M.at(113,32)=a[13]; M.at(113,33)=a[20]; M.at(113,48)=a[10]; M.at(113,50)=a[4]; M.at(113,53)=a[19]; M.at(113,58)=b[9]; M.at(113,59)=b[20]; M.at(113,61)=b[13]; M.at(113,80)=b[4]; M.at(113,81)=b[10]; M.at(113,82)=b[19]; M.at(113,85)=c[10]; M.at(113,86)=c[4]; M.at(113,87)=c[19]; M.at(113,115)=c[9]; M.at(113,116)=c[13]; M.at(113,117)=c[20]; - M.at(114,31)=a[13]; M.at(114,32)=a[20]; M.at(114,48)=a[4]; M.at(114,50)=a[19]; M.at(114,58)=b[13]; M.at(114,61)=b[20]; M.at(114,80)=b[19]; M.at(114,81)=b[4]; M.at(114,85)=c[4]; M.at(114,86)=c[19]; M.at(114,115)=c[13]; M.at(114,116)=c[20]; - M.at(115,31)=a[20]; M.at(115,48)=a[19]; M.at(115,58)=b[20]; M.at(115,81)=b[19]; M.at(115,85)=c[19]; M.at(115,115)=c[20]; - M.at(116,24)=u[2]; M.at(116,29)=a[13]; M.at(116,30)=a[20]; M.at(116,37)=a[11]; M.at(116,38)=a[3]; M.at(116,39)=a[2]; M.at(116,50)=a[15]; M.at(116,52)=a[9]; M.at(116,53)=a[18]; M.at(116,73)=b[9]; M.at(116,74)=b[13]; M.at(116,75)=b[20]; M.at(116,77)=b[11]; M.at(116,78)=b[3]; M.at(116,79)=b[2]; M.at(116,80)=b[15]; M.at(116,82)=b[18]; M.at(116,86)=c[15]; M.at(116,87)=c[18]; M.at(116,92)=c[3]; M.at(116,93)=c[9]; M.at(116,94)=c[13]; M.at(116,96)=c[2]; M.at(116,98)=c[11]; M.at(116,107)=c[20]; - M.at(117,29)=a[20]; M.at(117,37)=a[3]; M.at(117,38)=a[2]; M.at(117,48)=a[15]; M.at(117,50)=a[18]; M.at(117,52)=a[13]; M.at(117,73)=b[13]; M.at(117,74)=b[20]; M.at(117,77)=b[3]; M.at(117,78)=b[2]; M.at(117,80)=b[18]; M.at(117,81)=b[15]; M.at(117,85)=c[15]; M.at(117,86)=c[18]; M.at(117,92)=c[2]; M.at(117,93)=c[13]; M.at(117,94)=c[20]; M.at(117,98)=c[3]; - M.at(118,27)=a[13]; M.at(118,28)=a[20]; M.at(118,31)=a[4]; M.at(118,32)=a[19]; M.at(118,48)=a[14]; M.at(118,50)=a[8]; M.at(118,55)=b[13]; M.at(118,57)=b[20]; M.at(118,58)=b[4]; M.at(118,61)=b[19]; M.at(118,80)=b[8]; M.at(118,81)=b[14]; M.at(118,85)=c[14]; M.at(118,86)=c[8]; M.at(118,113)=c[13]; M.at(118,114)=c[20]; M.at(118,115)=c[4]; M.at(118,116)=c[19]; - M.at(119,27)=a[20]; M.at(119,31)=a[19]; M.at(119,48)=a[8]; M.at(119,55)=b[20]; M.at(119,58)=b[19]; M.at(119,81)=b[8]; M.at(119,85)=c[8]; M.at(119,113)=c[20]; M.at(119,115)=c[19]; - - return M.t(); -} - -Mat dls::Hessian(const double s[]) -{ - // the vector of monomials is - // m = [ const ; s1^2 * s2 ; s1 * s2 ; s1 * s3 ; s2 * s3 ; s2^2 * s3 ; s2^3 ; ... - // s1 * s3^2 ; s1 ; s3 ; s2 ; s2 * s3^2 ; s1^2 ; s3^2 ; s2^2 ; s3^3 ; ... - // s1 * s2 * s3 ; s1 * s2^2 ; s1^2 * s3 ; s1^3] - // - - // deriv of m w.r.t. s1 - //Hs3 = [0 ; 2 * s(1) * s(2) ; s(2) ; s(3) ; 0 ; 0 ; 0 ; ... - // s(3)^2 ; 1 ; 0 ; 0 ; 0 ; 2 * s(1) ; 0 ; 0 ; 0 ; ... - // s(2) * s(3) ; s(2)^2 ; 2*s(1)*s(3); 3 * s(1)^2]; - - double Hs1[20]; - Hs1[0]=0; Hs1[1]=2*s[0]*s[1]; Hs1[2]=s[1]; Hs1[3]=s[2]; Hs1[4]=0; Hs1[5]=0; Hs1[6]=0; - Hs1[7]=s[2]*s[2]; Hs1[8]=1; Hs1[9]=0; Hs1[10]=0; Hs1[11]=0; Hs1[12]=2*s[0]; Hs1[13]=0; - Hs1[14]=0; Hs1[15]=0; Hs1[16]=s[1]*s[2]; Hs1[17]=s[1]*s[1]; Hs1[18]=2*s[0]*s[2]; Hs1[19]=3*s[0]*s[0]; - - // deriv of m w.r.t. s2 - //Hs2 = [0 ; s(1)^2 ; s(1) ; 0 ; s(3) ; 2 * s(2) * s(3) ; 3 * s(2)^2 ; ... - // 0 ; 0 ; 0 ; 1 ; s(3)^2 ; 0 ; 0 ; 2 * s(2) ; 0 ; ... - // s(1) * s(3) ; s(1) * 2 * s(2) ; 0 ; 0]; - - double Hs2[20]; - Hs2[0]=0; Hs2[1]=s[0]*s[0]; Hs2[2]=s[0]; Hs2[3]=0; Hs2[4]=s[2]; Hs2[5]=2*s[1]*s[2]; Hs2[6]=3*s[1]*s[1]; - Hs2[7]=0; Hs2[8]=0; Hs2[9]=0; Hs2[10]=1; Hs2[11]=s[2]*s[2]; Hs2[12]=0; Hs2[13]=0; - Hs2[14]=2*s[1]; Hs2[15]=0; Hs2[16]=s[0]*s[2]; Hs2[17]=2*s[0]*s[1]; Hs2[18]=0; Hs2[19]=0; - - // deriv of m w.r.t. s3 - //Hs3 = [0 ; 0 ; 0 ; s(1) ; s(2) ; s(2)^2 ; 0 ; ... - // s(1) * 2 * s(3) ; 0 ; 1 ; 0 ; s(2) * 2 * s(3) ; 0 ; 2 * s(3) ; 0 ; 3 * s(3)^2 ; ... - // s(1) * s(2) ; 0 ; s(1)^2 ; 0]; - - double Hs3[20]; - Hs3[0]=0; Hs3[1]=0; Hs3[2]=0; Hs3[3]=s[0]; Hs3[4]=s[1]; Hs3[5]=s[1]*s[1]; Hs3[6]=0; - Hs3[7]=2*s[0]*s[2]; Hs3[8]=0; Hs3[9]=1; Hs3[10]=0; Hs3[11]=2*s[1]*s[2]; Hs3[12]=0; Hs3[13]=2*s[2]; - Hs3[14]=0; Hs3[15]=3*s[2]*s[2]; Hs3[16]=s[0]*s[1]; Hs3[17]=0; Hs3[18]=s[0]*s[0]; Hs3[19]=0; - - // fill Hessian matrix - Mat H(3, 3, CV_64F); - H.at(0,0) = Mat(Mat(f1coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs1)).at(0,0); - H.at(0,1) = Mat(Mat(f1coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs2)).at(0,0); - H.at(0,2) = Mat(Mat(f1coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs3)).at(0,0); - - H.at(1,0) = Mat(Mat(f2coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs1)).at(0,0); - H.at(1,1) = Mat(Mat(f2coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs2)).at(0,0); - H.at(1,2) = Mat(Mat(f2coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs3)).at(0,0); - - H.at(2,0) = Mat(Mat(f3coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs1)).at(0,0); - H.at(2,1) = Mat(Mat(f3coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs2)).at(0,0); - H.at(2,2) = Mat(Mat(f3coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs3)).at(0,0); - - return H; -} - -Mat dls::cayley2rotbar(const Mat& s) -{ - double s_mul1 = Mat(s.t()*s).at(0,0); - Mat s_mul2 = s*s.t(); - Mat eye = Mat::eye(3, 3, CV_64F); - - return Mat( eye.mul(1.-s_mul1) + skewsymm(&s).mul(2.) + s_mul2.mul(2.) ).t(); -} - -Mat dls::skewsymm(const Mat * X1) -{ - MatConstIterator_ it = X1->begin(); - return (Mat_(3,3) << 0, -*(it+2), *(it+1), - *(it+2), 0, -*(it+0), - -*(it+1), *(it+0), 0); -} - -Mat dls::rotx(const double t) -{ - // rotx: rotation about y-axis - double ct = std::cos(t); - double st = std::sin(t); - return (Mat_(3,3) << 1, 0, 0, 0, ct, -st, 0, st, ct); -} - -Mat dls::roty(const double t) -{ - // roty: rotation about y-axis - double ct = std::cos(t); - double st = std::sin(t); - return (Mat_(3,3) << ct, 0, st, 0, 1, 0, -st, 0, ct); -} - -Mat dls::rotz(const double t) -{ - // rotz: rotation about y-axis - double ct = std::cos(t); - double st = std::sin(t); - return (Mat_(3,3) << ct, -st, 0, st, ct, 0, 0, 0, 1); -} - -Mat dls::mean(const Mat& M) -{ - Mat m = Mat::zeros(3, 1, CV_64F); - for (int i = 0; i < M.cols; ++i) m += M.col(i); - return m.mul(1./(double)M.cols); -} - -bool dls::is_empty(const Mat * M) -{ - MatConstIterator_ it = M->begin(), it_end = M->end(); - for(; it != it_end; ++it) - { - if(*it < 0) return false; - } - return true; -} - -bool dls::positive_eigenvalues(const Mat * eigenvalues) -{ - CV_Assert(eigenvalues && !eigenvalues->empty()); - MatConstIterator_ it = eigenvalues->begin(); - return *(it) > 0 && *(it+1) > 0 && *(it+2) > 0; -} - -} // namespace cv diff --git a/modules/3d/src/dls.h b/modules/3d/src/dls.h deleted file mode 100644 index a4b0ffa45c..0000000000 --- a/modules/3d/src/dls.h +++ /dev/null @@ -1,772 +0,0 @@ -#ifndef DLS_H_ -#define DLS_H_ - -#include "precomp.hpp" -#include - -namespace cv { - -class dls -{ -public: - dls(const Mat& opoints, const Mat& ipoints); - ~dls(); - - bool compute_pose(Mat& R, Mat& t); - -private: - - // initialisation - template - void init_points(const Mat& opoints, const Mat& ipoints) - { - for(int i = 0; i < N; i++) - { - p.at(0,i) = opoints.at(i).x; - p.at(1,i) = opoints.at(i).y; - p.at(2,i) = opoints.at(i).z; - - // compute mean of object points - mn.at(0) += p.at(0,i); - mn.at(1) += p.at(1,i); - mn.at(2) += p.at(2,i); - - // make z into unit vectors from normalized pixel coords - double sr = std::pow(ipoints.at(i).x, 2) + - std::pow(ipoints.at(i).y, 2) + (double)1; - sr = std::sqrt(sr); - - z.at(0,i) = ipoints.at(i).x / sr; - z.at(1,i) = ipoints.at(i).y / sr; - z.at(2,i) = (double)1 / sr; - } - - mn.at(0) /= (double)N; - mn.at(1) /= (double)N; - mn.at(2) /= (double)N; - } - - // main algorithm - Mat LeftMultVec(const Mat& v); - void run_kernel(const Mat& pp); - void build_coeff_matrix(const Mat& pp, Mat& Mtilde, Mat& D); - void compute_eigenvec(const Mat& Mtilde, Mat& eigenval_real, Mat& eigenval_imag, - Mat& eigenvec_real, Mat& eigenvec_imag); - void fill_coeff(const Mat * D); - - // useful functions - Mat cayley_LS_M(const std::vector& a, const std::vector& b, - const std::vector& c, const std::vector& u); - Mat Hessian(const double s[]); - Mat cayley2rotbar(const Mat& s); - Mat skewsymm(const Mat * X1); - - // extra functions - Mat rotx(const double t); - Mat roty(const double t); - Mat rotz(const double t); - Mat mean(const Mat& M); - bool is_empty(const Mat * v); - bool positive_eigenvalues(const Mat * eigenvalues); - - Mat p, z, mn; // object-image points - int N; // number of input points - std::vector f1coeff, f2coeff, f3coeff, cost_; // coefficient for coefficients matrix - std::vector C_est_, t_est_; // optimal candidates - Mat C_est__, t_est__; // optimal found solution - double cost__; // optimal found solution -}; - -class EigenvalueDecomposition { -private: - - // Holds the data dimension. - int n; - - // Stores real/imag part of a complex division. - double cdivr, cdivi; - - // Pointer to internal memory. - double *d, *e, *ort; - double **V, **H; - - // Holds the computed eigenvalues. - Mat _eigenvalues; - - // Holds the computed eigenvectors. - Mat _eigenvectors; - - // Allocates memory. - template - _Tp *alloc_1d(int m) { - return new _Tp[m]; - } - - // Allocates memory. - template - _Tp *alloc_1d(int m, _Tp val) { - _Tp *arr = alloc_1d<_Tp> (m); - for (int i = 0; i < m; i++) - arr[i] = val; - return arr; - } - - // Allocates memory. - template - _Tp **alloc_2d(int m, int _n) { - _Tp **arr = new _Tp*[m]; - for (int i = 0; i < m; i++) - arr[i] = new _Tp[_n]; - return arr; - } - - // Allocates memory. - template - _Tp **alloc_2d(int m, int _n, _Tp val) { - _Tp **arr = alloc_2d<_Tp> (m, _n); - for (int i = 0; i < m; i++) { - for (int j = 0; j < _n; j++) { - arr[i][j] = val; - } - } - return arr; - } - - void cdiv(double xr, double xi, double yr, double yi) { - double r, dv; - if (std::abs(yr) > std::abs(yi)) { - r = yi / yr; - dv = yr + r * yi; - cdivr = (xr + r * xi) / dv; - cdivi = (xi - r * xr) / dv; - } else { - r = yr / yi; - dv = yi + r * yr; - cdivr = (r * xr + xi) / dv; - cdivi = (r * xi - xr) / dv; - } - } - - // Nonsymmetric reduction from Hessenberg to real Schur form. - - void hqr2() { - - // This is derived from the Algol procedure hqr2, - // by Martin and Wilkinson, Handbook for Auto. Comp., - // Vol.ii-Linear Algebra, and the corresponding - // Fortran subroutine in EISPACK. - - // Initialize - int nn = this->n; - int n1 = nn - 1; - int low = 0; - int high = nn - 1; - double eps = std::pow(2.0, -52.0); - double exshift = 0.0; - double p = 0, q = 0, r = 0, s = 0, z = 0, t, w, x, y; - - // Store roots isolated by balanc and compute matrix norm - - double norm = 0.0; - for (int i = 0; i < nn; i++) { - if (i < low || i > high) { - d[i] = H[i][i]; - e[i] = 0.0; - } - for (int j = std::max(i - 1, 0); j < nn; j++) { - norm = norm + std::abs(H[i][j]); - } - } - - // Outer loop over eigenvalue index - int iter = 0; - while (n1 >= low) { - - // Look for single small sub-diagonal element - int l = n1; - while (l > low) { - s = std::abs(H[l - 1][l - 1]) + std::abs(H[l][l]); - if (s == 0.0) { - s = norm; - } - if (std::abs(H[l][l - 1]) < eps * s) { - break; - } - l--; - } - - // Check for convergence - // One root found - - if (l == n1) { - H[n1][n1] = H[n1][n1] + exshift; - d[n1] = H[n1][n1]; - e[n1] = 0.0; - n1--; - iter = 0; - - // Two roots found - - } else if (l == n1 - 1) { - w = H[n1][n1 - 1] * H[n1 - 1][n1]; - p = (H[n1 - 1][n1 - 1] - H[n1][n1]) / 2.0; - q = p * p + w; - z = std::sqrt(std::abs(q)); - H[n1][n1] = H[n1][n1] + exshift; - H[n1 - 1][n1 - 1] = H[n1 - 1][n1 - 1] + exshift; - x = H[n1][n1]; - - // Real pair - - if (q >= 0) { - if (p >= 0) { - z = p + z; - } else { - z = p - z; - } - d[n1 - 1] = x + z; - d[n1] = d[n1 - 1]; - if (z != 0.0) { - d[n1] = x - w / z; - } - e[n1 - 1] = 0.0; - e[n1] = 0.0; - x = H[n1][n1 - 1]; - s = std::abs(x) + std::abs(z); - p = x / s; - q = z / s; - r = std::sqrt(p * p + q * q); - p = p / r; - q = q / r; - - // Row modification - - for (int j = n1 - 1; j < nn; j++) { - z = H[n1 - 1][j]; - H[n1 - 1][j] = q * z + p * H[n1][j]; - H[n1][j] = q * H[n1][j] - p * z; - } - - // Column modification - - for (int i = 0; i <= n1; i++) { - z = H[i][n1 - 1]; - H[i][n1 - 1] = q * z + p * H[i][n1]; - H[i][n1] = q * H[i][n1] - p * z; - } - - // Accumulate transformations - - for (int i = low; i <= high; i++) { - z = V[i][n1 - 1]; - V[i][n1 - 1] = q * z + p * V[i][n1]; - V[i][n1] = q * V[i][n1] - p * z; - } - - // Complex pair - - } else { - d[n1 - 1] = x + p; - d[n1] = x + p; - e[n1 - 1] = z; - e[n1] = -z; - } - n1 = n1 - 2; - iter = 0; - - // No convergence yet - - } else { - - // Form shift - - x = H[n1][n1]; - y = 0.0; - w = 0.0; - if (l < n1) { - y = H[n1 - 1][n1 - 1]; - w = H[n1][n1 - 1] * H[n1 - 1][n1]; - } - - // Wilkinson's original ad hoc shift - - if (iter == 10) { - exshift += x; - for (int i = low; i <= n1; i++) { - H[i][i] -= x; - } - s = std::abs(H[n1][n1 - 1]) + std::abs(H[n1 - 1][n1 - 2]); - x = y = 0.75 * s; - w = -0.4375 * s * s; - } - - // MATLAB's new ad hoc shift - - if (iter == 30) { - s = (y - x) / 2.0; - s = s * s + w; - if (s > 0) { - s = std::sqrt(s); - if (y < x) { - s = -s; - } - s = x - w / ((y - x) / 2.0 + s); - for (int i = low; i <= n1; i++) { - H[i][i] -= s; - } - exshift += s; - x = y = w = 0.964; - } - } - - iter = iter + 1; // (Could check iteration count here.) - - // Look for two consecutive small sub-diagonal elements - int m = n1 - 2; - while (m >= l) { - z = H[m][m]; - r = x - z; - s = y - z; - p = (r * s - w) / H[m + 1][m] + H[m][m + 1]; - q = H[m + 1][m + 1] - z - r - s; - r = H[m + 2][m + 1]; - s = std::abs(p) + std::abs(q) + std::abs(r); - p = p / s; - q = q / s; - r = r / s; - if (m == l) { - break; - } - if (std::abs(H[m][m - 1]) * (std::abs(q) + std::abs(r)) < eps * (std::abs(p) - * (std::abs(H[m - 1][m - 1]) + std::abs(z) + std::abs( - H[m + 1][m + 1])))) { - break; - } - m--; - } - - for (int i = m + 2; i <= n1; i++) { - H[i][i - 2] = 0.0; - if (i > m + 2) { - H[i][i - 3] = 0.0; - } - } - - // Double QR step involving rows l:n and columns m:n - - for (int k = m; k <= n1 - 1; k++) { - bool notlast = (k != n1 - 1); - if (k != m) { - p = H[k][k - 1]; - q = H[k + 1][k - 1]; - r = (notlast ? H[k + 2][k - 1] : 0.0); - x = std::abs(p) + std::abs(q) + std::abs(r); - if (x != 0.0) { - p = p / x; - q = q / x; - r = r / x; - } - } - if (x == 0.0) { - break; - } - s = std::sqrt(p * p + q * q + r * r); - if (p < 0) { - s = -s; - } - if (s != 0) { - if (k != m) { - H[k][k - 1] = -s * x; - } else if (l != m) { - H[k][k - 1] = -H[k][k - 1]; - } - p = p + s; - x = p / s; - y = q / s; - z = r / s; - q = q / p; - r = r / p; - - // Row modification - - for (int j = k; j < nn; j++) { - p = H[k][j] + q * H[k + 1][j]; - if (notlast) { - p = p + r * H[k + 2][j]; - H[k + 2][j] = H[k + 2][j] - p * z; - } - H[k][j] = H[k][j] - p * x; - H[k + 1][j] = H[k + 1][j] - p * y; - } - - // Column modification - - for (int i = 0; i <= std::min(n1, k + 3); i++) { - p = x * H[i][k] + y * H[i][k + 1]; - if (notlast) { - p = p + z * H[i][k + 2]; - H[i][k + 2] = H[i][k + 2] - p * r; - } - H[i][k] = H[i][k] - p; - H[i][k + 1] = H[i][k + 1] - p * q; - } - - // Accumulate transformations - - for (int i = low; i <= high; i++) { - p = x * V[i][k] + y * V[i][k + 1]; - if (notlast) { - p = p + z * V[i][k + 2]; - V[i][k + 2] = V[i][k + 2] - p * r; - } - V[i][k] = V[i][k] - p; - V[i][k + 1] = V[i][k + 1] - p * q; - } - } // (s != 0) - } // k loop - } // check convergence - } // while (n1 >= low) - - // Backsubstitute to find vectors of upper triangular form - - if (norm == 0.0) { - return; - } - - for (n1 = nn - 1; n1 >= 0; n1--) { - p = d[n1]; - q = e[n1]; - - // Real vector - - if (q == 0) { - int l = n1; - H[n1][n1] = 1.0; - for (int i = n1 - 1; i >= 0; i--) { - w = H[i][i] - p; - r = 0.0; - for (int j = l; j <= n1; j++) { - r = r + H[i][j] * H[j][n1]; - } - if (e[i] < 0.0) { - z = w; - s = r; - } else { - l = i; - if (e[i] == 0.0) { - if (w != 0.0) { - H[i][n1] = -r / w; - } else { - H[i][n1] = -r / (eps * norm); - } - - // Solve real equations - - } else { - x = H[i][i + 1]; - y = H[i + 1][i]; - q = (d[i] - p) * (d[i] - p) + e[i] * e[i]; - t = (x * s - z * r) / q; - H[i][n1] = t; - if (std::abs(x) > std::abs(z)) { - H[i + 1][n1] = (-r - w * t) / x; - } else { - H[i + 1][n1] = (-s - y * t) / z; - } - } - - // Overflow control - - t = std::abs(H[i][n1]); - if ((eps * t) * t > 1) { - for (int j = i; j <= n1; j++) { - H[j][n1] = H[j][n1] / t; - } - } - } - } - // Complex vector - } else if (q < 0) { - int l = n1 - 1; - - // Last vector component imaginary so matrix is triangular - - if (std::abs(H[n1][n1 - 1]) > std::abs(H[n1 - 1][n1])) { - H[n1 - 1][n1 - 1] = q / H[n1][n1 - 1]; - H[n1 - 1][n1] = -(H[n1][n1] - p) / H[n1][n1 - 1]; - } else { - cdiv(0.0, -H[n1 - 1][n1], H[n1 - 1][n1 - 1] - p, q); - H[n1 - 1][n1 - 1] = cdivr; - H[n1 - 1][n1] = cdivi; - } - H[n1][n1 - 1] = 0.0; - H[n1][n1] = 1.0; - for (int i = n1 - 2; i >= 0; i--) { - double ra, sa; - ra = 0.0; - sa = 0.0; - for (int j = l; j <= n1; j++) { - ra = ra + H[i][j] * H[j][n1 - 1]; - sa = sa + H[i][j] * H[j][n1]; - } - w = H[i][i] - p; - - if (e[i] < 0.0) { - z = w; - r = ra; - s = sa; - } else { - l = i; - if (e[i] == 0) { - cdiv(-ra, -sa, w, q); - H[i][n1 - 1] = cdivr; - H[i][n1] = cdivi; - } else { - - // Solve complex equations - - x = H[i][i + 1]; - y = H[i + 1][i]; - double vr = (d[i] - p) * (d[i] - p) + e[i] * e[i] - q * q; - double vi = (d[i] - p) * 2.0 * q; - if (vr == 0.0 && vi == 0.0) { - vr = eps * norm * (std::abs(w) + std::abs(q) + std::abs(x) - + std::abs(y) + std::abs(z)); - } - cdiv(x * r - z * ra + q * sa, - x * s - z * sa - q * ra, vr, vi); - H[i][n1 - 1] = cdivr; - H[i][n1] = cdivi; - if (std::abs(x) > (std::abs(z) + std::abs(q))) { - H[i + 1][n1 - 1] = (-ra - w * H[i][n1 - 1] + q - * H[i][n1]) / x; - H[i + 1][n1] = (-sa - w * H[i][n1] - q * H[i][n1 - - 1]) / x; - } else { - cdiv(-r - y * H[i][n1 - 1], -s - y * H[i][n1], z, - q); - H[i + 1][n1 - 1] = cdivr; - H[i + 1][n1] = cdivi; - } - } - - // Overflow control - - t = std::max(std::abs(H[i][n1 - 1]), std::abs(H[i][n1])); - if ((eps * t) * t > 1) { - for (int j = i; j <= n1; j++) { - H[j][n1 - 1] = H[j][n1 - 1] / t; - H[j][n1] = H[j][n1] / t; - } - } - } - } - } - } - - // Vectors of isolated roots - - for (int i = 0; i < nn; i++) { - if (i < low || i > high) { - for (int j = i; j < nn; j++) { - V[i][j] = H[i][j]; - } - } - } - - // Back transformation to get eigenvectors of original matrix - - for (int j = nn - 1; j >= low; j--) { - for (int i = low; i <= high; i++) { - z = 0.0; - for (int k = low; k <= std::min(j, high); k++) { - z = z + V[i][k] * H[k][j]; - } - V[i][j] = z; - } - } - } - - // Nonsymmetric reduction to Hessenberg form. - void orthes() { - // This is derived from the Algol procedures orthes and ortran, - // by Martin and Wilkinson, Handbook for Auto. Comp., - // Vol.ii-Linear Algebra, and the corresponding - // Fortran subroutines in EISPACK. - int low = 0; - int high = n - 1; - - for (int m = low + 1; m <= high - 1; m++) { - - // Scale column. - - double scale = 0.0; - for (int i = m; i <= high; i++) { - scale = scale + std::abs(H[i][m - 1]); - } - if (scale != 0.0) { - - // Compute Householder transformation. - - double h = 0.0; - for (int i = high; i >= m; i--) { - ort[i] = H[i][m - 1] / scale; - h += ort[i] * ort[i]; - } - double g = std::sqrt(h); - if (ort[m] > 0) { - g = -g; - } - h = h - ort[m] * g; - ort[m] = ort[m] - g; - - // Apply Householder similarity transformation - // H = (I-u*u'/h)*H*(I-u*u')/h) - - for (int j = m; j < n; j++) { - double f = 0.0; - for (int i = high; i >= m; i--) { - f += ort[i] * H[i][j]; - } - f = f / h; - for (int i = m; i <= high; i++) { - H[i][j] -= f * ort[i]; - } - } - - for (int i = 0; i <= high; i++) { - double f = 0.0; - for (int j = high; j >= m; j--) { - f += ort[j] * H[i][j]; - } - f = f / h; - for (int j = m; j <= high; j++) { - H[i][j] -= f * ort[j]; - } - } - ort[m] = scale * ort[m]; - H[m][m - 1] = scale * g; - } - } - - // Accumulate transformations (Algol's ortran). - - for (int i = 0; i < n; i++) { - for (int j = 0; j < n; j++) { - V[i][j] = (i == j ? 1.0 : 0.0); - } - } - - for (int m = high - 1; m >= low + 1; m--) { - if (H[m][m - 1] != 0.0) { - for (int i = m + 1; i <= high; i++) { - ort[i] = H[i][m - 1]; - } - for (int j = m; j <= high; j++) { - double g = 0.0; - for (int i = m; i <= high; i++) { - g += ort[i] * V[i][j]; - } - // Double division avoids possible underflow - g = (g / ort[m]) / H[m][m - 1]; - for (int i = m; i <= high; i++) { - V[i][j] += g * ort[i]; - } - } - } - } - } - - // Releases all internal working memory. - void release() { - // releases the working data - delete[] d; - delete[] e; - delete[] ort; - for (int i = 0; i < n; i++) { - delete[] H[i]; - delete[] V[i]; - } - delete[] H; - delete[] V; - } - - // Computes the Eigenvalue Decomposition for a matrix given in H. - void compute() { - // Allocate memory for the working data. - V = alloc_2d (n, n, 0.0); - d = alloc_1d (n); - e = alloc_1d (n); - ort = alloc_1d (n); - // Reduce to Hessenberg form. - orthes(); - // Reduce Hessenberg to real Schur form. - hqr2(); - // Copy eigenvalues to OpenCV Matrix. - _eigenvalues.create(1, n, CV_64FC1); - for (int i = 0; i < n; i++) { - _eigenvalues.at (0, i) = d[i]; - } - // Copy eigenvectors to OpenCV Matrix. - _eigenvectors.create(n, n, CV_64FC1); - for (int i = 0; i < n; i++) - for (int j = 0; j < n; j++) - _eigenvectors.at (i, j) = V[i][j]; - // Deallocate the memory by releasing all internal working data. - release(); - } - -public: - EigenvalueDecomposition() - : n(0), cdivr(0), cdivi(0), d(0), e(0), ort(0), V(0), H(0) {} - - // Initializes & computes the Eigenvalue Decomposition for a general matrix - // given in src. This function is a port of the EigenvalueSolver in JAMA, - // which has been released to public domain by The MathWorks and the - // National Institute of Standards and Technology (NIST). - EigenvalueDecomposition(InputArray src) { - compute(src); - } - - // This function computes the Eigenvalue Decomposition for a general matrix - // given in src. This function is a port of the EigenvalueSolver in JAMA, - // which has been released to public domain by The MathWorks and the - // National Institute of Standards and Technology (NIST). - void compute(InputArray src) - { - /*if(isSymmetric(src)) { - // Fall back to OpenCV for a symmetric matrix! - eigen(src, _eigenvalues, _eigenvectors); - } else {*/ - Mat tmp; - // Convert the given input matrix to double. Is there any way to - // prevent allocating the temporary memory? Only used for copying - // into working memory and deallocated after. - src.getMat().convertTo(tmp, CV_64FC1); - // Get dimension of the matrix. - this->n = tmp.cols; - // Allocate the matrix data to work on. - this->H = alloc_2d (n, n); - // Now safely copy the data. - for (int i = 0; i < tmp.rows; i++) { - for (int j = 0; j < tmp.cols; j++) { - this->H[i][j] = tmp.at(i, j); - } - } - // Deallocates the temporary matrix before computing. - tmp.release(); - // Performs the eigenvalue decomposition of H. - compute(); - // } - } - - ~EigenvalueDecomposition() {} - - // Returns the eigenvalues of the Eigenvalue Decomposition. - Mat eigenvalues() { return _eigenvalues; } - // Returns the eigenvectors of the Eigenvalue Decomposition. - Mat eigenvectors() { return _eigenvectors; } -}; - -} // namespace cv -#endif // DLS_H diff --git a/modules/3d/src/solvepnp.cpp b/modules/3d/src/solvepnp.cpp index a4f03b0269..9db007a6c5 100644 --- a/modules/3d/src/solvepnp.cpp +++ b/modules/3d/src/solvepnp.cpp @@ -41,8 +41,6 @@ //M*/ #include "precomp.hpp" -//#include "upnp.h" -#include "dls.h" #include "epnp.h" #include "p3p.h" #include "ap3p.h" @@ -840,17 +838,8 @@ int solvePnPGeneric( InputArray _opoints, InputArray _ipoints, Mat distCoeffs = Mat_(distCoeffs0); std::vector vec_rvecs, vec_tvecs; - if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP) + if (flags == SOLVEPNP_EPNP) { - if (flags == SOLVEPNP_DLS) - { - CV_LOG_DEBUG(NULL, "Broken implementation for SOLVEPNP_DLS. Fallback to EPnP."); - } - else if (flags == SOLVEPNP_UPNP) - { - CV_LOG_DEBUG(NULL, "Broken implementation for SOLVEPNP_UPNP. Fallback to EPnP."); - } - Mat undistortedPoints; undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); epnp PnP(cameraMatrix, opoints, undistortedPoints); @@ -1000,35 +989,9 @@ int solvePnPGeneric( InputArray _opoints, InputArray _ipoints, sqpnp::PoseSolver solver; solver.solve(opoints, undistortedPoints, vec_rvecs, vec_tvecs); } - /*else if (flags == SOLVEPNP_DLS) - { - Mat undistortedPoints; - undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); - - dls PnP(opoints, undistortedPoints); - - Mat rvec, tvec, R; - bool result = PnP.compute_pose(R, tvec); - if (result) - { - Rodrigues(R, rvec); - vec_rvecs.push_back(rvec); - vec_tvecs.push_back(tvec); - } - } - else if (flags == SOLVEPNP_UPNP) - { - upnp PnP(cameraMatrix, opoints, ipoints); - - Mat rvec, tvec, R; - PnP.compute_pose(R, tvec); - Rodrigues(R, rvec); - vec_rvecs.push_back(rvec); - vec_tvecs.push_back(tvec); - }*/ else CV_Error(cv::Error::StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, " - "SOLVEPNP_EPNP, SOLVEPNP_DLS, SOLVEPNP_UPNP, SOLVEPNP_AP3P, SOLVEPNP_IPPE, SOLVEPNP_IPPE_SQUARE or SOLVEPNP_SQPNP"); + "SOLVEPNP_EPNP, SOLVEPNP_AP3P, SOLVEPNP_IPPE, SOLVEPNP_IPPE_SQUARE or SOLVEPNP_SQPNP"); CV_Assert(vec_rvecs.size() == vec_tvecs.size()); diff --git a/modules/3d/test/test_solvepnp_ransac.cpp b/modules/3d/test/test_solvepnp_ransac.cpp index 39706000a1..c915c440a9 100644 --- a/modules/3d/test/test_solvepnp_ransac.cpp +++ b/modules/3d/test/test_solvepnp_ransac.cpp @@ -182,16 +182,12 @@ static std::string printMethod(int method) case 2: return "SOLVEPNP_P3P"; case 3: - return "SOLVEPNP_DLS (remapped to SOLVEPNP_EPNP)"; - case 4: - return "SOLVEPNP_UPNP (remapped to SOLVEPNP_EPNP)"; - case 5: return "SOLVEPNP_AP3P"; - case 6: + case 4: return "SOLVEPNP_IPPE"; - case 7: + case 5: return "SOLVEPNP_IPPE_SQUARE"; - case 8: + case 6: return "SOLVEPNP_SQPNP"; default: return "Unknown value"; @@ -207,8 +203,6 @@ public: eps[SOLVEPNP_EPNP] = 1.0e-2; eps[SOLVEPNP_P3P] = 1.0e-2; eps[SOLVEPNP_AP3P] = 1.0e-2; - eps[SOLVEPNP_DLS] = 1.0e-2; // DLS is remapped to EPnP, so we use the same threshold - eps[SOLVEPNP_UPNP] = 1.0e-2; // UPnP is remapped to EPnP, so we use the same threshold eps[SOLVEPNP_IPPE] = 1.0e-2; eps[SOLVEPNP_IPPE_SQUARE] = 1.0e-2; eps[SOLVEPNP_SQPNP] = 1.0e-2; @@ -313,12 +307,6 @@ protected: Mat trueRvec, trueTvec; Mat intrinsics, distCoeffs; generateCameraMatrix(intrinsics, rng); - //UPnP is mapped to EPnP - //Uncomment this when UPnP is fixed -// if (method == SOLVEPNP_UPNP) -// { -// intrinsics.at(1,1) = intrinsics.at(0,0); -// } if (mode == 0) { distCoeffs = Mat::zeros(4, 1, CV_64FC1); @@ -468,8 +456,6 @@ public: eps[SOLVEPNP_EPNP] = 1.0e-6; eps[SOLVEPNP_P3P] = 2.0e-4; eps[SOLVEPNP_AP3P] = 1.0e-4; - eps[SOLVEPNP_DLS] = 1.0e-6; // DLS is remapped to EPnP, so we use the same threshold - eps[SOLVEPNP_UPNP] = 1.0e-6; // UPnP is remapped to EPnP, so we use the same threshold eps[SOLVEPNP_IPPE] = 1.0e-6; eps[SOLVEPNP_IPPE_SQUARE] = 1.0e-6; eps[SOLVEPNP_SQPNP] = 1.0e-6; @@ -509,19 +495,12 @@ protected: if (mode == 0) { epsilon_trans[SOLVEPNP_EPNP] = 5.0e-3; - epsilon_trans[SOLVEPNP_DLS] = 5.0e-3; // DLS is remapped to EPnP, so we use the same threshold - epsilon_trans[SOLVEPNP_UPNP] = 5.0e-3; // UPnP is remapped to EPnP, so we use the same threshold - epsilon_rot[SOLVEPNP_EPNP] = 5.0e-3; - epsilon_rot[SOLVEPNP_DLS] = 5.0e-3; // DLS is remapped to EPnP, so we use the same threshold - epsilon_rot[SOLVEPNP_UPNP] = 5.0e-3; // UPnP is remapped to EPnP, so we use the same threshold } else { epsilon_trans[SOLVEPNP_ITERATIVE] = 1e-4; epsilon_trans[SOLVEPNP_EPNP] = 5e-3; - epsilon_trans[SOLVEPNP_DLS] = 5e-3; // DLS is remapped to EPnP, so we use the same threshold - epsilon_trans[SOLVEPNP_UPNP] = 5e-3; // UPnP is remapped to EPnP, so we use the same threshold epsilon_trans[SOLVEPNP_P3P] = 1e-4; epsilon_trans[SOLVEPNP_AP3P] = 1e-4; epsilon_trans[SOLVEPNP_IPPE] = 1e-4; @@ -529,8 +508,6 @@ protected: epsilon_rot[SOLVEPNP_ITERATIVE] = 1e-4; epsilon_rot[SOLVEPNP_EPNP] = 5e-3; - epsilon_rot[SOLVEPNP_DLS] = 5e-3; // DLS is remapped to EPnP, so we use the same threshold - epsilon_rot[SOLVEPNP_UPNP] = 5e-3; // UPnP is remapped to EPnP, so we use the same threshold epsilon_rot[SOLVEPNP_P3P] = 1e-4; epsilon_rot[SOLVEPNP_AP3P] = 1e-4; epsilon_rot[SOLVEPNP_IPPE] = 1e-4; @@ -541,12 +518,6 @@ protected: Mat trueRvec, trueTvec; Mat intrinsics, distCoeffs; generateCameraMatrix(intrinsics, rng); - //UPnP is mapped to EPnP - //Uncomment this when UPnP is fixed -// if (method == SOLVEPNP_UPNP) -// { -// intrinsics.at(1,1) = intrinsics.at(0,0); -// } if (mode == 0) { distCoeffs = Mat::zeros(4, 1, CV_64FC1); @@ -565,18 +536,6 @@ protected: case SOLVEPNP_AP3P: opoints = std::vector(points.begin(), points.begin()+4); break; - //UPnP is mapped to EPnP - //Uncomment this when UPnP is fixed -// case SOLVEPNP_UPNP: -// if (points.size() > 50) -// { -// opoints = std::vector(points.begin(), points.begin()+50); -// } -// else -// { -// opoints = points; -// } -// break; default: opoints = points; break;