Visual Servoing Platform version 3.5.0
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
47TEST_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
97int 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
114int 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.