48 #include <visp3/core/vpDebug.h>
49 #include <visp3/core/vpMatrix.h>
50 #include <visp3/core/vpRobust.h>
51 #include <visp3/vision/vpHomography.h>
54 #include <visp3/core/vpException.h>
55 #include <visp3/core/vpMatrixException.h>
118 aMb.
buildFrom(arb[0], arb[1], arb[2], arb[3], arb[4], arb[5]);
158 void vpHomography::insert(
const vpPlane &p) { this->bP = p; }
173 for (
unsigned int i = 0; i < 3; i++)
174 for (
unsigned int j = 0; j < 3; j++)
175 H[i][j] = Minv[i][j];
219 for (
unsigned int i = 0; i < 3; i++) {
220 for (
unsigned int j = 0; j < 3; j++) {
222 for (
unsigned int k = 0; k < 3; k++) {
223 s += (*this)[i][k] * H[k][j];
243 for (
unsigned int i = 0; i < 3; i++) {
245 for (
unsigned int j = 0; j < 3; j++)
246 a[i] += (*
this)[i][j] * b[j];
270 for (
unsigned int i = 0; i < 9; i++) {
295 v1[0] = (*this)[0][0] * v[0] + (*this)[0][1] * v[1] + (*this)[0][2] * v[2];
296 v1[1] = (*this)[1][0] * v[0] + (*this)[1][1] * v[1] + (*this)[1][2] * v[2];
297 v1[2] = (*this)[2][0] * v[0] + (*this)[2][1] * v[1] + (*this)[2][2] * v[2];
322 if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
327 for (
unsigned int i = 0; i < 9; i++) {
338 if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
343 for (
unsigned int i = 0; i < 9; i++)
357 for (
unsigned int i = 0; i < 3; i++)
358 for (
unsigned int j = 0; j < 3; j++)
359 (*
this)[i][j] = H[i][j];
376 for (
unsigned int i = 0; i < 3; i++)
377 for (
unsigned int j = 0; j < 3; j++)
378 (*
this)[i][j] = H[i][j];
394 for (
unsigned int i = 0; i < 3; i++)
395 for (
unsigned int j = 0; j < 3; j++) {
410 void vpHomography::build()
415 for (
unsigned int i = 0; i < 3; i++) {
417 for (
unsigned int j = 0; j < 3; j++)
418 aRb[i][j] = aMb[i][j];
423 double d = bP.
getD();
428 for (
unsigned int i = 0; i < 3; i++)
429 for (
unsigned int j = 0; j < 3; j++)
430 (*
this)[i][j] = aHb[i][j];
446 for (
unsigned int i = 0; i < 3; i++) {
448 for (
unsigned int j = 0; j < 3; j++)
449 aRb[i][j] = aMb[i][j];
454 double d = bP.
getD();
459 for (
unsigned int i = 0; i < 3; i++)
460 for (
unsigned int j = 0; j < 3; j++)
461 aHb[i][j] = aHb_[i][j];
470 for (
unsigned int i = 0; i < 3; i++)
471 for (
unsigned int j = 0; j < 3; j++)
478 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
486 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
502 double xa = iPa.
get_u();
503 double ya = iPa.
get_v();
505 double z = xa * H[2][0] + ya * H[2][1] + H[2][2];
506 double xb = (xa * H[0][0] + ya * H[0][1] + H[0][2]) / z;
507 double yb = (xa * H[1][0] + ya * H[1][1] + H[1][2]) / z;
528 double xa = Pa.
get_x();
529 double ya = Pa.
get_y();
530 double z = xa * bHa[2][0] + ya * bHa[2][1] + bHa[2][2];
531 double xb = (xa * bHa[0][0] + ya * bHa[0][1] + bHa[0][2]) / z;
532 double yb = (xa * bHa[1][0] + ya * bHa[1][1] + bHa[1][2]) / z;
571 void vpHomography::robust(
const std::vector<double> &xb,
const std::vector<double> &yb,
const std::vector<double> &xa,
572 const std::vector<double> &ya,
vpHomography &aHb, std::vector<bool> &inliers,
573 double &residual,
double weights_threshold,
unsigned int niter,
bool normalization)
575 unsigned int n = (
unsigned int)xb.size();
576 if (yb.size() != n || xa.size() != n || ya.size() != n)
584 std::vector<double> xan, yan, xbn, ybn;
586 double xg1 = 0., yg1 = 0., coef1 = 0., xg2 = 0., yg2 = 0., coef2 = 0.;
591 vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
592 vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
600 unsigned int nbLinesA = 2;
604 vpMatrix W(nbLinesA * n, nbLinesA * n);
612 for (
unsigned int i = 0; i < nbLinesA * n; i++) {
617 for (
unsigned int i = 0; i < n; i++) {
618 A[nbLinesA * i][0] = xbn[i];
619 A[nbLinesA * i][1] = ybn[i];
620 A[nbLinesA * i][2] = 1;
621 A[nbLinesA * i][3] = 0;
622 A[nbLinesA * i][4] = 0;
623 A[nbLinesA * i][5] = 0;
624 A[nbLinesA * i][6] = -xbn[i] * xan[i];
625 A[nbLinesA * i][7] = -ybn[i] * xan[i];
627 A[nbLinesA * i + 1][0] = 0;
628 A[nbLinesA * i + 1][1] = 0;
629 A[nbLinesA * i + 1][2] = 0;
630 A[nbLinesA * i + 1][3] = xbn[i];
631 A[nbLinesA * i + 1][4] = ybn[i];
632 A[nbLinesA * i + 1][5] = 1;
633 A[nbLinesA * i + 1][6] = -xbn[i] * yan[i];
634 A[nbLinesA * i + 1][7] = -ybn[i] * yan[i];
636 Y[nbLinesA * i] = xan[i];
637 Y[nbLinesA * i + 1] = yan[i];
642 unsigned int iter = 0;
645 while (iter < niter) {
653 r.setIteration(iter);
657 for (
unsigned int i = 0; i < n * nbLinesA; i++) {
661 for (
unsigned int i = 0; i < 8; i++)
666 aHbnorm /= aHbnorm[2][2];
672 unsigned int nbinliers = 0;
673 for (
unsigned int i = 0; i < n; i++) {
674 if (w[i * 2] < weights_threshold && w[i * 2 + 1] < weights_threshold)
684 vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
691 for (
unsigned int i = 0; i < n; i++) {
702 residual += (a - c).sumSquare();
706 residual = sqrt(residual / nbinliers);
722 double u = ipb.
get_u();
723 double v = ipb.
get_v();
725 double u_a = (*this)[0][0] * u + (*this)[0][1] * v + (*this)[0][2];
726 double v_a = (*this)[1][0] * u + (*this)[1][1] * v + (*this)[1][2];
727 double w_a = (*this)[2][0] * u + (*this)[2][1] * v + (*this)[2][2];
729 if (std::fabs(w_a) > std::numeric_limits<double>::epsilon()) {
730 ipa.
set_u(u_a / w_a);
731 ipa.
set_v(v_a / w_a);
744 for (
unsigned int i = 0; i < 3; i++)
745 for (
unsigned int j = 0; j < 3; j++)
746 M[i][j] = (*
this)[i][j];