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