Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
52namespace
53{
54void 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
62void 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
70TEST_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
82TEST_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
97TEST_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
113TEST_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
124TEST_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
135TEST_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
147TEST_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
159TEST_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
172TEST_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
185TEST_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
199TEST_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
212TEST_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
226int 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
243int main() { return EXIT_SUCCESS; }
244#endif
unsigned int getCols() const
Definition vpArray2D.h:280
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
unsigned int getRows() const
Definition vpArray2D.h:290
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double rad(double deg)
Definition vpMath.h:116
Implementation of a pose vector and operations on poses.
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.