Visual Servoing Platform  version 3.6.1 under development (2025-01-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 
44 BEGIN_VISP_NAMESPACE
45 const unsigned int vpTranslationVector::constr_val_3 = 3;
53 vpTranslationVector::vpTranslationVector(double tx, double ty, double tz) : vpArray2D<double>(constr_val_3, 1), m_index(0)
54 {
55  const unsigned int index_0 = 0;
56  const unsigned int index_1 = 1;
57  const unsigned int index_2 = 2;
58  (*this)[index_0] = tx;
59  (*this)[index_1] = ty;
60  (*this)[index_2] = tz;
61 }
62 
70 vpTranslationVector::vpTranslationVector(const vpHomogeneousMatrix &M) : vpArray2D<double>(constr_val_3, 1), m_index(0)
71 {
72  M.extract(*this);
73 }
74 
82 vpTranslationVector::vpTranslationVector(const vpPoseVector &p) : vpArray2D<double>(constr_val_3, 1), m_index(0)
83 {
84  const unsigned int index_0 = 0;
85  const unsigned int index_1 = 1;
86  const unsigned int index_2 = 2;
87  (*this)[index_0] = p[index_0];
88  (*this)[index_1] = p[index_1];
89  (*this)[index_2] = p[index_2];
90 }
91 
103 
119 {
120  const unsigned int val_3 = 3;
121  if (v.size() != val_3) {
123  "Cannot construct a translation vector from a "
124  "%d-dimension column vector",
125  v.size()));
126  }
127 }
128 
141 {
142  M.extract(*this);
143  return *this;
144 }
145 
156 {
157  const unsigned int index_0 = 0;
158  const unsigned int index_1 = 1;
159  const unsigned int index_2 = 2;
160  (*this)[index_0] = p[index_0];
161  (*this)[index_1] = p[index_1];
162  (*this)[index_2] = p[index_2];
163  return *this;
164 }
165 
176 {
177  const unsigned int index_0 = 0;
178  const unsigned int index_1 = 1;
179  const unsigned int index_2 = 2;
180  const unsigned int v_size = 3;
181  if (v.size() != v_size) {
183  "Cannot build a translation vector from a %d-dimension column vector", v.size()));
184  }
185 
186  (*this)[index_0] = v[index_0];
187  (*this)[index_1] = v[index_1];
188  (*this)[index_2] = v[index_2];
189  return *this;
190 }
191 
200 vpTranslationVector &vpTranslationVector::buildFrom(const double &tx, const double &ty, const double &tz)
201 {
202  set(tx, ty, tz);
203  return *this;
204 }
205 
212 void vpTranslationVector::set(double tx, double ty, double tz)
213 {
214  const unsigned int index_0 = 0;
215  const unsigned int index_1 = 1;
216  const unsigned int index_2 = 2;
217  (*this)[index_0] = tx;
218  (*this)[index_1] = ty;
219  (*this)[index_2] = tz;
220 }
221 
241 {
243 
244  const unsigned int val_3 = 3;
245  for (unsigned int i = 0; i < val_3; ++i) {
246  s[i] = (*this)[i] + tv[i];
247  }
248 
249  return s;
250 }
273 {
274  const unsigned int val_3 = 3;
275  if (v.size() != val_3) {
276  throw(vpException(vpException::dimensionError, "Cannot add translation vector to a %d-dimension column vector",
277  v.size()));
278  }
280 
281  for (unsigned int i = 0; i < val_3; ++i) {
282  s[i] = (*this)[i] + v[i];
283  }
284 
285  return s;
286 }
287 
307 {
309 
310  const unsigned int val_3 = 3;
311  for (unsigned int i = 0; i < val_3; ++i) {
312  sub[i] = (*this)[i] - tv[i];
313  }
314 
315  return sub;
316 }
317 
333 {
335  for (unsigned int i = 0; i < dsize; ++i) {
336  *(tv.data + i) = -*(data + i);
337  }
338 
339  return tv;
340 }
341 
359 {
361  for (unsigned int i = 0; i < dsize; ++i) {
362  *(tv.data + i) = (*(data + i)) * x;
363  }
364 
365  return tv;
366 }
367 
378 {
379  vpMatrix M(rowNum, v.getCols());
380  unsigned int v_col = v.getCols();
381  for (unsigned int i = 0; i < rowNum; ++i) {
382  for (unsigned int j = 0; j < v_col; ++j) {
383  M[i][j] = (*this)[i] * v[j];
384  }
385  }
386  return M;
387 }
388 
398 {
399  for (unsigned int i = 0; i < rowNum; ++i) {
400  (*this)[i] *= x;
401  }
402  return *this;
403 }
413 {
414  for (unsigned int i = 0; i < rowNum; ++i) {
415  (*this)[i] /= x;
416  }
417  return *this;
418 }
419 
437 {
439  for (unsigned int i = 0; i < dsize; ++i) {
440  *(tv.data + i) = (*(data + i)) / x;
441  }
442 
443  return tv;
444 }
445 
463 {
464  const unsigned int val_3 = 3;
465  if (tv.size() != val_3) {
467  "Cannot initialize a translation vector from a "
468  "%d-dimension col vector",
469  tv.size()));
470  }
471  unsigned int k = tv.size();
472  if (rowNum != k) {
473  try {
474  resize(k, 1);
475  }
476  catch (...) {
477  throw;
478  }
479  }
480 
481  memcpy(data, tv.data, rowNum * sizeof(double));
482 
483  return *this;
484 }
499 {
500  unsigned int k = tv.rowNum;
501  if (rowNum != k) {
502  try {
503  resize(k, 1);
504  }
505  catch (...) {
506  throw;
507  }
508  }
509 
510  memcpy(data, tv.data, rowNum * sizeof(double));
511 
512  return *this;
513 }
514 
527 {
528  double *d = data;
529 
530  const int val_3 = 3;
531  for (int i = 0; i < val_3; ++i) {
532  *(d++) = x;
533  }
534 
535  return *this;
536 }
537 
538 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
560 vpTranslationVector &vpTranslationVector::operator=(const std::initializer_list<double> &list)
561 {
562  if (list.size() > size()) {
563  throw(vpException(
565  "Cannot set translation vector out of bounds. It has only %d values while you try to initialize with %d values",
566  size(), list.size()));
567  }
568  std::copy(list.begin(), list.end(), data);
569  return *this;
570 }
571 #endif
572 
601 {
602  m_index = 0;
603  data[m_index] = val;
604  return *this;
605 }
606 
635 {
636  ++m_index;
637  if (m_index >= size()) {
638  throw(vpException(
640  "Cannot set translation vector out of bounds. It has only %d values while you try to initialize with %d values",
641  size(), m_index + 1));
642  }
643  data[m_index] = val;
644  return *this;
645 }
646 
665 {
666  const unsigned int index_0 = 0;
667  const unsigned int index_1 = 1;
668  const unsigned int index_2 = 2;
669  const unsigned int val_3 = 3;
670  M.resize(val_3, val_3);
671  M[index_0][index_0] = 0;
672  M[index_0][index_1] = -tv[index_2];
673  M[index_0][index_2] = tv[index_1];
674  M[index_1][index_0] = tv[index_2];
675  M[index_1][index_1] = 0;
676  M[index_1][index_2] = -tv[index_0];
677  M[index_2][index_0] = -tv[index_1];
678  M[index_2][index_1] = tv[index_0];
679  M[index_2][index_2] = 0;
680 }
681 
701 {
702  vpMatrix M(3, 3);
703  skew(tv, M);
704  return M;
705 }
706 
726 {
727  vpMatrix M(3, 3);
728  skew(*this, M);
729  return M;
730 }
731 
742 {
744  return static_cast<vpTranslationVector>(skew_a * b);
745 }
746 
751 {
752  vpRowVector v(rowNum);
753  memcpy(v.data, data, rowNum * sizeof(double));
754  return v;
755 }
756 
757 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
758 
769 double vpTranslationVector::euclideanNorm() const { return frobeniusNorm(); }
770 
771 #endif
772 
779 {
780  double norm = sumSquare();
781 
782  return sqrt(norm);
783 }
784 
792 {
793  double sum_square = 0.0;
794 
795  for (unsigned int i = 0; i < rowNum; ++i) {
796  double x = rowPtrs[i][0];
797  sum_square += x * x;
798  }
799 
800  return sum_square;
801 }
802 
811 vpTranslationVector vpTranslationVector::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
812 {
813  vpColVector meanT(3);
814  size_t vec_m_size = vec_M.size();
815  for (size_t i = 0; i < vec_m_size; ++i) {
816  meanT += static_cast<vpColVector>(vec_M[i].getTranslationVector());
817  }
818  meanT /= static_cast<double>(vec_M.size());
819 
820  vpTranslationVector t(meanT);
821  return t;
822 }
823 
832 vpTranslationVector vpTranslationVector::mean(const std::vector<vpTranslationVector> &vec_t)
833 {
834  vpColVector meanT(3);
835  size_t l_vec_t_size = vec_t.size();
836  for (size_t i = 0; i < l_vec_t_size; ++i) {
837  meanT += static_cast<vpColVector>(vec_t[i]);
838  }
839  meanT /= static_cast<double>(vec_t.size());
840 
841  vpTranslationVector t(meanT);
842  return t;
843 }
844 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:1105
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:1101
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:1107
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)