Main MRPT website > C++ reference for MRPT 1.9.9
dls.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
12 #include <iostream>
13 #include <mrpt/config.h>
14 #include <mrpt/otherlibs/do_opencv_includes.h>
15 
16 // Opencv 2.3 had a broken <opencv/eigen.h> in Ubuntu 14.04 Trusty => Disable
17 // PNP classes
18 #include <mrpt/config.h>
19 #if MRPT_HAS_OPENCV && MRPT_OPENCV_VERSION_NUM < 0x140
20 #undef MRPT_HAS_OPENCV
21 #define MRPT_HAS_OPENCV 0
22 #endif
23 
24 #if MRPT_HAS_OPENCV
25 
26 #include "dls.h"
27 
28 mrpt::vision::pnp::dls::dls(const cv::Mat& opoints, const cv::Mat& ipoints)
29 {
30  N = std::max(
31  opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
32  p = cv::Mat(3, N, CV_64F);
33  z = cv::Mat(3, N, CV_64F);
34  mn = cv::Mat::zeros(3, 1, CV_64F);
35 
36  cost__ = 9999;
37 
38  f1coeff.resize(21);
39  f2coeff.resize(21);
40  f3coeff.resize(21);
41 
42  if (opoints.depth() == ipoints.depth())
43  {
44  if (opoints.depth() == CV_32F)
45  init_points<cv::Point3f, cv::Point2f>(opoints, ipoints);
46  else
47  init_points<cv::Point3d, cv::Point2d>(opoints, ipoints);
48  }
49  else if (opoints.depth() == CV_32F)
50  init_points<cv::Point3f, cv::Point2d>(opoints, ipoints);
51  else
52  init_points<cv::Point3d, cv::Point2f>(opoints, ipoints);
53 }
54 
55 mrpt::vision::pnp::dls::~dls()
56 {
57  // TODO Auto-generated destructor stub
58 }
59 
60 bool mrpt::vision::pnp::dls::compute_pose(cv::Mat& R, cv::Mat& t)
61 {
62  std::vector<cv::Mat> R_;
63  R_.push_back(rotx(CV_PI / 2));
64  R_.push_back(roty(CV_PI / 2));
65  R_.push_back(rotz(CV_PI / 2));
66 
67  // version that calls dls 3 times, to avoid Cayley singularity
68  for (int i = 0; i < 3; ++i)
69  {
70  // Make a random rotation
71  cv::Mat pp = R_[i] * (p - cv::repeat(mn, 1, p.cols));
72 
73  // clear for new data
74  C_est_.clear();
75  t_est_.clear();
76  cost_.clear();
77 
78  this->run_kernel(pp); // run dls_pnp()
79 
80  // find global minimum
81  for (unsigned int j = 0; j < cost_.size(); ++j)
82  {
83  if (cost_[j] < cost__)
84  {
85  t_est__ = t_est_[j] - C_est_[j] * R_[i] * mn;
86  C_est__ = C_est_[j] * R_[i];
87  cost__ = cost_[j];
88  }
89  }
90  }
91 
92  if (C_est__.cols > 0 && C_est__.rows > 0)
93  {
94  C_est__.copyTo(R);
95  t_est__.copyTo(t);
96  return true;
97  }
98 
99  return false;
100 }
101 
102 void mrpt::vision::pnp::dls::run_kernel(const cv::Mat& pp)
103 {
104 #if MRPT_OPENCV_VERSION_NUM >= 0x240
105  cv::Mat Mtilde(27, 27, CV_64F);
106  cv::Mat D = cv::Mat::zeros(9, 9, CV_64F);
107  build_coeff_matrix(pp, Mtilde, D);
108 
109  cv::Mat eigenval_r, eigenval_i, eigenvec_r, eigenvec_i;
110  compute_eigenvec(Mtilde, eigenval_r, eigenval_i, eigenvec_r, eigenvec_i);
111 
112  /*
113  * Now check the solutions
114  */
115 
116  // extract the optimal solutions from the eigen decomposition of the
117  // Multiplication matrix
118 
119  cv::Mat sols = cv::Mat::zeros(3, 27, CV_64F);
120  std::vector<double> cost;
121  int count = 0;
122  for (int k = 0; k < 27; ++k)
123  {
124  // V(:,k) = V(:,k)/V(1,k);
125  cv::Mat V_kA = eigenvec_r.col(k); // 27x1
126  cv::Mat V_kB = cv::Mat(1, 1, z.depth(), V_kA.at<double>(0)); // 1x1
127  cv::Mat V_k;
128  cv::solve(V_kB.t(), V_kA.t(), V_k); // A/B = B'\A'
129  cv::Mat(V_k.t()).copyTo(eigenvec_r.col(k));
130 
131 // if (imag(V(2,k)) == 0)
132 #ifdef HAVE_EIGEN
133  const double epsilon = 1e-4;
134  if (eigenval_i.at<double>(k, 0) >= -epsilon &&
135  eigenval_i.at<double>(k, 0) <= epsilon)
136 #endif
137  {
138  double stmp[3];
139  stmp[0] = eigenvec_r.at<double>(9, k);
140  stmp[1] = eigenvec_r.at<double>(3, k);
141  stmp[2] = eigenvec_r.at<double>(1, k);
142 
143  cv::Mat H = Hessian(stmp);
144 
145  cv::Mat eigenvalues, eigenvectors;
146  cv::eigen(H, eigenvalues, eigenvectors);
147 
148  if (positive_eigenvalues(&eigenvalues))
149  {
150  // sols(:,i) = stmp;
151  cv::Mat stmp_mat(3, 1, CV_64F, &stmp);
152 
153  stmp_mat.copyTo(sols.col(count));
154 
155  cv::Mat Cbar = cayley2rotbar(stmp_mat);
156  cv::Mat Cbarvec = Cbar.reshape(1, 1).t();
157 
158  // cost(i) = CbarVec' * D * CbarVec;
159  cv::Mat cost_mat = Cbarvec.t() * D * Cbarvec;
160  cost.push_back(cost_mat.at<double>(0));
161 
162  count++;
163  }
164  }
165  }
166 
167  // extract solutions
168  sols = sols.clone().colRange(0, count);
169 
170  std::vector<cv::Mat> C_est, t_est;
171  for (int j = 0; j < sols.cols; ++j)
172  {
173  // recover the optimal orientation
174  // C_est(:,:,j) = 1/(1 + sols(:,j)' * sols(:,j)) *
175  // cayley2rotbar(sols(:,j));
176 
177  cv::Mat sols_j = sols.col(j);
178  double sols_mult =
179  1. / (1. + cv::Mat(sols_j.t() * sols_j).at<double>(0));
180  cv::Mat C_est_j = cayley2rotbar(sols_j).mul(sols_mult);
181  C_est.push_back(C_est_j);
182 
183  cv::Mat A2 = cv::Mat::zeros(3, 3, CV_64F);
184  cv::Mat b2 = cv::Mat::zeros(3, 1, CV_64F);
185  for (int i = 0; i < N; ++i)
186  {
187  cv::Mat eye = cv::Mat::eye(3, 3, CV_64F);
188  cv::Mat z_mul = z.col(i) * z.col(i).t();
189 
190  A2 += eye - z_mul;
191  b2 += (z_mul - eye) * C_est_j * pp.col(i);
192  }
193 
194  // recover the optimal translation
195  cv::Mat X2;
196  cv::solve(A2, b2, X2); // A\B
197  t_est.push_back(X2);
198  }
199 
200  // check that the points are infront of the center of perspectivity
201  for (int k = 0; k < sols.cols; ++k)
202  {
203  cv::Mat cam_points = C_est[k] * pp + cv::repeat(t_est[k], 1, pp.cols);
204  cv::Mat cam_points_k = cam_points.row(2);
205 
206  if (is_empty(&cam_points_k))
207  {
208  cv::Mat C_valid = C_est[k], t_valid = t_est[k];
209  double cost_valid = cost[k];
210 
211  C_est_.push_back(C_valid);
212  t_est_.push_back(t_valid);
213  cost_.push_back(cost_valid);
214  }
215  }
216 #else
217  THROW_EXCEPTION("Fix me: this method doesn't work with OpenCV < 2.4");
218 #endif
219 }
220 
221 void mrpt::vision::pnp::dls::build_coeff_matrix(
222  const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D)
223 {
224  cv::Mat eye = cv::Mat::eye(3, 3, CV_64F);
225 
226  // build coeff matrix
227  // An intermediate matrix, the inverse of what is called "H" in the paper
228  // (see eq. 25)
229 
230  cv::Mat H = cv::Mat::zeros(3, 3, CV_64F);
231  cv::Mat A = cv::Mat::zeros(3, 9, CV_64F);
232  cv::Mat pp_i(3, 1, CV_64F);
233 
234  cv::Mat z_i(3, 1, CV_64F);
235  for (int i = 0; i < N; ++i)
236  {
237  z.col(i).copyTo(z_i);
238  A += (z_i * z_i.t() - eye) * LeftMultVec(pp.col(i));
239  }
240 
241  H = eye.mul(N) - z * z.t();
242 
243  // A\B
244  cv::solve(H, A, A, cv::DECOMP_NORMAL);
245  H.release();
246 
247  cv::Mat ppi_A(3, 1, CV_64F);
248  for (int i = 0; i < N; ++i)
249  {
250  z.col(i).copyTo(z_i);
251  ppi_A = LeftMultVec(pp.col(i)) + A;
252  D += ppi_A.t() * (eye - z_i * z_i.t()) * ppi_A;
253  }
254  A.release();
255 
256  // fill the coefficients
257  fill_coeff(&D);
258 
259  // generate random samples
260  std::vector<double> u(5);
261  cv::randn(u, 0, 200);
262 
263  cv::Mat M2 = cayley_LS_M(f1coeff, f2coeff, f3coeff, u);
264 
265  cv::Mat M2_1 = M2(cv::Range(0, 27), cv::Range(0, 27));
266  cv::Mat M2_2 = M2(cv::Range(0, 27), cv::Range(27, 120));
267  cv::Mat M2_3 = M2(cv::Range(27, 120), cv::Range(27, 120));
268  cv::Mat M2_4 = M2(cv::Range(27, 120), cv::Range(0, 27));
269  M2.release();
270 
271  // A/B = B'\A'
272  cv::Mat M2_5;
273  cv::solve(M2_3.t(), M2_2.t(), M2_5);
274  M2_2.release();
275  M2_3.release();
276 
277  // construct the multiplication matrix via schur compliment of the Macaulay
278  // matrix
279  Mtilde = M2_1 - M2_5.t() * M2_4;
280 }
281 
282 void mrpt::vision::pnp::dls::compute_eigenvec(
283  const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Mat& eigenval_imag,
284  cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag)
285 {
286 #ifdef HAVE_EIGEN
287  Eigen::MatrixXd Mtilde_eig, zeros_eig;
288  cv::cv2eigen(Mtilde, Mtilde_eig);
289  cv::cv2eigen(cv::Mat::zeros(27, 27, CV_64F), zeros_eig);
290 
291  Eigen::MatrixXcd Mtilde_eig_cmplx(27, 27);
292  Mtilde_eig_cmplx.real() = Mtilde_eig;
293  Mtilde_eig_cmplx.imag() = zeros_eig;
294 
295  Eigen::ComplexEigenSolver<Eigen::MatrixXcd> ces;
296  ces.compute(Mtilde_eig_cmplx);
297 
298  Eigen::MatrixXd eigval_real = ces.eigenvalues().real();
299  Eigen::MatrixXd eigval_imag = ces.eigenvalues().imag();
300  Eigen::MatrixXd eigvec_real = ces.eigenvectors().real();
301  Eigen::MatrixXd eigvec_imag = ces.eigenvectors().imag();
302 
303  cv::eigen2cv(eigval_real, eigenval_real);
304  cv::eigen2cv(eigval_imag, eigenval_imag);
305  cv::eigen2cv(eigvec_real, eigenvec_real);
306  cv::eigen2cv(eigvec_imag, eigenvec_imag);
307 #else
308  EigenvalueDecomposition es(Mtilde);
309  eigenval_real = es.eigenvalues();
310  eigenvec_real = es.eigenvectors();
311  eigenval_imag = eigenvec_imag = cv::Mat();
312 #endif
313 }
314 
315 void mrpt::vision::pnp::dls::fill_coeff(const cv::Mat* D_mat)
316 {
317  // TODO: shift D and coefficients one position to left
318 
319  double D[10][10]; // put D_mat into array
320 
321  for (int i = 0; i < D_mat->rows; ++i)
322  {
323  const double* Di = D_mat->ptr<double>(i);
324  for (int j = 0; j < D_mat->cols; ++j)
325  {
326  D[i + 1][j + 1] = Di[j];
327  }
328  }
329 
330  // F1 COEFFICIENT
331 
332  f1coeff[1] = 2 * D[1][6] - 2 * D[1][8] + 2 * D[5][6] - 2 * D[5][8] +
333  2 * D[6][1] + 2 * D[6][5] + 2 * D[6][9] - 2 * D[8][1] -
334  2 * D[8][5] - 2 * D[8][9] + 2 * D[9][6] -
335  2 * D[9][8]; // constant term
336  f1coeff[2] = 6 * D[1][2] + 6 * D[1][4] + 6 * D[2][1] - 6 * D[2][5] -
337  6 * D[2][9] + 6 * D[4][1] - 6 * D[4][5] - 6 * D[4][9] -
338  6 * D[5][2] - 6 * D[5][4] - 6 * D[9][2] -
339  6 * D[9][4]; // s1^2 * s2
340  f1coeff[3] = 4 * D[1][7] - 4 * D[1][3] + 8 * D[2][6] - 8 * D[2][8] -
341  4 * D[3][1] + 4 * D[3][5] + 4 * D[3][9] + 8 * D[4][6] -
342  8 * D[4][8] + 4 * D[5][3] - 4 * D[5][7] + 8 * D[6][2] +
343  8 * D[6][4] + 4 * D[7][1] - 4 * D[7][5] - 4 * D[7][9] -
344  8 * D[8][2] - 8 * D[8][4] + 4 * D[9][3] -
345  4 * D[9][7]; // s1 * s2
346  f1coeff[4] = 4 * D[1][2] - 4 * D[1][4] + 4 * D[2][1] - 4 * D[2][5] -
347  4 * D[2][9] + 8 * D[3][6] - 8 * D[3][8] - 4 * D[4][1] +
348  4 * D[4][5] + 4 * D[4][9] - 4 * D[5][2] + 4 * D[5][4] +
349  8 * D[6][3] + 8 * D[6][7] + 8 * D[7][6] - 8 * D[7][8] -
350  8 * D[8][3] - 8 * D[8][7] - 4 * D[9][2] +
351  4 * D[9][4]; // s1 * s3
352  f1coeff[5] = 8 * D[2][2] - 8 * D[3][3] - 8 * D[4][4] + 8 * D[6][6] +
353  8 * D[7][7] - 8 * D[8][8]; // s2 * s3
354  f1coeff[6] = 4 * D[2][6] - 2 * D[1][7] - 2 * D[1][3] + 4 * D[2][8] -
355  2 * D[3][1] + 2 * D[3][5] - 2 * D[3][9] + 4 * D[4][6] +
356  4 * D[4][8] + 2 * D[5][3] + 2 * D[5][7] + 4 * D[6][2] +
357  4 * D[6][4] - 2 * D[7][1] + 2 * D[7][5] - 2 * D[7][9] +
358  4 * D[8][2] + 4 * D[8][4] - 2 * D[9][3] -
359  2 * D[9][7]; // s2^2 * s3
360  f1coeff[7] = 2 * D[2][5] - 2 * D[1][4] - 2 * D[2][1] - 2 * D[1][2] -
361  2 * D[2][9] - 2 * D[4][1] + 2 * D[4][5] - 2 * D[4][9] +
362  2 * D[5][2] + 2 * D[5][4] - 2 * D[9][2] - 2 * D[9][4]; // s2^3
363  f1coeff[8] = 4 * D[1][9] - 4 * D[1][1] + 8 * D[3][3] + 8 * D[3][7] +
364  4 * D[5][5] + 8 * D[7][3] + 8 * D[7][7] + 4 * D[9][1] -
365  4 * D[9][9]; // s1 * s3^2
366  f1coeff[9] = 4 * D[1][1] - 4 * D[5][5] - 4 * D[5][9] + 8 * D[6][6] -
367  8 * D[6][8] - 8 * D[8][6] + 8 * D[8][8] - 4 * D[9][5] -
368  4 * D[9][9]; // s1
369  f1coeff[10] = 2 * D[1][3] + 2 * D[1][7] + 4 * D[2][6] - 4 * D[2][8] +
370  2 * D[3][1] + 2 * D[3][5] + 2 * D[3][9] - 4 * D[4][6] +
371  4 * D[4][8] + 2 * D[5][3] + 2 * D[5][7] + 4 * D[6][2] -
372  4 * D[6][4] + 2 * D[7][1] + 2 * D[7][5] + 2 * D[7][9] -
373  4 * D[8][2] + 4 * D[8][4] + 2 * D[9][3] + 2 * D[9][7]; // s3
374  f1coeff[11] = 2 * D[1][2] + 2 * D[1][4] + 2 * D[2][1] + 2 * D[2][5] +
375  2 * D[2][9] - 4 * D[3][6] + 4 * D[3][8] + 2 * D[4][1] +
376  2 * D[4][5] + 2 * D[4][9] + 2 * D[5][2] + 2 * D[5][4] -
377  4 * D[6][3] + 4 * D[6][7] + 4 * D[7][6] - 4 * D[7][8] +
378  4 * D[8][3] - 4 * D[8][7] + 2 * D[9][2] + 2 * D[9][4]; // s2
379  f1coeff[12] = 2 * D[2][9] - 2 * D[1][4] - 2 * D[2][1] - 2 * D[2][5] -
380  2 * D[1][2] + 4 * D[3][6] + 4 * D[3][8] - 2 * D[4][1] -
381  2 * D[4][5] + 2 * D[4][9] - 2 * D[5][2] - 2 * D[5][4] +
382  4 * D[6][3] + 4 * D[6][7] + 4 * D[7][6] + 4 * D[7][8] +
383  4 * D[8][3] + 4 * D[8][7] + 2 * D[9][2] +
384  2 * D[9][4]; // s2 * s3^2
385  f1coeff[13] = 6 * D[1][6] - 6 * D[1][8] - 6 * D[5][6] + 6 * D[5][8] +
386  6 * D[6][1] - 6 * D[6][5] - 6 * D[6][9] - 6 * D[8][1] +
387  6 * D[8][5] + 6 * D[8][9] - 6 * D[9][6] +
388  6 * D[9][8]; // s1^2
389  f1coeff[14] = 2 * D[1][8] - 2 * D[1][6] + 4 * D[2][3] + 4 * D[2][7] +
390  4 * D[3][2] - 4 * D[3][4] - 4 * D[4][3] - 4 * D[4][7] -
391  2 * D[5][6] + 2 * D[5][8] - 2 * D[6][1] - 2 * D[6][5] +
392  2 * D[6][9] + 4 * D[7][2] - 4 * D[7][4] + 2 * D[8][1] +
393  2 * D[8][5] - 2 * D[8][9] + 2 * D[9][6] -
394  2 * D[9][8]; // s3^2
395  f1coeff[15] = 2 * D[1][8] - 2 * D[1][6] - 4 * D[2][3] + 4 * D[2][7] -
396  4 * D[3][2] - 4 * D[3][4] - 4 * D[4][3] + 4 * D[4][7] +
397  2 * D[5][6] - 2 * D[5][8] - 2 * D[6][1] + 2 * D[6][5] -
398  2 * D[6][9] + 4 * D[7][2] + 4 * D[7][4] + 2 * D[8][1] -
399  2 * D[8][5] + 2 * D[8][9] - 2 * D[9][6] +
400  2 * D[9][8]; // s2^2
401  f1coeff[16] = 2 * D[3][9] - 2 * D[1][7] - 2 * D[3][1] - 2 * D[3][5] -
402  2 * D[1][3] - 2 * D[5][3] - 2 * D[5][7] - 2 * D[7][1] -
403  2 * D[7][5] + 2 * D[7][9] + 2 * D[9][3] +
404  2 * D[9][7]; // s3^3
405  f1coeff[17] = 4 * D[1][6] + 4 * D[1][8] + 8 * D[2][3] + 8 * D[2][7] +
406  8 * D[3][2] + 8 * D[3][4] + 8 * D[4][3] + 8 * D[4][7] -
407  4 * D[5][6] - 4 * D[5][8] + 4 * D[6][1] - 4 * D[6][5] -
408  4 * D[6][9] + 8 * D[7][2] + 8 * D[7][4] + 4 * D[8][1] -
409  4 * D[8][5] - 4 * D[8][9] - 4 * D[9][6] -
410  4 * D[9][8]; // s1 * s2 * s3
411  f1coeff[18] = 4 * D[1][5] - 4 * D[1][1] + 8 * D[2][2] + 8 * D[2][4] +
412  8 * D[4][2] + 8 * D[4][4] + 4 * D[5][1] - 4 * D[5][5] +
413  4 * D[9][9]; // s1 * s2^2
414  f1coeff[19] = 6 * D[1][3] + 6 * D[1][7] + 6 * D[3][1] - 6 * D[3][5] -
415  6 * D[3][9] - 6 * D[5][3] - 6 * D[5][7] + 6 * D[7][1] -
416  6 * D[7][5] - 6 * D[7][9] - 6 * D[9][3] -
417  6 * D[9][7]; // s1^2 * s3
418  f1coeff[20] = 4 * D[1][1] - 4 * D[1][5] - 4 * D[1][9] - 4 * D[5][1] +
419  4 * D[5][5] + 4 * D[5][9] - 4 * D[9][1] + 4 * D[9][5] +
420  4 * D[9][9]; // s1^3
421 
422  // F2 COEFFICIENT
423 
424  f2coeff[1] = -2 * D[1][3] + 2 * D[1][7] - 2 * D[3][1] - 2 * D[3][5] -
425  2 * D[3][9] - 2 * D[5][3] + 2 * D[5][7] + 2 * D[7][1] +
426  2 * D[7][5] + 2 * D[7][9] - 2 * D[9][3] +
427  2 * D[9][7]; // constant term
428  f2coeff[2] = 4 * D[1][5] - 4 * D[1][1] + 8 * D[2][2] + 8 * D[2][4] +
429  8 * D[4][2] + 8 * D[4][4] + 4 * D[5][1] - 4 * D[5][5] +
430  4 * D[9][9]; // s1^2 * s2
431  f2coeff[3] = 4 * D[1][8] - 4 * D[1][6] - 8 * D[2][3] + 8 * D[2][7] -
432  8 * D[3][2] - 8 * D[3][4] - 8 * D[4][3] + 8 * D[4][7] +
433  4 * D[5][6] - 4 * D[5][8] - 4 * D[6][1] + 4 * D[6][5] -
434  4 * D[6][9] + 8 * D[7][2] + 8 * D[7][4] + 4 * D[8][1] -
435  4 * D[8][5] + 4 * D[8][9] - 4 * D[9][6] +
436  4 * D[9][8]; // s1 * s2
437  f2coeff[4] = 8 * D[2][2] - 8 * D[3][3] - 8 * D[4][4] + 8 * D[6][6] +
438  8 * D[7][7] - 8 * D[8][8]; // s1 * s3
439  f2coeff[5] = 4 * D[1][4] - 4 * D[1][2] - 4 * D[2][1] + 4 * D[2][5] -
440  4 * D[2][9] - 8 * D[3][6] - 8 * D[3][8] + 4 * D[4][1] -
441  4 * D[4][5] + 4 * D[4][9] + 4 * D[5][2] - 4 * D[5][4] -
442  8 * D[6][3] + 8 * D[6][7] + 8 * D[7][6] + 8 * D[7][8] -
443  8 * D[8][3] + 8 * D[8][7] - 4 * D[9][2] +
444  4 * D[9][4]; // s2 * s3
445  f2coeff[6] = 6 * D[5][6] - 6 * D[1][8] - 6 * D[1][6] + 6 * D[5][8] -
446  6 * D[6][1] + 6 * D[6][5] - 6 * D[6][9] - 6 * D[8][1] +
447  6 * D[8][5] - 6 * D[8][9] - 6 * D[9][6] -
448  6 * D[9][8]; // s2^2 * s3
449  f2coeff[7] = 4 * D[1][1] - 4 * D[1][5] + 4 * D[1][9] - 4 * D[5][1] +
450  4 * D[5][5] - 4 * D[5][9] + 4 * D[9][1] - 4 * D[9][5] +
451  4 * D[9][9]; // s2^3
452  f2coeff[8] = 2 * D[2][9] - 2 * D[1][4] - 2 * D[2][1] - 2 * D[2][5] -
453  2 * D[1][2] + 4 * D[3][6] + 4 * D[3][8] - 2 * D[4][1] -
454  2 * D[4][5] + 2 * D[4][9] - 2 * D[5][2] - 2 * D[5][4] +
455  4 * D[6][3] + 4 * D[6][7] + 4 * D[7][6] + 4 * D[7][8] +
456  4 * D[8][3] + 4 * D[8][7] + 2 * D[9][2] +
457  2 * D[9][4]; // s1 * s3^2
458  f2coeff[9] = 2 * D[1][2] + 2 * D[1][4] + 2 * D[2][1] + 2 * D[2][5] +
459  2 * D[2][9] - 4 * D[3][6] + 4 * D[3][8] + 2 * D[4][1] +
460  2 * D[4][5] + 2 * D[4][9] + 2 * D[5][2] + 2 * D[5][4] -
461  4 * D[6][3] + 4 * D[6][7] + 4 * D[7][6] - 4 * D[7][8] +
462  4 * D[8][3] - 4 * D[8][7] + 2 * D[9][2] + 2 * D[9][4]; // s1
463  f2coeff[10] = 2 * D[1][6] + 2 * D[1][8] - 4 * D[2][3] + 4 * D[2][7] -
464  4 * D[3][2] + 4 * D[3][4] + 4 * D[4][3] - 4 * D[4][7] +
465  2 * D[5][6] + 2 * D[5][8] + 2 * D[6][1] + 2 * D[6][5] +
466  2 * D[6][9] + 4 * D[7][2] - 4 * D[7][4] + 2 * D[8][1] +
467  2 * D[8][5] + 2 * D[8][9] + 2 * D[9][6] + 2 * D[9][8]; // s3
468  f2coeff[11] = 8 * D[3][3] - 4 * D[1][9] - 4 * D[1][1] - 8 * D[3][7] +
469  4 * D[5][5] - 8 * D[7][3] + 8 * D[7][7] - 4 * D[9][1] -
470  4 * D[9][9]; // s2
471  f2coeff[12] = 4 * D[1][1] - 4 * D[5][5] + 4 * D[5][9] + 8 * D[6][6] +
472  8 * D[6][8] + 8 * D[8][6] + 8 * D[8][8] + 4 * D[9][5] -
473  4 * D[9][9]; // s2 * s3^2
474  f2coeff[13] = 2 * D[1][7] - 2 * D[1][3] + 4 * D[2][6] - 4 * D[2][8] -
475  2 * D[3][1] + 2 * D[3][5] + 2 * D[3][9] + 4 * D[4][6] -
476  4 * D[4][8] + 2 * D[5][3] - 2 * D[5][7] + 4 * D[6][2] +
477  4 * D[6][4] + 2 * D[7][1] - 2 * D[7][5] - 2 * D[7][9] -
478  4 * D[8][2] - 4 * D[8][4] + 2 * D[9][3] -
479  2 * D[9][7]; // s1^2
480  f2coeff[14] = 2 * D[1][3] - 2 * D[1][7] + 4 * D[2][6] + 4 * D[2][8] +
481  2 * D[3][1] + 2 * D[3][5] - 2 * D[3][9] - 4 * D[4][6] -
482  4 * D[4][8] + 2 * D[5][3] - 2 * D[5][7] + 4 * D[6][2] -
483  4 * D[6][4] - 2 * D[7][1] - 2 * D[7][5] + 2 * D[7][9] +
484  4 * D[8][2] - 4 * D[8][4] - 2 * D[9][3] +
485  2 * D[9][7]; // s3^2
486  f2coeff[15] = 6 * D[1][3] - 6 * D[1][7] + 6 * D[3][1] - 6 * D[3][5] +
487  6 * D[3][9] - 6 * D[5][3] + 6 * D[5][7] - 6 * D[7][1] +
488  6 * D[7][5] - 6 * D[7][9] + 6 * D[9][3] -
489  6 * D[9][7]; // s2^2
490  f2coeff[16] = 2 * D[6][9] - 2 * D[1][8] - 2 * D[5][6] - 2 * D[5][8] -
491  2 * D[6][1] - 2 * D[6][5] - 2 * D[1][6] - 2 * D[8][1] -
492  2 * D[8][5] + 2 * D[8][9] + 2 * D[9][6] +
493  2 * D[9][8]; // s3^3
494  f2coeff[17] = 8 * D[2][6] - 4 * D[1][7] - 4 * D[1][3] + 8 * D[2][8] -
495  4 * D[3][1] + 4 * D[3][5] - 4 * D[3][9] + 8 * D[4][6] +
496  8 * D[4][8] + 4 * D[5][3] + 4 * D[5][7] + 8 * D[6][2] +
497  8 * D[6][4] - 4 * D[7][1] + 4 * D[7][5] - 4 * D[7][9] +
498  8 * D[8][2] + 8 * D[8][4] - 4 * D[9][3] -
499  4 * D[9][7]; // s1 * s2 * s3
500  f2coeff[18] = 6 * D[2][5] - 6 * D[1][4] - 6 * D[2][1] - 6 * D[1][2] -
501  6 * D[2][9] - 6 * D[4][1] + 6 * D[4][5] - 6 * D[4][9] +
502  6 * D[5][2] + 6 * D[5][4] - 6 * D[9][2] -
503  6 * D[9][4]; // s1 * s2^2
504  f2coeff[19] = 2 * D[1][6] + 2 * D[1][8] + 4 * D[2][3] + 4 * D[2][7] +
505  4 * D[3][2] + 4 * D[3][4] + 4 * D[4][3] + 4 * D[4][7] -
506  2 * D[5][6] - 2 * D[5][8] + 2 * D[6][1] - 2 * D[6][5] -
507  2 * D[6][9] + 4 * D[7][2] + 4 * D[7][4] + 2 * D[8][1] -
508  2 * D[8][5] - 2 * D[8][9] - 2 * D[9][6] -
509  2 * D[9][8]; // s1^2 * s3
510  f2coeff[20] = 2 * D[1][2] + 2 * D[1][4] + 2 * D[2][1] - 2 * D[2][5] -
511  2 * D[2][9] + 2 * D[4][1] - 2 * D[4][5] - 2 * D[4][9] -
512  2 * D[5][2] - 2 * D[5][4] - 2 * D[9][2] -
513  2 * D[9][4]; // s1^3
514 
515  // F3 COEFFICIENT
516 
517  f3coeff[1] = 2 * D[1][2] - 2 * D[1][4] + 2 * D[2][1] + 2 * D[2][5] +
518  2 * D[2][9] - 2 * D[4][1] - 2 * D[4][5] - 2 * D[4][9] +
519  2 * D[5][2] - 2 * D[5][4] + 2 * D[9][2] -
520  2 * D[9][4]; // constant term
521  f3coeff[2] = 2 * D[1][6] + 2 * D[1][8] + 4 * D[2][3] + 4 * D[2][7] +
522  4 * D[3][2] + 4 * D[3][4] + 4 * D[4][3] + 4 * D[4][7] -
523  2 * D[5][6] - 2 * D[5][8] + 2 * D[6][1] - 2 * D[6][5] -
524  2 * D[6][9] + 4 * D[7][2] + 4 * D[7][4] + 2 * D[8][1] -
525  2 * D[8][5] - 2 * D[8][9] - 2 * D[9][6] -
526  2 * D[9][8]; // s1^2 * s2
527  f3coeff[3] = 8 * D[2][2] - 8 * D[3][3] - 8 * D[4][4] + 8 * D[6][6] +
528  8 * D[7][7] - 8 * D[8][8]; // s1 * s2
529  f3coeff[4] = 4 * D[1][8] - 4 * D[1][6] + 8 * D[2][3] + 8 * D[2][7] +
530  8 * D[3][2] - 8 * D[3][4] - 8 * D[4][3] - 8 * D[4][7] -
531  4 * D[5][6] + 4 * D[5][8] - 4 * D[6][1] - 4 * D[6][5] +
532  4 * D[6][9] + 8 * D[7][2] - 8 * D[7][4] + 4 * D[8][1] +
533  4 * D[8][5] - 4 * D[8][9] + 4 * D[9][6] -
534  4 * D[9][8]; // s1 * s3
535  f3coeff[5] = 4 * D[1][3] - 4 * D[1][7] + 8 * D[2][6] + 8 * D[2][8] +
536  4 * D[3][1] + 4 * D[3][5] - 4 * D[3][9] - 8 * D[4][6] -
537  8 * D[4][8] + 4 * D[5][3] - 4 * D[5][7] + 8 * D[6][2] -
538  8 * D[6][4] - 4 * D[7][1] - 4 * D[7][5] + 4 * D[7][9] +
539  8 * D[8][2] - 8 * D[8][4] - 4 * D[9][3] +
540  4 * D[9][7]; // s2 * s3
541  f3coeff[6] = 4 * D[1][1] - 4 * D[5][5] + 4 * D[5][9] + 8 * D[6][6] +
542  8 * D[6][8] + 8 * D[8][6] + 8 * D[8][8] + 4 * D[9][5] -
543  4 * D[9][9]; // s2^2 * s3
544  f3coeff[7] = 2 * D[5][6] - 2 * D[1][8] - 2 * D[1][6] + 2 * D[5][8] -
545  2 * D[6][1] + 2 * D[6][5] - 2 * D[6][9] - 2 * D[8][1] +
546  2 * D[8][5] - 2 * D[8][9] - 2 * D[9][6] - 2 * D[9][8]; // s2^3
547  f3coeff[8] = 6 * D[3][9] - 6 * D[1][7] - 6 * D[3][1] - 6 * D[3][5] -
548  6 * D[1][3] - 6 * D[5][3] - 6 * D[5][7] - 6 * D[7][1] -
549  6 * D[7][5] + 6 * D[7][9] + 6 * D[9][3] +
550  6 * D[9][7]; // s1 * s3^2
551  f3coeff[9] = 2 * D[1][3] + 2 * D[1][7] + 4 * D[2][6] - 4 * D[2][8] +
552  2 * D[3][1] + 2 * D[3][5] + 2 * D[3][9] - 4 * D[4][6] +
553  4 * D[4][8] + 2 * D[5][3] + 2 * D[5][7] + 4 * D[6][2] -
554  4 * D[6][4] + 2 * D[7][1] + 2 * D[7][5] + 2 * D[7][9] -
555  4 * D[8][2] + 4 * D[8][4] + 2 * D[9][3] + 2 * D[9][7]; // s1
556  f3coeff[10] = 8 * D[2][2] - 4 * D[1][5] - 4 * D[1][1] - 8 * D[2][4] -
557  8 * D[4][2] + 8 * D[4][4] - 4 * D[5][1] - 4 * D[5][5] +
558  4 * D[9][9]; // s3
559  f3coeff[11] = 2 * D[1][6] + 2 * D[1][8] - 4 * D[2][3] + 4 * D[2][7] -
560  4 * D[3][2] + 4 * D[3][4] + 4 * D[4][3] - 4 * D[4][7] +
561  2 * D[5][6] + 2 * D[5][8] + 2 * D[6][1] + 2 * D[6][5] +
562  2 * D[6][9] + 4 * D[7][2] - 4 * D[7][4] + 2 * D[8][1] +
563  2 * D[8][5] + 2 * D[8][9] + 2 * D[9][6] + 2 * D[9][8]; // s2
564  f3coeff[12] = 6 * D[6][9] - 6 * D[1][8] - 6 * D[5][6] - 6 * D[5][8] -
565  6 * D[6][1] - 6 * D[6][5] - 6 * D[1][6] - 6 * D[8][1] -
566  6 * D[8][5] + 6 * D[8][9] + 6 * D[9][6] +
567  6 * D[9][8]; // s2 * s3^2
568  f3coeff[13] = 2 * D[1][2] - 2 * D[1][4] + 2 * D[2][1] - 2 * D[2][5] -
569  2 * D[2][9] + 4 * D[3][6] - 4 * D[3][8] - 2 * D[4][1] +
570  2 * D[4][5] + 2 * D[4][9] - 2 * D[5][2] + 2 * D[5][4] +
571  4 * D[6][3] + 4 * D[6][7] + 4 * D[7][6] - 4 * D[7][8] -
572  4 * D[8][3] - 4 * D[8][7] - 2 * D[9][2] +
573  2 * D[9][4]; // s1^2
574  f3coeff[14] = 6 * D[1][4] - 6 * D[1][2] - 6 * D[2][1] - 6 * D[2][5] +
575  6 * D[2][9] + 6 * D[4][1] + 6 * D[4][5] - 6 * D[4][9] -
576  6 * D[5][2] + 6 * D[5][4] + 6 * D[9][2] -
577  6 * D[9][4]; // s3^2
578  f3coeff[15] = 2 * D[1][4] - 2 * D[1][2] - 2 * D[2][1] + 2 * D[2][5] -
579  2 * D[2][9] - 4 * D[3][6] - 4 * D[3][8] + 2 * D[4][1] -
580  2 * D[4][5] + 2 * D[4][9] + 2 * D[5][2] - 2 * D[5][4] -
581  4 * D[6][3] + 4 * D[6][7] + 4 * D[7][6] + 4 * D[7][8] -
582  4 * D[8][3] + 4 * D[8][7] - 2 * D[9][2] +
583  2 * D[9][4]; // s2^2
584  f3coeff[16] = 4 * D[1][1] + 4 * D[1][5] - 4 * D[1][9] + 4 * D[5][1] +
585  4 * D[5][5] - 4 * D[5][9] - 4 * D[9][1] - 4 * D[9][5] +
586  4 * D[9][9]; // s3^3
587  f3coeff[17] = 4 * D[2][9] - 4 * D[1][4] - 4 * D[2][1] - 4 * D[2][5] -
588  4 * D[1][2] + 8 * D[3][6] + 8 * D[3][8] - 4 * D[4][1] -
589  4 * D[4][5] + 4 * D[4][9] - 4 * D[5][2] - 4 * D[5][4] +
590  8 * D[6][3] + 8 * D[6][7] + 8 * D[7][6] + 8 * D[7][8] +
591  8 * D[8][3] + 8 * D[8][7] + 4 * D[9][2] +
592  4 * D[9][4]; // s1 * s2 * s3
593  f3coeff[18] = 4 * D[2][6] - 2 * D[1][7] - 2 * D[1][3] + 4 * D[2][8] -
594  2 * D[3][1] + 2 * D[3][5] - 2 * D[3][9] + 4 * D[4][6] +
595  4 * D[4][8] + 2 * D[5][3] + 2 * D[5][7] + 4 * D[6][2] +
596  4 * D[6][4] - 2 * D[7][1] + 2 * D[7][5] - 2 * D[7][9] +
597  4 * D[8][2] + 4 * D[8][4] - 2 * D[9][3] -
598  2 * D[9][7]; // s1 * s2^2
599  f3coeff[19] = 4 * D[1][9] - 4 * D[1][1] + 8 * D[3][3] + 8 * D[3][7] +
600  4 * D[5][5] + 8 * D[7][3] + 8 * D[7][7] + 4 * D[9][1] -
601  4 * D[9][9]; // s1^2 * s3
602  f3coeff[20] = 2 * D[1][3] + 2 * D[1][7] + 2 * D[3][1] - 2 * D[3][5] -
603  2 * D[3][9] - 2 * D[5][3] - 2 * D[5][7] + 2 * D[7][1] -
604  2 * D[7][5] - 2 * D[7][9] - 2 * D[9][3] -
605  2 * D[9][7]; // s1^3
606 }
607 
608 cv::Mat mrpt::vision::pnp::dls::LeftMultVec(const cv::Mat& v)
609 {
610  cv::Mat mat_ = cv::Mat::zeros(3, 9, CV_64F);
611 
612  for (int i = 0; i < 3; ++i)
613  {
614  mat_.at<double>(i, 3 * i + 0) = v.at<double>(0);
615  mat_.at<double>(i, 3 * i + 1) = v.at<double>(1);
616  mat_.at<double>(i, 3 * i + 2) = v.at<double>(2);
617  }
618  return mat_;
619 }
620 
621 cv::Mat mrpt::vision::pnp::dls::cayley_LS_M(
622  const std::vector<double>& a, const std::vector<double>& b,
623  const std::vector<double>& c, const std::vector<double>& u)
624 {
625  // TODO: input matrix pointer
626  // TODO: shift coefficients one position to left
627 
628  cv::Mat M = cv::Mat::zeros(120, 120, CV_64F);
629 
630  M.at<double>(0, 0) = u[1];
631  M.at<double>(0, 35) = a[1];
632  M.at<double>(0, 83) = b[1];
633  M.at<double>(0, 118) = c[1];
634  M.at<double>(1, 0) = u[4];
635  M.at<double>(1, 1) = u[1];
636  M.at<double>(1, 34) = a[1];
637  M.at<double>(1, 35) = a[10];
638  M.at<double>(1, 54) = b[1];
639  M.at<double>(1, 83) = b[10];
640  M.at<double>(1, 99) = c[1];
641  M.at<double>(1, 118) = c[10];
642  M.at<double>(2, 1) = u[4];
643  M.at<double>(2, 2) = u[1];
644  M.at<double>(2, 34) = a[10];
645  M.at<double>(2, 35) = a[14];
646  M.at<double>(2, 51) = a[1];
647  M.at<double>(2, 54) = b[10];
648  M.at<double>(2, 65) = b[1];
649  M.at<double>(2, 83) = b[14];
650  M.at<double>(2, 89) = c[1];
651  M.at<double>(2, 99) = c[10];
652  M.at<double>(2, 118) = c[14];
653  M.at<double>(3, 0) = u[3];
654  M.at<double>(3, 3) = u[1];
655  M.at<double>(3, 35) = a[11];
656  M.at<double>(3, 49) = a[1];
657  M.at<double>(3, 76) = b[1];
658  M.at<double>(3, 83) = b[11];
659  M.at<double>(3, 118) = c[11];
660  M.at<double>(3, 119) = c[1];
661  M.at<double>(4, 1) = u[3];
662  M.at<double>(4, 3) = u[4];
663  M.at<double>(4, 4) = u[1];
664  M.at<double>(4, 34) = a[11];
665  M.at<double>(4, 35) = a[5];
666  M.at<double>(4, 43) = a[1];
667  M.at<double>(4, 49) = a[10];
668  M.at<double>(4, 54) = b[11];
669  M.at<double>(4, 71) = b[1];
670  M.at<double>(4, 76) = b[10];
671  M.at<double>(4, 83) = b[5];
672  M.at<double>(4, 99) = c[11];
673  M.at<double>(4, 100) = c[1];
674  M.at<double>(4, 118) = c[5];
675  M.at<double>(4, 119) = c[10];
676  M.at<double>(5, 2) = u[3];
677  M.at<double>(5, 4) = u[4];
678  M.at<double>(5, 5) = u[1];
679  M.at<double>(5, 34) = a[5];
680  M.at<double>(5, 35) = a[12];
681  M.at<double>(5, 41) = a[1];
682  M.at<double>(5, 43) = a[10];
683  M.at<double>(5, 49) = a[14];
684  M.at<double>(5, 51) = a[11];
685  M.at<double>(5, 54) = b[5];
686  M.at<double>(5, 62) = b[1];
687  M.at<double>(5, 65) = b[11];
688  M.at<double>(5, 71) = b[10];
689  M.at<double>(5, 76) = b[14];
690  M.at<double>(5, 83) = b[12];
691  M.at<double>(5, 89) = c[11];
692  M.at<double>(5, 99) = c[5];
693  M.at<double>(5, 100) = c[10];
694  M.at<double>(5, 111) = c[1];
695  M.at<double>(5, 118) = c[12];
696  M.at<double>(5, 119) = c[14];
697  M.at<double>(6, 3) = u[3];
698  M.at<double>(6, 6) = u[1];
699  M.at<double>(6, 30) = a[1];
700  M.at<double>(6, 35) = a[15];
701  M.at<double>(6, 49) = a[11];
702  M.at<double>(6, 75) = b[1];
703  M.at<double>(6, 76) = b[11];
704  M.at<double>(6, 83) = b[15];
705  M.at<double>(6, 107) = c[1];
706  M.at<double>(6, 118) = c[15];
707  M.at<double>(6, 119) = c[11];
708  M.at<double>(7, 4) = u[3];
709  M.at<double>(7, 6) = u[4];
710  M.at<double>(7, 7) = u[1];
711  M.at<double>(7, 30) = a[10];
712  M.at<double>(7, 34) = a[15];
713  M.at<double>(7, 35) = a[6];
714  M.at<double>(7, 43) = a[11];
715  M.at<double>(7, 45) = a[1];
716  M.at<double>(7, 49) = a[5];
717  M.at<double>(7, 54) = b[15];
718  M.at<double>(7, 63) = b[1];
719  M.at<double>(7, 71) = b[11];
720  M.at<double>(7, 75) = b[10];
721  M.at<double>(7, 76) = b[5];
722  M.at<double>(7, 83) = b[6];
723  M.at<double>(7, 99) = c[15];
724  M.at<double>(7, 100) = c[11];
725  M.at<double>(7, 107) = c[10];
726  M.at<double>(7, 112) = c[1];
727  M.at<double>(7, 118) = c[6];
728  M.at<double>(7, 119) = c[5];
729  M.at<double>(8, 5) = u[3];
730  M.at<double>(8, 7) = u[4];
731  M.at<double>(8, 8) = u[1];
732  M.at<double>(8, 30) = a[14];
733  M.at<double>(8, 34) = a[6];
734  M.at<double>(8, 41) = a[11];
735  M.at<double>(8, 43) = a[5];
736  M.at<double>(8, 45) = a[10];
737  M.at<double>(8, 46) = a[1];
738  M.at<double>(8, 49) = a[12];
739  M.at<double>(8, 51) = a[15];
740  M.at<double>(8, 54) = b[6];
741  M.at<double>(8, 62) = b[11];
742  M.at<double>(8, 63) = b[10];
743  M.at<double>(8, 65) = b[15];
744  M.at<double>(8, 66) = b[1];
745  M.at<double>(8, 71) = b[5];
746  M.at<double>(8, 75) = b[14];
747  M.at<double>(8, 76) = b[12];
748  M.at<double>(8, 89) = c[15];
749  M.at<double>(8, 99) = c[6];
750  M.at<double>(8, 100) = c[5];
751  M.at<double>(8, 102) = c[1];
752  M.at<double>(8, 107) = c[14];
753  M.at<double>(8, 111) = c[11];
754  M.at<double>(8, 112) = c[10];
755  M.at<double>(8, 119) = c[12];
756  M.at<double>(9, 0) = u[2];
757  M.at<double>(9, 9) = u[1];
758  M.at<double>(9, 35) = a[9];
759  M.at<double>(9, 36) = a[1];
760  M.at<double>(9, 83) = b[9];
761  M.at<double>(9, 84) = b[1];
762  M.at<double>(9, 88) = c[1];
763  M.at<double>(9, 118) = c[9];
764  M.at<double>(10, 1) = u[2];
765  M.at<double>(10, 9) = u[4];
766  M.at<double>(10, 10) = u[1];
767  M.at<double>(10, 33) = a[1];
768  M.at<double>(10, 34) = a[9];
769  M.at<double>(10, 35) = a[4];
770  M.at<double>(10, 36) = a[10];
771  M.at<double>(10, 54) = b[9];
772  M.at<double>(10, 59) = b[1];
773  M.at<double>(10, 83) = b[4];
774  M.at<double>(10, 84) = b[10];
775  M.at<double>(10, 88) = c[10];
776  M.at<double>(10, 99) = c[9];
777  M.at<double>(10, 117) = c[1];
778  M.at<double>(10, 118) = c[4];
779  M.at<double>(11, 2) = u[2];
780  M.at<double>(11, 10) = u[4];
781  M.at<double>(11, 11) = u[1];
782  M.at<double>(11, 28) = a[1];
783  M.at<double>(11, 33) = a[10];
784  M.at<double>(11, 34) = a[4];
785  M.at<double>(11, 35) = a[8];
786  M.at<double>(11, 36) = a[14];
787  M.at<double>(11, 51) = a[9];
788  M.at<double>(11, 54) = b[4];
789  M.at<double>(11, 57) = b[1];
790  M.at<double>(11, 59) = b[10];
791  M.at<double>(11, 65) = b[9];
792  M.at<double>(11, 83) = b[8];
793  M.at<double>(11, 84) = b[14];
794  M.at<double>(11, 88) = c[14];
795  M.at<double>(11, 89) = c[9];
796  M.at<double>(11, 99) = c[4];
797  M.at<double>(11, 114) = c[1];
798  M.at<double>(11, 117) = c[10];
799  M.at<double>(11, 118) = c[8];
800  M.at<double>(12, 3) = u[2];
801  M.at<double>(12, 9) = u[3];
802  M.at<double>(12, 12) = u[1];
803  M.at<double>(12, 35) = a[3];
804  M.at<double>(12, 36) = a[11];
805  M.at<double>(12, 39) = a[1];
806  M.at<double>(12, 49) = a[9];
807  M.at<double>(12, 76) = b[9];
808  M.at<double>(12, 79) = b[1];
809  M.at<double>(12, 83) = b[3];
810  M.at<double>(12, 84) = b[11];
811  M.at<double>(12, 88) = c[11];
812  M.at<double>(12, 96) = c[1];
813  M.at<double>(12, 118) = c[3];
814  M.at<double>(12, 119) = c[9];
815  M.at<double>(13, 4) = u[2];
816  M.at<double>(13, 10) = u[3];
817  M.at<double>(13, 12) = u[4];
818  M.at<double>(13, 13) = u[1];
819  M.at<double>(13, 33) = a[11];
820  M.at<double>(13, 34) = a[3];
821  M.at<double>(13, 35) = a[17];
822  M.at<double>(13, 36) = a[5];
823  M.at<double>(13, 39) = a[10];
824  M.at<double>(13, 43) = a[9];
825  M.at<double>(13, 47) = a[1];
826  M.at<double>(13, 49) = a[4];
827  M.at<double>(13, 54) = b[3];
828  M.at<double>(13, 59) = b[11];
829  M.at<double>(13, 60) = b[1];
830  M.at<double>(13, 71) = b[9];
831  M.at<double>(13, 76) = b[4];
832  M.at<double>(13, 79) = b[10];
833  M.at<double>(13, 83) = b[17];
834  M.at<double>(13, 84) = b[5];
835  M.at<double>(13, 88) = c[5];
836  M.at<double>(13, 90) = c[1];
837  M.at<double>(13, 96) = c[10];
838  M.at<double>(13, 99) = c[3];
839  M.at<double>(13, 100) = c[9];
840  M.at<double>(13, 117) = c[11];
841  M.at<double>(13, 118) = c[17];
842  M.at<double>(13, 119) = c[4];
843  M.at<double>(14, 5) = u[2];
844  M.at<double>(14, 11) = u[3];
845  M.at<double>(14, 13) = u[4];
846  M.at<double>(14, 14) = u[1];
847  M.at<double>(14, 28) = a[11];
848  M.at<double>(14, 33) = a[5];
849  M.at<double>(14, 34) = a[17];
850  M.at<double>(14, 36) = a[12];
851  M.at<double>(14, 39) = a[14];
852  M.at<double>(14, 41) = a[9];
853  M.at<double>(14, 42) = a[1];
854  M.at<double>(14, 43) = a[4];
855  M.at<double>(14, 47) = a[10];
856  M.at<double>(14, 49) = a[8];
857  M.at<double>(14, 51) = a[3];
858  M.at<double>(14, 54) = b[17];
859  M.at<double>(14, 56) = b[1];
860  M.at<double>(14, 57) = b[11];
861  M.at<double>(14, 59) = b[5];
862  M.at<double>(14, 60) = b[10];
863  M.at<double>(14, 62) = b[9];
864  M.at<double>(14, 65) = b[3];
865  M.at<double>(14, 71) = b[4];
866  M.at<double>(14, 76) = b[8];
867  M.at<double>(14, 79) = b[14];
868  M.at<double>(14, 84) = b[12];
869  M.at<double>(14, 88) = c[12];
870  M.at<double>(14, 89) = c[3];
871  M.at<double>(14, 90) = c[10];
872  M.at<double>(14, 96) = c[14];
873  M.at<double>(14, 99) = c[17];
874  M.at<double>(14, 100) = c[4];
875  M.at<double>(14, 106) = c[1];
876  M.at<double>(14, 111) = c[9];
877  M.at<double>(14, 114) = c[11];
878  M.at<double>(14, 117) = c[5];
879  M.at<double>(14, 119) = c[8];
880  M.at<double>(15, 6) = u[2];
881  M.at<double>(15, 12) = u[3];
882  M.at<double>(15, 15) = u[1];
883  M.at<double>(15, 29) = a[1];
884  M.at<double>(15, 30) = a[9];
885  M.at<double>(15, 35) = a[18];
886  M.at<double>(15, 36) = a[15];
887  M.at<double>(15, 39) = a[11];
888  M.at<double>(15, 49) = a[3];
889  M.at<double>(15, 74) = b[1];
890  M.at<double>(15, 75) = b[9];
891  M.at<double>(15, 76) = b[3];
892  M.at<double>(15, 79) = b[11];
893  M.at<double>(15, 83) = b[18];
894  M.at<double>(15, 84) = b[15];
895  M.at<double>(15, 88) = c[15];
896  M.at<double>(15, 94) = c[1];
897  M.at<double>(15, 96) = c[11];
898  M.at<double>(15, 107) = c[9];
899  M.at<double>(15, 118) = c[18];
900  M.at<double>(15, 119) = c[3];
901  M.at<double>(16, 7) = u[2];
902  M.at<double>(16, 13) = u[3];
903  M.at<double>(16, 15) = u[4];
904  M.at<double>(16, 16) = u[1];
905  M.at<double>(16, 29) = a[10];
906  M.at<double>(16, 30) = a[4];
907  M.at<double>(16, 33) = a[15];
908  M.at<double>(16, 34) = a[18];
909  M.at<double>(16, 36) = a[6];
910  M.at<double>(16, 39) = a[5];
911  M.at<double>(16, 43) = a[3];
912  M.at<double>(16, 44) = a[1];
913  M.at<double>(16, 45) = a[9];
914  M.at<double>(16, 47) = a[11];
915  M.at<double>(16, 49) = a[17];
916  M.at<double>(16, 54) = b[18];
917  M.at<double>(16, 59) = b[15];
918  M.at<double>(16, 60) = b[11];
919  M.at<double>(16, 63) = b[9];
920  M.at<double>(16, 68) = b[1];
921  M.at<double>(16, 71) = b[3];
922  M.at<double>(16, 74) = b[10];
923  M.at<double>(16, 75) = b[4];
924  M.at<double>(16, 76) = b[17];
925  M.at<double>(16, 79) = b[5];
926  M.at<double>(16, 84) = b[6];
927  M.at<double>(16, 88) = c[6];
928  M.at<double>(16, 90) = c[11];
929  M.at<double>(16, 94) = c[10];
930  M.at<double>(16, 96) = c[5];
931  M.at<double>(16, 97) = c[1];
932  M.at<double>(16, 99) = c[18];
933  M.at<double>(16, 100) = c[3];
934  M.at<double>(16, 107) = c[4];
935  M.at<double>(16, 112) = c[9];
936  M.at<double>(16, 117) = c[15];
937  M.at<double>(16, 119) = c[17];
938  M.at<double>(17, 8) = u[2];
939  M.at<double>(17, 14) = u[3];
940  M.at<double>(17, 16) = u[4];
941  M.at<double>(17, 17) = u[1];
942  M.at<double>(17, 28) = a[15];
943  M.at<double>(17, 29) = a[14];
944  M.at<double>(17, 30) = a[8];
945  M.at<double>(17, 33) = a[6];
946  M.at<double>(17, 39) = a[12];
947  M.at<double>(17, 41) = a[3];
948  M.at<double>(17, 42) = a[11];
949  M.at<double>(17, 43) = a[17];
950  M.at<double>(17, 44) = a[10];
951  M.at<double>(17, 45) = a[4];
952  M.at<double>(17, 46) = a[9];
953  M.at<double>(17, 47) = a[5];
954  M.at<double>(17, 51) = a[18];
955  M.at<double>(17, 56) = b[11];
956  M.at<double>(17, 57) = b[15];
957  M.at<double>(17, 59) = b[6];
958  M.at<double>(17, 60) = b[5];
959  M.at<double>(17, 62) = b[3];
960  M.at<double>(17, 63) = b[4];
961  M.at<double>(17, 65) = b[18];
962  M.at<double>(17, 66) = b[9];
963  M.at<double>(17, 68) = b[10];
964  M.at<double>(17, 71) = b[17];
965  M.at<double>(17, 74) = b[14];
966  M.at<double>(17, 75) = b[8];
967  M.at<double>(17, 79) = b[12];
968  M.at<double>(17, 89) = c[18];
969  M.at<double>(17, 90) = c[5];
970  M.at<double>(17, 94) = c[14];
971  M.at<double>(17, 96) = c[12];
972  M.at<double>(17, 97) = c[10];
973  M.at<double>(17, 100) = c[17];
974  M.at<double>(17, 102) = c[9];
975  M.at<double>(17, 106) = c[11];
976  M.at<double>(17, 107) = c[8];
977  M.at<double>(17, 111) = c[3];
978  M.at<double>(17, 112) = c[4];
979  M.at<double>(17, 114) = c[15];
980  M.at<double>(17, 117) = c[6];
981  M.at<double>(18, 9) = u[2];
982  M.at<double>(18, 18) = u[1];
983  M.at<double>(18, 35) = a[13];
984  M.at<double>(18, 36) = a[9];
985  M.at<double>(18, 53) = a[1];
986  M.at<double>(18, 82) = b[1];
987  M.at<double>(18, 83) = b[13];
988  M.at<double>(18, 84) = b[9];
989  M.at<double>(18, 87) = c[1];
990  M.at<double>(18, 88) = c[9];
991  M.at<double>(18, 118) = c[13];
992  M.at<double>(19, 10) = u[2];
993  M.at<double>(19, 18) = u[4];
994  M.at<double>(19, 19) = u[1];
995  M.at<double>(19, 32) = a[1];
996  M.at<double>(19, 33) = a[9];
997  M.at<double>(19, 34) = a[13];
998  M.at<double>(19, 35) = a[19];
999  M.at<double>(19, 36) = a[4];
1000  M.at<double>(19, 53) = a[10];
1001  M.at<double>(19, 54) = b[13];
1002  M.at<double>(19, 59) = b[9];
1003  M.at<double>(19, 61) = b[1];
1004  M.at<double>(19, 82) = b[10];
1005  M.at<double>(19, 83) = b[19];
1006  M.at<double>(19, 84) = b[4];
1007  M.at<double>(19, 87) = c[10];
1008  M.at<double>(19, 88) = c[4];
1009  M.at<double>(19, 99) = c[13];
1010  M.at<double>(19, 116) = c[1];
1011  M.at<double>(19, 117) = c[9];
1012  M.at<double>(19, 118) = c[19];
1013  M.at<double>(20, 11) = u[2];
1014  M.at<double>(20, 19) = u[4];
1015  M.at<double>(20, 20) = u[1];
1016  M.at<double>(20, 27) = a[1];
1017  M.at<double>(20, 28) = a[9];
1018  M.at<double>(20, 32) = a[10];
1019  M.at<double>(20, 33) = a[4];
1020  M.at<double>(20, 34) = a[19];
1021  M.at<double>(20, 36) = a[8];
1022  M.at<double>(20, 51) = a[13];
1023  M.at<double>(20, 53) = a[14];
1024  M.at<double>(20, 54) = b[19];
1025  M.at<double>(20, 55) = b[1];
1026  M.at<double>(20, 57) = b[9];
1027  M.at<double>(20, 59) = b[4];
1028  M.at<double>(20, 61) = b[10];
1029  M.at<double>(20, 65) = b[13];
1030  M.at<double>(20, 82) = b[14];
1031  M.at<double>(20, 84) = b[8];
1032  M.at<double>(20, 87) = c[14];
1033  M.at<double>(20, 88) = c[8];
1034  M.at<double>(20, 89) = c[13];
1035  M.at<double>(20, 99) = c[19];
1036  M.at<double>(20, 113) = c[1];
1037  M.at<double>(20, 114) = c[9];
1038  M.at<double>(20, 116) = c[10];
1039  M.at<double>(20, 117) = c[4];
1040  M.at<double>(21, 12) = u[2];
1041  M.at<double>(21, 18) = u[3];
1042  M.at<double>(21, 21) = u[1];
1043  M.at<double>(21, 35) = a[2];
1044  M.at<double>(21, 36) = a[3];
1045  M.at<double>(21, 38) = a[1];
1046  M.at<double>(21, 39) = a[9];
1047  M.at<double>(21, 49) = a[13];
1048  M.at<double>(21, 53) = a[11];
1049  M.at<double>(21, 76) = b[13];
1050  M.at<double>(21, 78) = b[1];
1051  M.at<double>(21, 79) = b[9];
1052  M.at<double>(21, 82) = b[11];
1053  M.at<double>(21, 83) = b[2];
1054  M.at<double>(21, 84) = b[3];
1055  M.at<double>(21, 87) = c[11];
1056  M.at<double>(21, 88) = c[3];
1057  M.at<double>(21, 92) = c[1];
1058  M.at<double>(21, 96) = c[9];
1059  M.at<double>(21, 118) = c[2];
1060  M.at<double>(21, 119) = c[13];
1061  M.at<double>(22, 13) = u[2];
1062  M.at<double>(22, 19) = u[3];
1063  M.at<double>(22, 21) = u[4];
1064  M.at<double>(22, 22) = u[1];
1065  M.at<double>(22, 32) = a[11];
1066  M.at<double>(22, 33) = a[3];
1067  M.at<double>(22, 34) = a[2];
1068  M.at<double>(22, 36) = a[17];
1069  M.at<double>(22, 38) = a[10];
1070  M.at<double>(22, 39) = a[4];
1071  M.at<double>(22, 40) = a[1];
1072  M.at<double>(22, 43) = a[13];
1073  M.at<double>(22, 47) = a[9];
1074  M.at<double>(22, 49) = a[19];
1075  M.at<double>(22, 53) = a[5];
1076  M.at<double>(22, 54) = b[2];
1077  M.at<double>(22, 59) = b[3];
1078  M.at<double>(22, 60) = b[9];
1079  M.at<double>(22, 61) = b[11];
1080  M.at<double>(22, 71) = b[13];
1081  M.at<double>(22, 72) = b[1];
1082  M.at<double>(22, 76) = b[19];
1083  M.at<double>(22, 78) = b[10];
1084  M.at<double>(22, 79) = b[4];
1085  M.at<double>(22, 82) = b[5];
1086  M.at<double>(22, 84) = b[17];
1087  M.at<double>(22, 87) = c[5];
1088  M.at<double>(22, 88) = c[17];
1089  M.at<double>(22, 90) = c[9];
1090  M.at<double>(22, 92) = c[10];
1091  M.at<double>(22, 95) = c[1];
1092  M.at<double>(22, 96) = c[4];
1093  M.at<double>(22, 99) = c[2];
1094  M.at<double>(22, 100) = c[13];
1095  M.at<double>(22, 116) = c[11];
1096  M.at<double>(22, 117) = c[3];
1097  M.at<double>(22, 119) = c[19];
1098  M.at<double>(23, 14) = u[2];
1099  M.at<double>(23, 20) = u[3];
1100  M.at<double>(23, 22) = u[4];
1101  M.at<double>(23, 23) = u[1];
1102  M.at<double>(23, 27) = a[11];
1103  M.at<double>(23, 28) = a[3];
1104  M.at<double>(23, 32) = a[5];
1105  M.at<double>(23, 33) = a[17];
1106  M.at<double>(23, 38) = a[14];
1107  M.at<double>(23, 39) = a[8];
1108  M.at<double>(23, 40) = a[10];
1109  M.at<double>(23, 41) = a[13];
1110  M.at<double>(23, 42) = a[9];
1111  M.at<double>(23, 43) = a[19];
1112  M.at<double>(23, 47) = a[4];
1113  M.at<double>(23, 51) = a[2];
1114  M.at<double>(23, 53) = a[12];
1115  M.at<double>(23, 55) = b[11];
1116  M.at<double>(23, 56) = b[9];
1117  M.at<double>(23, 57) = b[3];
1118  M.at<double>(23, 59) = b[17];
1119  M.at<double>(23, 60) = b[4];
1120  M.at<double>(23, 61) = b[5];
1121  M.at<double>(23, 62) = b[13];
1122  M.at<double>(23, 65) = b[2];
1123  M.at<double>(23, 71) = b[19];
1124  M.at<double>(23, 72) = b[10];
1125  M.at<double>(23, 78) = b[14];
1126  M.at<double>(23, 79) = b[8];
1127  M.at<double>(23, 82) = b[12];
1128  M.at<double>(23, 87) = c[12];
1129  M.at<double>(23, 89) = c[2];
1130  M.at<double>(23, 90) = c[4];
1131  M.at<double>(23, 92) = c[14];
1132  M.at<double>(23, 95) = c[10];
1133  M.at<double>(23, 96) = c[8];
1134  M.at<double>(23, 100) = c[19];
1135  M.at<double>(23, 106) = c[9];
1136  M.at<double>(23, 111) = c[13];
1137  M.at<double>(23, 113) = c[11];
1138  M.at<double>(23, 114) = c[3];
1139  M.at<double>(23, 116) = c[5];
1140  M.at<double>(23, 117) = c[17];
1141  M.at<double>(24, 15) = u[2];
1142  M.at<double>(24, 21) = u[3];
1143  M.at<double>(24, 24) = u[1];
1144  M.at<double>(24, 29) = a[9];
1145  M.at<double>(24, 30) = a[13];
1146  M.at<double>(24, 36) = a[18];
1147  M.at<double>(24, 38) = a[11];
1148  M.at<double>(24, 39) = a[3];
1149  M.at<double>(24, 49) = a[2];
1150  M.at<double>(24, 52) = a[1];
1151  M.at<double>(24, 53) = a[15];
1152  M.at<double>(24, 73) = b[1];
1153  M.at<double>(24, 74) = b[9];
1154  M.at<double>(24, 75) = b[13];
1155  M.at<double>(24, 76) = b[2];
1156  M.at<double>(24, 78) = b[11];
1157  M.at<double>(24, 79) = b[3];
1158  M.at<double>(24, 82) = b[15];
1159  M.at<double>(24, 84) = b[18];
1160  M.at<double>(24, 87) = c[15];
1161  M.at<double>(24, 88) = c[18];
1162  M.at<double>(24, 92) = c[11];
1163  M.at<double>(24, 93) = c[1];
1164  M.at<double>(24, 94) = c[9];
1165  M.at<double>(24, 96) = c[3];
1166  M.at<double>(24, 107) = c[13];
1167  M.at<double>(24, 119) = c[2];
1168  M.at<double>(25, 16) = u[2];
1169  M.at<double>(25, 22) = u[3];
1170  M.at<double>(25, 24) = u[4];
1171  M.at<double>(25, 25) = u[1];
1172  M.at<double>(25, 29) = a[4];
1173  M.at<double>(25, 30) = a[19];
1174  M.at<double>(25, 32) = a[15];
1175  M.at<double>(25, 33) = a[18];
1176  M.at<double>(25, 38) = a[5];
1177  M.at<double>(25, 39) = a[17];
1178  M.at<double>(25, 40) = a[11];
1179  M.at<double>(25, 43) = a[2];
1180  M.at<double>(25, 44) = a[9];
1181  M.at<double>(25, 45) = a[13];
1182  M.at<double>(25, 47) = a[3];
1183  M.at<double>(25, 52) = a[10];
1184  M.at<double>(25, 53) = a[6];
1185  M.at<double>(25, 59) = b[18];
1186  M.at<double>(25, 60) = b[3];
1187  M.at<double>(25, 61) = b[15];
1188  M.at<double>(25, 63) = b[13];
1189  M.at<double>(25, 68) = b[9];
1190  M.at<double>(25, 71) = b[2];
1191  M.at<double>(25, 72) = b[11];
1192  M.at<double>(25, 73) = b[10];
1193  M.at<double>(25, 74) = b[4];
1194  M.at<double>(25, 75) = b[19];
1195  M.at<double>(25, 78) = b[5];
1196  M.at<double>(25, 79) = b[17];
1197  M.at<double>(25, 82) = b[6];
1198  M.at<double>(25, 87) = c[6];
1199  M.at<double>(25, 90) = c[3];
1200  M.at<double>(25, 92) = c[5];
1201  M.at<double>(25, 93) = c[10];
1202  M.at<double>(25, 94) = c[4];
1203  M.at<double>(25, 95) = c[11];
1204  M.at<double>(25, 96) = c[17];
1205  M.at<double>(25, 97) = c[9];
1206  M.at<double>(25, 100) = c[2];
1207  M.at<double>(25, 107) = c[19];
1208  M.at<double>(25, 112) = c[13];
1209  M.at<double>(25, 116) = c[15];
1210  M.at<double>(25, 117) = c[18];
1211  M.at<double>(26, 17) = u[2];
1212  M.at<double>(26, 23) = u[3];
1213  M.at<double>(26, 25) = u[4];
1214  M.at<double>(26, 26) = u[1];
1215  M.at<double>(26, 27) = a[15];
1216  M.at<double>(26, 28) = a[18];
1217  M.at<double>(26, 29) = a[8];
1218  M.at<double>(26, 32) = a[6];
1219  M.at<double>(26, 38) = a[12];
1220  M.at<double>(26, 40) = a[5];
1221  M.at<double>(26, 41) = a[2];
1222  M.at<double>(26, 42) = a[3];
1223  M.at<double>(26, 44) = a[4];
1224  M.at<double>(26, 45) = a[19];
1225  M.at<double>(26, 46) = a[13];
1226  M.at<double>(26, 47) = a[17];
1227  M.at<double>(26, 52) = a[14];
1228  M.at<double>(26, 55) = b[15];
1229  M.at<double>(26, 56) = b[3];
1230  M.at<double>(26, 57) = b[18];
1231  M.at<double>(26, 60) = b[17];
1232  M.at<double>(26, 61) = b[6];
1233  M.at<double>(26, 62) = b[2];
1234  M.at<double>(26, 63) = b[19];
1235  M.at<double>(26, 66) = b[13];
1236  M.at<double>(26, 68) = b[4];
1237  M.at<double>(26, 72) = b[5];
1238  M.at<double>(26, 73) = b[14];
1239  M.at<double>(26, 74) = b[8];
1240  M.at<double>(26, 78) = b[12];
1241  M.at<double>(26, 90) = c[17];
1242  M.at<double>(26, 92) = c[12];
1243  M.at<double>(26, 93) = c[14];
1244  M.at<double>(26, 94) = c[8];
1245  M.at<double>(26, 95) = c[5];
1246  M.at<double>(26, 97) = c[4];
1247  M.at<double>(26, 102) = c[13];
1248  M.at<double>(26, 106) = c[3];
1249  M.at<double>(26, 111) = c[2];
1250  M.at<double>(26, 112) = c[19];
1251  M.at<double>(26, 113) = c[15];
1252  M.at<double>(26, 114) = c[18];
1253  M.at<double>(26, 116) = c[6];
1254  M.at<double>(27, 15) = u[3];
1255  M.at<double>(27, 29) = a[11];
1256  M.at<double>(27, 30) = a[3];
1257  M.at<double>(27, 36) = a[7];
1258  M.at<double>(27, 39) = a[15];
1259  M.at<double>(27, 49) = a[18];
1260  M.at<double>(27, 69) = b[9];
1261  M.at<double>(27, 70) = b[1];
1262  M.at<double>(27, 74) = b[11];
1263  M.at<double>(27, 75) = b[3];
1264  M.at<double>(27, 76) = b[18];
1265  M.at<double>(27, 79) = b[15];
1266  M.at<double>(27, 84) = b[7];
1267  M.at<double>(27, 88) = c[7];
1268  M.at<double>(27, 91) = c[1];
1269  M.at<double>(27, 94) = c[11];
1270  M.at<double>(27, 96) = c[15];
1271  M.at<double>(27, 107) = c[3];
1272  M.at<double>(27, 110) = c[9];
1273  M.at<double>(27, 119) = c[18];
1274  M.at<double>(28, 6) = u[3];
1275  M.at<double>(28, 30) = a[11];
1276  M.at<double>(28, 35) = a[7];
1277  M.at<double>(28, 49) = a[15];
1278  M.at<double>(28, 69) = b[1];
1279  M.at<double>(28, 75) = b[11];
1280  M.at<double>(28, 76) = b[15];
1281  M.at<double>(28, 83) = b[7];
1282  M.at<double>(28, 107) = c[11];
1283  M.at<double>(28, 110) = c[1];
1284  M.at<double>(28, 118) = c[7];
1285  M.at<double>(28, 119) = c[15];
1286  M.at<double>(29, 24) = u[3];
1287  M.at<double>(29, 29) = a[3];
1288  M.at<double>(29, 30) = a[2];
1289  M.at<double>(29, 38) = a[15];
1290  M.at<double>(29, 39) = a[18];
1291  M.at<double>(29, 52) = a[11];
1292  M.at<double>(29, 53) = a[7];
1293  M.at<double>(29, 69) = b[13];
1294  M.at<double>(29, 70) = b[9];
1295  M.at<double>(29, 73) = b[11];
1296  M.at<double>(29, 74) = b[3];
1297  M.at<double>(29, 75) = b[2];
1298  M.at<double>(29, 78) = b[15];
1299  M.at<double>(29, 79) = b[18];
1300  M.at<double>(29, 82) = b[7];
1301  M.at<double>(29, 87) = c[7];
1302  M.at<double>(29, 91) = c[9];
1303  M.at<double>(29, 92) = c[15];
1304  M.at<double>(29, 93) = c[11];
1305  M.at<double>(29, 94) = c[3];
1306  M.at<double>(29, 96) = c[18];
1307  M.at<double>(29, 107) = c[2];
1308  M.at<double>(29, 110) = c[13];
1309  M.at<double>(30, 37) = a[18];
1310  M.at<double>(30, 48) = a[7];
1311  M.at<double>(30, 52) = a[2];
1312  M.at<double>(30, 70) = b[20];
1313  M.at<double>(30, 73) = b[2];
1314  M.at<double>(30, 77) = b[18];
1315  M.at<double>(30, 81) = b[7];
1316  M.at<double>(30, 85) = c[7];
1317  M.at<double>(30, 91) = c[20];
1318  M.at<double>(30, 93) = c[2];
1319  M.at<double>(30, 98) = c[18];
1320  M.at<double>(31, 29) = a[2];
1321  M.at<double>(31, 37) = a[15];
1322  M.at<double>(31, 38) = a[18];
1323  M.at<double>(31, 50) = a[7];
1324  M.at<double>(31, 52) = a[3];
1325  M.at<double>(31, 69) = b[20];
1326  M.at<double>(31, 70) = b[13];
1327  M.at<double>(31, 73) = b[3];
1328  M.at<double>(31, 74) = b[2];
1329  M.at<double>(31, 77) = b[15];
1330  M.at<double>(31, 78) = b[18];
1331  M.at<double>(31, 80) = b[7];
1332  M.at<double>(31, 86) = c[7];
1333  M.at<double>(31, 91) = c[13];
1334  M.at<double>(31, 92) = c[18];
1335  M.at<double>(31, 93) = c[3];
1336  M.at<double>(31, 94) = c[2];
1337  M.at<double>(31, 98) = c[15];
1338  M.at<double>(31, 110) = c[20];
1339  M.at<double>(32, 48) = a[9];
1340  M.at<double>(32, 50) = a[13];
1341  M.at<double>(32, 53) = a[20];
1342  M.at<double>(32, 80) = b[13];
1343  M.at<double>(32, 81) = b[9];
1344  M.at<double>(32, 82) = b[20];
1345  M.at<double>(32, 85) = c[9];
1346  M.at<double>(32, 86) = c[13];
1347  M.at<double>(32, 87) = c[20];
1348  M.at<double>(33, 29) = a[15];
1349  M.at<double>(33, 30) = a[18];
1350  M.at<double>(33, 39) = a[7];
1351  M.at<double>(33, 64) = b[9];
1352  M.at<double>(33, 69) = b[3];
1353  M.at<double>(33, 70) = b[11];
1354  M.at<double>(33, 74) = b[15];
1355  M.at<double>(33, 75) = b[18];
1356  M.at<double>(33, 79) = b[7];
1357  M.at<double>(33, 91) = c[11];
1358  M.at<double>(33, 94) = c[15];
1359  M.at<double>(33, 96) = c[7];
1360  M.at<double>(33, 103) = c[9];
1361  M.at<double>(33, 107) = c[18];
1362  M.at<double>(33, 110) = c[3];
1363  M.at<double>(34, 29) = a[18];
1364  M.at<double>(34, 38) = a[7];
1365  M.at<double>(34, 52) = a[15];
1366  M.at<double>(34, 64) = b[13];
1367  M.at<double>(34, 69) = b[2];
1368  M.at<double>(34, 70) = b[3];
1369  M.at<double>(34, 73) = b[15];
1370  M.at<double>(34, 74) = b[18];
1371  M.at<double>(34, 78) = b[7];
1372  M.at<double>(34, 91) = c[3];
1373  M.at<double>(34, 92) = c[7];
1374  M.at<double>(34, 93) = c[15];
1375  M.at<double>(34, 94) = c[18];
1376  M.at<double>(34, 103) = c[13];
1377  M.at<double>(34, 110) = c[2];
1378  M.at<double>(35, 37) = a[7];
1379  M.at<double>(35, 52) = a[18];
1380  M.at<double>(35, 64) = b[20];
1381  M.at<double>(35, 70) = b[2];
1382  M.at<double>(35, 73) = b[18];
1383  M.at<double>(35, 77) = b[7];
1384  M.at<double>(35, 91) = c[2];
1385  M.at<double>(35, 93) = c[18];
1386  M.at<double>(35, 98) = c[7];
1387  M.at<double>(35, 103) = c[20];
1388  M.at<double>(36, 5) = u[4];
1389  M.at<double>(36, 34) = a[12];
1390  M.at<double>(36, 41) = a[10];
1391  M.at<double>(36, 43) = a[14];
1392  M.at<double>(36, 49) = a[16];
1393  M.at<double>(36, 51) = a[5];
1394  M.at<double>(36, 54) = b[12];
1395  M.at<double>(36, 62) = b[10];
1396  M.at<double>(36, 65) = b[5];
1397  M.at<double>(36, 71) = b[14];
1398  M.at<double>(36, 76) = b[16];
1399  M.at<double>(36, 89) = c[5];
1400  M.at<double>(36, 99) = c[12];
1401  M.at<double>(36, 100) = c[14];
1402  M.at<double>(36, 101) = c[1];
1403  M.at<double>(36, 109) = c[11];
1404  M.at<double>(36, 111) = c[10];
1405  M.at<double>(36, 119) = c[16];
1406  M.at<double>(37, 2) = u[4];
1407  M.at<double>(37, 34) = a[14];
1408  M.at<double>(37, 35) = a[16];
1409  M.at<double>(37, 51) = a[10];
1410  M.at<double>(37, 54) = b[14];
1411  M.at<double>(37, 65) = b[10];
1412  M.at<double>(37, 83) = b[16];
1413  M.at<double>(37, 89) = c[10];
1414  M.at<double>(37, 99) = c[14];
1415  M.at<double>(37, 109) = c[1];
1416  M.at<double>(37, 118) = c[16];
1417  M.at<double>(38, 30) = a[15];
1418  M.at<double>(38, 49) = a[7];
1419  M.at<double>(38, 64) = b[1];
1420  M.at<double>(38, 69) = b[11];
1421  M.at<double>(38, 75) = b[15];
1422  M.at<double>(38, 76) = b[7];
1423  M.at<double>(38, 103) = c[1];
1424  M.at<double>(38, 107) = c[15];
1425  M.at<double>(38, 110) = c[11];
1426  M.at<double>(38, 119) = c[7];
1427  M.at<double>(39, 28) = a[14];
1428  M.at<double>(39, 33) = a[16];
1429  M.at<double>(39, 51) = a[8];
1430  M.at<double>(39, 57) = b[14];
1431  M.at<double>(39, 59) = b[16];
1432  M.at<double>(39, 65) = b[8];
1433  M.at<double>(39, 89) = c[8];
1434  M.at<double>(39, 105) = c[9];
1435  M.at<double>(39, 108) = c[10];
1436  M.at<double>(39, 109) = c[4];
1437  M.at<double>(39, 114) = c[14];
1438  M.at<double>(39, 117) = c[16];
1439  M.at<double>(40, 27) = a[14];
1440  M.at<double>(40, 28) = a[8];
1441  M.at<double>(40, 32) = a[16];
1442  M.at<double>(40, 55) = b[14];
1443  M.at<double>(40, 57) = b[8];
1444  M.at<double>(40, 61) = b[16];
1445  M.at<double>(40, 105) = c[13];
1446  M.at<double>(40, 108) = c[4];
1447  M.at<double>(40, 109) = c[19];
1448  M.at<double>(40, 113) = c[14];
1449  M.at<double>(40, 114) = c[8];
1450  M.at<double>(40, 116) = c[16];
1451  M.at<double>(41, 30) = a[7];
1452  M.at<double>(41, 64) = b[11];
1453  M.at<double>(41, 69) = b[15];
1454  M.at<double>(41, 75) = b[7];
1455  M.at<double>(41, 103) = c[11];
1456  M.at<double>(41, 107) = c[7];
1457  M.at<double>(41, 110) = c[15];
1458  M.at<double>(42, 27) = a[8];
1459  M.at<double>(42, 31) = a[16];
1460  M.at<double>(42, 55) = b[8];
1461  M.at<double>(42, 58) = b[16];
1462  M.at<double>(42, 105) = c[20];
1463  M.at<double>(42, 108) = c[19];
1464  M.at<double>(42, 113) = c[8];
1465  M.at<double>(42, 115) = c[16];
1466  M.at<double>(43, 29) = a[7];
1467  M.at<double>(43, 64) = b[3];
1468  M.at<double>(43, 69) = b[18];
1469  M.at<double>(43, 70) = b[15];
1470  M.at<double>(43, 74) = b[7];
1471  M.at<double>(43, 91) = c[15];
1472  M.at<double>(43, 94) = c[7];
1473  M.at<double>(43, 103) = c[3];
1474  M.at<double>(43, 110) = c[18];
1475  M.at<double>(44, 28) = a[16];
1476  M.at<double>(44, 57) = b[16];
1477  M.at<double>(44, 105) = c[4];
1478  M.at<double>(44, 108) = c[14];
1479  M.at<double>(44, 109) = c[8];
1480  M.at<double>(44, 114) = c[16];
1481  M.at<double>(45, 27) = a[16];
1482  M.at<double>(45, 55) = b[16];
1483  M.at<double>(45, 105) = c[19];
1484  M.at<double>(45, 108) = c[8];
1485  M.at<double>(45, 113) = c[16];
1486  M.at<double>(46, 52) = a[7];
1487  M.at<double>(46, 64) = b[2];
1488  M.at<double>(46, 70) = b[18];
1489  M.at<double>(46, 73) = b[7];
1490  M.at<double>(46, 91) = c[18];
1491  M.at<double>(46, 93) = c[7];
1492  M.at<double>(46, 103) = c[2];
1493  M.at<double>(47, 40) = a[7];
1494  M.at<double>(47, 44) = a[18];
1495  M.at<double>(47, 52) = a[6];
1496  M.at<double>(47, 64) = b[19];
1497  M.at<double>(47, 67) = b[2];
1498  M.at<double>(47, 68) = b[18];
1499  M.at<double>(47, 70) = b[17];
1500  M.at<double>(47, 72) = b[7];
1501  M.at<double>(47, 73) = b[6];
1502  M.at<double>(47, 91) = c[17];
1503  M.at<double>(47, 93) = c[6];
1504  M.at<double>(47, 95) = c[7];
1505  M.at<double>(47, 97) = c[18];
1506  M.at<double>(47, 103) = c[19];
1507  M.at<double>(47, 104) = c[2];
1508  M.at<double>(48, 30) = a[6];
1509  M.at<double>(48, 43) = a[7];
1510  M.at<double>(48, 45) = a[15];
1511  M.at<double>(48, 63) = b[15];
1512  M.at<double>(48, 64) = b[10];
1513  M.at<double>(48, 67) = b[11];
1514  M.at<double>(48, 69) = b[5];
1515  M.at<double>(48, 71) = b[7];
1516  M.at<double>(48, 75) = b[6];
1517  M.at<double>(48, 100) = c[7];
1518  M.at<double>(48, 103) = c[10];
1519  M.at<double>(48, 104) = c[11];
1520  M.at<double>(48, 107) = c[6];
1521  M.at<double>(48, 110) = c[5];
1522  M.at<double>(48, 112) = c[15];
1523  M.at<double>(49, 41) = a[12];
1524  M.at<double>(49, 45) = a[16];
1525  M.at<double>(49, 46) = a[14];
1526  M.at<double>(49, 62) = b[12];
1527  M.at<double>(49, 63) = b[16];
1528  M.at<double>(49, 66) = b[14];
1529  M.at<double>(49, 101) = c[5];
1530  M.at<double>(49, 102) = c[14];
1531  M.at<double>(49, 105) = c[15];
1532  M.at<double>(49, 109) = c[6];
1533  M.at<double>(49, 111) = c[12];
1534  M.at<double>(49, 112) = c[16];
1535  M.at<double>(50, 41) = a[16];
1536  M.at<double>(50, 62) = b[16];
1537  M.at<double>(50, 101) = c[14];
1538  M.at<double>(50, 105) = c[5];
1539  M.at<double>(50, 109) = c[12];
1540  M.at<double>(50, 111) = c[16];
1541  M.at<double>(51, 64) = b[18];
1542  M.at<double>(51, 70) = b[7];
1543  M.at<double>(51, 91) = c[7];
1544  M.at<double>(51, 103) = c[18];
1545  M.at<double>(52, 41) = a[6];
1546  M.at<double>(52, 45) = a[12];
1547  M.at<double>(52, 46) = a[5];
1548  M.at<double>(52, 62) = b[6];
1549  M.at<double>(52, 63) = b[12];
1550  M.at<double>(52, 66) = b[5];
1551  M.at<double>(52, 67) = b[14];
1552  M.at<double>(52, 69) = b[16];
1553  M.at<double>(52, 101) = c[15];
1554  M.at<double>(52, 102) = c[5];
1555  M.at<double>(52, 104) = c[14];
1556  M.at<double>(52, 109) = c[7];
1557  M.at<double>(52, 110) = c[16];
1558  M.at<double>(52, 111) = c[6];
1559  M.at<double>(52, 112) = c[12];
1560  M.at<double>(53, 64) = b[15];
1561  M.at<double>(53, 69) = b[7];
1562  M.at<double>(53, 103) = c[15];
1563  M.at<double>(53, 110) = c[7];
1564  M.at<double>(54, 105) = c[14];
1565  M.at<double>(54, 109) = c[16];
1566  M.at<double>(55, 44) = a[7];
1567  M.at<double>(55, 64) = b[17];
1568  M.at<double>(55, 67) = b[18];
1569  M.at<double>(55, 68) = b[7];
1570  M.at<double>(55, 70) = b[6];
1571  M.at<double>(55, 91) = c[6];
1572  M.at<double>(55, 97) = c[7];
1573  M.at<double>(55, 103) = c[17];
1574  M.at<double>(55, 104) = c[18];
1575  M.at<double>(56, 105) = c[8];
1576  M.at<double>(56, 108) = c[16];
1577  M.at<double>(57, 64) = b[6];
1578  M.at<double>(57, 67) = b[7];
1579  M.at<double>(57, 103) = c[6];
1580  M.at<double>(57, 104) = c[7];
1581  M.at<double>(58, 46) = a[7];
1582  M.at<double>(58, 64) = b[12];
1583  M.at<double>(58, 66) = b[7];
1584  M.at<double>(58, 67) = b[6];
1585  M.at<double>(58, 102) = c[7];
1586  M.at<double>(58, 103) = c[12];
1587  M.at<double>(58, 104) = c[6];
1588  M.at<double>(59, 8) = u[4];
1589  M.at<double>(59, 30) = a[16];
1590  M.at<double>(59, 41) = a[5];
1591  M.at<double>(59, 43) = a[12];
1592  M.at<double>(59, 45) = a[14];
1593  M.at<double>(59, 46) = a[10];
1594  M.at<double>(59, 51) = a[6];
1595  M.at<double>(59, 62) = b[5];
1596  M.at<double>(59, 63) = b[14];
1597  M.at<double>(59, 65) = b[6];
1598  M.at<double>(59, 66) = b[10];
1599  M.at<double>(59, 71) = b[12];
1600  M.at<double>(59, 75) = b[16];
1601  M.at<double>(59, 89) = c[6];
1602  M.at<double>(59, 100) = c[12];
1603  M.at<double>(59, 101) = c[11];
1604  M.at<double>(59, 102) = c[10];
1605  M.at<double>(59, 107) = c[16];
1606  M.at<double>(59, 109) = c[15];
1607  M.at<double>(59, 111) = c[5];
1608  M.at<double>(59, 112) = c[14];
1609  M.at<double>(60, 8) = u[3];
1610  M.at<double>(60, 30) = a[12];
1611  M.at<double>(60, 41) = a[15];
1612  M.at<double>(60, 43) = a[6];
1613  M.at<double>(60, 45) = a[5];
1614  M.at<double>(60, 46) = a[11];
1615  M.at<double>(60, 51) = a[7];
1616  M.at<double>(60, 62) = b[15];
1617  M.at<double>(60, 63) = b[5];
1618  M.at<double>(60, 65) = b[7];
1619  M.at<double>(60, 66) = b[11];
1620  M.at<double>(60, 67) = b[10];
1621  M.at<double>(60, 69) = b[14];
1622  M.at<double>(60, 71) = b[6];
1623  M.at<double>(60, 75) = b[12];
1624  M.at<double>(60, 89) = c[7];
1625  M.at<double>(60, 100) = c[6];
1626  M.at<double>(60, 102) = c[11];
1627  M.at<double>(60, 104) = c[10];
1628  M.at<double>(60, 107) = c[12];
1629  M.at<double>(60, 110) = c[14];
1630  M.at<double>(60, 111) = c[15];
1631  M.at<double>(60, 112) = c[5];
1632  M.at<double>(61, 42) = a[16];
1633  M.at<double>(61, 56) = b[16];
1634  M.at<double>(61, 101) = c[8];
1635  M.at<double>(61, 105) = c[17];
1636  M.at<double>(61, 106) = c[16];
1637  M.at<double>(61, 108) = c[12];
1638  M.at<double>(62, 64) = b[7];
1639  M.at<double>(62, 103) = c[7];
1640  M.at<double>(63, 105) = c[16];
1641  M.at<double>(64, 46) = a[12];
1642  M.at<double>(64, 66) = b[12];
1643  M.at<double>(64, 67) = b[16];
1644  M.at<double>(64, 101) = c[6];
1645  M.at<double>(64, 102) = c[12];
1646  M.at<double>(64, 104) = c[16];
1647  M.at<double>(64, 105) = c[7];
1648  M.at<double>(65, 46) = a[6];
1649  M.at<double>(65, 64) = b[16];
1650  M.at<double>(65, 66) = b[6];
1651  M.at<double>(65, 67) = b[12];
1652  M.at<double>(65, 101) = c[7];
1653  M.at<double>(65, 102) = c[6];
1654  M.at<double>(65, 103) = c[16];
1655  M.at<double>(65, 104) = c[12];
1656  M.at<double>(66, 46) = a[16];
1657  M.at<double>(66, 66) = b[16];
1658  M.at<double>(66, 101) = c[12];
1659  M.at<double>(66, 102) = c[16];
1660  M.at<double>(66, 105) = c[6];
1661  M.at<double>(67, 101) = c[16];
1662  M.at<double>(67, 105) = c[12];
1663  M.at<double>(68, 41) = a[14];
1664  M.at<double>(68, 43) = a[16];
1665  M.at<double>(68, 51) = a[12];
1666  M.at<double>(68, 62) = b[14];
1667  M.at<double>(68, 65) = b[12];
1668  M.at<double>(68, 71) = b[16];
1669  M.at<double>(68, 89) = c[12];
1670  M.at<double>(68, 100) = c[16];
1671  M.at<double>(68, 101) = c[10];
1672  M.at<double>(68, 105) = c[11];
1673  M.at<double>(68, 109) = c[5];
1674  M.at<double>(68, 111) = c[14];
1675  M.at<double>(69, 37) = a[2];
1676  M.at<double>(69, 48) = a[18];
1677  M.at<double>(69, 52) = a[20];
1678  M.at<double>(69, 73) = b[20];
1679  M.at<double>(69, 77) = b[2];
1680  M.at<double>(69, 81) = b[18];
1681  M.at<double>(69, 85) = c[18];
1682  M.at<double>(69, 93) = c[20];
1683  M.at<double>(69, 98) = c[2];
1684  M.at<double>(70, 20) = u[2];
1685  M.at<double>(70, 27) = a[9];
1686  M.at<double>(70, 28) = a[13];
1687  M.at<double>(70, 31) = a[10];
1688  M.at<double>(70, 32) = a[4];
1689  M.at<double>(70, 33) = a[19];
1690  M.at<double>(70, 50) = a[14];
1691  M.at<double>(70, 51) = a[20];
1692  M.at<double>(70, 53) = a[8];
1693  M.at<double>(70, 55) = b[9];
1694  M.at<double>(70, 57) = b[13];
1695  M.at<double>(70, 58) = b[10];
1696  M.at<double>(70, 59) = b[19];
1697  M.at<double>(70, 61) = b[4];
1698  M.at<double>(70, 65) = b[20];
1699  M.at<double>(70, 80) = b[14];
1700  M.at<double>(70, 82) = b[8];
1701  M.at<double>(70, 86) = c[14];
1702  M.at<double>(70, 87) = c[8];
1703  M.at<double>(70, 89) = c[20];
1704  M.at<double>(70, 113) = c[9];
1705  M.at<double>(70, 114) = c[13];
1706  M.at<double>(70, 115) = c[10];
1707  M.at<double>(70, 116) = c[4];
1708  M.at<double>(70, 117) = c[19];
1709  M.at<double>(71, 45) = a[7];
1710  M.at<double>(71, 63) = b[7];
1711  M.at<double>(71, 64) = b[5];
1712  M.at<double>(71, 67) = b[15];
1713  M.at<double>(71, 69) = b[6];
1714  M.at<double>(71, 103) = c[5];
1715  M.at<double>(71, 104) = c[15];
1716  M.at<double>(71, 110) = c[6];
1717  M.at<double>(71, 112) = c[7];
1718  M.at<double>(72, 41) = a[7];
1719  M.at<double>(72, 45) = a[6];
1720  M.at<double>(72, 46) = a[15];
1721  M.at<double>(72, 62) = b[7];
1722  M.at<double>(72, 63) = b[6];
1723  M.at<double>(72, 64) = b[14];
1724  M.at<double>(72, 66) = b[15];
1725  M.at<double>(72, 67) = b[5];
1726  M.at<double>(72, 69) = b[12];
1727  M.at<double>(72, 102) = c[15];
1728  M.at<double>(72, 103) = c[14];
1729  M.at<double>(72, 104) = c[5];
1730  M.at<double>(72, 110) = c[12];
1731  M.at<double>(72, 111) = c[7];
1732  M.at<double>(72, 112) = c[6];
1733  M.at<double>(73, 48) = a[13];
1734  M.at<double>(73, 50) = a[20];
1735  M.at<double>(73, 80) = b[20];
1736  M.at<double>(73, 81) = b[13];
1737  M.at<double>(73, 85) = c[13];
1738  M.at<double>(73, 86) = c[20];
1739  M.at<double>(74, 25) = u[3];
1740  M.at<double>(74, 29) = a[17];
1741  M.at<double>(74, 32) = a[7];
1742  M.at<double>(74, 38) = a[6];
1743  M.at<double>(74, 40) = a[15];
1744  M.at<double>(74, 44) = a[3];
1745  M.at<double>(74, 45) = a[2];
1746  M.at<double>(74, 47) = a[18];
1747  M.at<double>(74, 52) = a[5];
1748  M.at<double>(74, 60) = b[18];
1749  M.at<double>(74, 61) = b[7];
1750  M.at<double>(74, 63) = b[2];
1751  M.at<double>(74, 67) = b[13];
1752  M.at<double>(74, 68) = b[3];
1753  M.at<double>(74, 69) = b[19];
1754  M.at<double>(74, 70) = b[4];
1755  M.at<double>(74, 72) = b[15];
1756  M.at<double>(74, 73) = b[5];
1757  M.at<double>(74, 74) = b[17];
1758  M.at<double>(74, 78) = b[6];
1759  M.at<double>(74, 90) = c[18];
1760  M.at<double>(74, 91) = c[4];
1761  M.at<double>(74, 92) = c[6];
1762  M.at<double>(74, 93) = c[5];
1763  M.at<double>(74, 94) = c[17];
1764  M.at<double>(74, 95) = c[15];
1765  M.at<double>(74, 97) = c[3];
1766  M.at<double>(74, 104) = c[13];
1767  M.at<double>(74, 110) = c[19];
1768  M.at<double>(74, 112) = c[2];
1769  M.at<double>(74, 116) = c[7];
1770  M.at<double>(75, 21) = u[2];
1771  M.at<double>(75, 36) = a[2];
1772  M.at<double>(75, 37) = a[1];
1773  M.at<double>(75, 38) = a[9];
1774  M.at<double>(75, 39) = a[13];
1775  M.at<double>(75, 49) = a[20];
1776  M.at<double>(75, 50) = a[11];
1777  M.at<double>(75, 53) = a[3];
1778  M.at<double>(75, 76) = b[20];
1779  M.at<double>(75, 77) = b[1];
1780  M.at<double>(75, 78) = b[9];
1781  M.at<double>(75, 79) = b[13];
1782  M.at<double>(75, 80) = b[11];
1783  M.at<double>(75, 82) = b[3];
1784  M.at<double>(75, 84) = b[2];
1785  M.at<double>(75, 86) = c[11];
1786  M.at<double>(75, 87) = c[3];
1787  M.at<double>(75, 88) = c[2];
1788  M.at<double>(75, 92) = c[9];
1789  M.at<double>(75, 96) = c[13];
1790  M.at<double>(75, 98) = c[1];
1791  M.at<double>(75, 119) = c[20];
1792  M.at<double>(76, 48) = a[20];
1793  M.at<double>(76, 81) = b[20];
1794  M.at<double>(76, 85) = c[20];
1795  M.at<double>(77, 34) = a[16];
1796  M.at<double>(77, 51) = a[14];
1797  M.at<double>(77, 54) = b[16];
1798  M.at<double>(77, 65) = b[14];
1799  M.at<double>(77, 89) = c[14];
1800  M.at<double>(77, 99) = c[16];
1801  M.at<double>(77, 105) = c[1];
1802  M.at<double>(77, 109) = c[10];
1803  M.at<double>(78, 27) = a[17];
1804  M.at<double>(78, 31) = a[12];
1805  M.at<double>(78, 37) = a[16];
1806  M.at<double>(78, 40) = a[8];
1807  M.at<double>(78, 42) = a[19];
1808  M.at<double>(78, 55) = b[17];
1809  M.at<double>(78, 56) = b[19];
1810  M.at<double>(78, 58) = b[12];
1811  M.at<double>(78, 72) = b[8];
1812  M.at<double>(78, 77) = b[16];
1813  M.at<double>(78, 95) = c[8];
1814  M.at<double>(78, 98) = c[16];
1815  M.at<double>(78, 101) = c[20];
1816  M.at<double>(78, 106) = c[19];
1817  M.at<double>(78, 108) = c[2];
1818  M.at<double>(78, 113) = c[17];
1819  M.at<double>(78, 115) = c[12];
1820  M.at<double>(79, 42) = a[12];
1821  M.at<double>(79, 44) = a[16];
1822  M.at<double>(79, 46) = a[8];
1823  M.at<double>(79, 56) = b[12];
1824  M.at<double>(79, 66) = b[8];
1825  M.at<double>(79, 68) = b[16];
1826  M.at<double>(79, 97) = c[16];
1827  M.at<double>(79, 101) = c[17];
1828  M.at<double>(79, 102) = c[8];
1829  M.at<double>(79, 105) = c[18];
1830  M.at<double>(79, 106) = c[12];
1831  M.at<double>(79, 108) = c[6];
1832  M.at<double>(80, 14) = u[4];
1833  M.at<double>(80, 28) = a[5];
1834  M.at<double>(80, 33) = a[12];
1835  M.at<double>(80, 39) = a[16];
1836  M.at<double>(80, 41) = a[4];
1837  M.at<double>(80, 42) = a[10];
1838  M.at<double>(80, 43) = a[8];
1839  M.at<double>(80, 47) = a[14];
1840  M.at<double>(80, 51) = a[17];
1841  M.at<double>(80, 56) = b[10];
1842  M.at<double>(80, 57) = b[5];
1843  M.at<double>(80, 59) = b[12];
1844  M.at<double>(80, 60) = b[14];
1845  M.at<double>(80, 62) = b[4];
1846  M.at<double>(80, 65) = b[17];
1847  M.at<double>(80, 71) = b[8];
1848  M.at<double>(80, 79) = b[16];
1849  M.at<double>(80, 89) = c[17];
1850  M.at<double>(80, 90) = c[14];
1851  M.at<double>(80, 96) = c[16];
1852  M.at<double>(80, 100) = c[8];
1853  M.at<double>(80, 101) = c[9];
1854  M.at<double>(80, 106) = c[10];
1855  M.at<double>(80, 108) = c[11];
1856  M.at<double>(80, 109) = c[3];
1857  M.at<double>(80, 111) = c[4];
1858  M.at<double>(80, 114) = c[5];
1859  M.at<double>(80, 117) = c[12];
1860  M.at<double>(81, 31) = a[3];
1861  M.at<double>(81, 32) = a[2];
1862  M.at<double>(81, 37) = a[4];
1863  M.at<double>(81, 38) = a[19];
1864  M.at<double>(81, 40) = a[13];
1865  M.at<double>(81, 47) = a[20];
1866  M.at<double>(81, 48) = a[5];
1867  M.at<double>(81, 50) = a[17];
1868  M.at<double>(81, 58) = b[3];
1869  M.at<double>(81, 60) = b[20];
1870  M.at<double>(81, 61) = b[2];
1871  M.at<double>(81, 72) = b[13];
1872  M.at<double>(81, 77) = b[4];
1873  M.at<double>(81, 78) = b[19];
1874  M.at<double>(81, 80) = b[17];
1875  M.at<double>(81, 81) = b[5];
1876  M.at<double>(81, 85) = c[5];
1877  M.at<double>(81, 86) = c[17];
1878  M.at<double>(81, 90) = c[20];
1879  M.at<double>(81, 92) = c[19];
1880  M.at<double>(81, 95) = c[13];
1881  M.at<double>(81, 98) = c[4];
1882  M.at<double>(81, 115) = c[3];
1883  M.at<double>(81, 116) = c[2];
1884  M.at<double>(82, 29) = a[6];
1885  M.at<double>(82, 44) = a[15];
1886  M.at<double>(82, 45) = a[18];
1887  M.at<double>(82, 47) = a[7];
1888  M.at<double>(82, 60) = b[7];
1889  M.at<double>(82, 63) = b[18];
1890  M.at<double>(82, 64) = b[4];
1891  M.at<double>(82, 67) = b[3];
1892  M.at<double>(82, 68) = b[15];
1893  M.at<double>(82, 69) = b[17];
1894  M.at<double>(82, 70) = b[5];
1895  M.at<double>(82, 74) = b[6];
1896  M.at<double>(82, 90) = c[7];
1897  M.at<double>(82, 91) = c[5];
1898  M.at<double>(82, 94) = c[6];
1899  M.at<double>(82, 97) = c[15];
1900  M.at<double>(82, 103) = c[4];
1901  M.at<double>(82, 104) = c[3];
1902  M.at<double>(82, 110) = c[17];
1903  M.at<double>(82, 112) = c[18];
1904  M.at<double>(83, 26) = u[2];
1905  M.at<double>(83, 27) = a[18];
1906  M.at<double>(83, 31) = a[6];
1907  M.at<double>(83, 37) = a[12];
1908  M.at<double>(83, 40) = a[17];
1909  M.at<double>(83, 42) = a[2];
1910  M.at<double>(83, 44) = a[19];
1911  M.at<double>(83, 46) = a[20];
1912  M.at<double>(83, 52) = a[8];
1913  M.at<double>(83, 55) = b[18];
1914  M.at<double>(83, 56) = b[2];
1915  M.at<double>(83, 58) = b[6];
1916  M.at<double>(83, 66) = b[20];
1917  M.at<double>(83, 68) = b[19];
1918  M.at<double>(83, 72) = b[17];
1919  M.at<double>(83, 73) = b[8];
1920  M.at<double>(83, 77) = b[12];
1921  M.at<double>(83, 93) = c[8];
1922  M.at<double>(83, 95) = c[17];
1923  M.at<double>(83, 97) = c[19];
1924  M.at<double>(83, 98) = c[12];
1925  M.at<double>(83, 102) = c[20];
1926  M.at<double>(83, 106) = c[2];
1927  M.at<double>(83, 113) = c[18];
1928  M.at<double>(83, 115) = c[6];
1929  M.at<double>(84, 16) = u[3];
1930  M.at<double>(84, 29) = a[5];
1931  M.at<double>(84, 30) = a[17];
1932  M.at<double>(84, 33) = a[7];
1933  M.at<double>(84, 39) = a[6];
1934  M.at<double>(84, 43) = a[18];
1935  M.at<double>(84, 44) = a[11];
1936  M.at<double>(84, 45) = a[3];
1937  M.at<double>(84, 47) = a[15];
1938  M.at<double>(84, 59) = b[7];
1939  M.at<double>(84, 60) = b[15];
1940  M.at<double>(84, 63) = b[3];
1941  M.at<double>(84, 67) = b[9];
1942  M.at<double>(84, 68) = b[11];
1943  M.at<double>(84, 69) = b[4];
1944  M.at<double>(84, 70) = b[10];
1945  M.at<double>(84, 71) = b[18];
1946  M.at<double>(84, 74) = b[5];
1947  M.at<double>(84, 75) = b[17];
1948  M.at<double>(84, 79) = b[6];
1949  M.at<double>(84, 90) = c[15];
1950  M.at<double>(84, 91) = c[10];
1951  M.at<double>(84, 94) = c[5];
1952  M.at<double>(84, 96) = c[6];
1953  M.at<double>(84, 97) = c[11];
1954  M.at<double>(84, 100) = c[18];
1955  M.at<double>(84, 104) = c[9];
1956  M.at<double>(84, 107) = c[17];
1957  M.at<double>(84, 110) = c[4];
1958  M.at<double>(84, 112) = c[3];
1959  M.at<double>(84, 117) = c[7];
1960  M.at<double>(85, 25) = u[2];
1961  M.at<double>(85, 29) = a[19];
1962  M.at<double>(85, 31) = a[15];
1963  M.at<double>(85, 32) = a[18];
1964  M.at<double>(85, 37) = a[5];
1965  M.at<double>(85, 38) = a[17];
1966  M.at<double>(85, 40) = a[3];
1967  M.at<double>(85, 44) = a[13];
1968  M.at<double>(85, 45) = a[20];
1969  M.at<double>(85, 47) = a[2];
1970  M.at<double>(85, 50) = a[6];
1971  M.at<double>(85, 52) = a[4];
1972  M.at<double>(85, 58) = b[15];
1973  M.at<double>(85, 60) = b[2];
1974  M.at<double>(85, 61) = b[18];
1975  M.at<double>(85, 63) = b[20];
1976  M.at<double>(85, 68) = b[13];
1977  M.at<double>(85, 72) = b[3];
1978  M.at<double>(85, 73) = b[4];
1979  M.at<double>(85, 74) = b[19];
1980  M.at<double>(85, 77) = b[5];
1981  M.at<double>(85, 78) = b[17];
1982  M.at<double>(85, 80) = b[6];
1983  M.at<double>(85, 86) = c[6];
1984  M.at<double>(85, 90) = c[2];
1985  M.at<double>(85, 92) = c[17];
1986  M.at<double>(85, 93) = c[4];
1987  M.at<double>(85, 94) = c[19];
1988  M.at<double>(85, 95) = c[3];
1989  M.at<double>(85, 97) = c[13];
1990  M.at<double>(85, 98) = c[5];
1991  M.at<double>(85, 112) = c[20];
1992  M.at<double>(85, 115) = c[15];
1993  M.at<double>(85, 116) = c[18];
1994  M.at<double>(86, 31) = a[18];
1995  M.at<double>(86, 37) = a[17];
1996  M.at<double>(86, 40) = a[2];
1997  M.at<double>(86, 44) = a[20];
1998  M.at<double>(86, 48) = a[6];
1999  M.at<double>(86, 52) = a[19];
2000  M.at<double>(86, 58) = b[18];
2001  M.at<double>(86, 68) = b[20];
2002  M.at<double>(86, 72) = b[2];
2003  M.at<double>(86, 73) = b[19];
2004  M.at<double>(86, 77) = b[17];
2005  M.at<double>(86, 81) = b[6];
2006  M.at<double>(86, 85) = c[6];
2007  M.at<double>(86, 93) = c[19];
2008  M.at<double>(86, 95) = c[2];
2009  M.at<double>(86, 97) = c[20];
2010  M.at<double>(86, 98) = c[17];
2011  M.at<double>(86, 115) = c[18];
2012  M.at<double>(87, 22) = u[2];
2013  M.at<double>(87, 31) = a[11];
2014  M.at<double>(87, 32) = a[3];
2015  M.at<double>(87, 33) = a[2];
2016  M.at<double>(87, 37) = a[10];
2017  M.at<double>(87, 38) = a[4];
2018  M.at<double>(87, 39) = a[19];
2019  M.at<double>(87, 40) = a[9];
2020  M.at<double>(87, 43) = a[20];
2021  M.at<double>(87, 47) = a[13];
2022  M.at<double>(87, 50) = a[5];
2023  M.at<double>(87, 53) = a[17];
2024  M.at<double>(87, 58) = b[11];
2025  M.at<double>(87, 59) = b[2];
2026  M.at<double>(87, 60) = b[13];
2027  M.at<double>(87, 61) = b[3];
2028  M.at<double>(87, 71) = b[20];
2029  M.at<double>(87, 72) = b[9];
2030  M.at<double>(87, 77) = b[10];
2031  M.at<double>(87, 78) = b[4];
2032  M.at<double>(87, 79) = b[19];
2033  M.at<double>(87, 80) = b[5];
2034  M.at<double>(87, 82) = b[17];
2035  M.at<double>(87, 86) = c[5];
2036  M.at<double>(87, 87) = c[17];
2037  M.at<double>(87, 90) = c[13];
2038  M.at<double>(87, 92) = c[4];
2039  M.at<double>(87, 95) = c[9];
2040  M.at<double>(87, 96) = c[19];
2041  M.at<double>(87, 98) = c[10];
2042  M.at<double>(87, 100) = c[20];
2043  M.at<double>(87, 115) = c[11];
2044  M.at<double>(87, 116) = c[3];
2045  M.at<double>(87, 117) = c[2];
2046  M.at<double>(88, 27) = a[2];
2047  M.at<double>(88, 31) = a[17];
2048  M.at<double>(88, 37) = a[8];
2049  M.at<double>(88, 40) = a[19];
2050  M.at<double>(88, 42) = a[20];
2051  M.at<double>(88, 48) = a[12];
2052  M.at<double>(88, 55) = b[2];
2053  M.at<double>(88, 56) = b[20];
2054  M.at<double>(88, 58) = b[17];
2055  M.at<double>(88, 72) = b[19];
2056  M.at<double>(88, 77) = b[8];
2057  M.at<double>(88, 81) = b[12];
2058  M.at<double>(88, 85) = c[12];
2059  M.at<double>(88, 95) = c[19];
2060  M.at<double>(88, 98) = c[8];
2061  M.at<double>(88, 106) = c[20];
2062  M.at<double>(88, 113) = c[2];
2063  M.at<double>(88, 115) = c[17];
2064  M.at<double>(89, 31) = a[7];
2065  M.at<double>(89, 37) = a[6];
2066  M.at<double>(89, 40) = a[18];
2067  M.at<double>(89, 44) = a[2];
2068  M.at<double>(89, 52) = a[17];
2069  M.at<double>(89, 58) = b[7];
2070  M.at<double>(89, 67) = b[20];
2071  M.at<double>(89, 68) = b[2];
2072  M.at<double>(89, 70) = b[19];
2073  M.at<double>(89, 72) = b[18];
2074  M.at<double>(89, 73) = b[17];
2075  M.at<double>(89, 77) = b[6];
2076  M.at<double>(89, 91) = c[19];
2077  M.at<double>(89, 93) = c[17];
2078  M.at<double>(89, 95) = c[18];
2079  M.at<double>(89, 97) = c[2];
2080  M.at<double>(89, 98) = c[6];
2081  M.at<double>(89, 104) = c[20];
2082  M.at<double>(89, 115) = c[7];
2083  M.at<double>(90, 27) = a[12];
2084  M.at<double>(90, 40) = a[16];
2085  M.at<double>(90, 42) = a[8];
2086  M.at<double>(90, 55) = b[12];
2087  M.at<double>(90, 56) = b[8];
2088  M.at<double>(90, 72) = b[16];
2089  M.at<double>(90, 95) = c[16];
2090  M.at<double>(90, 101) = c[19];
2091  M.at<double>(90, 105) = c[2];
2092  M.at<double>(90, 106) = c[8];
2093  M.at<double>(90, 108) = c[17];
2094  M.at<double>(90, 113) = c[12];
2095  M.at<double>(91, 23) = u[2];
2096  M.at<double>(91, 27) = a[3];
2097  M.at<double>(91, 28) = a[2];
2098  M.at<double>(91, 31) = a[5];
2099  M.at<double>(91, 32) = a[17];
2100  M.at<double>(91, 37) = a[14];
2101  M.at<double>(91, 38) = a[8];
2102  M.at<double>(91, 40) = a[4];
2103  M.at<double>(91, 41) = a[20];
2104  M.at<double>(91, 42) = a[13];
2105  M.at<double>(91, 47) = a[19];
2106  M.at<double>(91, 50) = a[12];
2107  M.at<double>(91, 55) = b[3];
2108  M.at<double>(91, 56) = b[13];
2109  M.at<double>(91, 57) = b[2];
2110  M.at<double>(91, 58) = b[5];
2111  M.at<double>(91, 60) = b[19];
2112  M.at<double>(91, 61) = b[17];
2113  M.at<double>(91, 62) = b[20];
2114  M.at<double>(91, 72) = b[4];
2115  M.at<double>(91, 77) = b[14];
2116  M.at<double>(91, 78) = b[8];
2117  M.at<double>(91, 80) = b[12];
2118  M.at<double>(91, 86) = c[12];
2119  M.at<double>(91, 90) = c[19];
2120  M.at<double>(91, 92) = c[8];
2121  M.at<double>(91, 95) = c[4];
2122  M.at<double>(91, 98) = c[14];
2123  M.at<double>(91, 106) = c[13];
2124  M.at<double>(91, 111) = c[20];
2125  M.at<double>(91, 113) = c[3];
2126  M.at<double>(91, 114) = c[2];
2127  M.at<double>(91, 115) = c[5];
2128  M.at<double>(91, 116) = c[17];
2129  M.at<double>(92, 17) = u[4];
2130  M.at<double>(92, 28) = a[6];
2131  M.at<double>(92, 29) = a[16];
2132  M.at<double>(92, 41) = a[17];
2133  M.at<double>(92, 42) = a[5];
2134  M.at<double>(92, 44) = a[14];
2135  M.at<double>(92, 45) = a[8];
2136  M.at<double>(92, 46) = a[4];
2137  M.at<double>(92, 47) = a[12];
2138  M.at<double>(92, 56) = b[5];
2139  M.at<double>(92, 57) = b[6];
2140  M.at<double>(92, 60) = b[12];
2141  M.at<double>(92, 62) = b[17];
2142  M.at<double>(92, 63) = b[8];
2143  M.at<double>(92, 66) = b[4];
2144  M.at<double>(92, 68) = b[14];
2145  M.at<double>(92, 74) = b[16];
2146  M.at<double>(92, 90) = c[12];
2147  M.at<double>(92, 94) = c[16];
2148  M.at<double>(92, 97) = c[14];
2149  M.at<double>(92, 101) = c[3];
2150  M.at<double>(92, 102) = c[4];
2151  M.at<double>(92, 106) = c[5];
2152  M.at<double>(92, 108) = c[15];
2153  M.at<double>(92, 109) = c[18];
2154  M.at<double>(92, 111) = c[17];
2155  M.at<double>(92, 112) = c[8];
2156  M.at<double>(92, 114) = c[6];
2157  M.at<double>(93, 17) = u[3];
2158  M.at<double>(93, 28) = a[7];
2159  M.at<double>(93, 29) = a[12];
2160  M.at<double>(93, 41) = a[18];
2161  M.at<double>(93, 42) = a[15];
2162  M.at<double>(93, 44) = a[5];
2163  M.at<double>(93, 45) = a[17];
2164  M.at<double>(93, 46) = a[3];
2165  M.at<double>(93, 47) = a[6];
2166  M.at<double>(93, 56) = b[15];
2167  M.at<double>(93, 57) = b[7];
2168  M.at<double>(93, 60) = b[6];
2169  M.at<double>(93, 62) = b[18];
2170  M.at<double>(93, 63) = b[17];
2171  M.at<double>(93, 66) = b[3];
2172  M.at<double>(93, 67) = b[4];
2173  M.at<double>(93, 68) = b[5];
2174  M.at<double>(93, 69) = b[8];
2175  M.at<double>(93, 70) = b[14];
2176  M.at<double>(93, 74) = b[12];
2177  M.at<double>(93, 90) = c[6];
2178  M.at<double>(93, 91) = c[14];
2179  M.at<double>(93, 94) = c[12];
2180  M.at<double>(93, 97) = c[5];
2181  M.at<double>(93, 102) = c[3];
2182  M.at<double>(93, 104) = c[4];
2183  M.at<double>(93, 106) = c[15];
2184  M.at<double>(93, 110) = c[8];
2185  M.at<double>(93, 111) = c[18];
2186  M.at<double>(93, 112) = c[17];
2187  M.at<double>(93, 114) = c[7];
2188  M.at<double>(94, 31) = a[2];
2189  M.at<double>(94, 37) = a[19];
2190  M.at<double>(94, 40) = a[20];
2191  M.at<double>(94, 48) = a[17];
2192  M.at<double>(94, 58) = b[2];
2193  M.at<double>(94, 72) = b[20];
2194  M.at<double>(94, 77) = b[19];
2195  M.at<double>(94, 81) = b[17];
2196  M.at<double>(94, 85) = c[17];
2197  M.at<double>(94, 95) = c[20];
2198  M.at<double>(94, 98) = c[19];
2199  M.at<double>(94, 115) = c[2];
2200  M.at<double>(95, 26) = u[4];
2201  M.at<double>(95, 27) = a[6];
2202  M.at<double>(95, 40) = a[12];
2203  M.at<double>(95, 42) = a[17];
2204  M.at<double>(95, 44) = a[8];
2205  M.at<double>(95, 46) = a[19];
2206  M.at<double>(95, 52) = a[16];
2207  M.at<double>(95, 55) = b[6];
2208  M.at<double>(95, 56) = b[17];
2209  M.at<double>(95, 66) = b[19];
2210  M.at<double>(95, 68) = b[8];
2211  M.at<double>(95, 72) = b[12];
2212  M.at<double>(95, 73) = b[16];
2213  M.at<double>(95, 93) = c[16];
2214  M.at<double>(95, 95) = c[12];
2215  M.at<double>(95, 97) = c[8];
2216  M.at<double>(95, 101) = c[2];
2217  M.at<double>(95, 102) = c[19];
2218  M.at<double>(95, 106) = c[17];
2219  M.at<double>(95, 108) = c[18];
2220  M.at<double>(95, 113) = c[6];
2221  M.at<double>(96, 23) = u[4];
2222  M.at<double>(96, 27) = a[5];
2223  M.at<double>(96, 28) = a[17];
2224  M.at<double>(96, 32) = a[12];
2225  M.at<double>(96, 38) = a[16];
2226  M.at<double>(96, 40) = a[14];
2227  M.at<double>(96, 41) = a[19];
2228  M.at<double>(96, 42) = a[4];
2229  M.at<double>(96, 47) = a[8];
2230  M.at<double>(96, 55) = b[5];
2231  M.at<double>(96, 56) = b[4];
2232  M.at<double>(96, 57) = b[17];
2233  M.at<double>(96, 60) = b[8];
2234  M.at<double>(96, 61) = b[12];
2235  M.at<double>(96, 62) = b[19];
2236  M.at<double>(96, 72) = b[14];
2237  M.at<double>(96, 78) = b[16];
2238  M.at<double>(96, 90) = c[8];
2239  M.at<double>(96, 92) = c[16];
2240  M.at<double>(96, 95) = c[14];
2241  M.at<double>(96, 101) = c[13];
2242  M.at<double>(96, 106) = c[4];
2243  M.at<double>(96, 108) = c[3];
2244  M.at<double>(96, 109) = c[2];
2245  M.at<double>(96, 111) = c[19];
2246  M.at<double>(96, 113) = c[5];
2247  M.at<double>(96, 114) = c[17];
2248  M.at<double>(96, 116) = c[12];
2249  M.at<double>(97, 42) = a[6];
2250  M.at<double>(97, 44) = a[12];
2251  M.at<double>(97, 46) = a[17];
2252  M.at<double>(97, 56) = b[6];
2253  M.at<double>(97, 66) = b[17];
2254  M.at<double>(97, 67) = b[8];
2255  M.at<double>(97, 68) = b[12];
2256  M.at<double>(97, 70) = b[16];
2257  M.at<double>(97, 91) = c[16];
2258  M.at<double>(97, 97) = c[12];
2259  M.at<double>(97, 101) = c[18];
2260  M.at<double>(97, 102) = c[17];
2261  M.at<double>(97, 104) = c[8];
2262  M.at<double>(97, 106) = c[6];
2263  M.at<double>(97, 108) = c[7];
2264  M.at<double>(98, 28) = a[12];
2265  M.at<double>(98, 41) = a[8];
2266  M.at<double>(98, 42) = a[14];
2267  M.at<double>(98, 47) = a[16];
2268  M.at<double>(98, 56) = b[14];
2269  M.at<double>(98, 57) = b[12];
2270  M.at<double>(98, 60) = b[16];
2271  M.at<double>(98, 62) = b[8];
2272  M.at<double>(98, 90) = c[16];
2273  M.at<double>(98, 101) = c[4];
2274  M.at<double>(98, 105) = c[3];
2275  M.at<double>(98, 106) = c[14];
2276  M.at<double>(98, 108) = c[5];
2277  M.at<double>(98, 109) = c[17];
2278  M.at<double>(98, 111) = c[8];
2279  M.at<double>(98, 114) = c[12];
2280  M.at<double>(99, 42) = a[7];
2281  M.at<double>(99, 44) = a[6];
2282  M.at<double>(99, 46) = a[18];
2283  M.at<double>(99, 56) = b[7];
2284  M.at<double>(99, 64) = b[8];
2285  M.at<double>(99, 66) = b[18];
2286  M.at<double>(99, 67) = b[17];
2287  M.at<double>(99, 68) = b[6];
2288  M.at<double>(99, 70) = b[12];
2289  M.at<double>(99, 91) = c[12];
2290  M.at<double>(99, 97) = c[6];
2291  M.at<double>(99, 102) = c[18];
2292  M.at<double>(99, 103) = c[8];
2293  M.at<double>(99, 104) = c[17];
2294  M.at<double>(99, 106) = c[7];
2295  M.at<double>(100, 51) = a[16];
2296  M.at<double>(100, 65) = b[16];
2297  M.at<double>(100, 89) = c[16];
2298  M.at<double>(100, 105) = c[10];
2299  M.at<double>(100, 109) = c[14];
2300  M.at<double>(101, 37) = a[9];
2301  M.at<double>(101, 38) = a[13];
2302  M.at<double>(101, 39) = a[20];
2303  M.at<double>(101, 48) = a[11];
2304  M.at<double>(101, 50) = a[3];
2305  M.at<double>(101, 53) = a[2];
2306  M.at<double>(101, 77) = b[9];
2307  M.at<double>(101, 78) = b[13];
2308  M.at<double>(101, 79) = b[20];
2309  M.at<double>(101, 80) = b[3];
2310  M.at<double>(101, 81) = b[11];
2311  M.at<double>(101, 82) = b[2];
2312  M.at<double>(101, 85) = c[11];
2313  M.at<double>(101, 86) = c[3];
2314  M.at<double>(101, 87) = c[2];
2315  M.at<double>(101, 92) = c[13];
2316  M.at<double>(101, 96) = c[20];
2317  M.at<double>(101, 98) = c[9];
2318  M.at<double>(102, 37) = a[13];
2319  M.at<double>(102, 38) = a[20];
2320  M.at<double>(102, 48) = a[3];
2321  M.at<double>(102, 50) = a[2];
2322  M.at<double>(102, 77) = b[13];
2323  M.at<double>(102, 78) = b[20];
2324  M.at<double>(102, 80) = b[2];
2325  M.at<double>(102, 81) = b[3];
2326  M.at<double>(102, 85) = c[3];
2327  M.at<double>(102, 86) = c[2];
2328  M.at<double>(102, 92) = c[20];
2329  M.at<double>(102, 98) = c[13];
2330  M.at<double>(103, 37) = a[20];
2331  M.at<double>(103, 48) = a[2];
2332  M.at<double>(103, 77) = b[20];
2333  M.at<double>(103, 81) = b[2];
2334  M.at<double>(103, 85) = c[2];
2335  M.at<double>(103, 98) = c[20];
2336  M.at<double>(104, 11) = u[4];
2337  M.at<double>(104, 28) = a[10];
2338  M.at<double>(104, 33) = a[14];
2339  M.at<double>(104, 34) = a[8];
2340  M.at<double>(104, 36) = a[16];
2341  M.at<double>(104, 51) = a[4];
2342  M.at<double>(104, 54) = b[8];
2343  M.at<double>(104, 57) = b[10];
2344  M.at<double>(104, 59) = b[14];
2345  M.at<double>(104, 65) = b[4];
2346  M.at<double>(104, 84) = b[16];
2347  M.at<double>(104, 88) = c[16];
2348  M.at<double>(104, 89) = c[4];
2349  M.at<double>(104, 99) = c[8];
2350  M.at<double>(104, 108) = c[1];
2351  M.at<double>(104, 109) = c[9];
2352  M.at<double>(104, 114) = c[10];
2353  M.at<double>(104, 117) = c[14];
2354  M.at<double>(105, 20) = u[4];
2355  M.at<double>(105, 27) = a[10];
2356  M.at<double>(105, 28) = a[4];
2357  M.at<double>(105, 32) = a[14];
2358  M.at<double>(105, 33) = a[8];
2359  M.at<double>(105, 51) = a[19];
2360  M.at<double>(105, 53) = a[16];
2361  M.at<double>(105, 55) = b[10];
2362  M.at<double>(105, 57) = b[4];
2363  M.at<double>(105, 59) = b[8];
2364  M.at<double>(105, 61) = b[14];
2365  M.at<double>(105, 65) = b[19];
2366  M.at<double>(105, 82) = b[16];
2367  M.at<double>(105, 87) = c[16];
2368  M.at<double>(105, 89) = c[19];
2369  M.at<double>(105, 108) = c[9];
2370  M.at<double>(105, 109) = c[13];
2371  M.at<double>(105, 113) = c[10];
2372  M.at<double>(105, 114) = c[4];
2373  M.at<double>(105, 116) = c[14];
2374  M.at<double>(105, 117) = c[8];
2375  M.at<double>(106, 27) = a[4];
2376  M.at<double>(106, 28) = a[19];
2377  M.at<double>(106, 31) = a[14];
2378  M.at<double>(106, 32) = a[8];
2379  M.at<double>(106, 50) = a[16];
2380  M.at<double>(106, 55) = b[4];
2381  M.at<double>(106, 57) = b[19];
2382  M.at<double>(106, 58) = b[14];
2383  M.at<double>(106, 61) = b[8];
2384  M.at<double>(106, 80) = b[16];
2385  M.at<double>(106, 86) = c[16];
2386  M.at<double>(106, 108) = c[13];
2387  M.at<double>(106, 109) = c[20];
2388  M.at<double>(106, 113) = c[4];
2389  M.at<double>(106, 114) = c[19];
2390  M.at<double>(106, 115) = c[14];
2391  M.at<double>(106, 116) = c[8];
2392  M.at<double>(107, 27) = a[19];
2393  M.at<double>(107, 31) = a[8];
2394  M.at<double>(107, 48) = a[16];
2395  M.at<double>(107, 55) = b[19];
2396  M.at<double>(107, 58) = b[8];
2397  M.at<double>(107, 81) = b[16];
2398  M.at<double>(107, 85) = c[16];
2399  M.at<double>(107, 108) = c[20];
2400  M.at<double>(107, 113) = c[19];
2401  M.at<double>(107, 115) = c[8];
2402  M.at<double>(108, 36) = a[20];
2403  M.at<double>(108, 48) = a[1];
2404  M.at<double>(108, 50) = a[9];
2405  M.at<double>(108, 53) = a[13];
2406  M.at<double>(108, 80) = b[9];
2407  M.at<double>(108, 81) = b[1];
2408  M.at<double>(108, 82) = b[13];
2409  M.at<double>(108, 84) = b[20];
2410  M.at<double>(108, 85) = c[1];
2411  M.at<double>(108, 86) = c[9];
2412  M.at<double>(108, 87) = c[13];
2413  M.at<double>(108, 88) = c[20];
2414  M.at<double>(109, 26) = u[3];
2415  M.at<double>(109, 27) = a[7];
2416  M.at<double>(109, 40) = a[6];
2417  M.at<double>(109, 42) = a[18];
2418  M.at<double>(109, 44) = a[17];
2419  M.at<double>(109, 46) = a[2];
2420  M.at<double>(109, 52) = a[12];
2421  M.at<double>(109, 55) = b[7];
2422  M.at<double>(109, 56) = b[18];
2423  M.at<double>(109, 66) = b[2];
2424  M.at<double>(109, 67) = b[19];
2425  M.at<double>(109, 68) = b[17];
2426  M.at<double>(109, 70) = b[8];
2427  M.at<double>(109, 72) = b[6];
2428  M.at<double>(109, 73) = b[12];
2429  M.at<double>(109, 91) = c[8];
2430  M.at<double>(109, 93) = c[12];
2431  M.at<double>(109, 95) = c[6];
2432  M.at<double>(109, 97) = c[17];
2433  M.at<double>(109, 102) = c[2];
2434  M.at<double>(109, 104) = c[19];
2435  M.at<double>(109, 106) = c[18];
2436  M.at<double>(109, 113) = c[7];
2437  M.at<double>(110, 7) = u[3];
2438  M.at<double>(110, 30) = a[5];
2439  M.at<double>(110, 34) = a[7];
2440  M.at<double>(110, 43) = a[15];
2441  M.at<double>(110, 45) = a[11];
2442  M.at<double>(110, 49) = a[6];
2443  M.at<double>(110, 54) = b[7];
2444  M.at<double>(110, 63) = b[11];
2445  M.at<double>(110, 67) = b[1];
2446  M.at<double>(110, 69) = b[10];
2447  M.at<double>(110, 71) = b[15];
2448  M.at<double>(110, 75) = b[5];
2449  M.at<double>(110, 76) = b[6];
2450  M.at<double>(110, 99) = c[7];
2451  M.at<double>(110, 100) = c[15];
2452  M.at<double>(110, 104) = c[1];
2453  M.at<double>(110, 107) = c[5];
2454  M.at<double>(110, 110) = c[10];
2455  M.at<double>(110, 112) = c[11];
2456  M.at<double>(110, 119) = c[6];
2457  M.at<double>(111, 18) = u[2];
2458  M.at<double>(111, 35) = a[20];
2459  M.at<double>(111, 36) = a[13];
2460  M.at<double>(111, 50) = a[1];
2461  M.at<double>(111, 53) = a[9];
2462  M.at<double>(111, 80) = b[1];
2463  M.at<double>(111, 82) = b[9];
2464  M.at<double>(111, 83) = b[20];
2465  M.at<double>(111, 84) = b[13];
2466  M.at<double>(111, 86) = c[1];
2467  M.at<double>(111, 87) = c[9];
2468  M.at<double>(111, 88) = c[13];
2469  M.at<double>(111, 118) = c[20];
2470  M.at<double>(112, 19) = u[2];
2471  M.at<double>(112, 31) = a[1];
2472  M.at<double>(112, 32) = a[9];
2473  M.at<double>(112, 33) = a[13];
2474  M.at<double>(112, 34) = a[20];
2475  M.at<double>(112, 36) = a[19];
2476  M.at<double>(112, 50) = a[10];
2477  M.at<double>(112, 53) = a[4];
2478  M.at<double>(112, 54) = b[20];
2479  M.at<double>(112, 58) = b[1];
2480  M.at<double>(112, 59) = b[13];
2481  M.at<double>(112, 61) = b[9];
2482  M.at<double>(112, 80) = b[10];
2483  M.at<double>(112, 82) = b[4];
2484  M.at<double>(112, 84) = b[19];
2485  M.at<double>(112, 86) = c[10];
2486  M.at<double>(112, 87) = c[4];
2487  M.at<double>(112, 88) = c[19];
2488  M.at<double>(112, 99) = c[20];
2489  M.at<double>(112, 115) = c[1];
2490  M.at<double>(112, 116) = c[9];
2491  M.at<double>(112, 117) = c[13];
2492  M.at<double>(113, 31) = a[9];
2493  M.at<double>(113, 32) = a[13];
2494  M.at<double>(113, 33) = a[20];
2495  M.at<double>(113, 48) = a[10];
2496  M.at<double>(113, 50) = a[4];
2497  M.at<double>(113, 53) = a[19];
2498  M.at<double>(113, 58) = b[9];
2499  M.at<double>(113, 59) = b[20];
2500  M.at<double>(113, 61) = b[13];
2501  M.at<double>(113, 80) = b[4];
2502  M.at<double>(113, 81) = b[10];
2503  M.at<double>(113, 82) = b[19];
2504  M.at<double>(113, 85) = c[10];
2505  M.at<double>(113, 86) = c[4];
2506  M.at<double>(113, 87) = c[19];
2507  M.at<double>(113, 115) = c[9];
2508  M.at<double>(113, 116) = c[13];
2509  M.at<double>(113, 117) = c[20];
2510  M.at<double>(114, 31) = a[13];
2511  M.at<double>(114, 32) = a[20];
2512  M.at<double>(114, 48) = a[4];
2513  M.at<double>(114, 50) = a[19];
2514  M.at<double>(114, 58) = b[13];
2515  M.at<double>(114, 61) = b[20];
2516  M.at<double>(114, 80) = b[19];
2517  M.at<double>(114, 81) = b[4];
2518  M.at<double>(114, 85) = c[4];
2519  M.at<double>(114, 86) = c[19];
2520  M.at<double>(114, 115) = c[13];
2521  M.at<double>(114, 116) = c[20];
2522  M.at<double>(115, 31) = a[20];
2523  M.at<double>(115, 48) = a[19];
2524  M.at<double>(115, 58) = b[20];
2525  M.at<double>(115, 81) = b[19];
2526  M.at<double>(115, 85) = c[19];
2527  M.at<double>(115, 115) = c[20];
2528  M.at<double>(116, 24) = u[2];
2529  M.at<double>(116, 29) = a[13];
2530  M.at<double>(116, 30) = a[20];
2531  M.at<double>(116, 37) = a[11];
2532  M.at<double>(116, 38) = a[3];
2533  M.at<double>(116, 39) = a[2];
2534  M.at<double>(116, 50) = a[15];
2535  M.at<double>(116, 52) = a[9];
2536  M.at<double>(116, 53) = a[18];
2537  M.at<double>(116, 73) = b[9];
2538  M.at<double>(116, 74) = b[13];
2539  M.at<double>(116, 75) = b[20];
2540  M.at<double>(116, 77) = b[11];
2541  M.at<double>(116, 78) = b[3];
2542  M.at<double>(116, 79) = b[2];
2543  M.at<double>(116, 80) = b[15];
2544  M.at<double>(116, 82) = b[18];
2545  M.at<double>(116, 86) = c[15];
2546  M.at<double>(116, 87) = c[18];
2547  M.at<double>(116, 92) = c[3];
2548  M.at<double>(116, 93) = c[9];
2549  M.at<double>(116, 94) = c[13];
2550  M.at<double>(116, 96) = c[2];
2551  M.at<double>(116, 98) = c[11];
2552  M.at<double>(116, 107) = c[20];
2553  M.at<double>(117, 29) = a[20];
2554  M.at<double>(117, 37) = a[3];
2555  M.at<double>(117, 38) = a[2];
2556  M.at<double>(117, 48) = a[15];
2557  M.at<double>(117, 50) = a[18];
2558  M.at<double>(117, 52) = a[13];
2559  M.at<double>(117, 73) = b[13];
2560  M.at<double>(117, 74) = b[20];
2561  M.at<double>(117, 77) = b[3];
2562  M.at<double>(117, 78) = b[2];
2563  M.at<double>(117, 80) = b[18];
2564  M.at<double>(117, 81) = b[15];
2565  M.at<double>(117, 85) = c[15];
2566  M.at<double>(117, 86) = c[18];
2567  M.at<double>(117, 92) = c[2];
2568  M.at<double>(117, 93) = c[13];
2569  M.at<double>(117, 94) = c[20];
2570  M.at<double>(117, 98) = c[3];
2571  M.at<double>(118, 27) = a[13];
2572  M.at<double>(118, 28) = a[20];
2573  M.at<double>(118, 31) = a[4];
2574  M.at<double>(118, 32) = a[19];
2575  M.at<double>(118, 48) = a[14];
2576  M.at<double>(118, 50) = a[8];
2577  M.at<double>(118, 55) = b[13];
2578  M.at<double>(118, 57) = b[20];
2579  M.at<double>(118, 58) = b[4];
2580  M.at<double>(118, 61) = b[19];
2581  M.at<double>(118, 80) = b[8];
2582  M.at<double>(118, 81) = b[14];
2583  M.at<double>(118, 85) = c[14];
2584  M.at<double>(118, 86) = c[8];
2585  M.at<double>(118, 113) = c[13];
2586  M.at<double>(118, 114) = c[20];
2587  M.at<double>(118, 115) = c[4];
2588  M.at<double>(118, 116) = c[19];
2589  M.at<double>(119, 27) = a[20];
2590  M.at<double>(119, 31) = a[19];
2591  M.at<double>(119, 48) = a[8];
2592  M.at<double>(119, 55) = b[20];
2593  M.at<double>(119, 58) = b[19];
2594  M.at<double>(119, 81) = b[8];
2595  M.at<double>(119, 85) = c[8];
2596  M.at<double>(119, 113) = c[20];
2597  M.at<double>(119, 115) = c[19];
2598 
2599  return M.t();
2600 }
2601 
2602 cv::Mat mrpt::vision::pnp::dls::Hessian(const double s[])
2603 {
2604  // the vector of monomials is
2605  // m = [ const ; s1^2 * s2 ; s1 * s2 ; s1 * s3 ; s2 * s3 ; s2^2 * s3 ; s2^3
2606  // ; ...
2607  // s1 * s3^2 ; s1 ; s3 ; s2 ; s2 * s3^2 ; s1^2 ; s3^2 ; s2^2 ; s3^3 ;
2608  // ...
2609  // s1 * s2 * s3 ; s1 * s2^2 ; s1^2 * s3 ; s1^3]
2610  //
2611 
2612  // deriv of m w.r.t. s1
2613  // Hs3 = [0 ; 2 * s(1) * s(2) ; s(2) ; s(3) ; 0 ; 0 ; 0 ; ...
2614  // s(3)^2 ; 1 ; 0 ; 0 ; 0 ; 2 * s(1) ; 0 ; 0 ; 0 ; ...
2615  // s(2) * s(3) ; s(2)^2 ; 2*s(1)*s(3); 3 * s(1)^2];
2616 
2617  double Hs1[20];
2618  Hs1[0] = 0;
2619  Hs1[1] = 2 * s[0] * s[1];
2620  Hs1[2] = s[1];
2621  Hs1[3] = s[2];
2622  Hs1[4] = 0;
2623  Hs1[5] = 0;
2624  Hs1[6] = 0;
2625  Hs1[7] = s[2] * s[2];
2626  Hs1[8] = 1;
2627  Hs1[9] = 0;
2628  Hs1[10] = 0;
2629  Hs1[11] = 0;
2630  Hs1[12] = 2 * s[0];
2631  Hs1[13] = 0;
2632  Hs1[14] = 0;
2633  Hs1[15] = 0;
2634  Hs1[16] = s[1] * s[2];
2635  Hs1[17] = s[1] * s[1];
2636  Hs1[18] = 2 * s[0] * s[2];
2637  Hs1[19] = 3 * s[0] * s[0];
2638 
2639  // deriv of m w.r.t. s2
2640  // Hs2 = [0 ; s(1)^2 ; s(1) ; 0 ; s(3) ; 2 * s(2) * s(3) ; 3 * s(2)^2 ; ...
2641  // 0 ; 0 ; 0 ; 1 ; s(3)^2 ; 0 ; 0 ; 2 * s(2) ; 0 ; ...
2642  // s(1) * s(3) ; s(1) * 2 * s(2) ; 0 ; 0];
2643 
2644  double Hs2[20];
2645  Hs2[0] = 0;
2646  Hs2[1] = s[0] * s[0];
2647  Hs2[2] = s[0];
2648  Hs2[3] = 0;
2649  Hs2[4] = s[2];
2650  Hs2[5] = 2 * s[1] * s[2];
2651  Hs2[6] = 3 * s[1] * s[1];
2652  Hs2[7] = 0;
2653  Hs2[8] = 0;
2654  Hs2[9] = 0;
2655  Hs2[10] = 1;
2656  Hs2[11] = s[2] * s[2];
2657  Hs2[12] = 0;
2658  Hs2[13] = 0;
2659  Hs2[14] = 2 * s[1];
2660  Hs2[15] = 0;
2661  Hs2[16] = s[0] * s[2];
2662  Hs2[17] = 2 * s[0] * s[1];
2663  Hs2[18] = 0;
2664  Hs2[19] = 0;
2665 
2666  // deriv of m w.r.t. s3
2667  // Hs3 = [0 ; 0 ; 0 ; s(1) ; s(2) ; s(2)^2 ; 0 ; ...
2668  // s(1) * 2 * s(3) ; 0 ; 1 ; 0 ; s(2) * 2 * s(3) ; 0 ; 2 * s(3) ; 0
2669  // ; 3 * s(3)^2 ; ...
2670  // s(1) * s(2) ; 0 ; s(1)^2 ; 0];
2671 
2672  double Hs3[20];
2673  Hs3[0] = 0;
2674  Hs3[1] = 0;
2675  Hs3[2] = 0;
2676  Hs3[3] = s[0];
2677  Hs3[4] = s[1];
2678  Hs3[5] = s[1] * s[1];
2679  Hs3[6] = 0;
2680  Hs3[7] = 2 * s[0] * s[2];
2681  Hs3[8] = 0;
2682  Hs3[9] = 1;
2683  Hs3[10] = 0;
2684  Hs3[11] = 2 * s[1] * s[2];
2685  Hs3[12] = 0;
2686  Hs3[13] = 2 * s[2];
2687  Hs3[14] = 0;
2688  Hs3[15] = 3 * s[2] * s[2];
2689  Hs3[16] = s[0] * s[1];
2690  Hs3[17] = 0;
2691  Hs3[18] = s[0] * s[0];
2692  Hs3[19] = 0;
2693 
2694  // fill Hessian matrix
2695  cv::Mat H(3, 3, CV_64F);
2696  H.at<double>(0, 0) =
2697  cv::Mat(
2698  cv::Mat(f1coeff).rowRange(1, 21).t() * cv::Mat(20, 1, CV_64F, &Hs1))
2699  .at<double>(0, 0);
2700  H.at<double>(0, 1) =
2701  cv::Mat(
2702  cv::Mat(f1coeff).rowRange(1, 21).t() * cv::Mat(20, 1, CV_64F, &Hs2))
2703  .at<double>(0, 0);
2704  H.at<double>(0, 2) =
2705  cv::Mat(
2706  cv::Mat(f1coeff).rowRange(1, 21).t() * cv::Mat(20, 1, CV_64F, &Hs3))
2707  .at<double>(0, 0);
2708 
2709  H.at<double>(1, 0) =
2710  cv::Mat(
2711  cv::Mat(f2coeff).rowRange(1, 21).t() * cv::Mat(20, 1, CV_64F, &Hs1))
2712  .at<double>(0, 0);
2713  H.at<double>(1, 1) =
2714  cv::Mat(
2715  cv::Mat(f2coeff).rowRange(1, 21).t() * cv::Mat(20, 1, CV_64F, &Hs2))
2716  .at<double>(0, 0);
2717  H.at<double>(1, 2) =
2718  cv::Mat(
2719  cv::Mat(f2coeff).rowRange(1, 21).t() * cv::Mat(20, 1, CV_64F, &Hs3))
2720  .at<double>(0, 0);
2721 
2722  H.at<double>(2, 0) =
2723  cv::Mat(
2724  cv::Mat(f3coeff).rowRange(1, 21).t() * cv::Mat(20, 1, CV_64F, &Hs1))
2725  .at<double>(0, 0);
2726  H.at<double>(2, 1) =
2727  cv::Mat(
2728  cv::Mat(f3coeff).rowRange(1, 21).t() * cv::Mat(20, 1, CV_64F, &Hs2))
2729  .at<double>(0, 0);
2730  H.at<double>(2, 2) =
2731  cv::Mat(
2732  cv::Mat(f3coeff).rowRange(1, 21).t() * cv::Mat(20, 1, CV_64F, &Hs3))
2733  .at<double>(0, 0);
2734 
2735  return H;
2736 }
2737 
2738 cv::Mat mrpt::vision::pnp::dls::cayley2rotbar(const cv::Mat& s)
2739 {
2740  double s_mul1 = cv::Mat(s.t() * s).at<double>(0, 0);
2741  cv::Mat s_mul2 = s * s.t();
2742  cv::Mat eye = cv::Mat::eye(3, 3, CV_64F);
2743 
2744  return cv::Mat(eye.mul(1. - s_mul1) + skewsymm(&s).mul(2.) + s_mul2.mul(2.))
2745  .t();
2746 }
2747 
2748 cv::Mat mrpt::vision::pnp::dls::skewsymm(const cv::Mat* X1)
2749 {
2750  cv::MatConstIterator_<double> it = X1->begin<double>();
2751  return (
2752  cv::Mat_<double>(3, 3) << 0, -*(it + 2), *(it + 1), *(it + 2), 0,
2753  -*(it + 0), -*(it + 1), *(it + 0), 0);
2754 }
2755 
2756 cv::Mat mrpt::vision::pnp::dls::rotx(const double t)
2757 {
2758  // rotx: rotation about y-axis
2759  double ct = cos(t);
2760  double st = sin(t);
2761  return (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, ct, -st, 0, st, ct);
2762 }
2763 
2764 cv::Mat mrpt::vision::pnp::dls::roty(const double t)
2765 {
2766  // roty: rotation about y-axis
2767  double ct = cos(t);
2768  double st = sin(t);
2769  return (cv::Mat_<double>(3, 3) << ct, 0, st, 0, 1, 0, -st, 0, ct);
2770 }
2771 
2772 cv::Mat mrpt::vision::pnp::dls::rotz(const double t)
2773 {
2774  // rotz: rotation about y-axis
2775  double ct = cos(t);
2776  double st = sin(t);
2777  return (cv::Mat_<double>(3, 3) << ct, -st, 0, st, ct, 0, 0, 0, 1);
2778 }
2779 
2780 cv::Mat mrpt::vision::pnp::dls::mean(const cv::Mat& M)
2781 {
2782  cv::Mat m = cv::Mat::zeros(3, 1, CV_64F);
2783  for (int i = 0; i < M.cols; ++i) m += M.col(i);
2784  return m.mul(1. / (double)M.cols);
2785 }
2786 
2787 bool mrpt::vision::pnp::dls::is_empty(const cv::Mat* M)
2788 {
2789  cv::MatConstIterator_<double> it = M->begin<double>(),
2790  it_end = M->end<double>();
2791  for (; it != it_end; ++it)
2792  {
2793  if (*it < 0) return false;
2794  }
2795  return true;
2796 }
2797 
2798 bool mrpt::vision::pnp::dls::positive_eigenvalues(const cv::Mat* eigenvalues)
2799 {
2800  cv::MatConstIterator_<double> it = eigenvalues->begin<double>();
2801  return *(it) > 0 && *(it + 1) > 0 && *(it + 2) > 0;
2802 }
2803 #endif
count
GLuint GLuint GLsizei count
Definition: glext.h:3528
mrpt::vision::pnp::dls::is_empty
bool is_empty(const cv::Mat *v)
Check for negative values in vector v.
t
GLdouble GLdouble t
Definition: glext.h:3689
z
GLdouble GLdouble z
Definition: glext.h:3872
mrpt::vision::pnp::dls::compute_pose
bool compute_pose(cv::Mat &R, cv::Mat &t)
OpenCV function for computing pose.
mrpt::vision::pnp::dls::build_coeff_matrix
void build_coeff_matrix(const cv::Mat &pp, cv::Mat &Mtilde, cv::Mat &D)
Build the Maucaulay matrix co-efficients.
CV_PI
#define CV_PI
Definition: polynom_solver.cpp:16
mrpt::vision::pnp::dls::fill_coeff
void fill_coeff(const cv::Mat *D)
Fill the hessian functions.
mrpt::vision::pnp::dls::f1coeff
std::vector< double > f1coeff
number of input points
Definition: dls.h:198
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: CColouredPointsMap.h:112
mrpt::vision::pnp::dls::roty
cv::Mat roty(const double t)
Rotation matrix along y-axis by angle t.
mrpt::vision::pnp::dls::f2coeff
std::vector< double > f2coeff
Definition: dls.h:198
mrpt::vision::pnp::dls::N
int N
object-image points
Definition: dls.h:197
s
GLdouble s
Definition: glext.h:3676
mrpt::vision::pnp::dls::compute_eigenvec
void compute_eigenvec(const cv::Mat &Mtilde, cv::Mat &eigenval_real, cv::Mat &eigenval_imag, cv::Mat &eigenvec_real, cv::Mat &eigenvec_imag)
Eigen Value Decomposition.
mrpt::vision::pnp::dls::rotz
cv::Mat rotz(const double t)
Rotation matrix along z-axis by angle t.
c
const GLubyte * c
Definition: glext.h:6313
b
GLubyte GLubyte b
Definition: glext.h:6279
mrpt::vision::pnp::dls::f3coeff
std::vector< double > f3coeff
Definition: dls.h:198
mrpt::vision::pnp::dls::cayley_LS_M
cv::Mat cayley_LS_M(const std::vector< double > &a, const std::vector< double > &b, const std::vector< double > &c, const std::vector< double > &u)
Fill the Maucaulay matrix with co-efficients.
mrpt::vision::pnp::dls::Hessian
cv::Mat Hessian(const double s[])
Compute the Hessian matrix for the polynomial co-efficient vector s.
mrpt::vision::pnp::dls::cost__
double cost__
optimal found solution
Definition: dls.h:202
mrpt::vision::pnp::dls::rotx
cv::Mat rotx(const double t)
Rotation matrix along x-axis by angle t.
mrpt::vision::pnp::dls::LeftMultVec
cv::Mat LeftMultVec(const cv::Mat &v)
Create a matrix from vector.
v
const GLdouble * v
Definition: glext.h:3678
R
const float R
Definition: CKinematicChain.cpp:149
mrpt::vision::pnp::dls::mean
cv::Mat mean(const cv::Mat &M)
Column-wise mean of matrix M.
mrpt::vision::pnp::dls::cayley2rotbar
cv::Mat cayley2rotbar(const cv::Mat &s)
Cayley parameters to Rotation Matrix.
mrpt::vision::pnp::dls::run_kernel
void run_kernel(const cv::Mat &pp)
Main function to run DLS-PnP.
zeros
EIGEN_STRONG_INLINE void zeros()
Set all elements to zero.
Definition: eigen_plugins.h:78
dls.h
Direct Least Squares (DLS) - Eigen wrapper for OpenCV Implementation.
eye
EIGEN_STRONG_INLINE void eye()
Make the matrix an identity matrix.
Definition: eigen_plugins.h:76
a
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
p
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::vision::pnp::dls::dls
dls(const cv::Mat &opoints, const cv::Mat &ipoints)
Constructor for DLS class.
mrpt::vision::pnp::dls::positive_eigenvalues
bool positive_eigenvalues(const cv::Mat *eigenvalues)
check for positive eigenvalues
mrpt::vision::pnp::dls::skewsymm
cv::Mat skewsymm(const cv::Mat *X1)
Create a skwy-symmetric matrix from a vector.


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019