ViSP  2.10.0
vpHomography.cpp
1 /****************************************************************************
2  *
3  * $Id: vpHomography.cpp 4661 2014-02-10 19:34:58Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Homography transformation.
36  *
37  * Authors:
38  * Muriel Pressigout
39  * Fabien Spindler
40  *
41  *****************************************************************************/
42 
43 
50 #include <stdio.h>
51 
52 #include <visp/vpDebug.h>
53 #include <visp/vpHomography.h>
54 #include <visp/vpMatrix.h>
55 #include <visp/vpRobust.h>
56 
57 // Exception
58 #include <visp/vpException.h>
59 #include <visp/vpMatrixException.h>
60 
64 vpHomography::vpHomography() : data(NULL), aMb(), bP()
65 {
66  data = new double [9];
67  setIdentity();
68 }
69 
70 
75 vpHomography::vpHomography(const vpHomography &H) : data(NULL), aMb(), bP()
76 {
77  data = new double [9];
78 
79  *this = H;
80 }
81 
85 vpHomography::vpHomography(const vpHomogeneousMatrix &M, const vpPlane &p) : data(NULL), aMb(), bP()
86 {
87  data = new double [9];
88  buildFrom(M, p) ;
89 }
90 
92  const vpTranslationVector &atb,
93  const vpPlane &p) : data(NULL), aMb(), bP()
94 {
95  data = new double [9];
96  buildFrom(tu, atb, p) ;
97 }
98 
100  const vpTranslationVector &atb,
101  const vpPlane &p) : data(NULL), aMb(), bP()
102 {
103  data = new double [9];
104  buildFrom(aRb, atb, p) ;
105 }
106 
107 vpHomography::vpHomography(const vpPoseVector &arb, const vpPlane &p) : data(NULL), aMb(), bP()
108 {
109  data = new double [9];
110  buildFrom(arb, p) ;
111 }
112 
114 {
115  if (data)
116  delete [] data;
117  data = NULL;
118 }
119 
120 void
122  const vpPlane &p)
123 {
124  insert(M) ;
125  insert(p) ;
126  build() ;
127 }
128 
129 void
131  const vpTranslationVector &atb,
132  const vpPlane &p)
133 {
134  insert(tu) ;
135  insert(atb) ;
136  insert(p) ;
137  build() ;
138 }
139 
140 void
142  const vpTranslationVector &atb,
143  const vpPlane &p)
144 {
145  insert(aRb) ;
146  insert(atb) ;
147  insert(p) ;
148  build() ;
149 }
150 
151 void
153 {
154  aMb.buildFrom(arb[0],arb[1],arb[2],arb[3],arb[4],arb[5]) ;
155  insert(p) ;
156  build() ;
157 }
158 
159 
160 
161 /*********************************************************************/
162 
163 
168 void
169 vpHomography::insert(const vpRotationMatrix &aRb)
170 {
171  aMb.insert(aRb) ;
172 }
173 
178 void
179 vpHomography::insert(const vpHomogeneousMatrix &M)
180 {
181  this->aMb = M ;
182 }
183 
189 void
190 vpHomography::insert(const vpThetaUVector &tu)
191 {
192  vpRotationMatrix aRb(tu) ;
193  aMb.insert(aRb) ;
194 }
195 
200 void
201 vpHomography::insert(const vpTranslationVector &atb)
202 {
203  aMb.insert(atb) ;
204 }
205 
210 void
211 vpHomography::insert(const vpPlane &p)
212 {
213  this->bP = p;
214 }
215 
223 {
224  vpMatrix M = (*this);
225  vpMatrix Minv;
226  M.pseudoInverse(Minv, 1e-16);
227 
228  vpHomography H;
229 
230  for(unsigned int i=0; i<3; i++)
231  for(unsigned int j=0; j<3; j++)
232  H[i][j] = Minv[i][j];
233 
234  return H;
235 }
236 
242 void
244 {
245  bHa = inverse() ;
246 }
247 
248 
249 void
250 vpHomography::save(std::ofstream &f) const
251 {
252  if (! f.fail())
253  {
254  f << *this ;
255  }
256  else
257  {
258  throw(vpException(vpException::ioError, "Cannot write the homography to the output stream")) ;
259  }
260 }
261 
276 {
277  vpHomography Hp;
278  for(unsigned int i = 0; i < 3; i++) {
279  for(unsigned int j = 0; j < 3; j++) {
280  double s = 0.;
281  for(unsigned int k = 0; k < 3; k ++) {
282  s += (*this)[i][k] * H[k][j];
283  }
284  Hp[i][j] = s;
285  }
286  }
287  return Hp;
288 }
289 
297 {
298  if (b.size() != 3)
299  throw(vpException(vpException::dimensionError, "Cannot multiply an homography by a vector of dimension %d", b.size()));
300 
301  vpColVector a(3);
302  for(unsigned int i=0; i<3; i++) {
303  a[i] = 0.;
304  for(unsigned int j=0; j<3; j++)
305  a[i] += (*this)[i][j] * b[j];
306  }
307 
308  return a;
309 }
310 
325 vpHomography vpHomography::operator*(const double &v) const
326 {
327  vpHomography H;
328 
329  for (unsigned int i=0; i < 9; i ++) {
330  H.data[i] = this->data[i] * v;
331  }
332 
333  return H;
334 }
335 
349 vpHomography vpHomography::operator/(const double &v) const
350 {
351  vpHomography H;
352  if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
353  throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
354 
355  double vinv = 1/v ;
356 
357  for (unsigned int i=0; i < 9; i ++) {
358  H.data[i] = this->data[i] * vinv;
359  }
360 
361  return H;
362 }
363 
366 {
367  //if (x == 0)
368  if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
369  throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
370 
371  double vinv = 1/v ;
372 
373  for (unsigned int i=0;i<9;i++)
374  data[i] *= vinv;
375 
376  return *this;
377 }
378 
385 vpHomography &
387 {
388  for (unsigned int i=0; i< 3; i++)
389  for (unsigned int j=0; j< 3; j++)
390  (*this)[i][j] = H[i][j];
391 
392  return *this;
393 }
400 vpHomography &
402 {
403  if (H.getRows() !=3 || H.getCols() != 3)
404  throw(vpException(vpException::dimensionError, "The matrix is not an homography"));
405 
406  for (unsigned int i=0; i< 3; i++)
407  for (unsigned int j=0; j< 3; j++)
408  (*this)[i][j] = H[i][j];
409 
410  return *this;
411 }
412 
419 void
420 vpHomography::load(std::ifstream &f)
421 {
422  if (! f.fail())
423  {
424  for (unsigned int i=0 ; i < 3 ; i++)
425  for (unsigned int j=0 ; j < 3 ; j++)
426  {
427  f >> (*this)[i][j] ;
428  }
429  }
430  else
431  {
432  throw(vpException(vpException::ioError, "Cannot read the homography from the input stream")) ;
433  }
434 }
435 
436 
437 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
438 void
441 {
442  std::cout <<*this << std::endl ;
443 }
444 #endif // #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
445 
453 void
454 vpHomography::build()
455 {
456  vpColVector n(3) ;
457  vpColVector atb(3) ;
458  vpMatrix aRb(3,3);
459  for (unsigned int i=0 ; i < 3 ; i++)
460  {
461  atb[i] = aMb[i][3];
462  for (unsigned int j=0 ; j < 3 ; j++)
463  aRb[i][j] = aMb[i][j];
464  }
465 
466  bP.getNormal(n) ;
467 
468  double d = bP.getD() ;
469  vpMatrix aHb = aRb - atb*n.t()/d ; // the d used in the equation is such as nX=d is the
470  // plane equation. So if the plane is described by
471  // Ax+By+Cz+D=0, d=-D
472 
473  for (unsigned int i=0 ; i < 3 ; i++)
474  for (unsigned int j=0 ; j < 3 ; j++)
475  (*this)[i][j] = aHb[i][j];
476 }
477 
486 void
487 vpHomography::build(vpHomography &aHb,
488  const vpHomogeneousMatrix &aMb,
489  const vpPlane &bP)
490 {
491  vpColVector n(3) ;
492  vpColVector atb(3) ;
493  vpMatrix aRb(3,3);
494  for (unsigned int i=0 ; i < 3 ; i++)
495  {
496  atb[i] = aMb[i][3] ;
497  for (unsigned int j=0 ; j < 3 ; j++)
498  aRb[i][j] = aMb[i][j];
499  }
500 
501  bP.getNormal(n) ;
502 
503  double d = bP.getD() ;
504  vpMatrix aHb_ = aRb - atb*n.t()/d ; // the d used in the equation is such as nX=d is the
505  // plane equation. So if the plane is described by
506  // Ax+By+Cz+D=0, d=-D
507 
508  for (unsigned int i=0 ; i < 3 ; i++)
509  for (unsigned int j=0 ; j < 3 ; j++)
510  aHb[i][j] = aHb_[i][j];
511 }
512 
516 void
518 {
519  for (unsigned int i=0 ; i < 3 ; i++)
520  for (unsigned int j=0 ; j < 3; j++)
521  if (i==j)
522  (*this)[i][j] = 1.0 ;
523  else
524  (*this)[i][j] = 0.0;
525 }
526 
538 {
539  double xa = iPa.get_u();
540  double ya = iPa.get_v();
541  vpMatrix H = cam.get_K() * bHa * cam.get_K_inverse();
542  double z = xa * H[2][0] + ya * H[2][1] + H[2][2];
543  double xb = (xa * H[0][0] + ya * H[0][1] + H[0][2]) / z;
544  double yb = (xa * H[1][0] + ya * H[1][1] + H[1][2]) / z;
545 
546  vpImagePoint iPb(yb, xb);
547 
548  return iPb;
549 }
560 {
561  double xa = Pa.get_x();
562  double ya = Pa.get_y();
563  double z = xa * bHa[2][0] + ya * bHa[2][1] + bHa[2][2];
564  double xb = (xa * bHa[0][0] + ya * bHa[0][1] + bHa[0][2]) / z;
565  double yb = (xa * bHa[1][0] + ya * bHa[1][1] + bHa[1][2]) / z;
566 
567  vpPoint Pb;
568  Pb.set_x(xb);
569  Pb.set_y(yb);
570 
571  return Pb;
572 }
573 
602 void
603 vpHomography::robust(const std::vector<double> &xb, const std::vector<double> &yb,
604  const std::vector<double> &xa, const std::vector<double> &ya,
605  vpHomography &aHb,
606  std::vector<bool> &inliers,
607  double &residual,
608  double weights_threshold,
609  unsigned int niter,
610  bool normalization)
611 {
612  unsigned int n = (unsigned int) xb.size();
613  if (yb.size() != n || xa.size() != n || ya.size() != n)
615  "Bad dimension for robust homography estimation"));
616 
617  // 4 point are required
618  if(n<4)
619  throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
620 
621  try{
622  std::vector<double> xan, yan, xbn, ybn;
623 
624  double xg1=0., yg1=0., coef1=0., xg2=0., yg2=0., coef2=0.;
625 
626  vpHomography aHbn;
627 
628  if (normalization) {
629  vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
630  vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
631  }
632  else {
633  xbn = xb;
634  ybn = yb;
635  xan = xa;
636  yan = ya;
637  }
638 
639  unsigned int nbLinesA = 2;
640  vpMatrix A(nbLinesA*n,8);
641  vpColVector X(8);
642  vpColVector Y(nbLinesA*n);
643  vpMatrix W(nbLinesA*n, nbLinesA*n) ; // Weight matrix
644 
645  vpColVector w(nbLinesA*n) ;
646 
647  // All the weights are set to 1 at the beginning to use a classical least square scheme
648  w = 1;
649  // Update the square matrix associated to the weights
650  for (unsigned int i=0; i < nbLinesA*n; i ++) {
651  W[i][i] = w[i];
652  }
653 
654  // build matrix A
655  for(unsigned int i=0; i<n;i++) {
656  A[nbLinesA*i][0]=xbn[i];
657  A[nbLinesA*i][1]=ybn[i];
658  A[nbLinesA*i][2]=1;
659  A[nbLinesA*i][3]=0 ;
660  A[nbLinesA*i][4]=0 ;
661  A[nbLinesA*i][5]=0;
662  A[nbLinesA*i][6]=-xbn[i]*xan[i] ;
663  A[nbLinesA*i][7]=-ybn[i]*xan[i];
664 
665  A[nbLinesA*i+1][0]=0 ;
666  A[nbLinesA*i+1][1]=0;
667  A[nbLinesA*i+1][2]=0;
668  A[nbLinesA*i+1][3]=xbn[i];
669  A[nbLinesA*i+1][4]=ybn[i];
670  A[nbLinesA*i+1][5]=1;
671  A[nbLinesA*i+1][6]=-xbn[i]*yan[i];
672  A[nbLinesA*i+1][7]=-ybn[i]*yan[i];
673 
674  Y[nbLinesA*i] = xan[i];
675  Y[nbLinesA*i+1] = yan[i];
676  }
677 
678  vpMatrix WA;
679  vpMatrix WAp;
680  unsigned int iter = 0;
681  vpRobust r(nbLinesA*n) ; // M-Estimator
682 
683  while (iter < niter) {
684  WA = W * A;
685 
686  X = WA.pseudoInverse(1e-26)*W*Y;
687  vpColVector residu;
688  residu = Y - A * X;
689 
690  // Compute the weights using the Tukey biweight M-Estimator
691  r.setIteration(iter) ;
692  r.MEstimator(vpRobust::TUKEY, residu, w) ;
693 
694  // Update the weights matrix
695  for (unsigned int i=0; i < n*nbLinesA; i ++) {
696  W[i][i] = w[i];
697  }
698  // Build the homography
699  for(unsigned int i=0;i<8;i++)
700  aHbn.data[i]= X[i];
701  aHbn[2][2] = 1;
702  {
703  vpMatrix aHbnorm = aHbn;
704  aHbnorm /= aHbnorm[2][2] ;
705  }
706 
707  iter ++;
708  }
709  inliers.resize(n);
710  unsigned int nbinliers = 0;
711  for(unsigned int i=0; i< n; i++) {
712  if (w[i*2] < weights_threshold && w[i*2+1] < weights_threshold)
713  inliers[i] = false;
714  else {
715  inliers[i] = true;
716  nbinliers ++;
717  }
718  }
719 
720  if (normalization) {
721  // H after denormalization
722  vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
723  }
724  else {
725  aHb = aHbn;
726  }
727 
728  residual = 0 ;
729  vpColVector a(3), b(3), c(3);
730  for (unsigned int i=0 ; i < n ; i++) {
731  if (inliers[i]) {
732  a[0] = xa[i] ; a[1] = ya[i] ; a[2] = 1 ;
733  b[0] = xb[i] ; b[1] = yb[i] ; b[2] = 1 ;
734 
735  c = aHb*b ; c /= c[2] ;
736  residual += (a-c).sumSquare();
737  }
738  }
739 
740  residual = sqrt(residual/nbinliers);
741  }
742  catch(...)
743  {
744  throw(vpException(vpException::fatalError, "Cannot estimate an homography")) ;
745  }
746 }
747 
751 VISP_EXPORT std::ostream &operator <<(std::ostream &s,const vpHomography &H)
752 {
753  std::ios_base::fmtflags original_flags = s.flags();
754 
755  s.precision(10) ;
756  for (unsigned int i=0;i<H.getRows();i++) {
757  for (unsigned int j=0;j<H.getCols();j++){
758  s << H[i][j] << " ";
759  }
760  // We don't add a \n char on the end of the last matrix line
761  if (i < H.getRows()-1)
762  s << std::endl;
763  }
764 
765  s.flags(original_flags); // restore s to standard state
766 
767  return s;
768 }
vpMatrix get_K_inverse() const
void buildFrom(const vpRotationMatrix &aRb, const vpTranslationVector &atb, const vpPlane &bP)
Construction from Translation and rotation and a plane.
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
double get_v() const
Definition: vpImagePoint.h:264
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:136
static vpImagePoint project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
double get_u() const
Definition: vpImagePoint.h:253
error that can be emited by ViSP classes.
Definition: vpException.h:76
void set_x(const double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.h:194
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.h:138
virtual ~vpHomography()
Class that defines what is a point.
Definition: vpPoint.h:65
The vpRotationMatrix considers the particular case of a rotation matrix.
unsigned int size() const
Definition: vpColVector.h:204
void insert(const vpRotationMatrix &R)
This class aims to compute the homography wrt.two images.
Definition: vpHomography.h:178
unsigned int getRows() const
Return the number of rows of the homography matrix.
Definition: vpHomography.h:257
vp_deprecated void print()
Print the matrix.
vpRowVector t() const
Transpose of a vector.
vpHomography()
initialize an homography as Identity
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.h:196
unsigned int getCols() const
Return the number of columns of the homography matrix.
Definition: vpHomography.h:259
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.h:136
vpColVector getNormal() const
Definition: vpPlane.cpp:250
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
vpHomography operator/(const double &v) const
void save(std::ofstream &f) const
Save an homography in a file.
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
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
The pose is a complete representation of every rigid motion in the euclidian space.
Definition: vpPoseVector.h:92
unsigned int getCols() const
Return the number of columns of the matrix.
Definition: vpMatrix.h:163
vpHomography inverse() const
invert the homography
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:63
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:1932
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:93
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:67
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
double * data
Data array.
Definition: vpHomography.h:182
vpHomography operator*(const vpHomography &H) const
void setIteration(const unsigned int iter)
Set iteration.
Definition: vpRobust.h:122
Class that consider the case of a translation vector.
Class that consider the case of the parameterization for the rotation.
void setIdentity()
double getD() const
Definition: vpPlane.h:121