35 #include <visp3/core/vpMath.h>
36 #include <visp3/vision/vpHomography.h>
43 const double vpHomography::m_sing_threshold = 0.0001;
62 #ifndef DOXYGEN_SHOULD_SKIP_THIS
95 double distanceFictive;
96 double sinusTheta, cosinusTheta, signeSinus = 1;
97 double s, determinantU, determinantV;
98 unsigned int vOrdre[3];
103 #ifdef DEBUG_Homographie
104 printf(
"debut : Homographie_EstimationDeplacementCamera\n");
115 for (
unsigned int i = 0; i < 3; ++i) {
116 for (
unsigned int j = 0; j < 3; ++j) {
117 mH[i][j] = aHb[i][j];
129 mTempU.svd(svTemp, mTempV);
134 if (svTemp[0] >= svTemp[1]) {
135 if (svTemp[0] >= svTemp[2]) {
136 if (svTemp[1] > svTemp[2]) {
151 if (svTemp[1] >= svTemp[2]) {
152 if (svTemp[0] > svTemp[2]) {
173 for (
unsigned int i = 0; i < 3; ++i) {
174 sv[i] = svTemp[vOrdre[i]];
175 for (
unsigned int j = 0; j < 3; ++j) {
176 mU[i][j] = mTempU[i][vOrdre[j]];
177 mV[i][j] = mTempV[i][vOrdre[j]];
181 #ifdef DEBUG_Homographie
183 std::cout << mU << std::endl;
185 std::cout << mV << std::endl;
186 printf(
"Valeurs singulieres : ");
191 determinantV = mV.det();
192 determinantU = mU.det();
194 s = determinantU * determinantV;
196 #ifdef DEBUG_Homographie
197 printf(
"s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, s);
204 distanceFictive = sv[1];
205 #ifdef DEBUG_Homographie
206 printf(
"d = %f\n", distanceFictive);
210 if (((sv[0] - sv[1]) < m_sing_threshold) && ((sv[0] - sv[2]) < m_sing_threshold)) {
220 n[0] = normaleDesiree[0];
221 n[1] = normaleDesiree[1];
222 n[2] = normaleDesiree[2];
224 #ifdef DEBUG_Homographie
225 printf(
"\nCas general\n");
239 mX[0][0] = sqrt(((sv[0] * sv[0]) - (sv[1] * sv[1])) / ((sv[0] * sv[0]) - (sv[2] * sv[2])));
241 mX[2][0] = sqrt(((sv[1] * sv[1]) - (sv[2] * sv[2])) / ((sv[0] * sv[0]) - (sv[2] * sv[2])));
243 mX[0][1] = -mX[0][0];
248 double cosinusAncien = 0.0;
249 for (
unsigned int w = 0; w < 2; ++w) {
250 for (
unsigned int k = 0; k < 2; ++k) {
253 for (
unsigned int i = 0; i < 3; ++i) {
254 normaleEstimee[i] = 0.0;
255 for (
unsigned int j = 0; j < 3; ++j) {
256 normaleEstimee[i] += ((2.0 * k) - 1.0) * mV[i][j] * mX[j][w];
261 double cosinusDesireeEstimee = 0.0;
262 for (
unsigned int i = 0; i < 3; ++i) {
263 cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i];
270 if (cosinusDesireeEstimee > cosinusAncien) {
271 cosinusAncien = cosinusDesireeEstimee;
274 for (
unsigned int j = 0; j < 3; ++j) {
275 n[j] = normaleEstimee[j];
279 aTbp[0] = ((2.0 * k) - 1.0) * (sv[0] - sv[2]) * mX[0][w];
280 aTbp[1] = ((2.0 * k) - 1.0) * (sv[0] - sv[2]) * mX[1][w];
281 aTbp[2] = -((2.0 * k) - 1.0) * (sv[0] - sv[2]) * mX[2][w];
296 for (
unsigned int i = 0; i < 3; ++i) {
298 for (
unsigned int j = 0; j < 3; ++j) {
299 atb[i] += mU[i][j] * aTbp[j];
301 atb[i] /= distanceFictive;
304 #ifdef DEBUG_Homographie
306 std::cout << aTbp.t();
308 std::cout << atb.
t();
321 (signeSinus * sqrt(((sv[0] * sv[0]) - (sv[1] * sv[1])) * ((sv[1] * sv[1]) - (sv[2] * sv[2])))) / ((sv[0] + sv[2]) * sv[1]);
323 cosinusTheta = ((sv[1] * sv[1]) + (sv[0] * sv[2])) / ((sv[0] + sv[2]) * sv[1]);
326 aRbp[0][0] = cosinusTheta;
328 aRbp[0][2] = -sinusTheta;
332 aRbp[2][0] = sinusTheta;
334 aRbp[2][2] = cosinusTheta;
337 for (
unsigned int i = 0; i < 3; ++i) {
338 for (
unsigned int j = 0; j < 3; ++j) {
340 for (
unsigned int k = 0; k < 3; ++k) {
341 aRbint[i][j] += mU[i][k] * aRbp[k][j];
347 for (
unsigned int i = 0; i < 3; ++i) {
348 for (
unsigned int j = 0; j < 3; ++j) {
350 for (
unsigned int k = 0; k < 3; ++k) {
351 aRb[i][j] += aRbint[i][k] * mV[j][k];
356 #ifdef DEBUG_Homographie
358 std::cout << aRb << std::endl;
389 double distanceFictive;
390 double sinusTheta, cosinusTheta, signeSinus = 1;
391 double s, determinantU, determinantV;
392 unsigned int vOrdre[3];
395 normaleDesiree[0] = 0;
396 normaleDesiree[1] = 0;
397 normaleDesiree[2] = 1;
400 #ifdef DEBUG_Homographie
401 printf(
"debut : Homographie_EstimationDeplacementCamera\n");
412 for (
unsigned int i = 0; i < 3; ++i) {
413 for (
unsigned int j = 0; j < 3; ++j) {
414 mH[i][j] = aHb[i][j];
426 mTempU.svd(svTemp, mTempV);
431 if (svTemp[0] >= svTemp[1]) {
432 if (svTemp[0] >= svTemp[2]) {
433 if (svTemp[1] > svTemp[2]) {
448 if (svTemp[1] >= svTemp[2]) {
449 if (svTemp[0] > svTemp[2]) {
470 for (
unsigned int i = 0; i < 3; ++i) {
471 sv[i] = svTemp[vOrdre[i]];
472 for (
unsigned int j = 0; j < 3; ++j) {
473 mU[i][j] = mTempU[i][vOrdre[j]];
474 mV[i][j] = mTempV[i][vOrdre[j]];
478 #ifdef DEBUG_Homographie
480 std::cout << mU << std::endl;
482 std::cout << mV << std::endl;
483 printf(
"Valeurs singulieres : ");
488 determinantV = mV.det();
489 determinantU = mU.det();
491 s = determinantU * determinantV;
493 #ifdef DEBUG_Homographie
494 printf(
"s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, s);
501 distanceFictive = sv[1];
502 #ifdef DEBUG_Homographie
503 printf(
"d = %f\n", distanceFictive);
507 if (((sv[0] - sv[1]) < m_sing_threshold) && ((sv[0] - sv[2]) < m_sing_threshold)) {
517 n[0] = normaleDesiree[0];
518 n[1] = normaleDesiree[1];
519 n[2] = normaleDesiree[2];
521 #ifdef DEBUG_Homographie
522 printf(
"\nCas general\n");
536 mX[0][0] = sqrt(((sv[0] * sv[0]) - (sv[1] * sv[1])) / ((sv[0] * sv[0]) - (sv[2] * sv[2])));
538 mX[2][0] = sqrt(((sv[1] * sv[1]) - (sv[2] * sv[2])) / ((sv[0] * sv[0]) - (sv[2] * sv[2])));
540 mX[0][1] = -mX[0][0];
545 double cosinusAncien = 0.0;
546 for (
unsigned int w = 0; w < 2; ++w) {
547 for (
unsigned int k = 0; k < 2; ++k) {
550 for (
unsigned int i = 0; i < 3; ++i) {
551 normaleEstimee[i] = 0.0;
552 for (
unsigned int j = 0; j < 3; ++j) {
553 normaleEstimee[i] += ((2.0 * k )- 1.0) * mV[i][j] * mX[j][w];
558 double cosinusDesireeEstimee = 0.0;
559 for (
unsigned int i = 0; i < 3; ++i) {
560 cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i];
567 if (cosinusDesireeEstimee > cosinusAncien) {
568 cosinusAncien = cosinusDesireeEstimee;
571 for (
unsigned int j = 0; j < 3; ++j) {
572 n[j] = normaleEstimee[j];
576 aTbp[0] = ((2.0 * k) - 1.0) * (sv[0] - sv[2]) * mX[0][w];
577 aTbp[1] = ((2.0 * k) - 1.0) * (sv[0] - sv[2]) * mX[1][w];
578 aTbp[2] = -((2.0 * k) - 1.0) * (sv[0] - sv[2]) * mX[2][w];
593 for (
unsigned int i = 0; i < 3; ++i) {
595 for (
unsigned int j = 0; j < 3; ++j) {
596 atb[i] += mU[i][j] * aTbp[j];
598 atb[i] /= distanceFictive;
601 #ifdef DEBUG_Homographie
603 std::cout << aTbp.t();
605 std::cout << atb.
t();
618 (signeSinus * sqrt(((sv[0] * sv[0]) - (sv[1] * sv[1])) * ((sv[1] * sv[1]) - (sv[2] * sv[2])))) / ((sv[0] + sv[2]) * sv[1]);
620 cosinusTheta = ((sv[1] * sv[1]) + (sv[0] * sv[2])) / ((sv[0] + sv[2]) * sv[1]);
623 aRbp[0][0] = cosinusTheta;
625 aRbp[0][2] = -sinusTheta;
629 aRbp[2][0] = sinusTheta;
631 aRbp[2][2] = cosinusTheta;
634 for (
unsigned int i = 0; i < 3; ++i) {
635 for (
unsigned int j = 0; j < 3; ++j) {
637 for (
unsigned int k = 0; k < 3; ++k) {
638 aRbint[i][j] += mU[i][k] * aRbp[k][j];
644 for (
unsigned int i = 0; i < 3; ++i) {
645 for (
unsigned int j = 0; j < 3; ++j) {
647 for (
unsigned int k = 0; k < 3; ++k) {
648 aRb[i][j] += aRbint[i][k] * mV[j][k];
653 #ifdef DEBUG_Homographie
655 std::cout << aRb << std::endl;
660 std::list<vpTranslationVector> &vT, std::list<vpColVector> &vN)
663 #ifdef DEBUG_Homographie
664 printf(
"debut : Homographie_EstimationDeplacementCamera\n");
672 int cas1 = 1, cas2 = 2, cas3 = 3, cas4 = 4;
674 bool norm1ok =
false, norm2ok =
false, norm3ok =
false, norm4ok =
false;
676 double tmp, determinantU, determinantV, s, distanceFictive;
677 vpMatrix mTempU(3, 3), mTempV(3, 3), U(3, 3), V(3, 3);
693 unsigned int vOrdre[3];
706 mTempU.
svd(svTemp, mTempV);
710 if (svTemp[0] >= svTemp[1]) {
711 if (svTemp[0] >= svTemp[2]) {
712 if (svTemp[1] > svTemp[2]) {
727 if (svTemp[1] >= svTemp[2]) {
728 if (svTemp[0] > svTemp[2]) {
748 for (
unsigned int i = 0; i < 3; ++i) {
749 sv[i] = svTemp[vOrdre[i]];
750 for (
unsigned int j = 0; j < 3; ++j) {
751 U[i][j] = mTempU[i][vOrdre[j]];
752 V[i][j] = mTempV[i][vOrdre[j]];
756 #ifdef DEBUG_Homographie
761 printf(
"Valeurs singulieres : ");
766 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]))) +
767 (U[0][2] * ((U[1][0] * U[2][1]) - (U[1][1] * U[2][0])));
769 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]))) +
770 (V[0][2] * ((V[1][0] * V[2][1]) - (V[1][1] * V[2][0])));
772 s = determinantU * determinantV;
774 #ifdef DEBUG_Homographie
775 printf(
"s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, s);
788 tmp = (H[2][0] * x) + (H[2][1] * y) + H[2][2];
790 if ((tmp / (sv[1] * s)) > 0) {
791 distanceFictive = sv[1];
794 distanceFictive = -sv[1];
803 if ((sv[0] - sv[1]) < m_sing_threshold) {
804 if ((sv[1] - sv[2]) < m_sing_threshold) {
811 if ((sv[1] - sv[2]) < m_sing_threshold) {
840 Nprim1[0] = sqrt(((sv[0] * sv[0]) - (sv[1] * sv[1])) / ((sv[0] * sv[0]) - (sv[2] * sv[2])));
842 Nprim1[2] = sqrt(((sv[1] * sv[1]) - (sv[2] * sv[2])) / ((sv[0] * sv[0]) - (sv[2] * sv[2])));
844 Nprim2[0] = Nprim1[0];
845 Nprim2[2] = -Nprim1[2];
846 Nprim3[0] = -Nprim1[0];
847 Nprim3[2] = Nprim1[2];
848 Nprim4[0] = -Nprim1[0];
849 Nprim4[2] = -Nprim1[2];
874 tmp = (N1[0] * x) + (N1[1] * y) + N1[2];
875 tmp /= (distanceFictive * s);
879 tmp = (N2[0] * x) + (N2[1] * y) + N2[2];
880 tmp /= (distanceFictive * s);
884 tmp = (N3[0] * x) + (N3[1] * y) + N3[2];
885 tmp /= (distanceFictive * s);
889 tmp = (N4[0] * x) + (N4[1] * y) + N4[2];
890 tmp /= (distanceFictive * s);
896 tmp = (N1[0] * x) + (N1[1] * y) + N1[2];
897 tmp /= (distanceFictive * s);
901 tmp = (N2[0] * x) + (N2[1] * y) + N2[2];
902 tmp /= (distanceFictive * s);
908 tmp = (N1[0] * x) + (N1[1] * y) + N1[2];
909 tmp /= (distanceFictive * s);
913 tmp = (N2[0] * x) + (N2[1] * y) + N2[2];
914 tmp /= (distanceFictive * s);
922 if (distanceFictive > 0) {
927 Rprim1[0][0] = (
vpMath::sqr(sv[1]) + (sv[0] * sv[2])) / ((sv[0] + sv[2]) * sv[1]);
929 Rprim1[2][2] = Rprim1[0][0];
933 ((sv[0] + sv[2]) * sv[1]);
935 Rprim1[0][2] = -Rprim1[2][0];
939 Tprim1[0] = Nprim1[0];
941 Tprim1[2] = -Nprim1[2];
943 Tprim1 *= (sv[0] - sv[2]);
949 Rprim2[0][0] = (
vpMath::sqr(sv[1]) + (sv[0] * sv[2])) / ((sv[0] + sv[2]) * sv[1]);
951 Rprim2[2][2] = Rprim2[0][0];
955 ((sv[0] + sv[2]) * sv[1]);
957 Rprim2[0][2] = -Rprim2[2][0];
961 Tprim2[0] = Nprim2[0];
963 Tprim2[2] = -Nprim2[2];
965 Tprim2 *= (sv[0] - sv[2]);
971 Rprim3[0][0] = (
vpMath::sqr(sv[1]) + (sv[0] * sv[2])) / ((sv[0] + sv[2]) * sv[1]);
973 Rprim3[2][2] = Rprim3[0][0];
977 ((sv[0] + sv[2]) * sv[1]);
979 Rprim3[0][2] = -Rprim3[2][0];
983 Tprim3[0] = Nprim3[0];
985 Tprim3[2] = -Nprim3[2];
987 Tprim3 *= (sv[0] - sv[2]);
993 Rprim4[0][0] = (
vpMath::sqr(sv[1]) + (sv[0] * sv[2])) / ((sv[0] + sv[2]) * sv[1]);
995 Rprim4[2][2] = Rprim4[0][0];
999 ((sv[0] + sv[2]) * sv[1]);
1001 Rprim4[0][2] = -Rprim4[2][0];
1005 Tprim4[0] = Nprim4[0];
1007 Tprim4[2] = -Nprim4[2];
1009 Tprim4 *= (sv[0] - sv[2]);
1020 Tprim1 *= (sv[2] - sv[0]);
1027 Tprim2 *= (sv[2] - sv[0]);
1035 Tprim1 *= (sv[0] - sv[1]);
1042 Tprim2 *= (sv[0] - sv[1]);
1052 if (distanceFictive < 0) {
1058 Rprim1[0][0] = ((sv[0] * sv[2]) -
vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1060 Rprim1[2][2] = -Rprim1[0][0];
1064 ((sv[0] - sv[2]) * sv[1]);
1066 Rprim1[0][2] = Rprim1[2][0];
1068 Rprim1[1][1] = -1.0;
1070 Tprim1[0] = Nprim1[0];
1072 Tprim1[2] = Nprim1[2];
1074 Tprim1 *= (sv[0] + sv[2]);
1080 Rprim2[0][0] = ((sv[0] * sv[2]) -
vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1082 Rprim2[2][2] = -Rprim2[0][0];
1086 ((sv[0] - sv[2]) * sv[1]);
1088 Rprim2[0][2] = Rprim2[2][0];
1090 Rprim2[1][1] = -1.0;
1092 Tprim2[0] = Nprim2[0];
1094 Tprim2[2] = Nprim2[2];
1096 Tprim2 *= (sv[0] + sv[2]);
1102 Rprim3[0][0] = ((sv[0] * sv[2]) -
vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1104 Rprim3[2][2] = -Rprim3[0][0];
1108 ((sv[0] - sv[2]) * sv[1]);
1110 Rprim3[0][2] = Rprim3[2][0];
1112 Rprim3[1][1] = -1.0;
1114 Tprim3[0] = Nprim3[0];
1116 Tprim3[2] = Nprim3[2];
1118 Tprim3 *= (sv[0] + sv[2]);
1123 Rprim4[0][0] = ((sv[0] * sv[2]) -
vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1125 Rprim4[2][2] = -Rprim4[0][0];
1129 ((sv[0] - sv[2]) * sv[1]);
1131 Rprim4[0][2] = Rprim4[2][0];
1133 Rprim4[1][1] = -1.0;
1135 Tprim4[0] = Nprim4[0];
1137 Tprim4[2] = Nprim4[2];
1139 Tprim4 *= (sv[0] + sv[2]);
1151 Tprim1 *= (sv[2] + sv[0]);
1160 Tprim2 *= (sv[2] + sv[0]);
1170 Tprim1 *= (sv[2] + sv[0]);
1179 Tprim2 *= (sv[0] + sv[2]);
1189 if ((distanceFictive > 0) || (cas != cas4)) {
1193 R1 = s * U * Rprim1 * V.
t();
1195 T1 /= (distanceFictive * s);
1204 R2 = s * U * Rprim2 * V.
t();
1206 T2 /= (distanceFictive * s);
1215 R3 = s * U * Rprim3 * V.
t();
1217 T3 /= (distanceFictive * s);
1225 R4 = s * U * Rprim4 * V.
t();
1227 T4 /= (distanceFictive * s);
1236 std::cout <<
"On tombe dans le cas particulier ou le mouvement n'est pas "
1241 #ifdef DEBUG_Homographie
1243 std::cout <<
"Analyse des resultats : "<< std::endl;
1245 std::cout <<
"On est dans le cas 1" << std::endl;
1248 std::cout <<
"On est dans le cas 2" << std::endl;
1251 std::cout <<
"On est dans le cas 3" << std::endl;
1254 std::cout <<
"On est dans le cas 4" << std::endl;
1257 if (distanceFictive < 0) {
1258 std::cout <<
"d'<0" << std::endl;
1261 std::cout <<
"d'>0" << std::endl;
1265 #ifdef DEBUG_Homographie
1266 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.