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


Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020