Visual Servoing Platform  version 3.5.0
testPoseVector.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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 http://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  * Test some vpColVector functionalities.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
44 #include <visp3/core/vpConfig.h>
45 
46 #ifdef VISP_HAVE_CATCH2
47 #include <limits>
48 #include <vector>
49 
50 #include <visp3/core/vpPoseVector.h>
51 
52 #define CATCH_CONFIG_RUNNER
53 #include <catch.hpp>
54 
55 namespace {
56  void checkSize(const vpPoseVector& pose, const std::vector<double>& ref)
57  {
58  REQUIRE(pose.size() == 6);
59  REQUIRE(pose.getRows() == 6);
60  REQUIRE(pose.getCols() == 1);
61  REQUIRE(pose.size() == ref.size());
62  }
63 
64  void checkData(const vpPoseVector& pose, const std::vector<double>& ref)
65  {
66  for (unsigned int i = 0; i < pose.size(); i++) {
67  REQUIRE(pose[i] == Approx(ref[i]).epsilon(std::numeric_limits<double>::epsilon()));
68  }
69  }
70 }
71 
72 TEST_CASE("vpPoseVector size", "[vpColVector]") {
73  vpPoseVector pose;
74  REQUIRE(pose.size() == 6);
75  REQUIRE(pose.getRows() == 6);
76  REQUIRE(pose.getCols() == 1);
77 
78  for (unsigned int i = 0; i < pose.getRows(); i++) {
79  REQUIRE(pose[i] == Approx(0).epsilon(std::numeric_limits<double>::epsilon()));
80  }
81 }
82 
83 TEST_CASE("vpPoseVector value assignment", "[vpColVector]") {
84  vpPoseVector pose;
85  std::vector<double> ref(6);
86  pose[0] = ref[0] = 0.1;
87  pose[1] = ref[1] = 0.2;
88  pose[2] = ref[2] = 0.3;
89  pose[3] = ref[3] = vpMath::rad(10);
90  pose[4] = ref[4] = vpMath::rad(20);
91  pose[5] = ref[5] = vpMath::rad(30);
92 
93  checkSize(pose, ref);
94  checkData(pose, ref);
95 }
96 
97 TEST_CASE("vpPoseVector constructor", "[vpColVector]") {
98  std::vector<double> ref(6);
99  ref[0] = 0.1;
100  ref[1] = 0.2;
101  ref[2] = 0.3;
102  ref[3] = vpMath::rad(10);
103  ref[4] = vpMath::rad(20);
104  ref[5] = vpMath::rad(30);
105 
106  vpPoseVector pose(ref[0], ref[1], ref[2], ref[3], ref[4], ref[5]);
107 
108  checkSize(pose, ref);
109  checkData(pose, ref);
110 }
111 
112 TEST_CASE("vpPoseVector copy constructor", "[vpColVector]") {
113  std::vector<double> ref = { 0.1, 0.2, 0.3,
114  vpMath::rad(10), vpMath::rad(20), vpMath::rad(30) };
115 
116  vpPoseVector pose1(ref[0], ref[1], ref[2], ref[3], ref[4], ref[5]);
117  vpPoseVector pose2(pose1);
118 
119  checkSize(pose2, ref);
120  checkData(pose2, ref);
121 }
122 
123 TEST_CASE("vpPoseVector object assignment", "[vpColVector]") {
124  std::vector<double> ref = { 0.1, 0.2, 0.3,
125  vpMath::rad(10), vpMath::rad(20), vpMath::rad(30) };
126 
127  vpPoseVector pose1(ref[0], ref[1], ref[2], ref[3], ref[4], ref[5]);
128  vpPoseVector pose2 = pose1;
129 
130  checkSize(pose2, ref);
131  checkData(pose2, ref);
132 }
133 
134 TEST_CASE("vpPoseVector set", "[vpColVector]") {
135  std::vector<double> ref = { 0.1, 0.2, 0.3,
136  vpMath::rad(10), vpMath::rad(20), vpMath::rad(30) };
137 
138  vpPoseVector pose1(ref[0], ref[1], ref[2], ref[3], ref[4], ref[5]);
139  vpPoseVector pose2;
140  pose2.set(pose1[0], pose1[1], pose1[2], pose1[3], pose1[4], pose1[5]);
141 
142  checkSize(pose2, ref);
143  checkData(pose2, ref);
144 }
145 
146 TEST_CASE("vpPoseVector constructor t, tu", "[vpColVector]") {
147  std::vector<double> ref = { 0.1, 0.2, 0.3,
148  vpMath::rad(10), vpMath::rad(20), vpMath::rad(30) };
149  vpTranslationVector t(ref[0], ref[1], ref[2]);
150  vpThetaUVector tu(ref[3], ref[4], ref[5]);
151 
152  vpPoseVector pose(t, tu);
153 
154  checkSize(pose, ref);
155  checkData(pose, ref);
156 }
157 
158 TEST_CASE("vpPoseVector buildFrom t, tu", "[vpColVector]") {
159  std::vector<double> ref = { 0.1, 0.2, 0.3,
160  vpMath::rad(10), vpMath::rad(20), vpMath::rad(30) };
161  vpTranslationVector t(ref[0], ref[1], ref[2]);
162  vpThetaUVector tu(ref[3], ref[4], ref[5]);
163 
164  vpPoseVector pose;
165  pose.buildFrom(t, tu);
166 
167  checkSize(pose, ref);
168  checkData(pose, ref);
169 }
170 
171 TEST_CASE("vpPoseVector constructor vpHomogeneousMatrix", "[vpColVector]") {
172  std::vector<double> ref = { 0.1, 0.2, 0.3,
173  vpMath::rad(10), vpMath::rad(20), vpMath::rad(30) };
174  vpTranslationVector t(ref[0], ref[1], ref[2]);
175  vpThetaUVector tu(ref[3], ref[4], ref[5]);
176  vpHomogeneousMatrix M(t, tu);
177 
178  vpPoseVector pose(M);
179 
180  checkSize(pose, ref);
181  checkData(pose, ref);
182 }
183 
184 TEST_CASE("vpPoseVector buildFrom vpHomogeneousMatrix", "[vpColVector]") {
185  std::vector<double> ref = { 0.1, 0.2, 0.3,
186  vpMath::rad(10), vpMath::rad(20), vpMath::rad(30) };
187  vpTranslationVector t(ref[0], ref[1], ref[2]);
188  vpThetaUVector tu(ref[3], ref[4], ref[5]);
189  vpHomogeneousMatrix M(t, tu);
190 
191  vpPoseVector pose;
192  pose.buildFrom(M);
193 
194  checkSize(pose, ref);
195  checkData(pose, ref);
196 }
197 
198 TEST_CASE("vpPoseVector constructor t, R", "[vpColVector]") {
199  std::vector<double> ref = { 0.1, 0.2, 0.3,
200  vpMath::rad(10), vpMath::rad(20), vpMath::rad(30) };
201  vpTranslationVector t(ref[0], ref[1], ref[2]);
202  vpThetaUVector tu(ref[3], ref[4], ref[5]);
203  vpRotationMatrix R(tu);
204 
205  vpPoseVector pose(t, R);
206 
207  checkSize(pose, ref);
208  checkData(pose, ref);
209 }
210 
211 TEST_CASE("vpPoseVector buildFrom t, R", "[vpColVector]") {
212  std::vector<double> ref = { 0.1, 0.2, 0.3,
213  vpMath::rad(10), vpMath::rad(20), vpMath::rad(30) };
214  vpTranslationVector t(ref[0], ref[1], ref[2]);
215  vpThetaUVector tu(ref[3], ref[4], ref[5]);
216  vpRotationMatrix R(tu);
217 
218  vpPoseVector pose;
219  pose.buildFrom(t, R);
220 
221  checkSize(pose, ref);
222  checkData(pose, ref);
223 }
224 
225 int main(int argc, char *argv[])
226 {
227  Catch::Session session; // There must be exactly one instance
228 
229  // Let Catch (using Clara) parse the command line
230  session.applyCommandLine(argc, argv);
231 
232  int numFailed = session.run();
233 
234  // numFailed is clamped to 255 as some unices only use the lower 8 bits.
235  // This clamping has already been applied, so just return it here
236  // You can also do any post run clean-up here
237  return numFailed;
238 }
239 #else
240 #include <iostream>
241 
242 int main()
243 {
244  return 0;
245 }
246 #endif
unsigned int getCols() const
Definition: vpArray2D.h:279
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double rad(double deg)
Definition: vpMath.h:110
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
void set(double tx, double ty, double tz, double tux, double tuy, double tuz)
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.