From 0d2bc9b0a14ced53d8563be5cbbf78fd0a42abeb Mon Sep 17 00:00:00 2001 From: edgarriba Date: Tue, 5 Aug 2014 17:02:06 +0200 Subject: [PATCH] Removed whitespaces --- modules/calib3d/src/dls.cpp | 1053 ++++++++--------- modules/calib3d/src/dls.h | 102 +- modules/calib3d/src/solvepnp.cpp | 304 +---- .../real_time_pose_estimation/CMakeLists.txt | 74 +- 4 files changed, 657 insertions(+), 876 deletions(-) diff --git a/modules/calib3d/src/dls.cpp b/modules/calib3d/src/dls.cpp index ef1ad58140..1bc00a8370 100644 --- a/modules/calib3d/src/dls.cpp +++ b/modules/calib3d/src/dls.cpp @@ -17,30 +17,29 @@ using namespace std; dls::dls(const cv::Mat& opoints, const cv::Mat& ipoints) { - - N = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); - p = cv::Mat(3, N, CV_64F); - z = cv::Mat(3, N, 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); - - norm_z_vector(); + N = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); + p = cv::Mat(3, N, CV_64F); + z = cv::Mat(3, N, 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); + + norm_z_vector(); } @@ -51,617 +50,617 @@ dls::~dls() void dls::norm_z_vector() { - // make z into unit vectors from normalized pixel coords - for (int i = 0; i < N; ++i) - { - cv::Mat col_i = z.col(i); - double sr = std::pow(col_i.at(0), 2) + - std::pow(col_i.at(1), 2) + - std::pow(col_i.at(2), 2); - sr = std::sqrt(sr); - - z.at(0, i) /= sr; - z.at(1, i) /= sr; - z.at(2, i) /= sr; - } + // make z into unit vectors from normalized pixel coords + for (int i = 0; i < N; ++i) + { + cv::Mat col_i = z.col(i); + double sr = std::pow(col_i.at(0), 2) + + std::pow(col_i.at(1), 2) + + std::pow(col_i.at(2), 2); + sr = std::sqrt(sr); + + z.at(0, i) /= sr; + z.at(1, i) /= sr; + z.at(2, i) /= sr; + } } bool dls::compute_pose(cv::Mat& R, cv::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)); - - cv::Mat t_mean = this->mean(p); - - // version that calls dls 3 times, to avoid Cayley singularity - for (int i = 0; i < 3; ++i) - { - // Make a random rotation - cv::Mat pp = R_[i] * ( p - cv::repeat(t_mean, 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] * t_mean; - 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; + std::vector R_; + R_.push_back(rotx(CV_PI/2)); + R_.push_back(roty(CV_PI/2)); + R_.push_back(rotz(CV_PI/2)); + + cv::Mat t_mean = this->mean(p); + + // version that calls dls 3 times, to avoid Cayley singularity + for (int i = 0; i < 3; ++i) + { + // Make a random rotation + cv::Mat pp = R_[i] * ( p - cv::repeat(t_mean, 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] * t_mean; + 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 cv::Mat& pp) { - cv::Mat Mtilde(27, 27, CV_64F); - cv::Mat D = cv::Mat::zeros(9, 9, CV_64F); - build_coeff_matrix(pp, Mtilde, D); - - cv::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 - - cv::Mat sols = cv::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); - cv::Mat V_kA = eigenvec_r.col(k); // 27x1 - cv::Mat V_kB = cv::Mat(1, 1, z.depth(), V_kA.at(0)); // 1x1 - cv::Mat V_k; cv::solve(V_kB.t(), V_kA.t(), V_k); // A/B = B'\A' - cv::Mat( V_k.t()).copyTo( eigenvec_r.col(k) ); - - //if (imag(V(2,k)) == 0) + cv::Mat Mtilde(27, 27, CV_64F); + cv::Mat D = cv::Mat::zeros(9, 9, CV_64F); + build_coeff_matrix(pp, Mtilde, D); + + cv::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 + + cv::Mat sols = cv::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); + cv::Mat V_kA = eigenvec_r.col(k); // 27x1 + cv::Mat V_kB = cv::Mat(1, 1, z.depth(), V_kA.at(0)); // 1x1 + cv::Mat V_k; cv::solve(V_kB.t(), V_kA.t(), V_k); // A/B = B'\A' + cv::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 ) + 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); + double stmp[3]; + stmp[0] = eigenvec_r.at(9, k); + stmp[1] = eigenvec_r.at(3, k); + stmp[2] = eigenvec_r.at(1, k); - cv::Mat H = Hessian(stmp); + cv::Mat H = Hessian(stmp); - cv::Mat eigenvalues, eigenvectors; - cv::eigen(H, eigenvalues, eigenvectors); + cv::Mat eigenvalues, eigenvectors; + cv::eigen(H, eigenvalues, eigenvectors); - if(positive_eigenvalues(&eigenvalues)) - { + if(positive_eigenvalues(&eigenvalues)) + { - // sols(:,i) = stmp; - cv::Mat stmp_mat(3, 1, CV_64F, &stmp); + // sols(:,i) = stmp; + cv::Mat stmp_mat(3, 1, CV_64F, &stmp); - stmp_mat.copyTo( sols.col(count) ); + stmp_mat.copyTo( sols.col(count) ); - cv::Mat Cbar = cayley2rotbar(stmp_mat); - cv::Mat Cbarvec = Cbar.reshape(1,1).t(); + cv::Mat Cbar = cayley2rotbar(stmp_mat); + cv::Mat Cbarvec = Cbar.reshape(1,1).t(); - // cost(i) = CbarVec' * D * CbarVec; - cv::Mat cost_mat = Cbarvec.t() * D * Cbarvec; - cost.push_back( cost_mat.at(0) ); + // cost(i) = CbarVec' * D * CbarVec; + cv::Mat cost_mat = Cbarvec.t() * D * Cbarvec; + cost.push_back( cost_mat.at(0) ); - count++; - } - } - } + count++; + } + } + } - // extract solutions - sols = sols.clone().colRange(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)); + 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)); - cv::Mat sols_j = sols.col(j); - double sols_mult = 1./(1.+cv::Mat( sols_j.t() * sols_j ).at(0)); - cv::Mat C_est_j = cayley2rotbar(sols_j).mul(sols_mult); - C_est.push_back( C_est_j ); + cv::Mat sols_j = sols.col(j); + double sols_mult = 1./(1.+cv::Mat( sols_j.t() * sols_j ).at(0)); + cv::Mat C_est_j = cayley2rotbar(sols_j).mul(sols_mult); + C_est.push_back( C_est_j ); - cv::Mat A2 = cv::Mat::zeros(3, 3, CV_64F); - cv::Mat b2 = cv::Mat::zeros(3, 1, CV_64F); - for (int i = 0; i < N; ++i) - { - cv::Mat eye = cv::Mat::eye(3, 3, CV_64F); - cv::Mat z_mul = z.col(i)*z.col(i).t(); + cv::Mat A2 = cv::Mat::zeros(3, 3, CV_64F); + cv::Mat b2 = cv::Mat::zeros(3, 1, CV_64F); + for (int i = 0; i < N; ++i) + { + cv::Mat eye = cv::Mat::eye(3, 3, CV_64F); + cv::Mat z_mul = z.col(i)*z.col(i).t(); - A2 += eye - z_mul; - b2 += (z_mul - eye) * C_est_j * pp.col(i); - } + A2 += eye - z_mul; + b2 += (z_mul - eye) * C_est_j * pp.col(i); + } - // recover the optimal translation - cv::Mat X2; cv::solve(A2, b2, X2); // A\B - t_est.push_back(X2); + // recover the optimal translation + cv::Mat X2; cv::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) - { - cv::Mat cam_points = C_est[k] * pp + cv::repeat(t_est[k], 1, pp.cols); - cv::Mat cam_points_k = cam_points.row(2); + // check that the points are infront of the center of perspectivity + for (int k = 0; k < sols.cols; ++k) + { + cv::Mat cam_points = C_est[k] * pp + cv::repeat(t_est[k], 1, pp.cols); + cv::Mat cam_points_k = cam_points.row(2); - if(is_empty(&cam_points_k)) - { - cv::Mat C_valid = C_est[k], t_valid = t_est[k]; - double cost_valid = cost[k]; + if(is_empty(&cam_points_k)) + { + cv::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); - } - } + C_est_.push_back(C_valid); + t_est_.push_back(t_valid); + cost_.push_back(cost_valid); + } + } } void dls::build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D) { - cv::Mat eye = cv::Mat::eye(3, 3, CV_64F); + cv::Mat eye = cv::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) + // build coeff matrix + // An intermediate matrix, the inverse of what is called "H" in the paper + // (see eq. 25) - cv::Mat H = cv::Mat::zeros(3, 3, CV_64F); - cv::Mat A = cv::Mat::zeros(3, 9, CV_64F); + cv::Mat H = cv::Mat::zeros(3, 3, CV_64F); + cv::Mat A = cv::Mat::zeros(3, 9, CV_64F); - for (int i = 0; i < N; ++i) - { - cv::Mat z_dot = z.col(i)*z.col(i).t(); - H += eye - z_dot; - A += ( z_dot - eye ) * LeftMultVec(pp.col(i)); - } + for (int i = 0; i < N; ++i) + { + cv::Mat z_dot = z.col(i)*z.col(i).t(); + H += eye - z_dot; + A += ( z_dot - eye ) * LeftMultVec(pp.col(i)); + } - // A\B - cv::solve(H, A, A); + // A\B + cv::solve(H, A, A); - for (int i = 0; i < N; ++i) - { - cv::Mat z_dot = z.col(i)*z.col(i).t(); - D += cv::Mat( LeftMultVec(pp.col(i)) + A ).t() * (eye-z_dot) * ( LeftMultVec(pp.col(i)) + A ); - } + for (int i = 0; i < N; ++i) + { + cv::Mat z_dot = z.col(i)*z.col(i).t(); + D += cv::Mat( LeftMultVec(pp.col(i)) + A ).t() * (eye-z_dot) * ( LeftMultVec(pp.col(i)) + A ); + } - // fill the coefficients - fill_coeff(&D); + // fill the coefficients + fill_coeff(&D); - // generate random samples - std::vector u(5); - cv::randn(u, 0, 200); + // generate random samples + std::vector u(5); + cv::randn(u, 0, 200); - cv::Mat M2 = cayley_LS_M(f1coeff, f2coeff, f3coeff, u); + cv::Mat M2 = cayley_LS_M(f1coeff, f2coeff, f3coeff, u); - cv::Mat M2_1 = M2(cv::Range(0,27), cv::Range(0,27)); - cv::Mat M2_2 = M2(cv::Range(0,27), cv::Range(27,120)); - cv::Mat M2_3 = M2(cv::Range(27,120), cv::Range(27,120)); - cv::Mat M2_4 = M2(cv::Range(27,120), cv::Range(0,27)); + cv::Mat M2_1 = M2(cv::Range(0,27), cv::Range(0,27)); + cv::Mat M2_2 = M2(cv::Range(0,27), cv::Range(27,120)); + cv::Mat M2_3 = M2(cv::Range(27,120), cv::Range(27,120)); + cv::Mat M2_4 = M2(cv::Range(27,120), cv::Range(0,27)); - // A/B = B'\A' - cv::Mat M2_5; cv::solve(M2_3.t(), M2_2.t(), M2_5); + // A/B = B'\A' + cv::Mat M2_5; cv::solve(M2_3.t(), M2_2.t(), M2_5); - // construct the multiplication matrix via schur compliment of the Macaulay - // matrix - Mtilde = M2_1 - M2_5.t()*M2_4; + // construct the multiplication matrix via schur compliment of the Macaulay + // matrix + Mtilde = M2_1 - M2_5.t()*M2_4; } void dls::compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Mat& eigenval_imag, - cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag) + cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag) { #ifdef HAVE_EIGEN - Eigen::MatrixXd Mtilde_eig, zeros_eig; - cv::cv2eigen(Mtilde, Mtilde_eig); - cv::cv2eigen(cv::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(); - - cv::eigen2cv(eigval_real, eigenval_real); - cv::eigen2cv(eigval_imag, eigenval_imag); - cv::eigen2cv(eigvec_real, eigenvec_real); - cv::eigen2cv(eigvec_imag, eigenvec_imag); + Eigen::MatrixXd Mtilde_eig, zeros_eig; + cv::cv2eigen(Mtilde, Mtilde_eig); + cv::cv2eigen(cv::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(); + + cv::eigen2cv(eigval_real, eigenval_real); + cv::eigen2cv(eigval_imag, eigenval_imag); + cv::eigen2cv(eigvec_real, eigenvec_real); + cv::eigen2cv(eigvec_imag, eigenvec_imag); #else - EigenvalueDecomposition es(Mtilde); - eigenval_real = es.eigenvalues(); - eigenvec_real = es.eigenvectors(); + EigenvalueDecomposition es(Mtilde); + eigenval_real = es.eigenvalues(); + eigenvec_real = es.eigenvectors(); #endif } void dls::fill_coeff(const cv::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 + // 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 } cv::Mat dls::LeftMultVec(const cv::Mat& v) { - cv::Mat mat, row1, row2, row3; - cv::Mat zeros16 = cv::Mat::zeros(1, 6, CV_64F); - cv::Mat zeros13 = cv::Mat::zeros(1, 3, CV_64F); + cv::Mat mat, row1, row2, row3; + cv::Mat zeros16 = cv::Mat::zeros(1, 6, CV_64F); + cv::Mat zeros13 = cv::Mat::zeros(1, 3, CV_64F); - cv::hconcat(v.clone().t(), zeros16, row1); // first row - cv::hconcat(zeros13, v.clone().t(), row2); // second row - cv::hconcat(row2, zeros13, row2); // second row - cv::hconcat(zeros16, v.clone().t(), row3); // third row + cv::hconcat(v.clone().t(), zeros16, row1); // first row + cv::hconcat(zeros13, v.clone().t(), row2); // second row + cv::hconcat(row2, zeros13, row2); // second row + cv::hconcat(zeros16, v.clone().t(), row3); // third row - mat.push_back(row1); - mat.push_back(row2); - mat.push_back(row3); + mat.push_back(row1); + mat.push_back(row2); + mat.push_back(row3); - return mat; + return mat; } cv::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 - - cv::Mat M = cv::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(); + // TODO: input matrix pointer + // TODO: shift coefficients one position to left + + cv::Mat M = cv::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(); } cv::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 - cv::Mat H(3, 3, CV_64F); - H.at(0,0) = cv::Mat(cv::Mat(f1coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs1)).at(0,0); - H.at(0,1) = cv::Mat(cv::Mat(f1coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs2)).at(0,0); - H.at(0,2) = cv::Mat(cv::Mat(f1coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs3)).at(0,0); - - H.at(1,0) = cv::Mat(cv::Mat(f2coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs1)).at(0,0); - H.at(1,1) = cv::Mat(cv::Mat(f2coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs2)).at(0,0); - H.at(1,2) = cv::Mat(cv::Mat(f2coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs3)).at(0,0); - - H.at(2,0) = cv::Mat(cv::Mat(f3coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs1)).at(0,0); - H.at(2,1) = cv::Mat(cv::Mat(f3coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs2)).at(0,0); - H.at(2,2) = cv::Mat(cv::Mat(f3coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs3)).at(0,0); - - return H; + // 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 + cv::Mat H(3, 3, CV_64F); + H.at(0,0) = cv::Mat(cv::Mat(f1coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs1)).at(0,0); + H.at(0,1) = cv::Mat(cv::Mat(f1coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs2)).at(0,0); + H.at(0,2) = cv::Mat(cv::Mat(f1coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs3)).at(0,0); + + H.at(1,0) = cv::Mat(cv::Mat(f2coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs1)).at(0,0); + H.at(1,1) = cv::Mat(cv::Mat(f2coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs2)).at(0,0); + H.at(1,2) = cv::Mat(cv::Mat(f2coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs3)).at(0,0); + + H.at(2,0) = cv::Mat(cv::Mat(f3coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs1)).at(0,0); + H.at(2,1) = cv::Mat(cv::Mat(f3coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs2)).at(0,0); + H.at(2,2) = cv::Mat(cv::Mat(f3coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs3)).at(0,0); + + return H; } cv::Mat dls::cayley2rotbar(const cv::Mat& s) { - double s_mul1 = cv::Mat(s.t()*s).at(0,0); - cv::Mat s_mul2 = s*s.t(); - cv::Mat eye = cv::Mat::eye(3, 3, CV_64F); + double s_mul1 = cv::Mat(s.t()*s).at(0,0); + cv::Mat s_mul2 = s*s.t(); + cv::Mat eye = cv::Mat::eye(3, 3, CV_64F); - return cv::Mat( eye.mul(1.-s_mul1) + skewsymm(&s).mul(2.) + s_mul2.mul(2.) ).t(); + return cv::Mat( eye.mul(1.-s_mul1) + skewsymm(&s).mul(2.) + s_mul2.mul(2.) ).t(); } cv::Mat dls::skewsymm(const cv::Mat * X1) { - cv::MatConstIterator_ it = X1->begin(); - return (cv::Mat_(3,3) << 0, -*(it+2), *(it+1), - *(it+2), 0, -*(it+0), - -*(it+1), *(it+0), 0); + cv::MatConstIterator_ it = X1->begin(); + return (cv::Mat_(3,3) << 0, -*(it+2), *(it+1), + *(it+2), 0, -*(it+0), + -*(it+1), *(it+0), 0); } cv::Mat dls::rotx(const double t) { - // rotx: rotation about y-axis - double ct = cos(t); - double st = sin(t); - return (cv::Mat_(3,3) << 1, 0, 0, 0, ct, -st, 0, st, ct); + // rotx: rotation about y-axis + double ct = cos(t); + double st = sin(t); + return (cv::Mat_(3,3) << 1, 0, 0, 0, ct, -st, 0, st, ct); } cv::Mat dls::roty(const double t) { - // roty: rotation about y-axis - double ct = cos(t); - double st = sin(t); - return (cv::Mat_(3,3) << ct, 0, st, 0, 1, 0, -st, 0, ct); + // roty: rotation about y-axis + double ct = cos(t); + double st = sin(t); + return (cv::Mat_(3,3) << ct, 0, st, 0, 1, 0, -st, 0, ct); } cv::Mat dls::rotz(const double t) { - // rotz: rotation about y-axis - double ct = cos(t); - double st = sin(t); - return (cv::Mat_(3,3) << ct, -st, 0, st, ct, 0, 0, 0, 1); + // rotz: rotation about y-axis + double ct = cos(t); + double st = sin(t); + return (cv::Mat_(3,3) << ct, -st, 0, st, ct, 0, 0, 0, 1); } cv::Mat dls::mean(const cv::Mat& M) { - cv::Mat m = cv::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); + cv::Mat m = cv::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 cv::Mat * M) { - cv::MatConstIterator_ it = M->begin(), it_end = M->end(); - for(; it != it_end; ++it) - { - if(*it < 0) return false; - } - return true; + cv::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 cv::Mat * eigenvalues) { - cv::MatConstIterator_ it = eigenvalues->begin(); - return *(it) > 0 && *(it+1) > 0 && *(it+2) > 0; + cv::MatConstIterator_ it = eigenvalues->begin(); + return *(it) > 0 && *(it+1) > 0 && *(it+2) > 0; } diff --git a/modules/calib3d/src/dls.h b/modules/calib3d/src/dls.h index 83f014f805..ba6c40a79a 100644 --- a/modules/calib3d/src/dls.h +++ b/modules/calib3d/src/dls.h @@ -9,61 +9,61 @@ using namespace cv; class dls { public: - dls(const cv::Mat& opoints, const cv::Mat& ipoints); - ~dls(); + dls(const cv::Mat& opoints, const cv::Mat& ipoints); + ~dls(); - bool compute_pose(cv::Mat& R, cv::Mat& t); + bool compute_pose(cv::Mat& R, cv::Mat& t); private: - // initialisation - template - void init_points(const cv::Mat& opoints, const cv::Mat& ipoints) - { - for(int i = 0; i < N; i++) - { - p.at(0,i) = opoints.at(0,i).x; - p.at(1,i) = opoints.at(0,i).y; - p.at(2,i) = opoints.at(0,i).z; - - z.at(0,i) = ipoints.at(0,i).x; - z.at(1,i) = ipoints.at(0,i).y; - z.at(2,i) = (double)1; - } - } - - void norm_z_vector(); - - // main algorithm - void run_kernel(const cv::Mat& pp); - void build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D); - void compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Mat& eigenval_imag, - cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag); - void fill_coeff(const cv::Mat * D); - - // useful functions - cv::Mat LeftMultVec(const cv::Mat& v); - cv::Mat cayley_LS_M(const std::vector& a, const std::vector& b, - const std::vector& c, const std::vector& u); - cv::Mat Hessian(const double s[]); - cv::Mat cayley2rotbar(const cv::Mat& s); - cv::Mat skewsymm(const cv::Mat * X1); - - // extra functions - cv::Mat rotx(const double t); - cv::Mat roty(const double t); - cv::Mat rotz(const double t); - cv::Mat mean(const cv::Mat& M); - bool is_empty(const cv::Mat * v); - bool positive_eigenvalues(const cv::Mat * eigenvalues); - - - cv::Mat p, z; // 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 - cv::Mat C_est__, t_est__; // optimal found solution - double cost__; // optimal found solution + // initialisation + template + void init_points(const cv::Mat& opoints, const cv::Mat& ipoints) + { + for(int i = 0; i < N; i++) + { + p.at(0,i) = opoints.at(0,i).x; + p.at(1,i) = opoints.at(0,i).y; + p.at(2,i) = opoints.at(0,i).z; + + z.at(0,i) = ipoints.at(0,i).x; + z.at(1,i) = ipoints.at(0,i).y; + z.at(2,i) = (double)1; + } + } + + void norm_z_vector(); + + // main algorithm + void run_kernel(const cv::Mat& pp); + void build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D); + void compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Mat& eigenval_imag, + cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag); + void fill_coeff(const cv::Mat * D); + + // useful functions + cv::Mat LeftMultVec(const cv::Mat& v); + cv::Mat cayley_LS_M(const std::vector& a, const std::vector& b, + const std::vector& c, const std::vector& u); + cv::Mat Hessian(const double s[]); + cv::Mat cayley2rotbar(const cv::Mat& s); + cv::Mat skewsymm(const cv::Mat * X1); + + // extra functions + cv::Mat rotx(const double t); + cv::Mat roty(const double t); + cv::Mat rotz(const double t); + cv::Mat mean(const cv::Mat& M); + bool is_empty(const cv::Mat * v); + bool positive_eigenvalues(const cv::Mat * eigenvalues); + + + cv::Mat p, z; // 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 + cv::Mat C_est__, t_est__; // optimal found solution + double cost__; // optimal found solution }; diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 4c1e80b2e2..7c2ebafbe8 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -96,15 +96,15 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints, } else if (flags == DLS) { - cv::Mat undistortedPoints; - cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); + cv::Mat undistortedPoints; + cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); - dls PnP(opoints, undistortedPoints); + dls PnP(opoints, undistortedPoints); - cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); - bool result = PnP.compute_pose(R, tvec); + cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); + bool result = PnP.compute_pose(R, tvec); if (result) - cv::Rodrigues(R, rvec); + cv::Rodrigues(R, rvec); return result; } else @@ -112,189 +112,14 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints, return false; } -/*namespace cv -{ - namespace pnpransac - { - const int MIN_POINTS_COUNT = 4; - - static void project3dPoints(const Mat& points, const Mat& rvec, const Mat& tvec, Mat& modif_points) - { - modif_points.create(1, points.cols, CV_32FC3); - Mat R(3, 3, CV_64FC1); - Rodrigues(rvec, R); - Mat transformation(3, 4, CV_64F); - Mat r = transformation.colRange(0, 3); - R.copyTo(r); - Mat t = transformation.colRange(3, 4); - tvec.copyTo(t); - transform(points, modif_points, transformation); - } - - struct CameraParameters - { - void init(Mat _intrinsics, Mat _distCoeffs) - { - _intrinsics.copyTo(intrinsics); - _distCoeffs.copyTo(distortion); - } - - Mat intrinsics; - Mat distortion; - }; - - struct Parameters - { - int iterationsCount; - float reprojectionError; - int minInliersCount; - bool useExtrinsicGuess; - int flags; - CameraParameters camera; - }; - - static void pnpTask(const std::vector& pointsMask, const Mat& objectPoints, const Mat& imagePoints, - const Parameters& params, std::vector& inliers, Mat& rvec, Mat& tvec, - const Mat& rvecInit, const Mat& tvecInit, Mutex& resultsMutex) - { - Mat modelObjectPoints(1, MIN_POINTS_COUNT, CV_32FC3), modelImagePoints(1, MIN_POINTS_COUNT, CV_32FC2); - for (int i = 0, colIndex = 0; i < (int)pointsMask.size(); i++) - { - if (pointsMask[i]) - { - Mat colModelImagePoints = modelImagePoints(Rect(colIndex, 0, 1, 1)); - imagePoints.col(i).copyTo(colModelImagePoints); - Mat colModelObjectPoints = modelObjectPoints(Rect(colIndex, 0, 1, 1)); - objectPoints.col(i).copyTo(colModelObjectPoints); - colIndex = colIndex+1; - } - } - - //filter same 3d points, hang in solvePnP - double eps = 1e-10; - int num_same_points = 0; - for (int i = 0; i < MIN_POINTS_COUNT; i++) - for (int j = i + 1; j < MIN_POINTS_COUNT; j++) - { - if (norm(modelObjectPoints.at(0, i) - modelObjectPoints.at(0, j)) < eps) - num_same_points++; - } - if (num_same_points > 0) - return; - - Mat localRvec, localTvec; - rvecInit.copyTo(localRvec); - tvecInit.copyTo(localTvec); - - solvePnP(modelObjectPoints, modelImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, - params.useExtrinsicGuess, params.flags); - - - std::vector projected_points; - projected_points.resize(objectPoints.cols); - projectPoints(objectPoints, localRvec, localTvec, params.camera.intrinsics, params.camera.distortion, projected_points); - - Mat rotatedPoints; - project3dPoints(objectPoints, localRvec, localTvec, rotatedPoints); - - std::vector localInliers; - for (int i = 0; i < objectPoints.cols; i++) - { - Point2f p(imagePoints.at(0, i)[0], imagePoints.at(0, i)[1]); - if ((norm(p - projected_points[i]) < params.reprojectionError) - && (rotatedPoints.at(0, i)[2] > 0)) //hack - { - localInliers.push_back(i); - } - } - - if (localInliers.size() > inliers.size()) - { - resultsMutex.lock(); - - inliers.clear(); - inliers.resize(localInliers.size()); - memcpy(&inliers[0], &localInliers[0], sizeof(int) * localInliers.size()); - localRvec.copyTo(rvec); - localTvec.copyTo(tvec); - - resultsMutex.unlock(); - } - } - - class PnPSolver - { - public: - void operator()( const BlockedRange& r ) const - { - std::vector pointsMask(objectPoints.cols, 0); - memset(&pointsMask[0], 1, MIN_POINTS_COUNT ); - for( int i=r.begin(); i!=r.end(); ++i ) - { - generateVar(pointsMask); - pnpTask(pointsMask, objectPoints, imagePoints, parameters, - inliers, rvec, tvec, initRvec, initTvec, syncMutex); - if ((int)inliers.size() >= parameters.minInliersCount) - { -#ifdef HAVE_TBB - tbb::task::self().cancel_group_execution(); -#else - break; -#endif - } - } - } - PnPSolver(const Mat& _objectPoints, const Mat& _imagePoints, const Parameters& _parameters, - Mat& _rvec, Mat& _tvec, std::vector& _inliers): - objectPoints(_objectPoints), imagePoints(_imagePoints), parameters(_parameters), - rvec(_rvec), tvec(_tvec), inliers(_inliers) - { - rvec.copyTo(initRvec); - tvec.copyTo(initTvec); - - generator.state = theRNG().state; //to control it somehow... - } - private: - PnPSolver& operator=(const PnPSolver&); - - const Mat& objectPoints; - const Mat& imagePoints; - const Parameters& parameters; - Mat &rvec, &tvec; - std::vector& inliers; - Mat initRvec, initTvec; - - static RNG generator; - static Mutex syncMutex; - - void generateVar(std::vector& mask) const - { - int size = (int)mask.size(); - for (int i = 0; i < size; i++) - { - int i1 = generator.uniform(0, size); - int i2 = generator.uniform(0, size); - char curr = mask[i1]; - mask[i1] = mask[i2]; - mask[i2] = curr; - } - } - }; - - Mutex PnPSolver::syncMutex; - RNG PnPSolver::generator; - - } -}*/ - class PnPRansacCallback : public PointSetRegistrator::Callback { public: - PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=cv::ITERATIVE, - bool _useExtrinsicGuess=false, Mat _rvec=Mat(), Mat _tvec=Mat() ) - : cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess), - rvec(_rvec), tvec(_tvec) {} + PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=cv::ITERATIVE, + bool _useExtrinsicGuess=false, Mat _rvec=Mat(), Mat _tvec=Mat() ) + : cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess), + rvec(_rvec), tvec(_tvec) {} /* Pre: True */ /* Post: compute _model with given points an return number of found models */ @@ -303,11 +128,11 @@ public: Mat opoints = _m1.getMat(), ipoints = _m2.getMat(); bool correspondence = cv::solvePnP( _m1, _m2, cameraMatrix, distCoeffs, - rvec, tvec, useExtrinsicGuess, flags ); + rvec, tvec, useExtrinsicGuess, flags ); - Mat _local_model; - cv::hconcat(rvec, tvec, _local_model); - _local_model.copyTo(_model); + Mat _local_model; + cv::hconcat(rvec, tvec, _local_model); + _local_model.copyTo(_model); return correspondence; } @@ -333,7 +158,7 @@ public: float* err = _err.getMat().ptr(); for ( i = 0; i < count; ++i) - err[i] = cv::norm( ipoints_ptr[i] - projpoints_ptr[i] ); + err[i] = cv::norm( ipoints_ptr[i] - projpoints_ptr[i] ); } @@ -375,89 +200,46 @@ bool cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, Ptr cb; // pointer to callback cb = makePtr( cameraMatrix, distCoeffs, flags, useExtrinsicGuess, rvec, tvec); - int model_points = flags == cv::P3P ? 4 : 6; // minimum of number of model points - double param1 = reprojectionError; // reprojection error - double param2 = confidence; // confidence - int param3 = iterationsCount; // number maximum iterations + int model_points = flags == cv::P3P ? 4 : 6; // minimum of number of model points + double param1 = reprojectionError; // reprojection error + double param2 = confidence; // confidence + int param3 = iterationsCount; // number maximum iterations cv::Mat _local_model(3, 2, CV_64FC1); cv::Mat _mask_local_inliers(1, opoints.rows, CV_8UC1); - // call Ransac - int result = createRANSACPointSetRegistrator(cb, model_points, param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers); - - if( result <= 0 || _local_model.rows <= 0) - { - _rvec.assign(rvec); // output rotation vector - _tvec.assign(tvec); // output translation vector - - if( _inliers.needed() ) - _inliers.release(); - - return false; - } - else - { - _rvec.assign(_local_model.col(0)); // output rotation vector - _tvec.assign(_local_model.col(1)); // output translation vector - } - - if(_inliers.needed()) - { - Mat _local_inliers; - int count = 0; - for (int i = 0; i < _mask_local_inliers.rows; ++i) - { - if((int)_mask_local_inliers.at(i) == 1) // inliers mask - { - _local_inliers.push_back(count); // output inliers vector - count++; - } - } - _local_inliers.copyTo(_inliers); - } - - // OLD IMPLEMENTATION - - /*std::vector localInliers; - Mat localRvec, localTvec; - rvec.copyTo(localRvec); - tvec.copyTo(localTvec); - - if (objectPoints.cols >= pnpransac::MIN_POINTS_COUNT) + // call Ransac + int result = createRANSACPointSetRegistrator(cb, model_points, param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers); + + if( result <= 0 || _local_model.rows <= 0) { - parallel_for(BlockedRange(0,iterationsCount), cv::pnpransac::PnPSolver(objectPoints, imagePoints, params, - localRvec, localTvec, localInliers)); + _rvec.assign(rvec); // output rotation vector + _tvec.assign(tvec); // output translation vector + + if( _inliers.needed() ) + _inliers.release(); + + return false; + } + else + { + _rvec.assign(_local_model.col(0)); // output rotation vector + _tvec.assign(_local_model.col(1)); // output translation vector } - if (localInliers.size() >= (size_t)pnpransac::MIN_POINTS_COUNT) + if(_inliers.needed()) { - if (flags != P3P) + Mat _local_inliers; + int count = 0; + for (int i = 0; i < _mask_local_inliers.rows; ++i) { - int i, pointsCount = (int)localInliers.size(); - Mat inlierObjectPoints(1, pointsCount, CV_32FC3), inlierImagePoints(1, pointsCount, CV_32FC2); - for (i = 0; i < pointsCount; i++) + if((int)_mask_local_inliers.at(i) == 1) // inliers mask { - int index = localInliers[i]; - Mat colInlierImagePoints = inlierImagePoints(Rect(i, 0, 1, 1)); - imagePoints.col(index).copyTo(colInlierImagePoints); - Mat colInlierObjectPoints = inlierObjectPoints(Rect(i, 0, 1, 1)); - objectPoints.col(index).copyTo(colInlierObjectPoints); + _local_inliers.push_back(count); // output inliers vector + count++; } - solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, true, flags); } - localRvec.copyTo(rvec); - localTvec.copyTo(tvec); - if (_inliers.needed()) - Mat(localInliers).copyTo(_inliers); + _local_inliers.copyTo(_inliers); } - else - { - tvec.setTo(Scalar(0)); - Mat R = Mat::eye(3, 3, CV_64F); - Rodrigues(R, rvec); - if( _inliers.needed() ) - _inliers.release(); - }*/ return true; } diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/CMakeLists.txt b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/CMakeLists.txt index 4382fd1e68..629c437117 100644 --- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/CMakeLists.txt +++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/CMakeLists.txt @@ -2,58 +2,58 @@ cmake_minimum_required(VERSION 2.8) project( PNP_DEMO ) ADD_DEFINITIONS( - -std=c++11 + -std=c++11 # Other flags ) find_package( OpenCV REQUIRED ) include_directories( - ${OpenCV_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} ) -add_executable( -pnp_registration - src/main_registration.cpp - src/CsvReader.cpp - src/CsvWriter.cpp - src/ModelRegistration.cpp - src/Mesh.cpp - src/Model.cpp - src/PnPProblem.cpp - src/Utils.cpp - src/RobustMatcher.cpp +add_executable( +pnp_registration + src/main_registration.cpp + src/CsvReader.cpp + src/CsvWriter.cpp + src/ModelRegistration.cpp + src/Mesh.cpp + src/Model.cpp + src/PnPProblem.cpp + src/Utils.cpp + src/RobustMatcher.cpp ) -add_executable( -pnp_verification - src/main_verification.cpp - src/CsvReader.cpp - src/CsvWriter.cpp - src/ModelRegistration.cpp - src/Mesh.cpp - src/Model.cpp - src/PnPProblem.cpp - src/Utils.cpp - src/RobustMatcher.cpp +add_executable( +pnp_verification + src/main_verification.cpp + src/CsvReader.cpp + src/CsvWriter.cpp + src/ModelRegistration.cpp + src/Mesh.cpp + src/Model.cpp + src/PnPProblem.cpp + src/Utils.cpp + src/RobustMatcher.cpp ) -add_executable( +add_executable( pnp_detection - src/main_detection.cpp - src/CsvReader.cpp - src/CsvWriter.cpp - src/ModelRegistration.cpp - src/Mesh.cpp - src/Model.cpp - src/PnPProblem.cpp - src/Utils.cpp - src/RobustMatcher.cpp + src/main_detection.cpp + src/CsvReader.cpp + src/CsvWriter.cpp + src/ModelRegistration.cpp + src/Mesh.cpp + src/Model.cpp + src/PnPProblem.cpp + src/Utils.cpp + src/RobustMatcher.cpp ) -add_executable( -pnp_test - src/test_pnp.cpp +add_executable( +pnp_test + src/test_pnp.cpp ) target_link_libraries( pnp_registration ${OpenCV_LIBS} )