Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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(double tx, double ty, double tz, double tux, double tuy, double tuz)
84  : vpArray2D<double>(6, 1)
85 {
86  (*this)[0] = tx;
87  (*this)[1] = ty;
88  (*this)[2] = tz;
89 
90  (*this)[3] = tux;
91  (*this)[4] = tuy;
92  (*this)[5] = tuz;
93 }
94 
106 {
107  buildFrom(tv, tu);
108 }
109 
123 {
124  buildFrom(tv, R);
125 }
126 
138 
154 void vpPoseVector::set(double tx, double ty, double tz, double tux, double tuy, double tuz)
155 {
156  (*this)[0] = tx;
157  (*this)[1] = ty;
158  (*this)[2] = tz;
159 
160  (*this)[3] = tux;
161  (*this)[4] = tuy;
162  (*this)[5] = tuz;
163 }
164 
181 vpPoseVector vpPoseVector::buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
182 {
183  (*this)[0] = tx;
184  (*this)[1] = ty;
185  (*this)[2] = tz;
186 
187  (*this)[3] = tux;
188  (*this)[4] = tuy;
189  (*this)[5] = tuz;
190  return *this;
191 }
192 
205 {
207  M.extract(R);
209  M.extract(tv);
210  buildFrom(tv, R);
211  return *this;
212 }
213 
226 {
227  for (unsigned int i = 0; i < 3; i++) {
228  (*this)[i] = tv[i];
229  (*this)[i + 3] = tu[i];
230  }
231  return *this;
232 }
233 
248 {
249  vpThetaUVector tu;
250  tu.buildFrom(R);
251 
252  buildFrom(tv, tu);
253  return *this;
254 }
255 
260 {
261  tv[0] = (*this)[0];
262  tv[1] = (*this)[1];
263  tv[2] = (*this)[2];
264 }
265 
270 {
271  tu[0] = (*this)[3];
272  tu[1] = (*this)[4];
273  tu[2] = (*this)[5];
274 }
279 {
280  vpRotationMatrix R((*this)[3], (*this)[4], (*this)[5]);
281  q.buildFrom(R);
282 }
286 void vpPoseVector::extract(vpRotationMatrix &R) const { R.buildFrom((*this)[3], (*this)[4], (*this)[5]); }
292 {
293  vpTranslationVector tr((*this)[0], (*this)[1], (*this)[2]);
294  return tr;
295 }
296 
302 {
303  vpRotationMatrix R((*this)[3], (*this)[4], (*this)[5]);
304  return R;
305 }
306 
312 {
313  vpThetaUVector tu((*this)[3], (*this)[4], (*this)[5]);
314  return tu;
315 }
316 
339 {
340  for (unsigned int i = 0; i < 6; i++)
341  if (i < 3)
342  std::cout << (*this)[i] << " ";
343  else
344  std::cout << vpMath::deg((*this)[i]) << " ";
345  std::cout << std::endl;
346 }
347 
359 void vpPoseVector::save(std::ofstream &f) const
360 {
361  if (!f.fail()) {
362  f << *this;
363  } else {
364  throw(vpException(vpException::ioError, "Cannot save the pose vector: ofstream not opened"));
365  }
366 }
367 
378 void vpPoseVector::load(std::ifstream &f)
379 {
380  if (!f.fail()) {
381  for (unsigned int i = 0; i < 6; i++) {
382  f >> (*this)[i];
383  }
384  } else {
385  throw(vpException(vpException::ioError, "Cannot read pose vector: ifstream not opened"));
386  }
387 }
388 
389 /*
390  Transpose the pose vector. The resulting vector becomes a row vector.
391 
392 */
394 {
395  vpRowVector v(rowNum);
396  memcpy(v.data, data, rowNum * sizeof(double));
397  return v;
398 }
399 
419 int vpPoseVector::print(std::ostream &s, unsigned int length, char const *intro) const
420 {
421  typedef std::string::size_type size_type;
422 
423  unsigned int m = getRows();
424  unsigned int n = 1;
425 
426  std::vector<std::string> values(m * n);
427  std::ostringstream oss;
428  std::ostringstream ossFixed;
429  std::ios_base::fmtflags original_flags = oss.flags();
430 
431  // ossFixed <<std::fixed;
432  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
433 
434  size_type maxBefore = 0; // the length of the integral part
435  size_type maxAfter = 0; // number of decimals plus
436  // one place for the decimal point
437  for (unsigned int i = 0; i < m; ++i) {
438  oss.str("");
439  oss << (*this)[i];
440  if (oss.str().find("e") != std::string::npos) {
441  ossFixed.str("");
442  ossFixed << (*this)[i];
443  oss.str(ossFixed.str());
444  }
445 
446  values[i] = oss.str();
447  size_type thislen = values[i].size();
448  size_type p = values[i].find('.');
449 
450  if (p == std::string::npos) {
451  maxBefore = vpMath::maximum(maxBefore, thislen);
452  // maxAfter remains the same
453  } else {
454  maxBefore = vpMath::maximum(maxBefore, p);
455  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
456  }
457  }
458 
459  size_type totalLength = length;
460  // increase totalLength according to maxBefore
461  totalLength = vpMath::maximum(totalLength, maxBefore);
462  // decrease maxAfter according to totalLength
463  maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
464  if (maxAfter == 1)
465  maxAfter = 0;
466 
467  // the following line is useful for debugging
468  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
469 
470  if (intro)
471  s << intro;
472  s << "[" << m << "," << n << "]=\n";
473 
474  for (unsigned int i = 0; i < m; i++) {
475  s << " ";
476  size_type p = values[i].find('.');
477  s.setf(std::ios::right, std::ios::adjustfield);
478  s.width((std::streamsize)maxBefore);
479  s << values[i].substr(0, p).c_str();
480 
481  if (maxAfter > 0) {
482  s.setf(std::ios::left, std::ios::adjustfield);
483  if (p != std::string::npos) {
484  s.width((std::streamsize)maxAfter);
485  s << values[i].substr(p, maxAfter).c_str();
486  } else {
487  assert(maxAfter > 1);
488  s.width((std::streamsize)maxAfter);
489  s << ".0";
490  }
491  }
492 
493  s << ' ';
494 
495  s << std::endl;
496  }
497 
498  s.flags(original_flags); // restore s to standard state
499 
500  return (int)(maxBefore + maxAfter);
501 }
void load(std::ifstream &f)
void save(std::ofstream &f) const
void extract(vpRotationMatrix &R) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:115
void set(double tx, double ty, double tz, double tux, double tuy, double tuz)
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
vpQuaternionVector buildFrom(const double qx, const double qy, const double qz, const double qw)
void print() const
Implementation of a generic 2D array used as base class for matrices and vectors. ...
Definition: vpArray2D.h:131
void extract(vpRotationMatrix &R) const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
vpTranslationVector getTranslationVector() const
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
Implementation of a rotation matrix and operations on such kind of matrices.
vpThetaUVector getThetaUVector() const
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
Implementation of a rotation vector as quaternion angle minimal representation.
static double deg(double rad)
Definition: vpMath.h:103
vpRotationMatrix getRotationMatrix() const
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
vpRowVector t() const
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.