Visual Servoing Platform  version 3.4.1 under development (2021-10-17)
testHomogeneousMatrix.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2021 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 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 TEST_CASE("vpHomogeneousMatrix re-orthogonalize rotation matrix", "[vpHomogeneousMatrix]") {
48  CHECK_NOTHROW([](){
50  0.9835, -0.0581, 0.1716, 0.0072,
51  -0.0489, -0.9972, -0.0571, 0.0352,
52  0.1744, 0.0478, -0.9835, 0.9470
53  };
54  }());
55 
56  CHECK_NOTHROW([](){
58  0.9835, -0.0581, 0.1716, 0.0072,
59  -0.0937, -0.9738, 0.2072, 0.0481,
60  0.1551, -0.2199, -0.9631, 0.9583
61  };
62 
63  std::cout << "Original data:" << std::endl;
64  std::cout << "0.9835 -0.0581 0.1716 0.0072" << std::endl;
65  std::cout << " -0.0937 -0.9738 0.2072 0.0481" << std::endl;
66  std::cout << "0.1551 -0.2199 -0.9631 0.9583" << std::endl;
67  std::cout << "0 0 0 1" << std::endl;
68  std::cout << "M after rotation re-orthogonalization:\n" << M << std::endl;
69  }());
70 
71  CHECK_NOTHROW([](){
73  0.9835, -0.0581, 0.1716, 0.0072,
74  -0.0937, -0.9738, 0.2072, 0.0481,
75  0.1551, -0.2199, -0.9631, 0.9583
76  };
77 
78  // if M1 contains a proper rotation matrix
79  // following R init should not throw exception since re-orthogonalization
80  // is done only in vpHomogeneousMatrix, not in vpRotationMatrix
82  M1[0][0], M1[0][1], M1[0][2],
83  M1[1][0], M1[1][1], M1[1][2],
84  M1[2][0], M1[2][1], M1[2][2]
85  };
86  }());
87 
88  CHECK_THROWS([](){
90  0.983, -0.058, 0.171, 0.0072,
91  -0.093, -0.973, 0.207, 0.0481,
92  0.155, -0.219, -0.963, 0.9583
93  };
94  }());
95 }
96 
97 int main(int argc, char *argv[])
98 {
99  Catch::Session session; // There must be exactly one instance
100 
101  // Let Catch (using Clara) parse the command line
102  session.applyCommandLine(argc, argv);
103 
104  int numFailed = session.run();
105 
106  // numFailed is clamped to 255 as some unices only use the lower 8 bits.
107  // This clamping has already been applied, so just return it here
108  // You can also do any post run clean-up here
109  return numFailed;
110 }
111 #else
112 #include <iostream>
113 
114 int main()
115 {
116  return 0;
117 }
118 #endif
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a rotation matrix and operations on such kind of matrices.