35 #include <visp3/core/vpMath.h>
36 #include <visp3/vision/vpHomography.h>
43 const double vpHomography::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];
105 #ifdef DEBUG_Homographie
106 printf(
"debut : Homographie_EstimationDeplacementCamera\n");
117 for (
unsigned int i = 0; i < 3; i++)
118 for (
unsigned int j = 0; j < 3; j++)
119 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);
203 distanceFictive = sv[1];
204 #ifdef DEBUG_Homographie
205 printf(
"d = %f\n", distanceFictive);
209 if (((sv[0] - sv[1]) < sing_threshold) && (sv[0] - sv[2]) < sing_threshold) {
222 n[0] = normaleDesiree[0];
223 n[1] = normaleDesiree[1];
224 n[2] = normaleDesiree[2];
226 #ifdef DEBUG_Homographie
227 printf(
"\nCas general\n");
241 mX[0][0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1]) / (sv[0] * sv[0] - sv[2] * sv[2]));
243 mX[2][0] = sqrt((sv[1] * sv[1] - sv[2] * sv[2]) / (sv[0] * sv[0] - sv[2] * sv[2]));
245 mX[0][1] = -mX[0][0];
250 double cosinusAncien = 0.0;
251 for (
unsigned int w = 0; w < 2; w++) {
252 for (
unsigned int k = 0; k < 2; k++) {
255 for (
unsigned int i = 0; i < 3; i++) {
256 normaleEstimee[i] = 0.0;
257 for (
unsigned int j = 0; j < 3; j++) {
258 normaleEstimee[i] += (2.0 * k - 1.0) * mV[i][j] * mX[j][w];
263 double cosinusDesireeEstimee = 0.0;
264 for (
unsigned int i = 0; i < 3; i++)
265 cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i];
271 if (cosinusDesireeEstimee > cosinusAncien) {
272 cosinusAncien = cosinusDesireeEstimee;
275 for (
unsigned int j = 0; j < 3; j++)
276 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];
294 for (
unsigned int i = 0; i < 3; i++) {
296 for (
unsigned int j = 0; j < 3; j++) {
297 atb[i] += mU[i][j] * aTbp[j];
299 atb[i] /= distanceFictive;
302 #ifdef DEBUG_Homographie
304 std::cout << aTbp.t();
306 std::cout << atb.
t();
319 signeSinus * sqrt((sv[0] * sv[0] - sv[1] * sv[1]) * (sv[1] * sv[1] - sv[2] * sv[2])) / ((sv[0] + sv[2]) * sv[1]);
321 cosinusTheta = (sv[1] * sv[1] + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
324 aRbp[0][0] = cosinusTheta;
326 aRbp[0][2] = -sinusTheta;
330 aRbp[2][0] = sinusTheta;
332 aRbp[2][2] = cosinusTheta;
335 for (
unsigned int i = 0; i < 3; i++) {
336 for (
unsigned int j = 0; j < 3; j++) {
338 for (
unsigned int k = 0; k < 3; k++) {
339 aRbint[i][j] += mU[i][k] * aRbp[k][j];
345 for (
unsigned int i = 0; i < 3; i++) {
346 for (
unsigned int j = 0; j < 3; j++) {
348 for (
unsigned int k = 0; k < 3; k++) {
349 aRb[i][j] += aRbint[i][k] * mV[j][k];
354 #ifdef DEBUG_Homographie
356 std::cout << aRb << std::endl;
387 double distanceFictive;
388 double sinusTheta, cosinusTheta, signeSinus = 1;
389 double s, determinantU, determinantV;
390 unsigned int vOrdre[3];
393 normaleDesiree[0] = 0;
394 normaleDesiree[1] = 0;
395 normaleDesiree[2] = 1;
398 #ifdef DEBUG_Homographie
399 printf(
"debut : Homographie_EstimationDeplacementCamera\n");
410 for (
unsigned int i = 0; i < 3; i++)
411 for (
unsigned int j = 0; j < 3; j++)
412 mH[i][j] = aHb[i][j];
422 mTempU.svd(svTemp, mTempV);
427 if (svTemp[0] >= svTemp[1]) {
428 if (svTemp[0] >= svTemp[2]) {
429 if (svTemp[1] > svTemp[2]) {
444 if (svTemp[1] >= svTemp[2]) {
445 if (svTemp[0] > svTemp[2]) {
466 for (
unsigned int i = 0; i < 3; i++) {
467 sv[i] = svTemp[vOrdre[i]];
468 for (
unsigned int j = 0; j < 3; j++) {
469 mU[i][j] = mTempU[i][vOrdre[j]];
470 mV[i][j] = mTempV[i][vOrdre[j]];
474 #ifdef DEBUG_Homographie
476 std::cout << mU << std::endl;
478 std::cout << mV << std::endl;
479 printf(
"Valeurs singulieres : ");
484 determinantV = mV.det();
485 determinantU = mU.det();
487 s = determinantU * determinantV;
489 #ifdef DEBUG_Homographie
490 printf(
"s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, s);
496 distanceFictive = sv[1];
497 #ifdef DEBUG_Homographie
498 printf(
"d = %f\n", distanceFictive);
502 if (((sv[0] - sv[1]) < sing_threshold) && (sv[0] - sv[2]) < sing_threshold) {
515 n[0] = normaleDesiree[0];
516 n[1] = normaleDesiree[1];
517 n[2] = normaleDesiree[2];
519 #ifdef DEBUG_Homographie
520 printf(
"\nCas general\n");
534 mX[0][0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1]) / (sv[0] * sv[0] - sv[2] * sv[2]));
536 mX[2][0] = sqrt((sv[1] * sv[1] - sv[2] * sv[2]) / (sv[0] * sv[0] - sv[2] * sv[2]));
538 mX[0][1] = -mX[0][0];
543 double cosinusAncien = 0.0;
544 for (
unsigned int w = 0; w < 2; w++) {
545 for (
unsigned int k = 0; k < 2; k++) {
548 for (
unsigned int i = 0; i < 3; i++) {
549 normaleEstimee[i] = 0.0;
550 for (
unsigned int j = 0; j < 3; j++) {
551 normaleEstimee[i] += (2.0 * k - 1.0) * mV[i][j] * mX[j][w];
556 double cosinusDesireeEstimee = 0.0;
557 for (
unsigned int i = 0; i < 3; i++)
558 cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i];
564 if (cosinusDesireeEstimee > cosinusAncien) {
565 cosinusAncien = cosinusDesireeEstimee;
568 for (
unsigned int j = 0; j < 3; j++)
569 n[j] = normaleEstimee[j];
572 aTbp[0] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[0][w];
573 aTbp[1] = (2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[1][w];
574 aTbp[2] = -(2.0 * k - 1.0) * (sv[0] - sv[2]) * mX[2][w];
587 for (
unsigned int i = 0; i < 3; i++) {
589 for (
unsigned int j = 0; j < 3; j++) {
590 atb[i] += mU[i][j] * aTbp[j];
592 atb[i] /= distanceFictive;
595 #ifdef DEBUG_Homographie
597 std::cout << aTbp.t();
599 std::cout << atb.
t();
612 signeSinus * sqrt((sv[0] * sv[0] - sv[1] * sv[1]) * (sv[1] * sv[1] - sv[2] * sv[2])) / ((sv[0] + sv[2]) * sv[1]);
614 cosinusTheta = (sv[1] * sv[1] + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
617 aRbp[0][0] = cosinusTheta;
619 aRbp[0][2] = -sinusTheta;
623 aRbp[2][0] = sinusTheta;
625 aRbp[2][2] = cosinusTheta;
628 for (
unsigned int i = 0; i < 3; i++) {
629 for (
unsigned int j = 0; j < 3; j++) {
631 for (
unsigned int k = 0; k < 3; k++) {
632 aRbint[i][j] += mU[i][k] * aRbp[k][j];
638 for (
unsigned int i = 0; i < 3; i++) {
639 for (
unsigned int j = 0; j < 3; j++) {
641 for (
unsigned int k = 0; k < 3; k++) {
642 aRb[i][j] += aRbint[i][k] * mV[j][k];
647 #ifdef DEBUG_Homographie
649 std::cout << aRb << std::endl;
654 std::list<vpTranslationVector> &vT, std::list<vpColVector> &vN)
657 #ifdef DEBUG_Homographie
658 printf(
"debut : Homographie_EstimationDeplacementCamera\n");
666 int cas1 = 1, cas2 = 2, cas3 = 3, cas4 = 4;
668 bool norm1ok =
false, norm2ok =
false, norm3ok =
false, norm4ok =
false;
670 double tmp, determinantU, determinantV, s, distanceFictive;
671 vpMatrix mTempU(3, 3), mTempV(3, 3), U(3, 3), V(3, 3);
687 unsigned int vOrdre[3];
700 mTempU.
svd(svTemp, mTempV);
704 if (svTemp[0] >= svTemp[1]) {
705 if (svTemp[0] >= svTemp[2]) {
706 if (svTemp[1] > svTemp[2]) {
721 if (svTemp[1] >= svTemp[2]) {
722 if (svTemp[0] > svTemp[2]) {
742 for (
unsigned int i = 0; i < 3; i++) {
743 sv[i] = svTemp[vOrdre[i]];
744 for (
unsigned int j = 0; j < 3; j++) {
745 U[i][j] = mTempU[i][vOrdre[j]];
746 V[i][j] = mTempV[i][vOrdre[j]];
750 #ifdef DEBUG_Homographie
755 printf(
"Valeurs singulieres : ");
760 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]) +
761 U[0][2] * (U[1][0] * U[2][1] - U[1][1] * U[2][0]);
763 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]) +
764 V[0][2] * (V[1][0] * V[2][1] - V[1][1] * V[2][0]);
766 s = determinantU * determinantV;
768 #ifdef DEBUG_Homographie
769 printf(
"s = det(U) * det(V) = %f * %f = %f\n", determinantU, determinantV, s);
782 tmp = H[2][0] * x + H[2][1] * y + H[2][2];
784 if ((tmp / (sv[1] * s)) > 0)
785 distanceFictive = sv[1];
787 distanceFictive = -sv[1];
795 if ((sv[0] - sv[1]) < sing_threshold) {
796 if ((sv[1] - sv[2]) < sing_threshold)
801 if ((sv[1] - sv[2]) < sing_threshold)
828 Nprim1[0] = sqrt((sv[0] * sv[0] - sv[1] * sv[1]) / (sv[0] * sv[0] - sv[2] * sv[2]));
830 Nprim1[2] = sqrt((sv[1] * sv[1] - sv[2] * sv[2]) / (sv[0] * sv[0] - sv[2] * sv[2]));
832 Nprim2[0] = Nprim1[0];
833 Nprim2[2] = -Nprim1[2];
834 Nprim3[0] = -Nprim1[0];
835 Nprim3[2] = Nprim1[2];
836 Nprim4[0] = -Nprim1[0];
837 Nprim4[2] = -Nprim1[2];
862 tmp = N1[0] * x + N1[1] * y + N1[2];
863 tmp /= (distanceFictive * s);
867 tmp = N2[0] * x + N2[1] * y + N2[2];
868 tmp /= (distanceFictive * s);
872 tmp = N3[0] * x + N3[1] * y + N3[2];
873 tmp /= (distanceFictive * s);
877 tmp = N4[0] * x + N4[1] * y + N4[2];
878 tmp /= (distanceFictive * s);
884 tmp = N1[0] * x + N1[1] * y + N1[2];
885 tmp /= (distanceFictive * s);
889 tmp = N2[0] * x + N2[1] * y + N2[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);
910 if (distanceFictive > 0) {
915 Rprim1[0][0] = (
vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
917 Rprim1[2][2] = Rprim1[0][0];
921 ((sv[0] + sv[2]) * sv[1]);
923 Rprim1[0][2] = -Rprim1[2][0];
927 Tprim1[0] = Nprim1[0];
929 Tprim1[2] = -Nprim1[2];
931 Tprim1 *= (sv[0] - sv[2]);
937 Rprim2[0][0] = (
vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
939 Rprim2[2][2] = Rprim2[0][0];
943 ((sv[0] + sv[2]) * sv[1]);
945 Rprim2[0][2] = -Rprim2[2][0];
949 Tprim2[0] = Nprim2[0];
951 Tprim2[2] = -Nprim2[2];
953 Tprim2 *= (sv[0] - sv[2]);
959 Rprim3[0][0] = (
vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
961 Rprim3[2][2] = Rprim3[0][0];
965 ((sv[0] + sv[2]) * sv[1]);
967 Rprim3[0][2] = -Rprim3[2][0];
971 Tprim3[0] = Nprim3[0];
973 Tprim3[2] = -Nprim3[2];
975 Tprim3 *= (sv[0] - sv[2]);
981 Rprim4[0][0] = (
vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
983 Rprim4[2][2] = Rprim4[0][0];
987 ((sv[0] + sv[2]) * sv[1]);
989 Rprim4[0][2] = -Rprim4[2][0];
993 Tprim4[0] = Nprim4[0];
995 Tprim4[2] = -Nprim4[2];
997 Tprim4 *= (sv[0] - sv[2]);
1008 Tprim1 *= (sv[2] - sv[0]);
1015 Tprim2 *= (sv[2] - sv[0]);
1023 Tprim1 *= (sv[0] - sv[1]);
1030 Tprim2 *= (sv[0] - sv[1]);
1040 if (distanceFictive < 0) {
1046 Rprim1[0][0] = (sv[0] * sv[2] -
vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1048 Rprim1[2][2] = -Rprim1[0][0];
1052 ((sv[0] - sv[2]) * sv[1]);
1054 Rprim1[0][2] = Rprim1[2][0];
1056 Rprim1[1][1] = -1.0;
1058 Tprim1[0] = Nprim1[0];
1060 Tprim1[2] = Nprim1[2];
1062 Tprim1 *= (sv[0] + sv[2]);
1068 Rprim2[0][0] = (sv[0] * sv[2] -
vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1070 Rprim2[2][2] = -Rprim2[0][0];
1074 ((sv[0] - sv[2]) * sv[1]);
1076 Rprim2[0][2] = Rprim2[2][0];
1078 Rprim2[1][1] = -1.0;
1080 Tprim2[0] = Nprim2[0];
1082 Tprim2[2] = Nprim2[2];
1084 Tprim2 *= (sv[0] + sv[2]);
1090 Rprim3[0][0] = (sv[0] * sv[2] -
vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1092 Rprim3[2][2] = -Rprim3[0][0];
1096 ((sv[0] - sv[2]) * sv[1]);
1098 Rprim3[0][2] = Rprim3[2][0];
1100 Rprim3[1][1] = -1.0;
1102 Tprim3[0] = Nprim3[0];
1104 Tprim3[2] = Nprim3[2];
1106 Tprim3 *= (sv[0] + sv[2]);
1111 Rprim4[0][0] = (sv[0] * sv[2] -
vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
1113 Rprim4[2][2] = -Rprim4[0][0];
1117 ((sv[0] - sv[2]) * sv[1]);
1119 Rprim4[0][2] = Rprim4[2][0];
1121 Rprim4[1][1] = -1.0;
1123 Tprim4[0] = Nprim4[0];
1125 Tprim4[2] = Nprim4[2];
1127 Tprim4 *= (sv[0] + sv[2]);
1139 Tprim1 *= (sv[2] + sv[0]);
1148 Tprim2 *= (sv[2] + sv[0]);
1158 Tprim1 *= (sv[2] + sv[0]);
1167 Tprim2 *= (sv[0] + sv[2]);
1177 if ((distanceFictive > 0) || (cas != cas4)) {
1181 R1 = s * U * Rprim1 * V.
t();
1183 T1 /= (distanceFictive * s);
1192 R2 = s * U * Rprim2 * V.
t();
1194 T2 /= (distanceFictive * s);
1203 R3 = s * U * Rprim3 * V.
t();
1205 T3 /= (distanceFictive * s);
1213 R4 = s * U * Rprim4 * V.
t();
1215 T4 /= (distanceFictive * s);
1224 std::cout <<
"On tombe dans le cas particulier ou le mouvement n'est pas "
1245 #ifdef DEBUG_Homographie
1246 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.