ViSP  2.8.0
vpHomogeneousMatrix.cpp
1 /****************************************************************************
2  *
3  * $Id: vpHomogeneousMatrix.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  * Homogeneous matrix.
36  *
37  * Authors:
38  * Eric Marchand
39  *
40  *****************************************************************************/
41 
42 
49 #include <visp/vpDebug.h>
50 #include <visp/vpMatrix.h>
51 #include <visp/vpHomogeneousMatrix.h>
52 #include <visp/vpQuaternionVector.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 
65 void
67 {
68  unsigned int i,j ;
69 
70  try {
71  resize(4,4) ;
72  }
73  catch(vpException me)
74  {
75  vpERROR_TRACE("Error caught") ;
76  throw ;
77  }
78 
79 
80  for (i=0 ; i < 4 ; i++)
81  for (j=0 ; j < 4; j++)
82  if (i==j)
83  (*this)[i][j] = 1.0 ;
84  else
85  (*this)[i][j] = 0.0;
86 
87 }
88 
90  const vpQuaternionVector &q)
91 {
92  init();
93  buildFrom(t,q);
94 }
95 
100 {
101  init() ;
102 }
103 
104 
109 {
110  init() ;
111  *this = M ;
112 }
113 
115  const vpThetaUVector &tu) : vpMatrix()
116 {
117  init() ;
118  buildFrom(t,tu) ;
119 }
120 
122  const vpRotationMatrix &R) : vpMatrix()
123 {
124  init() ;
125  insert(R) ;
126  insert(t) ;
127 }
128 
130 {
131 
132  init() ;
133  buildFrom(p[0],p[1],p[2],p[3],p[4],p[5]) ;
134 }
135 
137  const double ty,
138  const double tz,
139  const double tux,
140  const double tuy,
141  const double tuz) : vpMatrix()
142 {
143  init() ;
144  buildFrom(tx, ty, tz,tux, tuy, tuz) ;
145 }
146 
147 void
149  const vpThetaUVector &tu)
150 {
151  insert(tu) ;
152  insert(t) ;
153 }
154 
155 void
157  const vpRotationMatrix &R)
158 {
159  init() ;
160  insert(R) ;
161  insert(t) ;
162 }
163 
164 
165 void
167 {
168 
169  vpTranslationVector t(p[0],p[1],p[2]) ;
170  vpThetaUVector tu(p[3],p[4],p[5]) ;
171 
172  insert(tu) ;
173  insert(t) ;
174 }
175 
177  const vpQuaternionVector &q)
178 {
179  insert(t);
180  insert(q);
181 }
182 
183 void
185  const double ty,
186  const double tz,
187  const double tux,
188  const double tuy,
189  const double tuz)
190 {
191  vpRotationMatrix R(tux, tuy, tuz) ;
192  vpTranslationVector t(tx, ty, tz) ;
193 
194  insert(R) ;
195  insert(t) ;
196 }
197 
205 {
206 
207  if (rowPtrs != M.rowPtrs) init() ;
208 
209  for (int i=0; i<4; i++)
210  {
211  for (int j=0; j<4; j++)
212  {
213  rowPtrs[i][j] = M.rowPtrs[i][j];
214  }
215  }
216  return *this;
217 }
218 
238 {
239  vpHomogeneousMatrix p,p1 ;
240 
241  vpRotationMatrix R1, R2, R ;
242  vpTranslationVector T1, T2 , T;
243 
244 
245  extract(T1) ;
246  M.extract(T2) ;
247 
248  extract (R1) ;
249  M.extract (R2) ;
250 
251  R = R1*R2 ;
252 
253  T = R1*T2 + T1 ;
254 
255  p.insert(T) ;
256  p.insert(R) ;
257 
258  return p;
259 }
260 
263 {
264  vpColVector p(rowNum);
265 
266  p = 0.0;
267 
268  for (unsigned int j=0;j<4;j++) {
269  for (unsigned int i=0;i<4;i++) {
270  p[i]+=rowPtrs[i][j] * v[j];
271  }
272  }
273 
274  return p;
275 }
276 
277 
278 /*********************************************************************/
279 
284 bool
286 {
287  vpRotationMatrix R ;
288  extract(R) ;
289 
290  return R.isARotationMatrix() ;
291 }
292 
297 void
299 {
300  unsigned int i,j ;
301 
302  for (i=0 ; i < 3 ; i++)
303  for (j=0 ; j < 3; j++)
304  R[i][j] = (*this)[i][j] ;
305 }
306 
310 void
312 {
313  t[0] = (*this)[0][3] ;
314  t[1] = (*this)[1][3] ;
315  t[2] = (*this)[2][3] ;
316 }
320 void
322 {
323 
325  (*this).extract(R);
326  tu.buildFrom(R);
327 }
328 
332 void
334 {
336  (*this).extract(R);
337  q.buildFrom(R);
338 }
339 
343 void
345 {
346  unsigned int i,j ;
347 
348  for (i=0 ; i < 3 ; i++)
349  for (j=0 ; j < 3; j++)
350  (*this)[i][j] = R[i][j] ;
351 }
352 
359 void
361 {
362  vpRotationMatrix R(tu) ;
363  insert(R) ;
364 }
365 
369 void
371 {
372  (*this)[0][3] = T[0] ;
373  (*this)[1][3] = T[1] ;
374  (*this)[2][3] = T[2] ;
375 }
376 
383 void
386 }
387 
404 {
406 
407 
408  vpRotationMatrix R ; extract(R) ;
410 
411  vpTranslationVector RtT ; RtT = -(R.t()*T) ;
412 
413 
414  Mi.insert(R.t()) ;
415  Mi.insert(RtT) ;
416 
417  return Mi ;
418 }
419 
424 {
425  (*this)[0][0] = 1 ;
426  (*this)[1][1] = 1 ;
427  (*this)[2][2] = 1 ;
428 
429  (*this)[0][1] = (*this)[0][2] = 0 ;
430  (*this)[1][0] = (*this)[1][2] = 0 ;
431  (*this)[2][0] = (*this)[2][1] = 0 ;
432 
433  (*this)[0][3] = 0 ;
434  (*this)[1][3] = 0 ;
435  (*this)[2][3] = 0 ;
436 }
437 
452 void
454 {
455  M = inverse() ;
456 }
457 
458 
481 void
482 vpHomogeneousMatrix::save(std::ofstream &f) const
483 {
484  if (f != NULL)
485  {
486  f << *this ;
487  }
488  else
489  {
490  vpERROR_TRACE("\t\t file not open " );
491  throw(vpException(vpException::ioError, "\t\t file not open")) ;
492  }
493 }
494 
495 
514 void
515 vpHomogeneousMatrix::load(std::ifstream &f)
516 {
517  if (f != NULL)
518  {
519  for (unsigned int i=0 ; i < 4 ; i++)
520  for (unsigned int j=0 ; j < 4 ; j++)
521  {
522  f>> (*this)[i][j] ;
523  }
524  }
525  else
526  {
527  vpERROR_TRACE("\t\t file not open " );
528  throw(vpException(vpException::ioError, "\t\t file not open")) ;
529  }
530 }
531 
533 void
535 {
536  vpPoseVector r(*this) ;
537  std::cout << r.t() ;
538 }
540 void
542 {
543  init() ;
544 }
545 
546 
547 /*
548  * Local variables:
549  * c-basic-offset: 2
550  * End:
551  */
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
void print()
Print the matrix as a vector [T thetaU].
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
void setIdentity()
Basic initialisation (identity)
error that can be emited by ViSP classes.
Definition: vpException.h:75
vpRotationMatrix t() const
transpose
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
void load(std::ifstream &f)
bool isARotationMatrix() const
test if the matrix is an rotation matrix
The vpRotationMatrix considers the particular case of a rotation matrix.
vpHomogeneousMatrix & operator=(const vpHomogeneousMatrix &M)
Copy operator from vpHomogeneousMatrix.
void insert(const vpRotationMatrix &R)
double ** rowPtrs
address of the first element of each rows
Definition: vpMatrix.h:119
vpHomogeneousMatrix()
Basic constructor.
vpRowVector t() const
transpose of Vector
void extract(vpRotationMatrix &R) const
Defines a quaternion and its basic operations.
void buildFrom(const vpRotationMatrix &R)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
bool isAnHomogeneousMatrix() const
unsigned int rowNum
number of rows
Definition: vpMatrix.h:110
void save(std::ofstream &f) const
vpMatrix t() const
Definition: vpMatrix.cpp:1176
vpHomogeneousMatrix operator*(const vpHomogeneousMatrix &M) const
Multiply two homogeneous matrices: aMb = aMc*cMb.
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
vpHomogeneousMatrix inverse() const
void init()
Basic initialisation (identity).
Class that consider the case of a translation vector.
Class that consider the case of the parameterization for the rotation.