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