Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpHomography.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Homography transformation.
32  *
33  * Authors:
34  * Muriel Pressigout
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 
46 #include <stdio.h>
47 
48 #include <visp3/core/vpDebug.h>
49 #include <visp3/vision/vpHomography.h>
50 #include <visp3/core/vpMatrix.h>
51 #include <visp3/core/vpRobust.h>
52 
53 // Exception
54 #include <visp3/core/vpException.h>
55 #include <visp3/core/vpMatrixException.h>
56 
61  : vpArray2D<double>(3,3), aMb(), bP()
62 {
63  eye();
64 }
65 
66 
72  : vpArray2D<double>(3,3), aMb(), bP()
73 {
74  *this = H;
75 }
76 
81  : vpArray2D<double>(3,3), aMb(), bP()
82 {
83  buildFrom(M, p) ;
84 }
85 
87  const vpTranslationVector &atb,
88  const vpPlane &p)
89  : vpArray2D<double>(3,3), aMb(), bP()
90 {
91  buildFrom(tu, atb, p) ;
92 }
93 
95  const vpTranslationVector &atb,
96  const vpPlane &p)
97  : vpArray2D<double>(3,3), aMb(), bP()
98 {
99  buildFrom(aRb, atb, p) ;
100 }
101 
103  : vpArray2D<double>(3,3), aMb(), bP()
104 {
105  buildFrom(arb, p) ;
106 }
107 
108 void
110  const vpPlane &p)
111 {
112  insert(M) ;
113  insert(p) ;
114  build() ;
115 }
116 
117 void
119  const vpTranslationVector &atb,
120  const vpPlane &p)
121 {
122  insert(tu) ;
123  insert(atb) ;
124  insert(p) ;
125  build() ;
126 }
127 
128 void
130  const vpTranslationVector &atb,
131  const vpPlane &p)
132 {
133  insert(aRb) ;
134  insert(atb) ;
135  insert(p) ;
136  build() ;
137 }
138 
139 void
141 {
142  aMb.buildFrom(arb[0],arb[1],arb[2],arb[3],arb[4],arb[5]) ;
143  insert(p) ;
144  build() ;
145 }
146 
147 
148 
149 /*********************************************************************/
150 
151 
156 void
157 vpHomography::insert(const vpRotationMatrix &aRb)
158 {
159  aMb.insert(aRb) ;
160 }
161 
166 void
167 vpHomography::insert(const vpHomogeneousMatrix &M)
168 {
169  this->aMb = M ;
170 }
171 
177 void
178 vpHomography::insert(const vpThetaUVector &tu)
179 {
180  vpRotationMatrix aRb(tu) ;
181  aMb.insert(aRb) ;
182 }
183 
188 void
189 vpHomography::insert(const vpTranslationVector &atb)
190 {
191  aMb.insert(atb) ;
192 }
193 
198 void
199 vpHomography::insert(const vpPlane &p)
200 {
201  this->bP = p;
202 }
203 
211 {
212  vpMatrix M = (*this).convert();
213  vpMatrix Minv;
214  M.pseudoInverse(Minv, 1e-16);
215 
216  vpHomography H;
217 
218  for(unsigned int i=0; i<3; i++)
219  for(unsigned int j=0; j<3; j++)
220  H[i][j] = Minv[i][j];
221 
222  return H;
223 }
224 
230 void
232 {
233  bHa = inverse() ;
234 }
235 
242 void
243 vpHomography::save(std::ofstream &f) const
244 {
245  if (! f.fail())
246  {
247  f << *this ;
248  }
249  else
250  {
251  throw(vpException(vpException::ioError, "Cannot write the homography to the output stream")) ;
252  }
253 }
254 
269 {
270  vpHomography Hp;
271  for(unsigned int i = 0; i < 3; i++) {
272  for(unsigned int j = 0; j < 3; j++) {
273  double s = 0.;
274  for(unsigned int k = 0; k < 3; k ++) {
275  s += (*this)[i][k] * H[k][j];
276  }
277  Hp[i][j] = s;
278  }
279  }
280  return Hp;
281 }
282 
290 {
291  if (b.size() != 3)
292  throw(vpException(vpException::dimensionError, "Cannot multiply an homography by a vector of dimension %d", b.size()));
293 
294  vpColVector a(3);
295  for(unsigned int i=0; i<3; i++) {
296  a[i] = 0.;
297  for(unsigned int j=0; j<3; j++)
298  a[i] += (*this)[i][j] * b[j];
299  }
300 
301  return a;
302 }
303 
318 vpHomography vpHomography::operator*(const double &v) const
319 {
320  vpHomography H;
321 
322  for (unsigned int i=0; i < 9; i ++) {
323  H.data[i] = this->data[i] * v;
324  }
325 
326  return H;
327 }
328 
338 {
339  vpPoint a_P ;
340  vpColVector v(3),v1(3) ;
341 
342  v[0] = b_P.get_x() ;
343  v[1] = b_P.get_y() ;
344  v[2] = b_P.get_w() ;
345 
346  v1[0] = (*this)[0][0]*v[0] + (*this)[0][1]*v[1]+ (*this)[0][2]*v[2] ;
347  v1[1] = (*this)[1][0]*v[0] + (*this)[1][1]*v[1]+ (*this)[1][2]*v[2] ;
348  v1[2] = (*this)[2][0]*v[0] + (*this)[2][1]*v[1]+ (*this)[2][2]*v[2] ;
349 
350  // v1 = M*v ;
351  a_P.set_x(v1[0]) ;
352  a_P.set_y(v1[1]) ;
353  a_P.set_w(v1[2]) ;
354 
355  return a_P ;
356 }
370 vpHomography vpHomography::operator/(const double &v) const
371 {
372  vpHomography H;
373  if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
374  throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
375 
376  double vinv = 1/v ;
377 
378  for (unsigned int i=0; i < 9; i ++) {
379  H.data[i] = this->data[i] * vinv;
380  }
381 
382  return H;
383 }
384 
387 {
388  //if (x == 0)
389  if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
390  throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
391 
392  double vinv = 1/v ;
393 
394  for (unsigned int i=0;i<9;i++)
395  data[i] *= vinv;
396 
397  return *this;
398 }
399 
406 vpHomography &
408 {
409  for (unsigned int i=0; i< 3; i++)
410  for (unsigned int j=0; j< 3; j++)
411  (*this)[i][j] = H[i][j];
412 
413  return *this;
414 }
421 vpHomography &
423 {
424  if (H.getRows() !=3 || H.getCols() != 3)
425  throw(vpException(vpException::dimensionError, "The matrix is not an homography"));
426 
427  for (unsigned int i=0; i< 3; i++)
428  for (unsigned int j=0; j< 3; j++)
429  (*this)[i][j] = H[i][j];
430 
431  return *this;
432 }
433 
442 void
443 vpHomography::load(std::ifstream &f)
444 {
445  if (! f.fail())
446  {
447  for (unsigned int i=0 ; i < 3 ; i++)
448  for (unsigned int j=0 ; j < 3 ; j++)
449  {
450  f >> (*this)[i][j] ;
451  }
452  }
453  else
454  {
455  throw(vpException(vpException::ioError, "Cannot read the homography from the input stream")) ;
456  }
457 }
458 
466 void
467 vpHomography::build()
468 {
469  vpColVector n(3) ;
470  vpColVector atb(3) ;
471  vpMatrix aRb(3,3);
472  for (unsigned int i=0 ; i < 3 ; i++)
473  {
474  atb[i] = aMb[i][3];
475  for (unsigned int j=0 ; j < 3 ; j++)
476  aRb[i][j] = aMb[i][j];
477  }
478 
479  bP.getNormal(n) ;
480 
481  double d = bP.getD() ;
482  vpMatrix aHb = aRb - atb*n.t()/d ; // the d used in the equation is such as nX=d is the
483  // plane equation. So if the plane is described by
484  // Ax+By+Cz+D=0, d=-D
485 
486  for (unsigned int i=0 ; i < 3 ; i++)
487  for (unsigned int j=0 ; j < 3 ; j++)
488  (*this)[i][j] = aHb[i][j];
489 }
490 
499 void
500 vpHomography::build(vpHomography &aHb,
501  const vpHomogeneousMatrix &aMb,
502  const vpPlane &bP)
503 {
504  vpColVector n(3) ;
505  vpColVector atb(3) ;
506  vpMatrix aRb(3,3);
507  for (unsigned int i=0 ; i < 3 ; i++)
508  {
509  atb[i] = aMb[i][3] ;
510  for (unsigned int j=0 ; j < 3 ; j++)
511  aRb[i][j] = aMb[i][j];
512  }
513 
514  bP.getNormal(n) ;
515 
516  double d = bP.getD() ;
517  vpMatrix aHb_ = aRb - atb*n.t()/d ; // the d used in the equation is such as nX=d is the
518  // plane equation. So if the plane is described by
519  // Ax+By+Cz+D=0, d=-D
520 
521  for (unsigned int i=0 ; i < 3 ; i++)
522  for (unsigned int j=0 ; j < 3 ; j++)
523  aHb[i][j] = aHb_[i][j];
524 }
525 
530 void
532 {
533  for (unsigned int i=0 ; i < 3 ; i++)
534  for (unsigned int j=0 ; j < 3; j++)
535  if (i==j)
536  (*this)[i][j] = 1.0 ;
537  else
538  (*this)[i][j] = 0.0;
539 }
540 
541 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
542 
548 void
550 {
551  eye();
552 }
553 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
554 
566 {
567  double xa = iPa.get_u();
568  double ya = iPa.get_v();
569  vpMatrix H = cam.get_K() * bHa.convert() * cam.get_K_inverse();
570  double z = xa * H[2][0] + ya * H[2][1] + H[2][2];
571  double xb = (xa * H[0][0] + ya * H[0][1] + H[0][2]) / z;
572  double yb = (xa * H[1][0] + ya * H[1][1] + H[1][2]) / z;
573 
574  vpImagePoint iPb(yb, xb);
575 
576  return iPb;
577 }
578 
589 {
590  double xa = Pa.get_x();
591  double ya = Pa.get_y();
592  double z = xa * bHa[2][0] + ya * bHa[2][1] + bHa[2][2];
593  double xb = (xa * bHa[0][0] + ya * bHa[0][1] + bHa[0][2]) / z;
594  double yb = (xa * bHa[1][0] + ya * bHa[1][1] + bHa[1][2]) / z;
595 
596  vpPoint Pb;
597  Pb.set_x(xb);
598  Pb.set_y(yb);
599 
600  return Pb;
601 }
602 
631 void
632 vpHomography::robust(const std::vector<double> &xb, const std::vector<double> &yb,
633  const std::vector<double> &xa, const std::vector<double> &ya,
634  vpHomography &aHb,
635  std::vector<bool> &inliers,
636  double &residual,
637  double weights_threshold,
638  unsigned int niter,
639  bool normalization)
640 {
641  unsigned int n = (unsigned int) xb.size();
642  if (yb.size() != n || xa.size() != n || ya.size() != n)
644  "Bad dimension for robust homography estimation"));
645 
646  // 4 point are required
647  if(n<4)
648  throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
649 
650  try{
651  std::vector<double> xan, yan, xbn, ybn;
652 
653  double xg1=0., yg1=0., coef1=0., xg2=0., yg2=0., coef2=0.;
654 
655  vpHomography aHbn;
656 
657  if (normalization) {
658  vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
659  vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
660  }
661  else {
662  xbn = xb;
663  ybn = yb;
664  xan = xa;
665  yan = ya;
666  }
667 
668  unsigned int nbLinesA = 2;
669  vpMatrix A(nbLinesA*n,8);
670  vpColVector X(8);
671  vpColVector Y(nbLinesA*n);
672  vpMatrix W(nbLinesA*n, nbLinesA*n) ; // Weight matrix
673 
674  vpColVector w(nbLinesA*n) ;
675 
676  // All the weights are set to 1 at the beginning to use a classical least square scheme
677  w = 1;
678  // Update the square matrix associated to the weights
679  for (unsigned int i=0; i < nbLinesA*n; i ++) {
680  W[i][i] = w[i];
681  }
682 
683  // build matrix A
684  for(unsigned int i=0; i<n;i++) {
685  A[nbLinesA*i][0]=xbn[i];
686  A[nbLinesA*i][1]=ybn[i];
687  A[nbLinesA*i][2]=1;
688  A[nbLinesA*i][3]=0 ;
689  A[nbLinesA*i][4]=0 ;
690  A[nbLinesA*i][5]=0;
691  A[nbLinesA*i][6]=-xbn[i]*xan[i] ;
692  A[nbLinesA*i][7]=-ybn[i]*xan[i];
693 
694  A[nbLinesA*i+1][0]=0 ;
695  A[nbLinesA*i+1][1]=0;
696  A[nbLinesA*i+1][2]=0;
697  A[nbLinesA*i+1][3]=xbn[i];
698  A[nbLinesA*i+1][4]=ybn[i];
699  A[nbLinesA*i+1][5]=1;
700  A[nbLinesA*i+1][6]=-xbn[i]*yan[i];
701  A[nbLinesA*i+1][7]=-ybn[i]*yan[i];
702 
703  Y[nbLinesA*i] = xan[i];
704  Y[nbLinesA*i+1] = yan[i];
705  }
706 
707  vpMatrix WA;
708  vpMatrix WAp;
709  unsigned int iter = 0;
710  vpRobust r(nbLinesA*n) ; // M-Estimator
711 
712  while (iter < niter) {
713  WA = W * A;
714 
715  X = WA.pseudoInverse(1e-26)*W*Y;
716  vpColVector residu;
717  residu = Y - A * X;
718 
719  // Compute the weights using the Tukey biweight M-Estimator
720  r.setIteration(iter) ;
721  r.MEstimator(vpRobust::TUKEY, residu, w) ;
722 
723  // Update the weights matrix
724  for (unsigned int i=0; i < n*nbLinesA; i ++) {
725  W[i][i] = w[i];
726  }
727  // Build the homography
728  for(unsigned int i=0;i<8;i++)
729  aHbn.data[i]= X[i];
730  aHbn[2][2] = 1;
731  {
732  vpMatrix aHbnorm = aHbn.convert();
733  aHbnorm /= aHbnorm[2][2] ;
734  }
735 
736  iter ++;
737  }
738  inliers.resize(n);
739  unsigned int nbinliers = 0;
740  for(unsigned int i=0; i< n; i++) {
741  if (w[i*2] < weights_threshold && w[i*2+1] < weights_threshold)
742  inliers[i] = false;
743  else {
744  inliers[i] = true;
745  nbinliers ++;
746  }
747  }
748 
749  if (normalization) {
750  // H after denormalization
751  vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
752  }
753  else {
754  aHb = aHbn;
755  }
756 
757  residual = 0 ;
758  vpColVector a(3), b(3), c(3);
759  for (unsigned int i=0 ; i < n ; i++) {
760  if (inliers[i]) {
761  a[0] = xa[i] ; a[1] = ya[i] ; a[2] = 1 ;
762  b[0] = xb[i] ; b[1] = yb[i] ; b[2] = 1 ;
763 
764  c = aHb*b ; c /= c[2] ;
765  residual += (a-c).sumSquare();
766  }
767  }
768 
769  residual = sqrt(residual/nbinliers);
770  }
771  catch(...)
772  {
773  throw(vpException(vpException::fatalError, "Cannot estimate an homography")) ;
774  }
775 }
776 
786 {
787  vpImagePoint ipa;
788  double u = ipb.get_u();
789  double v = ipb.get_v();
790 
791  double u_a = (*this)[0][0] * u + (*this)[0][1] * v + (*this)[0][2];
792  double v_a = (*this)[1][0] * u + (*this)[1][1] * v + (*this)[1][2];
793  double w_a = (*this)[2][0] * u + (*this)[2][1] * v + (*this)[2][2];
794 
795  if(std::fabs(w_a) > std::numeric_limits<double>::epsilon()){
796  ipa.set_u(u_a / w_a);
797  ipa.set_v(v_a / w_a);
798  }
799 
800  return ipa;
801 }
802 
808 {
809  vpMatrix M(3, 3);
810  for(unsigned int i=0; i<3; i++)
811  for(unsigned int j=0; j<3; j++)
812  M[i][j] = (*this)[i][j];
813 
814  return M;
815 }
vpMatrix get_K_inverse() const
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:97
double get_v() const
Definition: vpImagePoint.h:268
vpHomography & operator=(const vpHomography &H)
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:182
static vpImagePoint project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
Implementation of an homogeneous matrix and operations on such kind of matrices.
double get_u() const
Definition: vpImagePoint.h:257
error that can be emited by ViSP classes.
Definition: vpException.h:73
void set_x(const double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:496
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:84
Implementation of a generic 2D array used as vase class of matrices and vectors.
Definition: vpArray2D.h:70
vpImagePoint projection(const vpImagePoint &p)
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:156
unsigned int getCols() const
Return the number of columns of the 2D array.
Definition: vpArray2D.h:154
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:458
double get_w() const
Get the point w coordinate in the image plane.
Definition: vpPoint.cpp:460
Class that defines what is a point.
Definition: vpPoint.h:59
Implementation of a rotation matrix and operations on such kind of matrices.
void insert(const vpRotationMatrix &R)
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:179
void set_u(const double u)
Definition: vpImagePoint.h:221
vpHomography()
initialize an homography as Identity
void set_v(const double v)
Definition: vpImagePoint.h:232
void load(std::ifstream &f)
Load an homography from a file.
Generic class defining intrinsic camera parameters.
void set_y(const double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:498
void set_w(const double w)
Set the point w coordinate in the image plane.
Definition: vpPoint.cpp:500
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:456
vpRowVector t() const
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
vpColVector getNormal() const
Definition: vpPlane.cpp:246
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomography operator/(const double &v) const
void save(std::ofstream &f) const
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)
vpMatrix get_K() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:93
vpHomography inverse() const
invert the homography
vpMatrix convert() const
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:60
error that can be emited by the vpMatrix class and its derivates
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1741
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
vpHomography operator*(const vpHomography &H) const
void setIteration(const unsigned int iter)
Set iteration.
Definition: vpRobust.h:123
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
void setIdentity()
double getD() const
Definition: vpPlane.h:112