Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testMatrixConditionNumber.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 matrix condition number.
33 *
34*****************************************************************************/
35
41#include <iostream>
42#include <stdlib.h>
43#include <visp3/core/vpMatrix.h>
44
45int test_condition_number(const std::string &test_name, const vpMatrix &M)
46{
47 double precision = 1e-6;
48
49 bool is_square = (M.getCols() == M.getRows() ? true : false);
50 double inducedL2_norm_inv = 0, cond_inv = 0;
51
52 double inducedL2_norm = M.inducedL2Norm();
53 double inducedL2_norm_pinv = M.pseudoInverse().inducedL2Norm();
54 double cond = M.cond();
55 double cond_pinv = inducedL2_norm * inducedL2_norm_pinv;
56
57 vpMatrix kerMt;
58 double rank = M.kernel(kerMt);
59
60 if (is_square) {
61 inducedL2_norm_inv = M.inverseByLU().inducedL2Norm();
62 cond_inv = inducedL2_norm * inducedL2_norm_inv;
63 }
64
65 M.print(std::cout, 4, test_name);
66 std::cout << " Matrix rank: " << rank << std::endl;
67 std::cout << " Matrix induced L2 norm ||M||_L2: " << inducedL2_norm << std::endl;
68 if (is_square) {
69 std::cout << " Inverse induced L2 norm ||M^-1||_L2: " << inducedL2_norm_inv << std::endl;
70 }
71 std::cout << " Pseudo inverse induced L2 norm norm ||M^+||_L2: " << inducedL2_norm_pinv << std::endl;
72 if (is_square) {
73 std::cout << " Condition number such as cond(M)=||M||_L2 * ||M^-1||_L2: " << cond_inv << std::endl;
74 }
75 std::cout << " Condition number such as cond(M)=||M||_L2 * ||M^+||_L2: " << cond_pinv << std::endl;
76 std::cout << " Condition number cond(M): " << cond << std::endl;
77 if (!vpMath::equal(cond, cond_pinv, precision)) {
78 std::cout << " Condition number differ from the one computed with the pseudo inverse" << std::endl;
79 return EXIT_FAILURE;
80 }
81 if (is_square) {
82 if (!vpMath::equal(cond, cond_inv, precision)) {
83 std::cout << " Condition number differ from the one computed with the inverse" << std::endl;
84 return EXIT_FAILURE;
85 }
86 }
87
88 return EXIT_SUCCESS;
89}
90
91int main()
92{
93#if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV)
94 vpMatrix M(3, 3);
95 M.eye();
96
97 if (test_condition_number("* Test square matrix M", M)) {
98 std::cout << " - Condition number computation fails" << std::endl;
99 return EXIT_FAILURE;
100 } else {
101 std::cout << " + Condition number computation succeed" << std::endl;
102 }
103
104 M.resize(2, 3);
105 M[0][0] = 1;
106 M[0][1] = 2;
107 M[0][2] = 3;
108 M[1][0] = 4;
109 M[1][1] = 5;
110 M[1][2] = 6;
111
112 if (test_condition_number("* Test rect matrix M", M)) {
113 std::cout << " - Condition number computation fails" << std::endl;
114 return EXIT_FAILURE;
115 } else {
116 std::cout << " + Condition number computation succeed" << std::endl;
117 }
118
119 M = M.transpose();
120
121 if (test_condition_number("* Test rect matrix M", M)) {
122 std::cout << " - Condition number computation fails" << std::endl;
123 return EXIT_FAILURE;
124 } else {
125 std::cout << " + Condition number computation succeed" << std::endl;
126 }
127
128 M.resize(3, 4);
129 M[0][0] = 1;
130 M[0][1] = 2;
131 M[0][2] = 3;
132 M[0][3] = 0;
133 M[1][0] = 4;
134 M[1][1] = 5;
135 M[1][2] = 6;
136 M[1][3] = 0;
137 M[2][0] = 0;
138 M[2][1] = 0;
139 M[2][2] = 0;
140 M[2][3] = 0;
141
142 if (test_condition_number("* Test rect matrix M", M)) {
143 std::cout << " - Condition number computation fails" << std::endl;
144 return EXIT_FAILURE;
145 } else {
146 std::cout << " + Condition number computation succeed" << std::endl;
147 }
148
149 M = M.transpose();
150
151 if (test_condition_number("* Test rect matrix M", M)) {
152 std::cout << " - Condition number computation fails" << std::endl;
153 return EXIT_FAILURE;
154 } else {
155 std::cout << " + Condition number computation succeed" << std::endl;
156 }
157 std::cout << "Test succeed" << std::endl;
158#else
159 std::cout << "Test ignored: install Lapack, Eigen3 or OpenCV" << std::endl;
160#endif
161 return EXIT_SUCCESS;
162}
unsigned int getCols() const
Definition vpArray2D.h:280
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int getRows() const
Definition vpArray2D.h:290
static bool equal(double x, double y, double threshold=0.001)
Definition vpMath.h:369
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
double cond(double svThreshold=1e-6) const
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
void eye()
Definition vpMatrix.cpp:446
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
vpMatrix inverseByLU() const
double inducedL2Norm() const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
vpMatrix transpose() const
Definition vpMatrix.cpp:468