Visual Servoing Platform version 3.5.0
vpTranslationVector.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 * Translation vector.
33 *
34 * Authors:
35 * Eric Marchand
36 * Fabien Spindler
37 *
38 *****************************************************************************/
39
40#include <stdio.h>
41#include <string.h>
42
43#include <visp3/core/vpTranslationVector.h>
44
57vpTranslationVector::vpTranslationVector(double tx, double ty, double tz) : vpArray2D<double>(3, 1), m_index(0)
58{
59 (*this)[0] = tx;
60 (*this)[1] = ty;
61 (*this)[2] = tz;
62}
63
71vpTranslationVector::vpTranslationVector(const vpHomogeneousMatrix &M) : vpArray2D<double>(3, 1), m_index(0) { M.extract(*this); }
72
81{
82 (*this)[0] = p[0];
83 (*this)[1] = p[1];
84 (*this)[2] = p[2];
85}
86
98
114{
115 if (v.size() != 3) {
117 "Cannot construct a translation vector from a "
118 "%d-dimension column vector",
119 v.size()));
120 }
121}
122
135{
136 M.extract(*this);
137 return *this;
138}
139
150{
151 (*this)[0] = p[0];
152 (*this)[1] = p[1];
153 (*this)[2] = p[2];
154 return *this;
155}
156
167{
168 if (v.size() != 3) {
170 "Cannot build a translation vector from a %d-dimension column vector", v.size()));
171 }
172
173 (*this)[0] = v[0];
174 (*this)[1] = v[1];
175 (*this)[2] = v[2];
176 return *this;
177}
178
188{
189 set(tx, ty, tz);
190 return *this;
191}
192
199void vpTranslationVector::set(double tx, double ty, double tz)
200{
201 (*this)[0] = tx;
202 (*this)[1] = ty;
203 (*this)[2] = tz;
204}
205
225{
227
228 for (unsigned int i = 0; i < 3; i++)
229 s[i] = (*this)[i] + tv[i];
230
231 return s;
232}
255{
256 if (v.size() != 3) {
257 throw(vpException(vpException::dimensionError, "Cannot add translation vector to a %d-dimension column vector",
258 v.size()));
259 }
261
262 for (unsigned int i = 0; i < 3; i++)
263 s[i] = (*this)[i] + v[i];
264
265 return s;
266}
267
287{
289
290 for (unsigned int i = 0; i < 3; i++)
291 sub[i] = (*this)[i] - tv[i];
292
293 return sub;
294}
295
311{
313 for (unsigned int i = 0; i < dsize; i++) {
314 *(tv.data + i) = -*(data + i);
315 }
316
317 return tv;
318}
319
337{
339 for (unsigned int i = 0; i < dsize; i++) {
340 *(tv.data + i) = (*(data + i)) * x;
341 }
342
343 return tv;
344}
345
356{
357 vpMatrix M(rowNum, v.getCols());
358 for (unsigned int i = 0; i < rowNum; i++) {
359 for (unsigned int j = 0; j < v.getCols(); j++) {
360 M[i][j] = (*this)[i] * v[j];
361 }
362 }
363 return M;
364}
365
375{
376 for (unsigned int i = 0; i < rowNum; i++)
377 (*this)[i] *= x;
378 return (*this);
379}
389{
390 for (unsigned int i = 0; i < rowNum; i++)
391 (*this)[i] /= x;
392 return (*this);
393}
394
412{
414 for (unsigned int i = 0; i < dsize; i++) {
415 *(tv.data + i) = (*(data + i)) / x;
416 }
417
418 return tv;
419}
420
438{
439 if (tv.size() != 3) {
441 "Cannot initialize a translation vector from a "
442 "%d-dimension col vector",
443 tv.size()));
444 }
445 unsigned int k = tv.size();
446 if (rowNum != k) {
447 try {
448 resize(k, 1);
449 } catch (...) {
450 throw;
451 }
452 }
453
454 memcpy(data, tv.data, rowNum * sizeof(double));
455
456 return *this;
457}
472{
473 unsigned int k = tv.rowNum;
474 if (rowNum != k) {
475 try {
476 resize(k, 1);
477 } catch (...) {
478 throw;
479 }
480 }
481
482 memcpy(data, tv.data, rowNum * sizeof(double));
483
484 return *this;
485}
486
499{
500 double *d = data;
501
502 for (int i = 0; i < 3; i++)
503 *(d++) = x;
504
505 return *this;
506}
507
508#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
526vpTranslationVector &vpTranslationVector::operator=(const std::initializer_list<double> &list)
527{
528 if (list.size() > size()) {
529 throw(vpException(vpException::dimensionError, "Cannot set translation vector out of bounds. It has only %d values while you try to initialize with %d values", size(), list.size()));
530 }
531 std::copy(list.begin(), list.end(), data);
532 return *this;
533}
534#endif
535
560{
561 m_index = 0;
562 data[m_index] = val;
563 return *this;
564}
565
590{
591 m_index ++;
592 if (m_index >= size()) {
593 throw(vpException(vpException::dimensionError, "Cannot set translation vector out of bounds. It has only %d values while you try to initialize with %d values", size(), m_index+1));
594 }
595 data[m_index] = val;
596 return *this;
597}
598
617{
618 M.resize(3, 3);
619 M[0][0] = 0;
620 M[0][1] = -tv[2];
621 M[0][2] = tv[1];
622 M[1][0] = tv[2];
623 M[1][1] = 0;
624 M[1][2] = -tv[0];
625 M[2][0] = -tv[1];
626 M[2][1] = tv[0];
627 M[2][2] = 0;
628}
629
649{
650 vpMatrix M(3, 3);
651 skew(tv, M);
652 return M;
653}
654
674{
675 vpMatrix M(3, 3);
676 skew(*this, M);
677 return M;
678}
679
690{
692 return (vpTranslationVector)(skew_a * b);
693}
694
699{
701 memcpy(v.data, data, rowNum * sizeof(double));
702 return v;
703}
704
716{
717 return frobeniusNorm();
718}
719
726{
727 double norm = sumSquare();
728
729 return sqrt(norm);
730}
731
739{
740 double sum_square = 0.0;
741
742 for (unsigned int i = 0; i < rowNum; i++) {
743 double x = rowPtrs[i][0];
744 sum_square += x * x;
745 }
746
747 return sum_square;
748}
749
758vpTranslationVector vpTranslationVector::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
759{
760 vpColVector meanT(3);
761 for (size_t i = 0; i < vec_M.size(); i++) {
762 meanT += (vpColVector) vec_M[i].getTranslationVector();
763 }
764 meanT /= static_cast<double>(vec_M.size());
765
766 vpTranslationVector t(meanT);
767 return t;
768}
769
778vpTranslationVector vpTranslationVector::mean(const std::vector<vpTranslationVector> &vec_t)
779{
780 vpColVector meanT(3);
781 for (size_t i = 0; i < vec_t.size(); i++) {
782 meanT += (vpColVector) vec_t[i];
783 }
784 meanT /= static_cast<double>(vec_t.size());
785
786 vpTranslationVector t(meanT);
787 return t;
788}
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:132
unsigned int getCols() const
Definition: vpArray2D.h:279
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:139
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:141
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:116
Class that consider the case of a translation vector.
void set(double tx, double ty, double tz)
vpTranslationVector & operator=(const vpColVector &tv)
static vpTranslationVector mean(const std::vector< vpHomogeneousMatrix > &vec_M)
vp_deprecated double euclideanNorm() const
vpTranslationVector operator/(double x) const
vpTranslationVector & operator/=(double x)
vpTranslationVector & operator*=(double x)
vpMatrix operator*(const vpRowVector &v) const
vpTranslationVector operator-() const
vpTranslationVector operator+(const vpTranslationVector &tv) const
vpTranslationVector & operator<<(double val)
vpTranslationVector & operator,(double val)
static vpTranslationVector cross(const vpTranslationVector &a, const vpTranslationVector &b)
vpTranslationVector buildFrom(double tx, double ty, double tz)
vpRowVector t() const
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true)