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