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