Visual Servoing Platform  version 3.6.1 under development (2024-10-13)
vpTranslationVector.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  * Translation vector.
32  */
33 
39 #include <stdio.h>
40 #include <string.h>
41 
42 #include <visp3/core/vpTranslationVector.h>
43 
44 BEGIN_VISP_NAMESPACE
52 vpTranslationVector::vpTranslationVector(double tx, double ty, double tz) : vpArray2D<double>(3, 1), m_index(0)
53 {
54  const unsigned int index_0 = 0;
55  const unsigned int index_1 = 1;
56  const unsigned int index_2 = 2;
57  (*this)[index_0] = tx;
58  (*this)[index_1] = ty;
59  (*this)[index_2] = tz;
60 }
61 
70 {
71  M.extract(*this);
72 }
73 
82 {
83  const unsigned int index_0 = 0;
84  const unsigned int index_1 = 1;
85  const unsigned int index_2 = 2;
86  (*this)[index_0] = p[index_0];
87  (*this)[index_1] = p[index_1];
88  (*this)[index_2] = p[index_2];
89 }
90 
102 
118 {
119  if (v.size() != 3) {
121  "Cannot construct a translation vector from a "
122  "%d-dimension column vector",
123  v.size()));
124  }
125 }
126 
139 {
140  M.extract(*this);
141  return *this;
142 }
143 
154 {
155  const unsigned int index_0 = 0;
156  const unsigned int index_1 = 1;
157  const unsigned int index_2 = 2;
158  (*this)[index_0] = p[index_0];
159  (*this)[index_1] = p[index_1];
160  (*this)[index_2] = p[index_2];
161  return *this;
162 }
163 
174 {
175  const unsigned int index_0 = 0;
176  const unsigned int index_1 = 1;
177  const unsigned int index_2 = 2;
178  const unsigned int v_size = 3;
179  if (v.size() != v_size) {
181  "Cannot build a translation vector from a %d-dimension column vector", v.size()));
182  }
183 
184  (*this)[index_0] = v[index_0];
185  (*this)[index_1] = v[index_1];
186  (*this)[index_2] = v[index_2];
187  return *this;
188 }
189 
198 vpTranslationVector &vpTranslationVector::buildFrom(const double &tx, const double &ty, const double &tz)
199 {
200  set(tx, ty, tz);
201  return *this;
202 }
203 
210 void vpTranslationVector::set(double tx, double ty, double tz)
211 {
212  const unsigned int index_0 = 0;
213  const unsigned int index_1 = 1;
214  const unsigned int index_2 = 2;
215  (*this)[index_0] = tx;
216  (*this)[index_1] = ty;
217  (*this)[index_2] = tz;
218 }
219 
239 {
241 
242  const unsigned int val_3 = 3;
243  for (unsigned int i = 0; i < val_3; ++i) {
244  s[i] = (*this)[i] + tv[i];
245  }
246 
247  return s;
248 }
271 {
272  if (v.size() != 3) {
273  throw(vpException(vpException::dimensionError, "Cannot add translation vector to a %d-dimension column vector",
274  v.size()));
275  }
277 
278  const unsigned int val_3 = 3;
279  for (unsigned int i = 0; i < val_3; ++i) {
280  s[i] = (*this)[i] + v[i];
281  }
282 
283  return s;
284 }
285 
305 {
307 
308  const unsigned int val_3 = 3;
309  for (unsigned int i = 0; i < val_3; ++i) {
310  sub[i] = (*this)[i] - tv[i];
311  }
312 
313  return sub;
314 }
315 
331 {
333  for (unsigned int i = 0; i < dsize; ++i) {
334  *(tv.data + i) = -*(data + i);
335  }
336 
337  return tv;
338 }
339 
357 {
359  for (unsigned int i = 0; i < dsize; ++i) {
360  *(tv.data + i) = (*(data + i)) * x;
361  }
362 
363  return tv;
364 }
365 
376 {
377  vpMatrix M(rowNum, v.getCols());
378  unsigned int v_col = v.getCols();
379  for (unsigned int i = 0; i < rowNum; ++i) {
380  for (unsigned int j = 0; j < v_col; ++j) {
381  M[i][j] = (*this)[i] * v[j];
382  }
383  }
384  return M;
385 }
386 
396 {
397  for (unsigned int i = 0; i < rowNum; ++i) {
398  (*this)[i] *= x;
399  }
400  return *this;
401 }
411 {
412  for (unsigned int i = 0; i < rowNum; ++i) {
413  (*this)[i] /= x;
414  }
415  return *this;
416 }
417 
435 {
437  for (unsigned int i = 0; i < dsize; ++i) {
438  *(tv.data + i) = (*(data + i)) / x;
439  }
440 
441  return tv;
442 }
443 
461 {
462  if (tv.size() != 3) {
464  "Cannot initialize a translation vector from a "
465  "%d-dimension col vector",
466  tv.size()));
467  }
468  unsigned int k = tv.size();
469  if (rowNum != k) {
470  try {
471  resize(k, 1);
472  }
473  catch (...) {
474  throw;
475  }
476  }
477 
478  memcpy(data, tv.data, rowNum * sizeof(double));
479 
480  return *this;
481 }
496 {
497  unsigned int k = tv.rowNum;
498  if (rowNum != k) {
499  try {
500  resize(k, 1);
501  }
502  catch (...) {
503  throw;
504  }
505  }
506 
507  memcpy(data, tv.data, rowNum * sizeof(double));
508 
509  return *this;
510 }
511 
524 {
525  double *d = data;
526 
527  for (int i = 0; i < 3; ++i) {
528  *(d++) = x;
529  }
530 
531  return *this;
532 }
533 
534 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
556 vpTranslationVector &vpTranslationVector::operator=(const std::initializer_list<double> &list)
557 {
558  if (list.size() > size()) {
559  throw(vpException(
561  "Cannot set translation vector out of bounds. It has only %d values while you try to initialize with %d values",
562  size(), list.size()));
563  }
564  std::copy(list.begin(), list.end(), data);
565  return *this;
566 }
567 #endif
568 
597 {
598  m_index = 0;
599  data[m_index] = val;
600  return *this;
601 }
602 
631 {
632  ++m_index;
633  if (m_index >= size()) {
634  throw(vpException(
636  "Cannot set translation vector out of bounds. It has only %d values while you try to initialize with %d values",
637  size(), m_index + 1));
638  }
639  data[m_index] = val;
640  return *this;
641 }
642 
661 {
662  const unsigned int index_0 = 0;
663  const unsigned int index_1 = 1;
664  const unsigned int index_2 = 2;
665  M.resize(3, 3);
666  M[index_0][index_0] = 0;
667  M[index_0][index_1] = -tv[index_2];
668  M[index_0][index_2] = tv[index_1];
669  M[index_1][index_0] = tv[index_2];
670  M[index_1][index_1] = 0;
671  M[index_1][index_2] = -tv[index_0];
672  M[index_2][index_0] = -tv[index_1];
673  M[index_2][index_1] = tv[index_0];
674  M[index_2][index_2] = 0;
675 }
676 
696 {
697  vpMatrix M(3, 3);
698  skew(tv, M);
699  return M;
700 }
701 
721 {
722  vpMatrix M(3, 3);
723  skew(*this, M);
724  return M;
725 }
726 
737 {
739  return static_cast<vpTranslationVector>(skew_a * b);
740 }
741 
746 {
747  vpRowVector v(rowNum);
748  memcpy(v.data, data, rowNum * sizeof(double));
749  return v;
750 }
751 
752 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
753 
764 double vpTranslationVector::euclideanNorm() const { return frobeniusNorm(); }
765 
766 #endif
767 
774 {
775  double norm = sumSquare();
776 
777  return sqrt(norm);
778 }
779 
787 {
788  double sum_square = 0.0;
789 
790  for (unsigned int i = 0; i < rowNum; ++i) {
791  double x = rowPtrs[i][0];
792  sum_square += x * x;
793  }
794 
795  return sum_square;
796 }
797 
806 vpTranslationVector vpTranslationVector::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
807 {
808  vpColVector meanT(3);
809  size_t vec_m_size = vec_M.size();
810  for (size_t i = 0; i < vec_m_size; ++i) {
811  meanT += static_cast<vpColVector>(vec_M[i].getTranslationVector());
812  }
813  meanT /= static_cast<double>(vec_M.size());
814 
815  vpTranslationVector t(meanT);
816  return t;
817 }
818 
827 vpTranslationVector vpTranslationVector::mean(const std::vector<vpTranslationVector> &vec_t)
828 {
829  vpColVector meanT(3);
830  size_t l_vec_t_size = vec_t.size();
831  for (size_t i = 0; i < l_vec_t_size; ++i) {
832  meanT += static_cast<vpColVector>(vec_t[i]);
833  }
834  meanT /= static_cast<double>(vec_t.size());
835 
836  vpTranslationVector t(meanT);
837  return t;
838 }
839 END_VISP_NAMESPACE
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:145
unsigned int getCols() const
Definition: vpArray2D.h:337
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:148
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:1102
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:1098
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:1104
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
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:169
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:203
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:124
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)
vpTranslationVector & buildFrom(const double &tx, const double &ty, const double &tz)
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)
vpRowVector t() const
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true)