Visual Servoing Platform version 3.5.0
testMatrixPseudoInverse.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 various svd decompositions.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
44#include <algorithm>
45#include <stdio.h>
46#include <stdlib.h>
47#include <vector>
48
49#include <visp3/core/vpColVector.h>
50#include <visp3/core/vpMatrix.h>
51#include <visp3/core/vpTime.h>
52#include <visp3/io/vpParseArgv.h>
53
54// List of allowed command line options
55#define GETOPTARGS "cdn:i:pf:R:C:vh"
56
65void usage(const char *name, const char *badparam)
66{
67 fprintf(stdout, "\n\
68Test matrix pseudo-inverse.\n\
69Outputs a comparison of the results obtained by supported 3rd parties.\n\
70\n\
71SYNOPSIS\n\
72 %s [-n <number of matrices>] [-f <plot filename>]\n\
73 [-R <number of rows>] [-C <number of columns>]\n\
74 [-i <number of iterations>] [-p] [-h]\n", name);
75
76 fprintf(stdout, "\n\
77OPTIONS: Default\n\
78 -n <number of matrices> \n\
79 Number of matrices inverted during each test loop.\n\
80\n\
81 -i <number of iterations> \n\
82 Number of iterations of the test.\n\
83\n\
84 -f <plot filename> \n\
85 Set output path for plot output.\n\
86 The plot logs the times of \n\
87 the different inversion methods: \n\
88 QR,LU,Cholesky and Pseudo-inverse.\n\
89\n\
90 -R <number of rows>\n\
91 Number of rows of the automatically generated matrices \n\
92 we test on.\n\
93\n\
94 -C <number of columns>\n\
95 Number of colums of the automatically generated matrices \n\
96 we test on.\n\
97\n\
98 -p \n\
99 Plot into filename in the gnuplot format. \n\
100 If this option is used, tests results will be logged \n\
101 into a filename specified with -f.\n\
102\n\
103 -h\n\
104 Print the help.\n\n");
105
106 if (badparam) {
107 fprintf(stderr, "ERROR: \n");
108 fprintf(stderr, "\nBad parameter [%s]\n", badparam);
109 }
110}
111
119bool getOptions(int argc, const char **argv, unsigned int &nb_matrices, unsigned int &nb_iterations,
120 bool &use_plot_file, std::string &plotfile, unsigned int &nbrows, unsigned int &nbcols, bool &verbose)
121{
122 const char *optarg_;
123 int c;
124 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
125
126 switch (c) {
127 case 'h':
128 usage(argv[0], NULL);
129 return false;
130 break;
131 case 'n':
132 nb_matrices = (unsigned int)atoi(optarg_);
133 break;
134 case 'i':
135 nb_iterations = (unsigned int)atoi(optarg_);
136 break;
137 case 'f':
138 plotfile = optarg_;
139 use_plot_file = true;
140 break;
141 case 'p':
142 use_plot_file = true;
143 break;
144 case 'R':
145 nbrows = (unsigned int)atoi(optarg_);
146 break;
147 case 'C':
148 nbcols = (unsigned int)atoi(optarg_);
149 break;
150 case 'v':
151 verbose = true;
152 break;
153 // add default options -c -d
154 case 'c':
155 break;
156 case 'd':
157 break;
158 default:
159 usage(argv[0], optarg_);
160 return false;
161 break;
162 }
163 }
164
165 if ((c == 1) || (c == -1)) {
166 // standalone param or error
167 usage(argv[0], NULL);
168 std::cerr << "ERROR: " << std::endl;
169 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
170 return false;
171 }
172
173 return true;
174}
175
176vpMatrix make_random_matrix(unsigned int nbrows, unsigned int nbcols)
177{
178 vpMatrix A;
179 A.resize(nbrows, nbcols);
180
181 for (unsigned int i = 0; i < A.getRows(); i++) {
182 for (unsigned int j = 0; j < A.getCols(); j++) {
183 A[i][j] = (double)rand() / (double)RAND_MAX;
184 }
185 }
186
187 return A;
188}
189
190void create_bench_random_matrix(unsigned int nb_matrices, unsigned int nb_rows, unsigned int nb_cols, bool verbose,
191 std::vector<vpMatrix> &bench)
192{
193 if (verbose)
194 std::cout << "Create a bench of " << nb_matrices << " " << nb_rows << " by " << nb_cols << " matrices" << std::endl;
195 bench.clear();
196 for (unsigned int i = 0; i < nb_matrices; i++) {
197 vpMatrix M = make_random_matrix(nb_rows, nb_cols);
198 bench.push_back(M);
199 }
200}
201
202int test_pseudo_inverse(const std::vector<vpMatrix> &A, const std::vector<vpMatrix> &Api)
203{
204 double allowed_error = 1e-3;
205
206 for (unsigned int i = 0; i < A.size(); i++) {
207 double error = (A[i] * Api[i] * A[i] - A[i]).frobeniusNorm();
208 if (error > allowed_error) {
209 std::cout << "Bad pseudo-inverse [" << i << "]: euclidean norm: " << error << std::endl;
210 return EXIT_FAILURE;
211 }
212 }
213 return EXIT_SUCCESS;
214}
215
216int test_pseudo_inverse(const std::vector<vpMatrix> &A, const std::vector<vpMatrix> &Api,
217 const std::vector<vpColVector> &sv, const std::vector<vpMatrix> &imA,
218 const std::vector<vpMatrix> &imAt, const std::vector<vpMatrix> &kerAt)
219{
220 double allowed_error = 1e-3;
221 // test Api
222 for (unsigned int i = 0; i < A.size(); i++) {
223 double error = (A[i] * Api[i] * A[i] - A[i]).frobeniusNorm();
224 if (error > allowed_error) {
225 std::cout << "Bad pseudo-inverse [" << i << "]: euclidean norm: " << error << std::endl;
226 return EXIT_FAILURE;
227 }
228 }
229
230 // test kerA
231 for (unsigned int i = 0; i < kerAt.size(); i++) {
232 if (kerAt[i].size()) {
233 vpMatrix nullspace = A[i] * kerAt[i].t();
234 double error = nullspace.frobeniusNorm();
235
236 if (error > allowed_error) {
237 std::cout << "Bad kernel [" << i << "]: euclidean norm: " << error << std::endl;
238 return EXIT_FAILURE;
239 }
240 }
241 }
242
243 // test sv, imA, imAt, kerA
244 for (unsigned int i = 0; i < kerAt.size(); i++) {
245 unsigned int rank = imA[i].getCols();
246 vpMatrix U, S(rank, A[i].getCols()), Vt(A[i].getCols(), A[i].getCols());
247 U = imA[i];
248
249 for (unsigned int j = 0; j < rank; j++)
250 S[j][j] = sv[i][j];
251
252 Vt.insert(imAt[i].t(), 0, 0);
253 Vt.insert(kerAt[i], imAt[i].getCols(), 0);
254
255 double error = (U * S * Vt - A[i]).frobeniusNorm();
256
257 if (error > allowed_error) {
258 std::cout << "Bad imA, imAt, sv, kerAt [" << i << "]: euclidean norm: " << error << std::endl;
259 return EXIT_FAILURE;
260 }
261 }
262
263 return EXIT_SUCCESS;
264}
265
266int test_pseudo_inverse_default(bool verbose, const std::vector<vpMatrix> &bench, std::vector<double> &time)
267{
268 if (verbose)
269 std::cout << "Test pseudo-inverse using default 3rd party" << std::endl;
270 if (verbose)
271 std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl;
272
273 size_t size = bench.size();
274 std::vector<vpMatrix> PI(size), imA(size), imAt(size), kerAt(size);
275 std::vector<vpColVector> sv(size);
276 int ret = EXIT_SUCCESS;
277
278 // test 0
279 unsigned int test = 0;
280 double t = vpTime::measureTimeMs();
281 for (unsigned int i = 0; i < bench.size(); i++) {
282 PI[i] = bench[i].pseudoInverse();
283 }
284 time[test] = vpTime::measureTimeMs() - t;
285 for (unsigned int i = 0; i < time.size(); i++) {
286 ret += test_pseudo_inverse(bench, PI);
287 }
288
289 // test 1
290 test++;
292 for (unsigned int i = 0; i < bench.size(); i++) {
293 bench[i].pseudoInverse(PI[i]);
294 }
295 time[test] = vpTime::measureTimeMs() - t;
296 for (unsigned int i = 0; i < time.size(); i++) {
297 ret += test_pseudo_inverse(bench, PI);
298 }
299
300 // test 2
301 test++;
303 for (unsigned int i = 0; i < bench.size(); i++) {
304 bench[i].pseudoInverse(PI[i], sv[i]);
305 }
306 time[test] = vpTime::measureTimeMs() - t;
307 for (unsigned int i = 0; i < time.size(); i++) {
308 ret += test_pseudo_inverse(bench, PI);
309 }
310
311 // test 3
312 test++;
314 for (unsigned int i = 0; i < bench.size(); i++) {
315 bench[i].pseudoInverse(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]);
316 }
317 time[test] = vpTime::measureTimeMs() - t;
318
319 for (unsigned int i = 0; i < time.size(); i++) {
320 ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
321 }
322
323 return ret;
324}
325
326#if defined(VISP_HAVE_EIGEN3)
327int test_pseudo_inverse_eigen3(bool verbose, const std::vector<vpMatrix> &bench, std::vector<double> &time)
328{
329 if (verbose)
330 std::cout << "Test pseudo-inverse using Eigen3 3rd party" << std::endl;
331 if (verbose)
332 std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl;
333
334 size_t size = bench.size();
335 std::vector<vpMatrix> PI(size), imA(size), imAt(size), kerAt(size);
336 std::vector<vpColVector> sv(size);
337 int ret = EXIT_SUCCESS;
338
339 // test 0
340 unsigned int test = 0;
341 double t = vpTime::measureTimeMs();
342 for (unsigned int i = 0; i < bench.size(); i++) {
343 PI[i] = bench[i].pseudoInverseEigen3();
344 }
345 time[test] = vpTime::measureTimeMs() - t;
346 for (unsigned int i = 0; i < time.size(); i++) {
347 ret += test_pseudo_inverse(bench, PI);
348 }
349
350 // test 1
351 test++;
353 for (unsigned int i = 0; i < bench.size(); i++) {
354 bench[i].pseudoInverseEigen3(PI[i]);
355 }
356 time[test] = vpTime::measureTimeMs() - t;
357 for (unsigned int i = 0; i < time.size(); i++) {
358 ret += test_pseudo_inverse(bench, PI);
359 }
360
361 // test 2
362 test++;
364 for (unsigned int i = 0; i < bench.size(); i++) {
365 bench[i].pseudoInverseEigen3(PI[i], sv[i]);
366 }
367 time[test] = vpTime::measureTimeMs() - t;
368 for (unsigned int i = 0; i < time.size(); i++) {
369 ret += test_pseudo_inverse(bench, PI);
370 }
371
372 // test 3
373 test++;
375 for (unsigned int i = 0; i < bench.size(); i++) {
376 bench[i].pseudoInverseEigen3(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]);
377 }
378 time[test] = vpTime::measureTimeMs() - t;
379
380 for (unsigned int i = 0; i < time.size(); i++) {
381 ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
382 }
383
384 return ret;
385}
386#endif
387
388#if defined(VISP_HAVE_LAPACK)
389int test_pseudo_inverse_lapack(bool verbose, const std::vector<vpMatrix> &bench, std::vector<double> &time)
390{
391 if (verbose)
392 std::cout << "Test pseudo-inverse using Lapack 3rd party" << std::endl;
393 if (verbose)
394 std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl;
395
396 size_t size = bench.size();
397 std::vector<vpMatrix> PI(size), imA(size), imAt(size), kerAt(size);
398 std::vector<vpColVector> sv(size);
399 int ret = EXIT_SUCCESS;
400
401 // test 0
402 unsigned int test = 0;
403 double t = vpTime::measureTimeMs();
404 for (unsigned int i = 0; i < bench.size(); i++) {
405 PI[i] = bench[i].pseudoInverseLapack();
406 }
407 time[test] = vpTime::measureTimeMs() - t;
408 for (unsigned int i = 0; i < time.size(); i++) {
409 ret += test_pseudo_inverse(bench, PI);
410 }
411
412 // test 1
413 test++;
415 for (unsigned int i = 0; i < bench.size(); i++) {
416 bench[i].pseudoInverseLapack(PI[i]);
417 }
418 time[test] = vpTime::measureTimeMs() - t;
419 for (unsigned int i = 0; i < time.size(); i++) {
420 ret += test_pseudo_inverse(bench, PI);
421 }
422
423 // test 2
424 test++;
426 for (unsigned int i = 0; i < bench.size(); i++) {
427 bench[i].pseudoInverseLapack(PI[i], sv[i]);
428 }
429 time[test] = vpTime::measureTimeMs() - t;
430 for (unsigned int i = 0; i < time.size(); i++) {
431 ret += test_pseudo_inverse(bench, PI);
432 }
433
434 // test 3
435 test++;
437 for (unsigned int i = 0; i < bench.size(); i++) {
438 bench[i].pseudoInverseLapack(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]);
439 }
440 time[test] = vpTime::measureTimeMs() - t;
441
442 for (unsigned int i = 0; i < time.size(); i++) {
443 ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
444 }
445
446 return ret;
447}
448#endif
449
450#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
451int test_pseudo_inverse_opencv(bool verbose, const std::vector<vpMatrix> &bench, std::vector<double> &time)
452{
453 if (verbose)
454 std::cout << "Test pseudo-inverse using OpenCV 3rd party" << std::endl;
455 if (verbose)
456 std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl;
457
458 size_t size = bench.size();
459 std::vector<vpMatrix> PI(size), imA(size), imAt(size), kerAt(size);
460 std::vector<vpColVector> sv(size);
461 int ret = EXIT_SUCCESS;
462
463 // test 0
464 unsigned int test = 0;
465 double t = vpTime::measureTimeMs();
466 for (unsigned int i = 0; i < bench.size(); i++) {
467 PI[i] = bench[i].pseudoInverseOpenCV();
468 }
469 time[test] = vpTime::measureTimeMs() - t;
470 for (unsigned int i = 0; i < time.size(); i++) {
471 ret += test_pseudo_inverse(bench, PI);
472 }
473
474 // test 1
475 test++;
477 for (unsigned int i = 0; i < bench.size(); i++) {
478 bench[i].pseudoInverseOpenCV(PI[i]);
479 }
480 time[test] = vpTime::measureTimeMs() - t;
481 for (unsigned int i = 0; i < time.size(); i++) {
482 ret += test_pseudo_inverse(bench, PI);
483 }
484
485 // test 2
486 test++;
488 for (unsigned int i = 0; i < bench.size(); i++) {
489 bench[i].pseudoInverseOpenCV(PI[i], sv[i]);
490 }
491 time[test] = vpTime::measureTimeMs() - t;
492 for (unsigned int i = 0; i < time.size(); i++) {
493 ret += test_pseudo_inverse(bench, PI);
494 }
495
496 // test 3
497 test++;
499 for (unsigned int i = 0; i < bench.size(); i++) {
500 bench[i].pseudoInverseOpenCV(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]);
501 }
502 time[test] = vpTime::measureTimeMs() - t;
503
504 for (unsigned int i = 0; i < time.size(); i++) {
505 ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
506 }
507
508 return ret;
509}
510#endif
511
512void save_time(const std::string &method, unsigned int nrows, unsigned int ncols, bool verbose, bool use_plot_file,
513 std::ofstream &of, const std::vector<double> &time)
514{
515 for (size_t i = 0; i < time.size(); i++) {
516 if (use_plot_file)
517 of << time[i] << "\t";
518 if (verbose) {
519 std::cout << " " << method << " svd(" << nrows << "x" << ncols << ")"
520 << " test " << i << ": " << time[i] << std::endl;
521 }
522 }
523}
524
525int main(int argc, const char *argv[])
526{
527 try {
528#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || (VISP_HAVE_OPENCV_VERSION >= 0x020101)
529 unsigned int nb_matrices = 10;
530 unsigned int nb_iterations = 10;
531 unsigned int nb_rows = 12;
532 unsigned int nb_cols = 6;
533 bool verbose = false;
534 std::string plotfile("plot-pseudo-inv.csv");
535 bool use_plot_file = false;
536 std::ofstream of;
537
538 unsigned int nb_svd_functions = 4; // 4 tests for each existing vpMatrix::pseudoInverse(...) functions
539 unsigned int nb_test_matrix_size = 3; // 3 tests: m > n, m = n, m < n
540 std::vector<double> time(nb_svd_functions);
541 std::vector<unsigned int> nrows(nb_test_matrix_size), ncols(nb_test_matrix_size);
542
543 // Read the command line options
544 if (getOptions(argc, argv, nb_matrices, nb_iterations, use_plot_file, plotfile, nb_rows, nb_cols, verbose) ==
545 false) {
546 exit(-1);
547 }
548
549 for (unsigned int s = 0; s < nb_test_matrix_size; s++) {
550 // consider m > n, m = n, m < n
551 if (s == 0) {
552 nrows[s] = nb_rows;
553 ncols[s] = nb_cols;
554 } else if (s == 1) {
555 nrows[s] = nb_cols;
556 ncols[s] = nb_cols;
557 } else {
558 nrows[s] = nb_cols;
559 ncols[s] = nb_rows;
560 }
561 }
562
563 if (use_plot_file) {
564 of.open(plotfile.c_str());
565 of << "iter"
566 << "\t";
567
568 for (unsigned int s = 0; s < nb_test_matrix_size; s++) {
569 for (unsigned int i = 0; i < nb_svd_functions; i++)
570 of << "\"default " << nrows[s] << "x" << ncols[s] << " test " << i << "\""
571 << "\t";
572
573#if defined(VISP_HAVE_LAPACK)
574 for (unsigned int i = 0; i < nb_svd_functions; i++)
575 of << "\"Lapack " << nrows[s] << "x" << ncols[s] << " test " << i << "\""
576 << "\t";
577#endif
578#if defined(VISP_HAVE_EIGEN3)
579 for (unsigned int i = 0; i < nb_svd_functions; i++)
580 of << "\"Eigen3 " << nrows[s] << "x" << ncols[s] << " test " << i << "\""
581 << "\t";
582#endif
583#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
584 for (unsigned int i = 0; i < nb_svd_functions; i++)
585 of << "\"OpenCV " << nrows[s] << "x" << ncols[s] << " test " << i << "\""
586 << "\t";
587#endif
588 }
589 of << std::endl;
590 }
591
592 int ret = EXIT_SUCCESS;
593 for (unsigned int iter = 0; iter < nb_iterations; iter++) {
594
595 if (use_plot_file)
596 of << iter << "\t";
597
598 for (unsigned int s = 0; s < nb_test_matrix_size; s++) {
599 std::vector<vpMatrix> bench_random_matrices;
600 create_bench_random_matrix(nb_matrices, nrows[s], ncols[s], verbose, bench_random_matrices);
601
602 ret += test_pseudo_inverse_default(verbose, bench_random_matrices, time);
603 save_time("default -", nrows[s], ncols[s], verbose, use_plot_file, of, time);
604
605#if defined(VISP_HAVE_LAPACK)
606 ret += test_pseudo_inverse_lapack(verbose, bench_random_matrices, time);
607 save_time("Lapack -", nrows[s], ncols[s], verbose, use_plot_file, of, time);
608#endif
609
610#if defined(VISP_HAVE_EIGEN3)
611 ret += test_pseudo_inverse_eigen3(verbose, bench_random_matrices, time);
612 save_time("Eigen3 -", nrows[s], ncols[s], verbose, use_plot_file, of, time);
613#endif
614
615#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
616 ret += test_pseudo_inverse_opencv(verbose, bench_random_matrices, time);
617 save_time("OpenCV -", nrows[s], ncols[s], verbose, use_plot_file, of, time);
618#endif
619 }
620 if (use_plot_file)
621 of << std::endl;
622 }
623 if (use_plot_file) {
624 of.close();
625 std::cout << "Result saved in " << plotfile << std::endl;
626 }
627
628 if (ret == EXIT_SUCCESS) {
629 std::cout << "Test succeed" << std::endl;
630 } else {
631 std::cout << "Test failed" << std::endl;
632 }
633
634 return ret;
635#else
636 (void)argc;
637 (void)argv;
638 std::cout << "Test does nothing since you dont't have Lapack, Eigen3 or OpenCV 3rd party"
639 << std::endl;
640 return EXIT_SUCCESS;
641#endif
642 } catch (const vpException &e) {
643 std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
644 return EXIT_FAILURE;
645 }
646}
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
error that can be emited by ViSP classes.
Definition: vpException.h:72
const std::string & getStringMessage() const
Send a reference (constant) related the error message (can be empty).
Definition: vpException.cpp:92
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpMatrix t() const
Definition: vpMatrix.cpp:464
double frobeniusNorm() const
Definition: vpMatrix.cpp:6703
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5988
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
VISP_EXPORT double measureTimeMs()