Visual Servoing Platform  version 3.6.1 under development (2024-07-18)
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 
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 
127 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
141 {
142  build(M);
143  return *this;
144 }
145 
157 {
158  build(p);
159  return *this;
160 }
161 
172 {
173  build(v);
174  return *this;
175 }
176 
186 vpTranslationVector vpTranslationVector::buildFrom(double tx, double ty, double tz)
187 {
188  build(tx, ty, tz);
189  return *this;
190 }
191 #endif
192 
205 {
206  M.extract(*this);
207  return *this;
208 }
209 
220 {
221  const unsigned int index_0 = 0;
222  const unsigned int index_1 = 1;
223  const unsigned int index_2 = 2;
224  (*this)[index_0] = p[index_0];
225  (*this)[index_1] = p[index_1];
226  (*this)[index_2] = p[index_2];
227  return *this;
228 }
229 
240 {
241  const unsigned int index_0 = 0;
242  const unsigned int index_1 = 1;
243  const unsigned int index_2 = 2;
244  const unsigned int v_size = 3;
245  if (v.size() != v_size) {
247  "Cannot build a translation vector from a %d-dimension column vector", v.size()));
248  }
249 
250  (*this)[index_0] = v[index_0];
251  (*this)[index_1] = v[index_1];
252  (*this)[index_2] = v[index_2];
253  return *this;
254 }
255 
264 vpTranslationVector &vpTranslationVector::build(const double &tx, const double &ty, const double &tz)
265 {
266  set(tx, ty, tz);
267  return *this;
268 }
269 
276 void vpTranslationVector::set(double tx, double ty, double tz)
277 {
278  const unsigned int index_0 = 0;
279  const unsigned int index_1 = 1;
280  const unsigned int index_2 = 2;
281  (*this)[index_0] = tx;
282  (*this)[index_1] = ty;
283  (*this)[index_2] = tz;
284 }
285 
305 {
307 
308  const unsigned int val_3 = 3;
309  for (unsigned int i = 0; i < val_3; ++i) {
310  s[i] = (*this)[i] + tv[i];
311  }
312 
313  return s;
314 }
337 {
338  if (v.size() != 3) {
339  throw(vpException(vpException::dimensionError, "Cannot add translation vector to a %d-dimension column vector",
340  v.size()));
341  }
343 
344  const unsigned int val_3 = 3;
345  for (unsigned int i = 0; i < val_3; ++i) {
346  s[i] = (*this)[i] + v[i];
347  }
348 
349  return s;
350 }
351 
371 {
373 
374  const unsigned int val_3 = 3;
375  for (unsigned int i = 0; i < val_3; ++i) {
376  sub[i] = (*this)[i] - tv[i];
377  }
378 
379  return sub;
380 }
381 
397 {
399  for (unsigned int i = 0; i < dsize; ++i) {
400  *(tv.data + i) = -*(data + i);
401  }
402 
403  return tv;
404 }
405 
423 {
425  for (unsigned int i = 0; i < dsize; ++i) {
426  *(tv.data + i) = (*(data + i)) * x;
427  }
428 
429  return tv;
430 }
431 
442 {
443  vpMatrix M(rowNum, v.getCols());
444  unsigned int v_col = v.getCols();
445  for (unsigned int i = 0; i < rowNum; ++i) {
446  for (unsigned int j = 0; j < v_col; ++j) {
447  M[i][j] = (*this)[i] * v[j];
448  }
449  }
450  return M;
451 }
452 
462 {
463  for (unsigned int i = 0; i < rowNum; ++i) {
464  (*this)[i] *= x;
465  }
466  return *this;
467 }
477 {
478  for (unsigned int i = 0; i < rowNum; ++i) {
479  (*this)[i] /= x;
480  }
481  return *this;
482 }
483 
501 {
503  for (unsigned int i = 0; i < dsize; ++i) {
504  *(tv.data + i) = (*(data + i)) / x;
505  }
506 
507  return tv;
508 }
509 
527 {
528  if (tv.size() != 3) {
530  "Cannot initialize a translation vector from a "
531  "%d-dimension col vector",
532  tv.size()));
533  }
534  unsigned int k = tv.size();
535  if (rowNum != k) {
536  try {
537  resize(k, 1);
538  }
539  catch (...) {
540  throw;
541  }
542  }
543 
544  memcpy(data, tv.data, rowNum * sizeof(double));
545 
546  return *this;
547 }
562 {
563  unsigned int k = tv.rowNum;
564  if (rowNum != k) {
565  try {
566  resize(k, 1);
567  }
568  catch (...) {
569  throw;
570  }
571  }
572 
573  memcpy(data, tv.data, rowNum * sizeof(double));
574 
575  return *this;
576 }
577 
590 {
591  double *d = data;
592 
593  for (int i = 0; i < 3; ++i) {
594  *(d++) = x;
595  }
596 
597  return *this;
598 }
599 
600 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
622 vpTranslationVector &vpTranslationVector::operator=(const std::initializer_list<double> &list)
623 {
624  if (list.size() > size()) {
625  throw(vpException(
627  "Cannot set translation vector out of bounds. It has only %d values while you try to initialize with %d values",
628  size(), list.size()));
629  }
630  std::copy(list.begin(), list.end(), data);
631  return *this;
632 }
633 #endif
634 
663 {
664  m_index = 0;
665  data[m_index] = val;
666  return *this;
667 }
668 
697 {
698  ++m_index;
699  if (m_index >= size()) {
700  throw(vpException(
702  "Cannot set translation vector out of bounds. It has only %d values while you try to initialize with %d values",
703  size(), m_index + 1));
704  }
705  data[m_index] = val;
706  return *this;
707 }
708 
727 {
728  const unsigned int index_0 = 0;
729  const unsigned int index_1 = 1;
730  const unsigned int index_2 = 2;
731  M.resize(3, 3);
732  M[index_0][index_0] = 0;
733  M[index_0][index_1] = -tv[index_2];
734  M[index_0][index_2] = tv[index_1];
735  M[index_1][index_0] = tv[index_2];
736  M[index_1][index_1] = 0;
737  M[index_1][index_2] = -tv[index_0];
738  M[index_2][index_0] = -tv[index_1];
739  M[index_2][index_1] = tv[index_0];
740  M[index_2][index_2] = 0;
741 }
742 
762 {
763  vpMatrix M(3, 3);
764  skew(tv, M);
765  return M;
766 }
767 
787 {
788  vpMatrix M(3, 3);
789  skew(*this, M);
790  return M;
791 }
792 
803 {
805  return static_cast<vpTranslationVector>(skew_a * b);
806 }
807 
812 {
813  vpRowVector v(rowNum);
814  memcpy(v.data, data, rowNum * sizeof(double));
815  return v;
816 }
817 
818 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
819 
831 
832 #endif
833 
840 {
841  double norm = sumSquare();
842 
843  return sqrt(norm);
844 }
845 
853 {
854  double sum_square = 0.0;
855 
856  for (unsigned int i = 0; i < rowNum; ++i) {
857  double x = rowPtrs[i][0];
858  sum_square += x * x;
859  }
860 
861  return sum_square;
862 }
863 
872 vpTranslationVector vpTranslationVector::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
873 {
874  vpColVector meanT(3);
875  size_t vec_m_size = vec_M.size();
876  for (size_t i = 0; i < vec_m_size; ++i) {
877  meanT += static_cast<vpColVector>(vec_M[i].getTranslationVector());
878  }
879  meanT /= static_cast<double>(vec_M.size());
880 
881  vpTranslationVector t(meanT);
882  return t;
883 }
884 
893 vpTranslationVector vpTranslationVector::mean(const std::vector<vpTranslationVector> &vec_t)
894 {
895  vpColVector meanT(3);
896  size_t l_vec_t_size = vec_t.size();
897  for (size_t i = 0; i < l_vec_t_size; ++i) {
898  meanT += static_cast<vpColVector>(vec_t[i]);
899  }
900  meanT /= static_cast<double>(vec_t.size());
901 
902  vpTranslationVector t(meanT);
903  return t;
904 }
905 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 & build(const double &tx, const double &ty, const 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)
VP_DEPRECATED vpTranslationVector buildFrom(double tx, double ty, double tz)
vpRowVector t() const
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true)