Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpPoseVector.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  * Pose object. A pose is a size 6 vector [t, tu]^T where tu is
32  * a rotation vector (theta u representation) and t is a translation vector.
33  *
34  * Authors:
35  * Eric Marchand
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
40 
47 #include <sstream>
48 #include <assert.h>
49 
50 #include <visp3/core/vpPoseVector.h>
51 #include <visp3/core/vpMath.h>
52 #include <visp3/core/vpDebug.h>
53 #include <visp3/core/vpMatrixException.h>
54 #include <visp3/core/vpException.h>
55 
56 
68  : vpArray2D<double>(6, 1)
69 {}
70 
87  const double ty,
88  const double tz,
89  const double tux,
90  const double tuy,
91  const double tuz)
92  : vpArray2D<double>(6, 1)
93 {
94  (*this)[0] = tx;
95  (*this)[1] = ty;
96  (*this)[2] = tz;
97 
98  (*this)[3] = tux;
99  (*this)[4] = tuy;
100  (*this)[5] = tuz;
101 }
102 
114  const vpThetaUVector& tu)
115  : vpArray2D<double>(6, 1)
116 {
117  buildFrom(tv, tu) ;
118 }
119 
133  const vpRotationMatrix& R)
134  : vpArray2D<double>(6, 1)
135 {
136  buildFrom(tv, R) ;
137 }
138 
150  : vpArray2D<double>(6, 1)
151 {
152  buildFrom(M) ;
153 }
154 
170 void
171 vpPoseVector::set(const double tx, const double ty, const double tz,
172  const double tux, const double tuy, const double tuz)
173 {
174  (*this)[0] = tx;
175  (*this)[1] = ty;
176  (*this)[2] = tz;
177 
178  (*this)[3] = tux;
179  (*this)[4] = tuy;
180  (*this)[5] = tuz;
181 }
182 
200 vpPoseVector::buildFrom(const double tx, const double ty, const double tz,
201  const double tux, const double tuy, const double tuz)
202 {
203  (*this)[0] = tx;
204  (*this)[1] = ty;
205  (*this)[2] = tz;
206 
207  (*this)[3] = tux;
208  (*this)[4] = tuy;
209  (*this)[5] = tuz;
210  return *this ;
211 }
212 
226 {
227  vpRotationMatrix R ; M.extract(R) ;
228  vpTranslationVector tv ; M.extract(tv) ;
229  buildFrom(tv,R) ;
230  return *this ;
231 }
232 
246  const vpThetaUVector& tu)
247 {
248  for (unsigned int i =0 ; i < 3 ; i++) {
249  (*this)[i] = tv[i] ;
250  (*this)[i+3] = tu[i] ;
251  }
252  return *this ;
253 }
254 
270  const vpRotationMatrix& R)
271 {
272  vpThetaUVector tu ;
273  tu.buildFrom(R) ;
274 
275  buildFrom(tv, tu) ;
276  return *this ;
277 }
278 
282 void
284 {
285  tv[0] = (*this)[0];
286  tv[1] = (*this)[1];
287  tv[2] = (*this)[2];
288 }
289 
293 void
295 {
296  tu[0] = (*this)[3];
297  tu[1] = (*this)[4];
298  tu[2] = (*this)[5];
299 }
303 void
305 {
306  vpRotationMatrix R((*this)[3], (*this)[4], (*this)[5]);
307  q.buildFrom(R);
308 }
312 void
314 {
315  R.buildFrom((*this)[3], (*this)[4], (*this)[5]);
316 }
322 {
323  vpTranslationVector tr((*this)[0], (*this)[1], (*this)[2]);
324  return tr;
325 }
326 
332 {
333  vpRotationMatrix R((*this)[0], (*this)[1], (*this)[2]);
334  return R;
335 }
336 
342 {
343  vpThetaUVector tu((*this)[3], (*this)[4], (*this)[5]);
344  return tu;
345 }
346 
368 void
370 {
371  for (unsigned int i =0 ; i < 6 ; i++)
372  if (i<3) std::cout << (*this)[i] <<" " ;
373  else std::cout << vpMath::deg((*this)[i]) <<" " ;
374  std::cout <<std::endl ;
375 }
376 
387 void
388 vpPoseVector::save(std::ofstream &f) const
389 {
390  if (! f.fail()) {
391  f << *this ;
392  }
393  else {
395  "Cannot save the pose vector: ofstream not openned")) ;
396  }
397 }
398 
399 
410 void
411 vpPoseVector::load(std::ifstream &f)
412 {
413  if (! f.fail()) {
414  for (unsigned int i=0 ; i < 6 ; i++) {
415  f >> (*this)[i] ;
416  }
417  }
418  else {
420  "Cannot read pose vector: ifstream not openned")) ;
421  }
422 }
423 
424 /*
425  Transpose the pose vector. The resulting vector becomes a row vector.
426 
427 */
429 {
430  vpRowVector v(rowNum);
431  memcpy(v.data, data, rowNum*sizeof(double)) ;
432  return v;
433 }
434 
454 int
455 vpPoseVector::print(std::ostream& s, unsigned int length, char const* intro) const
456 {
457  typedef std::string::size_type size_type;
458 
459  unsigned int m = getRows();
460  unsigned int n = 1;
461 
462  std::vector<std::string> values(m*n);
463  std::ostringstream oss;
464  std::ostringstream ossFixed;
465  std::ios_base::fmtflags original_flags = oss.flags();
466 
467  // ossFixed <<std::fixed;
468  ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
469 
470  size_type maxBefore=0; // the length of the integral part
471  size_type maxAfter=0; // number of decimals plus
472  // one place for the decimal point
473  for (unsigned int i=0;i<m;++i) {
474  oss.str("");
475  oss << (*this)[i];
476  if (oss.str().find("e")!=std::string::npos){
477  ossFixed.str("");
478  ossFixed << (*this)[i];
479  oss.str(ossFixed.str());
480  }
481 
482  values[i]=oss.str();
483  size_type thislen=values[i].size();
484  size_type p=values[i].find('.');
485 
486  if (p==std::string::npos){
487  maxBefore=vpMath::maximum(maxBefore, thislen);
488  // maxAfter remains the same
489  } else{
490  maxBefore=vpMath::maximum(maxBefore, p);
491  maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
492  }
493 
494  }
495 
496  size_type totalLength=length;
497  // increase totalLength according to maxBefore
498  totalLength=vpMath::maximum(totalLength,maxBefore);
499  // decrease maxAfter according to totalLength
500  maxAfter=std::min(maxAfter, totalLength-maxBefore);
501  if (maxAfter==1) maxAfter=0;
502 
503  // the following line is useful for debugging
504  //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
505 
506  if (intro) s <<intro;
507  s <<"["<<m<<","<<n<<"]=\n";
508 
509  for (unsigned int i=0;i<m;i++) {
510  s <<" ";
511  size_type p=values[i].find('.');
512  s.setf(std::ios::right, std::ios::adjustfield);
513  s.width((std::streamsize)maxBefore);
514  s <<values[i].substr(0,p).c_str();
515 
516  if (maxAfter>0){
517  s.setf(std::ios::left, std::ios::adjustfield);
518  if (p!=std::string::npos){
519  s.width((std::streamsize)maxAfter);
520  s <<values[i].substr(p,maxAfter).c_str();
521  } else{
522  assert(maxAfter>1);
523  s.width((std::streamsize)maxAfter);
524  s <<".0";
525  }
526  }
527 
528  s <<' ';
529 
530  s <<std::endl;
531  }
532 
533  s.flags(original_flags); // restore s to standard state
534 
535  return (int)(maxBefore+maxAfter);
536 }
void load(std::ifstream &f)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:70
vpTranslationVector getTranslationVector() const
error that can be emited by ViSP classes.
Definition: vpException.h:73
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:84
vpQuaternionVector buildFrom(const double qx, const double qy, const double qz, const double qw)
Implementation of a generic 2D array used as vase class of matrices and vectors.
Definition: vpArray2D.h:70
void print() const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:140
Implementation of a rotation matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:74
void save(std::ofstream &f) const
void set(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
vpRotationMatrix getRotationMatrix() const
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
void extract(vpRotationMatrix &R) const
Implementation of a rotation vector as quaternion angle minimal representation.
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
static double deg(double rad)
Definition: vpMath.h:97
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:93
vpPoseVector buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
vpThetaUVector getThetaUVector() const
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
vpRowVector t() const