Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
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 
47 bool 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 
59 TEST_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 
118 TEST_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([]() {
134  vpRotationMatrix R {
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 
161 TEST_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 
202 TEST_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 
224 int 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 
241 int 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:449
static vpHomogeneousMatrix enu2ned(const vpHomogeneousMatrix &enu_M)
Definition: vpMath.cpp:737
Implementation of a rotation matrix and operations on such kind of matrices.