Visual Servoing Platform  version 3.5.1 under development (2023-03-29)
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 }
502 
507 std::vector<double> vpPoseVector::toStdVector() const
508 {
509  std::vector<double> v(this->size());
510 
511  for (unsigned int i = 0; i < this->size(); i++)
512  v[i] = data[i];
513  return v;
514 }
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:129
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:142
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:132
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:290
unsigned int getRows() const
Definition: vpArray2D.h:288
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ ioError
I/O error.
Definition: vpException.h:91
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:175
static double deg(double rad)
Definition: vpMath.h:111
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
vpTranslationVector getTranslationVector() const
std::vector< double > toStdVector() const
void load(std::ifstream &f)
void save(std::ofstream &f) const
vpRowVector t() const
void extract(vpRotationMatrix &R) const
void set(double tx, double ty, double tz, double tux, double tuy, double tuz)
vpThetaUVector getThetaUVector() const
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
void print() const
vpRotationMatrix getRotationMatrix() const
Implementation of a rotation vector as quaternion angle minimal representation.
vpQuaternionVector buildFrom(const double qx, const double qy, const double qz, const double qw)
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:116
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.