Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
vpHomography.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Homography transformation.
33  *
34  * Authors:
35  * Muriel Pressigout
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
46 #include <stdio.h>
47 
48 #include <visp3/core/vpDebug.h>
49 #include <visp3/core/vpMatrix.h>
50 #include <visp3/core/vpRobust.h>
51 #include <visp3/vision/vpHomography.h>
52 
53 // Exception
54 #include <visp3/core/vpException.h>
55 #include <visp3/core/vpMatrixException.h>
56 
60 vpHomography::vpHomography() : vpArray2D<double>(3, 3), aMb(), bP() { eye(); }
61 
66 vpHomography::vpHomography(const vpHomography &H) : vpArray2D<double>(3, 3), aMb(), bP() { *this = H; }
67 
71 vpHomography::vpHomography(const vpHomogeneousMatrix &M, const vpPlane &p) : vpArray2D<double>(3, 3), aMb(), bP()
72 {
73  buildFrom(M, p);
74 }
75 
77  : vpArray2D<double>(3, 3), aMb(), bP()
78 {
79  buildFrom(tu, atb, p);
80 }
81 
83  : vpArray2D<double>(3, 3), aMb(), bP()
84 {
85  buildFrom(aRb, atb, p);
86 }
87 
88 vpHomography::vpHomography(const vpPoseVector &arb, const vpPlane &p) : vpArray2D<double>(3, 3), aMb(), bP()
89 {
90  buildFrom(arb, p);
91 }
92 
94 {
95  insert(M);
96  insert(p);
97  build();
98 }
99 
101 {
102  insert(tu);
103  insert(atb);
104  insert(p);
105  build();
106 }
107 
109 {
110  insert(aRb);
111  insert(atb);
112  insert(p);
113  build();
114 }
115 
116 void vpHomography::buildFrom(const vpPoseVector &arb, const vpPlane &p)
117 {
118  aMb.buildFrom(arb[0], arb[1], arb[2], arb[3], arb[4], arb[5]);
119  insert(p);
120  build();
121 }
122 
123 /*********************************************************************/
124 
129 void vpHomography::insert(const vpRotationMatrix &aRb) { aMb.insert(aRb); }
130 
135 void vpHomography::insert(const vpHomogeneousMatrix &M) { this->aMb = M; }
136 
142 void vpHomography::insert(const vpThetaUVector &tu)
143 {
144  vpRotationMatrix aRb(tu);
145  aMb.insert(aRb);
146 }
147 
152 void vpHomography::insert(const vpTranslationVector &atb) { aMb.insert(atb); }
153 
158 void vpHomography::insert(const vpPlane &p) { this->bP = p; }
159 
166 {
167  vpMatrix M = (*this).convert();
168  vpMatrix Minv;
169  M.pseudoInverse(Minv, 1e-16);
170 
171  vpHomography H;
172 
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];
176 
177  return H;
178 }
179 
185 void vpHomography::inverse(vpHomography &bHa) const { bHa = inverse(); }
186 
194 void vpHomography::save(std::ofstream &f) const
195 {
196  if (!f.fail()) {
197  f << *this;
198  } else {
199  throw(vpException(vpException::ioError, "Cannot write the homography to the output stream"));
200  }
201 }
202 
217 {
218  vpHomography Hp;
219  for (unsigned int i = 0; i < 3; i++) {
220  for (unsigned int j = 0; j < 3; j++) {
221  double s = 0.;
222  for (unsigned int k = 0; k < 3; k++) {
223  s += (*this)[i][k] * H[k][j];
224  }
225  Hp[i][j] = s;
226  }
227  }
228  return Hp;
229 }
230 
237 {
238  if (b.size() != 3)
239  throw(vpException(vpException::dimensionError, "Cannot multiply an homography by a vector of dimension %d",
240  b.size()));
241 
242  vpColVector a(3);
243  for (unsigned int i = 0; i < 3; i++) {
244  a[i] = 0.;
245  for (unsigned int j = 0; j < 3; j++)
246  a[i] += (*this)[i][j] * b[j];
247  }
248 
249  return a;
250 }
251 
266 vpHomography vpHomography::operator*(const double &v) const
267 {
268  vpHomography H;
269 
270  for (unsigned int i = 0; i < 9; i++) {
271  H.data[i] = this->data[i] * v;
272  }
273 
274  return H;
275 }
276 
287 {
288  vpPoint a_P;
289  vpColVector v(3), v1(3);
290 
291  v[0] = b_P.get_x();
292  v[1] = b_P.get_y();
293  v[2] = b_P.get_w();
294 
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];
298 
299  // v1 = M*v ;
300  a_P.set_x(v1[0]);
301  a_P.set_y(v1[1]);
302  a_P.set_w(v1[2]);
303 
304  return a_P;
305 }
319 vpHomography vpHomography::operator/(const double &v) const
320 {
321  vpHomography H;
322  if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
323  throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
324 
325  double vinv = 1 / v;
326 
327  for (unsigned int i = 0; i < 9; i++) {
328  H.data[i] = this->data[i] * vinv;
329  }
330 
331  return H;
332 }
333 
336 {
337  // if (x == 0)
338  if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
339  throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
340 
341  double vinv = 1 / v;
342 
343  for (unsigned int i = 0; i < 9; i++)
344  data[i] *= vinv;
345 
346  return *this;
347 }
348 
356 {
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];
360 
361  aMb = H.aMb;
362  bP = H.bP;
363  return *this;
364 }
372 {
373  if (H.getRows() != 3 || H.getCols() != 3)
374  throw(vpException(vpException::dimensionError, "The matrix is not an homography"));
375 
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];
379 
380  return *this;
381 }
382 
391 void vpHomography::load(std::ifstream &f)
392 {
393  if (!f.fail()) {
394  for (unsigned int i = 0; i < 3; i++)
395  for (unsigned int j = 0; j < 3; j++) {
396  f >> (*this)[i][j];
397  }
398  } else {
399  throw(vpException(vpException::ioError, "Cannot read the homography from the input stream"));
400  }
401 }
402 
410 void vpHomography::build()
411 {
412  vpColVector n(3);
413  vpColVector atb(3);
414  vpMatrix aRb(3, 3);
415  for (unsigned int i = 0; i < 3; i++) {
416  atb[i] = aMb[i][3];
417  for (unsigned int j = 0; j < 3; j++)
418  aRb[i][j] = aMb[i][j];
419  }
420 
421  bP.getNormal(n);
422 
423  double d = bP.getD();
424  vpMatrix aHb = aRb - atb * n.t() / d; // the d used in the equation is such as nX=d is the
425  // plane equation. So if the plane is described by
426  // Ax+By+Cz+D=0, d=-D
427 
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];
431 }
432 
441 void vpHomography::build(vpHomography &aHb, const vpHomogeneousMatrix &aMb, const vpPlane &bP)
442 {
443  vpColVector n(3);
444  vpColVector atb(3);
445  vpMatrix aRb(3, 3);
446  for (unsigned int i = 0; i < 3; i++) {
447  atb[i] = aMb[i][3];
448  for (unsigned int j = 0; j < 3; j++)
449  aRb[i][j] = aMb[i][j];
450  }
451 
452  bP.getNormal(n);
453 
454  double d = bP.getD();
455  vpMatrix aHb_ = aRb - atb * n.t() / d; // the d used in the equation is such as nX=d is the
456  // plane equation. So if the plane is described by
457  // Ax+By+Cz+D=0, d=-D
458 
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];
462 }
463 
469 {
470  for (unsigned int i = 0; i < 3; i++)
471  for (unsigned int j = 0; j < 3; j++)
472  if (i == j)
473  (*this)[i][j] = 1.0;
474  else
475  (*this)[i][j] = 0.0;
476 }
477 
478 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
479 
486 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
487 
501 {
502  double xa = iPa.get_u();
503  double ya = iPa.get_v();
504  vpMatrix H = cam.get_K() * bHa.convert() * cam.get_K_inverse();
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;
508 
509  vpImagePoint iPb(yb, xb);
510 
511  return iPb;
512 }
513 
527 {
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;
533 
534  vpPoint Pb;
535  Pb.set_x(xb);
536  Pb.set_y(yb);
537 
538  return Pb;
539 }
540 
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)
574 {
575  unsigned int n = (unsigned int)xb.size();
576  if (yb.size() != n || xa.size() != n || ya.size() != n)
577  throw(vpException(vpException::dimensionError, "Bad dimension for robust homography estimation"));
578 
579  // 4 point are required
580  if (n < 4)
581  throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
582 
583  try {
584  std::vector<double> xan, yan, xbn, ybn;
585 
586  double xg1 = 0., yg1 = 0., coef1 = 0., xg2 = 0., yg2 = 0., coef2 = 0.;
587 
588  vpHomography aHbn;
589 
590  if (normalization) {
591  vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
592  vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
593  } else {
594  xbn = xb;
595  ybn = yb;
596  xan = xa;
597  yan = ya;
598  }
599 
600  unsigned int nbLinesA = 2;
601  vpMatrix A(nbLinesA * n, 8);
602  vpColVector X(8);
603  vpColVector Y(nbLinesA * n);
604  vpMatrix W(nbLinesA * n, nbLinesA * n); // Weight matrix
605 
606  vpColVector w(nbLinesA * n);
607 
608  // All the weights are set to 1 at the beginning to use a classical least
609  // square scheme
610  w = 1;
611  // Update the square matrix associated to the weights
612  for (unsigned int i = 0; i < nbLinesA * n; i++) {
613  W[i][i] = w[i];
614  }
615 
616  // build matrix A
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];
626 
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];
635 
636  Y[nbLinesA * i] = xan[i];
637  Y[nbLinesA * i + 1] = yan[i];
638  }
639 
640  vpMatrix WA;
641  vpMatrix WAp;
642  unsigned int iter = 0;
643  vpRobust r(nbLinesA * n); // M-Estimator
644 
645  while (iter < niter) {
646  WA = W * A;
647 
648  X = WA.pseudoInverse(1e-26) * W * Y;
649  vpColVector residu;
650  residu = Y - A * X;
651 
652  // Compute the weights using the Tukey biweight M-Estimator
653  r.setIteration(iter);
654  r.MEstimator(vpRobust::TUKEY, residu, w);
655 
656  // Update the weights matrix
657  for (unsigned int i = 0; i < n * nbLinesA; i++) {
658  W[i][i] = w[i];
659  }
660  // Build the homography
661  for (unsigned int i = 0; i < 8; i++)
662  aHbn.data[i] = X[i];
663  aHbn[2][2] = 1;
664  {
665  vpMatrix aHbnorm = aHbn.convert();
666  aHbnorm /= aHbnorm[2][2];
667  }
668 
669  iter++;
670  }
671  inliers.resize(n);
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)
675  inliers[i] = false;
676  else {
677  inliers[i] = true;
678  nbinliers++;
679  }
680  }
681 
682  if (normalization) {
683  // H after denormalization
684  vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
685  } else {
686  aHb = aHbn;
687  }
688 
689  residual = 0;
690  vpColVector a(3), b(3), c(3);
691  for (unsigned int i = 0; i < n; i++) {
692  if (inliers[i]) {
693  a[0] = xa[i];
694  a[1] = ya[i];
695  a[2] = 1;
696  b[0] = xb[i];
697  b[1] = yb[i];
698  b[2] = 1;
699 
700  c = aHb * b;
701  c /= c[2];
702  residual += (a - c).sumSquare();
703  }
704  }
705 
706  residual = sqrt(residual / nbinliers);
707  } catch (...) {
708  throw(vpException(vpException::fatalError, "Cannot estimate an homography"));
709  }
710 }
711 
720 {
721  vpImagePoint ipa;
722  double u = ipb.get_u();
723  double v = ipb.get_v();
724 
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];
728 
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);
732  }
733 
734  return ipa;
735 }
736 
742 {
743  vpMatrix M(3, 3);
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];
747 
748  return M;
749 }
void buildFrom(const vpRotationMatrix &aRb, const vpTranslationVector &atb, const vpPlane &bP)
Construction from Translation and rotation and a plane.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2206
vpHomography & operator=(const vpHomography &H)
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:305
vpHomography & operator/=(double v)
Divide all the element of the homography matrix by v : Hij = Hij / v.
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:176
static vpImagePoint project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void set_u(double u)
Definition: vpImagePoint.h:226
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
vpRowVector t() const
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
Implementation of a generic 2D array used as base class for matrices and vectors. ...
Definition: vpArray2D.h:131
vpImagePoint projection(const vpImagePoint &p)
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
vpHomography operator*(const vpHomography &H) const
vpMatrix get_K() const
Class that defines what is a point.
Definition: vpPoint.h:58
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:472
Implementation of a rotation matrix and operations on such kind of matrices.
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:474
unsigned int getCols() const
Definition: vpArray2D.h:279
void insert(const vpRotationMatrix &R)
double get_w() const
Get the point w coordinate in the image plane.
Definition: vpPoint.cpp:435
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
double get_u() const
Definition: vpImagePoint.h:263
double getD() const
Definition: vpPlane.h:108
vpHomography()
initialize an homography as Identity
void load(std::ifstream &f)
Load an homography from a file.
Generic class defining intrinsic camera parameters.
vpMatrix convert() const
vpHomography inverse() const
invert the homography
void save(std::ofstream &f) const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static void robust(const std::vector< double > &xb, const std::vector< double > &yb, const std::vector< double > &xa, const std::vector< double > &ya, vpHomography &aHb, std::vector< bool > &inlier, double &residual, double weights_threshold=0.4, unsigned int niter=4, bool normalization=true)
vpHomography operator/(const double &v) const
vpColVector getNormal() const
Definition: vpPlane.cpp:238
void set_v(double v)
Definition: vpImagePoint.h:237
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:431
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
void set_w(double w)
Set the point w coordinate in the image plane.
Definition: vpPoint.cpp:476
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:433
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:58
error that can be emited by the vpMatrix class and its derivates
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
void convert(std::vector< float > &M)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
void setIdentity()
double get_v() const
Definition: vpImagePoint.h:274
void setIteration(unsigned int iter)
Set iteration.
Definition: vpRobust.h:109
vpMatrix get_K_inverse() const