Visual Servoing Platform version 3.5.0
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
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 }
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}
unsigned int getCols() const
Definition: vpArray2D.h:279
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
unsigned int getRows() const
Definition: vpArray2D.h:289
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:295
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6622
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5598
void eye()
Definition: vpMatrix.cpp:449
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6264
vpMatrix inverseByLU() const
double inducedL2Norm() const
Definition: vpMatrix.cpp:6722
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
vpMatrix transpose() const
Definition: vpMatrix.cpp:474