Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testHomogeneousMatrix.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 vpHomogeneousMatrix functionalities.
33 *
34*****************************************************************************/
35
39#include <visp3/core/vpConfig.h>
40
41#ifdef VISP_HAVE_CATCH2
42#include <visp3/core/vpHomogeneousMatrix.h>
43
44#define CATCH_CONFIG_RUNNER
45#include <catch.hpp>
46
47bool test_matrix_equal(const vpHomogeneousMatrix &M1, const vpHomogeneousMatrix &M2, double epsilon = 1e-10)
48{
49 for (unsigned int i = 0; i < 4; i++) {
50 for (unsigned int j = 0; j < 4; j++) {
51 if (!vpMath::equal(M1[i][j], M2[i][j], epsilon)) {
52 return false;
53 }
54 }
55 }
56 return true;
57}
58
59TEST_CASE("vpHomogeneousMatrix re-orthogonalize rotation matrix", "[vpHomogeneousMatrix]")
60{
61 CHECK_NOTHROW([]() {
62 vpHomogeneousMatrix M { 0.9835, -0.0581, 0.1716, 0.0072, -0.0489, -0.9972,
63 -0.0571, 0.0352, 0.1744, 0.0478, -0.9835, 0.9470 };
64 }());
65
66 SECTION("check re-orthogonalize rotation part")
67 {
68 vpHomogeneousMatrix M1 { 0.9835, -0.0581, 0.1716, 0.0072, -0.0489, -0.9972,
69 -0.0571, 0.0352, 0.1744, 0.0478, -0.9835, 0.9470 };
70
72 M2[0][0] = 0.9835;
73 M2[0][1] = -0.0581;
74 M2[0][2] = 0.1716;
75 M2[0][3] = 0.0072;
76 M2[1][0] = -0.0489;
77 M2[1][1] = -0.9972;
78 M2[1][2] = -0.0571;
79 M2[1][3] = 0.0352;
80 M2[2][0] = 0.1744;
81 M2[2][1] = 0.0478;
82 M2[2][2] = -0.9835;
83 M2[2][3] = 0.9470;
85
86 for (unsigned int i = 0; i < 4; i++) {
87 for (unsigned int j = 0; j < 4; j++) {
88 CHECK(M1[i][j] == Approx(M2[i][j]).margin(std::numeric_limits<double>::epsilon()));
89 }
90 }
91 }
92
93 CHECK_NOTHROW([]() {
94 vpHomogeneousMatrix M { 0.9835, -0.0581, 0.1716, 0.0072, -0.0937, -0.9738,
95 0.2072, 0.0481, 0.1551, -0.2199, -0.9631, 0.9583 };
96
97 std::cout << "Original data:" << std::endl;
98 std::cout << "0.9835 -0.0581 0.1716 0.0072" << std::endl;
99 std::cout << " -0.0937 -0.9738 0.2072 0.0481" << std::endl;
100 std::cout << "0.1551 -0.2199 -0.9631 0.9583" << std::endl;
101 std::cout << "0 0 0 1" << std::endl;
102 std::cout << "M after rotation re-orthogonalization:\n" << M << std::endl;
103 }());
104
105 CHECK_NOTHROW([]() {
106 vpHomogeneousMatrix M1 { 0.9835, -0.0581, 0.1716, 0.0072, -0.0937, -0.9738,
107 0.2072, 0.0481, 0.1551, -0.2199, -0.9631, 0.9583 };
108
109 // following R init should not throw an exception
110 vpRotationMatrix R { M1[0][0], M1[0][1], M1[0][2], M1[1][0], M1[1][1], M1[1][2], M1[2][0], M1[2][1], M1[2][2] };
111 }());
112
113 CHECK_THROWS([]() {
114 vpHomogeneousMatrix M { 0.983, -0.058, 0.171, 0.0072, -0.093, -0.973, 0.207, 0.0481, 0.155, -0.219, -0.963, 0.9583 };
115 }());
116}
117
118TEST_CASE("vpRotationMatrix re-orthogonalize rotation matrix", "[vpRotationMatrix]")
119{
120 CHECK_NOTHROW(
121 []() { vpRotationMatrix R { 0.9835, -0.0581, 0.1716, -0.0489, -0.9972, -0.0571, 0.1744, 0.0478, -0.9835 }; }());
122
123 CHECK_NOTHROW([]() {
124 vpRotationMatrix R { 0.9835, -0.0581, 0.1716, -0.0937, -0.9738, 0.2072, 0.1551, -0.2199, -0.9631 };
125
126 std::cout << "Original data:" << std::endl;
127 std::cout << "0.9835 -0.0581 0.1716" << std::endl;
128 std::cout << " -0.0937 -0.9738 0.2072" << std::endl;
129 std::cout << "0.1551 -0.2199 -0.9631" << std::endl;
130 std::cout << "R after rotation re-orthogonalization:\n" << R << std::endl;
131 }());
132
133 CHECK_NOTHROW([]() {
135 0.46682, -0.74434, 0.47754, -0.83228, -0.55233, -0.04733, 0.29899, -0.37535, -0.87734,
136 };
137
138 std::cout << "Original data:" << std::endl;
139 std::cout << "0.46682, -0.74434, 0.47754" << std::endl;
140 std::cout << "-0.83228, -0.55233, -0.04733" << std::endl;
141 std::cout << "0.29899, -0.37535, -0.87734" << std::endl;
142 std::cout << "R after rotation re-orthogonalization:\n" << R << std::endl;
143 }());
144
145 CHECK_NOTHROW([]() {
147 R = {
148 0.46682, -0.74434, 0.47754, -0.83228, -0.55233, -0.04733, 0.29899, -0.37535, -0.87734,
149 };
150
151 std::cout << "Original data:" << std::endl;
152 std::cout << "0.46682, -0.74434, 0.47754" << std::endl;
153 std::cout << "-0.83228, -0.55233, -0.04733" << std::endl;
154 std::cout << "0.29899, -0.37535, -0.87734" << std::endl;
155 std::cout << "R after rotation re-orthogonalization:\n" << R << std::endl;
156 }());
157
158 CHECK_THROWS([]() { vpRotationMatrix R { 0.983, -0.058, 0.171, -0.093, -0.973, 0.207, 0.155, -0.219, -0.963 }; }());
159}
160
161TEST_CASE("ENU to NED conversion", "[enu2ned]")
162{
163 vpHomogeneousMatrix enu_M_flu { 0, -1, 0, 0.2, 1, 0, 0, 1., 0, 0, 1, 0.3 };
164 std::cout << "enu_M_flu:\n" << enu_M_flu << std::endl;
165
166 vpHomogeneousMatrix enu_M_ned { 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, -1, 0 };
167 std::cout << "enu_M_ned:\n" << enu_M_ned << std::endl;
168
169 vpHomogeneousMatrix flu_M_frd { 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0 };
170 std::cout << "flu_M_frd:\n" << flu_M_frd << std::endl;
171
172 vpHomogeneousMatrix enu_M_frd = enu_M_flu * flu_M_frd;
173
174 // Test1
175 {
176 vpHomogeneousMatrix ned_M_frd = enu_M_ned.inverse() * enu_M_flu * flu_M_frd;
177 std::cout << "ned_M_frd:\n" << ned_M_frd << std::endl;
178
179 vpHomogeneousMatrix ned_M_frd_est = vpMath::enu2ned(enu_M_frd);
180 std::cout << "ned_M_frd_est:\n" << ned_M_frd_est << std::endl;
181
182 bool success = test_matrix_equal(ned_M_frd, ned_M_frd_est);
183 std::cout << "Test enu2ned 1 " << (success ? "succeed" : "failed") << std::endl;
184
185 CHECK(success);
186 }
187 // Test2
188 {
189 vpHomogeneousMatrix ned_M_flu = enu_M_ned.inverse() * enu_M_flu;
190 std::cout << "ned_M_flu:\n" << ned_M_flu << std::endl;
191
192 vpHomogeneousMatrix ned_M_flu_est = vpMath::enu2ned(enu_M_flu);
193 std::cout << "ned_M_flu_est:\n" << ned_M_flu_est << std::endl;
194
195 bool success = test_matrix_equal(ned_M_flu, ned_M_flu_est);
196 std::cout << "Test enu2ned 2 " << (success ? "succeed" : "failed") << std::endl;
197
198 CHECK(success);
199 }
200}
201
202TEST_CASE("vpHomogenousMatrix * vpRotationMatrix", "[operator*]")
203{
204 // Test rotation_matrix * homogeneous_matrix
205 vpHomogeneousMatrix _1_M_2_ {
206 0.9835, -0.0581, 0.1716, 0.0072,
207 -0.0489, -0.9972, -0.0571, 0.0352,
208 0.1744, 0.0478, -0.9835, 0.9470
209 };
210 vpHomogeneousMatrix _2_M_3_truth {
211 0.9835, -0.0581, 0.1716, 0,
212 -0.0489, -0.9972, -0.0571, 0,
213 0.1744, 0.0478, -0.9835, 0
214 };
215
216 vpRotationMatrix _2_R_3_ = _2_M_3_truth.getRotationMatrix();
217 vpHomogeneousMatrix _1_M_3_(_1_M_2_* _2_R_3_);
218 vpHomogeneousMatrix _1_M_3_truth(_1_M_2_ * _2_M_3_truth);
219 bool success = test_matrix_equal(_1_M_3_, _1_M_3_truth);
220 std::cout << "Test vpHomogeneousMatrix vpHomogeneousMatrix::operator*(vpRotationMatrix) " << (success ? "succeed" : "failed") << std::endl;
221 CHECK(success);
222}
223
224int main(int argc, char *argv[])
225{
226 Catch::Session session; // There must be exactly one instance
227
228 // Let Catch (using Clara) parse the command line
229 session.applyCommandLine(argc, argv);
230
231 int numFailed = session.run();
232
233 // numFailed is clamped to 255 as some unices only use the lower 8 bits.
234 // This clamping has already been applied, so just return it here
235 // You can also do any post run clean-up here
236 return numFailed;
237}
238#else
239#include <iostream>
240
241int main() { return EXIT_SUCCESS; }
242#endif
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static bool equal(double x, double y, double threshold=0.001)
Definition vpMath.h:369
static vpHomogeneousMatrix enu2ned(const vpHomogeneousMatrix &enu_M)
Definition vpMath.cpp:736
Implementation of a rotation matrix and operations on such kind of matrices.