MRPT  1.9.9
upnp.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 // M*//////////////////////////////////////////////////////////////////////////////////////
13 //
14 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
15 //
16 // By downloading, copying, installing or using the software you agree to this
17 // license.
18 // If you do not agree to this license, do not download, install,
19 // copy or use the software.
20 //
21 //
22 // License Agreement
23 // For Open Source Computer Vision Library
24 //
25 // Copyright (C) 2000, Intel Corporation, all rights reserved.
26 // Copyright (C) 2013, OpenCV Foundation, all rights reserved.
27 // Third party copyrights are property of their respective owners.
28 //
29 // Redistribution and use in source and binary forms, with or without
30 // modification,
31 // are permitted provided that the following conditions are met:
32 //
33 // * Redistribution's of source code must retain the above copyright notice,
34 // this list of conditions and the following disclaimer.
35 //
36 // * Redistribution's in binary form must reproduce the above copyright notice,
37 // this list of conditions and the following disclaimer in the documentation
38 // and/or other materials provided with the distribution.
39 //
40 // * The name of the copyright holders may not be used to endorse or promote
41 // products
42 // derived from this software without specific prior written permission.
43 //
44 // This software is provided by the copyright holders and contributors "as is"
45 // and
46 // any express or implied warranties, including, but not limited to, the implied
47 // warranties of merchantability and fitness for a particular purpose are
48 // disclaimed.
49 // In no event shall the Intel Corporation or contributors be liable for any
50 // direct,
51 // indirect, incidental, special, exemplary, or consequential damages
52 // (including, but not limited to, procurement of substitute goods or services;
53 // loss of use, data, or profits; or business interruption) however caused
54 // and on any theory of liability, whether in contract, strict liability,
55 // or tort (including negligence or otherwise) arising in any way out of
56 // the use of this software, even if advised of the possibility of such damage.
57 //
58 // M*/
59 
60 /****************************************************************************************\
61 * Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation.
62 * Contributed by Edgar Riba
63 \****************************************************************************************/
64 
65 #include <mrpt/3rdparty/do_opencv_includes.h>
66 #include <iostream>
67 
68 #include <limits>
69 using namespace std;
70 #if MRPT_HAS_OPENCV
71 
72 using namespace cv;
73 
74 #include "upnp.h"
75 using namespace mrpt::vision::pnp;
76 
77 upnp::upnp(const Mat& cameraMatrix, const Mat& opoints, const Mat& ipoints)
78 {
79  if (cameraMatrix.depth() == CV_32F)
80  init_camera_parameters<float>(cameraMatrix);
81  else
82  init_camera_parameters<double>(cameraMatrix);
83 
84  number_of_correspondences = std::max(
85  opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
86 
87  pws.resize(3 * number_of_correspondences);
88  us.resize(2 * number_of_correspondences);
89 
90  if (opoints.depth() == ipoints.depth())
91  {
92  if (opoints.depth() == CV_32F)
93  init_points<Point3f, Point2f>(opoints, ipoints);
94  else
95  init_points<Point3d, Point2d>(opoints, ipoints);
96  }
97  else if (opoints.depth() == CV_32F)
98  init_points<Point3f, Point2d>(opoints, ipoints);
99  else
100  init_points<Point3d, Point2f>(opoints, ipoints);
101 
102  alphas.resize(4 * number_of_correspondences);
103  pcs.resize(3 * number_of_correspondences);
104 
105  max_nr = 0;
106  A1 = nullptr;
107  A2 = nullptr;
108 }
109 
110 upnp::~upnp()
111 {
112  if (A1) delete[] A1;
113  if (A2) delete[] A2;
114 }
115 
116 double upnp::compute_pose(Mat& R, Mat& t)
117 {
118  choose_control_points();
119  compute_alphas();
120 
121  Mat* M = new Mat(2 * number_of_correspondences, 12, CV_64F);
122 
123  for (int i = 0; i < number_of_correspondences; i++)
124  {
125  fill_M(M, 2 * i, &alphas[0] + 4 * i, us[2 * i], us[2 * i + 1]);
126  }
127 
128  double mtm[12 * 12], d[12], ut[12 * 12], vt[12 * 12];
129  Mat MtM = Mat(12, 12, CV_64F, mtm);
130  Mat D = Mat(12, 1, CV_64F, d);
131  Mat Ut = Mat(12, 12, CV_64F, ut);
132  Mat Vt = Mat(12, 12, CV_64F, vt);
133 
134  MtM = M->t() * (*M);
135  SVD::compute(MtM, D, Ut, Vt, SVD::MODIFY_A | SVD::FULL_UV);
136  Mat(Ut.t()).copyTo(Ut);
137  M->release();
138  delete M;
139 
140  double l_6x12[6 * 12], rho[6];
141  Mat L_6x12 = Mat(6, 12, CV_64F, l_6x12);
142  Mat Rho = Mat(6, 1, CV_64F, rho);
143 
144  compute_L_6x12(ut, l_6x12);
145  compute_rho(rho);
146 
147  double Betas[3][4], Efs[3][1], rep_errors[3];
148  double Rs[3][3][3], ts[3][3];
149 
150  find_betas_and_focal_approx_1(&Ut, &Rho, Betas[1], Efs[1]);
151  gauss_newton(&L_6x12, &Rho, Betas[1], Efs[1]);
152  rep_errors[1] = compute_R_and_t(ut, Betas[1], Rs[1], ts[1]);
153 
154  find_betas_and_focal_approx_2(&Ut, &Rho, Betas[2], Efs[2]);
155  gauss_newton(&L_6x12, &Rho, Betas[2], Efs[2]);
156  rep_errors[2] = compute_R_and_t(ut, Betas[2], Rs[2], ts[2]);
157 
158  int N = 1;
159  if (rep_errors[2] < rep_errors[1]) N = 2;
160 
161  Mat(3, 1, CV_64F, ts[N]).copyTo(t);
162  Mat(3, 3, CV_64F, Rs[N]).copyTo(R);
163  fu = fv = Efs[N][0];
164 
165  return fu;
166 }
167 
168 void upnp::copy_R_and_t(
169  const double R_src[3][3], const double t_src[3], double R_dst[3][3],
170  double t_dst[3])
171 {
172  for (int i = 0; i < 3; i++)
173  {
174  for (int j = 0; j < 3; j++) R_dst[i][j] = R_src[i][j];
175  t_dst[i] = t_src[i];
176  }
177 }
178 
179 void upnp::estimate_R_and_t(double R[3][3], double t[3])
180 {
181  double pc0[3], pw0[3];
182 
183  pc0[0] = pc0[1] = pc0[2] = 0.0;
184  pw0[0] = pw0[1] = pw0[2] = 0.0;
185 
186  for (int i = 0; i < number_of_correspondences; i++)
187  {
188  const double* pc = &pcs[3 * i];
189  const double* pw = &pws[3 * i];
190 
191  for (int j = 0; j < 3; j++)
192  {
193  pc0[j] += pc[j];
194  pw0[j] += pw[j];
195  }
196  }
197  for (int j = 0; j < 3; j++)
198  {
199  pc0[j] /= number_of_correspondences;
200  pw0[j] /= number_of_correspondences;
201  }
202 
203  double abt[3 * 3], abt_d[3], abt_u[3 * 3], abt_v[3 * 3];
204  Mat ABt = Mat(3, 3, CV_64F, abt);
205  Mat ABt_D = Mat(3, 1, CV_64F, abt_d);
206  Mat ABt_U = Mat(3, 3, CV_64F, abt_u);
207  Mat ABt_V = Mat(3, 3, CV_64F, abt_v);
208 
209  ABt.setTo(0.0);
210  for (int i = 0; i < number_of_correspondences; i++)
211  {
212  double* pc = &pcs[3 * i];
213  double* pw = &pws[3 * i];
214 
215  for (int j = 0; j < 3; j++)
216  {
217  abt[3 * j] += (pc[j] - pc0[j]) * (pw[0] - pw0[0]);
218  abt[3 * j + 1] += (pc[j] - pc0[j]) * (pw[1] - pw0[1]);
219  abt[3 * j + 2] += (pc[j] - pc0[j]) * (pw[2] - pw0[2]);
220  }
221  }
222 
223  SVD::compute(ABt, ABt_D, ABt_U, ABt_V, SVD::MODIFY_A);
224  Mat(ABt_V.t()).copyTo(ABt_V);
225 
226  for (int i = 0; i < 3; i++)
227  for (int j = 0; j < 3; j++) R[i][j] = dot(abt_u + 3 * i, abt_v + 3 * j);
228 
229  const double det =
230  R[0][0] * R[1][1] * R[2][2] + R[0][1] * R[1][2] * R[2][0] +
231  R[0][2] * R[1][0] * R[2][1] - R[0][2] * R[1][1] * R[2][0] -
232  R[0][1] * R[1][0] * R[2][2] - R[0][0] * R[1][2] * R[2][1];
233 
234  if (det < 0)
235  {
236  R[2][0] = -R[2][0];
237  R[2][1] = -R[2][1];
238  R[2][2] = -R[2][2];
239  }
240 
241  t[0] = pc0[0] - dot(R[0], pw0);
242  t[1] = pc0[1] - dot(R[1], pw0);
243  t[2] = pc0[2] - dot(R[2], pw0);
244 }
245 
246 void upnp::solve_for_sign()
247 {
248  if (pcs[2] < 0.0)
249  {
250  for (int i = 0; i < 4; i++)
251  for (int j = 0; j < 3; j++) ccs[i][j] = -ccs[i][j];
252 
253  for (int i = 0; i < number_of_correspondences; i++)
254  {
255  pcs[3 * i] = -pcs[3 * i];
256  pcs[3 * i + 1] = -pcs[3 * i + 1];
257  pcs[3 * i + 2] = -pcs[3 * i + 2];
258  }
259  }
260 }
261 
262 double upnp::compute_R_and_t(
263  const double* ut, const double* betas, double R[3][3], double t[3])
264 {
265  compute_ccs(betas, ut);
266  compute_pcs();
267 
268  solve_for_sign();
269 
270  estimate_R_and_t(R, t);
271 
272  return reprojection_error(R, t);
273 }
274 
275 double upnp::reprojection_error(const double R[3][3], const double t[3])
276 {
277  double sum2 = 0.0;
278 
279  for (int i = 0; i < number_of_correspondences; i++)
280  {
281  double* pw = &pws[3 * i];
282  double Xc = dot(R[0], pw) + t[0];
283  double Yc = dot(R[1], pw) + t[1];
284  double inv_Zc = 1.0 / (dot(R[2], pw) + t[2]);
285  double ue = uc + fu * Xc * inv_Zc;
286  double ve = vc + fv * Yc * inv_Zc;
287  double u = us[2 * i], v = us[2 * i + 1];
288 
289  sum2 += sqrt((u - ue) * (u - ue) + (v - ve) * (v - ve));
290  }
291 
292  return sum2 / number_of_correspondences;
293 }
294 
295 void upnp::choose_control_points()
296 {
297  for (int i = 0; i < 4; ++i) cws[i][0] = cws[i][1] = cws[i][2] = 0.0;
298  cws[0][0] = cws[1][1] = cws[2][2] = 1.0;
299 }
300 
301 void upnp::compute_alphas()
302 {
303  Mat CC = Mat(4, 3, CV_64F, &cws);
304  Mat PC = Mat(number_of_correspondences, 3, CV_64F, &pws[0]);
305  Mat ALPHAS = Mat(number_of_correspondences, 4, CV_64F, &alphas[0]);
306 
307  Mat CC_ = CC.clone().t();
308  Mat PC_ = PC.clone().t();
309 
310  Mat row14 = Mat::ones(1, 4, CV_64F);
311  Mat row1n = Mat::ones(1, number_of_correspondences, CV_64F);
312 
313  CC_.push_back(row14);
314  PC_.push_back(row1n);
315 
316  ALPHAS = Mat(CC_.inv() * PC_).t();
317 }
318 
319 void upnp::fill_M(
320  Mat* M, const int row, const double* as, const double u, const double v)
321 {
322  auto* M1 = M->ptr<double>(row);
323  double* M2 = M1 + 12;
324 
325  for (int i = 0; i < 4; i++)
326  {
327  M1[3 * i] = as[i] * fu;
328  M1[3 * i + 1] = 0.0;
329  M1[3 * i + 2] = as[i] * (uc - u);
330 
331  M2[3 * i] = 0.0;
332  M2[3 * i + 1] = as[i] * fv;
333  M2[3 * i + 2] = as[i] * (vc - v);
334  }
335 }
336 
337 void upnp::compute_ccs(const double* betas, const double* ut)
338 {
339  for (int i = 0; i < 4; ++i) ccs[i][0] = ccs[i][1] = ccs[i][2] = 0.0;
340 
341  int N = 4;
342  for (int i = 0; i < N; ++i)
343  {
344  const double* v = ut + 12 * (9 + i);
345  for (int j = 0; j < 4; ++j)
346  for (int k = 0; k < 3; ++k) ccs[j][k] += betas[i] * v[3 * j + k];
347  }
348 
349  for (int i = 0; i < 4; ++i) ccs[i][2] *= fu;
350 }
351 
352 void upnp::compute_pcs()
353 {
354  for (int i = 0; i < number_of_correspondences; i++)
355  {
356  double* a = &alphas[0] + 4 * i;
357  double* pc = &pcs[0] + 3 * i;
358 
359  for (int j = 0; j < 3; j++)
360  pc[j] = a[0] * ccs[0][j] + a[1] * ccs[1][j] + a[2] * ccs[2][j] +
361  a[3] * ccs[3][j];
362  }
363 }
364 
365 void upnp::find_betas_and_focal_approx_1(
366  Mat* Ut, Mat* Rho, double* betas, double* efs)
367 {
368  Mat Kmf1 = Mat(12, 1, CV_64F, Ut->ptr<double>(11));
369  Mat dsq = Mat(6, 1, CV_64F, Rho->ptr<double>(0));
370 
371  Mat D = compute_constraint_distance_2param_6eq_2unk_f_unk(Kmf1);
372  Mat Dt = D.t();
373 
374  Mat A = Dt * D;
375  Mat b = Dt * dsq;
376 
377  Mat x = Mat(2, 1, CV_64F);
378  solve(A, b, x);
379 
380  betas[0] = sqrt(std::abs(x.at<double>(0)));
381  betas[1] = betas[2] = betas[3] = 0.0;
382 
383  efs[0] = sqrt(std::abs(x.at<double>(1))) / betas[0];
384 }
385 
386 void upnp::find_betas_and_focal_approx_2(
387  Mat* Ut, Mat* Rho, double* betas, double* efs)
388 {
389  double u[12 * 12];
390  Mat U = Mat(12, 12, CV_64F, u);
391  Ut->copyTo(U);
392 
393  Mat Kmf1 = Mat(12, 1, CV_64F, Ut->ptr<double>(10));
394  Mat Kmf2 = Mat(12, 1, CV_64F, Ut->ptr<double>(11));
395  Mat dsq = Mat(6, 1, CV_64F, Rho->ptr<double>(0));
396 
397  Mat D = compute_constraint_distance_3param_6eq_6unk_f_unk(Kmf1, Kmf2);
398 
399  Mat A = D;
400  Mat b = dsq;
401 
402  double x[6];
403  Mat X = Mat(6, 1, CV_64F, x);
404 
405  solve(A, b, X, DECOMP_QR);
406 
407  double solutions[18][3];
408  generate_all_possible_solutions_for_f_unk(x, solutions);
409 
410  // find solution with minimum reprojection error
411  double min_error = std::numeric_limits<double>::max();
412  int min_sol = 0;
413  for (int i = 0; i < 18; ++i)
414  {
415  betas[3] = solutions[i][0];
416  betas[2] = solutions[i][1];
417  betas[1] = betas[0] = 0.0;
418  fu = fv = solutions[i][2];
419 
420  double Rs[3][3], ts[3];
421  double error_i = compute_R_and_t(u, betas, Rs, ts);
422 
423  if (error_i < min_error)
424  {
425  min_error = error_i;
426  min_sol = i;
427  }
428  }
429 
430  betas[0] = solutions[min_sol][0];
431  betas[1] = solutions[min_sol][1];
432  betas[2] = betas[3] = 0.0;
433 
434  efs[0] = solutions[min_sol][2];
435 }
436 
437 Mat upnp::compute_constraint_distance_2param_6eq_2unk_f_unk(const Mat& M1)
438 {
439  Mat P = Mat(6, 2, CV_64F);
440 
441  double m[13];
442  for (int i = 1; i < 13; ++i) m[i] = *M1.ptr<double>(i - 1);
443 
444  double t1 = pow(m[4], 2);
445  double t4 = pow(m[1], 2);
446  double t5 = pow(m[5], 2);
447  double t8 = pow(m[2], 2);
448  double t10 = pow(m[6], 2);
449  double t13 = pow(m[3], 2);
450  double t15 = pow(m[7], 2);
451  double t18 = pow(m[8], 2);
452  double t22 = pow(m[9], 2);
453  double t26 = pow(m[10], 2);
454  double t29 = pow(m[11], 2);
455  double t33 = pow(m[12], 2);
456 
457  *P.ptr<double>(0, 0) =
458  t1 - 2 * m[4] * m[1] + t4 + t5 - 2 * m[5] * m[2] + t8;
459  *P.ptr<double>(0, 1) = t10 - 2 * m[6] * m[3] + t13;
460  *P.ptr<double>(1, 0) =
461  t15 - 2 * m[7] * m[1] + t4 + t18 - 2 * m[8] * m[2] + t8;
462  *P.ptr<double>(1, 1) = t22 - 2 * m[9] * m[3] + t13;
463  *P.ptr<double>(2, 0) =
464  t26 - 2 * m[10] * m[1] + t4 + t29 - 2 * m[11] * m[2] + t8;
465  *P.ptr<double>(2, 1) = t33 - 2 * m[12] * m[3] + t13;
466  *P.ptr<double>(3, 0) =
467  t15 - 2 * m[7] * m[4] + t1 + t18 - 2 * m[8] * m[5] + t5;
468  *P.ptr<double>(3, 1) = t22 - 2 * m[9] * m[6] + t10;
469  *P.ptr<double>(4, 0) =
470  t26 - 2 * m[10] * m[4] + t1 + t29 - 2 * m[11] * m[5] + t5;
471  *P.ptr<double>(4, 1) = t33 - 2 * m[12] * m[6] + t10;
472  *P.ptr<double>(5, 0) =
473  t26 - 2 * m[10] * m[7] + t15 + t29 - 2 * m[11] * m[8] + t18;
474  *P.ptr<double>(5, 1) = t33 - 2 * m[12] * m[9] + t22;
475 
476  return P;
477 }
478 
479 Mat upnp::compute_constraint_distance_3param_6eq_6unk_f_unk(
480  const Mat& M1, const Mat& M2)
481 {
482  Mat P = Mat(6, 6, CV_64F);
483 
484  double m[3][13];
485  for (int i = 1; i < 13; ++i)
486  {
487  m[1][i] = *M1.ptr<double>(i - 1);
488  m[2][i] = *M2.ptr<double>(i - 1);
489  }
490 
491  double t1 = pow(m[1][4], 2);
492  double t2 = pow(m[1][1], 2);
493  double t7 = pow(m[1][5], 2);
494  double t8 = pow(m[1][2], 2);
495  double t11 = m[1][1] * m[2][1];
496  double t12 = m[1][5] * m[2][5];
497  double t15 = m[1][2] * m[2][2];
498  double t16 = m[1][4] * m[2][4];
499  double t19 = pow(m[2][4], 2);
500  double t22 = pow(m[2][2], 2);
501  double t23 = pow(m[2][1], 2);
502  double t24 = pow(m[2][5], 2);
503  double t28 = pow(m[1][6], 2);
504  double t29 = pow(m[1][3], 2);
505  double t34 = pow(m[1][3], 2);
506  double t36 = m[1][6] * m[2][6];
507  double t40 = pow(m[2][6], 2);
508  double t41 = pow(m[2][3], 2);
509  double t47 = pow(m[1][7], 2);
510  double t48 = pow(m[1][8], 2);
511  double t52 = m[1][7] * m[2][7];
512  double t55 = m[1][8] * m[2][8];
513  double t59 = pow(m[2][8], 2);
514  double t62 = pow(m[2][7], 2);
515  double t64 = pow(m[1][9], 2);
516  double t68 = m[1][9] * m[2][9];
517  double t74 = pow(m[2][9], 2);
518  double t78 = pow(m[1][10], 2);
519  double t79 = pow(m[1][11], 2);
520  double t84 = m[1][10] * m[2][10];
521  double t87 = m[1][11] * m[2][11];
522  double t90 = pow(m[2][10], 2);
523  double t95 = pow(m[2][11], 2);
524  double t99 = pow(m[1][12], 2);
525  double t101 = m[1][12] * m[2][12];
526  double t105 = pow(m[2][12], 2);
527 
528  *P.ptr<double>(0, 0) =
529  t1 + t2 - 2 * m[1][4] * m[1][1] - 2 * m[1][5] * m[1][2] + t7 + t8;
530  *P.ptr<double>(0, 1) = -2 * m[2][4] * m[1][1] + 2 * t11 + 2 * t12 -
531  2 * m[1][4] * m[2][1] - 2 * m[2][5] * m[1][2] +
532  2 * t15 + 2 * t16 - 2 * m[1][5] * m[2][2];
533  *P.ptr<double>(0, 2) =
534  t19 - 2 * m[2][4] * m[2][1] + t22 + t23 + t24 - 2 * m[2][5] * m[2][2];
535  *P.ptr<double>(0, 3) = t28 + t29 - 2 * m[1][6] * m[1][3];
536  *P.ptr<double>(0, 4) =
537  -2 * m[2][6] * m[1][3] + 2 * t34 - 2 * m[1][6] * m[2][3] + 2 * t36;
538  *P.ptr<double>(0, 5) = -2 * m[2][6] * m[2][3] + t40 + t41;
539 
540  *P.ptr<double>(1, 0) =
541  t8 - 2 * m[1][8] * m[1][2] - 2 * m[1][7] * m[1][1] + t47 + t48 + t2;
542  *P.ptr<double>(1, 1) =
543  2 * t15 - 2 * m[1][8] * m[2][2] - 2 * m[2][8] * m[1][2] + 2 * t52 -
544  2 * m[1][7] * m[2][1] - 2 * m[2][7] * m[1][1] + 2 * t55 + 2 * t11;
545  *P.ptr<double>(1, 2) =
546  -2 * m[2][8] * m[2][2] + t22 + t23 + t59 - 2 * m[2][7] * m[2][1] + t62;
547  *P.ptr<double>(1, 3) = t29 + t64 - 2 * m[1][9] * m[1][3];
548  *P.ptr<double>(1, 4) =
549  2 * t34 + 2 * t68 - 2 * m[2][9] * m[1][3] - 2 * m[1][9] * m[2][3];
550  *P.ptr<double>(1, 5) = -2 * m[2][9] * m[2][3] + t74 + t41;
551 
552  *P.ptr<double>(2, 0) =
553  -2 * m[1][11] * m[1][2] + t2 + t8 + t78 + t79 - 2 * m[1][10] * m[1][1];
554  *P.ptr<double>(2, 1) = 2 * t15 - 2 * m[1][11] * m[2][2] + 2 * t84 -
555  2 * m[1][10] * m[2][1] - 2 * m[2][10] * m[1][1] +
556  2 * t87 - 2 * m[2][11] * m[1][2] + 2 * t11;
557  *P.ptr<double>(2, 2) =
558  t90 + t22 - 2 * m[2][10] * m[2][1] + t23 - 2 * m[2][11] * m[2][2] + t95;
559  *P.ptr<double>(2, 3) = -2 * m[1][12] * m[1][3] + t99 + t29;
560  *P.ptr<double>(2, 4) =
561  2 * t34 + 2 * t101 - 2 * m[2][12] * m[1][3] - 2 * m[1][12] * m[2][3];
562  *P.ptr<double>(2, 5) = t41 + t105 - 2 * m[2][12] * m[2][3];
563 
564  *P.ptr<double>(3, 0) =
565  t48 + t1 - 2 * m[1][8] * m[1][5] + t7 - 2 * m[1][7] * m[1][4] + t47;
566  *P.ptr<double>(3, 1) = 2 * t16 - 2 * m[1][7] * m[2][4] + 2 * t55 + 2 * t52 -
567  2 * m[1][8] * m[2][5] - 2 * m[2][8] * m[1][5] -
568  2 * m[2][7] * m[1][4] + 2 * t12;
569  *P.ptr<double>(3, 2) =
570  t24 - 2 * m[2][8] * m[2][5] + t19 - 2 * m[2][7] * m[2][4] + t62 + t59;
571  *P.ptr<double>(3, 3) = -2 * m[1][9] * m[1][6] + t64 + t28;
572  *P.ptr<double>(3, 4) =
573  2 * t68 + 2 * t36 - 2 * m[2][9] * m[1][6] - 2 * m[1][9] * m[2][6];
574  *P.ptr<double>(3, 5) = t40 + t74 - 2 * m[2][9] * m[2][6];
575 
576  *P.ptr<double>(4, 0) =
577  t1 - 2 * m[1][10] * m[1][4] + t7 + t78 + t79 - 2 * m[1][11] * m[1][5];
578  *P.ptr<double>(4, 1) =
579  2 * t84 - 2 * m[1][11] * m[2][5] - 2 * m[1][10] * m[2][4] + 2 * t16 -
580  2 * m[2][11] * m[1][5] + 2 * t87 - 2 * m[2][10] * m[1][4] + 2 * t12;
581  *P.ptr<double>(4, 2) =
582  t19 + t24 - 2 * m[2][10] * m[2][4] - 2 * m[2][11] * m[2][5] + t95 + t90;
583  *P.ptr<double>(4, 3) = t28 - 2 * m[1][12] * m[1][6] + t99;
584  *P.ptr<double>(4, 4) =
585  2 * t101 + 2 * t36 - 2 * m[2][12] * m[1][6] - 2 * m[1][12] * m[2][6];
586  *P.ptr<double>(4, 5) = t105 - 2 * m[2][12] * m[2][6] + t40;
587 
588  *P.ptr<double>(5, 0) = -2 * m[1][10] * m[1][7] + t47 + t48 + t78 + t79 -
589  2 * m[1][11] * m[1][8];
590  *P.ptr<double>(5, 1) = 2 * t84 + 2 * t87 - 2 * m[2][11] * m[1][8] -
591  2 * m[1][10] * m[2][7] - 2 * m[2][10] * m[1][7] +
592  2 * t55 + 2 * t52 - 2 * m[1][11] * m[2][8];
593  *P.ptr<double>(5, 2) = -2 * m[2][10] * m[2][7] - 2 * m[2][11] * m[2][8] +
594  t62 + t59 + t90 + t95;
595  *P.ptr<double>(5, 3) = t64 - 2 * m[1][12] * m[1][9] + t99;
596  *P.ptr<double>(5, 4) =
597  2 * t68 - 2 * m[2][12] * m[1][9] - 2 * m[1][12] * m[2][9] + 2 * t101;
598  *P.ptr<double>(5, 5) = t105 - 2 * m[2][12] * m[2][9] + t74;
599 
600  return P;
601 }
602 
603 void upnp::generate_all_possible_solutions_for_f_unk(
604  const double betas[5], double solutions[18][3])
605 {
606  int matrix_to_resolve[18][9] = {
607  {2, 0, 0, 1, 1, 0, 2, 0, 2}, {2, 0, 0, 1, 1, 0, 1, 1, 2},
608  {2, 0, 0, 1, 1, 0, 0, 2, 2}, {2, 0, 0, 0, 2, 0, 2, 0, 2},
609  {2, 0, 0, 0, 2, 0, 1, 1, 2}, {2, 0, 0, 0, 2, 0, 0, 2, 2},
610  {2, 0, 0, 2, 0, 2, 1, 1, 2}, {2, 0, 0, 2, 0, 2, 0, 2, 2},
611  {2, 0, 0, 1, 1, 2, 0, 2, 2}, {1, 1, 0, 0, 2, 0, 2, 0, 2},
612  {1, 1, 0, 0, 2, 0, 1, 1, 2}, {1, 1, 0, 2, 0, 2, 0, 2, 2},
613  {1, 1, 0, 2, 0, 2, 1, 1, 2}, {1, 1, 0, 2, 0, 2, 0, 2, 2},
614  {1, 1, 0, 1, 1, 2, 0, 2, 2}, {0, 2, 0, 2, 0, 2, 1, 1, 2},
615  {0, 2, 0, 2, 0, 2, 0, 2, 2}, {0, 2, 0, 1, 1, 2, 0, 2, 2}};
616 
617  int combination[18][3] = {
618  {1, 2, 4}, {1, 2, 5}, {1, 2, 6}, {1, 3, 4}, {1, 3, 5}, {1, 3, 6},
619  {1, 4, 5}, {1, 4, 6}, {1, 5, 6}, {2, 3, 4}, {2, 3, 5}, {2, 3, 6},
620  {2, 4, 5}, {2, 4, 6}, {2, 5, 6}, {3, 4, 5}, {3, 4, 6}, {3, 5, 6}};
621 
622  for (int i = 0; i < 18; ++i)
623  {
624  double matrix[9], independent_term[3];
625  Mat M = Mat(3, 3, CV_64F, matrix);
626  Mat I = Mat(3, 1, CV_64F, independent_term);
627  Mat S = Mat(1, 3, CV_64F);
628 
629  for (int j = 0; j < 9; ++j) matrix[j] = (double)matrix_to_resolve[i][j];
630 
631  independent_term[0] = log(std::abs(betas[combination[i][0] - 1]));
632  independent_term[1] = log(std::abs(betas[combination[i][1] - 1]));
633  independent_term[2] = log(std::abs(betas[combination[i][2] - 1]));
634 
635  exp(Mat(M.inv() * I), S);
636 
637  solutions[i][0] = S.at<double>(0);
638  solutions[i][1] = S.at<double>(1) * sign(betas[1]);
639  solutions[i][2] = std::abs(S.at<double>(2));
640  }
641 }
642 
643 void upnp::gauss_newton(
644  const Mat* L_6x12, const Mat* Rho, double betas[4], double* f)
645 {
646  const int iterations_number = 50;
647 
648  double a[6 * 4], b[6], x[4];
649  Mat* A = new Mat(6, 4, CV_64F, a);
650  Mat* B = new Mat(6, 1, CV_64F, b);
651  Mat* X = new Mat(4, 1, CV_64F, x);
652 
653  for (int k = 0; k < iterations_number; k++)
654  {
655  compute_A_and_b_gauss_newton(
656  L_6x12->ptr<double>(0), Rho->ptr<double>(0), betas, A, B, f[0]);
657  qr_solve(A, B, X);
658  for (int i = 0; i < 3; i++) betas[i] += x[i];
659  f[0] += x[3];
660  }
661 
662  if (f[0] < 0) f[0] = -f[0];
663  fu = fv = f[0];
664 
665  A->release();
666  delete A;
667 
668  B->release();
669  delete B;
670 
671  X->release();
672  delete X;
673 }
674 
675 void upnp::compute_A_and_b_gauss_newton(
676  const double* l_6x12, const double* rho, const double betas[4], Mat* A,
677  Mat* b, double const f)
678 {
679  for (int i = 0; i < 6; i++)
680  {
681  const double* rowL = l_6x12 + i * 12;
682  auto* rowA = A->ptr<double>(i);
683 
684  rowA[0] = 2 * rowL[0] * betas[0] + rowL[1] * betas[1] +
685  rowL[2] * betas[2] +
686  f * f *
687  (2 * rowL[6] * betas[0] + rowL[7] * betas[1] +
688  rowL[8] * betas[2]);
689  rowA[1] = rowL[1] * betas[0] + 2 * rowL[3] * betas[1] +
690  rowL[4] * betas[2] +
691  f * f *
692  (rowL[7] * betas[0] + 2 * rowL[9] * betas[1] +
693  rowL[10] * betas[2]);
694  rowA[2] = rowL[2] * betas[0] + rowL[4] * betas[1] +
695  2 * rowL[5] * betas[2] +
696  f * f *
697  (rowL[8] * betas[0] + rowL[10] * betas[1] +
698  2 * rowL[11] * betas[2]);
699  rowA[3] =
700  2 * f *
701  (rowL[6] * betas[0] * betas[0] + rowL[7] * betas[0] * betas[1] +
702  rowL[8] * betas[0] * betas[2] + rowL[9] * betas[1] * betas[1] +
703  rowL[10] * betas[1] * betas[2] + rowL[11] * betas[2] * betas[2]);
704 
705  *b->ptr<double>(i) =
706  rho[i] -
707  (rowL[0] * betas[0] * betas[0] + rowL[1] * betas[0] * betas[1] +
708  rowL[2] * betas[0] * betas[2] + rowL[3] * betas[1] * betas[1] +
709  rowL[4] * betas[1] * betas[2] + rowL[5] * betas[2] * betas[2] +
710  f * f * rowL[6] * betas[0] * betas[0] +
711  f * f * rowL[7] * betas[0] * betas[1] +
712  f * f * rowL[8] * betas[0] * betas[2] +
713  f * f * rowL[9] * betas[1] * betas[1] +
714  f * f * rowL[10] * betas[1] * betas[2] +
715  f * f * rowL[11] * betas[2] * betas[2]);
716  }
717 }
718 
719 void upnp::compute_L_6x12(const double* ut, double* l_6x12)
720 {
721  const double* v[3];
722 
723  v[0] = ut + 12 * 9;
724  v[1] = ut + 12 * 10;
725  v[2] = ut + 12 * 11;
726 
727  double dv[3][6][3];
728 
729  for (int i = 0; i < 3; i++)
730  {
731  int a = 0, b = 1;
732  for (int j = 0; j < 6; j++)
733  {
734  dv[i][j][0] = v[i][3 * a] - v[i][3 * b];
735  dv[i][j][1] = v[i][3 * a + 1] - v[i][3 * b + 1];
736  dv[i][j][2] = v[i][3 * a + 2] - v[i][3 * b + 2];
737 
738  b++;
739  if (b > 3)
740  {
741  a++;
742  b = a + 1;
743  }
744  }
745  }
746 
747  for (int i = 0; i < 6; i++)
748  {
749  double* row = l_6x12 + 12 * i;
750 
751  row[0] = dotXY(dv[0][i], dv[0][i]);
752  row[1] = 2.0f * dotXY(dv[0][i], dv[1][i]);
753  row[2] = dotXY(dv[1][i], dv[1][i]);
754  row[3] = 2.0f * dotXY(dv[0][i], dv[2][i]);
755  row[4] = 2.0f * dotXY(dv[1][i], dv[2][i]);
756  row[5] = dotXY(dv[2][i], dv[2][i]);
757 
758  row[6] = dotZ(dv[0][i], dv[0][i]);
759  row[7] = 2.0f * dotZ(dv[0][i], dv[1][i]);
760  row[8] = 2.0f * dotZ(dv[0][i], dv[2][i]);
761  row[9] = dotZ(dv[1][i], dv[1][i]);
762  row[10] = 2.0f * dotZ(dv[1][i], dv[2][i]);
763  row[11] = dotZ(dv[2][i], dv[2][i]);
764  }
765 }
766 
767 void upnp::compute_rho(double* rho)
768 {
769  rho[0] = dist2(cws[0], cws[1]);
770  rho[1] = dist2(cws[0], cws[2]);
771  rho[2] = dist2(cws[0], cws[3]);
772  rho[3] = dist2(cws[1], cws[2]);
773  rho[4] = dist2(cws[1], cws[3]);
774  rho[5] = dist2(cws[2], cws[3]);
775 }
776 
777 double upnp::dist2(const double* p1, const double* p2)
778 {
779  return (p1[0] - p2[0]) * (p1[0] - p2[0]) +
780  (p1[1] - p2[1]) * (p1[1] - p2[1]) +
781  (p1[2] - p2[2]) * (p1[2] - p2[2]);
782 }
783 
784 double upnp::dot(const double* v1, const double* v2)
785 {
786  return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
787 }
788 
789 double upnp::dotXY(const double* v1, const double* v2)
790 {
791  return v1[0] * v2[0] + v1[1] * v2[1];
792 }
793 
794 double upnp::dotZ(const double* v1, const double* v2) { return v1[2] * v2[2]; }
795 double upnp::sign(const double v)
796 {
797  return (v < 0.0) ? -1.0 : (v > 0.0) ? 1.0 : 0.0;
798 }
799 
800 void upnp::qr_solve(Mat* A, Mat* b, Mat* X)
801 {
802  const int nr = A->rows;
803  const int nc = A->cols;
804 
805  if (max_nr != 0 && max_nr < nr)
806  {
807  delete[] A1;
808  delete[] A2;
809  }
810  if (max_nr < nr)
811  {
812  max_nr = nr;
813  A1 = new double[nr];
814  A2 = new double[nr];
815  }
816 
817  double *pA = A->ptr<double>(0), *ppAkk = pA;
818  for (int k = 0; k < nc; k++)
819  {
820  double *ppAik1 = ppAkk, eta = fabs(*ppAik1);
821  for (int i = k + 1; i < nr; i++)
822  {
823  double elt = fabs(*ppAik1);
824  if (eta < elt) eta = elt;
825  ppAik1 += nc;
826  }
827  if (eta == 0)
828  {
829  A1[k] = A2[k] = 0.0;
830  // cerr << "God damnit, A is singular, this shouldn't happen." <<
831  // endl;
832  return;
833  }
834  else
835  {
836  double *ppAik2 = ppAkk, sum2 = 0.0, inv_eta = 1. / eta;
837  for (int i = k; i < nr; i++)
838  {
839  *ppAik2 *= inv_eta;
840  sum2 += *ppAik2 * *ppAik2;
841  ppAik2 += nc;
842  }
843  double sigma = sqrt(sum2);
844  if (*ppAkk < 0) sigma = -sigma;
845  *ppAkk += sigma;
846  A1[k] = sigma * *ppAkk;
847  A2[k] = -eta * sigma;
848  for (int j = k + 1; j < nc; j++)
849  {
850  double *ppAik = ppAkk, sum = 0;
851  for (int i = k; i < nr; i++)
852  {
853  sum += *ppAik * ppAik[j - k];
854  ppAik += nc;
855  }
856  double tau = sum / A1[k];
857  ppAik = ppAkk;
858  for (int i = k; i < nr; i++)
859  {
860  ppAik[j - k] -= tau * *ppAik;
861  ppAik += nc;
862  }
863  }
864  }
865  ppAkk += nc + 1;
866  }
867 
868  // b <- Qt b
869  double *ppAjj = pA, *pb = b->ptr<double>(0);
870  for (int j = 0; j < nc; j++)
871  {
872  double *ppAij = ppAjj, tau = 0;
873  for (int i = j; i < nr; i++)
874  {
875  tau += *ppAij * pb[i];
876  ppAij += nc;
877  }
878  tau /= A1[j];
879  ppAij = ppAjj;
880  for (int i = j; i < nr; i++)
881  {
882  pb[i] -= tau * *ppAij;
883  ppAij += nc;
884  }
885  ppAjj += nc + 1;
886  }
887 
888  // X = R-1 b
889  auto* pX = X->ptr<double>(0);
890  pX[nc - 1] = pb[nc - 1] / A2[nc - 1];
891  for (int i = nc - 2; i >= 0; i--)
892  {
893  double *ppAij = pA + i * nc + (i + 1), sum = 0;
894 
895  for (int j = i + 1; j < nc; j++)
896  {
897  sum += *ppAij * pX[j];
898  ppAij++;
899  }
900  pX[i] = (pb[i] - sum) / A2[i];
901  }
902 }
903 #endif
Unified PnP - Eigen Wrapper for OpenCV function.
Perspective n Point (PnP) Algorithms toolkit for MRPT mrpt_vision_grp.
Definition: pnp_algos.h:23
STL namespace.
Definition: img/CImage.h:23
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
int sign(T x)
Returns the sign of X as "1" or "-1".
const float R



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020