Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
53vpTranslationVector::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) {
115 throw(vpException(vpException::dimensionError,
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
187{
188 set(tx, ty, tz);
189 return *this;
190}
191
198void 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)
527vpTranslationVector &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
701
706{
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
766vpTranslationVector 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
786vpTranslationVector 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
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int rowNum
Definition vpArray2D.h:134
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< double > &A)
Definition vpArray2D.h:529
unsigned int dsize
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.
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.
Implementation of row vector and the associated operations.
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)
static vpTranslationVector cross(const vpTranslationVector &a, const vpTranslationVector &b)
vpTranslationVector buildFrom(double tx, double ty, double tz)
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true)