Visual Servoing Platform  version 3.4.1 under development (2021-10-17)
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
Implementation of an homogeneous matrix and operations on such kind of matrices.
void set(double tx, double ty, double tz, double tux, double tuy, double tuz)
unsigned int getRows() const
Definition: vpArray2D.h:289
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
Implementation of a rotation matrix and operations on such kind of matrices.
unsigned int getCols() const
Definition: vpArray2D.h:279
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
static double rad(double deg)
Definition: vpMath.h:110
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.