Visual Servoing Platform  version 3.6.1 under development (2024-04-23)
vpCalibrationTools.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Camera calibration.
32  */
33 
34 #include <visp3/core/vpMath.h>
35 #include <visp3/core/vpPixelMeterConversion.h>
36 #include <visp3/vision/vpCalibration.h>
37 #include <visp3/vision/vpPose.h>
38 
39 #include <cmath> // std::fabs
40 #include <limits> // numeric_limits
41 
42 #define DEBUG_LEVEL1 0
43 #define DEBUG_LEVEL2 0
44 
45 #undef MAX /* FC unused anywhere */
46 #undef MIN /* FC unused anywhere */
47 
48 void vpCalibration::calibLagrange(vpCameraParameters &cam_est, vpHomogeneousMatrix &cMo_est)
49 {
50 
51  vpMatrix A(2 * m_npt, 3);
52  vpMatrix B(2 * m_npt, 9);
53 
54  std::list<double>::const_iterator it_LoX = m_LoX.begin();
55  std::list<double>::const_iterator it_LoY = m_LoY.begin();
56  std::list<double>::const_iterator it_LoZ = m_LoZ.begin();
57  std::list<vpImagePoint>::const_iterator it_Lip = m_Lip.begin();
58 
59  vpImagePoint ip;
60 
61  for (unsigned int i = 0; i < m_npt; i++) {
62 
63  double x0 = *it_LoX;
64  double y0 = *it_LoY;
65  double z0 = *it_LoZ;
66 
67  ip = *it_Lip;
68 
69  double xi = ip.get_u();
70  double yi = ip.get_v();
71 
72  A[2 * i][0] = x0 * xi;
73  A[2 * i][1] = y0 * xi;
74  A[2 * i][2] = z0 * xi;
75  B[2 * i][0] = -x0;
76  B[2 * i][1] = -y0;
77  B[2 * i][2] = -z0;
78  B[2 * i][3] = 0.0;
79  B[2 * i][4] = 0.0;
80  B[2 * i][5] = 0.0;
81  B[2 * i][6] = -1.0;
82  B[2 * i][7] = 0.0;
83  B[2 * i][8] = xi;
84  A[2 * i + 1][0] = x0 * yi;
85  A[2 * i + 1][1] = y0 * yi;
86  A[2 * i + 1][2] = z0 * yi;
87  B[2 * i + 1][0] = 0.0;
88  B[2 * i + 1][1] = 0.0;
89  B[2 * i + 1][2] = 0.0;
90  B[2 * i + 1][3] = -x0;
91  B[2 * i + 1][4] = -y0;
92  B[2 * i + 1][5] = -z0;
93  B[2 * i + 1][6] = 0.0;
94  B[2 * i + 1][7] = -1.0;
95  B[2 * i + 1][8] = yi;
96 
97  ++it_LoX;
98  ++it_LoY;
99  ++it_LoZ;
100  ++it_Lip;
101  }
102 
103  vpMatrix BtB; /* compute B^T B */
104  BtB = B.t() * B;
105 
106  /* compute (B^T B)^(-1) */
107  /* input : btb (dimension 9 x 9) = (B^T B) */
108  /* output : btbinv (dimension 9 x 9) = (B^T B)^(-1) */
109 
110  vpMatrix BtBinv;
111  BtBinv = BtB.pseudoInverse(1e-16);
112 
113  vpMatrix BtA;
114  BtA = B.t() * A; /* compute B^T A */
115 
116  vpMatrix r;
117  r = BtBinv * BtA; /* compute (B^T B)^(-1) B^T A */
118 
119  vpMatrix e; /* compute - A^T B (B^T B)^(-1) B^T A*/
120  e = -(A.t() * B) * r;
121 
122  vpMatrix AtA; /* compute A^T A */
123  AtA = A.AtA();
124 
125  e += AtA; /* compute E = A^T A - A^T B (B^T B)^(-1) B^T A */
126 
127  vpColVector x1(3);
128  vpColVector x2;
129 
130  e.svd(x1, AtA); // destructive on e
131  // eigenvector computation of E corresponding to the min eigenvalue.
132  /* SVmax computation*/
133  double svm = 0.0;
134  unsigned int imin = 1;
135  for (unsigned int i = 0; i < x1.getRows(); i++) {
136  if (x1[i] > svm) {
137  svm = x1[i];
138  imin = i;
139  }
140  }
141 
142  // svm *= 0.1; /* for the rank */
143 
144  for (unsigned int i = 0; i < x1.getRows(); i++) {
145  if (x1[i] < x1[imin])
146  imin = i;
147  }
148 
149  for (unsigned int i = 0; i < x1.getRows(); i++)
150  x1[i] = AtA[i][imin];
151 
152  x2 = -(r * x1); // X_2 = - (B^T B)^(-1) B^T A X_1
153 
154  vpColVector sol(12);
155  vpColVector resul(7);
156  for (unsigned int i = 0; i < 3; i++)
157  sol[i] = x1[i]; /* X_1 */
158  for (unsigned int i = 0; i < 9; i++) /* X_2 = - (B^T B)^(-1) B^T A X_1 */
159  {
160  sol[i + 3] = x2[i];
161  }
162 
163  if (sol[11] < 0.0)
164  for (unsigned int i = 0; i < 12; i++)
165  sol[i] = -sol[i]; /* since Z0 > 0 */
166 
167  resul[0] = sol[3] * sol[0] + sol[4] * sol[1] + sol[5] * sol[2]; /* u0 */
168 
169  resul[1] = sol[6] * sol[0] + sol[7] * sol[1] + sol[8] * sol[2]; /* v0 */
170 
171  resul[2] = sqrt(sol[3] * sol[3] + sol[4] * sol[4] + sol[5] * sol[5] /* px */
172  - resul[0] * resul[0]);
173 
174  if (m_aspect_ratio > 0.) {
175  resul[3] = resul[2] / m_aspect_ratio;
176  }
177  else {
178  resul[3] = sqrt(sol[6] * sol[6] + sol[7] * sol[7] + sol[8] * sol[8] /* py */
179  - resul[1] * resul[1]);
180  }
181 
182  cam_est.initPersProjWithoutDistortion(resul[2], resul[3], resul[0], resul[1]);
183 
184  resul[4] = (sol[9] - sol[11] * resul[0]) / resul[2]; /* X0 */
185  resul[5] = (sol[10] - sol[11] * resul[1]) / resul[3]; /* Y0 */
186  resul[6] = sol[11]; /* Z0 */
187 
188  vpMatrix rd(3, 3);
189  /* fill rotation matrix */
190  for (unsigned int i = 0; i < 3; i++)
191  rd[0][i] = (sol[i + 3] - sol[i] * resul[0]) / resul[2];
192  for (unsigned int i = 0; i < 3; i++)
193  rd[1][i] = (sol[i + 6] - sol[i] * resul[1]) / resul[3];
194  for (unsigned int i = 0; i < 3; i++)
195  rd[2][i] = sol[i];
196 
197  // std::cout << "norme X1 " << x1.sumSquare() <<std::endl;
198  // std::cout << rd*rd.t() ;
199 
200  for (unsigned int i = 0; i < 3; i++) {
201  for (unsigned int j = 0; j < 3; j++)
202  cMo_est[i][j] = rd[i][j];
203  }
204  for (unsigned int i = 0; i < 3; i++)
205  cMo_est[i][3] = resul[i + 4];
206 
207  this->cMo = cMo_est;
208  this->cMo_dist = cMo_est;
209 
210  double deviation, deviation_dist;
211  this->computeStdDeviation(deviation, deviation_dist);
212 }
213 
214 void vpCalibration::calibVVS(vpCameraParameters &cam_est, vpHomogeneousMatrix &cMo_est, bool verbose)
215 {
216  std::ios::fmtflags original_flags(std::cout.flags());
217  std::cout.precision(10);
218  unsigned int n_points = m_npt;
219 
220  vpColVector oX(n_points), cX(n_points);
221  vpColVector oY(n_points), cY(n_points);
222  vpColVector oZ(n_points), cZ(n_points);
223  vpColVector u(n_points);
224  vpColVector v(n_points);
225 
226  vpColVector P(2 * n_points);
227  vpColVector Pd(2 * n_points);
228 
229  vpImagePoint ip;
230 
231  std::list<double>::const_iterator it_LoX = m_LoX.begin();
232  std::list<double>::const_iterator it_LoY = m_LoY.begin();
233  std::list<double>::const_iterator it_LoZ = m_LoZ.begin();
234  std::list<vpImagePoint>::const_iterator it_Lip = m_Lip.begin();
235 
236  for (unsigned int i = 0; i < n_points; i++) {
237  oX[i] = *it_LoX;
238  oY[i] = *it_LoY;
239  oZ[i] = *it_LoZ;
240 
241  ip = *it_Lip;
242 
243  u[i] = ip.get_u();
244  v[i] = ip.get_v();
245 
246  ++it_LoX;
247  ++it_LoY;
248  ++it_LoZ;
249  ++it_Lip;
250  }
251 
252  // double lambda = 0.1 ;
253  unsigned int iter = 0;
254 
255  double residu_1 = 1e12;
256  double r = 1e12 - 1;
257  while (vpMath::equal(residu_1, r, m_threshold) == false && iter < m_nbIterMax) {
258  iter++;
259  residu_1 = r;
260 
261  double px, py;
262  if (m_aspect_ratio > 0.) {
263  px = cam_est.get_px();
264  py = px / m_aspect_ratio;
265  }
266  else {
267  px = cam_est.get_px(); // default
268  py = cam_est.get_py();
269  }
270  double u0 = cam_est.get_u0();
271  double v0 = cam_est.get_v0();
272 
273  r = 0;
274 
275  for (unsigned int i = 0; i < n_points; i++) {
276  cX[i] = oX[i] * cMo_est[0][0] + oY[i] * cMo_est[0][1] + oZ[i] * cMo_est[0][2] + cMo_est[0][3];
277  cY[i] = oX[i] * cMo_est[1][0] + oY[i] * cMo_est[1][1] + oZ[i] * cMo_est[1][2] + cMo_est[1][3];
278  cZ[i] = oX[i] * cMo_est[2][0] + oY[i] * cMo_est[2][1] + oZ[i] * cMo_est[2][2] + cMo_est[2][3];
279 
280  Pd[2 * i] = u[i];
281  Pd[2 * i + 1] = v[i];
282 
283  P[2 * i] = cX[i] / cZ[i] * px + u0;
284  P[2 * i + 1] = cY[i] / cZ[i] * py + v0;
285 
286  r += ((vpMath::sqr(P[2 * i] - Pd[2 * i]) + vpMath::sqr(P[2 * i + 1] - Pd[2 * i + 1])));
287  }
288 
289  vpColVector error;
290  error = P - Pd;
291  // r = r/n_points ;
292 
293  vpMatrix L(n_points * 2, 9 + (m_aspect_ratio > 0. ? 0 : 1));
294  for (unsigned int i = 0; i < n_points; i++) {
295  double x = cX[i];
296  double y = cY[i];
297  double z = cZ[i];
298  double inv_z = 1 / z;
299 
300  double X = x * inv_z;
301  double Y = y * inv_z;
302 
303  //---------------
304  {
305  L[2 * i][0] = px * (-inv_z);
306  L[2 * i][1] = 0;
307  L[2 * i][2] = px * X * inv_z;
308  L[2 * i][3] = px * X * Y;
309  L[2 * i][4] = -px * (1 + X * X);
310  L[2 * i][5] = px * Y;
311  }
312  {
313  L[2 * i][6] = 1;
314  L[2 * i][7] = 0;
315  L[2 * i][8] = X;
316  if (m_aspect_ratio > 0.) {
317  L[2 * i][8] = X;
318  }
319  else // default
320  {
321  L[2 * i][8] = X;
322  L[2 * i][9] = 0;
323  }
324  }
325  {
326  L[2 * i + 1][0] = 0;
327  L[2 * i + 1][1] = py * (-inv_z);
328  L[2 * i + 1][2] = py * (Y * inv_z);
329  L[2 * i + 1][3] = py * (1 + Y * Y);
330  L[2 * i + 1][4] = -py * X * Y;
331  L[2 * i + 1][5] = -py * X;
332  }
333  {
334  L[2 * i + 1][6] = 0;
335  L[2 * i + 1][7] = 1;
336  if (m_aspect_ratio > 0.) {
337  L[2 * i + 1][8] = Y;
338  }
339  else {
340  L[2 * i + 1][8] = 0;
341  L[2 * i + 1][9] = Y;
342  }
343  }
344  } // end interaction
345  vpMatrix Lp;
346  Lp = L.pseudoInverse(1e-10);
347 
348  vpColVector e;
349  e = Lp * error;
350 
351  vpColVector Tc, Tc_v(6);
352  Tc = -e * m_gain;
353 
354  // Tc_v =0 ;
355  for (unsigned int i = 0; i < 6; i++)
356  Tc_v[i] = Tc[i];
357 
358  if (m_aspect_ratio > 0.) {
359  cam_est.initPersProjWithoutDistortion(px + Tc[8], (px + Tc[8]) / m_aspect_ratio, u0 + Tc[6], v0 + Tc[7]);
360  }
361  else {
362  cam_est.initPersProjWithoutDistortion(px + Tc[8], py + Tc[9], u0 + Tc[6], v0 + Tc[7]); // default
363  }
364 
365  cMo_est = vpExponentialMap::direct(Tc_v).inverse() * cMo_est;
366  if (verbose)
367  std::cout << " std dev " << sqrt(r / n_points) << std::endl;
368  }
369  if (iter == m_nbIterMax) {
370  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", m_nbIterMax);
371  throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
372  }
373  this->cMo = cMo_est;
374  this->cMo_dist = cMo_est;
375  this->m_residual = r;
376  this->m_residual_dist = r;
377  if (verbose)
378  std::cout << " std dev " << sqrt(r / n_points) << std::endl;
379  // Restore ostream format
380  std::cout.flags(original_flags);
381 }
382 
383 void vpCalibration::calibVVSMulti(std::vector<vpCalibration> &table_cal, vpCameraParameters &cam_est,
384  double &globalReprojectionError, bool verbose, double aspect_ratio)
385 {
386  std::ios::fmtflags original_flags(std::cout.flags());
387  std::cout.precision(10);
388  unsigned int nbPoint[256]; // number of points by image
389  unsigned int nbPointTotal = 0; // total number of points
390  unsigned int nbPose = (unsigned int)table_cal.size();
391  unsigned int nbPose6 = 6 * nbPose;
392 
393  for (unsigned int i = 0; i < nbPose; i++) {
394  nbPoint[i] = table_cal[i].m_npt;
395  nbPointTotal += nbPoint[i];
396  }
397 
398  if (nbPointTotal < 4) {
399  // vpERROR_TRACE("Not enough point to calibrate");
400  throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate"));
401  }
402 
403  vpColVector oX(nbPointTotal), cX(nbPointTotal);
404  vpColVector oY(nbPointTotal), cY(nbPointTotal);
405  vpColVector oZ(nbPointTotal), cZ(nbPointTotal);
406  vpColVector u(nbPointTotal);
407  vpColVector v(nbPointTotal);
408 
409  vpColVector P(2 * nbPointTotal);
410  vpColVector Pd(2 * nbPointTotal);
411  vpImagePoint ip;
412 
413  unsigned int curPoint = 0; // current point index
414  for (unsigned int p = 0; p < nbPose; p++) {
415  std::list<double>::const_iterator it_LoX = table_cal[p].m_LoX.begin();
416  std::list<double>::const_iterator it_LoY = table_cal[p].m_LoY.begin();
417  std::list<double>::const_iterator it_LoZ = table_cal[p].m_LoZ.begin();
418  std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].m_Lip.begin();
419 
420  for (unsigned int i = 0; i < nbPoint[p]; i++) {
421  oX[curPoint] = *it_LoX;
422  oY[curPoint] = *it_LoY;
423  oZ[curPoint] = *it_LoZ;
424 
425  ip = *it_Lip;
426  u[curPoint] = ip.get_u();
427  v[curPoint] = ip.get_v();
428 
429  ++it_LoX;
430  ++it_LoY;
431  ++it_LoZ;
432  ++it_Lip;
433 
434  curPoint++;
435  }
436  }
437  // double lambda = 0.1 ;
438  unsigned int iter = 0;
439 
440  double residu_1 = 1e12;
441  double r = 1e12 - 1;
442  while (vpMath::equal(residu_1, r, m_threshold) == false && iter < m_nbIterMax) {
443 
444  iter++;
445  residu_1 = r;
446 
447  double px, py;
448  if (aspect_ratio > 0.) {
449  px = cam_est.get_px();
450  py = px / aspect_ratio;
451  }
452  else {
453  px = cam_est.get_px(); // default
454  py = cam_est.get_py();
455  }
456  double u0 = cam_est.get_u0();
457  double v0 = cam_est.get_v0();
458 
459  r = 0;
460  curPoint = 0; // current point index
461  for (unsigned int p = 0; p < nbPose; p++) {
462  vpHomogeneousMatrix cMoTmp = table_cal[p].cMo;
463  for (unsigned int i = 0; i < nbPoint[p]; i++) {
464  unsigned int curPoint2 = 2 * curPoint;
465 
466  cX[curPoint] =
467  oX[curPoint] * cMoTmp[0][0] + oY[curPoint] * cMoTmp[0][1] + oZ[curPoint] * cMoTmp[0][2] + cMoTmp[0][3];
468  cY[curPoint] =
469  oX[curPoint] * cMoTmp[1][0] + oY[curPoint] * cMoTmp[1][1] + oZ[curPoint] * cMoTmp[1][2] + cMoTmp[1][3];
470  cZ[curPoint] =
471  oX[curPoint] * cMoTmp[2][0] + oY[curPoint] * cMoTmp[2][1] + oZ[curPoint] * cMoTmp[2][2] + cMoTmp[2][3];
472 
473  Pd[curPoint2] = u[curPoint];
474  Pd[curPoint2 + 1] = v[curPoint];
475 
476  P[curPoint2] = cX[curPoint] / cZ[curPoint] * px + u0;
477  P[curPoint2 + 1] = cY[curPoint] / cZ[curPoint] * py + v0;
478 
479  r += (vpMath::sqr(P[curPoint2] - Pd[curPoint2]) + vpMath::sqr(P[curPoint2 + 1] - Pd[curPoint2 + 1]));
480  curPoint++;
481  }
482  }
483 
484  vpColVector error;
485  error = P - Pd;
486  // r = r/nbPointTotal ;
487 
488  vpMatrix L(nbPointTotal * 2, nbPose6 + 3 + (aspect_ratio > 0. ? 0 : 1));
489  curPoint = 0; // current point index
490  for (unsigned int p = 0; p < nbPose; p++) {
491  unsigned int q = 6 * p;
492  for (unsigned int i = 0; i < nbPoint[p]; i++) {
493  unsigned int curPoint2 = 2 * curPoint;
494  unsigned int curPoint21 = curPoint2 + 1;
495 
496  double x = cX[curPoint];
497  double y = cY[curPoint];
498  double z = cZ[curPoint];
499 
500  double inv_z = 1 / z;
501 
502  double X = x * inv_z;
503  double Y = y * inv_z;
504 
505  //---------------
506  {
507  {
508  L[curPoint2][q] = px * (-inv_z);
509  L[curPoint2][q + 1] = 0;
510  L[curPoint2][q + 2] = px * (X * inv_z);
511  L[curPoint2][q + 3] = px * X * Y;
512  L[curPoint2][q + 4] = -px * (1 + X * X);
513  L[curPoint2][q + 5] = px * Y;
514  }
515  {
516  L[curPoint2][nbPose6] = 1;
517  L[curPoint2][nbPose6 + 1] = 0;
518  if (aspect_ratio > 0.) {
519  L[curPoint2][nbPose6 + 2] = X;
520  }
521  else { // default
522  L[curPoint2][nbPose6 + 2] = X;
523  L[curPoint2][nbPose6 + 3] = 0;
524  }
525  }
526  {
527  L[curPoint21][q] = 0;
528  L[curPoint21][q + 1] = py * (-inv_z);
529  L[curPoint21][q + 2] = py * (Y * inv_z);
530  L[curPoint21][q + 3] = py * (1 + Y * Y);
531  L[curPoint21][q + 4] = -py * X * Y;
532  L[curPoint21][q + 5] = -py * X;
533  }
534  {
535  L[curPoint21][nbPose6] = 0;
536  L[curPoint21][nbPose6 + 1] = 1;
537  if (aspect_ratio > 0.) {
538  L[curPoint21][nbPose6 + 2] = Y;
539  }
540  else { // default
541  L[curPoint21][nbPose6 + 2] = 0;
542  L[curPoint21][nbPose6 + 3] = Y;
543  }
544  }
545  }
546  curPoint++;
547  } // end interaction
548  }
549  vpMatrix Lp;
550  Lp = L.pseudoInverse(1e-10);
551 
552  vpColVector e;
553  e = Lp * error;
554 
555  vpColVector Tc, Tc_v(nbPose6);
556  Tc = -e * m_gain;
557 
558  // Tc_v =0 ;
559  for (unsigned int i = 0; i < nbPose6; i++)
560  Tc_v[i] = Tc[i];
561 
562  if (aspect_ratio > 0.) {
563  cam_est.initPersProjWithoutDistortion(px + Tc[nbPose6 + 2], (px + Tc[nbPose6 + 2]) / aspect_ratio,
564  u0 + Tc[nbPose6], v0 + Tc[nbPose6 + 1]);
565  }
566  else // default
567  {
568  cam_est.initPersProjWithoutDistortion(px + Tc[nbPose6 + 2], py + Tc[nbPose6 + 3], u0 + Tc[nbPose6],
569  v0 + Tc[nbPose6 + 1]);
570  }
571 
572  // cam.setKd(get_kd() + Tc[10]) ;
573  vpColVector Tc_v_Tmp(6);
574 
575  for (unsigned int p = 0; p < nbPose; p++) {
576  for (unsigned int i = 0; i < 6; i++)
577  Tc_v_Tmp[i] = Tc_v[6 * p + i];
578 
579  table_cal[p].cMo = vpExponentialMap::direct(Tc_v_Tmp, 1).inverse() * table_cal[p].cMo;
580  }
581 
582  if (verbose)
583  std::cout << " std dev " << sqrt(r / nbPointTotal) << std::endl;
584  }
585  if (iter == m_nbIterMax) {
586  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", m_nbIterMax);
587  throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
588  }
589  for (unsigned int p = 0; p < nbPose; p++) {
590  table_cal[p].cMo_dist = table_cal[p].cMo;
591  table_cal[p].cam = cam_est;
592  table_cal[p].cam_dist = cam_est;
593  double deviation, deviation_dist;
594  table_cal[p].computeStdDeviation(deviation, deviation_dist);
595  }
596  globalReprojectionError = sqrt(r / nbPointTotal);
597  // Restore ostream format
598  std::cout.flags(original_flags);
599 }
600 
601 void vpCalibration::calibVVSWithDistortion(vpCameraParameters &cam_est, vpHomogeneousMatrix &cMo_est, bool verbose)
602 {
603  std::ios::fmtflags original_flags(std::cout.flags());
604  std::cout.precision(10);
605  unsigned int n_points = m_npt;
606 
607  vpColVector oX(n_points), cX(n_points);
608  vpColVector oY(n_points), cY(n_points);
609  vpColVector oZ(n_points), cZ(n_points);
610  vpColVector u(n_points);
611  vpColVector v(n_points);
612 
613  vpColVector P(4 * n_points);
614  vpColVector Pd(4 * n_points);
615 
616  std::list<double>::const_iterator it_LoX = m_LoX.begin();
617  std::list<double>::const_iterator it_LoY = m_LoY.begin();
618  std::list<double>::const_iterator it_LoZ = m_LoZ.begin();
619  std::list<vpImagePoint>::const_iterator it_Lip = m_Lip.begin();
620 
621  vpImagePoint ip;
622 
623  for (unsigned int i = 0; i < n_points; i++) {
624  oX[i] = *it_LoX;
625  oY[i] = *it_LoY;
626  oZ[i] = *it_LoZ;
627 
628  ip = *it_Lip;
629  u[i] = ip.get_u();
630  v[i] = ip.get_v();
631 
632  ++it_LoX;
633  ++it_LoY;
634  ++it_LoZ;
635  ++it_Lip;
636  }
637 
638  // double lambda = 0.1 ;
639  unsigned int iter = 0;
640 
641  double residu_1 = 1e12;
642  double r = 1e12 - 1;
643  while (vpMath::equal(residu_1, r, m_threshold) == false && iter < m_nbIterMax) {
644  iter++;
645  residu_1 = r;
646 
647  r = 0;
648  double u0 = cam_est.get_u0();
649  double v0 = cam_est.get_v0();
650 
651  double px, py;
652  if (m_aspect_ratio > 0.) {
653  px = cam_est.get_px();
654  py = px / m_aspect_ratio;
655  }
656  else {
657  px = cam_est.get_px(); // default
658  py = cam_est.get_py();
659  }
660 
661  double inv_px = 1 / px;
662  double inv_py = 1 / py;
663 
664  double kud = cam_est.get_kud();
665  double kdu = cam_est.get_kdu();
666 
667  double k2ud = 2 * kud;
668  double k2du = 2 * kdu;
669  vpMatrix L(n_points * 4, 11 + (m_aspect_ratio > 0. ? 0 : 1));
670 
671  for (unsigned int i = 0; i < n_points; i++) {
672  unsigned int i4 = 4 * i;
673  unsigned int i41 = 4 * i + 1;
674  unsigned int i42 = 4 * i + 2;
675  unsigned int i43 = 4 * i + 3;
676 
677  cX[i] = oX[i] * cMo_est[0][0] + oY[i] * cMo_est[0][1] + oZ[i] * cMo_est[0][2] + cMo_est[0][3];
678  cY[i] = oX[i] * cMo_est[1][0] + oY[i] * cMo_est[1][1] + oZ[i] * cMo_est[1][2] + cMo_est[1][3];
679  cZ[i] = oX[i] * cMo_est[2][0] + oY[i] * cMo_est[2][1] + oZ[i] * cMo_est[2][2] + cMo_est[2][3];
680 
681  double x = cX[i];
682  double y = cY[i];
683  double z = cZ[i];
684  double inv_z = 1 / z;
685 
686  double X = x * inv_z;
687  double Y = y * inv_z;
688 
689  double X2 = X * X;
690  double Y2 = Y * Y;
691  double XY = X * Y;
692 
693  double up = u[i];
694  double vp = v[i];
695 
696  Pd[i4] = up;
697  Pd[i41] = vp;
698 
699  double up0 = up - u0;
700  double vp0 = vp - v0;
701 
702  double xp0 = up0 * inv_px;
703  double xp02 = xp0 * xp0;
704 
705  double yp0 = vp0 * inv_py;
706  double yp02 = yp0 * yp0;
707 
708  double r2du = xp02 + yp02;
709  double kr2du = kdu * r2du;
710 
711  P[i4] = u0 + px * X - kr2du * (up0);
712  P[i41] = v0 + py * Y - kr2du * (vp0);
713 
714  double r2ud = X2 + Y2;
715  double kr2ud = 1 + kud * r2ud;
716 
717  double Axx = px * (kr2ud + k2ud * X2);
718  double Axy = px * k2ud * XY;
719  double Ayy = py * (kr2ud + k2ud * Y2);
720  double Ayx = py * k2ud * XY;
721 
722  Pd[i42] = up;
723  Pd[i43] = vp;
724 
725  P[i42] = u0 + px * X * kr2ud;
726  P[i43] = v0 + py * Y * kr2ud;
727 
728  r += (vpMath::sqr(P[i4] - Pd[i4]) + vpMath::sqr(P[i41] - Pd[i41]) + vpMath::sqr(P[i42] - Pd[i42]) +
729  vpMath::sqr(P[i43] - Pd[i43])) *
730  0.5;
731 
732  //--distorted to undistorted
733  {
734  {
735  L[i4][0] = px * (-inv_z);
736  L[i4][1] = 0;
737  L[i4][2] = px * X * inv_z;
738  L[i4][3] = px * X * Y;
739  L[i4][4] = -px * (1 + X2);
740  L[i4][5] = px * Y;
741  }
742  {
743  L[i4][6] = 1 + kr2du + k2du * xp02;
744  L[i4][7] = k2du * up0 * yp0 * inv_py;
745 
746  if (m_aspect_ratio > 0.) {
747  L[i4][8] = X + k2du * xp02 * xp0 + k2du * up0 * yp02 * inv_py;
748  L[i4][9] = -(up0) * (r2du);
749  L[i4][10] = 0;
750  }
751  else // default
752  {
753  L[i4][8] = X + k2du * xp02 * xp0;
754  L[i4][9] = k2du * up0 * yp02 * inv_py;
755  L[i4][10] = -(up0) * (r2du);
756  L[i4][11] = 0;
757  }
758  }
759  {
760  L[i41][0] = 0;
761  L[i41][1] = py * (-inv_z);
762  L[i41][2] = py * Y * inv_z;
763  L[i41][3] = py * (1 + Y2);
764  L[i41][4] = -py * XY;
765  L[i41][5] = -py * X;
766  }
767  {
768  L[i41][6] = k2du * xp0 * vp0 * inv_px;
769  L[i41][7] = 1 + kr2du + k2du * yp02;
770  if (m_aspect_ratio > 0.) {
771  L[i41][8] = k2du * vp0 * xp02 * inv_px + Y + k2du * yp02 * yp0;
772  L[i41][9] = -vp0 * r2du;
773  L[i41][10] = 0;
774  }
775  else // default
776  {
777  L[i41][8] = k2du * vp0 * xp02 * inv_px;
778  L[i41][9] = Y + k2du * yp02 * yp0;
779  L[i41][10] = -vp0 * r2du;
780  L[i41][11] = 0;
781  }
782  }
783  //---undistorted to distorted
784  {
785  L[i42][0] = Axx * (-inv_z);
786  L[i42][1] = Axy * (-inv_z);
787  L[i42][2] = Axx * (X * inv_z) + Axy * (Y * inv_z);
788  L[i42][3] = Axx * X * Y + Axy * (1 + Y2);
789  L[i42][4] = -Axx * (1 + X2) - Axy * XY;
790  L[i42][5] = Axx * Y - Axy * X;
791  }
792  {
793  L[i42][6] = 1;
794  L[i42][7] = 0;
795  if (m_aspect_ratio > 0.) {
796  L[i42][8] = X * kr2ud;
797  L[i42][9] = 0;
798  L[i42][10] = px * X * r2ud;
799  }
800  else // default
801  {
802  L[i42][8] = X * kr2ud;
803  L[i42][9] = 0;
804  L[i42][10] = 0;
805  L[i42][11] = px * X * r2ud;
806  }
807  }
808  {
809  L[i43][0] = Ayx * (-inv_z);
810  L[i43][1] = Ayy * (-inv_z);
811  L[i43][2] = Ayx * (X * inv_z) + Ayy * (Y * inv_z);
812  L[i43][3] = Ayx * XY + Ayy * (1 + Y2);
813  L[i43][4] = -Ayx * (1 + X2) - Ayy * XY;
814  L[i43][5] = Ayx * Y - Ayy * X;
815  }
816  {
817  L[i43][6] = 0;
818  L[i43][7] = 1;
819  if (m_aspect_ratio > 0.) {
820  L[i43][8] = Y * kr2ud;
821  L[i43][9] = 0;
822  L[i43][10] = py * Y * r2ud;
823  }
824  else {
825  L[i43][8] = 0;
826  L[i43][9] = Y * kr2ud;
827  L[i43][10] = 0;
828  L[i43][11] = py * Y * r2ud;
829  }
830  }
831  } // end interaction
832  } // end interaction
833 
834  vpColVector error;
835  error = P - Pd;
836  // r = r/n_points ;
837 
838  vpMatrix Lp;
839  Lp = L.pseudoInverse(1e-10);
840 
841  vpColVector e;
842  e = Lp * error;
843 
844  vpColVector Tc, Tc_v(6);
845  Tc = -e * m_gain;
846 
847  for (unsigned int i = 0; i < 6; i++)
848  Tc_v[i] = Tc[i];
849 
850  if (m_aspect_ratio > 0.) {
851  cam_est.initPersProjWithDistortion(px + Tc[8], (px + Tc[8]) / m_aspect_ratio, u0 + Tc[6], v0 + Tc[7],
852  kud + Tc[10], kdu + Tc[9]);
853  }
854  else {
855  cam_est.initPersProjWithDistortion(px + Tc[8], py + Tc[9], u0 + Tc[6], v0 + Tc[7], kud + Tc[11], kdu + Tc[10]);
856  }
857 
858  cMo_est = vpExponentialMap::direct(Tc_v).inverse() * cMo_est;
859  if (verbose)
860  std::cout << " std dev " << sqrt(r / n_points) << std::endl;
861  }
862  if (iter == m_nbIterMax) {
863  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", m_nbIterMax);
864  throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
865  }
866  this->m_residual_dist = r;
867  this->cMo_dist = cMo_est;
868  this->cam_dist = cam_est;
869 
870  if (verbose)
871  std::cout << " std dev " << sqrt(r / n_points) << std::endl;
872 
873  // Restore ostream format
874  std::cout.flags(original_flags);
875 }
876 
877 void vpCalibration::calibVVSWithDistortionMulti(std::vector<vpCalibration> &table_cal, vpCameraParameters &cam_est,
878  double &globalReprojectionError, bool verbose, double aspect_ratio)
879 {
880  std::ios::fmtflags original_flags(std::cout.flags());
881  std::cout.precision(10);
882  unsigned int nbPoint[1024]; // number of points by image
883  unsigned int nbPointTotal = 0; // total number of points
884  unsigned int nbPose = (unsigned int)table_cal.size();
885  unsigned int nbPose6 = 6 * nbPose;
886  for (unsigned int i = 0; i < nbPose; i++) {
887  nbPoint[i] = table_cal[i].m_npt;
888  nbPointTotal += nbPoint[i];
889  }
890 
891  if (nbPointTotal < 4) {
892  // vpERROR_TRACE("Not enough point to calibrate");
893  throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate"));
894  }
895 
896  vpColVector oX(nbPointTotal), cX(nbPointTotal);
897  vpColVector oY(nbPointTotal), cY(nbPointTotal);
898  vpColVector oZ(nbPointTotal), cZ(nbPointTotal);
899  vpColVector u(nbPointTotal);
900  vpColVector v(nbPointTotal);
901 
902  vpColVector P(4 * nbPointTotal);
903  vpColVector Pd(4 * nbPointTotal);
904  vpImagePoint ip;
905 
906  unsigned int curPoint = 0; // current point index
907  for (unsigned int p = 0; p < nbPose; p++) {
908  std::list<double>::const_iterator it_LoX = table_cal[p].m_LoX.begin();
909  std::list<double>::const_iterator it_LoY = table_cal[p].m_LoY.begin();
910  std::list<double>::const_iterator it_LoZ = table_cal[p].m_LoZ.begin();
911  std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].m_Lip.begin();
912 
913  for (unsigned int i = 0; i < nbPoint[p]; i++) {
914  oX[curPoint] = *it_LoX;
915  oY[curPoint] = *it_LoY;
916  oZ[curPoint] = *it_LoZ;
917 
918  ip = *it_Lip;
919  u[curPoint] = ip.get_u();
920  v[curPoint] = ip.get_v();
921 
922  ++it_LoX;
923  ++it_LoY;
924  ++it_LoZ;
925  ++it_Lip;
926  curPoint++;
927  }
928  }
929  // double lambda = 0.1 ;
930  unsigned int iter = 0;
931 
932  double residu_1 = 1e12;
933  double r = 1e12 - 1;
934  while (vpMath::equal(residu_1, r, m_threshold) == false && iter < m_nbIterMax) {
935  iter++;
936  residu_1 = r;
937 
938  r = 0;
939  curPoint = 0; // current point index
940  for (unsigned int p = 0; p < nbPose; p++) {
941  vpHomogeneousMatrix cMoTmp = table_cal[p].cMo_dist;
942  for (unsigned int i = 0; i < nbPoint[p]; i++) {
943  cX[curPoint] =
944  oX[curPoint] * cMoTmp[0][0] + oY[curPoint] * cMoTmp[0][1] + oZ[curPoint] * cMoTmp[0][2] + cMoTmp[0][3];
945  cY[curPoint] =
946  oX[curPoint] * cMoTmp[1][0] + oY[curPoint] * cMoTmp[1][1] + oZ[curPoint] * cMoTmp[1][2] + cMoTmp[1][3];
947  cZ[curPoint] =
948  oX[curPoint] * cMoTmp[2][0] + oY[curPoint] * cMoTmp[2][1] + oZ[curPoint] * cMoTmp[2][2] + cMoTmp[2][3];
949 
950  curPoint++;
951  }
952  }
953 
954  vpMatrix L(nbPointTotal * 4, nbPose6 + 5 + (aspect_ratio > 0. ? 0 : 1));
955  curPoint = 0; // current point index
956  double px, py;
957  if (aspect_ratio > 0.) {
958  px = cam_est.get_px();
959  py = px / aspect_ratio;
960  }
961  else {
962  px = cam_est.get_px(); // default
963  py = cam_est.get_py();
964  }
965  double u0 = cam_est.get_u0();
966  double v0 = cam_est.get_v0();
967 
968  double inv_px = 1 / px;
969  double inv_py = 1 / py;
970 
971  double kud = cam_est.get_kud();
972  double kdu = cam_est.get_kdu();
973 
974  double k2ud = 2 * kud;
975  double k2du = 2 * kdu;
976 
977  for (unsigned int p = 0; p < nbPose; p++) {
978  unsigned int q = 6 * p;
979  for (unsigned int i = 0; i < nbPoint[p]; i++) {
980  unsigned int curPoint4 = 4 * curPoint;
981  double x = cX[curPoint];
982  double y = cY[curPoint];
983  double z = cZ[curPoint];
984 
985  double inv_z = 1 / z;
986  double X = x * inv_z;
987  double Y = y * inv_z;
988 
989  double X2 = X * X;
990  double Y2 = Y * Y;
991  double XY = X * Y;
992 
993  double up = u[curPoint];
994  double vp = v[curPoint];
995 
996  Pd[curPoint4] = up;
997  Pd[curPoint4 + 1] = vp;
998 
999  double up0 = up - u0;
1000  double vp0 = vp - v0;
1001 
1002  double xp0 = up0 * inv_px;
1003  double xp02 = xp0 * xp0;
1004 
1005  double yp0 = vp0 * inv_py;
1006  double yp02 = yp0 * yp0;
1007 
1008  double r2du = xp02 + yp02;
1009  double kr2du = kdu * r2du;
1010 
1011  P[curPoint4] = u0 + px * X - kr2du * (up0);
1012  P[curPoint4 + 1] = v0 + py * Y - kr2du * (vp0);
1013 
1014  double r2ud = X2 + Y2;
1015  double kr2ud = 1 + kud * r2ud;
1016 
1017  double Axx = px * (kr2ud + k2ud * X2);
1018  double Axy = px * k2ud * XY;
1019  double Ayy = py * (kr2ud + k2ud * Y2);
1020  double Ayx = py * k2ud * XY;
1021 
1022  Pd[curPoint4 + 2] = up;
1023  Pd[curPoint4 + 3] = vp;
1024 
1025  P[curPoint4 + 2] = u0 + px * X * kr2ud;
1026  P[curPoint4 + 3] = v0 + py * Y * kr2ud;
1027 
1028  r += (vpMath::sqr(P[curPoint4] - Pd[curPoint4]) + vpMath::sqr(P[curPoint4 + 1] - Pd[curPoint4 + 1]) +
1029  vpMath::sqr(P[curPoint4 + 2] - Pd[curPoint4 + 2]) + vpMath::sqr(P[curPoint4 + 3] - Pd[curPoint4 + 3])) *
1030  0.5;
1031 
1032  unsigned int curInd = curPoint4;
1033  //---------------
1034  {
1035  {
1036  L[curInd][q] = px * (-inv_z);
1037  L[curInd][q + 1] = 0;
1038  L[curInd][q + 2] = px * X * inv_z;
1039  L[curInd][q + 3] = px * X * Y;
1040  L[curInd][q + 4] = -px * (1 + X2);
1041  L[curInd][q + 5] = px * Y;
1042  }
1043  {
1044  L[curInd][nbPose6] = 1 + kr2du + k2du * xp02;
1045  L[curInd][nbPose6 + 1] = k2du * up0 * yp0 * inv_py;
1046  if (aspect_ratio > 0.) {
1047  L[curInd][nbPose6 + 2] = X + k2du * xp02 * xp0 + k2du * up0 * yp02 * inv_py;
1048  L[curInd][nbPose6 + 3] = -(up0) * (r2du);
1049  L[curInd][nbPose6 + 4] = 0;
1050  }
1051  else {
1052  L[curInd][nbPose6 + 2] = X + k2du * xp02 * xp0;
1053  L[curInd][nbPose6 + 3] = k2du * up0 * yp02 * inv_py;
1054  L[curInd][nbPose6 + 4] = -(up0) * (r2du);
1055  L[curInd][nbPose6 + 5] = 0;
1056  }
1057  }
1058  curInd++;
1059  {
1060  L[curInd][q] = 0;
1061  L[curInd][q + 1] = py * (-inv_z);
1062  L[curInd][q + 2] = py * Y * inv_z;
1063  L[curInd][q + 3] = py * (1 + Y2);
1064  L[curInd][q + 4] = -py * XY;
1065  L[curInd][q + 5] = -py * X;
1066  }
1067  {
1068  L[curInd][nbPose6] = k2du * xp0 * vp0 * inv_px;
1069  L[curInd][nbPose6 + 1] = 1 + kr2du + k2du * yp02;
1070  if (aspect_ratio > 0.) {
1071  L[curInd][nbPose6 + 2] = k2du * vp0 * xp02 * inv_px + Y + k2du * yp02 * yp0;
1072  L[curInd][nbPose6 + 3] = -vp0 * r2du;
1073  L[curInd][nbPose6 + 4] = 0;
1074  }
1075  else {
1076  L[curInd][nbPose6 + 2] = k2du * vp0 * xp02 * inv_px;
1077  L[curInd][nbPose6 + 3] = Y + k2du * yp02 * yp0;
1078  L[curInd][nbPose6 + 4] = -vp0 * r2du;
1079  L[curInd][nbPose6 + 5] = 0;
1080  }
1081  }
1082  curInd++;
1083  //---undistorted to distorted
1084  {
1085  L[curInd][q] = Axx * (-inv_z);
1086  L[curInd][q + 1] = Axy * (-inv_z);
1087  L[curInd][q + 2] = Axx * (X * inv_z) + Axy * (Y * inv_z);
1088  L[curInd][q + 3] = Axx * X * Y + Axy * (1 + Y2);
1089  L[curInd][q + 4] = -Axx * (1 + X2) - Axy * XY;
1090  L[curInd][q + 5] = Axx * Y - Axy * X;
1091  }
1092  {
1093  L[curInd][nbPose6] = 1;
1094  L[curInd][nbPose6 + 1] = 0;
1095  if (aspect_ratio > 0.) {
1096  L[curInd][nbPose6 + 2] = X * kr2ud;
1097  L[curInd][nbPose6 + 3] = 0;
1098  L[curInd][nbPose6 + 4] = px * X * r2ud;
1099  }
1100  else {
1101  L[curInd][nbPose6 + 2] = X * kr2ud;
1102  L[curInd][nbPose6 + 3] = 0;
1103  L[curInd][nbPose6 + 4] = 0;
1104  L[curInd][nbPose6 + 5] = px * X * r2ud;
1105  }
1106  }
1107  curInd++;
1108  {
1109  L[curInd][q] = Ayx * (-inv_z);
1110  L[curInd][q + 1] = Ayy * (-inv_z);
1111  L[curInd][q + 2] = Ayx * (X * inv_z) + Ayy * (Y * inv_z);
1112  L[curInd][q + 3] = Ayx * XY + Ayy * (1 + Y2);
1113  L[curInd][q + 4] = -Ayx * (1 + X2) - Ayy * XY;
1114  L[curInd][q + 5] = Ayx * Y - Ayy * X;
1115  }
1116  {
1117  L[curInd][nbPose6] = 0;
1118  L[curInd][nbPose6 + 1] = 1;
1119  if (aspect_ratio > 0.) {
1120  L[curInd][nbPose6 + 2] = Y * kr2ud;
1121  L[curInd][nbPose6 + 3] = 0;
1122  L[curInd][nbPose6 + 4] = py * Y * r2ud;
1123  }
1124  else {
1125  L[curInd][nbPose6 + 2] = 0;
1126  L[curInd][nbPose6 + 3] = Y * kr2ud;
1127  L[curInd][nbPose6 + 4] = 0;
1128  L[curInd][nbPose6 + 5] = py * Y * r2ud;
1129  }
1130  }
1131  } // end interaction
1132  curPoint++;
1133  } // end interaction
1134  }
1135 
1136  vpColVector error;
1137  error = P - Pd;
1138  // r = r/nbPointTotal ;
1139 
1140  vpMatrix Lp;
1141  /*double rank =*/
1142  L.pseudoInverse(Lp, 1e-10);
1143  vpColVector e;
1144  e = Lp * error;
1145  vpColVector Tc, Tc_v(6 * nbPose);
1146  Tc = -e * m_gain;
1147  for (unsigned int i = 0; i < 6 * nbPose; i++)
1148  Tc_v[i] = Tc[i];
1149 
1150  if (aspect_ratio > 0.) {
1151  cam_est.initPersProjWithDistortion(px + Tc[nbPose6 + 2], (px + Tc[nbPose6 + 2]) / aspect_ratio, u0 + Tc[nbPose6],
1152  v0 + Tc[nbPose6 + 1], kud + Tc[nbPose6 + 4], kdu + Tc[nbPose6 + 3]);
1153  }
1154  else {
1155  cam_est.initPersProjWithDistortion(px + Tc[nbPose6 + 2], py + Tc[nbPose6 + 3], u0 + Tc[nbPose6],
1156  v0 + Tc[nbPose6 + 1], kud + Tc[nbPose6 + 5], kdu + Tc[nbPose6 + 4]);
1157  }
1158 
1159  vpColVector Tc_v_Tmp(6);
1160  for (unsigned int p = 0; p < nbPose; p++) {
1161  for (unsigned int i = 0; i < 6; i++)
1162  Tc_v_Tmp[i] = Tc_v[6 * p + i];
1163 
1164  table_cal[p].cMo_dist = vpExponentialMap::direct(Tc_v_Tmp).inverse() * table_cal[p].cMo_dist;
1165  }
1166  if (verbose)
1167  std::cout << " std dev: " << sqrt(r / nbPointTotal) << std::endl;
1168  // std::cout << " residual: " << r << std::endl;
1169  }
1170  if (iter == m_nbIterMax) {
1171  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", m_nbIterMax);
1172  throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
1173  }
1174 
1175  // double perViewError;
1176  // double totalError = 0;
1177  // int totalPoints = 0;
1178  for (unsigned int p = 0; p < nbPose; p++) {
1179  table_cal[p].cam_dist = cam_est;
1180  // perViewError =
1181  table_cal[p].computeStdDeviation_dist(table_cal[p].cMo_dist, cam_est);
1182  // totalError += perViewError*perViewError * table_cal[p].npt;
1183  // totalPoints += (int)table_cal[p].npt;
1184  }
1185  globalReprojectionError = sqrt(r / (nbPointTotal));
1186 
1187  if (verbose)
1188  std::cout << " Global std dev " << globalReprojectionError << std::endl;
1189 
1190  // Restore ostream format
1191  std::cout.flags(original_flags);
1192 }
1193 
1194 void vpCalibration::calibVVSMulti(unsigned int nbPose, vpCalibration table_cal[], vpCameraParameters &cam_est,
1195  bool verbose, double aspect_ratio)
1196 {
1197  std::vector<vpCalibration> v_table_cal(nbPose);
1198  double globalReprojectionError = 0;
1199  for (unsigned int i = 0; i < nbPose; i++) {
1200  v_table_cal[i] = table_cal[i];
1201  }
1202 
1203  calibVVSMulti(v_table_cal, cam_est, globalReprojectionError, verbose, aspect_ratio);
1204 
1205  for (unsigned int i = 0; i < nbPose; i++) {
1206  table_cal[i] = v_table_cal[i];
1207  }
1208 }
1209 
1210 void vpCalibration::calibVVSWithDistortionMulti(unsigned int nbPose, vpCalibration table_cal[],
1211  vpCameraParameters &cam_est, bool verbose, double aspect_ratio)
1212 {
1213  std::vector<vpCalibration> v_table_cal(nbPose);
1214  double globalReprojectionError = 0;
1215  for (unsigned int i = 0; i < nbPose; i++) {
1216  v_table_cal[i] = table_cal[i];
1217  }
1218 
1219  calibVVSWithDistortionMulti(v_table_cal, cam_est, globalReprojectionError, verbose, aspect_ratio);
1220 
1221  for (unsigned int i = 0; i < nbPose; i++) {
1222  table_cal[i] = v_table_cal[i];
1223  }
1224 }
1225 
1226 #undef DEBUG_LEVEL1
1227 #undef DEBUG_LEVEL2
Error that can be emitted by the vpCalibration class.
@ convergencyError
Iterative algorithm doesn't converge.
@ notInitializedError
Something is not initialized.
Tools for perspective camera calibration.
Definition: vpCalibration.h:61
void computeStdDeviation(double &deviation, double &deviation_dist)
double m_aspect_ratio
vpHomogeneousMatrix cMo
Definition: vpCalibration.h:86
vpCameraParameters cam_dist
Definition: vpCalibration.h:99
vpHomogeneousMatrix cMo_dist
Definition: vpCalibration.h:91
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
double get_kdu() const
double get_kud() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
static vpHomogeneousMatrix direct(const vpColVector &v)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
double get_u() const
Definition: vpImagePoint.h:136
double get_v() const
Definition: vpImagePoint.h:147
static double sqr(double x)
Definition: vpMath.h:201
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:449
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2132
vpMatrix t() const
Definition: vpMatrix.cpp:465
vpMatrix AtA() const
Definition: vpMatrix.cpp:645
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2343
#define vpERROR_TRACE
Definition: vpDebug.h:382