40 #include <visp3/core/vpMath.h>
41 #include <visp3/vision/vpHomography.h>
48 const double vpHomography::sing_threshold = 0.0001;
98 #ifndef DOXYGEN_SHOULD_SKIP_THIS
131 double distanceFictive;
132 double sinusTheta, cosinusTheta, signeSinus = 1;
133 double s, determinantU, determinantV;
134 unsigned int vOrdre[3];
141 #ifdef DEBUG_Homographie
142 printf(
"debut : Homographie_EstimationDeplacementCamera\n");
153 for (
unsigned int i = 0; i < 3; i++)
154 for (
unsigned int j = 0; j < 3; j++)
155 mH[i][j] = aHb[i][j];
165 mTempU.svd(svTemp, mTempV);
170 if (svTemp[0] >= svTemp[1]) {
171 if (svTemp[0] >= svTemp[2]) {
172 if (svTemp[1] > svTemp[2]) {
187 if (svTemp[1] >= svTemp[2]) {
188 if (svTemp[0] > svTemp[2]) {
209 for (
unsigned int i = 0; i < 3; i++) {
210 sv[i] = svTemp[vOrdre[i]];
211 for (
unsigned int j = 0; j < 3; j++) {
212 mU[i][j] = mTempU[i][vOrdre[j]];
213 mV[i][j] = mTempV[i][vOrdre[j]];
217 #ifdef DEBUG_Homographie
219 std::cout << mU << std::endl;
221 std::cout << mV << std::endl;
222 printf(
"Valeurs singulieres : ");
227 determinantV = mV.det();
228 determinantU = mU.det();
230 s = determinantU * determinantV;
232 #ifdef DEBUG_Homographie
233 printf(
"s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, s);
239 distanceFictive = sv[1];
240 #ifdef DEBUG_Homographie
241 printf(
"d = %f\n", distanceFictive);
245 if (((sv[0] - sv[1]) < sing_threshold) && (sv[0] - sv[2]) < sing_threshold) {
258 n[0] = normaleDesiree[0];
259 n[1] = normaleDesiree[1];
260 n[2] = normaleDesiree[2];
262 #ifdef DEBUG_Homographie
263 printf(
"\nCas general\n");
277 mX[0][0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1]) / (sv[0] * sv[0] - sv[2] * sv[2]));
279 mX[2][0] = sqrt((sv[1] * sv[1] - sv[2] * sv[2]) / (sv[0] * sv[0] - sv[2] * sv[2]));
281 mX[0][1] = -mX[0][0];
286 double cosinusAncien = 0.0;
287 for (
unsigned int w = 0; w < 2; w++) {
288 for (
unsigned int k = 0; k < 2; k++) {
291 for (
unsigned int i = 0; i < 3; i++) {
292 normaleEstimee[i] = 0.0;
293 for (
unsigned int j = 0; j < 3; j++) {
294 normaleEstimee[i] += (2.0 * k - 1.0) * mV[i][j] * mX[j][w];
299 double cosinusDesireeEstimee = 0.0;
300 for (
unsigned int i = 0; i < 3; i++)
301 cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i];
307 if (cosinusDesireeEstimee > cosinusAncien) {
308 cosinusAncien = cosinusDesireeEstimee;
311 for (
unsigned int j = 0; j < 3; j++)
312 n[j] = normaleEstimee[j];
315 aTbp[0] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[0][w];
316 aTbp[1] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[1][w];
317 aTbp[2] = -(2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[2][w];
330 for (
unsigned int i = 0; i < 3; i++) {
332 for (
unsigned int j = 0; j < 3; j++) {
333 atb[i] += mU[i][j] * aTbp[j];
335 atb[i] /= distanceFictive;
338 #ifdef DEBUG_Homographie
340 std::cout << aTbp.t();
342 std::cout << atb.
t();
355 signeSinus * sqrt((sv[0] * sv[0] - sv[1] * sv[1]) * (sv[1] * sv[1] - sv[2] * sv[2])) / ((sv[0] + sv[2]) * sv[1]);
357 cosinusTheta = (sv[1] * sv[1] + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
360 aRbp[0][0] = cosinusTheta;
362 aRbp[0][2] = -sinusTheta;
366 aRbp[2][0] = sinusTheta;
368 aRbp[2][2] = cosinusTheta;
371 for (
unsigned int i = 0; i < 3; i++) {
372 for (
unsigned int j = 0; j < 3; j++) {
374 for (
unsigned int k = 0; k < 3; k++) {
375 aRbint[i][j] += mU[i][k] * aRbp[k][j];
381 for (
unsigned int i = 0; i < 3; i++) {
382 for (
unsigned int j = 0; j < 3; j++) {
384 for (
unsigned int k = 0; k < 3; k++) {
385 aRb[i][j] += aRbint[i][k] * mV[j][k];
390 #ifdef DEBUG_Homographie
392 std::cout << aRb << std::endl;
423 double distanceFictive;
424 double sinusTheta, cosinusTheta, signeSinus = 1;
425 double s, determinantU, determinantV;
426 unsigned int vOrdre[3];
429 normaleDesiree[0] = 0;
430 normaleDesiree[1] = 0;
431 normaleDesiree[2] = 1;
434 #ifdef DEBUG_Homographie
435 printf(
"debut : Homographie_EstimationDeplacementCamera\n");
446 for (
unsigned int i = 0; i < 3; i++)
447 for (
unsigned int j = 0; j < 3; j++)
448 mH[i][j] = aHb[i][j];
458 mTempU.svd(svTemp, mTempV);
463 if (svTemp[0] >= svTemp[1]) {
464 if (svTemp[0] >= svTemp[2]) {
465 if (svTemp[1] > svTemp[2]) {
480 if (svTemp[1] >= svTemp[2]) {
481 if (svTemp[0] > svTemp[2]) {
502 for (
unsigned int i = 0; i < 3; i++) {
503 sv[i] = svTemp[vOrdre[i]];
504 for (
unsigned int j = 0; j < 3; j++) {
505 mU[i][j] = mTempU[i][vOrdre[j]];
506 mV[i][j] = mTempV[i][vOrdre[j]];
510 #ifdef DEBUG_Homographie
512 std::cout << mU << std::endl;
514 std::cout << mV << std::endl;
515 printf(
"Valeurs singulieres : ");
520 determinantV = mV.det();
521 determinantU = mU.det();
523 s = determinantU * determinantV;
525 #ifdef DEBUG_Homographie
526 printf(
"s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, s);
532 distanceFictive = sv[1];
533 #ifdef DEBUG_Homographie
534 printf(
"d = %f\n", distanceFictive);
538 if (((sv[0] - sv[1]) < sing_threshold) && (sv[0] - sv[2]) < sing_threshold) {
551 n[0] = normaleDesiree[0];
552 n[1] = normaleDesiree[1];
553 n[2] = normaleDesiree[2];
555 #ifdef DEBUG_Homographie
556 printf(
"\nCas general\n");
570 mX[0][0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1]) / (sv[0] * sv[0] - sv[2] * sv[2]));
572 mX[2][0] = sqrt((sv[1] * sv[1] - sv[2] * sv[2]) / (sv[0] * sv[0] - sv[2] * sv[2]));
574 mX[0][1] = -mX[0][0];
579 double cosinusAncien = 0.0;
580 for (
unsigned int w = 0; w < 2; w++) {
581 for (
unsigned int k = 0; k < 2; k++) {
584 for (
unsigned int i = 0; i < 3; i++) {
585 normaleEstimee[i] = 0.0;
586 for (
unsigned int j = 0; j < 3; j++) {
587 normaleEstimee[i] += (2.0 * k - 1.0) * mV[i][j] * mX[j][w];
592 double cosinusDesireeEstimee = 0.0;
593 for (
unsigned int i = 0; i < 3; i++)
594 cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i];
600 if (cosinusDesireeEstimee > cosinusAncien) {
601 cosinusAncien = cosinusDesireeEstimee;
604 for (
unsigned int j = 0; j < 3; j++)
605 n[j] = normaleEstimee[j];
608 aTbp[0] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[0][w];
609 aTbp[1] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[1][w];
610 aTbp[2] = -(2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[2][w];
623 for (
unsigned int i = 0; i < 3; i++) {
625 for (
unsigned int j = 0; j < 3; j++) {
626 atb[i] += mU[i][j] * aTbp[j];
628 atb[i] /= distanceFictive;
631 #ifdef DEBUG_Homographie
633 std::cout << aTbp.t();
635 std::cout << atb.
t();
648 signeSinus * sqrt((sv[0] * sv[0] - sv[1] * sv[1]) * (sv[1] * sv[1] - sv[2] * sv[2])) / ((sv[0] + sv[2]) * sv[1]);
650 cosinusTheta = (sv[1] * sv[1] + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
653 aRbp[0][0] = cosinusTheta;
655 aRbp[0][2] = -sinusTheta;
659 aRbp[2][0] = sinusTheta;
661 aRbp[2][2] = cosinusTheta;
664 for (
unsigned int i = 0; i < 3; i++) {
665 for (
unsigned int j = 0; j < 3; j++) {
667 for (
unsigned int k = 0; k < 3; k++) {
668 aRbint[i][j] += mU[i][k] * aRbp[k][j];
674 for (
unsigned int i = 0; i < 3; i++) {
675 for (
unsigned int j = 0; j < 3; j++) {
677 for (
unsigned int k = 0; k < 3; k++) {
678 aRb[i][j] += aRbint[i][k] * mV[j][k];
683 #ifdef DEBUG_Homographie
685 std::cout << aRb << std::endl;
690 std::list<vpTranslationVector> &vT, std::list<vpColVector> &vN)
693 #ifdef DEBUG_Homographie
694 printf(
"debut : Homographie_EstimationDeplacementCamera\n");
702 int cas1 = 1, cas2 = 2, cas3 = 3, cas4 = 4;
704 bool norm1ok =
false, norm2ok =
false, norm3ok =
false, norm4ok =
false;
706 double tmp, determinantU, determinantV, s, distanceFictive;
707 vpMatrix mTempU(3, 3), mTempV(3, 3), U(3, 3), V(3, 3);
723 unsigned int vOrdre[3];
736 mTempU.
svd(svTemp, mTempV);
740 if (svTemp[0] >= svTemp[1]) {
741 if (svTemp[0] >= svTemp[2]) {
742 if (svTemp[1] > svTemp[2]) {
757 if (svTemp[1] >= svTemp[2]) {
758 if (svTemp[0] > svTemp[2]) {
778 for (
unsigned int i = 0; i < 3; i++) {
779 sv[i] = svTemp[vOrdre[i]];
780 for (
unsigned int j = 0; j < 3; j++) {
781 U[i][j] = mTempU[i][vOrdre[j]];
782 V[i][j] = mTempV[i][vOrdre[j]];
786 #ifdef DEBUG_Homographie
791 printf(
"Valeurs singulieres : ");
796 determinantU = U[0][0] * (U[1][1] * U[2][2] - U[1][2] * U[2][1]) + U[0][1] * (U[1][2] * U[2][0] - U[1][0] * U[2][2]) +
797 U[0][2] * (U[1][0] * U[2][1] - U[1][1] * U[2][0]);
799 determinantV = V[0][0] * (V[1][1] * V[2][2] - V[1][2] * V[2][1]) + V[0][1] * (V[1][2] * V[2][0] - V[1][0] * V[2][2]) +
800 V[0][2] * (V[1][0] * V[2][1] - V[1][1] * V[2][0]);
802 s = determinantU * determinantV;
804 #ifdef DEBUG_Homographie
805 printf(
"s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, s);
818 tmp = H[2][0] * x + H[2][1] * y + H[2][2];
820 if ((tmp / (sv[1] * s)) > 0)
821 distanceFictive = sv[1];
823 distanceFictive = -sv[1];
831 if ((sv[0] - sv[1]) < sing_threshold) {
832 if ((sv[1] - sv[2]) < sing_threshold)
837 if ((sv[1] - sv[2]) < sing_threshold)
864 Nprim1[0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1]) / (sv[0] * sv[0] - sv[2] * sv[2]));
866 Nprim1[2] = sqrt((sv[1] * sv[1] - sv[2] * sv[2]) / (sv[0] * sv[0] - sv[2] * sv[2]));
868 Nprim2[0] = Nprim1[0];
869 Nprim2[2] = -Nprim1[2];
870 Nprim3[0] = -Nprim1[0];
871 Nprim3[2] = Nprim1[2];
872 Nprim4[0] = -Nprim1[0];
873 Nprim4[2] = -Nprim1[2];
898 tmp = N1[0] * x + N1[1] * y + N1[2];
899 tmp /= (distanceFictive * s);
903 tmp = N2[0] * x + N2[1] * y + N2[2];
904 tmp /= (distanceFictive * s);
908 tmp = N3[0] * x + N3[1] * y + N3[2];
909 tmp /= (distanceFictive * s);
913 tmp = N4[0] * x + N4[1] * y + N4[2];
914 tmp /= (distanceFictive * s);
920 tmp = N1[0] * x + N1[1] * y + N1[2];
921 tmp /= (distanceFictive * s);
925 tmp = N2[0] * x + N2[1] * y + N2[2];
926 tmp /= (distanceFictive * s);
932 tmp = N1[0] * x + N1[1] * y + N1[2];
933 tmp /= (distanceFictive * s);
937 tmp = N2[0] * x + N2[1] * y + N2[2];
938 tmp /= (distanceFictive * s);
946 if (distanceFictive > 0) {
951 Rprim1[0][0] = (
vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
953 Rprim1[2][2] = Rprim1[0][0];
957 ((sv[0] + sv[2]) * sv[1]);
959 Rprim1[0][2] = -Rprim1[2][0];
963 Tprim1[0] = Nprim1[0];
965 Tprim1[2] = -Nprim1[2];
967 Tprim1 *= (sv[0] - sv[2]);
973 Rprim2[0][0] = (
vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
975 Rprim2[2][2] = Rprim2[0][0];
979 ((sv[0] + sv[2]) * sv[1]);
981 Rprim2[0][2] = -Rprim2[2][0];
985 Tprim2[0] = Nprim2[0];
987 Tprim2[2] = -Nprim2[2];
989 Tprim2 *= (sv[0] - sv[2]);
995 Rprim3[0][0] = (
vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
997 Rprim3[2][2] = Rprim3[0][0];
1001 ((sv[0] + sv[2]) * sv[1]);
1003 Rprim3[0][2] = -Rprim3[2][0];
1007 Tprim3[0] = Nprim3[0];
1009 Tprim3[2] = -Nprim3[2];
1011 Tprim3 *= (sv[0] - sv[2]);
1017 Rprim4[0][0] = (
vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
1019 Rprim4[2][2] = Rprim4[0][0];
1023 ((sv[0] + sv[2]) * sv[1]);
1025 Rprim4[0][2] = -Rprim4[2][0];
1029 Tprim4[0] = Nprim4[0];
1031 Tprim4[2] = -Nprim4[2];
1033 Tprim4 *= (sv[0] - sv[2]);
1044 Tprim1 *= (sv[2] - sv[0]);
1051 Tprim2 *= (sv[2] - sv[0]);
1059 Tprim1 *= (sv[0] - sv[1]);
1066 Tprim2 *= (sv[0] - sv[1]);
1076 if (distanceFictive < 0) {
1082 Rprim1[0][0] = (sv[0] * sv[2] -
vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1084 Rprim1[2][2] = -Rprim1[0][0];
1088 ((sv[0] - sv[2]) * sv[1]);
1090 Rprim1[0][2] = Rprim1[2][0];
1092 Rprim1[1][1] = -1.0;
1094 Tprim1[0] = Nprim1[0];
1096 Tprim1[2] = Nprim1[2];
1098 Tprim1 *= (sv[0] + sv[2]);
1104 Rprim2[0][0] = (sv[0] * sv[2] -
vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1106 Rprim2[2][2] = -Rprim2[0][0];
1110 ((sv[0] - sv[2]) * sv[1]);
1112 Rprim2[0][2] = Rprim2[2][0];
1114 Rprim2[1][1] = -1.0;
1116 Tprim2[0] = Nprim2[0];
1118 Tprim2[2] = Nprim2[2];
1120 Tprim2 *= (sv[0] + sv[2]);
1126 Rprim3[0][0] = (sv[0] * sv[2] -
vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1128 Rprim3[2][2] = -Rprim3[0][0];
1132 ((sv[0] - sv[2]) * sv[1]);
1134 Rprim3[0][2] = Rprim3[2][0];
1136 Rprim3[1][1] = -1.0;
1138 Tprim3[0] = Nprim3[0];
1140 Tprim3[2] = Nprim3[2];
1142 Tprim3 *= (sv[0] + sv[2]);
1147 Rprim4[0][0] = (sv[0] * sv[2] -
vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1149 Rprim4[2][2] = -Rprim4[0][0];
1153 ((sv[0] - sv[2]) * sv[1]);
1155 Rprim4[0][2] = Rprim4[2][0];
1157 Rprim4[1][1] = -1.0;
1159 Tprim4[0] = Nprim4[0];
1161 Tprim4[2] = Nprim4[2];
1163 Tprim4 *= (sv[0] + sv[2]);
1175 Tprim1 *= (sv[2] + sv[0]);
1184 Tprim2 *= (sv[2] + sv[0]);
1194 Tprim1 *= (sv[2] + sv[0]);
1203 Tprim2 *= (sv[0] + sv[2]);
1213 if ((distanceFictive > 0) || (cas != cas4)) {
1217 R1 = s * U * Rprim1 * V.
t();
1219 T1 /= (distanceFictive * s);
1228 R2 = s * U * Rprim2 * V.
t();
1230 T2 /= (distanceFictive * s);
1239 R3 = s * U * Rprim3 * V.
t();
1241 T3 /= (distanceFictive * s);
1249 R4 = s * U * Rprim4 * V.
t();
1251 T4 /= (distanceFictive * s);
1260 std::cout <<
"On tombe dans le cas particulier ou le mouvement n'est pas "
1281 #ifdef DEBUG_Homographie
1282 printf(
"fin : Homographie_EstimationDeplacementCamera\n");
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
Implementation of an homography and operations on homographies.
void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n)
static double sqr(double x)
Implementation of a matrix and operations on matrices.
void svd(vpColVector &w, vpMatrix &V)
Implementation of a rotation matrix and operations on such kind of matrices.
bool isARotationMatrix(double threshold=1e-6) const
vpRotationMatrix t() const
Class that consider the case of a translation vector.