Visual Servoing Platform  version 3.1.0
vpCalibrationTools.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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 #undef MAX
49 #undef MIN
50 
51 void vpCalibration::calibLagrange(vpCameraParameters &cam_est, vpHomogeneousMatrix &cMo_est)
52 {
53 
54  vpMatrix A(2 * npt, 3);
55  vpMatrix B(2 * npt, 9);
56 
57  std::list<double>::const_iterator it_LoX = LoX.begin();
58  std::list<double>::const_iterator it_LoY = LoY.begin();
59  std::list<double>::const_iterator it_LoZ = LoZ.begin();
60  std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
61 
62  vpImagePoint ip;
63 
64  for (unsigned int i = 0; i < npt; i++) {
65 
66  double x0 = *it_LoX;
67  double y0 = *it_LoY;
68  double z0 = *it_LoZ;
69 
70  ip = *it_Lip;
71 
72  double xi = ip.get_u();
73  double yi = ip.get_v();
74 
75  A[2 * i][0] = x0 * xi;
76  A[2 * i][1] = y0 * xi;
77  A[2 * i][2] = z0 * xi;
78  B[2 * i][0] = -x0;
79  B[2 * i][1] = -y0;
80  B[2 * i][2] = -z0;
81  B[2 * i][3] = 0.0;
82  B[2 * i][4] = 0.0;
83  B[2 * i][5] = 0.0;
84  B[2 * i][6] = -1.0;
85  B[2 * i][7] = 0.0;
86  B[2 * i][8] = xi;
87  A[2 * i + 1][0] = x0 * yi;
88  A[2 * i + 1][1] = y0 * yi;
89  A[2 * i + 1][2] = z0 * yi;
90  B[2 * i + 1][0] = 0.0;
91  B[2 * i + 1][1] = 0.0;
92  B[2 * i + 1][2] = 0.0;
93  B[2 * i + 1][3] = -x0;
94  B[2 * i + 1][4] = -y0;
95  B[2 * i + 1][5] = -z0;
96  B[2 * i + 1][6] = 0.0;
97  B[2 * i + 1][7] = -1.0;
98  B[2 * i + 1][8] = yi;
99 
100  ++it_LoX;
101  ++it_LoY;
102  ++it_LoZ;
103  ++it_Lip;
104  }
105 
106  vpMatrix BtB; /* compute B^T B */
107  BtB = B.t() * B;
108 
109  /* compute (B^T B)^(-1) */
110  /* input : btb (dimension 9 x 9) = (B^T B) */
111  /* output : btbinv (dimension 9 x 9) = (B^T B)^(-1) */
112 
113  vpMatrix BtBinv;
114  BtBinv = BtB.pseudoInverse(1e-16);
115 
116  vpMatrix BtA;
117  BtA = B.t() * A; /* compute B^T A */
118 
119  vpMatrix r;
120  r = BtBinv * BtA; /* compute (B^T B)^(-1) B^T A */
121 
122  vpMatrix e; /* compute - A^T B (B^T B)^(-1) B^T A*/
123  e = -(A.t() * B) * r;
124 
125  vpMatrix AtA; /* compute A^T A */
126  AtA = A.AtA();
127 
128  e += AtA; /* compute E = A^T A - A^T B (B^T B)^(-1) B^T A */
129 
130  vpColVector x1(3);
131  vpColVector x2;
132 
133  e.svd(x1, AtA); // destructive on e
134  // eigenvector computation of E corresponding to the min eigenvalue.
135  /* SVmax computation*/
136  double svm = 0.0;
137  unsigned int imin = 1;
138  for (unsigned int i = 0; i < x1.getRows(); i++) {
139  if (x1[i] > svm) {
140  svm = x1[i];
141  imin = i;
142  }
143  }
144 
145  // svm *= 0.1; /* for the rank */
146 
147  for (unsigned int i = 0; i < x1.getRows(); i++) {
148  if (x1[i] < x1[imin])
149  imin = i;
150  }
151 
152  for (unsigned int i = 0; i < x1.getRows(); i++)
153  x1[i] = AtA[i][imin];
154 
155  x2 = -(r * x1); // X_2 = - (B^T B)^(-1) B^T A X_1
156 
157  vpColVector sol(12);
158  vpColVector resul(7);
159  for (unsigned int i = 0; i < 3; i++)
160  sol[i] = x1[i]; /* X_1 */
161  for (unsigned int i = 0; i < 9; i++) /* X_2 = - (B^T B)^(-1) B^T A X_1 */
162  {
163  sol[i + 3] = x2[i];
164  }
165 
166  if (sol[11] < 0.0)
167  for (unsigned int i = 0; i < 12; i++)
168  sol[i] = -sol[i]; /* since Z0 > 0 */
169 
170  resul[0] = sol[3] * sol[0] + sol[4] * sol[1] + sol[5] * sol[2]; /* u0 */
171 
172  resul[1] = sol[6] * sol[0] + sol[7] * sol[1] + sol[8] * sol[2]; /* v0 */
173 
174  resul[2] = sqrt(sol[3] * sol[3] + sol[4] * sol[4] + sol[5] * sol[5] /* px */
175  - resul[0] * resul[0]);
176  resul[3] = sqrt(sol[6] * sol[6] + sol[7] * sol[7] + sol[8] * sol[8] /* py */
177  - resul[1] * resul[1]);
178 
179  cam_est.initPersProjWithoutDistortion(resul[2], resul[3], resul[0], resul[1]);
180 
181  resul[4] = (sol[9] - sol[11] * resul[0]) / resul[2]; /* X0 */
182  resul[5] = (sol[10] - sol[11] * resul[1]) / resul[3]; /* Y0 */
183  resul[6] = sol[11]; /* Z0 */
184 
185  vpMatrix rd(3, 3);
186  /* fill rotation matrix */
187  for (unsigned int i = 0; i < 3; i++)
188  rd[0][i] = (sol[i + 3] - sol[i] * resul[0]) / resul[2];
189  for (unsigned int i = 0; i < 3; i++)
190  rd[1][i] = (sol[i + 6] - sol[i] * resul[1]) / resul[3];
191  for (unsigned int i = 0; i < 3; i++)
192  rd[2][i] = sol[i];
193 
194  // std::cout << "norme X1 " << x1.sumSquare() <<std::endl;
195  // std::cout << rd*rd.t() ;
196 
197  for (unsigned int i = 0; i < 3; i++) {
198  for (unsigned int j = 0; j < 3; j++)
199  cMo_est[i][j] = rd[i][j];
200  }
201  for (unsigned int i = 0; i < 3; i++)
202  cMo_est[i][3] = resul[i + 4];
203 
204  this->cMo = cMo_est;
205  this->cMo_dist = cMo_est;
206 
207  double deviation, deviation_dist;
208  this->computeStdDeviation(deviation, deviation_dist);
209 }
210 
211 void vpCalibration::calibVVS(vpCameraParameters &cam_est, vpHomogeneousMatrix &cMo_est, bool verbose)
212 {
213  std::ios::fmtflags original_flags(std::cout.flags());
214  std::cout.precision(10);
215  unsigned int n_points = npt;
216 
217  vpColVector oX(n_points), cX(n_points);
218  vpColVector oY(n_points), cY(n_points);
219  vpColVector oZ(n_points), cZ(n_points);
220  vpColVector u(n_points);
221  vpColVector v(n_points);
222 
223  vpColVector P(2 * n_points);
224  vpColVector Pd(2 * n_points);
225 
226  vpImagePoint ip;
227 
228  std::list<double>::const_iterator it_LoX = LoX.begin();
229  std::list<double>::const_iterator it_LoY = LoY.begin();
230  std::list<double>::const_iterator it_LoZ = LoZ.begin();
231  std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
232 
233  for (unsigned int i = 0; i < n_points; i++) {
234  oX[i] = *it_LoX;
235  oY[i] = *it_LoY;
236  oZ[i] = *it_LoZ;
237 
238  ip = *it_Lip;
239 
240  u[i] = ip.get_u();
241  v[i] = ip.get_v();
242 
243  ++it_LoX;
244  ++it_LoY;
245  ++it_LoZ;
246  ++it_Lip;
247  }
248 
249  // double lambda = 0.1 ;
250  unsigned int iter = 0;
251 
252  double residu_1 = 1e12;
253  double r = 1e12 - 1;
254  while (vpMath::equal(residu_1, r, threshold) == false && iter < nbIterMax) {
255  iter++;
256  residu_1 = r;
257 
258  double px = cam_est.get_px();
259  double py = cam_est.get_py();
260  double u0 = cam_est.get_u0();
261  double v0 = cam_est.get_v0();
262 
263  r = 0;
264 
265  for (unsigned int i = 0; i < n_points; i++) {
266  cX[i] = oX[i] * cMo_est[0][0] + oY[i] * cMo_est[0][1] + oZ[i] * cMo_est[0][2] + cMo_est[0][3];
267  cY[i] = oX[i] * cMo_est[1][0] + oY[i] * cMo_est[1][1] + oZ[i] * cMo_est[1][2] + cMo_est[1][3];
268  cZ[i] = oX[i] * cMo_est[2][0] + oY[i] * cMo_est[2][1] + oZ[i] * cMo_est[2][2] + cMo_est[2][3];
269 
270  Pd[2 * i] = u[i];
271  Pd[2 * i + 1] = v[i];
272 
273  P[2 * i] = cX[i] / cZ[i] * px + u0;
274  P[2 * i + 1] = cY[i] / cZ[i] * py + v0;
275 
276  r += ((vpMath::sqr(P[2 * i] - Pd[2 * i]) + vpMath::sqr(P[2 * i + 1] - Pd[2 * i + 1])));
277  }
278 
279  vpColVector error;
280  error = P - Pd;
281  // r = r/n_points ;
282 
283  vpMatrix L(n_points * 2, 10);
284  for (unsigned int i = 0; i < n_points; i++) {
285  double x = cX[i];
286  double y = cY[i];
287  double z = cZ[i];
288  double inv_z = 1 / z;
289 
290  double X = x * inv_z;
291  double Y = y * inv_z;
292 
293  //---------------
294  {
295  L[2 * i][0] = px * (-inv_z);
296  L[2 * i][1] = 0;
297  L[2 * i][2] = px * X * inv_z;
298  L[2 * i][3] = px * X * Y;
299  L[2 * i][4] = -px * (1 + X * X);
300  L[2 * i][5] = px * Y;
301  }
302  {
303  L[2 * i][6] = 1;
304  L[2 * i][7] = 0;
305  L[2 * i][8] = X;
306  L[2 * i][9] = 0;
307  }
308  {
309  L[2 * i + 1][0] = 0;
310  L[2 * i + 1][1] = py * (-inv_z);
311  L[2 * i + 1][2] = py * (Y * inv_z);
312  L[2 * i + 1][3] = py * (1 + Y * Y);
313  L[2 * i + 1][4] = -py * X * Y;
314  L[2 * i + 1][5] = -py * X;
315  }
316  {
317  L[2 * i + 1][6] = 0;
318  L[2 * i + 1][7] = 1;
319  L[2 * i + 1][8] = 0;
320  L[2 * i + 1][9] = Y;
321  }
322  } // end interaction
323  vpMatrix Lp;
324  Lp = L.pseudoInverse(1e-10);
325 
326  vpColVector e;
327  e = Lp * error;
328 
329  vpColVector Tc, Tc_v(6);
330  Tc = -e * gain;
331 
332  // Tc_v =0 ;
333  for (unsigned int i = 0; i < 6; i++)
334  Tc_v[i] = Tc[i];
335 
336  cam_est.initPersProjWithoutDistortion(px + Tc[8], py + Tc[9], u0 + Tc[6], v0 + Tc[7]);
337 
338  cMo_est = vpExponentialMap::direct(Tc_v).inverse() * cMo_est;
339  if (verbose)
340  std::cout << " std dev " << sqrt(r / n_points) << std::endl;
341  }
342  if (iter == nbIterMax) {
343  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
344  throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
345  }
346  this->cMo = cMo_est;
347  this->cMo_dist = cMo_est;
348  this->residual = r;
349  this->residual_dist = r;
350  if (verbose)
351  std::cout << " std dev " << sqrt(r / n_points) << std::endl;
352  // Restore ostream format
353  std::cout.flags(original_flags);
354 }
355 
356 void vpCalibration::calibVVSMulti(std::vector<vpCalibration> &table_cal, vpCameraParameters &cam_est,
357  double &globalReprojectionError, bool verbose)
358 {
359  std::ios::fmtflags original_flags(std::cout.flags());
360  std::cout.precision(10);
361  unsigned int nbPoint[256]; // number of points by image
362  unsigned int nbPointTotal = 0; // total number of points
363  unsigned int nbPose = (unsigned int)table_cal.size();
364  unsigned int nbPose6 = 6 * nbPose;
365 
366  for (unsigned int i = 0; i < nbPose; i++) {
367  nbPoint[i] = table_cal[i].npt;
368  nbPointTotal += nbPoint[i];
369  }
370 
371  if (nbPointTotal < 4) {
372  // vpERROR_TRACE("Not enough point to calibrate");
373  throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate"));
374  }
375 
376  vpColVector oX(nbPointTotal), cX(nbPointTotal);
377  vpColVector oY(nbPointTotal), cY(nbPointTotal);
378  vpColVector oZ(nbPointTotal), cZ(nbPointTotal);
379  vpColVector u(nbPointTotal);
380  vpColVector v(nbPointTotal);
381 
382  vpColVector P(2 * nbPointTotal);
383  vpColVector Pd(2 * nbPointTotal);
384  vpImagePoint ip;
385 
386  unsigned int curPoint = 0; // current point indice
387  for (unsigned int p = 0; p < nbPose; p++) {
388  std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
389  std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
390  std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
391  std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
392 
393  for (unsigned int i = 0; i < nbPoint[p]; i++) {
394  oX[curPoint] = *it_LoX;
395  oY[curPoint] = *it_LoY;
396  oZ[curPoint] = *it_LoZ;
397 
398  ip = *it_Lip;
399  u[curPoint] = ip.get_u();
400  v[curPoint] = ip.get_v();
401 
402  ++it_LoX;
403  ++it_LoY;
404  ++it_LoZ;
405  ++it_Lip;
406 
407  curPoint++;
408  }
409  }
410  // double lambda = 0.1 ;
411  unsigned int iter = 0;
412 
413  double residu_1 = 1e12;
414  double r = 1e12 - 1;
415  while (vpMath::equal(residu_1, r, threshold) == false && iter < nbIterMax) {
416 
417  iter++;
418  residu_1 = r;
419 
420  double px = cam_est.get_px();
421  double py = cam_est.get_py();
422  double u0 = cam_est.get_u0();
423  double v0 = cam_est.get_v0();
424 
425  r = 0;
426  curPoint = 0; // current point indice
427  for (unsigned int p = 0; p < nbPose; p++) {
428  vpHomogeneousMatrix cMoTmp = table_cal[p].cMo;
429  for (unsigned int i = 0; i < nbPoint[p]; i++) {
430  unsigned int curPoint2 = 2 * curPoint;
431 
432  cX[curPoint] =
433  oX[curPoint] * cMoTmp[0][0] + oY[curPoint] * cMoTmp[0][1] + oZ[curPoint] * cMoTmp[0][2] + cMoTmp[0][3];
434  cY[curPoint] =
435  oX[curPoint] * cMoTmp[1][0] + oY[curPoint] * cMoTmp[1][1] + oZ[curPoint] * cMoTmp[1][2] + cMoTmp[1][3];
436  cZ[curPoint] =
437  oX[curPoint] * cMoTmp[2][0] + oY[curPoint] * cMoTmp[2][1] + oZ[curPoint] * cMoTmp[2][2] + cMoTmp[2][3];
438 
439  Pd[curPoint2] = u[curPoint];
440  Pd[curPoint2 + 1] = v[curPoint];
441 
442  P[curPoint2] = cX[curPoint] / cZ[curPoint] * px + u0;
443  P[curPoint2 + 1] = cY[curPoint] / cZ[curPoint] * py + v0;
444 
445  r += (vpMath::sqr(P[curPoint2] - Pd[curPoint2]) + vpMath::sqr(P[curPoint2 + 1] - Pd[curPoint2 + 1]));
446  curPoint++;
447  }
448  }
449 
450  vpColVector error;
451  error = P - Pd;
452  // r = r/nbPointTotal ;
453 
454  vpMatrix L(nbPointTotal * 2, nbPose6 + 4);
455  curPoint = 0; // current point indice
456  for (unsigned int p = 0; p < nbPose; p++) {
457  unsigned int q = 6 * p;
458  for (unsigned int i = 0; i < nbPoint[p]; i++) {
459  unsigned int curPoint2 = 2 * curPoint;
460  unsigned int curPoint21 = curPoint2 + 1;
461 
462  double x = cX[curPoint];
463  double y = cY[curPoint];
464  double z = cZ[curPoint];
465 
466  double inv_z = 1 / z;
467 
468  double X = x * inv_z;
469  double Y = y * inv_z;
470 
471  //---------------
472  {
473  {
474  L[curPoint2][q] = px * (-inv_z);
475  L[curPoint2][q + 1] = 0;
476  L[curPoint2][q + 2] = px * (X * inv_z);
477  L[curPoint2][q + 3] = px * X * Y;
478  L[curPoint2][q + 4] = -px * (1 + X * X);
479  L[curPoint2][q + 5] = px * Y;
480  }
481  {
482  L[curPoint2][nbPose6] = 1;
483  L[curPoint2][nbPose6 + 1] = 0;
484  L[curPoint2][nbPose6 + 2] = X;
485  L[curPoint2][nbPose6 + 3] = 0;
486  }
487  {
488  L[curPoint21][q] = 0;
489  L[curPoint21][q + 1] = py * (-inv_z);
490  L[curPoint21][q + 2] = py * (Y * inv_z);
491  L[curPoint21][q + 3] = py * (1 + Y * Y);
492  L[curPoint21][q + 4] = -py * X * Y;
493  L[curPoint21][q + 5] = -py * X;
494  }
495  {
496  L[curPoint21][nbPose6] = 0;
497  L[curPoint21][nbPose6 + 1] = 1;
498  L[curPoint21][nbPose6 + 2] = 0;
499  L[curPoint21][nbPose6 + 3] = Y;
500  }
501  }
502  curPoint++;
503  } // end interaction
504  }
505  vpMatrix Lp;
506  Lp = L.pseudoInverse(1e-10);
507 
508  vpColVector e;
509  e = Lp * error;
510 
511  vpColVector Tc, Tc_v(nbPose6);
512  Tc = -e * gain;
513 
514  // Tc_v =0 ;
515  for (unsigned int i = 0; i < nbPose6; i++)
516  Tc_v[i] = Tc[i];
517 
518  cam_est.initPersProjWithoutDistortion(px + Tc[nbPose6 + 2], py + Tc[nbPose6 + 3], u0 + Tc[nbPose6],
519  v0 + Tc[nbPose6 + 1]);
520 
521  // cam.setKd(get_kd() + Tc[10]) ;
522  vpColVector Tc_v_Tmp(6);
523 
524  for (unsigned int p = 0; p < nbPose; p++) {
525  for (unsigned int i = 0; i < 6; i++)
526  Tc_v_Tmp[i] = Tc_v[6 * p + i];
527 
528  table_cal[p].cMo = vpExponentialMap::direct(Tc_v_Tmp, 1).inverse() * table_cal[p].cMo;
529  }
530 
531  if (verbose)
532  std::cout << " std dev " << sqrt(r / nbPointTotal) << std::endl;
533  }
534  if (iter == nbIterMax) {
535  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
536  throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
537  }
538  for (unsigned int p = 0; p < nbPose; p++) {
539  table_cal[p].cMo_dist = table_cal[p].cMo;
540  table_cal[p].cam = cam_est;
541  table_cal[p].cam_dist = cam_est;
542  double deviation, deviation_dist;
543  table_cal[p].computeStdDeviation(deviation, deviation_dist);
544  }
545  globalReprojectionError = sqrt(r / nbPointTotal);
546  // Restore ostream format
547  std::cout.flags(original_flags);
548 }
549 
550 void vpCalibration::calibVVSWithDistortion(vpCameraParameters &cam_est, vpHomogeneousMatrix &cMo_est, bool verbose)
551 {
552  std::ios::fmtflags original_flags(std::cout.flags());
553  std::cout.precision(10);
554  unsigned int n_points = npt;
555 
556  vpColVector oX(n_points), cX(n_points);
557  vpColVector oY(n_points), cY(n_points);
558  vpColVector oZ(n_points), cZ(n_points);
559  vpColVector u(n_points);
560  vpColVector v(n_points);
561 
562  vpColVector P(4 * n_points);
563  vpColVector Pd(4 * n_points);
564 
565  std::list<double>::const_iterator it_LoX = LoX.begin();
566  std::list<double>::const_iterator it_LoY = LoY.begin();
567  std::list<double>::const_iterator it_LoZ = LoZ.begin();
568  std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
569 
570  vpImagePoint ip;
571 
572  for (unsigned int i = 0; i < n_points; i++) {
573  oX[i] = *it_LoX;
574  oY[i] = *it_LoY;
575  oZ[i] = *it_LoZ;
576 
577  ip = *it_Lip;
578  u[i] = ip.get_u();
579  v[i] = ip.get_v();
580 
581  ++it_LoX;
582  ++it_LoY;
583  ++it_LoZ;
584  ++it_Lip;
585  }
586 
587  // double lambda = 0.1 ;
588  unsigned int iter = 0;
589 
590  double residu_1 = 1e12;
591  double r = 1e12 - 1;
592  while (vpMath::equal(residu_1, r, threshold) == false && iter < nbIterMax) {
593  iter++;
594  residu_1 = r;
595 
596  r = 0;
597  double u0 = cam_est.get_u0();
598  double v0 = cam_est.get_v0();
599 
600  double px = cam_est.get_px();
601  double py = cam_est.get_py();
602 
603  double inv_px = 1 / px;
604  double inv_py = 1 / py;
605 
606  double kud = cam_est.get_kud();
607  double kdu = cam_est.get_kdu();
608 
609  double k2ud = 2 * kud;
610  double k2du = 2 * kdu;
611  vpMatrix L(n_points * 4, 12);
612 
613  for (unsigned int i = 0; i < n_points; i++) {
614  unsigned int i4 = 4 * i;
615  unsigned int i41 = 4 * i + 1;
616  unsigned int i42 = 4 * i + 2;
617  unsigned int i43 = 4 * i + 3;
618 
619  cX[i] = oX[i] * cMo_est[0][0] + oY[i] * cMo_est[0][1] + oZ[i] * cMo_est[0][2] + cMo_est[0][3];
620  cY[i] = oX[i] * cMo_est[1][0] + oY[i] * cMo_est[1][1] + oZ[i] * cMo_est[1][2] + cMo_est[1][3];
621  cZ[i] = oX[i] * cMo_est[2][0] + oY[i] * cMo_est[2][1] + oZ[i] * cMo_est[2][2] + cMo_est[2][3];
622 
623  double x = cX[i];
624  double y = cY[i];
625  double z = cZ[i];
626  double inv_z = 1 / z;
627 
628  double X = x * inv_z;
629  double Y = y * inv_z;
630 
631  double X2 = X * X;
632  double Y2 = Y * Y;
633  double XY = X * Y;
634 
635  double up = u[i];
636  double vp = v[i];
637 
638  Pd[i4] = up;
639  Pd[i41] = vp;
640 
641  double up0 = up - u0;
642  double vp0 = vp - v0;
643 
644  double xp0 = up0 * inv_px;
645  double xp02 = xp0 * xp0;
646 
647  double yp0 = vp0 * inv_py;
648  double yp02 = yp0 * yp0;
649 
650  double r2du = xp02 + yp02;
651  double kr2du = kdu * r2du;
652 
653  P[i4] = u0 + px * X - kr2du * (up0);
654  P[i41] = v0 + py * Y - kr2du * (vp0);
655 
656  double r2ud = X2 + Y2;
657  double kr2ud = 1 + kud * r2ud;
658 
659  double Axx = px * (kr2ud + k2ud * X2);
660  double Axy = px * k2ud * XY;
661  double Ayy = py * (kr2ud + k2ud * Y2);
662  double Ayx = py * k2ud * XY;
663 
664  Pd[i42] = up;
665  Pd[i43] = vp;
666 
667  P[i42] = u0 + px * X * kr2ud;
668  P[i43] = v0 + py * Y * kr2ud;
669 
670  r += (vpMath::sqr(P[i4] - Pd[i4]) + vpMath::sqr(P[i41] - Pd[i41]) + vpMath::sqr(P[i42] - Pd[i42]) +
671  vpMath::sqr(P[i43] - Pd[i43])) *
672  0.5;
673 
674  //--distorted to undistorted
675  {
676  {
677  L[i4][0] = px * (-inv_z);
678  L[i4][1] = 0;
679  L[i4][2] = px * X * inv_z;
680  L[i4][3] = px * X * Y;
681  L[i4][4] = -px * (1 + X2);
682  L[i4][5] = px * Y;
683  }
684  {
685  L[i4][6] = 1 + kr2du + k2du * xp02;
686  L[i4][7] = k2du * up0 * yp0 * inv_py;
687  L[i4][8] = X + k2du * xp02 * xp0;
688  L[i4][9] = k2du * up0 * yp02 * inv_py;
689  L[i4][10] = -(up0) * (r2du);
690  L[i4][11] = 0;
691  }
692  {
693  L[i41][0] = 0;
694  L[i41][1] = py * (-inv_z);
695  L[i41][2] = py * Y * inv_z;
696  L[i41][3] = py * (1 + Y2);
697  L[i41][4] = -py * XY;
698  L[i41][5] = -py * X;
699  }
700  {
701  L[i41][6] = k2du * xp0 * vp0 * inv_px;
702  L[i41][7] = 1 + kr2du + k2du * yp02;
703  L[i41][8] = k2du * vp0 * xp02 * inv_px;
704  L[i41][9] = Y + k2du * yp02 * yp0;
705  L[i41][10] = -vp0 * r2du;
706  L[i41][11] = 0;
707  }
708  //---undistorted to distorted
709  {
710  L[i42][0] = Axx * (-inv_z);
711  L[i42][1] = Axy * (-inv_z);
712  L[i42][2] = Axx * (X * inv_z) + Axy * (Y * inv_z);
713  L[i42][3] = Axx * X * Y + Axy * (1 + Y2);
714  L[i42][4] = -Axx * (1 + X2) - Axy * XY;
715  L[i42][5] = Axx * Y - Axy * X;
716  }
717  {
718  L[i42][6] = 1;
719  L[i42][7] = 0;
720  L[i42][8] = X * kr2ud;
721  L[i42][9] = 0;
722  L[i42][10] = 0;
723  L[i42][11] = px * X * r2ud;
724  }
725  {
726  L[i43][0] = Ayx * (-inv_z);
727  L[i43][1] = Ayy * (-inv_z);
728  L[i43][2] = Ayx * (X * inv_z) + Ayy * (Y * inv_z);
729  L[i43][3] = Ayx * XY + Ayy * (1 + Y2);
730  L[i43][4] = -Ayx * (1 + X2) - Ayy * XY;
731  L[i43][5] = Ayx * Y - Ayy * X;
732  }
733  {
734  L[i43][6] = 0;
735  L[i43][7] = 1;
736  L[i43][8] = 0;
737  L[i43][9] = Y * kr2ud;
738  L[i43][10] = 0;
739  L[i43][11] = py * Y * r2ud;
740  }
741  } // end interaction
742  } // end interaction
743 
744  vpColVector error;
745  error = P - Pd;
746  // r = r/n_points ;
747 
748  vpMatrix Lp;
749  Lp = L.pseudoInverse(1e-10);
750 
751  vpColVector e;
752  e = Lp * error;
753 
754  vpColVector Tc, Tc_v(6);
755  Tc = -e * gain;
756 
757  for (unsigned int i = 0; i < 6; i++)
758  Tc_v[i] = Tc[i];
759 
760  cam_est.initPersProjWithDistortion(px + Tc[8], py + Tc[9], u0 + Tc[6], v0 + Tc[7], kud + Tc[11], kdu + Tc[10]);
761 
762  cMo_est = vpExponentialMap::direct(Tc_v).inverse() * cMo_est;
763  if (verbose)
764  std::cout << " std dev " << sqrt(r / n_points) << std::endl;
765  }
766  if (iter == nbIterMax) {
767  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
768  throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
769  }
770  this->residual_dist = r;
771  this->cMo_dist = cMo_est;
772  this->cam_dist = cam_est;
773 
774  if (verbose)
775  std::cout << " std dev " << sqrt(r / n_points) << std::endl;
776 
777  // Restore ostream format
778  std::cout.flags(original_flags);
779 }
780 
781 void vpCalibration::calibVVSWithDistortionMulti(std::vector<vpCalibration> &table_cal, vpCameraParameters &cam_est,
782  double &globalReprojectionError, bool verbose)
783 {
784  std::ios::fmtflags original_flags(std::cout.flags());
785  std::cout.precision(10);
786  unsigned int nbPoint[1024]; // number of points by image
787  unsigned int nbPointTotal = 0; // total number of points
788  unsigned int nbPose = (unsigned int)table_cal.size();
789  unsigned int nbPose6 = 6 * nbPose;
790  for (unsigned int i = 0; i < nbPose; i++) {
791  nbPoint[i] = table_cal[i].npt;
792  nbPointTotal += nbPoint[i];
793  }
794 
795  if (nbPointTotal < 4) {
796  // vpERROR_TRACE("Not enough point to calibrate");
797  throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate"));
798  }
799 
800  vpColVector oX(nbPointTotal), cX(nbPointTotal);
801  vpColVector oY(nbPointTotal), cY(nbPointTotal);
802  vpColVector oZ(nbPointTotal), cZ(nbPointTotal);
803  vpColVector u(nbPointTotal);
804  vpColVector v(nbPointTotal);
805 
806  vpColVector P(4 * nbPointTotal);
807  vpColVector Pd(4 * nbPointTotal);
808  vpImagePoint ip;
809 
810  unsigned int curPoint = 0; // current point indice
811  for (unsigned int p = 0; p < nbPose; p++) {
812  std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
813  std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
814  std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
815  std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
816 
817  for (unsigned int i = 0; i < nbPoint[p]; i++) {
818  oX[curPoint] = *it_LoX;
819  oY[curPoint] = *it_LoY;
820  oZ[curPoint] = *it_LoZ;
821 
822  ip = *it_Lip;
823  u[curPoint] = ip.get_u();
824  v[curPoint] = ip.get_v();
825 
826  ++it_LoX;
827  ++it_LoY;
828  ++it_LoZ;
829  ++it_Lip;
830  curPoint++;
831  }
832  }
833  // double lambda = 0.1 ;
834  unsigned int iter = 0;
835 
836  double residu_1 = 1e12;
837  double r = 1e12 - 1;
838  while (vpMath::equal(residu_1, r, threshold) == false && iter < nbIterMax) {
839  iter++;
840  residu_1 = r;
841 
842  r = 0;
843  curPoint = 0; // current point indice
844  for (unsigned int p = 0; p < nbPose; p++) {
845  vpHomogeneousMatrix cMoTmp = table_cal[p].cMo_dist;
846  for (unsigned int i = 0; i < nbPoint[p]; i++) {
847  cX[curPoint] =
848  oX[curPoint] * cMoTmp[0][0] + oY[curPoint] * cMoTmp[0][1] + oZ[curPoint] * cMoTmp[0][2] + cMoTmp[0][3];
849  cY[curPoint] =
850  oX[curPoint] * cMoTmp[1][0] + oY[curPoint] * cMoTmp[1][1] + oZ[curPoint] * cMoTmp[1][2] + cMoTmp[1][3];
851  cZ[curPoint] =
852  oX[curPoint] * cMoTmp[2][0] + oY[curPoint] * cMoTmp[2][1] + oZ[curPoint] * cMoTmp[2][2] + cMoTmp[2][3];
853 
854  curPoint++;
855  }
856  }
857 
858  vpMatrix L(nbPointTotal * 4, nbPose6 + 6);
859  curPoint = 0; // current point indice
860  double px = cam_est.get_px();
861  double py = cam_est.get_py();
862  double u0 = cam_est.get_u0();
863  double v0 = cam_est.get_v0();
864 
865  double inv_px = 1 / px;
866  double inv_py = 1 / py;
867 
868  double kud = cam_est.get_kud();
869  double kdu = cam_est.get_kdu();
870 
871  double k2ud = 2 * kud;
872  double k2du = 2 * kdu;
873 
874  for (unsigned int p = 0; p < nbPose; p++) {
875  unsigned int q = 6 * p;
876  for (unsigned int i = 0; i < nbPoint[p]; i++) {
877  unsigned int curPoint4 = 4 * curPoint;
878  double x = cX[curPoint];
879  double y = cY[curPoint];
880  double z = cZ[curPoint];
881 
882  double inv_z = 1 / z;
883  double X = x * inv_z;
884  double Y = y * inv_z;
885 
886  double X2 = X * X;
887  double Y2 = Y * Y;
888  double XY = X * Y;
889 
890  double up = u[curPoint];
891  double vp = v[curPoint];
892 
893  Pd[curPoint4] = up;
894  Pd[curPoint4 + 1] = vp;
895 
896  double up0 = up - u0;
897  double vp0 = vp - v0;
898 
899  double xp0 = up0 * inv_px;
900  double xp02 = xp0 * xp0;
901 
902  double yp0 = vp0 * inv_py;
903  double yp02 = yp0 * yp0;
904 
905  double r2du = xp02 + yp02;
906  double kr2du = kdu * r2du;
907 
908  P[curPoint4] = u0 + px * X - kr2du * (up0);
909  P[curPoint4 + 1] = v0 + py * Y - kr2du * (vp0);
910 
911  double r2ud = X2 + Y2;
912  double kr2ud = 1 + kud * r2ud;
913 
914  double Axx = px * (kr2ud + k2ud * X2);
915  double Axy = px * k2ud * XY;
916  double Ayy = py * (kr2ud + k2ud * Y2);
917  double Ayx = py * k2ud * XY;
918 
919  Pd[curPoint4 + 2] = up;
920  Pd[curPoint4 + 3] = vp;
921 
922  P[curPoint4 + 2] = u0 + px * X * kr2ud;
923  P[curPoint4 + 3] = v0 + py * Y * kr2ud;
924 
925  r += (vpMath::sqr(P[curPoint4] - Pd[curPoint4]) + vpMath::sqr(P[curPoint4 + 1] - Pd[curPoint4 + 1]) +
926  vpMath::sqr(P[curPoint4 + 2] - Pd[curPoint4 + 2]) + vpMath::sqr(P[curPoint4 + 3] - Pd[curPoint4 + 3])) *
927  0.5;
928 
929  unsigned int curInd = curPoint4;
930  //---------------
931  {
932  {
933  L[curInd][q] = px * (-inv_z);
934  L[curInd][q + 1] = 0;
935  L[curInd][q + 2] = px * X * inv_z;
936  L[curInd][q + 3] = px * X * Y;
937  L[curInd][q + 4] = -px * (1 + X2);
938  L[curInd][q + 5] = px * Y;
939  }
940  {
941  L[curInd][nbPose6] = 1 + kr2du + k2du * xp02;
942  L[curInd][nbPose6 + 1] = k2du * up0 * yp0 * inv_py;
943  L[curInd][nbPose6 + 2] = X + k2du * xp02 * xp0;
944  L[curInd][nbPose6 + 3] = k2du * up0 * yp02 * inv_py;
945  L[curInd][nbPose6 + 4] = -(up0) * (r2du);
946  L[curInd][nbPose6 + 5] = 0;
947  }
948  curInd++;
949  {
950  L[curInd][q] = 0;
951  L[curInd][q + 1] = py * (-inv_z);
952  L[curInd][q + 2] = py * Y * inv_z;
953  L[curInd][q + 3] = py * (1 + Y2);
954  L[curInd][q + 4] = -py * XY;
955  L[curInd][q + 5] = -py * X;
956  }
957  {
958  L[curInd][nbPose6] = k2du * xp0 * vp0 * inv_px;
959  L[curInd][nbPose6 + 1] = 1 + kr2du + k2du * yp02;
960  L[curInd][nbPose6 + 2] = k2du * vp0 * xp02 * inv_px;
961  L[curInd][nbPose6 + 3] = Y + k2du * yp02 * yp0;
962  L[curInd][nbPose6 + 4] = -vp0 * r2du;
963  L[curInd][nbPose6 + 5] = 0;
964  }
965  curInd++;
966  //---undistorted to distorted
967  {
968  L[curInd][q] = Axx * (-inv_z);
969  L[curInd][q + 1] = Axy * (-inv_z);
970  L[curInd][q + 2] = Axx * (X * inv_z) + Axy * (Y * inv_z);
971  L[curInd][q + 3] = Axx * X * Y + Axy * (1 + Y2);
972  L[curInd][q + 4] = -Axx * (1 + X2) - Axy * XY;
973  L[curInd][q + 5] = Axx * Y - Axy * X;
974  }
975  {
976  L[curInd][nbPose6] = 1;
977  L[curInd][nbPose6 + 1] = 0;
978  L[curInd][nbPose6 + 2] = X * kr2ud;
979  L[curInd][nbPose6 + 3] = 0;
980  L[curInd][nbPose6 + 4] = 0;
981  L[curInd][nbPose6 + 5] = px * X * r2ud;
982  }
983  curInd++;
984  {
985  L[curInd][q] = Ayx * (-inv_z);
986  L[curInd][q + 1] = Ayy * (-inv_z);
987  L[curInd][q + 2] = Ayx * (X * inv_z) + Ayy * (Y * inv_z);
988  L[curInd][q + 3] = Ayx * XY + Ayy * (1 + Y2);
989  L[curInd][q + 4] = -Ayx * (1 + X2) - Ayy * XY;
990  L[curInd][q + 5] = Ayx * Y - Ayy * X;
991  }
992  {
993  L[curInd][nbPose6] = 0;
994  L[curInd][nbPose6 + 1] = 1;
995  L[curInd][nbPose6 + 2] = 0;
996  L[curInd][nbPose6 + 3] = Y * kr2ud;
997  L[curInd][nbPose6 + 4] = 0;
998  L[curInd][nbPose6 + 5] = py * Y * r2ud;
999  }
1000  } // end interaction
1001  curPoint++;
1002  } // end interaction
1003  }
1004 
1005  vpColVector error;
1006  error = P - Pd;
1007  // r = r/nbPointTotal ;
1008 
1009  vpMatrix Lp;
1010  /*double rank =*/
1011  L.pseudoInverse(Lp, 1e-10);
1012  vpColVector e;
1013  e = Lp * error;
1014  vpColVector Tc, Tc_v(6 * nbPose);
1015  Tc = -e * gain;
1016  for (unsigned int i = 0; i < 6 * nbPose; i++)
1017  Tc_v[i] = Tc[i];
1018 
1019  cam_est.initPersProjWithDistortion(px + Tc[nbPose6 + 2], py + Tc[nbPose6 + 3], u0 + Tc[nbPose6],
1020  v0 + Tc[nbPose6 + 1], kud + Tc[nbPose6 + 5], kdu + Tc[nbPose6 + 4]);
1021 
1022  vpColVector Tc_v_Tmp(6);
1023  for (unsigned int p = 0; p < nbPose; p++) {
1024  for (unsigned int i = 0; i < 6; i++)
1025  Tc_v_Tmp[i] = Tc_v[6 * p + i];
1026 
1027  table_cal[p].cMo_dist = vpExponentialMap::direct(Tc_v_Tmp).inverse() * table_cal[p].cMo_dist;
1028  }
1029  if (verbose)
1030  std::cout << " std dev: " << sqrt(r / nbPointTotal) << std::endl;
1031  // std::cout << " residual: " << r << std::endl;
1032  }
1033  if (iter == nbIterMax) {
1034  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
1035  throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
1036  }
1037 
1038  // double perViewError;
1039  // double totalError = 0;
1040  // int totalPoints = 0;
1041  for (unsigned int p = 0; p < nbPose; p++) {
1042  table_cal[p].cam_dist = cam_est;
1043  // perViewError =
1044  // table_cal[p].computeStdDeviation_dist(table_cal[p].cMo_dist, cam_est);
1045  // totalError += perViewError*perViewError * table_cal[p].npt;
1046  // totalPoints += (int)table_cal[p].npt;
1047  }
1048  globalReprojectionError = sqrt(r / (nbPointTotal));
1049 
1050  // Restore ostream format
1051  std::cout.flags(original_flags);
1052 }
1053 
1069 void vpCalibration::calibrationTsai(const std::vector<vpHomogeneousMatrix> &cMo,
1070  const std::vector<vpHomogeneousMatrix> &rMe, vpHomogeneousMatrix &eMc)
1071 {
1072 
1073  vpColVector x;
1074  unsigned int nbPose = (unsigned int)cMo.size();
1075  if (cMo.size() != rMe.size())
1076  throw vpCalibrationException(vpCalibrationException::dimensionError, "cMo and rMe have different sizes");
1077  {
1078  vpMatrix A;
1079  vpColVector B;
1080  unsigned int k = 0;
1081  // for all couples ij
1082  for (unsigned int i = 0; i < nbPose; i++) {
1083  vpRotationMatrix rRei, ciRo;
1084  rMe[i].extract(rRei);
1085  cMo[i].extract(ciRo);
1086  // std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
1087 
1088  for (unsigned int j = 0; j < nbPose; j++) {
1089  if (j > i) // we don't use two times same couples...
1090  {
1091  vpRotationMatrix rRej, cjRo;
1092  rMe[j].extract(rRej);
1093  cMo[j].extract(cjRo);
1094  // std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
1095 
1096  vpRotationMatrix rReij = rRej.t() * rRei;
1097 
1098  vpRotationMatrix cijRo = cjRo * ciRo.t();
1099 
1100  vpThetaUVector rPeij(rReij);
1101 
1102  double theta = sqrt(rPeij[0] * rPeij[0] + rPeij[1] * rPeij[1] + rPeij[2] * rPeij[2]);
1103 
1104  for (unsigned int m = 0; m < 3; m++)
1105  rPeij[m] = rPeij[m] * vpMath::sinc(theta / 2);
1106 
1107  vpThetaUVector cijPo(cijRo);
1108  theta = sqrt(cijPo[0] * cijPo[0] + cijPo[1] * cijPo[1] + cijPo[2] * cijPo[2]);
1109  for (unsigned int m = 0; m < 3; m++)
1110  cijPo[m] = cijPo[m] * vpMath::sinc(theta / 2);
1111 
1112  vpMatrix As;
1113  vpColVector b(3);
1114 
1115  As = vpColVector::skew(vpColVector(rPeij) + vpColVector(cijPo));
1116 
1117  b = (vpColVector)cijPo - (vpColVector)rPeij; // A.40
1118 
1119  if (k == 0) {
1120  A = As;
1121  B = b;
1122  } else {
1123  A = vpMatrix::stack(A, As);
1124  B = vpColVector::stack(B, b);
1125  }
1126  k++;
1127  }
1128  }
1129  }
1130 
1131  // the linear system is defined
1132  // x = AtA^-1AtB is solved
1133  vpMatrix AtA = A.AtA();
1134 
1135  vpMatrix Ap;
1136  AtA.pseudoInverse(Ap, 1e-6); // rank 3
1137  x = Ap * A.t() * B;
1138 
1139  // {
1140  // // Residual
1141  // vpColVector residual;
1142  // residual = A*x-B;
1143  // std::cout << "Residual: " << std::endl << residual << std::endl;
1144 
1145  // double res = 0;
1146  // for (int i=0; i < residual.getRows(); i++)
1147  // res += residual[i]*residual[i];
1148  // res = sqrt(res/residual.getRows());
1149  // printf("Mean residual = %lf\n",res);
1150  // }
1151 
1152  // extraction of theta and U
1153  double theta;
1154  double d = x.sumSquare();
1155  for (unsigned int i = 0; i < 3; i++)
1156  x[i] = 2 * x[i] / sqrt(1 + d);
1157  theta = sqrt(x.sumSquare()) / 2;
1158  theta = 2 * asin(theta);
1159  // if (theta !=0)
1160  if (std::fabs(theta) > std::numeric_limits<double>::epsilon()) {
1161  for (unsigned int i = 0; i < 3; i++)
1162  x[i] *= theta / (2 * sin(theta / 2));
1163  } else
1164  x = 0;
1165  }
1166 
1167  // Building of the rotation matrix eRc
1168  vpThetaUVector xP(x[0], x[1], x[2]);
1169  vpRotationMatrix eRc(xP);
1170 
1171  {
1172  vpMatrix A;
1173  vpColVector B;
1174  // Building of the system for the translation estimation
1175  // for all couples ij
1176  vpRotationMatrix I3;
1177  I3.eye();
1178  int k = 0;
1179  for (unsigned int i = 0; i < nbPose; i++) {
1180  vpRotationMatrix rRei, ciRo;
1181  vpTranslationVector rTei, ciTo;
1182  rMe[i].extract(rRei);
1183  cMo[i].extract(ciRo);
1184  rMe[i].extract(rTei);
1185  cMo[i].extract(ciTo);
1186 
1187  for (unsigned int j = 0; j < nbPose; j++) {
1188  if (j > i) // we don't use two times same couples...
1189  {
1190 
1191  vpRotationMatrix rRej, cjRo;
1192  rMe[j].extract(rRej);
1193  cMo[j].extract(cjRo);
1194 
1195  vpTranslationVector rTej, cjTo;
1196  rMe[j].extract(rTej);
1197  cMo[j].extract(cjTo);
1198 
1199  vpRotationMatrix rReij = rRej.t() * rRei;
1200 
1201  vpTranslationVector rTeij = rTej + (-rTei);
1202 
1203  rTeij = rRej.t() * rTeij;
1204 
1205  vpMatrix a = vpMatrix(rReij) - vpMatrix(I3);
1206 
1207  vpTranslationVector b;
1208  b = eRc * cjTo - rReij * eRc * ciTo + rTeij;
1209 
1210  if (k == 0) {
1211  A = a;
1212  B = b;
1213  } else {
1214  A = vpMatrix::stack(A, a);
1215  B = vpColVector::stack(B, b);
1216  }
1217  k++;
1218  }
1219  }
1220  }
1221 
1222  // the linear system is solved
1223  // x = AtA^-1AtB is solved
1224  vpMatrix AtA = A.AtA();
1225  vpMatrix Ap;
1226  vpColVector AeTc;
1227  AtA.pseudoInverse(Ap, 1e-6);
1228  AeTc = Ap * A.t() * B;
1229 
1230  // {
1231  // // residual
1232  // vpColVector residual;
1233  // residual = A*AeTc-B;
1234  // std::cout << "Residual: " << std::endl << residual << std::endl;
1235  // double res = 0;
1236  // for (int i=0; i < residual.getRows(); i++)
1237  // res += residual[i]*residual[i];
1238  // res = sqrt(res/residual.getRows());
1239  // printf("mean residual = %lf\n",res);
1240  // }
1241 
1242  vpTranslationVector eTc(AeTc[0], AeTc[1], AeTc[2]);
1243 
1244  eMc.insert(eTc);
1245  eMc.insert(eRc);
1246  }
1247 }
1248 
1249 void vpCalibration::calibVVSMulti(unsigned int nbPose, vpCalibration table_cal[], vpCameraParameters &cam_est,
1250  bool verbose)
1251 {
1252  std::ios::fmtflags original_flags(std::cout.flags());
1253  std::cout.precision(10);
1254  unsigned int nbPoint[256]; // number of points by image
1255  unsigned int nbPointTotal = 0; // total number of points
1256 
1257  unsigned int nbPose6 = 6 * nbPose;
1258 
1259  for (unsigned int i = 0; i < nbPose; i++) {
1260  nbPoint[i] = table_cal[i].npt;
1261  nbPointTotal += nbPoint[i];
1262  }
1263 
1264  if (nbPointTotal < 4) {
1265  // vpERROR_TRACE("Not enough point to calibrate");
1266  throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate"));
1267  }
1268 
1269  vpColVector oX(nbPointTotal), cX(nbPointTotal);
1270  vpColVector oY(nbPointTotal), cY(nbPointTotal);
1271  vpColVector oZ(nbPointTotal), cZ(nbPointTotal);
1272  vpColVector u(nbPointTotal);
1273  vpColVector v(nbPointTotal);
1274 
1275  vpColVector P(2 * nbPointTotal);
1276  vpColVector Pd(2 * nbPointTotal);
1277  vpImagePoint ip;
1278 
1279  unsigned int curPoint = 0; // current point indice
1280  for (unsigned int p = 0; p < nbPose; p++) {
1281  std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
1282  std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
1283  std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
1284  std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
1285 
1286  for (unsigned int i = 0; i < nbPoint[p]; i++) {
1287  oX[curPoint] = *it_LoX;
1288  oY[curPoint] = *it_LoY;
1289  oZ[curPoint] = *it_LoZ;
1290 
1291  ip = *it_Lip;
1292  u[curPoint] = ip.get_u();
1293  v[curPoint] = ip.get_v();
1294 
1295  ++it_LoX;
1296  ++it_LoY;
1297  ++it_LoZ;
1298  ++it_Lip;
1299 
1300  curPoint++;
1301  }
1302  }
1303  // double lambda = 0.1 ;
1304  unsigned int iter = 0;
1305 
1306  double residu_1 = 1e12;
1307  double r = 1e12 - 1;
1308  while (vpMath::equal(residu_1, r, threshold) == false && iter < nbIterMax) {
1309 
1310  iter++;
1311  residu_1 = r;
1312 
1313  double px = cam_est.get_px();
1314  double py = cam_est.get_py();
1315  double u0 = cam_est.get_u0();
1316  double v0 = cam_est.get_v0();
1317 
1318  r = 0;
1319  curPoint = 0; // current point indice
1320  for (unsigned int p = 0; p < nbPose; p++) {
1321  vpHomogeneousMatrix cMoTmp = table_cal[p].cMo;
1322  for (unsigned int i = 0; i < nbPoint[p]; i++) {
1323  unsigned int curPoint2 = 2 * curPoint;
1324 
1325  cX[curPoint] =
1326  oX[curPoint] * cMoTmp[0][0] + oY[curPoint] * cMoTmp[0][1] + oZ[curPoint] * cMoTmp[0][2] + cMoTmp[0][3];
1327  cY[curPoint] =
1328  oX[curPoint] * cMoTmp[1][0] + oY[curPoint] * cMoTmp[1][1] + oZ[curPoint] * cMoTmp[1][2] + cMoTmp[1][3];
1329  cZ[curPoint] =
1330  oX[curPoint] * cMoTmp[2][0] + oY[curPoint] * cMoTmp[2][1] + oZ[curPoint] * cMoTmp[2][2] + cMoTmp[2][3];
1331 
1332  Pd[curPoint2] = u[curPoint];
1333  Pd[curPoint2 + 1] = v[curPoint];
1334 
1335  P[curPoint2] = cX[curPoint] / cZ[curPoint] * px + u0;
1336  P[curPoint2 + 1] = cY[curPoint] / cZ[curPoint] * py + v0;
1337 
1338  r += (vpMath::sqr(P[curPoint2] - Pd[curPoint2]) + vpMath::sqr(P[curPoint2 + 1] - Pd[curPoint2 + 1]));
1339  curPoint++;
1340  }
1341  }
1342 
1343  vpColVector error;
1344  error = P - Pd;
1345  // r = r/nbPointTotal ;
1346 
1347  vpMatrix L(nbPointTotal * 2, nbPose6 + 4);
1348  curPoint = 0; // current point indice
1349  for (unsigned int p = 0; p < nbPose; p++) {
1350  unsigned int q = 6 * p;
1351  for (unsigned int i = 0; i < nbPoint[p]; i++) {
1352  unsigned int curPoint2 = 2 * curPoint;
1353  unsigned int curPoint21 = curPoint2 + 1;
1354 
1355  double x = cX[curPoint];
1356  double y = cY[curPoint];
1357  double z = cZ[curPoint];
1358 
1359  double inv_z = 1 / z;
1360 
1361  double X = x * inv_z;
1362  double Y = y * inv_z;
1363 
1364  //---------------
1365  {
1366  {
1367  L[curPoint2][q] = px * (-inv_z);
1368  L[curPoint2][q + 1] = 0;
1369  L[curPoint2][q + 2] = px * (X * inv_z);
1370  L[curPoint2][q + 3] = px * X * Y;
1371  L[curPoint2][q + 4] = -px * (1 + X * X);
1372  L[curPoint2][q + 5] = px * Y;
1373  }
1374  {
1375  L[curPoint2][nbPose6] = 1;
1376  L[curPoint2][nbPose6 + 1] = 0;
1377  L[curPoint2][nbPose6 + 2] = X;
1378  L[curPoint2][nbPose6 + 3] = 0;
1379  }
1380  {
1381  L[curPoint21][q] = 0;
1382  L[curPoint21][q + 1] = py * (-inv_z);
1383  L[curPoint21][q + 2] = py * (Y * inv_z);
1384  L[curPoint21][q + 3] = py * (1 + Y * Y);
1385  L[curPoint21][q + 4] = -py * X * Y;
1386  L[curPoint21][q + 5] = -py * X;
1387  }
1388  {
1389  L[curPoint21][nbPose6] = 0;
1390  L[curPoint21][nbPose6 + 1] = 1;
1391  L[curPoint21][nbPose6 + 2] = 0;
1392  L[curPoint21][nbPose6 + 3] = Y;
1393  }
1394  }
1395  curPoint++;
1396  } // end interaction
1397  }
1398  vpMatrix Lp;
1399  Lp = L.pseudoInverse(1e-10);
1400 
1401  vpColVector e;
1402  e = Lp * error;
1403 
1404  vpColVector Tc, Tc_v(nbPose6);
1405  Tc = -e * gain;
1406 
1407  // Tc_v =0 ;
1408  for (unsigned int i = 0; i < nbPose6; i++)
1409  Tc_v[i] = Tc[i];
1410 
1411  cam_est.initPersProjWithoutDistortion(px + Tc[nbPose6 + 2], py + Tc[nbPose6 + 3], u0 + Tc[nbPose6],
1412  v0 + Tc[nbPose6 + 1]);
1413 
1414  // cam.setKd(get_kd() + Tc[10]) ;
1415  vpColVector Tc_v_Tmp(6);
1416 
1417  for (unsigned int p = 0; p < nbPose; p++) {
1418  for (unsigned int i = 0; i < 6; i++)
1419  Tc_v_Tmp[i] = Tc_v[6 * p + i];
1420 
1421  table_cal[p].cMo = vpExponentialMap::direct(Tc_v_Tmp, 1).inverse() * table_cal[p].cMo;
1422  }
1423  if (verbose)
1424  std::cout << " std dev " << sqrt(r / nbPointTotal) << std::endl;
1425  }
1426  if (iter == nbIterMax) {
1427  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
1428  throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
1429  }
1430  for (unsigned int p = 0; p < nbPose; p++) {
1431  table_cal[p].cMo_dist = table_cal[p].cMo;
1432  table_cal[p].cam = cam_est;
1433  table_cal[p].cam_dist = cam_est;
1434  double deviation, deviation_dist;
1435  table_cal[p].computeStdDeviation(deviation, deviation_dist);
1436  }
1437  if (verbose)
1438  std::cout << " Global std dev " << sqrt(r / nbPointTotal) << std::endl;
1439 
1440  // Restore ostream format
1441  std::cout.flags(original_flags);
1442 }
1443 
1444 void vpCalibration::calibVVSWithDistortionMulti(unsigned int nbPose, vpCalibration table_cal[],
1445  vpCameraParameters &cam_est, bool verbose)
1446 {
1447  std::ios::fmtflags original_flags(std::cout.flags());
1448  std::cout.precision(10);
1449  unsigned int nbPoint[1024]; // number of points by image
1450  unsigned int nbPointTotal = 0; // total number of points
1451 
1452  unsigned int nbPose6 = 6 * nbPose;
1453  for (unsigned int i = 0; i < nbPose; i++) {
1454  nbPoint[i] = table_cal[i].npt;
1455  nbPointTotal += nbPoint[i];
1456  }
1457 
1458  if (nbPointTotal < 4) {
1459  // vpERROR_TRACE("Not enough point to calibrate");
1460  throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate"));
1461  }
1462 
1463  vpColVector oX(nbPointTotal), cX(nbPointTotal);
1464  vpColVector oY(nbPointTotal), cY(nbPointTotal);
1465  vpColVector oZ(nbPointTotal), cZ(nbPointTotal);
1466  vpColVector u(nbPointTotal);
1467  vpColVector v(nbPointTotal);
1468 
1469  vpColVector P(4 * nbPointTotal);
1470  vpColVector Pd(4 * nbPointTotal);
1471  vpImagePoint ip;
1472 
1473  unsigned int curPoint = 0; // current point indice
1474  for (unsigned int p = 0; p < nbPose; p++) {
1475  std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
1476  std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
1477  std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
1478  std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
1479 
1480  for (unsigned int i = 0; i < nbPoint[p]; i++) {
1481  oX[curPoint] = *it_LoX;
1482  oY[curPoint] = *it_LoY;
1483  oZ[curPoint] = *it_LoZ;
1484 
1485  ip = *it_Lip;
1486  u[curPoint] = ip.get_u();
1487  v[curPoint] = ip.get_v();
1488 
1489  ++it_LoX;
1490  ++it_LoY;
1491  ++it_LoZ;
1492  ++it_Lip;
1493  curPoint++;
1494  }
1495  }
1496  // double lambda = 0.1 ;
1497  unsigned int iter = 0;
1498 
1499  double residu_1 = 1e12;
1500  double r = 1e12 - 1;
1501  while (vpMath::equal(residu_1, r, threshold) == false && iter < nbIterMax) {
1502  iter++;
1503  residu_1 = r;
1504 
1505  r = 0;
1506  curPoint = 0; // current point indice
1507  for (unsigned int p = 0; p < nbPose; p++) {
1508  vpHomogeneousMatrix cMoTmp = table_cal[p].cMo_dist;
1509  for (unsigned int i = 0; i < nbPoint[p]; i++) {
1510  cX[curPoint] =
1511  oX[curPoint] * cMoTmp[0][0] + oY[curPoint] * cMoTmp[0][1] + oZ[curPoint] * cMoTmp[0][2] + cMoTmp[0][3];
1512  cY[curPoint] =
1513  oX[curPoint] * cMoTmp[1][0] + oY[curPoint] * cMoTmp[1][1] + oZ[curPoint] * cMoTmp[1][2] + cMoTmp[1][3];
1514  cZ[curPoint] =
1515  oX[curPoint] * cMoTmp[2][0] + oY[curPoint] * cMoTmp[2][1] + oZ[curPoint] * cMoTmp[2][2] + cMoTmp[2][3];
1516 
1517  curPoint++;
1518  }
1519  }
1520 
1521  vpMatrix L(nbPointTotal * 4, nbPose6 + 6);
1522  curPoint = 0; // current point indice
1523  double px = cam_est.get_px();
1524  double py = cam_est.get_py();
1525  double u0 = cam_est.get_u0();
1526  double v0 = cam_est.get_v0();
1527 
1528  double inv_px = 1 / px;
1529  double inv_py = 1 / py;
1530 
1531  double kud = cam_est.get_kud();
1532  double kdu = cam_est.get_kdu();
1533 
1534  double k2ud = 2 * kud;
1535  double k2du = 2 * kdu;
1536 
1537  for (unsigned int p = 0; p < nbPose; p++) {
1538  unsigned int q = 6 * p;
1539  for (unsigned int i = 0; i < nbPoint[p]; i++) {
1540  unsigned int curPoint4 = 4 * curPoint;
1541  double x = cX[curPoint];
1542  double y = cY[curPoint];
1543  double z = cZ[curPoint];
1544 
1545  double inv_z = 1 / z;
1546  double X = x * inv_z;
1547  double Y = y * inv_z;
1548 
1549  double X2 = X * X;
1550  double Y2 = Y * Y;
1551  double XY = X * Y;
1552 
1553  double up = u[curPoint];
1554  double vp = v[curPoint];
1555 
1556  Pd[curPoint4] = up;
1557  Pd[curPoint4 + 1] = vp;
1558 
1559  double up0 = up - u0;
1560  double vp0 = vp - v0;
1561 
1562  double xp0 = up0 * inv_px;
1563  double xp02 = xp0 * xp0;
1564 
1565  double yp0 = vp0 * inv_py;
1566  double yp02 = yp0 * yp0;
1567 
1568  double r2du = xp02 + yp02;
1569  double kr2du = kdu * r2du;
1570 
1571  P[curPoint4] = u0 + px * X - kr2du * (up0);
1572  P[curPoint4 + 1] = v0 + py * Y - kr2du * (vp0);
1573 
1574  double r2ud = X2 + Y2;
1575  double kr2ud = 1 + kud * r2ud;
1576 
1577  double Axx = px * (kr2ud + k2ud * X2);
1578  double Axy = px * k2ud * XY;
1579  double Ayy = py * (kr2ud + k2ud * Y2);
1580  double Ayx = py * k2ud * XY;
1581 
1582  Pd[curPoint4 + 2] = up;
1583  Pd[curPoint4 + 3] = vp;
1584 
1585  P[curPoint4 + 2] = u0 + px * X * kr2ud;
1586  P[curPoint4 + 3] = v0 + py * Y * kr2ud;
1587 
1588  r += (vpMath::sqr(P[curPoint4] - Pd[curPoint4]) + vpMath::sqr(P[curPoint4 + 1] - Pd[curPoint4 + 1]) +
1589  vpMath::sqr(P[curPoint4 + 2] - Pd[curPoint4 + 2]) + vpMath::sqr(P[curPoint4 + 3] - Pd[curPoint4 + 3])) *
1590  0.5;
1591 
1592  unsigned int curInd = curPoint4;
1593  //---------------
1594  {
1595  {
1596  L[curInd][q] = px * (-inv_z);
1597  L[curInd][q + 1] = 0;
1598  L[curInd][q + 2] = px * X * inv_z;
1599  L[curInd][q + 3] = px * X * Y;
1600  L[curInd][q + 4] = -px * (1 + X2);
1601  L[curInd][q + 5] = px * Y;
1602  }
1603  {
1604  L[curInd][nbPose6] = 1 + kr2du + k2du * xp02;
1605  L[curInd][nbPose6 + 1] = k2du * up0 * yp0 * inv_py;
1606  L[curInd][nbPose6 + 2] = X + k2du * xp02 * xp0;
1607  L[curInd][nbPose6 + 3] = k2du * up0 * yp02 * inv_py;
1608  L[curInd][nbPose6 + 4] = -(up0) * (r2du);
1609  L[curInd][nbPose6 + 5] = 0;
1610  }
1611  curInd++;
1612  {
1613  L[curInd][q] = 0;
1614  L[curInd][q + 1] = py * (-inv_z);
1615  L[curInd][q + 2] = py * Y * inv_z;
1616  L[curInd][q + 3] = py * (1 + Y2);
1617  L[curInd][q + 4] = -py * XY;
1618  L[curInd][q + 5] = -py * X;
1619  }
1620  {
1621  L[curInd][nbPose6] = k2du * xp0 * vp0 * inv_px;
1622  L[curInd][nbPose6 + 1] = 1 + kr2du + k2du * yp02;
1623  L[curInd][nbPose6 + 2] = k2du * vp0 * xp02 * inv_px;
1624  L[curInd][nbPose6 + 3] = Y + k2du * yp02 * yp0;
1625  L[curInd][nbPose6 + 4] = -vp0 * r2du;
1626  L[curInd][nbPose6 + 5] = 0;
1627  }
1628  curInd++;
1629  //---undistorted to distorted
1630  {
1631  L[curInd][q] = Axx * (-inv_z);
1632  L[curInd][q + 1] = Axy * (-inv_z);
1633  L[curInd][q + 2] = Axx * (X * inv_z) + Axy * (Y * inv_z);
1634  L[curInd][q + 3] = Axx * X * Y + Axy * (1 + Y2);
1635  L[curInd][q + 4] = -Axx * (1 + X2) - Axy * XY;
1636  L[curInd][q + 5] = Axx * Y - Axy * X;
1637  }
1638  {
1639  L[curInd][nbPose6] = 1;
1640  L[curInd][nbPose6 + 1] = 0;
1641  L[curInd][nbPose6 + 2] = X * kr2ud;
1642  L[curInd][nbPose6 + 3] = 0;
1643  L[curInd][nbPose6 + 4] = 0;
1644  L[curInd][nbPose6 + 5] = px * X * r2ud;
1645  }
1646  curInd++;
1647  {
1648  L[curInd][q] = Ayx * (-inv_z);
1649  L[curInd][q + 1] = Ayy * (-inv_z);
1650  L[curInd][q + 2] = Ayx * (X * inv_z) + Ayy * (Y * inv_z);
1651  L[curInd][q + 3] = Ayx * XY + Ayy * (1 + Y2);
1652  L[curInd][q + 4] = -Ayx * (1 + X2) - Ayy * XY;
1653  L[curInd][q + 5] = Ayx * Y - Ayy * X;
1654  }
1655  {
1656  L[curInd][nbPose6] = 0;
1657  L[curInd][nbPose6 + 1] = 1;
1658  L[curInd][nbPose6 + 2] = 0;
1659  L[curInd][nbPose6 + 3] = Y * kr2ud;
1660  L[curInd][nbPose6 + 4] = 0;
1661  L[curInd][nbPose6 + 5] = py * Y * r2ud;
1662  }
1663  } // end interaction
1664  curPoint++;
1665  } // end interaction
1666  }
1667 
1668  vpColVector error;
1669  error = P - Pd;
1670  // r = r/nbPointTotal ;
1671 
1672  vpMatrix Lp;
1673  /*double rank =*/
1674  L.pseudoInverse(Lp, 1e-10);
1675  vpColVector e;
1676  e = Lp * error;
1677  vpColVector Tc, Tc_v(6 * nbPose);
1678  Tc = -e * gain;
1679  for (unsigned int i = 0; i < 6 * nbPose; i++)
1680  Tc_v[i] = Tc[i];
1681 
1682  cam_est.initPersProjWithDistortion(px + Tc[nbPose6 + 2], py + Tc[nbPose6 + 3], u0 + Tc[nbPose6],
1683  v0 + Tc[nbPose6 + 1], kud + Tc[nbPose6 + 5], kdu + Tc[nbPose6 + 4]);
1684 
1685  vpColVector Tc_v_Tmp(6);
1686  for (unsigned int p = 0; p < nbPose; p++) {
1687  for (unsigned int i = 0; i < 6; i++)
1688  Tc_v_Tmp[i] = Tc_v[6 * p + i];
1689 
1690  table_cal[p].cMo_dist = vpExponentialMap::direct(Tc_v_Tmp).inverse() * table_cal[p].cMo_dist;
1691  }
1692  if (verbose)
1693  std::cout << " std dev: " << sqrt(r / nbPointTotal) << std::endl;
1694  // std::cout << " residual: " << r << std::endl;
1695  }
1696  if (iter == nbIterMax) {
1697  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
1698  throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
1699  }
1700 
1701  for (unsigned int p = 0; p < nbPose; p++) {
1702  table_cal[p].cam_dist = cam_est;
1703  table_cal[p].computeStdDeviation_dist(table_cal[p].cMo_dist, cam_est);
1704  }
1705  if (verbose)
1706  std::cout << " Global std dev " << sqrt(r / (nbPointTotal)) << std::endl;
1707 
1708  // Restore ostream format
1709  std::cout.flags(original_flags);
1710 }
vpMatrix::svd
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:1791
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
vpMatrix::pseudoInverse
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:1931
vpCalibrationException
Error that can be emited by the vpCalibration class.
Definition: vpCalibrationException.h:69
vpCameraParameters::get_kdu
double get_kdu() const
Definition: vpCameraParameters.h:331
vpColVector::stack
void stack(const double &d)
Definition: vpColVector.cpp:837
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:92
vpMatrix::AtA
vpMatrix AtA() const
Definition: vpMatrix.cpp:524
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:290
vpMatrix::stack
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4447
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:643
vpArray2D::size
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:158
vpCalibration
Tools for perspective camera calibration.
Definition: vpCalibration.h:71
vpRotationMatrix::t
vpRotationMatrix t() const
Definition: vpRotationMatrix.cpp:385
vpMath::sinc
static double sinc(double x)
Definition: vpMath.cpp:170
vpCalibration::cMo
vpHomogeneousMatrix cMo
(as a 3x4 matrix [R T])
Definition: vpCalibration.h:93
vpRotationMatrix
Implementation of a rotation matrix and operations on such kind of matrices.
Definition: vpRotationMatrix.h:71
vpCameraParameters::initPersProjWithoutDistortion
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
Definition: vpCameraParameters.cpp:183
vpHomogeneousMatrix::insert
void insert(const vpRotationMatrix &R)
Definition: vpHomogeneousMatrix.cpp:592
vpMatrix::t
vpMatrix t() const
Definition: vpMatrix.cpp:375
vpImagePoint::get_u
double get_u() const
Definition: vpImagePoint.h:263
vpMath::sqr
static double sqr(double x)
Definition: vpMath.h:108
vp
Definition: vpContours.h:157
vpCalibration::cMo_dist
vpHomogeneousMatrix cMo_dist
Definition: vpCalibration.h:95
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:232
vpCalibration::rMe
vpHomogeneousMatrix rMe
reference coordinates (manipulator base coordinates)
Definition: vpCalibration.h:103
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:72
vpCalibration::calibrationTsai
static void calibrationTsai(const std::vector< vpHomogeneousMatrix > &cMo, const std::vector< vpHomogeneousMatrix > &rMe, vpHomogeneousMatrix &eMc)
calibration method of effector-camera from R. Tsai and R. Lorenz .
Definition: vpCalibrationTools.cpp:1069
vpCalibrationException::convergencyError
iterative algorithm doesn&#39;t converge
Definition: vpCalibrationException.h:86
vpColVector::sumSquare
double sumSquare() const
Definition: vpColVector.cpp:1360
vpExponentialMap::direct
static vpHomogeneousMatrix direct(const vpColVector &v)
Definition: vpExponentialMap.cpp:59
vpCalibration::eMc
vpHomogeneousMatrix eMc
position of the camera in relation to the effector
Definition: vpCalibration.h:105
vpCameraParameters::get_kud
double get_kud() const
Definition: vpCameraParameters.h:330
vpColVector::skew
static vpMatrix skew(const vpColVector &v)
Definition: vpColVector.cpp:1039
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
vpTranslationVector
Class that consider the case of a translation vector.
Definition: vpTranslationVector.h:91
vpThetaUVector
Implementation of a rotation vector as axis-angle minimal representation.
Definition: vpThetaUVector.h:146
vpCameraParameters::initPersProjWithDistortion
void initPersProjWithDistortion(const double px, const double py, const double u0, const double v0, const double kud, const double kdu)
Definition: vpCameraParameters.cpp:249
vpImagePoint::get_v
double get_v() const
Definition: vpImagePoint.h:274