Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpPoseVector.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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*****************************************************************************/
36
43#include <assert.h>
44#include <sstream>
45
46#include <visp3/core/vpDebug.h>
47#include <visp3/core/vpException.h>
48#include <visp3/core/vpMath.h>
49#include <visp3/core/vpMatrixException.h>
50#include <visp3/core/vpPoseVector.h>
51
63
79vpPoseVector::vpPoseVector(double tx, double ty, double tz, double tux, double tuy, double tuz)
80 : vpArray2D<double>(6, 1)
81{
82 (*this)[0] = tx;
83 (*this)[1] = ty;
84 (*this)[2] = tz;
85
86 (*this)[3] = tux;
87 (*this)[4] = tuy;
88 (*this)[5] = tuz;
89}
90
102{
103 buildFrom(tv, tu);
104}
105
119{
120 buildFrom(tv, R);
121}
122
134
150void vpPoseVector::set(double tx, double ty, double tz, double tux, double tuy, double tuz)
151{
152 (*this)[0] = tx;
153 (*this)[1] = ty;
154 (*this)[2] = tz;
155
156 (*this)[3] = tux;
157 (*this)[4] = tuy;
158 (*this)[5] = tuz;
159}
160
177vpPoseVector vpPoseVector::buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
178{
179 (*this)[0] = tx;
180 (*this)[1] = ty;
181 (*this)[2] = tz;
182
183 (*this)[3] = tux;
184 (*this)[4] = tuy;
185 (*this)[5] = tuz;
186 return *this;
187}
188
201{
203 M.extract(R);
205 M.extract(tv);
206 buildFrom(tv, R);
207 return *this;
208}
209
222{
223 for (unsigned int i = 0; i < 3; i++) {
224 (*this)[i] = tv[i];
225 (*this)[i + 3] = tu[i];
226 }
227 return *this;
228}
229
244{
246 tu.buildFrom(R);
247
248 buildFrom(tv, tu);
249 return *this;
250}
251
256{
257 tv[0] = (*this)[0];
258 tv[1] = (*this)[1];
259 tv[2] = (*this)[2];
260}
261
266{
267 tu[0] = (*this)[3];
268 tu[1] = (*this)[4];
269 tu[2] = (*this)[5];
270}
275{
276 vpRotationMatrix R((*this)[3], (*this)[4], (*this)[5]);
277 q.buildFrom(R);
278}
282void vpPoseVector::extract(vpRotationMatrix &R) const { R.buildFrom((*this)[3], (*this)[4], (*this)[5]); }
288{
289 vpTranslationVector tr((*this)[0], (*this)[1], (*this)[2]);
290 return tr;
291}
292
298{
299 vpRotationMatrix R((*this)[3], (*this)[4], (*this)[5]);
300 return R;
301}
302
308{
309 vpThetaUVector tu((*this)[3], (*this)[4], (*this)[5]);
310 return tu;
311}
312
335{
336 for (unsigned int i = 0; i < 6; i++)
337 if (i < 3)
338 std::cout << (*this)[i] << " ";
339 else
340 std::cout << vpMath::deg((*this)[i]) << " ";
341 std::cout << std::endl;
342}
343
355void vpPoseVector::save(std::ofstream &f) const
356{
357 if (!f.fail()) {
358 f << *this;
359 }
360 else {
361 throw(vpException(vpException::ioError, "Cannot save the pose vector: ofstream not opened"));
362 }
363}
364
375void vpPoseVector::load(std::ifstream &f)
376{
377 if (!f.fail()) {
378 for (unsigned int i = 0; i < 6; i++) {
379 f >> (*this)[i];
380 }
381 }
382 else {
383 throw(vpException(vpException::ioError, "Cannot read pose vector: ifstream not opened"));
384 }
385}
386
387/*
388 Transpose the pose vector. The resulting vector becomes a row vector.
389
390*/
392{
394 memcpy(v.data, data, rowNum * sizeof(double));
395 return v;
396}
397
417int vpPoseVector::print(std::ostream &s, unsigned int length, char const *intro) const
418{
419 typedef std::string::size_type size_type;
420
421 unsigned int m = getRows();
422 unsigned int n = 1;
423
424 std::vector<std::string> values(m * n);
425 std::ostringstream oss;
426 std::ostringstream ossFixed;
427 std::ios_base::fmtflags original_flags = oss.flags();
428
429 // ossFixed <<std::fixed;
430 ossFixed.setf(std::ios::fixed, std::ios::floatfield);
431
432 size_type maxBefore = 0; // the length of the integral part
433 size_type maxAfter = 0; // number of decimals plus
434 // one place for the decimal point
435 for (unsigned int i = 0; i < m; ++i) {
436 oss.str("");
437 oss << (*this)[i];
438 if (oss.str().find("e") != std::string::npos) {
439 ossFixed.str("");
440 ossFixed << (*this)[i];
441 oss.str(ossFixed.str());
442 }
443
444 values[i] = oss.str();
445 size_type thislen = values[i].size();
446 size_type p = values[i].find('.');
447
448 if (p == std::string::npos) {
449 maxBefore = vpMath::maximum(maxBefore, thislen);
450 // maxAfter remains the same
451 }
452 else {
453 maxBefore = vpMath::maximum(maxBefore, p);
454 maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
455 }
456 }
457
458 size_type totalLength = length;
459 // increase totalLength according to maxBefore
460 totalLength = vpMath::maximum(totalLength, maxBefore);
461 // decrease maxAfter according to totalLength
462 maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
463 if (maxAfter == 1)
464 maxAfter = 0;
465
466 // the following line is useful for debugging
467 // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
468
469 if (intro)
470 s << intro;
471 s << "[" << m << "," << n << "]=\n";
472
473 for (unsigned int i = 0; i < m; i++) {
474 s << " ";
475 size_type p = values[i].find('.');
476 s.setf(std::ios::right, std::ios::adjustfield);
477 s.width((std::streamsize)maxBefore);
478 s << values[i].substr(0, p).c_str();
479
480 if (maxAfter > 0) {
481 s.setf(std::ios::left, std::ios::adjustfield);
482 if (p != std::string::npos) {
483 s.width((std::streamsize)maxAfter);
484 s << values[i].substr(p, maxAfter).c_str();
485 }
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
507std::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}
515
516#ifdef VISP_HAVE_NLOHMANN_JSON
517#include <visp3/core/vpJsonParsing.h>
518const std::string vpPoseVector::jsonTypeName = "vpPoseVector";
519void vpPoseVector::convert_to_json(nlohmann::json &j) const
520{
521 const vpArray2D<double> *asArray = (vpArray2D<double>*) this;
522 to_json(j, *asArray);
523 j["type"] = vpPoseVector::jsonTypeName;
524}
525void vpPoseVector::parse_json(const nlohmann::json &j)
526{
527 vpArray2D<double> *asArray = (vpArray2D<double>*) this;
528 if (j.is_object() && j.contains("type")) { // Specific conversions
529 const bool converted = convertFromTypeAndBuildFrom<vpPoseVector, vpHomogeneousMatrix>(j, *this);
530 if (!converted) {
531 from_json(j, *asArray);
532 }
533 }
534 else { // Generic 2D array conversion
535 from_json(j, *asArray);
536 }
537
538 if (getCols() != 1 && getRows() != 6) {
539 throw vpException(vpException::badValue, "From JSON, tried to read something that is not a 6D pose vector");
540 }
541}
542#endif
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition vpArray2D.h:131
unsigned int getCols() const
Definition vpArray2D.h:280
Type * data
Address of the first element of the data array.
Definition vpArray2D.h:144
unsigned int rowNum
Definition vpArray2D.h:134
unsigned int size() const
Definition vpArray2D.h:292
unsigned int getRows() const
Definition vpArray2D.h:290
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ ioError
I/O error.
Definition vpException.h:79
@ badValue
Used to indicate that a value is not in the allowed range.
Definition vpException.h:85
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:172
static double deg(double rad)
Definition vpMath.h:106
Implementation of a pose vector and operations on poses.
vpTranslationVector getTranslationVector() const
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)
vpRowVector t() const
friend void to_json(nlohmann::json &j, const vpPoseVector &cam)
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
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.
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.