Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpPoseVector.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Pose object. A pose is a size 6 vector [t, tu]^T where tu is
33  * a rotation vector (theta u representation) and t is a translation vector.
34  *
35  * Authors:
36  * Eric Marchand
37  * Fabien Spindler
38  *
39  *****************************************************************************/
40 
47 #include <assert.h>
48 #include <sstream>
49 
50 #include <visp3/core/vpDebug.h>
51 #include <visp3/core/vpException.h>
52 #include <visp3/core/vpMath.h>
53 #include <visp3/core/vpMatrixException.h>
54 #include <visp3/core/vpPoseVector.h>
55 
67 
83 vpPoseVector::vpPoseVector(const double tx, const double ty, const double tz, const double tux, const double tuy,
84  const double tuz)
85  : vpArray2D<double>(6, 1)
86 {
87  (*this)[0] = tx;
88  (*this)[1] = ty;
89  (*this)[2] = tz;
90 
91  (*this)[3] = tux;
92  (*this)[4] = tuy;
93  (*this)[5] = tuz;
94 }
95 
107 {
108  buildFrom(tv, tu);
109 }
110 
124 {
125  buildFrom(tv, R);
126 }
127 
139 
155 void vpPoseVector::set(const double tx, const double ty, const double tz, const double tux, const double tuy,
156  const double tuz)
157 {
158  (*this)[0] = tx;
159  (*this)[1] = ty;
160  (*this)[2] = tz;
161 
162  (*this)[3] = tux;
163  (*this)[4] = tuy;
164  (*this)[5] = tuz;
165 }
166 
183 vpPoseVector vpPoseVector::buildFrom(const double tx, const double ty, const double tz, const double tux,
184  const double tuy, const double tuz)
185 {
186  (*this)[0] = tx;
187  (*this)[1] = ty;
188  (*this)[2] = tz;
189 
190  (*this)[3] = tux;
191  (*this)[4] = tuy;
192  (*this)[5] = tuz;
193  return *this;
194 }
195 
208 {
210  M.extract(R);
212  M.extract(tv);
213  buildFrom(tv, R);
214  return *this;
215 }
216 
229 {
230  for (unsigned int i = 0; i < 3; i++) {
231  (*this)[i] = tv[i];
232  (*this)[i + 3] = tu[i];
233  }
234  return *this;
235 }
236 
251 {
252  vpThetaUVector tu;
253  tu.buildFrom(R);
254 
255  buildFrom(tv, tu);
256  return *this;
257 }
258 
263 {
264  tv[0] = (*this)[0];
265  tv[1] = (*this)[1];
266  tv[2] = (*this)[2];
267 }
268 
273 {
274  tu[0] = (*this)[3];
275  tu[1] = (*this)[4];
276  tu[2] = (*this)[5];
277 }
282 {
283  vpRotationMatrix R((*this)[3], (*this)[4], (*this)[5]);
284  q.buildFrom(R);
285 }
289 void vpPoseVector::extract(vpRotationMatrix &R) const { R.buildFrom((*this)[3], (*this)[4], (*this)[5]); }
295 {
296  vpTranslationVector tr((*this)[0], (*this)[1], (*this)[2]);
297  return tr;
298 }
299 
305 {
306  vpRotationMatrix R((*this)[3], (*this)[4], (*this)[5]);
307  return R;
308 }
309 
315 {
316  vpThetaUVector tu((*this)[3], (*this)[4], (*this)[5]);
317  return tu;
318 }
319 
342 {
343  for (unsigned int i = 0; i < 6; i++)
344  if (i < 3)
345  std::cout << (*this)[i] << " ";
346  else
347  std::cout << vpMath::deg((*this)[i]) << " ";
348  std::cout << std::endl;
349 }
350 
362 void vpPoseVector::save(std::ofstream &f) const
363 {
364  if (!f.fail()) {
365  f << *this;
366  } else {
367  throw(vpException(vpException::ioError, "Cannot save the pose vector: ofstream not openned"));
368  }
369 }
370 
381 void vpPoseVector::load(std::ifstream &f)
382 {
383  if (!f.fail()) {
384  for (unsigned int i = 0; i < 6; i++) {
385  f >> (*this)[i];
386  }
387  } else {
388  throw(vpException(vpException::ioError, "Cannot read pose vector: ifstream not openned"));
389  }
390 }
391 
392 /*
393  Transpose the pose vector. The resulting vector becomes a row vector.
394 
395 */
397 {
398  vpRowVector v(rowNum);
399  memcpy(v.data, data, rowNum * sizeof(double));
400  return v;
401 }
402 
422 int vpPoseVector::print(std::ostream &s, unsigned int length, char const *intro) const
423 {
424  typedef std::string::size_type size_type;
425 
426  unsigned int m = getRows();
427  unsigned int n = 1;
428 
429  std::vector<std::string> values(m * n);
430  std::ostringstream oss;
431  std::ostringstream ossFixed;
432  std::ios_base::fmtflags original_flags = oss.flags();
433 
434  // ossFixed <<std::fixed;
435  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
436 
437  size_type maxBefore = 0; // the length of the integral part
438  size_type maxAfter = 0; // number of decimals plus
439  // one place for the decimal point
440  for (unsigned int i = 0; i < m; ++i) {
441  oss.str("");
442  oss << (*this)[i];
443  if (oss.str().find("e") != std::string::npos) {
444  ossFixed.str("");
445  ossFixed << (*this)[i];
446  oss.str(ossFixed.str());
447  }
448 
449  values[i] = oss.str();
450  size_type thislen = values[i].size();
451  size_type p = values[i].find('.');
452 
453  if (p == std::string::npos) {
454  maxBefore = vpMath::maximum(maxBefore, thislen);
455  // maxAfter remains the same
456  } else {
457  maxBefore = vpMath::maximum(maxBefore, p);
458  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
459  }
460  }
461 
462  size_type totalLength = length;
463  // increase totalLength according to maxBefore
464  totalLength = vpMath::maximum(totalLength, maxBefore);
465  // decrease maxAfter according to totalLength
466  maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
467  if (maxAfter == 1)
468  maxAfter = 0;
469 
470  // the following line is useful for debugging
471  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
472 
473  if (intro)
474  s << intro;
475  s << "[" << m << "," << n << "]=\n";
476 
477  for (unsigned int i = 0; i < m; i++) {
478  s << " ";
479  size_type p = values[i].find('.');
480  s.setf(std::ios::right, std::ios::adjustfield);
481  s.width((std::streamsize)maxBefore);
482  s << values[i].substr(0, p).c_str();
483 
484  if (maxAfter > 0) {
485  s.setf(std::ios::left, std::ios::adjustfield);
486  if (p != std::string::npos) {
487  s.width((std::streamsize)maxAfter);
488  s << values[i].substr(p, maxAfter).c_str();
489  } else {
490  assert(maxAfter > 1);
491  s.width((std::streamsize)maxAfter);
492  s << ".0";
493  }
494  }
495 
496  s << ' ';
497 
498  s << std::endl;
499  }
500 
501  s.flags(original_flags); // restore s to standard state
502 
503  return (int)(maxBefore + maxAfter);
504 }
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:72
vpTranslationVector getTranslationVector() const
error that can be emited by ViSP classes.
Definition: vpException.h:71
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:137
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
Definition: vpArray2D.h:156
static double deg(double rad)
Definition: vpMath.h:95
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:92
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