Visual Servoing Platform  version 3.4.1 under development (2021-10-17)
testMatrixConditionNumber.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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 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  }
101  else {
102  std::cout << " + Condition number computation succeed" << std::endl;
103  }
104 
105  M.resize(2, 3);
106  M[0][0] = 1; M[0][1] = 2; M[0][2] = 3;
107  M[1][0] = 4; M[1][1] = 5; M[1][2] = 6;
108 
109  if (test_condition_number("* Test rect matrix M", M)) {
110  std::cout << " - Condition number computation fails" << std::endl;
111  return EXIT_FAILURE;
112  }
113  else {
114  std::cout << " + Condition number computation succeed" << std::endl;
115  }
116 
117  M = M.transpose();
118 
119  if (test_condition_number("* Test rect matrix M", M)) {
120  std::cout << " - Condition number computation fails" << std::endl;
121  return EXIT_FAILURE;
122  }
123  else {
124  std::cout << " + Condition number computation succeed" << std::endl;
125  }
126 
127  M.resize(3, 4);
128  M[0][0] = 1; M[0][1] = 2; M[0][2] = 3; M[0][3] = 0;
129  M[1][0] = 4; M[1][1] = 5; M[1][2] = 6; M[1][3] = 0;
130  M[2][0] = 0; M[2][1] = 0; M[2][2] = 0; M[2][3] = 0;
131 
132  if (test_condition_number("* Test rect matrix M", M)) {
133  std::cout << " - Condition number computation fails" << std::endl;
134  return EXIT_FAILURE;
135  }
136  else {
137  std::cout << " + Condition number computation succeed" << std::endl;
138  }
139 
140  M = M.transpose();
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  }
146  else {
147  std::cout << " + Condition number computation succeed" << std::endl;
148  }
149  std::cout << "Test succeed" << std::endl;
150 #else
151  std::cout << "Test ignored: install Lapack, Eigen3 or OpenCV" << std::endl;
152 #endif
153  return EXIT_SUCCESS;
154 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:293
vpMatrix inverseByLU() const
unsigned int getRows() const
Definition: vpArray2D.h:289
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6623
unsigned int getCols() const
Definition: vpArray2D.h:279
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6265
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5598
vpMatrix transpose() const
Definition: vpMatrix.cpp:474
double inducedL2Norm() const
Definition: vpMatrix.cpp:6723
void eye()
Definition: vpMatrix.cpp:449