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