Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
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  resul[3] = sqrt(sol[6] * sol[6] + sol[7] * sol[7] + sol[8] * sol[8] /* py */
180  - resul[1] * resul[1]);
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 = 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 = LoX.begin();
232  std::list<double>::const_iterator it_LoY = LoY.begin();
233  std::list<double>::const_iterator it_LoZ = LoZ.begin();
234  std::list<vpImagePoint>::const_iterator it_Lip = 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, threshold) == false && iter < nbIterMax) {
258  iter++;
259  residu_1 = r;
260 
261  double px = cam_est.get_px();
262  double py = cam_est.get_py();
263  double u0 = cam_est.get_u0();
264  double v0 = cam_est.get_v0();
265 
266  r = 0;
267 
268  for (unsigned int i = 0; i < n_points; i++) {
269  cX[i] = oX[i] * cMo_est[0][0] + oY[i] * cMo_est[0][1] + oZ[i] * cMo_est[0][2] + cMo_est[0][3];
270  cY[i] = oX[i] * cMo_est[1][0] + oY[i] * cMo_est[1][1] + oZ[i] * cMo_est[1][2] + cMo_est[1][3];
271  cZ[i] = oX[i] * cMo_est[2][0] + oY[i] * cMo_est[2][1] + oZ[i] * cMo_est[2][2] + cMo_est[2][3];
272 
273  Pd[2 * i] = u[i];
274  Pd[2 * i + 1] = v[i];
275 
276  P[2 * i] = cX[i] / cZ[i] * px + u0;
277  P[2 * i + 1] = cY[i] / cZ[i] * py + v0;
278 
279  r += ((vpMath::sqr(P[2 * i] - Pd[2 * i]) + vpMath::sqr(P[2 * i + 1] - Pd[2 * i + 1])));
280  }
281 
282  vpColVector error;
283  error = P - Pd;
284  // r = r/n_points ;
285 
286  vpMatrix L(n_points * 2, 10);
287  for (unsigned int i = 0; i < n_points; i++) {
288  double x = cX[i];
289  double y = cY[i];
290  double z = cZ[i];
291  double inv_z = 1 / z;
292 
293  double X = x * inv_z;
294  double Y = y * inv_z;
295 
296  //---------------
297  {
298  L[2 * i][0] = px * (-inv_z);
299  L[2 * i][1] = 0;
300  L[2 * i][2] = px * X * inv_z;
301  L[2 * i][3] = px * X * Y;
302  L[2 * i][4] = -px * (1 + X * X);
303  L[2 * i][5] = px * Y;
304  }
305  {
306  L[2 * i][6] = 1;
307  L[2 * i][7] = 0;
308  L[2 * i][8] = X;
309  L[2 * i][9] = 0;
310  }
311  {
312  L[2 * i + 1][0] = 0;
313  L[2 * i + 1][1] = py * (-inv_z);
314  L[2 * i + 1][2] = py * (Y * inv_z);
315  L[2 * i + 1][3] = py * (1 + Y * Y);
316  L[2 * i + 1][4] = -py * X * Y;
317  L[2 * i + 1][5] = -py * X;
318  }
319  {
320  L[2 * i + 1][6] = 0;
321  L[2 * i + 1][7] = 1;
322  L[2 * i + 1][8] = 0;
323  L[2 * i + 1][9] = Y;
324  }
325  } // end interaction
326  vpMatrix Lp;
327  Lp = L.pseudoInverse(1e-10);
328 
329  vpColVector e;
330  e = Lp * error;
331 
332  vpColVector Tc, Tc_v(6);
333  Tc = -e * gain;
334 
335  // Tc_v =0 ;
336  for (unsigned int i = 0; i < 6; i++)
337  Tc_v[i] = Tc[i];
338 
339  cam_est.initPersProjWithoutDistortion(px + Tc[8], py + Tc[9], u0 + Tc[6], v0 + Tc[7]);
340 
341  cMo_est = vpExponentialMap::direct(Tc_v).inverse() * cMo_est;
342  if (verbose)
343  std::cout << " std dev " << sqrt(r / n_points) << std::endl;
344  }
345  if (iter == nbIterMax) {
346  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
347  throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
348  }
349  this->cMo = cMo_est;
350  this->cMo_dist = cMo_est;
351  this->residual = r;
352  this->residual_dist = r;
353  if (verbose)
354  std::cout << " std dev " << sqrt(r / n_points) << std::endl;
355  // Restore ostream format
356  std::cout.flags(original_flags);
357 }
358 
359 void vpCalibration::calibVVSMulti(std::vector<vpCalibration> &table_cal, vpCameraParameters &cam_est,
360  double &globalReprojectionError, bool verbose)
361 {
362  std::ios::fmtflags original_flags(std::cout.flags());
363  std::cout.precision(10);
364  unsigned int nbPoint[256]; // number of points by image
365  unsigned int nbPointTotal = 0; // total number of points
366  unsigned int nbPose = (unsigned int)table_cal.size();
367  unsigned int nbPose6 = 6 * nbPose;
368 
369  for (unsigned int i = 0; i < nbPose; i++) {
370  nbPoint[i] = table_cal[i].npt;
371  nbPointTotal += nbPoint[i];
372  }
373 
374  if (nbPointTotal < 4) {
375  // vpERROR_TRACE("Not enough point to calibrate");
376  throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate"));
377  }
378 
379  vpColVector oX(nbPointTotal), cX(nbPointTotal);
380  vpColVector oY(nbPointTotal), cY(nbPointTotal);
381  vpColVector oZ(nbPointTotal), cZ(nbPointTotal);
382  vpColVector u(nbPointTotal);
383  vpColVector v(nbPointTotal);
384 
385  vpColVector P(2 * nbPointTotal);
386  vpColVector Pd(2 * nbPointTotal);
387  vpImagePoint ip;
388 
389  unsigned int curPoint = 0; // current point indice
390  for (unsigned int p = 0; p < nbPose; p++) {
391  std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
392  std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
393  std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
394  std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
395 
396  for (unsigned int i = 0; i < nbPoint[p]; i++) {
397  oX[curPoint] = *it_LoX;
398  oY[curPoint] = *it_LoY;
399  oZ[curPoint] = *it_LoZ;
400 
401  ip = *it_Lip;
402  u[curPoint] = ip.get_u();
403  v[curPoint] = ip.get_v();
404 
405  ++it_LoX;
406  ++it_LoY;
407  ++it_LoZ;
408  ++it_Lip;
409 
410  curPoint++;
411  }
412  }
413  // double lambda = 0.1 ;
414  unsigned int iter = 0;
415 
416  double residu_1 = 1e12;
417  double r = 1e12 - 1;
418  while (vpMath::equal(residu_1, r, threshold) == false && iter < nbIterMax) {
419 
420  iter++;
421  residu_1 = r;
422 
423  double px = cam_est.get_px();
424  double py = cam_est.get_py();
425  double u0 = cam_est.get_u0();
426  double v0 = cam_est.get_v0();
427 
428  r = 0;
429  curPoint = 0; // current point indice
430  for (unsigned int p = 0; p < nbPose; p++) {
431  vpHomogeneousMatrix cMoTmp = table_cal[p].cMo;
432  for (unsigned int i = 0; i < nbPoint[p]; i++) {
433  unsigned int curPoint2 = 2 * curPoint;
434 
435  cX[curPoint] =
436  oX[curPoint] * cMoTmp[0][0] + oY[curPoint] * cMoTmp[0][1] + oZ[curPoint] * cMoTmp[0][2] + cMoTmp[0][3];
437  cY[curPoint] =
438  oX[curPoint] * cMoTmp[1][0] + oY[curPoint] * cMoTmp[1][1] + oZ[curPoint] * cMoTmp[1][2] + cMoTmp[1][3];
439  cZ[curPoint] =
440  oX[curPoint] * cMoTmp[2][0] + oY[curPoint] * cMoTmp[2][1] + oZ[curPoint] * cMoTmp[2][2] + cMoTmp[2][3];
441 
442  Pd[curPoint2] = u[curPoint];
443  Pd[curPoint2 + 1] = v[curPoint];
444 
445  P[curPoint2] = cX[curPoint] / cZ[curPoint] * px + u0;
446  P[curPoint2 + 1] = cY[curPoint] / cZ[curPoint] * py + v0;
447 
448  r += (vpMath::sqr(P[curPoint2] - Pd[curPoint2]) + vpMath::sqr(P[curPoint2 + 1] - Pd[curPoint2 + 1]));
449  curPoint++;
450  }
451  }
452 
453  vpColVector error;
454  error = P - Pd;
455  // r = r/nbPointTotal ;
456 
457  vpMatrix L(nbPointTotal * 2, nbPose6 + 4);
458  curPoint = 0; // current point indice
459  for (unsigned int p = 0; p < nbPose; p++) {
460  unsigned int q = 6 * p;
461  for (unsigned int i = 0; i < nbPoint[p]; i++) {
462  unsigned int curPoint2 = 2 * curPoint;
463  unsigned int curPoint21 = curPoint2 + 1;
464 
465  double x = cX[curPoint];
466  double y = cY[curPoint];
467  double z = cZ[curPoint];
468 
469  double inv_z = 1 / z;
470 
471  double X = x * inv_z;
472  double Y = y * inv_z;
473 
474  //---------------
475  {
476  {
477  L[curPoint2][q] = px * (-inv_z);
478  L[curPoint2][q + 1] = 0;
479  L[curPoint2][q + 2] = px * (X * inv_z);
480  L[curPoint2][q + 3] = px * X * Y;
481  L[curPoint2][q + 4] = -px * (1 + X * X);
482  L[curPoint2][q + 5] = px * Y;
483  }
484  {
485  L[curPoint2][nbPose6] = 1;
486  L[curPoint2][nbPose6 + 1] = 0;
487  L[curPoint2][nbPose6 + 2] = X;
488  L[curPoint2][nbPose6 + 3] = 0;
489  }
490  {
491  L[curPoint21][q] = 0;
492  L[curPoint21][q + 1] = py * (-inv_z);
493  L[curPoint21][q + 2] = py * (Y * inv_z);
494  L[curPoint21][q + 3] = py * (1 + Y * Y);
495  L[curPoint21][q + 4] = -py * X * Y;
496  L[curPoint21][q + 5] = -py * X;
497  }
498  {
499  L[curPoint21][nbPose6] = 0;
500  L[curPoint21][nbPose6 + 1] = 1;
501  L[curPoint21][nbPose6 + 2] = 0;
502  L[curPoint21][nbPose6 + 3] = Y;
503  }
504  }
505  curPoint++;
506  } // end interaction
507  }
508  vpMatrix Lp;
509  Lp = L.pseudoInverse(1e-10);
510 
511  vpColVector e;
512  e = Lp * error;
513 
514  vpColVector Tc, Tc_v(nbPose6);
515  Tc = -e * gain;
516 
517  // Tc_v =0 ;
518  for (unsigned int i = 0; i < nbPose6; i++)
519  Tc_v[i] = Tc[i];
520 
521  cam_est.initPersProjWithoutDistortion(px + Tc[nbPose6 + 2], py + Tc[nbPose6 + 3], u0 + Tc[nbPose6],
522  v0 + Tc[nbPose6 + 1]);
523 
524  // cam.setKd(get_kd() + Tc[10]) ;
525  vpColVector Tc_v_Tmp(6);
526 
527  for (unsigned int p = 0; p < nbPose; p++) {
528  for (unsigned int i = 0; i < 6; i++)
529  Tc_v_Tmp[i] = Tc_v[6 * p + i];
530 
531  table_cal[p].cMo = vpExponentialMap::direct(Tc_v_Tmp, 1).inverse() * table_cal[p].cMo;
532  }
533 
534  if (verbose)
535  std::cout << " std dev " << sqrt(r / nbPointTotal) << std::endl;
536  }
537  if (iter == nbIterMax) {
538  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
539  throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
540  }
541  for (unsigned int p = 0; p < nbPose; p++) {
542  table_cal[p].cMo_dist = table_cal[p].cMo;
543  table_cal[p].cam = cam_est;
544  table_cal[p].cam_dist = cam_est;
545  double deviation, deviation_dist;
546  table_cal[p].computeStdDeviation(deviation, deviation_dist);
547  }
548  globalReprojectionError = sqrt(r / nbPointTotal);
549  // Restore ostream format
550  std::cout.flags(original_flags);
551 }
552 
553 void vpCalibration::calibVVSWithDistortion(vpCameraParameters &cam_est, vpHomogeneousMatrix &cMo_est, bool verbose)
554 {
555  std::ios::fmtflags original_flags(std::cout.flags());
556  std::cout.precision(10);
557  unsigned int n_points = npt;
558 
559  vpColVector oX(n_points), cX(n_points);
560  vpColVector oY(n_points), cY(n_points);
561  vpColVector oZ(n_points), cZ(n_points);
562  vpColVector u(n_points);
563  vpColVector v(n_points);
564 
565  vpColVector P(4 * n_points);
566  vpColVector Pd(4 * n_points);
567 
568  std::list<double>::const_iterator it_LoX = LoX.begin();
569  std::list<double>::const_iterator it_LoY = LoY.begin();
570  std::list<double>::const_iterator it_LoZ = LoZ.begin();
571  std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
572 
573  vpImagePoint ip;
574 
575  for (unsigned int i = 0; i < n_points; i++) {
576  oX[i] = *it_LoX;
577  oY[i] = *it_LoY;
578  oZ[i] = *it_LoZ;
579 
580  ip = *it_Lip;
581  u[i] = ip.get_u();
582  v[i] = ip.get_v();
583 
584  ++it_LoX;
585  ++it_LoY;
586  ++it_LoZ;
587  ++it_Lip;
588  }
589 
590  // double lambda = 0.1 ;
591  unsigned int iter = 0;
592 
593  double residu_1 = 1e12;
594  double r = 1e12 - 1;
595  while (vpMath::equal(residu_1, r, threshold) == false && iter < nbIterMax) {
596  iter++;
597  residu_1 = r;
598 
599  r = 0;
600  double u0 = cam_est.get_u0();
601  double v0 = cam_est.get_v0();
602 
603  double px = cam_est.get_px();
604  double py = cam_est.get_py();
605 
606  double inv_px = 1 / px;
607  double inv_py = 1 / py;
608 
609  double kud = cam_est.get_kud();
610  double kdu = cam_est.get_kdu();
611 
612  double k2ud = 2 * kud;
613  double k2du = 2 * kdu;
614  vpMatrix L(n_points * 4, 12);
615 
616  for (unsigned int i = 0; i < n_points; i++) {
617  unsigned int i4 = 4 * i;
618  unsigned int i41 = 4 * i + 1;
619  unsigned int i42 = 4 * i + 2;
620  unsigned int i43 = 4 * i + 3;
621 
622  cX[i] = oX[i] * cMo_est[0][0] + oY[i] * cMo_est[0][1] + oZ[i] * cMo_est[0][2] + cMo_est[0][3];
623  cY[i] = oX[i] * cMo_est[1][0] + oY[i] * cMo_est[1][1] + oZ[i] * cMo_est[1][2] + cMo_est[1][3];
624  cZ[i] = oX[i] * cMo_est[2][0] + oY[i] * cMo_est[2][1] + oZ[i] * cMo_est[2][2] + cMo_est[2][3];
625 
626  double x = cX[i];
627  double y = cY[i];
628  double z = cZ[i];
629  double inv_z = 1 / z;
630 
631  double X = x * inv_z;
632  double Y = y * inv_z;
633 
634  double X2 = X * X;
635  double Y2 = Y * Y;
636  double XY = X * Y;
637 
638  double up = u[i];
639  double vp = v[i];
640 
641  Pd[i4] = up;
642  Pd[i41] = vp;
643 
644  double up0 = up - u0;
645  double vp0 = vp - v0;
646 
647  double xp0 = up0 * inv_px;
648  double xp02 = xp0 * xp0;
649 
650  double yp0 = vp0 * inv_py;
651  double yp02 = yp0 * yp0;
652 
653  double r2du = xp02 + yp02;
654  double kr2du = kdu * r2du;
655 
656  P[i4] = u0 + px * X - kr2du * (up0);
657  P[i41] = v0 + py * Y - kr2du * (vp0);
658 
659  double r2ud = X2 + Y2;
660  double kr2ud = 1 + kud * r2ud;
661 
662  double Axx = px * (kr2ud + k2ud * X2);
663  double Axy = px * k2ud * XY;
664  double Ayy = py * (kr2ud + k2ud * Y2);
665  double Ayx = py * k2ud * XY;
666 
667  Pd[i42] = up;
668  Pd[i43] = vp;
669 
670  P[i42] = u0 + px * X * kr2ud;
671  P[i43] = v0 + py * Y * kr2ud;
672 
673  r += (vpMath::sqr(P[i4] - Pd[i4]) + vpMath::sqr(P[i41] - Pd[i41]) + vpMath::sqr(P[i42] - Pd[i42]) +
674  vpMath::sqr(P[i43] - Pd[i43])) *
675  0.5;
676 
677  //--distorted to undistorted
678  {
679  {
680  L[i4][0] = px * (-inv_z);
681  L[i4][1] = 0;
682  L[i4][2] = px * X * inv_z;
683  L[i4][3] = px * X * Y;
684  L[i4][4] = -px * (1 + X2);
685  L[i4][5] = px * Y;
686  }
687  {
688  L[i4][6] = 1 + kr2du + k2du * xp02;
689  L[i4][7] = k2du * up0 * yp0 * inv_py;
690  L[i4][8] = X + k2du * xp02 * xp0;
691  L[i4][9] = k2du * up0 * yp02 * inv_py;
692  L[i4][10] = -(up0) * (r2du);
693  L[i4][11] = 0;
694  }
695  {
696  L[i41][0] = 0;
697  L[i41][1] = py * (-inv_z);
698  L[i41][2] = py * Y * inv_z;
699  L[i41][3] = py * (1 + Y2);
700  L[i41][4] = -py * XY;
701  L[i41][5] = -py * X;
702  }
703  {
704  L[i41][6] = k2du * xp0 * vp0 * inv_px;
705  L[i41][7] = 1 + kr2du + k2du * yp02;
706  L[i41][8] = k2du * vp0 * xp02 * inv_px;
707  L[i41][9] = Y + k2du * yp02 * yp0;
708  L[i41][10] = -vp0 * r2du;
709  L[i41][11] = 0;
710  }
711  //---undistorted to distorted
712  {
713  L[i42][0] = Axx * (-inv_z);
714  L[i42][1] = Axy * (-inv_z);
715  L[i42][2] = Axx * (X * inv_z) + Axy * (Y * inv_z);
716  L[i42][3] = Axx * X * Y + Axy * (1 + Y2);
717  L[i42][4] = -Axx * (1 + X2) - Axy * XY;
718  L[i42][5] = Axx * Y - Axy * X;
719  }
720  {
721  L[i42][6] = 1;
722  L[i42][7] = 0;
723  L[i42][8] = X * kr2ud;
724  L[i42][9] = 0;
725  L[i42][10] = 0;
726  L[i42][11] = px * X * r2ud;
727  }
728  {
729  L[i43][0] = Ayx * (-inv_z);
730  L[i43][1] = Ayy * (-inv_z);
731  L[i43][2] = Ayx * (X * inv_z) + Ayy * (Y * inv_z);
732  L[i43][3] = Ayx * XY + Ayy * (1 + Y2);
733  L[i43][4] = -Ayx * (1 + X2) - Ayy * XY;
734  L[i43][5] = Ayx * Y - Ayy * X;
735  }
736  {
737  L[i43][6] = 0;
738  L[i43][7] = 1;
739  L[i43][8] = 0;
740  L[i43][9] = Y * kr2ud;
741  L[i43][10] = 0;
742  L[i43][11] = py * Y * r2ud;
743  }
744  } // end interaction
745  } // end interaction
746 
747  vpColVector error;
748  error = P - Pd;
749  // r = r/n_points ;
750 
751  vpMatrix Lp;
752  Lp = L.pseudoInverse(1e-10);
753 
754  vpColVector e;
755  e = Lp * error;
756 
757  vpColVector Tc, Tc_v(6);
758  Tc = -e * gain;
759 
760  for (unsigned int i = 0; i < 6; i++)
761  Tc_v[i] = Tc[i];
762 
763  cam_est.initPersProjWithDistortion(px + Tc[8], py + Tc[9], u0 + Tc[6], v0 + Tc[7], kud + Tc[11], kdu + Tc[10]);
764 
765  cMo_est = vpExponentialMap::direct(Tc_v).inverse() * cMo_est;
766  if (verbose)
767  std::cout << " std dev " << sqrt(r / n_points) << std::endl;
768  }
769  if (iter == nbIterMax) {
770  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
771  throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
772  }
773  this->residual_dist = r;
774  this->cMo_dist = cMo_est;
775  this->cam_dist = cam_est;
776 
777  if (verbose)
778  std::cout << " std dev " << sqrt(r / n_points) << std::endl;
779 
780  // Restore ostream format
781  std::cout.flags(original_flags);
782 }
783 
784 void vpCalibration::calibVVSWithDistortionMulti(std::vector<vpCalibration> &table_cal, vpCameraParameters &cam_est,
785  double &globalReprojectionError, bool verbose)
786 {
787  std::ios::fmtflags original_flags(std::cout.flags());
788  std::cout.precision(10);
789  unsigned int nbPoint[1024]; // number of points by image
790  unsigned int nbPointTotal = 0; // total number of points
791  unsigned int nbPose = (unsigned int)table_cal.size();
792  unsigned int nbPose6 = 6 * nbPose;
793  for (unsigned int i = 0; i < nbPose; i++) {
794  nbPoint[i] = table_cal[i].npt;
795  nbPointTotal += nbPoint[i];
796  }
797 
798  if (nbPointTotal < 4) {
799  // vpERROR_TRACE("Not enough point to calibrate");
800  throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate"));
801  }
802 
803  vpColVector oX(nbPointTotal), cX(nbPointTotal);
804  vpColVector oY(nbPointTotal), cY(nbPointTotal);
805  vpColVector oZ(nbPointTotal), cZ(nbPointTotal);
806  vpColVector u(nbPointTotal);
807  vpColVector v(nbPointTotal);
808 
809  vpColVector P(4 * nbPointTotal);
810  vpColVector Pd(4 * nbPointTotal);
811  vpImagePoint ip;
812 
813  unsigned int curPoint = 0; // current point indice
814  for (unsigned int p = 0; p < nbPose; p++) {
815  std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
816  std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
817  std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
818  std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
819 
820  for (unsigned int i = 0; i < nbPoint[p]; i++) {
821  oX[curPoint] = *it_LoX;
822  oY[curPoint] = *it_LoY;
823  oZ[curPoint] = *it_LoZ;
824 
825  ip = *it_Lip;
826  u[curPoint] = ip.get_u();
827  v[curPoint] = ip.get_v();
828 
829  ++it_LoX;
830  ++it_LoY;
831  ++it_LoZ;
832  ++it_Lip;
833  curPoint++;
834  }
835  }
836  // double lambda = 0.1 ;
837  unsigned int iter = 0;
838 
839  double residu_1 = 1e12;
840  double r = 1e12 - 1;
841  while (vpMath::equal(residu_1, r, threshold) == false && iter < nbIterMax) {
842  iter++;
843  residu_1 = r;
844 
845  r = 0;
846  curPoint = 0; // current point indice
847  for (unsigned int p = 0; p < nbPose; p++) {
848  vpHomogeneousMatrix cMoTmp = table_cal[p].cMo_dist;
849  for (unsigned int i = 0; i < nbPoint[p]; i++) {
850  cX[curPoint] =
851  oX[curPoint] * cMoTmp[0][0] + oY[curPoint] * cMoTmp[0][1] + oZ[curPoint] * cMoTmp[0][2] + cMoTmp[0][3];
852  cY[curPoint] =
853  oX[curPoint] * cMoTmp[1][0] + oY[curPoint] * cMoTmp[1][1] + oZ[curPoint] * cMoTmp[1][2] + cMoTmp[1][3];
854  cZ[curPoint] =
855  oX[curPoint] * cMoTmp[2][0] + oY[curPoint] * cMoTmp[2][1] + oZ[curPoint] * cMoTmp[2][2] + cMoTmp[2][3];
856 
857  curPoint++;
858  }
859  }
860 
861  vpMatrix L(nbPointTotal * 4, nbPose6 + 6);
862  curPoint = 0; // current point indice
863  double px = cam_est.get_px();
864  double py = cam_est.get_py();
865  double u0 = cam_est.get_u0();
866  double v0 = cam_est.get_v0();
867 
868  double inv_px = 1 / px;
869  double inv_py = 1 / py;
870 
871  double kud = cam_est.get_kud();
872  double kdu = cam_est.get_kdu();
873 
874  double k2ud = 2 * kud;
875  double k2du = 2 * kdu;
876 
877  for (unsigned int p = 0; p < nbPose; p++) {
878  unsigned int q = 6 * p;
879  for (unsigned int i = 0; i < nbPoint[p]; i++) {
880  unsigned int curPoint4 = 4 * curPoint;
881  double x = cX[curPoint];
882  double y = cY[curPoint];
883  double z = cZ[curPoint];
884 
885  double inv_z = 1 / z;
886  double X = x * inv_z;
887  double Y = y * inv_z;
888 
889  double X2 = X * X;
890  double Y2 = Y * Y;
891  double XY = X * Y;
892 
893  double up = u[curPoint];
894  double vp = v[curPoint];
895 
896  Pd[curPoint4] = up;
897  Pd[curPoint4 + 1] = vp;
898 
899  double up0 = up - u0;
900  double vp0 = vp - v0;
901 
902  double xp0 = up0 * inv_px;
903  double xp02 = xp0 * xp0;
904 
905  double yp0 = vp0 * inv_py;
906  double yp02 = yp0 * yp0;
907 
908  double r2du = xp02 + yp02;
909  double kr2du = kdu * r2du;
910 
911  P[curPoint4] = u0 + px * X - kr2du * (up0);
912  P[curPoint4 + 1] = v0 + py * Y - kr2du * (vp0);
913 
914  double r2ud = X2 + Y2;
915  double kr2ud = 1 + kud * r2ud;
916 
917  double Axx = px * (kr2ud + k2ud * X2);
918  double Axy = px * k2ud * XY;
919  double Ayy = py * (kr2ud + k2ud * Y2);
920  double Ayx = py * k2ud * XY;
921 
922  Pd[curPoint4 + 2] = up;
923  Pd[curPoint4 + 3] = vp;
924 
925  P[curPoint4 + 2] = u0 + px * X * kr2ud;
926  P[curPoint4 + 3] = v0 + py * Y * kr2ud;
927 
928  r += (vpMath::sqr(P[curPoint4] - Pd[curPoint4]) + vpMath::sqr(P[curPoint4 + 1] - Pd[curPoint4 + 1]) +
929  vpMath::sqr(P[curPoint4 + 2] - Pd[curPoint4 + 2]) + vpMath::sqr(P[curPoint4 + 3] - Pd[curPoint4 + 3])) *
930  0.5;
931 
932  unsigned int curInd = curPoint4;
933  //---------------
934  {
935  {
936  L[curInd][q] = px * (-inv_z);
937  L[curInd][q + 1] = 0;
938  L[curInd][q + 2] = px * X * inv_z;
939  L[curInd][q + 3] = px * X * Y;
940  L[curInd][q + 4] = -px * (1 + X2);
941  L[curInd][q + 5] = px * Y;
942  }
943  {
944  L[curInd][nbPose6] = 1 + kr2du + k2du * xp02;
945  L[curInd][nbPose6 + 1] = k2du * up0 * yp0 * inv_py;
946  L[curInd][nbPose6 + 2] = X + k2du * xp02 * xp0;
947  L[curInd][nbPose6 + 3] = k2du * up0 * yp02 * inv_py;
948  L[curInd][nbPose6 + 4] = -(up0) * (r2du);
949  L[curInd][nbPose6 + 5] = 0;
950  }
951  curInd++;
952  {
953  L[curInd][q] = 0;
954  L[curInd][q + 1] = py * (-inv_z);
955  L[curInd][q + 2] = py * Y * inv_z;
956  L[curInd][q + 3] = py * (1 + Y2);
957  L[curInd][q + 4] = -py * XY;
958  L[curInd][q + 5] = -py * X;
959  }
960  {
961  L[curInd][nbPose6] = k2du * xp0 * vp0 * inv_px;
962  L[curInd][nbPose6 + 1] = 1 + kr2du + k2du * yp02;
963  L[curInd][nbPose6 + 2] = k2du * vp0 * xp02 * inv_px;
964  L[curInd][nbPose6 + 3] = Y + k2du * yp02 * yp0;
965  L[curInd][nbPose6 + 4] = -vp0 * r2du;
966  L[curInd][nbPose6 + 5] = 0;
967  }
968  curInd++;
969  //---undistorted to distorted
970  {
971  L[curInd][q] = Axx * (-inv_z);
972  L[curInd][q + 1] = Axy * (-inv_z);
973  L[curInd][q + 2] = Axx * (X * inv_z) + Axy * (Y * inv_z);
974  L[curInd][q + 3] = Axx * X * Y + Axy * (1 + Y2);
975  L[curInd][q + 4] = -Axx * (1 + X2) - Axy * XY;
976  L[curInd][q + 5] = Axx * Y - Axy * X;
977  }
978  {
979  L[curInd][nbPose6] = 1;
980  L[curInd][nbPose6 + 1] = 0;
981  L[curInd][nbPose6 + 2] = X * kr2ud;
982  L[curInd][nbPose6 + 3] = 0;
983  L[curInd][nbPose6 + 4] = 0;
984  L[curInd][nbPose6 + 5] = px * X * r2ud;
985  }
986  curInd++;
987  {
988  L[curInd][q] = Ayx * (-inv_z);
989  L[curInd][q + 1] = Ayy * (-inv_z);
990  L[curInd][q + 2] = Ayx * (X * inv_z) + Ayy * (Y * inv_z);
991  L[curInd][q + 3] = Ayx * XY + Ayy * (1 + Y2);
992  L[curInd][q + 4] = -Ayx * (1 + X2) - Ayy * XY;
993  L[curInd][q + 5] = Ayx * Y - Ayy * X;
994  }
995  {
996  L[curInd][nbPose6] = 0;
997  L[curInd][nbPose6 + 1] = 1;
998  L[curInd][nbPose6 + 2] = 0;
999  L[curInd][nbPose6 + 3] = Y * kr2ud;
1000  L[curInd][nbPose6 + 4] = 0;
1001  L[curInd][nbPose6 + 5] = py * Y * r2ud;
1002  }
1003  } // end interaction
1004  curPoint++;
1005  } // end interaction
1006  }
1007 
1008  vpColVector error;
1009  error = P - Pd;
1010  // r = r/nbPointTotal ;
1011 
1012  vpMatrix Lp;
1013  /*double rank =*/
1014  L.pseudoInverse(Lp, 1e-10);
1015  vpColVector e;
1016  e = Lp * error;
1017  vpColVector Tc, Tc_v(6 * nbPose);
1018  Tc = -e * gain;
1019  for (unsigned int i = 0; i < 6 * nbPose; i++)
1020  Tc_v[i] = Tc[i];
1021 
1022  cam_est.initPersProjWithDistortion(px + Tc[nbPose6 + 2], py + Tc[nbPose6 + 3], u0 + Tc[nbPose6],
1023  v0 + Tc[nbPose6 + 1], kud + Tc[nbPose6 + 5], kdu + Tc[nbPose6 + 4]);
1024 
1025  vpColVector Tc_v_Tmp(6);
1026  for (unsigned int p = 0; p < nbPose; p++) {
1027  for (unsigned int i = 0; i < 6; i++)
1028  Tc_v_Tmp[i] = Tc_v[6 * p + i];
1029 
1030  table_cal[p].cMo_dist = vpExponentialMap::direct(Tc_v_Tmp).inverse() * table_cal[p].cMo_dist;
1031  }
1032  if (verbose)
1033  std::cout << " std dev: " << sqrt(r / nbPointTotal) << std::endl;
1034  // std::cout << " residual: " << r << std::endl;
1035  }
1036  if (iter == nbIterMax) {
1037  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
1038  throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
1039  }
1040 
1041  // double perViewError;
1042  // double totalError = 0;
1043  // int totalPoints = 0;
1044  for (unsigned int p = 0; p < nbPose; p++) {
1045  table_cal[p].cam_dist = cam_est;
1046  // perViewError =
1047  // table_cal[p].computeStdDeviation_dist(table_cal[p].cMo_dist, cam_est);
1048  // totalError += perViewError*perViewError * table_cal[p].npt;
1049  // totalPoints += (int)table_cal[p].npt;
1050  }
1051  globalReprojectionError = sqrt(r / (nbPointTotal));
1052 
1053  // Restore ostream format
1054  std::cout.flags(original_flags);
1055 }
1056 
1057 void vpCalibration::calibVVSMulti(unsigned int nbPose, vpCalibration table_cal[], vpCameraParameters &cam_est,
1058  bool verbose)
1059 {
1060  std::ios::fmtflags original_flags(std::cout.flags());
1061  std::cout.precision(10);
1062  unsigned int nbPoint[256]; // number of points by image
1063  unsigned int nbPointTotal = 0; // total number of points
1064 
1065  unsigned int nbPose6 = 6 * nbPose;
1066 
1067  for (unsigned int i = 0; i < nbPose; i++) {
1068  nbPoint[i] = table_cal[i].npt;
1069  nbPointTotal += nbPoint[i];
1070  }
1071 
1072  if (nbPointTotal < 4) {
1073  // vpERROR_TRACE("Not enough point to calibrate");
1074  throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate"));
1075  }
1076 
1077  vpColVector oX(nbPointTotal), cX(nbPointTotal);
1078  vpColVector oY(nbPointTotal), cY(nbPointTotal);
1079  vpColVector oZ(nbPointTotal), cZ(nbPointTotal);
1080  vpColVector u(nbPointTotal);
1081  vpColVector v(nbPointTotal);
1082 
1083  vpColVector P(2 * nbPointTotal);
1084  vpColVector Pd(2 * nbPointTotal);
1085  vpImagePoint ip;
1086 
1087  unsigned int curPoint = 0; // current point indice
1088  for (unsigned int p = 0; p < nbPose; p++) {
1089  std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
1090  std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
1091  std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
1092  std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
1093 
1094  for (unsigned int i = 0; i < nbPoint[p]; i++) {
1095  oX[curPoint] = *it_LoX;
1096  oY[curPoint] = *it_LoY;
1097  oZ[curPoint] = *it_LoZ;
1098 
1099  ip = *it_Lip;
1100  u[curPoint] = ip.get_u();
1101  v[curPoint] = ip.get_v();
1102 
1103  ++it_LoX;
1104  ++it_LoY;
1105  ++it_LoZ;
1106  ++it_Lip;
1107 
1108  curPoint++;
1109  }
1110  }
1111  // double lambda = 0.1 ;
1112  unsigned int iter = 0;
1113 
1114  double residu_1 = 1e12;
1115  double r = 1e12 - 1;
1116  while (vpMath::equal(residu_1, r, threshold) == false && iter < nbIterMax) {
1117 
1118  iter++;
1119  residu_1 = r;
1120 
1121  double px = cam_est.get_px();
1122  double py = cam_est.get_py();
1123  double u0 = cam_est.get_u0();
1124  double v0 = cam_est.get_v0();
1125 
1126  r = 0;
1127  curPoint = 0; // current point indice
1128  for (unsigned int p = 0; p < nbPose; p++) {
1129  vpHomogeneousMatrix cMoTmp = table_cal[p].cMo;
1130  for (unsigned int i = 0; i < nbPoint[p]; i++) {
1131  unsigned int curPoint2 = 2 * curPoint;
1132 
1133  cX[curPoint] =
1134  oX[curPoint] * cMoTmp[0][0] + oY[curPoint] * cMoTmp[0][1] + oZ[curPoint] * cMoTmp[0][2] + cMoTmp[0][3];
1135  cY[curPoint] =
1136  oX[curPoint] * cMoTmp[1][0] + oY[curPoint] * cMoTmp[1][1] + oZ[curPoint] * cMoTmp[1][2] + cMoTmp[1][3];
1137  cZ[curPoint] =
1138  oX[curPoint] * cMoTmp[2][0] + oY[curPoint] * cMoTmp[2][1] + oZ[curPoint] * cMoTmp[2][2] + cMoTmp[2][3];
1139 
1140  Pd[curPoint2] = u[curPoint];
1141  Pd[curPoint2 + 1] = v[curPoint];
1142 
1143  P[curPoint2] = cX[curPoint] / cZ[curPoint] * px + u0;
1144  P[curPoint2 + 1] = cY[curPoint] / cZ[curPoint] * py + v0;
1145 
1146  r += (vpMath::sqr(P[curPoint2] - Pd[curPoint2]) + vpMath::sqr(P[curPoint2 + 1] - Pd[curPoint2 + 1]));
1147  curPoint++;
1148  }
1149  }
1150 
1151  vpColVector error;
1152  error = P - Pd;
1153  // r = r/nbPointTotal ;
1154 
1155  vpMatrix L(nbPointTotal * 2, nbPose6 + 4);
1156  curPoint = 0; // current point indice
1157  for (unsigned int p = 0; p < nbPose; p++) {
1158  unsigned int q = 6 * p;
1159  for (unsigned int i = 0; i < nbPoint[p]; i++) {
1160  unsigned int curPoint2 = 2 * curPoint;
1161  unsigned int curPoint21 = curPoint2 + 1;
1162 
1163  double x = cX[curPoint];
1164  double y = cY[curPoint];
1165  double z = cZ[curPoint];
1166 
1167  double inv_z = 1 / z;
1168 
1169  double X = x * inv_z;
1170  double Y = y * inv_z;
1171 
1172  //---------------
1173  {
1174  {
1175  L[curPoint2][q] = px * (-inv_z);
1176  L[curPoint2][q + 1] = 0;
1177  L[curPoint2][q + 2] = px * (X * inv_z);
1178  L[curPoint2][q + 3] = px * X * Y;
1179  L[curPoint2][q + 4] = -px * (1 + X * X);
1180  L[curPoint2][q + 5] = px * Y;
1181  }
1182  {
1183  L[curPoint2][nbPose6] = 1;
1184  L[curPoint2][nbPose6 + 1] = 0;
1185  L[curPoint2][nbPose6 + 2] = X;
1186  L[curPoint2][nbPose6 + 3] = 0;
1187  }
1188  {
1189  L[curPoint21][q] = 0;
1190  L[curPoint21][q + 1] = py * (-inv_z);
1191  L[curPoint21][q + 2] = py * (Y * inv_z);
1192  L[curPoint21][q + 3] = py * (1 + Y * Y);
1193  L[curPoint21][q + 4] = -py * X * Y;
1194  L[curPoint21][q + 5] = -py * X;
1195  }
1196  {
1197  L[curPoint21][nbPose6] = 0;
1198  L[curPoint21][nbPose6 + 1] = 1;
1199  L[curPoint21][nbPose6 + 2] = 0;
1200  L[curPoint21][nbPose6 + 3] = Y;
1201  }
1202  }
1203  curPoint++;
1204  } // end interaction
1205  }
1206  vpMatrix Lp;
1207  Lp = L.pseudoInverse(1e-10);
1208 
1209  vpColVector e;
1210  e = Lp * error;
1211 
1212  vpColVector Tc, Tc_v(nbPose6);
1213  Tc = -e * gain;
1214 
1215  // Tc_v =0 ;
1216  for (unsigned int i = 0; i < nbPose6; i++)
1217  Tc_v[i] = Tc[i];
1218 
1219  cam_est.initPersProjWithoutDistortion(px + Tc[nbPose6 + 2], py + Tc[nbPose6 + 3], u0 + Tc[nbPose6],
1220  v0 + Tc[nbPose6 + 1]);
1221 
1222  // cam.setKd(get_kd() + Tc[10]) ;
1223  vpColVector Tc_v_Tmp(6);
1224 
1225  for (unsigned int p = 0; p < nbPose; p++) {
1226  for (unsigned int i = 0; i < 6; i++)
1227  Tc_v_Tmp[i] = Tc_v[6 * p + i];
1228 
1229  table_cal[p].cMo = vpExponentialMap::direct(Tc_v_Tmp, 1).inverse() * table_cal[p].cMo;
1230  }
1231  if (verbose)
1232  std::cout << " std dev " << sqrt(r / nbPointTotal) << std::endl;
1233  }
1234  if (iter == nbIterMax) {
1235  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
1236  throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
1237  }
1238  for (unsigned int p = 0; p < nbPose; p++) {
1239  table_cal[p].cMo_dist = table_cal[p].cMo;
1240  table_cal[p].cam = cam_est;
1241  table_cal[p].cam_dist = cam_est;
1242  double deviation, deviation_dist;
1243  table_cal[p].computeStdDeviation(deviation, deviation_dist);
1244  }
1245  if (verbose)
1246  std::cout << " Global std dev " << sqrt(r / nbPointTotal) << std::endl;
1247 
1248  // Restore ostream format
1249  std::cout.flags(original_flags);
1250 }
1251 
1252 void vpCalibration::calibVVSWithDistortionMulti(unsigned int nbPose, vpCalibration table_cal[],
1253  vpCameraParameters &cam_est, bool verbose)
1254 {
1255  std::ios::fmtflags original_flags(std::cout.flags());
1256  std::cout.precision(10);
1257  unsigned int nbPoint[1024]; // number of points by image
1258  unsigned int nbPointTotal = 0; // total number of points
1259 
1260  unsigned int nbPose6 = 6 * nbPose;
1261  for (unsigned int i = 0; i < nbPose; i++) {
1262  nbPoint[i] = table_cal[i].npt;
1263  nbPointTotal += nbPoint[i];
1264  }
1265 
1266  if (nbPointTotal < 4) {
1267  // vpERROR_TRACE("Not enough point to calibrate");
1268  throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate"));
1269  }
1270 
1271  vpColVector oX(nbPointTotal), cX(nbPointTotal);
1272  vpColVector oY(nbPointTotal), cY(nbPointTotal);
1273  vpColVector oZ(nbPointTotal), cZ(nbPointTotal);
1274  vpColVector u(nbPointTotal);
1275  vpColVector v(nbPointTotal);
1276 
1277  vpColVector P(4 * nbPointTotal);
1278  vpColVector Pd(4 * nbPointTotal);
1279  vpImagePoint ip;
1280 
1281  unsigned int curPoint = 0; // current point indice
1282  for (unsigned int p = 0; p < nbPose; p++) {
1283  std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
1284  std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
1285  std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
1286  std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
1287 
1288  for (unsigned int i = 0; i < nbPoint[p]; i++) {
1289  oX[curPoint] = *it_LoX;
1290  oY[curPoint] = *it_LoY;
1291  oZ[curPoint] = *it_LoZ;
1292 
1293  ip = *it_Lip;
1294  u[curPoint] = ip.get_u();
1295  v[curPoint] = ip.get_v();
1296 
1297  ++it_LoX;
1298  ++it_LoY;
1299  ++it_LoZ;
1300  ++it_Lip;
1301  curPoint++;
1302  }
1303  }
1304  // double lambda = 0.1 ;
1305  unsigned int iter = 0;
1306 
1307  double residu_1 = 1e12;
1308  double r = 1e12 - 1;
1309  while (vpMath::equal(residu_1, r, threshold) == false && iter < nbIterMax) {
1310  iter++;
1311  residu_1 = r;
1312 
1313  r = 0;
1314  curPoint = 0; // current point indice
1315  for (unsigned int p = 0; p < nbPose; p++) {
1316  vpHomogeneousMatrix cMoTmp = table_cal[p].cMo_dist;
1317  for (unsigned int i = 0; i < nbPoint[p]; i++) {
1318  cX[curPoint] =
1319  oX[curPoint] * cMoTmp[0][0] + oY[curPoint] * cMoTmp[0][1] + oZ[curPoint] * cMoTmp[0][2] + cMoTmp[0][3];
1320  cY[curPoint] =
1321  oX[curPoint] * cMoTmp[1][0] + oY[curPoint] * cMoTmp[1][1] + oZ[curPoint] * cMoTmp[1][2] + cMoTmp[1][3];
1322  cZ[curPoint] =
1323  oX[curPoint] * cMoTmp[2][0] + oY[curPoint] * cMoTmp[2][1] + oZ[curPoint] * cMoTmp[2][2] + cMoTmp[2][3];
1324 
1325  curPoint++;
1326  }
1327  }
1328 
1329  vpMatrix L(nbPointTotal * 4, nbPose6 + 6);
1330  curPoint = 0; // current point indice
1331  double px = cam_est.get_px();
1332  double py = cam_est.get_py();
1333  double u0 = cam_est.get_u0();
1334  double v0 = cam_est.get_v0();
1335 
1336  double inv_px = 1 / px;
1337  double inv_py = 1 / py;
1338 
1339  double kud = cam_est.get_kud();
1340  double kdu = cam_est.get_kdu();
1341 
1342  double k2ud = 2 * kud;
1343  double k2du = 2 * kdu;
1344 
1345  for (unsigned int p = 0; p < nbPose; p++) {
1346  unsigned int q = 6 * p;
1347  for (unsigned int i = 0; i < nbPoint[p]; i++) {
1348  unsigned int curPoint4 = 4 * curPoint;
1349  double x = cX[curPoint];
1350  double y = cY[curPoint];
1351  double z = cZ[curPoint];
1352 
1353  double inv_z = 1 / z;
1354  double X = x * inv_z;
1355  double Y = y * inv_z;
1356 
1357  double X2 = X * X;
1358  double Y2 = Y * Y;
1359  double XY = X * Y;
1360 
1361  double up = u[curPoint];
1362  double vp = v[curPoint];
1363 
1364  Pd[curPoint4] = up;
1365  Pd[curPoint4 + 1] = vp;
1366 
1367  double up0 = up - u0;
1368  double vp0 = vp - v0;
1369 
1370  double xp0 = up0 * inv_px;
1371  double xp02 = xp0 * xp0;
1372 
1373  double yp0 = vp0 * inv_py;
1374  double yp02 = yp0 * yp0;
1375 
1376  double r2du = xp02 + yp02;
1377  double kr2du = kdu * r2du;
1378 
1379  P[curPoint4] = u0 + px * X - kr2du * (up0);
1380  P[curPoint4 + 1] = v0 + py * Y - kr2du * (vp0);
1381 
1382  double r2ud = X2 + Y2;
1383  double kr2ud = 1 + kud * r2ud;
1384 
1385  double Axx = px * (kr2ud + k2ud * X2);
1386  double Axy = px * k2ud * XY;
1387  double Ayy = py * (kr2ud + k2ud * Y2);
1388  double Ayx = py * k2ud * XY;
1389 
1390  Pd[curPoint4 + 2] = up;
1391  Pd[curPoint4 + 3] = vp;
1392 
1393  P[curPoint4 + 2] = u0 + px * X * kr2ud;
1394  P[curPoint4 + 3] = v0 + py * Y * kr2ud;
1395 
1396  r += (vpMath::sqr(P[curPoint4] - Pd[curPoint4]) + vpMath::sqr(P[curPoint4 + 1] - Pd[curPoint4 + 1]) +
1397  vpMath::sqr(P[curPoint4 + 2] - Pd[curPoint4 + 2]) + vpMath::sqr(P[curPoint4 + 3] - Pd[curPoint4 + 3])) *
1398  0.5;
1399 
1400  unsigned int curInd = curPoint4;
1401  //---------------
1402  {
1403  {
1404  L[curInd][q] = px * (-inv_z);
1405  L[curInd][q + 1] = 0;
1406  L[curInd][q + 2] = px * X * inv_z;
1407  L[curInd][q + 3] = px * X * Y;
1408  L[curInd][q + 4] = -px * (1 + X2);
1409  L[curInd][q + 5] = px * Y;
1410  }
1411  {
1412  L[curInd][nbPose6] = 1 + kr2du + k2du * xp02;
1413  L[curInd][nbPose6 + 1] = k2du * up0 * yp0 * inv_py;
1414  L[curInd][nbPose6 + 2] = X + k2du * xp02 * xp0;
1415  L[curInd][nbPose6 + 3] = k2du * up0 * yp02 * inv_py;
1416  L[curInd][nbPose6 + 4] = -(up0) * (r2du);
1417  L[curInd][nbPose6 + 5] = 0;
1418  }
1419  curInd++;
1420  {
1421  L[curInd][q] = 0;
1422  L[curInd][q + 1] = py * (-inv_z);
1423  L[curInd][q + 2] = py * Y * inv_z;
1424  L[curInd][q + 3] = py * (1 + Y2);
1425  L[curInd][q + 4] = -py * XY;
1426  L[curInd][q + 5] = -py * X;
1427  }
1428  {
1429  L[curInd][nbPose6] = k2du * xp0 * vp0 * inv_px;
1430  L[curInd][nbPose6 + 1] = 1 + kr2du + k2du * yp02;
1431  L[curInd][nbPose6 + 2] = k2du * vp0 * xp02 * inv_px;
1432  L[curInd][nbPose6 + 3] = Y + k2du * yp02 * yp0;
1433  L[curInd][nbPose6 + 4] = -vp0 * r2du;
1434  L[curInd][nbPose6 + 5] = 0;
1435  }
1436  curInd++;
1437  //---undistorted to distorted
1438  {
1439  L[curInd][q] = Axx * (-inv_z);
1440  L[curInd][q + 1] = Axy * (-inv_z);
1441  L[curInd][q + 2] = Axx * (X * inv_z) + Axy * (Y * inv_z);
1442  L[curInd][q + 3] = Axx * X * Y + Axy * (1 + Y2);
1443  L[curInd][q + 4] = -Axx * (1 + X2) - Axy * XY;
1444  L[curInd][q + 5] = Axx * Y - Axy * X;
1445  }
1446  {
1447  L[curInd][nbPose6] = 1;
1448  L[curInd][nbPose6 + 1] = 0;
1449  L[curInd][nbPose6 + 2] = X * kr2ud;
1450  L[curInd][nbPose6 + 3] = 0;
1451  L[curInd][nbPose6 + 4] = 0;
1452  L[curInd][nbPose6 + 5] = px * X * r2ud;
1453  }
1454  curInd++;
1455  {
1456  L[curInd][q] = Ayx * (-inv_z);
1457  L[curInd][q + 1] = Ayy * (-inv_z);
1458  L[curInd][q + 2] = Ayx * (X * inv_z) + Ayy * (Y * inv_z);
1459  L[curInd][q + 3] = Ayx * XY + Ayy * (1 + Y2);
1460  L[curInd][q + 4] = -Ayx * (1 + X2) - Ayy * XY;
1461  L[curInd][q + 5] = Ayx * Y - Ayy * X;
1462  }
1463  {
1464  L[curInd][nbPose6] = 0;
1465  L[curInd][nbPose6 + 1] = 1;
1466  L[curInd][nbPose6 + 2] = 0;
1467  L[curInd][nbPose6 + 3] = Y * kr2ud;
1468  L[curInd][nbPose6 + 4] = 0;
1469  L[curInd][nbPose6 + 5] = py * Y * r2ud;
1470  }
1471  } // end interaction
1472  curPoint++;
1473  } // end interaction
1474  }
1475 
1476  vpColVector error;
1477  error = P - Pd;
1478  // r = r/nbPointTotal ;
1479 
1480  vpMatrix Lp;
1481  /*double rank =*/
1482  L.pseudoInverse(Lp, 1e-10);
1483  vpColVector e;
1484  e = Lp * error;
1485  vpColVector Tc, Tc_v(6 * nbPose);
1486  Tc = -e * gain;
1487  for (unsigned int i = 0; i < 6 * nbPose; i++)
1488  Tc_v[i] = Tc[i];
1489 
1490  cam_est.initPersProjWithDistortion(px + Tc[nbPose6 + 2], py + Tc[nbPose6 + 3], u0 + Tc[nbPose6],
1491  v0 + Tc[nbPose6 + 1], kud + Tc[nbPose6 + 5], kdu + Tc[nbPose6 + 4]);
1492 
1493  vpColVector Tc_v_Tmp(6);
1494  for (unsigned int p = 0; p < nbPose; p++) {
1495  for (unsigned int i = 0; i < 6; i++)
1496  Tc_v_Tmp[i] = Tc_v[6 * p + i];
1497 
1498  table_cal[p].cMo_dist = vpExponentialMap::direct(Tc_v_Tmp).inverse() * table_cal[p].cMo_dist;
1499  }
1500  if (verbose)
1501  std::cout << " std dev: " << sqrt(r / nbPointTotal) << std::endl;
1502  // std::cout << " residual: " << r << std::endl;
1503  }
1504  if (iter == nbIterMax) {
1505  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
1506  throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
1507  }
1508 
1509  for (unsigned int p = 0; p < nbPose; p++) {
1510  table_cal[p].cam_dist = cam_est;
1511  table_cal[p].computeStdDeviation_dist(table_cal[p].cMo_dist, cam_est);
1512  }
1513  if (verbose)
1514  std::cout << " Global std dev " << sqrt(r / (nbPointTotal)) << std::endl;
1515 
1516  // Restore ostream format
1517  std::cout.flags(original_flags);
1518 }
1519 
1520 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1521 
1522 #include <visp3/vision/vpHandEyeCalibration.h>
1523 
1538 void vpCalibration::calibrationTsai(const std::vector<vpHomogeneousMatrix> &cMo,
1539  const std::vector<vpHomogeneousMatrix> &rMe, vpHomogeneousMatrix &eMc)
1540 {
1541  vpHandEyeCalibration::calibrate(cMo, rMe, eMc);
1542 }
1543 
1561 int vpCalibration::computeCalibrationTsai(const std::vector<vpCalibration> &table_cal, vpHomogeneousMatrix &eMc,
1562  vpHomogeneousMatrix &eMc_dist)
1563 {
1564  unsigned int nbPose = (unsigned int)table_cal.size();
1565  if (nbPose > 2) {
1566  std::vector<vpHomogeneousMatrix> table_cMo(nbPose);
1567  std::vector<vpHomogeneousMatrix> table_cMo_dist(nbPose);
1568  std::vector<vpHomogeneousMatrix> table_rMe(nbPose);
1569 
1570  for (unsigned int i = 0; i < nbPose; i++) {
1571  table_cMo[i] = table_cal[i].cMo;
1572  table_cMo_dist[i] = table_cal[i].cMo_dist;
1573  table_rMe[i] = table_cal[i].rMe;
1574  }
1575  vpHandEyeCalibration::calibrate(table_cMo, table_rMe, eMc);
1576  vpHandEyeCalibration::calibrate(table_cMo_dist, table_rMe, eMc_dist);
1577 
1578  return 0;
1579  } else {
1580  throw (vpException(vpException::dimensionError, "At least 3 images are needed to compute hand-eye calibration !\n"));
1581  }
1582 }
1583 
1584 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1585 
1586 
1587 #undef DEBUG_LEVEL1
1588 #undef DEBUG_LEVEL2
vpMatrix::svd
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2066
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpMatrix::pseudoInverse
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2206
vpCalibrationException
Error that can be emited by the vpCalibration class.
Definition: vpCalibrationException.h:69
vpCameraParameters::get_kdu
double get_kdu() const
Definition: vpCameraParameters.h:335
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:149
vpHandEyeCalibration::calibrate
static int calibrate(const std::vector< vpHomogeneousMatrix > &cMo, const std::vector< vpHomogeneousMatrix > &rMe, vpHomogeneousMatrix &eMc)
Definition: vpHandEyeCalibration.cpp:837
vpMatrix::AtA
vpMatrix AtA() const
Definition: vpMatrix.cpp:693
vpERROR_TRACE
#define vpERROR_TRACE
Definition: vpDebug.h:393
vpMath::equal
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:296
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:641
vpCalibration
Tools for perspective camera calibration.
Definition: vpCalibration.h:71
vpCameraParameters::initPersProjWithDistortion
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
Definition: vpCameraParameters.cpp:248
vpCalibration::cMo
vpHomogeneousMatrix cMo
(as a 3x4 matrix [R T])
Definition: vpCalibration.h:93
vpMatrix::t
vpMatrix t() const
Definition: vpMatrix.cpp:507
vpImagePoint::get_u
double get_u() const
Definition: vpImagePoint.h:263
vpCalibration::computeCalibrationTsai
static vp_deprecated int computeCalibrationTsai(const std::vector< vpCalibration > &table_cal, vpHomogeneousMatrix &eMc, vpHomogeneousMatrix &eMc_dist)
Definition: vpCalibrationTools.cpp:1561
vpMath::sqr
static double sqr(double x)
Definition: vpMath.h:114
vpCalibration::cMo_dist
vpHomogeneousMatrix cMo_dist
Definition: vpCalibration.h:95
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:233
vpCalibration::eMc_dist
vpHomogeneousMatrix eMc_dist
Position of the camera in end-effector frame using camera parameters with distorsion.
Definition: vpCalibration.h:106
vpCalibration::rMe
vpHomogeneousMatrix rMe
reference coordinates (manipulator base coordinates)
Definition: vpCalibration.h:103
vpCameraParameters::initPersProjWithoutDistortion
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Definition: vpCameraParameters.cpp:182
vpCalibration::computeStdDeviation_dist
double computeStdDeviation_dist(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
Definition: vpCalibration.cpp:272
vpCalibration::cam_dist
vpCameraParameters cam_dist
projection model with distortion
Definition: vpCalibration.h:100
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpCalibration::calibrationTsai
static vp_deprecated void calibrationTsai(const std::vector< vpHomogeneousMatrix > &cMo, const std::vector< vpHomogeneousMatrix > &rMe, vpHomogeneousMatrix &eMc)
Definition: vpCalibrationTools.cpp:1538
vpCalibrationException::convergencyError
iterative algorithm doesn&#39;t converge
Definition: vpCalibrationException.h:86
vpExponentialMap::direct
static vpHomogeneousMatrix direct(const vpColVector &v)
Definition: vpExponentialMap.cpp:59
vpCalibration::eMc
vpHomogeneousMatrix eMc
Position of the camera in end-effector frame using camera parameters without distorsion.
Definition: vpCalibration.h:105
vpCameraParameters::get_kud
double get_kud() const
Definition: vpCameraParameters.h:334
vpImagePoint
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
vpCalibration::computeStdDeviation
void computeStdDeviation(double &deviation, double &deviation_dist)
Definition: vpCalibration.cpp:339
vpCalibration::cam
vpCameraParameters cam
projection model without distortion
Definition: vpCalibration.h:98
vpImagePoint::get_v
double get_v() const
Definition: vpImagePoint.h:274