34 #include <visp3/core/vpMath.h>
35 #include <visp3/core/vpPixelMeterConversion.h>
36 #include <visp3/vision/vpCalibration.h>
37 #include <visp3/vision/vpPose.h>
42 #define DEBUG_LEVEL1 0
43 #define DEBUG_LEVEL2 0
54 std::list<double>::const_iterator it_LoX = m_LoX.begin();
55 std::list<double>::const_iterator it_LoY = m_LoY.begin();
56 std::list<double>::const_iterator it_LoZ = m_LoZ.begin();
57 std::list<vpImagePoint>::const_iterator it_Lip = m_Lip.begin();
61 for (
unsigned int i = 0; i < m_npt; i++) {
69 double xi = ip.
get_u();
70 double yi = ip.
get_v();
72 A[2 * i][0] = x0 * xi;
73 A[2 * i][1] = y0 * xi;
74 A[2 * i][2] = z0 * 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;
120 e = -(A.t() * B) * r;
134 unsigned int imin = 1;
135 for (
unsigned int i = 0; i < x1.getRows(); i++) {
144 for (
unsigned int i = 0; i < x1.getRows(); i++) {
145 if (x1[i] < x1[imin])
149 for (
unsigned int i = 0; i < x1.getRows(); i++)
150 x1[i] = AtA[i][imin];
156 for (
unsigned int i = 0; i < 3; i++)
158 for (
unsigned int i = 0; i < 9; i++)
164 for (
unsigned int i = 0; i < 12; i++)
167 resul[0] = sol[3] * sol[0] + sol[4] * sol[1] + sol[5] * sol[2];
169 resul[1] = sol[6] * sol[0] + sol[7] * sol[1] + sol[8] * sol[2];
171 resul[2] = sqrt(sol[3] * sol[3] + sol[4] * sol[4] + sol[5] * sol[5]
172 - resul[0] * resul[0]);
178 resul[3] = sqrt(sol[6] * sol[6] + sol[7] * sol[7] + sol[8] * sol[8]
179 - resul[1] * resul[1]);
184 resul[4] = (sol[9] - sol[11] * resul[0]) / resul[2];
185 resul[5] = (sol[10] - sol[11] * resul[1]) / resul[3];
190 for (
unsigned int i = 0; i < 3; i++)
191 rd[0][i] = (sol[i + 3] - sol[i] * resul[0]) / resul[2];
192 for (
unsigned int i = 0; i < 3; i++)
193 rd[1][i] = (sol[i + 6] - sol[i] * resul[1]) / resul[3];
194 for (
unsigned int i = 0; i < 3; i++)
200 for (
unsigned int i = 0; i < 3; i++) {
201 for (
unsigned int j = 0; j < 3; j++)
202 cMo_est[i][j] = rd[i][j];
204 for (
unsigned int i = 0; i < 3; i++)
205 cMo_est[i][3] = resul[i + 4];
210 double deviation, deviation_dist;
216 std::ios::fmtflags original_flags(std::cout.flags());
217 std::cout.precision(10);
218 unsigned int n_points = m_npt;
231 std::list<double>::const_iterator it_LoX = m_LoX.begin();
232 std::list<double>::const_iterator it_LoY = m_LoY.begin();
233 std::list<double>::const_iterator it_LoZ = m_LoZ.begin();
234 std::list<vpImagePoint>::const_iterator it_Lip = m_Lip.begin();
236 for (
unsigned int i = 0; i < n_points; i++) {
253 unsigned int iter = 0;
255 double residu_1 = 1e12;
257 while (
vpMath::equal(residu_1, r, m_threshold) ==
false && iter < m_nbIterMax) {
270 double u0 = cam_est.
get_u0();
271 double v0 = cam_est.
get_v0();
275 for (
unsigned int i = 0; i < n_points; i++) {
276 cX[i] = oX[i] * cMo_est[0][0] + oY[i] * cMo_est[0][1] + oZ[i] * cMo_est[0][2] + cMo_est[0][3];
277 cY[i] = oX[i] * cMo_est[1][0] + oY[i] * cMo_est[1][1] + oZ[i] * cMo_est[1][2] + cMo_est[1][3];
278 cZ[i] = oX[i] * cMo_est[2][0] + oY[i] * cMo_est[2][1] + oZ[i] * cMo_est[2][2] + cMo_est[2][3];
281 Pd[2 * i + 1] = v[i];
283 P[2 * i] = cX[i] / cZ[i] * px + u0;
284 P[2 * i + 1] = cY[i] / cZ[i] * py + v0;
294 for (
unsigned int i = 0; i < n_points; i++) {
298 double inv_z = 1 / z;
300 double X = x * inv_z;
301 double Y = y * inv_z;
305 L[2 * i][0] = px * (-inv_z);
307 L[2 * i][2] = px * X * inv_z;
308 L[2 * i][3] = px * X * Y;
309 L[2 * i][4] = -px * (1 + X * X);
310 L[2 * i][5] = px * Y;
327 L[2 * i + 1][1] = py * (-inv_z);
328 L[2 * i + 1][2] = py * (Y * inv_z);
329 L[2 * i + 1][3] = py * (1 + Y * Y);
330 L[2 * i + 1][4] = -py * X * Y;
331 L[2 * i + 1][5] = -py * X;
346 Lp = L.pseudoInverse(1e-10);
355 for (
unsigned int i = 0; i < 6; i++)
367 std::cout <<
" std dev " << sqrt(r / n_points) << std::endl;
369 if (iter == m_nbIterMax) {
370 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)", m_nbIterMax);
375 this->m_residual = r;
376 this->m_residual_dist = r;
378 std::cout <<
" std dev " << sqrt(r / n_points) << std::endl;
380 std::cout.flags(original_flags);
383 void vpCalibration::calibVVSMulti(std::vector<vpCalibration> &table_cal,
vpCameraParameters &cam_est,
384 double &globalReprojectionError,
bool verbose,
double aspect_ratio)
386 std::ios::fmtflags original_flags(std::cout.flags());
387 std::cout.precision(10);
388 unsigned int nbPoint[256];
389 unsigned int nbPointTotal = 0;
390 unsigned int nbPose = (
unsigned int)table_cal.size();
391 unsigned int nbPose6 = 6 * nbPose;
393 for (
unsigned int i = 0; i < nbPose; i++) {
394 nbPoint[i] = table_cal[i].m_npt;
395 nbPointTotal += nbPoint[i];
398 if (nbPointTotal < 4) {
413 unsigned int curPoint = 0;
414 for (
unsigned int p = 0; p < nbPose; p++) {
415 std::list<double>::const_iterator it_LoX = table_cal[p].m_LoX.begin();
416 std::list<double>::const_iterator it_LoY = table_cal[p].m_LoY.begin();
417 std::list<double>::const_iterator it_LoZ = table_cal[p].m_LoZ.begin();
418 std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].m_Lip.begin();
420 for (
unsigned int i = 0; i < nbPoint[p]; i++) {
421 oX[curPoint] = *it_LoX;
422 oY[curPoint] = *it_LoY;
423 oZ[curPoint] = *it_LoZ;
426 u[curPoint] = ip.
get_u();
427 v[curPoint] = ip.
get_v();
438 unsigned int iter = 0;
440 double residu_1 = 1e12;
442 while (
vpMath::equal(residu_1, r, m_threshold) ==
false && iter < m_nbIterMax) {
448 if (aspect_ratio > 0.) {
450 py = px / aspect_ratio;
456 double u0 = cam_est.
get_u0();
457 double v0 = cam_est.
get_v0();
461 for (
unsigned int p = 0; p < nbPose; p++) {
463 for (
unsigned int i = 0; i < nbPoint[p]; i++) {
464 unsigned int curPoint2 = 2 * curPoint;
467 oX[curPoint] * cMoTmp[0][0] + oY[curPoint] * cMoTmp[0][1] + oZ[curPoint] * cMoTmp[0][2] + cMoTmp[0][3];
469 oX[curPoint] * cMoTmp[1][0] + oY[curPoint] * cMoTmp[1][1] + oZ[curPoint] * cMoTmp[1][2] + cMoTmp[1][3];
471 oX[curPoint] * cMoTmp[2][0] + oY[curPoint] * cMoTmp[2][1] + oZ[curPoint] * cMoTmp[2][2] + cMoTmp[2][3];
473 Pd[curPoint2] = u[curPoint];
474 Pd[curPoint2 + 1] = v[curPoint];
476 P[curPoint2] = cX[curPoint] / cZ[curPoint] * px + u0;
477 P[curPoint2 + 1] = cY[curPoint] / cZ[curPoint] * py + v0;
488 vpMatrix L(nbPointTotal * 2, nbPose6 + 3 + (aspect_ratio > 0. ? 0 : 1));
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;
496 double x = cX[curPoint];
497 double y = cY[curPoint];
498 double z = cZ[curPoint];
500 double inv_z = 1 / z;
502 double X = x * inv_z;
503 double Y = y * inv_z;
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;
516 L[curPoint2][nbPose6] = 1;
517 L[curPoint2][nbPose6 + 1] = 0;
518 if (aspect_ratio > 0.) {
519 L[curPoint2][nbPose6 + 2] = X;
522 L[curPoint2][nbPose6 + 2] = X;
523 L[curPoint2][nbPose6 + 3] = 0;
527 L[curPoint21][q] = 0;
528 L[curPoint21][q + 1] = py * (-inv_z);
529 L[curPoint21][q + 2] = py * (Y * inv_z);
530 L[curPoint21][q + 3] = py * (1 + Y * Y);
531 L[curPoint21][q + 4] = -py * X * Y;
532 L[curPoint21][q + 5] = -py * X;
535 L[curPoint21][nbPose6] = 0;
536 L[curPoint21][nbPose6 + 1] = 1;
537 if (aspect_ratio > 0.) {
538 L[curPoint21][nbPose6 + 2] = Y;
541 L[curPoint21][nbPose6 + 2] = 0;
542 L[curPoint21][nbPose6 + 3] = Y;
550 Lp = L.pseudoInverse(1e-10);
559 for (
unsigned int i = 0; i < nbPose6; i++)
562 if (aspect_ratio > 0.) {
564 u0 + Tc[nbPose6], v0 + Tc[nbPose6 + 1]);
569 v0 + Tc[nbPose6 + 1]);
575 for (
unsigned int p = 0; p < nbPose; p++) {
576 for (
unsigned int i = 0; i < 6; i++)
577 Tc_v_Tmp[i] = Tc_v[6 * p + i];
583 std::cout <<
" std dev " << sqrt(r / nbPointTotal) << std::endl;
585 if (iter == m_nbIterMax) {
586 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)", m_nbIterMax);
589 for (
unsigned int p = 0; p < nbPose; p++) {
590 table_cal[p].cMo_dist = table_cal[p].cMo;
591 table_cal[p].cam = cam_est;
592 table_cal[p].cam_dist = cam_est;
593 double deviation, deviation_dist;
594 table_cal[p].computeStdDeviation(deviation, deviation_dist);
596 globalReprojectionError = sqrt(r / nbPointTotal);
598 std::cout.flags(original_flags);
603 std::ios::fmtflags original_flags(std::cout.flags());
604 std::cout.precision(10);
605 unsigned int n_points = m_npt;
616 std::list<double>::const_iterator it_LoX = m_LoX.begin();
617 std::list<double>::const_iterator it_LoY = m_LoY.begin();
618 std::list<double>::const_iterator it_LoZ = m_LoZ.begin();
619 std::list<vpImagePoint>::const_iterator it_Lip = m_Lip.begin();
623 for (
unsigned int i = 0; i < n_points; i++) {
639 unsigned int iter = 0;
641 double residu_1 = 1e12;
643 while (
vpMath::equal(residu_1, r, m_threshold) ==
false && iter < m_nbIterMax) {
648 double u0 = cam_est.
get_u0();
649 double v0 = cam_est.
get_v0();
661 double inv_px = 1 / px;
662 double inv_py = 1 / py;
664 double kud = cam_est.
get_kud();
665 double kdu = cam_est.
get_kdu();
667 double k2ud = 2 * kud;
668 double k2du = 2 * kdu;
671 for (
unsigned int i = 0; i < n_points; i++) {
672 unsigned int i4 = 4 * i;
673 unsigned int i41 = 4 * i + 1;
674 unsigned int i42 = 4 * i + 2;
675 unsigned int i43 = 4 * i + 3;
677 cX[i] = oX[i] * cMo_est[0][0] + oY[i] * cMo_est[0][1] + oZ[i] * cMo_est[0][2] + cMo_est[0][3];
678 cY[i] = oX[i] * cMo_est[1][0] + oY[i] * cMo_est[1][1] + oZ[i] * cMo_est[1][2] + cMo_est[1][3];
679 cZ[i] = oX[i] * cMo_est[2][0] + oY[i] * cMo_est[2][1] + oZ[i] * cMo_est[2][2] + cMo_est[2][3];
684 double inv_z = 1 / z;
686 double X = x * inv_z;
687 double Y = y * inv_z;
699 double up0 = up - u0;
700 double vp0 =
vp - v0;
702 double xp0 = up0 * inv_px;
703 double xp02 = xp0 * xp0;
705 double yp0 = vp0 * inv_py;
706 double yp02 = yp0 * yp0;
708 double r2du = xp02 + yp02;
709 double kr2du = kdu * r2du;
711 P[i4] = u0 + px * X - kr2du * (up0);
712 P[i41] = v0 + py * Y - kr2du * (vp0);
714 double r2ud = X2 + Y2;
715 double kr2ud = 1 + kud * r2ud;
717 double Axx = px * (kr2ud + k2ud * X2);
718 double Axy = px * k2ud * XY;
719 double Ayy = py * (kr2ud + k2ud * Y2);
720 double Ayx = py * k2ud * XY;
725 P[i42] = u0 + px * X * kr2ud;
726 P[i43] = v0 + py * Y * kr2ud;
735 L[i4][0] = px * (-inv_z);
737 L[i4][2] = px * X * inv_z;
738 L[i4][3] = px * X * Y;
739 L[i4][4] = -px * (1 + X2);
743 L[i4][6] = 1 + kr2du + k2du * xp02;
744 L[i4][7] = k2du * up0 * yp0 * inv_py;
747 L[i4][8] = X + k2du * xp02 * xp0 + k2du * up0 * yp02 * inv_py;
748 L[i4][9] = -(up0) * (r2du);
753 L[i4][8] = X + k2du * xp02 * xp0;
754 L[i4][9] = k2du * up0 * yp02 * inv_py;
755 L[i4][10] = -(up0) * (r2du);
761 L[i41][1] = py * (-inv_z);
762 L[i41][2] = py * Y * inv_z;
763 L[i41][3] = py * (1 + Y2);
764 L[i41][4] = -py * XY;
768 L[i41][6] = k2du * xp0 * vp0 * inv_px;
769 L[i41][7] = 1 + kr2du + k2du * yp02;
771 L[i41][8] = k2du * vp0 * xp02 * inv_px + Y + k2du * yp02 * yp0;
772 L[i41][9] = -vp0 * r2du;
777 L[i41][8] = k2du * vp0 * xp02 * inv_px;
778 L[i41][9] = Y + k2du * yp02 * yp0;
779 L[i41][10] = -vp0 * r2du;
785 L[i42][0] = Axx * (-inv_z);
786 L[i42][1] = Axy * (-inv_z);
787 L[i42][2] = Axx * (X * inv_z) + Axy * (Y * inv_z);
788 L[i42][3] = Axx * X * Y + Axy * (1 + Y2);
789 L[i42][4] = -Axx * (1 + X2) - Axy * XY;
790 L[i42][5] = Axx * Y - Axy * X;
796 L[i42][8] = X * kr2ud;
798 L[i42][10] = px * X * r2ud;
802 L[i42][8] = X * kr2ud;
805 L[i42][11] = px * X * r2ud;
809 L[i43][0] = Ayx * (-inv_z);
810 L[i43][1] = Ayy * (-inv_z);
811 L[i43][2] = Ayx * (X * inv_z) + Ayy * (Y * inv_z);
812 L[i43][3] = Ayx * XY + Ayy * (1 + Y2);
813 L[i43][4] = -Ayx * (1 + X2) - Ayy * XY;
814 L[i43][5] = Ayx * Y - Ayy * X;
820 L[i43][8] = Y * kr2ud;
822 L[i43][10] = py * Y * r2ud;
826 L[i43][9] = Y * kr2ud;
828 L[i43][11] = py * Y * r2ud;
839 Lp = L.pseudoInverse(1e-10);
847 for (
unsigned int i = 0; i < 6; i++)
852 kud + Tc[10], kdu + Tc[9]);
860 std::cout <<
" std dev " << sqrt(r / n_points) << std::endl;
862 if (iter == m_nbIterMax) {
863 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)", m_nbIterMax);
866 this->m_residual_dist = r;
871 std::cout <<
" std dev " << sqrt(r / n_points) << std::endl;
874 std::cout.flags(original_flags);
877 void vpCalibration::calibVVSWithDistortionMulti(std::vector<vpCalibration> &table_cal,
vpCameraParameters &cam_est,
878 double &globalReprojectionError,
bool verbose,
double aspect_ratio)
880 std::ios::fmtflags original_flags(std::cout.flags());
881 std::cout.precision(10);
882 unsigned int nbPoint[1024];
883 unsigned int nbPointTotal = 0;
884 unsigned int nbPose = (
unsigned int)table_cal.size();
885 unsigned int nbPose6 = 6 * nbPose;
886 for (
unsigned int i = 0; i < nbPose; i++) {
887 nbPoint[i] = table_cal[i].m_npt;
888 nbPointTotal += nbPoint[i];
891 if (nbPointTotal < 4) {
906 unsigned int curPoint = 0;
907 for (
unsigned int p = 0; p < nbPose; p++) {
908 std::list<double>::const_iterator it_LoX = table_cal[p].m_LoX.begin();
909 std::list<double>::const_iterator it_LoY = table_cal[p].m_LoY.begin();
910 std::list<double>::const_iterator it_LoZ = table_cal[p].m_LoZ.begin();
911 std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].m_Lip.begin();
913 for (
unsigned int i = 0; i < nbPoint[p]; i++) {
914 oX[curPoint] = *it_LoX;
915 oY[curPoint] = *it_LoY;
916 oZ[curPoint] = *it_LoZ;
919 u[curPoint] = ip.
get_u();
920 v[curPoint] = ip.
get_v();
930 unsigned int iter = 0;
932 double residu_1 = 1e12;
934 while (
vpMath::equal(residu_1, r, m_threshold) ==
false && iter < m_nbIterMax) {
940 for (
unsigned int p = 0; p < nbPose; p++) {
942 for (
unsigned int i = 0; i < nbPoint[p]; i++) {
944 oX[curPoint] * cMoTmp[0][0] + oY[curPoint] * cMoTmp[0][1] + oZ[curPoint] * cMoTmp[0][2] + cMoTmp[0][3];
946 oX[curPoint] * cMoTmp[1][0] + oY[curPoint] * cMoTmp[1][1] + oZ[curPoint] * cMoTmp[1][2] + cMoTmp[1][3];
948 oX[curPoint] * cMoTmp[2][0] + oY[curPoint] * cMoTmp[2][1] + oZ[curPoint] * cMoTmp[2][2] + cMoTmp[2][3];
954 vpMatrix L(nbPointTotal * 4, nbPose6 + 5 + (aspect_ratio > 0. ? 0 : 1));
957 if (aspect_ratio > 0.) {
959 py = px / aspect_ratio;
965 double u0 = cam_est.
get_u0();
966 double v0 = cam_est.
get_v0();
968 double inv_px = 1 / px;
969 double inv_py = 1 / py;
971 double kud = cam_est.
get_kud();
972 double kdu = cam_est.
get_kdu();
974 double k2ud = 2 * kud;
975 double k2du = 2 * kdu;
977 for (
unsigned int p = 0; p < nbPose; p++) {
978 unsigned int q = 6 * p;
979 for (
unsigned int i = 0; i < nbPoint[p]; i++) {
980 unsigned int curPoint4 = 4 * curPoint;
981 double x = cX[curPoint];
982 double y = cY[curPoint];
983 double z = cZ[curPoint];
985 double inv_z = 1 / z;
986 double X = x * inv_z;
987 double Y = y * inv_z;
993 double up = u[curPoint];
994 double vp = v[curPoint];
997 Pd[curPoint4 + 1] =
vp;
999 double up0 = up - u0;
1000 double vp0 =
vp - v0;
1002 double xp0 = up0 * inv_px;
1003 double xp02 = xp0 * xp0;
1005 double yp0 = vp0 * inv_py;
1006 double yp02 = yp0 * yp0;
1008 double r2du = xp02 + yp02;
1009 double kr2du = kdu * r2du;
1011 P[curPoint4] = u0 + px * X - kr2du * (up0);
1012 P[curPoint4 + 1] = v0 + py * Y - kr2du * (vp0);
1014 double r2ud = X2 + Y2;
1015 double kr2ud = 1 + kud * r2ud;
1017 double Axx = px * (kr2ud + k2ud * X2);
1018 double Axy = px * k2ud * XY;
1019 double Ayy = py * (kr2ud + k2ud * Y2);
1020 double Ayx = py * k2ud * XY;
1022 Pd[curPoint4 + 2] = up;
1023 Pd[curPoint4 + 3] =
vp;
1025 P[curPoint4 + 2] = u0 + px * X * kr2ud;
1026 P[curPoint4 + 3] = v0 + py * Y * kr2ud;
1032 unsigned int curInd = curPoint4;
1036 L[curInd][q] = px * (-inv_z);
1037 L[curInd][q + 1] = 0;
1038 L[curInd][q + 2] = px * X * inv_z;
1039 L[curInd][q + 3] = px * X * Y;
1040 L[curInd][q + 4] = -px * (1 + X2);
1041 L[curInd][q + 5] = px * Y;
1044 L[curInd][nbPose6] = 1 + kr2du + k2du * xp02;
1045 L[curInd][nbPose6 + 1] = k2du * up0 * yp0 * inv_py;
1046 if (aspect_ratio > 0.) {
1047 L[curInd][nbPose6 + 2] = X + k2du * xp02 * xp0 + k2du * up0 * yp02 * inv_py;
1048 L[curInd][nbPose6 + 3] = -(up0) * (r2du);
1049 L[curInd][nbPose6 + 4] = 0;
1052 L[curInd][nbPose6 + 2] = X + k2du * xp02 * xp0;
1053 L[curInd][nbPose6 + 3] = k2du * up0 * yp02 * inv_py;
1054 L[curInd][nbPose6 + 4] = -(up0) * (r2du);
1055 L[curInd][nbPose6 + 5] = 0;
1061 L[curInd][q + 1] = py * (-inv_z);
1062 L[curInd][q + 2] = py * Y * inv_z;
1063 L[curInd][q + 3] = py * (1 + Y2);
1064 L[curInd][q + 4] = -py * XY;
1065 L[curInd][q + 5] = -py * X;
1068 L[curInd][nbPose6] = k2du * xp0 * vp0 * inv_px;
1069 L[curInd][nbPose6 + 1] = 1 + kr2du + k2du * yp02;
1070 if (aspect_ratio > 0.) {
1071 L[curInd][nbPose6 + 2] = k2du * vp0 * xp02 * inv_px + Y + k2du * yp02 * yp0;
1072 L[curInd][nbPose6 + 3] = -vp0 * r2du;
1073 L[curInd][nbPose6 + 4] = 0;
1076 L[curInd][nbPose6 + 2] = k2du * vp0 * xp02 * inv_px;
1077 L[curInd][nbPose6 + 3] = Y + k2du * yp02 * yp0;
1078 L[curInd][nbPose6 + 4] = -vp0 * r2du;
1079 L[curInd][nbPose6 + 5] = 0;
1085 L[curInd][q] = Axx * (-inv_z);
1086 L[curInd][q + 1] = Axy * (-inv_z);
1087 L[curInd][q + 2] = Axx * (X * inv_z) + Axy * (Y * inv_z);
1088 L[curInd][q + 3] = Axx * X * Y + Axy * (1 + Y2);
1089 L[curInd][q + 4] = -Axx * (1 + X2) - Axy * XY;
1090 L[curInd][q + 5] = Axx * Y - Axy * X;
1093 L[curInd][nbPose6] = 1;
1094 L[curInd][nbPose6 + 1] = 0;
1095 if (aspect_ratio > 0.) {
1096 L[curInd][nbPose6 + 2] = X * kr2ud;
1097 L[curInd][nbPose6 + 3] = 0;
1098 L[curInd][nbPose6 + 4] = px * X * r2ud;
1101 L[curInd][nbPose6 + 2] = X * kr2ud;
1102 L[curInd][nbPose6 + 3] = 0;
1103 L[curInd][nbPose6 + 4] = 0;
1104 L[curInd][nbPose6 + 5] = px * X * r2ud;
1109 L[curInd][q] = Ayx * (-inv_z);
1110 L[curInd][q + 1] = Ayy * (-inv_z);
1111 L[curInd][q + 2] = Ayx * (X * inv_z) + Ayy * (Y * inv_z);
1112 L[curInd][q + 3] = Ayx * XY + Ayy * (1 + Y2);
1113 L[curInd][q + 4] = -Ayx * (1 + X2) - Ayy * XY;
1114 L[curInd][q + 5] = Ayx * Y - Ayy * X;
1117 L[curInd][nbPose6] = 0;
1118 L[curInd][nbPose6 + 1] = 1;
1119 if (aspect_ratio > 0.) {
1120 L[curInd][nbPose6 + 2] = Y * kr2ud;
1121 L[curInd][nbPose6 + 3] = 0;
1122 L[curInd][nbPose6 + 4] = py * Y * r2ud;
1125 L[curInd][nbPose6 + 2] = 0;
1126 L[curInd][nbPose6 + 3] = Y * kr2ud;
1127 L[curInd][nbPose6 + 4] = 0;
1128 L[curInd][nbPose6 + 5] = py * Y * r2ud;
1142 L.pseudoInverse(Lp, 1e-10);
1147 for (
unsigned int i = 0; i < 6 * nbPose; i++)
1150 if (aspect_ratio > 0.) {
1152 v0 + Tc[nbPose6 + 1], kud + Tc[nbPose6 + 4], kdu + Tc[nbPose6 + 3]);
1156 v0 + Tc[nbPose6 + 1], kud + Tc[nbPose6 + 5], kdu + Tc[nbPose6 + 4]);
1160 for (
unsigned int p = 0; p < nbPose; p++) {
1161 for (
unsigned int i = 0; i < 6; i++)
1162 Tc_v_Tmp[i] = Tc_v[6 * p + i];
1167 std::cout <<
" std dev: " << sqrt(r / nbPointTotal) << std::endl;
1170 if (iter == m_nbIterMax) {
1171 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)", m_nbIterMax);
1178 for (
unsigned int p = 0; p < nbPose; p++) {
1179 table_cal[p].cam_dist = cam_est;
1181 table_cal[p].computeStdDeviation_dist(table_cal[p].
cMo_dist, cam_est);
1185 globalReprojectionError = sqrt(r / (nbPointTotal));
1188 std::cout <<
" Global std dev " << globalReprojectionError << std::endl;
1191 std::cout.flags(original_flags);
1195 bool verbose,
double aspect_ratio)
1197 std::vector<vpCalibration> v_table_cal(nbPose);
1198 double globalReprojectionError = 0;
1199 for (
unsigned int i = 0; i < nbPose; i++) {
1200 v_table_cal[i] = table_cal[i];
1203 calibVVSMulti(v_table_cal, cam_est, globalReprojectionError, verbose, aspect_ratio);
1205 for (
unsigned int i = 0; i < nbPose; i++) {
1206 table_cal[i] = v_table_cal[i];
1210 void vpCalibration::calibVVSWithDistortionMulti(
unsigned int nbPose,
vpCalibration table_cal[],
1213 std::vector<vpCalibration> v_table_cal(nbPose);
1214 double globalReprojectionError = 0;
1215 for (
unsigned int i = 0; i < nbPose; i++) {
1216 v_table_cal[i] = table_cal[i];
1219 calibVVSWithDistortionMulti(v_table_cal, cam_est, globalReprojectionError, verbose, aspect_ratio);
1221 for (
unsigned int i = 0; i < nbPose; i++) {
1222 table_cal[i] = v_table_cal[i];
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)
vpCameraParameters cam_dist
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.
static vpHomogeneousMatrix direct(const vpColVector &v)
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 ...
static double sqr(double x)
static bool equal(double x, double y, double threshold=0.001)
Implementation of a matrix and operations on matrices.
void svd(vpColVector &w, vpMatrix &V)
vpMatrix pseudoInverse(double svThreshold=1e-6) const