Visual Servoing Platform  version 3.6.0 under development (2023-09-25)
vpTranslationVector.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
36 #include <stdio.h>
37 #include <string.h>
38 
39 #include <visp3/core/vpTranslationVector.h>
40 
53 vpTranslationVector::vpTranslationVector(double tx, double ty, double tz) : vpArray2D<double>(3, 1), m_index(0)
54 {
55  (*this)[0] = tx;
56  (*this)[1] = ty;
57  (*this)[2] = tz;
58 }
59 
68 {
69  M.extract(*this);
70 }
71 
80 {
81  (*this)[0] = p[0];
82  (*this)[1] = p[1];
83  (*this)[2] = p[2];
84 }
85 
97 
113 {
114  if (v.size() != 3) {
116  "Cannot construct a translation vector from a "
117  "%d-dimension column vector",
118  v.size()));
119  }
120 }
121 
134 {
135  M.extract(*this);
136  return *this;
137 }
138 
149 {
150  (*this)[0] = p[0];
151  (*this)[1] = p[1];
152  (*this)[2] = p[2];
153  return *this;
154 }
155 
166 {
167  if (v.size() != 3) {
169  "Cannot build a translation vector from a %d-dimension column vector", v.size()));
170  }
171 
172  (*this)[0] = v[0];
173  (*this)[1] = v[1];
174  (*this)[2] = v[2];
175  return *this;
176 }
177 
186 vpTranslationVector vpTranslationVector::buildFrom(double tx, double ty, double tz)
187 {
188  set(tx, ty, tz);
189  return *this;
190 }
191 
198 void vpTranslationVector::set(double tx, double ty, double tz)
199 {
200  (*this)[0] = tx;
201  (*this)[1] = ty;
202  (*this)[2] = tz;
203 }
204 
224 {
226 
227  for (unsigned int i = 0; i < 3; i++)
228  s[i] = (*this)[i] + tv[i];
229 
230  return s;
231 }
254 {
255  if (v.size() != 3) {
256  throw(vpException(vpException::dimensionError, "Cannot add translation vector to a %d-dimension column vector",
257  v.size()));
258  }
260 
261  for (unsigned int i = 0; i < 3; i++)
262  s[i] = (*this)[i] + v[i];
263 
264  return s;
265 }
266 
286 {
288 
289  for (unsigned int i = 0; i < 3; i++)
290  sub[i] = (*this)[i] - tv[i];
291 
292  return sub;
293 }
294 
310 {
312  for (unsigned int i = 0; i < dsize; i++) {
313  *(tv.data + i) = -*(data + i);
314  }
315 
316  return tv;
317 }
318 
336 {
338  for (unsigned int i = 0; i < dsize; i++) {
339  *(tv.data + i) = (*(data + i)) * x;
340  }
341 
342  return tv;
343 }
344 
355 {
356  vpMatrix M(rowNum, v.getCols());
357  for (unsigned int i = 0; i < rowNum; i++) {
358  for (unsigned int j = 0; j < v.getCols(); j++) {
359  M[i][j] = (*this)[i] * v[j];
360  }
361  }
362  return M;
363 }
364 
374 {
375  for (unsigned int i = 0; i < rowNum; i++)
376  (*this)[i] *= x;
377  return (*this);
378 }
388 {
389  for (unsigned int i = 0; i < rowNum; i++)
390  (*this)[i] /= x;
391  return (*this);
392 }
393 
411 {
413  for (unsigned int i = 0; i < dsize; i++) {
414  *(tv.data + i) = (*(data + i)) / x;
415  }
416 
417  return tv;
418 }
419 
437 {
438  if (tv.size() != 3) {
440  "Cannot initialize a translation vector from a "
441  "%d-dimension col vector",
442  tv.size()));
443  }
444  unsigned int k = tv.size();
445  if (rowNum != k) {
446  try {
447  resize(k, 1);
448  }
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  }
478  catch (...) {
479  throw;
480  }
481  }
482 
483  memcpy(data, tv.data, rowNum * sizeof(double));
484 
485  return *this;
486 }
487 
500 {
501  double *d = data;
502 
503  for (int i = 0; i < 3; i++)
504  *(d++) = x;
505 
506  return *this;
507 }
508 
509 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
527 vpTranslationVector &vpTranslationVector::operator=(const std::initializer_list<double> &list)
528 {
529  if (list.size() > size()) {
530  throw(vpException(
532  "Cannot set translation vector out of bounds. It has only %d values while you try to initialize with %d values",
533  size(), list.size()));
534  }
535  std::copy(list.begin(), list.end(), data);
536  return *this;
537 }
538 #endif
539 
564 {
565  m_index = 0;
566  data[m_index] = val;
567  return *this;
568 }
569 
594 {
595  m_index++;
596  if (m_index >= size()) {
597  throw(vpException(
599  "Cannot set translation vector out of bounds. It has only %d values while you try to initialize with %d values",
600  size(), m_index + 1));
601  }
602  data[m_index] = val;
603  return *this;
604 }
605 
624 {
625  M.resize(3, 3);
626  M[0][0] = 0;
627  M[0][1] = -tv[2];
628  M[0][2] = tv[1];
629  M[1][0] = tv[2];
630  M[1][1] = 0;
631  M[1][2] = -tv[0];
632  M[2][0] = -tv[1];
633  M[2][1] = tv[0];
634  M[2][2] = 0;
635 }
636 
656 {
657  vpMatrix M(3, 3);
658  skew(tv, M);
659  return M;
660 }
661 
681 {
682  vpMatrix M(3, 3);
683  skew(*this, M);
684  return M;
685 }
686 
697 {
699  return (vpTranslationVector)(skew_a * b);
700 }
701 
706 {
707  vpRowVector v(rowNum);
708  memcpy(v.data, data, rowNum * sizeof(double));
709  return v;
710 }
711 
712 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
713 
725 
726 #endif
727 
734 {
735  double norm = sumSquare();
736 
737  return sqrt(norm);
738 }
739 
747 {
748  double sum_square = 0.0;
749 
750  for (unsigned int i = 0; i < rowNum; i++) {
751  double x = rowPtrs[i][0];
752  sum_square += x * x;
753  }
754 
755  return sum_square;
756 }
757 
766 vpTranslationVector vpTranslationVector::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
767 {
768  vpColVector meanT(3);
769  for (size_t i = 0; i < vec_M.size(); i++) {
770  meanT += (vpColVector)vec_M[i].getTranslationVector();
771  }
772  meanT /= static_cast<double>(vec_M.size());
773 
774  vpTranslationVector t(meanT);
775  return t;
776 }
777 
786 vpTranslationVector vpTranslationVector::mean(const std::vector<vpTranslationVector> &vec_t)
787 {
788  vpColVector meanT(3);
789  for (size_t i = 0; i < vec_t.size(); i++) {
790  meanT += (vpColVector)vec_t[i];
791  }
792  meanT /= static_cast<double>(vec_t.size());
793 
794  vpTranslationVector t(meanT);
795  return t;
796 }
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:131
unsigned int getCols() const
Definition: vpArray2D.h:280
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:144
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:138
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:305
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:134
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:140
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:292
Implementation of column vector and the associated operations.
Definition: vpColVector.h:167
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ dimensionError
Bad dimension.
Definition: vpException.h:83
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:152
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:192
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:111
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)