Visual Servoing Platform version 3.5.0
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
83vpPoseVector::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
154void 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
181vpPoseVector 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{
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}
286void 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
359void 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
378void 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{
396 memcpy(v.data, data, rowNum * sizeof(double));
397 return v;
398}
399
419int 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}
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:132
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
unsigned int getRows() const
Definition: vpArray2D.h:289
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:145
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
vpTranslationVector getTranslationVector() 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.