Visual Servoing Platform  version 3.6.1 under development (2024-06-22)
vpPoseVector.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
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 https://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 *****************************************************************************/
35 
42 #include <assert.h>
43 #include <sstream>
44 
45 #include <visp3/core/vpException.h>
46 #include <visp3/core/vpMath.h>
47 #include <visp3/core/vpMatrixException.h>
48 #include <visp3/core/vpPoseVector.h>
49 
51 
63 
79 vpPoseVector::vpPoseVector(double tx, double ty, double tz, double tux, double tuy, double tuz)
80  : vpArray2D<double>(6, 1)
81 {
82  const unsigned int index_0 = 0;
83  const unsigned int index_1 = 1;
84  const unsigned int index_2 = 2;
85  const unsigned int index_3 = 3;
86  const unsigned int index_4 = 4;
87  const unsigned int index_5 = 5;
88  (*this)[index_0] = tx;
89  (*this)[index_1] = ty;
90  (*this)[index_2] = tz;
91 
92  (*this)[index_3] = tux;
93  (*this)[index_4] = tuy;
94  (*this)[index_5] = tuz;
95 }
96 
108 {
109  build(tv, tu);
110 }
111 
125 {
126  build(tv, R);
127 }
128 
140 
156 void vpPoseVector::set(double tx, double ty, double tz, double tux, double tuy, double tuz)
157 {
158  const unsigned int index_0 = 0;
159  const unsigned int index_1 = 1;
160  const unsigned int index_2 = 2;
161  const unsigned int index_3 = 3;
162  const unsigned int index_4 = 4;
163  const unsigned int index_5 = 5;
164  (*this)[index_0] = tx;
165  (*this)[index_1] = ty;
166  (*this)[index_2] = tz;
167 
168  (*this)[index_3] = tux;
169  (*this)[index_4] = tuy;
170  (*this)[index_5] = tuz;
171 }
172 
173 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
191 vpPoseVector vpPoseVector::buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
192 {
193  build(tx, ty, tz, tux, tuy, tuz);
194  return *this;
195 }
196 
210 {
211  build(M);
212  return *this;
213 }
214 
227 {
228  build(tv, tu);
229  return *this;
230 }
231 
246 {
247  build(tv, R);
248  return *this;
249 }
250 #endif
251 
268 vpPoseVector &vpPoseVector::build(const double &tx, const double &ty, const double &tz, const double &tux, const double &tuy, const double &tuz)
269 {
270  const unsigned int index_0 = 0;
271  const unsigned int index_1 = 1;
272  const unsigned int index_2 = 2;
273  const unsigned int index_3 = 3;
274  const unsigned int index_4 = 4;
275  const unsigned int index_5 = 5;
276  (*this)[index_0] = tx;
277  (*this)[index_1] = ty;
278  (*this)[index_2] = tz;
279 
280  (*this)[index_3] = tux;
281  (*this)[index_4] = tuy;
282  (*this)[index_5] = tuz;
283  return *this;
284 }
285 
298 {
300  M.extract(R);
302  M.extract(tv);
303  build(tv, R);
304  return *this;
305 }
306 
319 {
320  const unsigned int val_3 = 3;
321  for (unsigned int i = 0; i < val_3; ++i) {
322  (*this)[i] = tv[i];
323  (*this)[i + 3] = tu[i];
324  }
325  return *this;
326 }
327 
342 {
343  vpThetaUVector tu;
344  tu.build(R);
345 
346  build(tv, tu);
347  return *this;
348 }
349 
354 {
355  const unsigned int index_0 = 0;
356  const unsigned int index_1 = 1;
357  const unsigned int index_2 = 2;
358  tv[index_0] = (*this)[index_0];
359  tv[index_1] = (*this)[index_1];
360  tv[index_2] = (*this)[index_2];
361 }
362 
367 {
368  const unsigned int index_0 = 0;
369  const unsigned int index_1 = 1;
370  const unsigned int index_2 = 2;
371  const unsigned int index_3 = 3;
372  const unsigned int index_4 = 4;
373  const unsigned int index_5 = 5;
374  tu[index_0] = (*this)[index_3];
375  tu[index_1] = (*this)[index_4];
376  tu[index_2] = (*this)[index_5];
377 }
382 {
383  vpRotationMatrix R((*this)[3], (*this)[4], (*this)[5]);
384  q.build(R);
385 }
389 void vpPoseVector::extract(vpRotationMatrix &R) const { R.build((*this)[3], (*this)[4], (*this)[5]); }
395 {
396  const unsigned int index_0 = 0;
397  const unsigned int index_1 = 1;
398  const unsigned int index_2 = 2;
399  vpTranslationVector tr((*this)[index_0], (*this)[index_1], (*this)[index_2]);
400  return tr;
401 }
402 
408 {
409  vpRotationMatrix R((*this)[3], (*this)[4], (*this)[5]);
410  return R;
411 }
412 
418 {
419  vpThetaUVector tu((*this)[3], (*this)[4], (*this)[5]);
420  return tu;
421 }
422 
445 {
446  const unsigned int nparam = 6;
447  const unsigned int nparam_t = 3;
448  for (unsigned int i = 0; i < nparam; ++i) {
449  if (i < nparam_t) {
450  std::cout << (*this)[i] << " ";
451  }
452  else {
453  std::cout << vpMath::deg((*this)[i]) << " ";
454  }
455  }
456  std::cout << std::endl;
457 }
458 
470 void vpPoseVector::save(std::ofstream &f) const
471 {
472  if (!f.fail()) {
473  f << *this;
474  }
475  else {
476  throw(vpException(vpException::ioError, "Cannot save the pose vector: ofstream not opened"));
477  }
478 }
479 
490 void vpPoseVector::load(std::ifstream &f)
491 {
492  if (!f.fail()) {
493  const unsigned int nparam = 6;
494  for (unsigned int i = 0; i < nparam; ++i) {
495  f >> (*this)[i];
496  }
497  }
498  else {
499  throw(vpException(vpException::ioError, "Cannot read pose vector: ifstream not opened"));
500  }
501 }
502 
503 /*
504  Transpose the pose vector. The resulting vector becomes a row vector.
505 
506 */
508 {
509  vpRowVector v(rowNum);
510  memcpy(v.data, data, rowNum * sizeof(double));
511  return v;
512 }
513 
533 int vpPoseVector::print(std::ostream &s, unsigned int length, char const *intro) const
534 {
535  typedef std::string::size_type size_type;
536 
537  unsigned int m = getRows();
538  unsigned int n = 1;
539 
540  std::vector<std::string> values(m * n);
541  std::ostringstream oss;
542  std::ostringstream ossFixed;
543  std::ios_base::fmtflags original_flags = oss.flags();
544 
545  // --comment: ossFixed less less std fixed
546  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
547 
548  size_type maxBefore = 0; // the length of the integral part
549  size_type maxAfter = 0; // number of decimals plus
550  // one place for the decimal point
551  for (unsigned int i = 0; i < m; ++i) {
552  oss.str("");
553  oss << (*this)[i];
554  if (oss.str().find("e") != std::string::npos) {
555  ossFixed.str("");
556  ossFixed << (*this)[i];
557  oss.str(ossFixed.str());
558  }
559 
560  values[i] = oss.str();
561  size_type thislen = values[i].size();
562  size_type p = values[i].find('.');
563 
564  if (p == std::string::npos) {
565  maxBefore = vpMath::maximum(maxBefore, thislen);
566  // maxAfter remains the same
567  }
568  else {
569  maxBefore = vpMath::maximum(maxBefore, p);
570  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
571  }
572  }
573 
574  size_type totalLength = length;
575  // increase totalLength according to maxBefore
576  totalLength = vpMath::maximum(totalLength, maxBefore);
577  // decrease maxAfter according to totalLength
578  maxAfter = std::min<size_type>(maxAfter, totalLength - maxBefore);
579  if (maxAfter == 1) {
580  maxAfter = 0;
581  }
582 
583  // the following line is useful for debugging
584  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
585 
586  if (intro) {
587  s << intro;
588  }
589  s << "[" << m << "," << n << "]=\n";
590 
591  for (unsigned int i = 0; i < m; ++i) {
592  s << " ";
593  size_type p = values[i].find('.');
594  s.setf(std::ios::right, std::ios::adjustfield);
595  s.width(static_cast<std::streamsize>(maxBefore));
596  s << values[i].substr(0, p).c_str();
597 
598  if (maxAfter > 0) {
599  s.setf(std::ios::left, std::ios::adjustfield);
600  if (p != std::string::npos) {
601  s.width(static_cast<std::streamsize>(maxAfter));
602  s << values[i].substr(p, maxAfter).c_str();
603  }
604  else {
605  assert(maxAfter > 1);
606  s.width(static_cast<std::streamsize>(maxAfter));
607  s << ".0";
608  }
609  }
610 
611  s << ' ';
612 
613  s << std::endl;
614  }
615 
616  s.flags(original_flags); // restore s to standard state
617 
618  return static_cast<int>(maxBefore + maxAfter);
619 }
620 
625 std::vector<double> vpPoseVector::toStdVector() const
626 {
627  std::vector<double> v(this->size());
628 
629  unsigned int this_size = this->size();
630  for (unsigned int i = 0; i < this_size; ++i) {
631  v[i] = data[i];
632  }
633  return v;
634 }
635 
636 #ifdef VISP_HAVE_NLOHMANN_JSON
637 #include <visp3/core/vpJsonParsing.h>
638 const std::string vpPoseVector::jsonTypeName = "vpPoseVector";
639 void vpPoseVector::convert_to_json(nlohmann::json &j) const
640 {
641  const vpArray2D<double> *asArray = (vpArray2D<double>*) this;
642  to_json(j, *asArray);
643  j["type"] = vpPoseVector::jsonTypeName;
644 }
645 void vpPoseVector::parse_json(const nlohmann::json &j)
646 {
647 #ifdef ENABLE_VISP_NAMESPACE
648  using namespace VISP_NAMESPACE_NAME;
649 #endif
650  vpArray2D<double> *asArray = (vpArray2D<double>*) this;
651  if (j.is_object() && j.contains("type")) { // Specific conversions
652  const bool converted = convertFromTypeAndBuildFrom<vpPoseVector, vpHomogeneousMatrix>(j, *this);
653  if (!converted) {
654  from_json(j, *asArray);
655  }
656  }
657  else { // Generic 2D array conversion
658  from_json(j, *asArray);
659  }
660 
661  if ((getCols() != 1) && (getRows() != 6)) {
662  throw vpException(vpException::badValue, "From JSON, tried to read something that is not a 6D pose vector");
663  }
664 }
665 #endif
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:127
unsigned int getCols() const
Definition: vpArray2D.h:319
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:130
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:1080
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:331
unsigned int getRows() const
Definition: vpArray2D.h:329
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ ioError
I/O error.
Definition: vpException.h:67
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:73
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:254
static double deg(double rad)
Definition: vpMath.h:119
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:187
vpTranslationVector getTranslationVector() const
vpPoseVector & build(const double &tx, const double &ty, const double &tz, const double &tux, const double &tuy, const double &tuz)
std::vector< double > toStdVector() const
void load(std::ifstream &f)
void save(std::ofstream &f) const
friend void from_json(const nlohmann::json &j, vpPoseVector &cam)
Definition: vpPoseVector.h:334
vpRowVector t() const
friend void to_json(nlohmann::json &j, const vpPoseVector &cam)
Definition: vpPoseVector.h:329
void extract(vpRotationMatrix &R) const
void set(double tx, double ty, double tz, double tux, double tuy, double tuz)
vpThetaUVector getThetaUVector() const
static const std::string jsonTypeName
Definition: vpPoseVector.h:303
vp_deprecated 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 & build(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 & build(const vpHomogeneousMatrix &M)
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:110
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector & build(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.