Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
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 
45 int 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 
91 int 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:274
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:299
unsigned int getRows() const
Definition: vpArray2D.h:284
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:449
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6346
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5324
void eye()
Definition: vpMatrix.cpp:442
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:5987
vpMatrix inverseByLU() const
double inducedL2Norm() const
Definition: vpMatrix.cpp:6446
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2287
vpMatrix transpose() const
Definition: vpMatrix.cpp:464