Visual Servoing Platform  version 3.0.1
testPoseVector.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
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 http://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  * Test some vpColVector functionalities.
32  *
33  * Authors:
34  * Eric Marchand
35  *
36  *****************************************************************************/
37 
44 #include <cmath>
45 #include <vector>
46 #include <limits>
47 
48 #include <visp3/core/vpPoseVector.h>
49 
50 bool test(const std::string &s, const vpArray2D<double> &A, const std::vector<double> &bench)
51 {
52  static unsigned int cpt = 0;
53  std::cout << "** Test " << ++cpt << std::endl;
54  std::cout << s << "(" << A.getRows() << "," << A.getCols() << ") =" << A << std::endl;
55  if(bench.size() != A.size()) {
56  std::cout << "Test fails: bad size wrt bench" << std::endl;
57  return false;
58  }
59  for (unsigned int i=0; i<A.size(); i++) {
60  if (std::fabs(A.data[i]-bench[i]) > std::fabs(A.data[i])*std::numeric_limits<double>::epsilon()) {
61  std::cout << "Test fails: bad content" << std::endl;
62  return false;
63  }
64  }
65 
66  return true;
67 }
68 int main()
69 {
70  {
71  vpPoseVector p;
72  std::vector<double> bench(6,0);
73  int err = 1;
74  if (test("p", p, bench) == false)
75  return err;
76  p[0] = bench[0] = 0.1;
77  p[1] = bench[1] = 0.2;
78  p[2] = bench[2] = 0.3;
79  p[3] = bench[3] = vpMath::rad(10);
80  p[4] = bench[4] = vpMath::rad(20);
81  p[5] = bench[5] = vpMath::rad(30);
82 
83  if (test("p", p, bench) == false)
84  return err;
85 
86  vpPoseVector p1(p[0], p[1], p[2], p[3], p[4], p[5]);
87  if (test("p1", p1, bench) == false)
88  return err;
89  vpPoseVector p2(p1);
90  if (test("p2", p2, bench) == false)
91  return err;
92  vpPoseVector p3 = p1;
93  if (test("p3", p3, bench) == false)
94  return err;
95  vpPoseVector p4;
96  p4.set(p[0], p[1], p[2], p[3], p[4], p[5]);
97  if (test("p4", p4, bench) == false)
98  return err;
99 
100  vpTranslationVector t(p[0], p[1], p[2]);
101  vpThetaUVector tu(p[3], p[4], p[5]);
102  vpPoseVector p5(t, tu);
103  if (test("p5", p5, bench) == false)
104  return err;
105  vpPoseVector p6;
106  p6.buildFrom(t, tu);
107  if (test("p6", p6, bench) == false)
108  return err;
109 
110  vpHomogeneousMatrix M(t, tu);
111  vpPoseVector p7(M);
112  if (test("p7", p7, bench) == false)
113  return err;
114  vpPoseVector p8;
115  p8.buildFrom(M);
116  if (test("p8", p8, bench) == false)
117  return err;
118 
119  vpRotationMatrix R(tu);
120  vpPoseVector p9(t, R);
121  if (test("p9", p9, bench) == false)
122  return err;
123  vpPoseVector p10;
124  p10.buildFrom(t, R);
125  if (test("p10", p10, bench) == false)
126  return err;
127  }
128  std::cout << "All tests succeed" << std::endl;
129  return 0;
130 }
Implementation of an homogeneous matrix and operations on such kind of matrices.
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:84
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:156
Implementation of a rotation matrix and operations on such kind of matrices.
unsigned int getCols() const
Return the number of columns of the 2D array.
Definition: vpArray2D.h:154
void set(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
static double rad(double deg)
Definition: vpMath.h:104
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:93
vpPoseVector buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.