ViSP  2.8.0
vpHomography.cpp
1 /****************************************************************************
2  *
3  * $Id: vpHomography.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 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 <visp/vpHomography.h>
51 #include <visp/vpDebug.h>
52 #include <visp/vpMatrix.h>
53 
54 // Exception
55 #include <visp/vpException.h>
56 #include <visp/vpMatrixException.h>
57 
58 // Debug trace
59 #include <visp/vpDebug.h>
60 
61 #include <stdio.h>
62 
63 
64 
69 void
70 vpHomography::init()
71 {
72  unsigned int i,j ;
73 
74  try {
75  vpMatrix::resize(3,3) ;
76  }
77  catch(vpException me)
78  {
79  vpERROR_TRACE("Error caught") ;
80  throw ;
81  }
82 
83 
84  for (i=0 ; i < 3 ; i++)
85  for (j=0 ; j < 3; j++)
86  if (i==j)
87  (*this)[i][j] = 1.0 ;
88  else
89  (*this)[i][j] = 0.0;
90 
91 }
92 
97 {
98  init() ;
99 }
100 
101 
107 {
108  init() ;
109  *this = aHb ;
110 }
111 
117  const vpPlane &_bP) : vpMatrix()
118 {
119 
120 
121  init() ;
122 
123  buildFrom(aMb,_bP) ;
124 
125 
126 }
127 
129  const vpTranslationVector &atb,
130  const vpPlane &_bP) : vpMatrix()
131 {
132  init() ;
133  buildFrom(tu,atb,_bP) ;
134 }
135 
137  const vpTranslationVector &atb,
138  const vpPlane &_bP) : vpMatrix()
139 {
140  init() ;
141  buildFrom(aRb,atb,_bP) ;
142  }
143 
145  const vpPlane &_bP) : vpMatrix()
146 {
147 
148  init() ;
149  buildFrom(arb,_bP) ;
150 }
151 
152 
153 
154 void
156  const vpPlane &_bP)
157 {
158 
159 
160  insert(aMb) ;
161  insert(_bP) ;
162  build() ;
163 
164 
165 }
166 
167 void
169  const vpTranslationVector &atb,
170  const vpPlane &_bP)
171 {
172 
173  insert(tu) ;
174  insert(atb) ;
175  insert(_bP) ;
176  build() ;
177 }
178 
179 void
181  const vpTranslationVector &atb,
182  const vpPlane &_bP)
183 {
184  init() ;
185  insert(aRb) ;
186  insert(atb) ;
187  insert(_bP) ;
188  build() ;
189 }
190 
191 void
193  const vpPlane &_bP)
194 {
195 
196  aMb.buildFrom(arb[0],arb[1],arb[2],arb[3],arb[4],arb[5]) ;
197  insert(_bP) ;
198  build() ;
199 }
200 
201 
202 
203 /*********************************************************************/
204 
205 
210 void
211 vpHomography::insert(const vpRotationMatrix &aRb)
212 {
213  aMb.insert(aRb) ;
214  //build() ;
215 }
220 void
221 vpHomography::insert(const vpHomogeneousMatrix &_aMb)
222 {
223 
224  aMb = _aMb ;
225  //build() ;
226 }
227 
228 
234 void
235 vpHomography::insert(const vpThetaUVector &tu)
236 {
237  vpRotationMatrix aRb(tu) ;
238  aMb.insert(aRb) ;
239  //build() ;
240 }
241 
242 
247 void
248 vpHomography::insert(const vpTranslationVector &atb)
249 {
250  aMb.insert(atb) ;
251  //build() ;
252 }
253 
258 void
259 vpHomography::insert(const vpPlane &_bP)
260 {
261 
262  bP= _bP ;
263  //build() ;
264 }
265 
266 
275 {
276  vpHomography bHa ;
277 
278 
279  vpMatrix::pseudoInverse(bHa,1e-16) ;
280 
281  return bHa;
282 }
283 
290 void
292 {
293  bHa = inverse() ;
294 }
295 
296 
297 void
298 vpHomography::save(std::ofstream &f) const
299 {
300  if (f != NULL)
301  {
302  f << *this ;
303  }
304  else
305  {
306  vpERROR_TRACE("\t\t file not open " );
307  throw(vpException(vpException::ioError, "\t\t file not open")) ;
308  }
309 }
310 
325 {
326  vpHomography Hp;
327  for(unsigned int i = 0; i < 3; i++) {
328  for(unsigned int j = 0; j < 3; j++) {
329  double s = 0.;
330  for(unsigned int k = 0; k < 3; k ++) {
331  s += (*this)[i][k] * H[k][j];
332  }
333  Hp[i][j] = s;
334  }
335  }
336  return Hp;
337 }
338 
353 vpHomography vpHomography::operator*(const double &v) const
354 {
355  vpHomography H;
356 
357  for (unsigned int i=0; i < 9; i ++) {
358  H.data[i] = this->data[i] * v;
359  }
360 
361  return H;
362 }
363 
377 vpHomography vpHomography::operator/(const double &v) const
378 {
379  vpHomography H;
380  double one_over_v = 1. / v;
381 
382  for (unsigned int i=0; i < 9; i ++) {
383  H.data[i] = this->data[i] * one_over_v;
384  }
385 
386  return H;
387 }
394 void
395 vpHomography::load(std::ifstream &f)
396 {
397  if (f != NULL)
398  {
399  for (unsigned int i=0 ; i < 3 ; i++)
400  for (unsigned int j=0 ; j < 3 ; j++)
401  {
402  f>> (*this)[i][j] ;
403  }
404  }
405  else
406  {
407  vpERROR_TRACE("\t\t file not open " );
408  throw(vpException(vpException::ioError, "\t\t file not open")) ;
409  }
410 }
411 
412 
413 
415 void
417 {
418  std::cout <<*this << std::endl ;
419 }
420 
428 void
430 {
431  unsigned int i,j ;
432 
433  vpColVector n(3) ;
434  vpColVector atb(3) ;
435  for (i=0 ; i < 3 ; i++)
436  {
437  atb[i] = aMb[i][3] ;
438  for (j=0 ; j < 3 ; j++) (*this)[i][j] = aMb[i][j];
439  }
440 
441  bP.getNormal(n) ;
442 
443  double d = bP.getD() ;
444  *this -= atb*n.t()/d ; // the d used in the equation is such as nX=d is the
445  // plane equation. So if the plane is described by
446  // Ax+By+Cz+D=0, d=-D
447 
448 }
449 
458 void
460  const vpHomogeneousMatrix &aMb,
461  const vpPlane &bP)
462 {
463  unsigned int i,j ;
464 
465  vpColVector n(3) ;
466  vpColVector atb(3) ;
467  for (i=0 ; i < 3 ; i++)
468  {
469  atb[i] = aMb[i][3] ;
470  for (j=0 ; j < 3 ; j++) aHb[i][j] = aMb[i][j];
471  }
472 
473  bP.getNormal(n) ;
474 
475  double d = bP.getD() ;
476  aHb -= atb*n.t()/d ; // the d used in the equation is such as nX=d is the
477  // plane equation. So if the plane is described by
478  // Ax+By+Cz+D=0, d=-D
479 
480 }
481 
482 /*
483  * Local variables:
484  * c-basic-offset: 2
485  * End:
486  */
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:96
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:174
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:379
error that can be emited by ViSP classes.
Definition: vpException.h:75
The vpRotationMatrix considers the particular case of a rotation matrix.
double * data
address of the first element of the data array
Definition: vpMatrix.h:116
void insert(const vpRotationMatrix &R)
This class aims to compute the homography wrt.two images.
Definition: vpHomography.h:173
void print()
Print the matrix.
vpRowVector t() const
transpose of Vector
vpHomography()
initialize an homography as Identity
void load(std::ifstream &f)
Load an homography from a file.
vpColVector getNormal() const
Definition: vpPlane.cpp:226
void build()
build the homography from aMb and Rb
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.
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
vpHomography inverse() const
invert the homography
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1812
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:67
vpHomography operator*(const vpHomography &H) const
Class that consider the case of a translation vector.
Class that consider the case of the parameterization for the rotation.
double getD() const
Definition: vpPlane.h:118