Visual Servoing Platform  version 3.6.1 under development (2024-10-15)
testMatrixConditionNumber.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Test matrix condition number.
32  */
33 
39 #include <iostream>
40 #include <stdlib.h>
41 #include <visp3/core/vpMatrix.h>
42 
43 #ifdef ENABLE_VISP_NAMESPACE
44 using namespace VISP_NAMESPACE_NAME;
45 #endif
46 
47 int test_condition_number(const std::string &test_name, const vpMatrix &M)
48 {
49  double precision = 1e-6;
50 
51  bool is_square = (M.getCols() == M.getRows() ? true : false);
52  double inducedL2_norm_inv = 0, cond_inv = 0;
53 
54  double inducedL2_norm = M.inducedL2Norm();
55  double inducedL2_norm_pinv = M.pseudoInverse().inducedL2Norm();
56  double cond = M.cond();
57  double cond_pinv = inducedL2_norm * inducedL2_norm_pinv;
58 
59  vpMatrix kerMt;
60  double rank = M.kernel(kerMt);
61 
62  if (is_square) {
63  inducedL2_norm_inv = M.inverseByLU().inducedL2Norm();
64  cond_inv = inducedL2_norm * inducedL2_norm_inv;
65  }
66 
67  M.print(std::cout, 4, test_name);
68  std::cout << " Matrix rank: " << rank << std::endl;
69  std::cout << " Matrix induced L2 norm ||M||_L2: " << inducedL2_norm << std::endl;
70  if (is_square) {
71  std::cout << " Inverse induced L2 norm ||M^-1||_L2: " << inducedL2_norm_inv << std::endl;
72  }
73  std::cout << " Pseudo inverse induced L2 norm norm ||M^+||_L2: " << inducedL2_norm_pinv << std::endl;
74  if (is_square) {
75  std::cout << " Condition number such as cond(M)=||M||_L2 * ||M^-1||_L2: " << cond_inv << std::endl;
76  }
77  std::cout << " Condition number such as cond(M)=||M||_L2 * ||M^+||_L2: " << cond_pinv << std::endl;
78  std::cout << " Condition number cond(M): " << cond << std::endl;
79  if (!vpMath::equal(cond, cond_pinv, precision)) {
80  std::cout << " Condition number differ from the one computed with the pseudo inverse" << std::endl;
81  return EXIT_FAILURE;
82  }
83  if (is_square) {
84  if (!vpMath::equal(cond, cond_inv, precision)) {
85  std::cout << " Condition number differ from the one computed with the inverse" << std::endl;
86  return EXIT_FAILURE;
87  }
88  }
89 
90  return EXIT_SUCCESS;
91 }
92 
93 int main()
94 {
95 #if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV)
96  vpMatrix M(3, 3);
97  M.eye();
98 
99  if (test_condition_number("* Test square matrix M", M)) {
100  std::cout << " - Condition number computation fails" << std::endl;
101  return EXIT_FAILURE;
102  }
103  else {
104  std::cout << " + Condition number computation succeed" << std::endl;
105  }
106 
107  M.resize(2, 3);
108  M[0][0] = 1;
109  M[0][1] = 2;
110  M[0][2] = 3;
111  M[1][0] = 4;
112  M[1][1] = 5;
113  M[1][2] = 6;
114 
115  if (test_condition_number("* Test rect matrix M", M)) {
116  std::cout << " - Condition number computation fails" << std::endl;
117  return EXIT_FAILURE;
118  }
119  else {
120  std::cout << " + Condition number computation succeed" << std::endl;
121  }
122 
123  M = M.transpose();
124 
125  if (test_condition_number("* Test rect matrix M", M)) {
126  std::cout << " - Condition number computation fails" << std::endl;
127  return EXIT_FAILURE;
128  }
129  else {
130  std::cout << " + Condition number computation succeed" << std::endl;
131  }
132 
133  M.resize(3, 4);
134  M[0][0] = 1;
135  M[0][1] = 2;
136  M[0][2] = 3;
137  M[0][3] = 0;
138  M[1][0] = 4;
139  M[1][1] = 5;
140  M[1][2] = 6;
141  M[1][3] = 0;
142  M[2][0] = 0;
143  M[2][1] = 0;
144  M[2][2] = 0;
145  M[2][3] = 0;
146 
147  if (test_condition_number("* Test rect matrix M", M)) {
148  std::cout << " - Condition number computation fails" << std::endl;
149  return EXIT_FAILURE;
150  }
151  else {
152  std::cout << " + Condition number computation succeed" << std::endl;
153  }
154 
155  M = M.transpose();
156 
157  if (test_condition_number("* Test rect matrix M", M)) {
158  std::cout << " - Condition number computation fails" << std::endl;
159  return EXIT_FAILURE;
160  }
161  else {
162  std::cout << " + Condition number computation succeed" << std::endl;
163  }
164  std::cout << "Test succeed" << std::endl;
165 #else
166  std::cout << "Test ignored: install Lapack, Eigen3 or OpenCV" << std::endl;
167 #endif
168  return EXIT_SUCCESS;
169 }
unsigned int getCols() const
Definition: vpArray2D.h:337
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
unsigned int getRows() const
Definition: vpArray2D.h:347
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:459
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:1810
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:824
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:1416
vpMatrix inverseByLU() const
double inducedL2Norm() const
Definition: vpMatrix.cpp:1913
vpMatrix pseudoInverse(double svThreshold=1e-6) const
vpMatrix transpose() const