Visual Servoing Platform  version 3.5.1 under development (2023-03-14)
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 
57 vpTranslationVector::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 
72 {
73  M.extract(*this);
74 }
75 
84 {
85  (*this)[0] = p[0];
86  (*this)[1] = p[1];
87  (*this)[2] = p[2];
88 }
89 
101 
117 {
118  if (v.size() != 3) {
120  "Cannot construct a translation vector from a "
121  "%d-dimension column vector",
122  v.size()));
123  }
124 }
125 
138 {
139  M.extract(*this);
140  return *this;
141 }
142 
153 {
154  (*this)[0] = p[0];
155  (*this)[1] = p[1];
156  (*this)[2] = p[2];
157  return *this;
158 }
159 
170 {
171  if (v.size() != 3) {
173  "Cannot build a translation vector from a %d-dimension column vector", v.size()));
174  }
175 
176  (*this)[0] = v[0];
177  (*this)[1] = v[1];
178  (*this)[2] = v[2];
179  return *this;
180 }
181 
190 vpTranslationVector vpTranslationVector::buildFrom(double tx, double ty, double tz)
191 {
192  set(tx, ty, tz);
193  return *this;
194 }
195 
202 void vpTranslationVector::set(double tx, double ty, double tz)
203 {
204  (*this)[0] = tx;
205  (*this)[1] = ty;
206  (*this)[2] = tz;
207 }
208 
228 {
230 
231  for (unsigned int i = 0; i < 3; i++)
232  s[i] = (*this)[i] + tv[i];
233 
234  return s;
235 }
258 {
259  if (v.size() != 3) {
260  throw(vpException(vpException::dimensionError, "Cannot add translation vector to a %d-dimension column vector",
261  v.size()));
262  }
264 
265  for (unsigned int i = 0; i < 3; i++)
266  s[i] = (*this)[i] + v[i];
267 
268  return s;
269 }
270 
290 {
292 
293  for (unsigned int i = 0; i < 3; i++)
294  sub[i] = (*this)[i] - tv[i];
295 
296  return sub;
297 }
298 
314 {
316  for (unsigned int i = 0; i < dsize; i++) {
317  *(tv.data + i) = -*(data + i);
318  }
319 
320  return tv;
321 }
322 
340 {
342  for (unsigned int i = 0; i < dsize; i++) {
343  *(tv.data + i) = (*(data + i)) * x;
344  }
345 
346  return tv;
347 }
348 
359 {
360  vpMatrix M(rowNum, v.getCols());
361  for (unsigned int i = 0; i < rowNum; i++) {
362  for (unsigned int j = 0; j < v.getCols(); j++) {
363  M[i][j] = (*this)[i] * v[j];
364  }
365  }
366  return M;
367 }
368 
378 {
379  for (unsigned int i = 0; i < rowNum; i++)
380  (*this)[i] *= x;
381  return (*this);
382 }
392 {
393  for (unsigned int i = 0; i < rowNum; i++)
394  (*this)[i] /= x;
395  return (*this);
396 }
397 
415 {
417  for (unsigned int i = 0; i < dsize; i++) {
418  *(tv.data + i) = (*(data + i)) / x;
419  }
420 
421  return tv;
422 }
423 
441 {
442  if (tv.size() != 3) {
444  "Cannot initialize a translation vector from a "
445  "%d-dimension col vector",
446  tv.size()));
447  }
448  unsigned int k = tv.size();
449  if (rowNum != k) {
450  try {
451  resize(k, 1);
452  } catch (...) {
453  throw;
454  }
455  }
456 
457  memcpy(data, tv.data, rowNum * sizeof(double));
458 
459  return *this;
460 }
475 {
476  unsigned int k = tv.rowNum;
477  if (rowNum != k) {
478  try {
479  resize(k, 1);
480  } catch (...) {
481  throw;
482  }
483  }
484 
485  memcpy(data, tv.data, rowNum * sizeof(double));
486 
487  return *this;
488 }
489 
502 {
503  double *d = data;
504 
505  for (int i = 0; i < 3; i++)
506  *(d++) = x;
507 
508  return *this;
509 }
510 
511 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
529 vpTranslationVector &vpTranslationVector::operator=(const std::initializer_list<double> &list)
530 {
531  if (list.size() > size()) {
532  throw(vpException(
534  "Cannot set translation vector out of bounds. It has only %d values while you try to initialize with %d values",
535  size(), list.size()));
536  }
537  std::copy(list.begin(), list.end(), data);
538  return *this;
539 }
540 #endif
541 
566 {
567  m_index = 0;
568  data[m_index] = val;
569  return *this;
570 }
571 
596 {
597  m_index++;
598  if (m_index >= size()) {
599  throw(vpException(
601  "Cannot set translation vector out of bounds. It has only %d values while you try to initialize with %d values",
602  size(), m_index + 1));
603  }
604  data[m_index] = val;
605  return *this;
606 }
607 
626 {
627  M.resize(3, 3);
628  M[0][0] = 0;
629  M[0][1] = -tv[2];
630  M[0][2] = tv[1];
631  M[1][0] = tv[2];
632  M[1][1] = 0;
633  M[1][2] = -tv[0];
634  M[2][0] = -tv[1];
635  M[2][1] = tv[0];
636  M[2][2] = 0;
637 }
638 
658 {
659  vpMatrix M(3, 3);
660  skew(tv, M);
661  return M;
662 }
663 
683 {
684  vpMatrix M(3, 3);
685  skew(*this, M);
686  return M;
687 }
688 
699 {
701  return (vpTranslationVector)(skew_a * b);
702 }
703 
708 {
709  vpRowVector v(rowNum);
710  memcpy(v.data, data, rowNum * sizeof(double));
711  return v;
712 }
713 
725 
732 {
733  double norm = sumSquare();
734 
735  return sqrt(norm);
736 }
737 
745 {
746  double sum_square = 0.0;
747 
748  for (unsigned int i = 0; i < rowNum; i++) {
749  double x = rowPtrs[i][0];
750  sum_square += x * x;
751  }
752 
753  return sum_square;
754 }
755 
764 vpTranslationVector vpTranslationVector::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
765 {
766  vpColVector meanT(3);
767  for (size_t i = 0; i < vec_M.size(); i++) {
768  meanT += (vpColVector)vec_M[i].getTranslationVector();
769  }
770  meanT /= static_cast<double>(vec_M.size());
771 
772  vpTranslationVector t(meanT);
773  return t;
774 }
775 
784 vpTranslationVector vpTranslationVector::mean(const std::vector<vpTranslationVector> &vec_t)
785 {
786  vpColVector meanT(3);
787  for (size_t i = 0; i < vec_t.size(); i++) {
788  meanT += (vpColVector)vec_t[i];
789  }
790  meanT /= static_cast<double>(vec_t.size());
791 
792  vpTranslationVector t(meanT);
793  return t;
794 }
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:129
unsigned int getCols() const
Definition: vpArray2D.h:278
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:142
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:136
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:303
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:132
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:138
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:290
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)