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