Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMatrix.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 * Matrix manipulation.
33 *
34*****************************************************************************/
40#include <algorithm>
41#include <assert.h>
42#include <cmath> // std::fabs
43#include <fstream>
44#include <limits> // numeric_limits
45#include <sstream>
46#include <stdio.h>
47#include <stdlib.h>
48#include <string.h>
49#include <string>
50#include <vector>
51
52#include <visp3/core/vpCPUFeatures.h>
53#include <visp3/core/vpColVector.h>
54#include <visp3/core/vpConfig.h>
55#include <visp3/core/vpDebug.h>
56#include <visp3/core/vpException.h>
57#include <visp3/core/vpMath.h>
58#include <visp3/core/vpMatrix.h>
59#include <visp3/core/vpTranslationVector.h>
60
61#include <Simd/SimdLib.hpp>
62
63#ifdef VISP_HAVE_LAPACK
64#ifdef VISP_HAVE_GSL
65#include <gsl/gsl_eigen.h>
66#include <gsl/gsl_linalg.h>
67#include <gsl/gsl_math.h>
68#elif defined(VISP_HAVE_MKL)
69#include <mkl.h>
70typedef MKL_INT integer;
71
72void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_, double *w_data,
73 double *work_data, int lwork_, int &info_)
74{
75 MKL_INT n = static_cast<MKL_INT>(n_);
76 MKL_INT lda = static_cast<MKL_INT>(lda_);
77 MKL_INT lwork = static_cast<MKL_INT>(lwork_);
78 MKL_INT info = static_cast<MKL_INT>(info_);
79
80 dsyev(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
81}
82
83#else
84#if defined(VISP_HAVE_LAPACK_BUILT_IN)
85typedef long int integer;
86#else
87typedef int integer;
88#endif
89extern "C" integer dsyev_(char *jobz, char *uplo, integer *n, double *a, integer *lda, double *w, double *WORK,
90 integer *lwork, integer *info);
91
92void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_, double *w_data,
93 double *work_data, int lwork_, int &info_)
94{
95 integer n = static_cast<integer>(n_);
96 integer lda = static_cast<integer>(lda_);
97 integer lwork = static_cast<integer>(lwork_);
98 integer info = static_cast<integer>(info_);
99
100 dsyev_(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
101
102 lwork_ = static_cast<int>(lwork);
103 info_ = static_cast<int>(info);
104}
105#endif
106#endif
107
108#if !defined(VISP_USE_MSVC) || (defined(VISP_USE_MSVC) && !defined(VISP_BUILD_SHARED_LIBS))
109const unsigned int vpMatrix::m_lapack_min_size_default = 0;
110unsigned int vpMatrix::m_lapack_min_size = vpMatrix::m_lapack_min_size_default;
111#endif
112
113// Prototypes of specific functions
114vpMatrix subblock(const vpMatrix &, unsigned int, unsigned int);
115
116void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows,
117 unsigned int ncols, double svThreshold, vpMatrix &Ap, int &rank_out, int *rank_in,
118 vpMatrix *imA, vpMatrix *imAt, vpMatrix *kerAt)
119{
120 Ap.resize(ncols, nrows, true, false);
121
122 // compute the highest singular value and the rank of h
123 double maxsv = sv[0];
124
125 rank_out = 0;
126
127 for (unsigned int i = 0; i < ncols; i++) {
128 if (sv[i] > maxsv * svThreshold) {
129 rank_out++;
130 }
131 }
132
133 unsigned int rank = static_cast<unsigned int>(rank_out);
134 if (rank_in) {
135 rank = static_cast<unsigned int>(*rank_in);
136 }
137
138 for (unsigned int i = 0; i < ncols; i++) {
139 for (unsigned int j = 0; j < nrows; j++) {
140 for (unsigned int k = 0; k < rank; k++) {
141 Ap[i][j] += V[i][k] * U[j][k] / sv[k];
142 }
143 }
144 }
145
146 // Compute im(A)
147 if (imA) {
148 imA->resize(nrows, rank);
149
150 for (unsigned int i = 0; i < nrows; i++) {
151 for (unsigned int j = 0; j < rank; j++) {
152 (*imA)[i][j] = U[i][j];
153 }
154 }
155 }
156
157 // Compute im(At)
158 if (imAt) {
159 imAt->resize(ncols, rank);
160 for (unsigned int i = 0; i < ncols; i++) {
161 for (unsigned int j = 0; j < rank; j++) {
162 (*imAt)[i][j] = V[i][j];
163 }
164 }
165 }
166
167 // Compute ker(At)
168 if (kerAt) {
169 kerAt->resize(ncols - rank, ncols);
170 if (rank != ncols) {
171 for (unsigned int k = 0; k < (ncols - rank); k++) {
172 unsigned j = k + rank;
173 for (unsigned int i = 0; i < V.getRows(); i++) {
174 (*kerAt)[k][i] = V[i][j];
175 }
176 }
177 }
178 }
179}
180
186vpMatrix::vpMatrix(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
187 : vpArray2D<double>()
188{
189 if (((r + nrows) > M.rowNum) || ((c + ncols) > M.colNum)) {
190 throw(vpException(vpException::dimensionError,
191 "Cannot construct a sub matrix (%dx%d) starting at "
192 "position (%d,%d) that is not contained in the "
193 "original matrix (%dx%d)",
194 nrows, ncols, r, c, M.rowNum, M.colNum));
195 }
196
197 init(M, r, c, nrows, ncols);
198}
199
200#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
202{
203 rowNum = A.rowNum;
204 colNum = A.colNum;
205 rowPtrs = A.rowPtrs;
206 dsize = A.dsize;
207 data = A.data;
208
209 A.rowNum = 0;
210 A.colNum = 0;
211 A.rowPtrs = NULL;
212 A.dsize = 0;
213 A.data = NULL;
214}
215
241vpMatrix::vpMatrix(const std::initializer_list<double> &list) : vpArray2D<double>(list) { }
242
267vpMatrix::vpMatrix(unsigned int nrows, unsigned int ncols, const std::initializer_list<double> &list)
268 : vpArray2D<double>(nrows, ncols, list)
269{ }
270
293vpMatrix::vpMatrix(const std::initializer_list<std::initializer_list<double> > &lists) : vpArray2D<double>(lists) { }
294
295#endif
296
343void vpMatrix::init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
344{
345 unsigned int rnrows = r + nrows;
346 unsigned int cncols = c + ncols;
347
348 if (rnrows > M.getRows())
349 throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
350 M.getRows()));
351 if (cncols > M.getCols())
352 throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
353 M.getCols()));
354 resize(nrows, ncols, false, false);
355
356 if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
357 return; // Noting to do
358 for (unsigned int i = 0; i < nrows; i++) {
359 memcpy((*this)[i], &M[i + r][c], ncols * sizeof(double));
360 }
361}
362
404vpMatrix vpMatrix::extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
405{
406 unsigned int rnrows = r + nrows;
407 unsigned int cncols = c + ncols;
408
409 if (rnrows > getRows())
410 throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
411 getRows()));
412 if (cncols > getCols())
413 throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
414 getCols()));
415
416 vpMatrix M;
417 M.resize(nrows, ncols, false, false);
418 for (unsigned int i = 0; i < nrows; i++) {
419 memcpy(M[i], &(*this)[i + r][c], ncols * sizeof(double));
420 }
421
422 return M;
423}
424
429void vpMatrix::eye(unsigned int n) { eye(n, n); }
430
435void vpMatrix::eye(unsigned int m, unsigned int n)
436{
437 resize(m, n);
438
439 eye();
440}
441
447{
448 for (unsigned int i = 0; i < rowNum; i++) {
449 for (unsigned int j = 0; j < colNum; j++) {
450 if (i == j)
451 (*this)[i][j] = 1.0;
452 else
453 (*this)[i][j] = 0;
454 }
455 }
456}
457
461vpMatrix vpMatrix::t() const { return transpose(); }
462
469{
470 vpMatrix At;
471 transpose(At);
472 return At;
473}
474
481{
482 At.resize(colNum, rowNum, false, false);
483
484 if (rowNum <= 16 || colNum <= 16) {
485 for (unsigned int i = 0; i < rowNum; i++) {
486 for (unsigned int j = 0; j < colNum; j++) {
487 At[j][i] = (*this)[i][j];
488 }
489 }
490 }
491 else {
492 SimdMatTranspose(data, rowNum, colNum, At.data);
493 }
494}
495
502{
503 vpMatrix B;
504
505 AAt(B);
506
507 return B;
508}
509
522{
523 if ((B.rowNum != rowNum) || (B.colNum != rowNum))
524 B.resize(rowNum, rowNum, false, false);
525
526 // If available use Lapack only for large matrices
527 bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
528#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
529 useLapack = false;
530#endif
531
532 if (useLapack) {
533#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
534 const double alpha = 1.0;
535 const double beta = 0.0;
536 const char transa = 't';
537 const char transb = 'n';
538
539 vpMatrix::blas_dgemm(transa, transb, rowNum, rowNum, colNum, alpha, data, colNum, data, colNum, beta, B.data,
540 rowNum);
541#endif
542 }
543 else {
544 // compute A*A^T
545 for (unsigned int i = 0; i < rowNum; i++) {
546 for (unsigned int j = i; j < rowNum; j++) {
547 double *pi = rowPtrs[i]; // row i
548 double *pj = rowPtrs[j]; // row j
549
550 // sum (row i .* row j)
551 double ssum = 0;
552 for (unsigned int k = 0; k < colNum; k++)
553 ssum += *(pi++) * *(pj++);
554
555 B[i][j] = ssum; // upper triangle
556 if (i != j)
557 B[j][i] = ssum; // lower triangle
558 }
559 }
560 }
561}
562
575{
576 if ((B.rowNum != colNum) || (B.colNum != colNum))
577 B.resize(colNum, colNum, false, false);
578
579 // If available use Lapack only for large matrices
580 bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
581#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
582 useLapack = false;
583#endif
584
585 if (useLapack) {
586#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
587 const double alpha = 1.0;
588 const double beta = 0.0;
589 const char transa = 'n';
590 const char transb = 't';
591
592 vpMatrix::blas_dgemm(transa, transb, colNum, colNum, rowNum, alpha, data, colNum, data, colNum, beta, B.data,
593 colNum);
594#endif
595 }
596 else {
597 for (unsigned int i = 0; i < colNum; i++) {
598 double *Bi = B[i];
599 for (unsigned int j = 0; j < i; j++) {
600 double *ptr = data;
601 double s = 0;
602 for (unsigned int k = 0; k < rowNum; k++) {
603 s += (*(ptr + i)) * (*(ptr + j));
604 ptr += colNum;
605 }
606 *Bi++ = s;
607 B[j][i] = s;
608 }
609 double *ptr = data;
610 double s = 0;
611 for (unsigned int k = 0; k < rowNum; k++) {
612 s += (*(ptr + i)) * (*(ptr + i));
613 ptr += colNum;
614 }
615 *Bi = s;
616 }
617 }
618}
619
626{
627 vpMatrix B;
628
629 AtA(B);
630
631 return B;
632}
633
651{
652 resize(A.getRows(), A.getCols(), false, false);
653
654 if (data != NULL && A.data != NULL && data != A.data) {
655 memcpy(data, A.data, dsize * sizeof(double));
656 }
657
658 return *this;
659}
660
661#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
663{
664 resize(A.getRows(), A.getCols(), false, false);
665
666 if (data != NULL && A.data != NULL && data != A.data) {
667 memcpy(data, A.data, dsize * sizeof(double));
668 }
669
670 return *this;
671}
672
674{
675 if (this != &other) {
676 free(data);
677 free(rowPtrs);
678
679 rowNum = other.rowNum;
680 colNum = other.colNum;
681 rowPtrs = other.rowPtrs;
682 dsize = other.dsize;
683 data = other.data;
684
685 other.rowNum = 0;
686 other.colNum = 0;
687 other.rowPtrs = NULL;
688 other.dsize = 0;
689 other.data = NULL;
690 }
691
692 return *this;
693}
694
717vpMatrix &vpMatrix::operator=(const std::initializer_list<double> &list)
718{
719 if (dsize != static_cast<unsigned int>(list.size())) {
720 resize(1, static_cast<unsigned int>(list.size()), false, false);
721 }
722
723 std::copy(list.begin(), list.end(), data);
724
725 return *this;
726}
727
751vpMatrix &vpMatrix::operator=(const std::initializer_list<std::initializer_list<double> > &lists)
752{
753 unsigned int nrows = static_cast<unsigned int>(lists.size()), ncols = 0;
754 for (auto &l : lists) {
755 if (static_cast<unsigned int>(l.size()) > ncols) {
756 ncols = static_cast<unsigned int>(l.size());
757 }
758 }
759
760 resize(nrows, ncols, false, false);
761 auto it = lists.begin();
762 for (unsigned int i = 0; i < rowNum; i++, ++it) {
763 std::copy(it->begin(), it->end(), rowPtrs[i]);
764 }
765
766 return *this;
767}
768#endif
769
772{
773 std::fill(data, data + rowNum * colNum, x);
774 return *this;
775}
776
783{
784 for (unsigned int i = 0; i < rowNum; i++) {
785 for (unsigned int j = 0; j < colNum; j++) {
786 rowPtrs[i][j] = *x++;
787 }
788 }
789 return *this;
790}
791
793{
794 resize(1, 1, false, false);
795 rowPtrs[0][0] = val;
796 return *this;
797}
798
800{
801 resize(1, colNum + 1, false, false);
802 rowPtrs[0][colNum - 1] = val;
803 return *this;
804}
805
842{
843 unsigned int rows = A.getRows();
844 this->resize(rows, rows);
845
846 (*this) = 0;
847 for (unsigned int i = 0; i < rows; i++)
848 (*this)[i][i] = A[i];
849}
850
881void vpMatrix::diag(const double &val)
882{
883 (*this) = 0;
884 unsigned int min_ = (rowNum < colNum) ? rowNum : colNum;
885 for (unsigned int i = 0; i < min_; i++)
886 (*this)[i][i] = val;
887}
888
901{
902 unsigned int rows = A.getRows();
903 DA.resize(rows, rows, true);
904
905 for (unsigned int i = 0; i < rows; i++)
906 DA[i][i] = A[i];
907}
908
914{
916
917 if (rowNum != 3 || colNum != 3) {
918 throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector",
919 rowNum, colNum, tv.getRows(), tv.getCols()));
920 }
921
922 for (unsigned int j = 0; j < 3; j++)
923 t_out[j] = 0;
924
925 for (unsigned int j = 0; j < 3; j++) {
926 double tj = tv[j]; // optimization em 5/12/2006
927 for (unsigned int i = 0; i < 3; i++) {
928 t_out[i] += rowPtrs[i][j] * tj;
929 }
930 }
931 return t_out;
932}
933
939{
940 vpColVector v_out;
941 vpMatrix::multMatrixVector(*this, v, v_out);
942 return v_out;
943}
944
954{
955 if (A.colNum != v.getRows()) {
956 throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
957 A.getRows(), A.getCols(), v.getRows()));
958 }
959
960 if (A.rowNum != w.rowNum)
961 w.resize(A.rowNum, false);
962
963 // If available use Lapack only for large matrices
964 bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size);
965#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
966 useLapack = false;
967#endif
968
969 if (useLapack) {
970#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
971 double alpha = 1.0;
972 double beta = 0.0;
973 char trans = 't';
974 int incr = 1;
975
976 vpMatrix::blas_dgemv(trans, A.colNum, A.rowNum, alpha, A.data, A.colNum, v.data, incr, beta, w.data, incr);
977#endif
978 }
979 else {
980 w = 0.0;
981 for (unsigned int j = 0; j < A.colNum; j++) {
982 double vj = v[j]; // optimization em 5/12/2006
983 for (unsigned int i = 0; i < A.rowNum; i++) {
984 w[i] += A.rowPtrs[i][j] * vj;
985 }
986 }
987 }
988}
989
990//---------------------------------
991// Matrix operations.
992//---------------------------------
993
1004{
1005 if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1006 C.resize(A.rowNum, B.colNum, false, false);
1007
1008 if (A.colNum != B.rowNum) {
1009 throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (%dx%d) matrix", A.getRows(),
1010 A.getCols(), B.getRows(), B.getCols()));
1011 }
1012
1013 // If available use Lapack only for large matrices
1014 bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size ||
1015 B.colNum > vpMatrix::m_lapack_min_size);
1016#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1017 useLapack = false;
1018#endif
1019
1020 if (useLapack) {
1021#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1022 const double alpha = 1.0;
1023 const double beta = 0.0;
1024 const char trans = 'n';
1025 vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1026 C.data, B.colNum);
1027#endif
1028 }
1029 else {
1030 // 5/12/06 some "very" simple optimization to avoid indexation
1031 const unsigned int BcolNum = B.colNum;
1032 const unsigned int BrowNum = B.rowNum;
1033 double **BrowPtrs = B.rowPtrs;
1034 for (unsigned int i = 0; i < A.rowNum; i++) {
1035 const double *rowptri = A.rowPtrs[i];
1036 double *ci = C[i];
1037 for (unsigned int j = 0; j < BcolNum; j++) {
1038 double s = 0;
1039 for (unsigned int k = 0; k < BrowNum; k++)
1040 s += rowptri[k] * BrowPtrs[k][j];
1041 ci[j] = s;
1042 }
1043 }
1044 }
1045}
1046
1061{
1062 if (A.colNum != 3 || A.rowNum != 3 || B.colNum != 3 || B.rowNum != 3) {
1064 "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1065 "rotation matrix",
1066 A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1067 }
1068 // 5/12/06 some "very" simple optimization to avoid indexation
1069 const unsigned int BcolNum = B.colNum;
1070 const unsigned int BrowNum = B.rowNum;
1071 double **BrowPtrs = B.rowPtrs;
1072 for (unsigned int i = 0; i < A.rowNum; i++) {
1073 const double *rowptri = A.rowPtrs[i];
1074 double *ci = C[i];
1075 for (unsigned int j = 0; j < BcolNum; j++) {
1076 double s = 0;
1077 for (unsigned int k = 0; k < BrowNum; k++)
1078 s += rowptri[k] * BrowPtrs[k][j];
1079 ci[j] = s;
1080 }
1081 }
1082}
1083
1098{
1099 if (A.colNum != 4 || A.rowNum != 4 || B.colNum != 4 || B.rowNum != 4) {
1101 "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1102 "rotation matrix",
1103 A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1104 }
1105 // Considering perfMatrixMultiplication.cpp benchmark,
1106 // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1107 // Lapack usage needs to be validated again.
1108 // If available use Lapack only for large matrices.
1109 // Using SSE2 doesn't speed up.
1110 bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size ||
1111 B.colNum > vpMatrix::m_lapack_min_size);
1112#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1113 useLapack = false;
1114#endif
1115
1116 if (useLapack) {
1117#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1118 const double alpha = 1.0;
1119 const double beta = 0.0;
1120 const char trans = 'n';
1121 vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1122 C.data, B.colNum);
1123#endif
1124 }
1125 else {
1126 // 5/12/06 some "very" simple optimization to avoid indexation
1127 const unsigned int BcolNum = B.colNum;
1128 const unsigned int BrowNum = B.rowNum;
1129 double **BrowPtrs = B.rowPtrs;
1130 for (unsigned int i = 0; i < A.rowNum; i++) {
1131 const double *rowptri = A.rowPtrs[i];
1132 double *ci = C[i];
1133 for (unsigned int j = 0; j < BcolNum; j++) {
1134 double s = 0;
1135 for (unsigned int k = 0; k < BrowNum; k++)
1136 s += rowptri[k] * BrowPtrs[k][j];
1137 ci[j] = s;
1138 }
1139 }
1140 }
1141}
1142
1156{
1158}
1159
1165{
1166 vpMatrix C;
1167
1168 vpMatrix::mult2Matrices(*this, B, C);
1169
1170 return C;
1171}
1172
1178{
1179 if (colNum != R.getRows()) {
1180 throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1181 colNum));
1182 }
1183 vpMatrix C;
1184 C.resize(rowNum, 3, false, false);
1185
1186 unsigned int RcolNum = R.getCols();
1187 unsigned int RrowNum = R.getRows();
1188 for (unsigned int i = 0; i < rowNum; i++) {
1189 double *rowptri = rowPtrs[i];
1190 double *ci = C[i];
1191 for (unsigned int j = 0; j < RcolNum; j++) {
1192 double s = 0;
1193 for (unsigned int k = 0; k < RrowNum; k++)
1194 s += rowptri[k] * R[k][j];
1195 ci[j] = s;
1196 }
1197 }
1198
1199 return C;
1200}
1201
1207{
1208 if (colNum != M.getRows()) {
1209 throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1210 colNum));
1211 }
1212 vpMatrix C;
1213 C.resize(rowNum, 4, false, false);
1214
1215 const unsigned int McolNum = M.getCols();
1216 const unsigned int MrowNum = M.getRows();
1217 for (unsigned int i = 0; i < rowNum; i++) {
1218 const double *rowptri = rowPtrs[i];
1219 double *ci = C[i];
1220 for (unsigned int j = 0; j < McolNum; j++) {
1221 double s = 0;
1222 for (unsigned int k = 0; k < MrowNum; k++)
1223 s += rowptri[k] * M[k][j];
1224 ci[j] = s;
1225 }
1226 }
1227
1228 return C;
1229}
1230
1236{
1237 if (colNum != V.getRows()) {
1238 throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix",
1239 rowNum, colNum));
1240 }
1241 vpMatrix M;
1242 M.resize(rowNum, 6, false, false);
1243
1244 // Considering perfMatrixMultiplication.cpp benchmark,
1245 // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1246 // Lapack usage needs to be validated again.
1247 // If available use Lapack only for large matrices.
1248 // Speed up obtained using SSE2.
1249 bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size ||
1250 V.colNum > vpMatrix::m_lapack_min_size);
1251#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1252 useLapack = false;
1253#endif
1254
1255 if (useLapack) {
1256#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1257 const double alpha = 1.0;
1258 const double beta = 0.0;
1259 const char trans = 'n';
1260 vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, V.colNum, data, colNum, beta, M.data,
1261 M.colNum);
1262#endif
1263 }
1264 else {
1265 SimdMatMulTwist(data, rowNum, V.data, M.data);
1266 }
1267
1268 return M;
1269}
1270
1276{
1277 if (colNum != V.getRows()) {
1278 throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) force/torque twist matrix",
1279 rowNum, colNum));
1280 }
1281 vpMatrix M;
1282 M.resize(rowNum, 6, false, false);
1283
1284 // Considering perfMatrixMultiplication.cpp benchmark,
1285 // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1286 // Lapack usage needs to be validated again.
1287 // If available use Lapack only for large matrices.
1288 // Speed up obtained using SSE2.
1289 bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size ||
1290 V.getCols() > vpMatrix::m_lapack_min_size);
1291#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1292 useLapack = false;
1293#endif
1294
1295 if (useLapack) {
1296#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1297 const double alpha = 1.0;
1298 const double beta = 0.0;
1299 const char trans = 'n';
1300 vpMatrix::blas_dgemm(trans, trans, V.getCols(), rowNum, colNum, alpha, V.data, V.getCols(), data, colNum, beta,
1301 M.data, M.colNum);
1302#endif
1303 }
1304 else {
1305 SimdMatMulTwist(data, rowNum, V.data, M.data);
1306 }
1307
1308 return M;
1309}
1310
1321void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB,
1322 vpMatrix &C)
1323{
1324 if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1325 C.resize(A.rowNum, B.colNum, false, false);
1326
1327 if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1328 throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1329 A.getCols(), B.getRows(), B.getCols()));
1330 }
1331
1332 double **ArowPtrs = A.rowPtrs;
1333 double **BrowPtrs = B.rowPtrs;
1334 double **CrowPtrs = C.rowPtrs;
1335
1336 for (unsigned int i = 0; i < A.rowNum; i++)
1337 for (unsigned int j = 0; j < A.colNum; j++)
1338 CrowPtrs[i][j] = wB * BrowPtrs[i][j] + wA * ArowPtrs[i][j];
1339}
1340
1351{
1352 if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1353 C.resize(A.rowNum, B.colNum, false, false);
1354
1355 if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1356 throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1357 A.getCols(), B.getRows(), B.getCols()));
1358 }
1359
1360 double **ArowPtrs = A.rowPtrs;
1361 double **BrowPtrs = B.rowPtrs;
1362 double **CrowPtrs = C.rowPtrs;
1363
1364 for (unsigned int i = 0; i < A.rowNum; i++) {
1365 for (unsigned int j = 0; j < A.colNum; j++) {
1366 CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1367 }
1368 }
1369}
1370
1384{
1385 if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1386 C.resize(A.rowNum);
1387
1388 if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1389 throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1390 A.getCols(), B.getRows(), B.getCols()));
1391 }
1392
1393 double **ArowPtrs = A.rowPtrs;
1394 double **BrowPtrs = B.rowPtrs;
1395 double **CrowPtrs = C.rowPtrs;
1396
1397 for (unsigned int i = 0; i < A.rowNum; i++) {
1398 for (unsigned int j = 0; j < A.colNum; j++) {
1399 CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1400 }
1401 }
1402}
1403
1409{
1410 vpMatrix C;
1411 vpMatrix::add2Matrices(*this, B, C);
1412 return C;
1413}
1414
1431{
1432 if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1433 C.resize(A.rowNum);
1434
1435 if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1436 throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1437 A.getCols(), B.getRows(), B.getCols()));
1438 }
1439
1440 double **ArowPtrs = A.rowPtrs;
1441 double **BrowPtrs = B.rowPtrs;
1442 double **CrowPtrs = C.rowPtrs;
1443
1444 for (unsigned int i = 0; i < A.rowNum; i++) {
1445 for (unsigned int j = 0; j < A.colNum; j++) {
1446 CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1447 }
1448 }
1449}
1450
1464{
1465 if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1466 C.resize(A.rowNum, A.colNum, false, false);
1467
1468 if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1469 throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1470 A.getCols(), B.getRows(), B.getCols()));
1471 }
1472
1473 double **ArowPtrs = A.rowPtrs;
1474 double **BrowPtrs = B.rowPtrs;
1475 double **CrowPtrs = C.rowPtrs;
1476
1477 for (unsigned int i = 0; i < A.rowNum; i++) {
1478 for (unsigned int j = 0; j < A.colNum; j++) {
1479 CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1480 }
1481 }
1482}
1483
1489{
1490 vpMatrix C;
1491 vpMatrix::sub2Matrices(*this, B, C);
1492 return C;
1493}
1494
1496
1498{
1499 if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1500 throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1501 B.getRows(), B.getCols()));
1502 }
1503
1504 double **BrowPtrs = B.rowPtrs;
1505
1506 for (unsigned int i = 0; i < rowNum; i++)
1507 for (unsigned int j = 0; j < colNum; j++)
1508 rowPtrs[i][j] += BrowPtrs[i][j];
1509
1510 return *this;
1511}
1512
1515{
1516 if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1517 throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1518 B.getRows(), B.getCols()));
1519 }
1520
1521 double **BrowPtrs = B.rowPtrs;
1522 for (unsigned int i = 0; i < rowNum; i++)
1523 for (unsigned int j = 0; j < colNum; j++)
1524 rowPtrs[i][j] -= BrowPtrs[i][j];
1525
1526 return *this;
1527}
1528
1539{
1540 if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1541 C.resize(A.rowNum, A.colNum, false, false);
1542
1543 double **ArowPtrs = A.rowPtrs;
1544 double **CrowPtrs = C.rowPtrs;
1545
1546 for (unsigned int i = 0; i < A.rowNum; i++)
1547 for (unsigned int j = 0; j < A.colNum; j++)
1548 CrowPtrs[i][j] = -ArowPtrs[i][j];
1549}
1550
1556{
1557 vpMatrix C;
1558 vpMatrix::negateMatrix(*this, C);
1559 return C;
1560}
1561
1562double vpMatrix::sum() const
1563{
1564 double s = 0.0;
1565 for (unsigned int i = 0; i < rowNum; i++) {
1566 for (unsigned int j = 0; j < colNum; j++) {
1567 s += rowPtrs[i][j];
1568 }
1569 }
1570
1571 return s;
1572}
1573
1574//---------------------------------
1575// Matrix/vector operations.
1576//---------------------------------
1577
1578//---------------------------------
1579// Matrix/real operations.
1580//---------------------------------
1581
1586vpMatrix operator*(const double &x, const vpMatrix &B)
1587{
1588 if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1589 return B;
1590 }
1591
1592 unsigned int Brow = B.getRows();
1593 unsigned int Bcol = B.getCols();
1594
1595 vpMatrix C;
1596 C.resize(Brow, Bcol, false, false);
1597
1598 for (unsigned int i = 0; i < Brow; i++)
1599 for (unsigned int j = 0; j < Bcol; j++)
1600 C[i][j] = B[i][j] * x;
1601
1602 return C;
1603}
1604
1610{
1611 if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1612 return (*this);
1613 }
1614
1615 vpMatrix M;
1616 M.resize(rowNum, colNum, false, false);
1617
1618 for (unsigned int i = 0; i < rowNum; i++)
1619 for (unsigned int j = 0; j < colNum; j++)
1620 M[i][j] = rowPtrs[i][j] * x;
1621
1622 return M;
1623}
1624
1627{
1628 if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1629 return (*this);
1630 }
1631
1632 if (std::fabs(x) < std::numeric_limits<double>::epsilon()) {
1633 throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1634 }
1635
1636 vpMatrix C;
1637 C.resize(rowNum, colNum, false, false);
1638
1639 double xinv = 1 / x;
1640
1641 for (unsigned int i = 0; i < rowNum; i++)
1642 for (unsigned int j = 0; j < colNum; j++)
1643 C[i][j] = rowPtrs[i][j] * xinv;
1644
1645 return C;
1646}
1647
1650{
1651 for (unsigned int i = 0; i < rowNum; i++)
1652 for (unsigned int j = 0; j < colNum; j++)
1653 rowPtrs[i][j] += x;
1654
1655 return *this;
1656}
1657
1660{
1661 for (unsigned int i = 0; i < rowNum; i++)
1662 for (unsigned int j = 0; j < colNum; j++)
1663 rowPtrs[i][j] -= x;
1664
1665 return *this;
1666}
1667
1673{
1674 if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1675 return *this;
1676 }
1677
1678 for (unsigned int i = 0; i < rowNum; i++)
1679 for (unsigned int j = 0; j < colNum; j++)
1680 rowPtrs[i][j] *= x;
1681
1682 return *this;
1683}
1684
1687{
1688 if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1689 return *this;
1690 }
1691
1692 if (std::fabs(x) < std::numeric_limits<double>::epsilon())
1693 throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1694
1695 double xinv = 1 / x;
1696
1697 for (unsigned int i = 0; i < rowNum; i++)
1698 for (unsigned int j = 0; j < colNum; j++)
1699 rowPtrs[i][j] *= xinv;
1700
1701 return *this;
1702}
1703
1704//----------------------------------------------------------------
1705// Matrix Operation
1706//----------------------------------------------------------------
1707
1713{
1714 if ((out.rowNum != colNum * rowNum) || (out.colNum != 1))
1715 out.resize(colNum * rowNum, false, false);
1716
1717 double *optr = out.data;
1718 for (unsigned int j = 0; j < colNum; j++) {
1719 for (unsigned int i = 0; i < rowNum; i++) {
1720 *(optr++) = rowPtrs[i][j];
1721 }
1722 }
1723}
1724
1730{
1731 vpColVector out(colNum * rowNum);
1732 stackColumns(out);
1733 return out;
1734}
1735
1741{
1742 if ((out.getRows() != 1) || (out.getCols() != colNum * rowNum))
1743 out.resize(colNum * rowNum, false, false);
1744
1745 memcpy(out.data, data, sizeof(double) * out.getCols());
1746}
1752{
1753 vpRowVector out(colNum * rowNum);
1754 stackRows(out);
1755 return out;
1756}
1757
1765{
1766 if (m.getRows() != rowNum || m.getCols() != colNum) {
1767 throw(vpException(vpException::dimensionError, "In Hadamard product: bad dimension of input matrix"));
1768 }
1769
1770 vpMatrix out;
1771 out.resize(rowNum, colNum, false, false);
1772
1773 SimdVectorHadamard(data, m.data, dsize, out.data);
1774
1775 return out;
1776}
1777
1784void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out)
1785{
1786 unsigned int r1 = m1.getRows();
1787 unsigned int c1 = m1.getCols();
1788 unsigned int r2 = m2.getRows();
1789 unsigned int c2 = m2.getCols();
1790
1791 out.resize(r1 * r2, c1 * c2, false, false);
1792
1793 for (unsigned int r = 0; r < r1; r++) {
1794 for (unsigned int c = 0; c < c1; c++) {
1795 double alpha = m1[r][c];
1796 double *m2ptr = m2[0];
1797 unsigned int roffset = r * r2;
1798 unsigned int coffset = c * c2;
1799 for (unsigned int rr = 0; rr < r2; rr++) {
1800 for (unsigned int cc = 0; cc < c2; cc++) {
1801 out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1802 }
1803 }
1804 }
1805 }
1806}
1807
1814void vpMatrix::kron(const vpMatrix &m, vpMatrix &out) const { kron(*this, m, out); }
1815
1823{
1824 unsigned int r1 = m1.getRows();
1825 unsigned int c1 = m1.getCols();
1826 unsigned int r2 = m2.getRows();
1827 unsigned int c2 = m2.getCols();
1828
1829 vpMatrix out;
1830 out.resize(r1 * r2, c1 * c2, false, false);
1831
1832 for (unsigned int r = 0; r < r1; r++) {
1833 for (unsigned int c = 0; c < c1; c++) {
1834 double alpha = m1[r][c];
1835 double *m2ptr = m2[0];
1836 unsigned int roffset = r * r2;
1837 unsigned int coffset = c * c2;
1838 for (unsigned int rr = 0; rr < r2; rr++) {
1839 for (unsigned int cc = 0; cc < c2; cc++) {
1840 out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1841 }
1842 }
1843 }
1844 }
1845 return out;
1846}
1847
1853vpMatrix vpMatrix::kron(const vpMatrix &m) const { return kron(*this, m); }
1854
1905void vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const { x = pseudoInverse(1e-6) * b; }
1906
1957{
1959
1960 solveBySVD(B, X);
1961 return X;
1962}
1963
2028{
2029#if defined(VISP_HAVE_LAPACK)
2030 svdLapack(w, V);
2031#elif defined(VISP_HAVE_EIGEN3)
2032 svdEigen3(w, V);
2033#elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2034 svdOpenCV(w, V);
2035#else
2036 (void)w;
2037 (void)V;
2038 throw(vpException(vpException::fatalError, "Cannot compute SVD. Install Lapack, Eigen3 or OpenCV 3rd party"));
2039#endif
2040}
2041
2096unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
2097{
2098#if defined(VISP_HAVE_LAPACK)
2099 return pseudoInverseLapack(Ap, svThreshold);
2100#elif defined(VISP_HAVE_EIGEN3)
2101 return pseudoInverseEigen3(Ap, svThreshold);
2102#elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2103 return pseudoInverseOpenCV(Ap, svThreshold);
2104#else
2105 (void)Ap;
2106 (void)svThreshold;
2107 throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2108 "Install Lapack, Eigen3 or OpenCV 3rd party"));
2109#endif
2110}
2111
2172int vpMatrix::pseudoInverse(vpMatrix &Ap, int rank_in) const
2173{
2174#if defined(VISP_HAVE_LAPACK)
2175 return pseudoInverseLapack(Ap, rank_in);
2176#elif defined(VISP_HAVE_EIGEN3)
2177 return pseudoInverseEigen3(Ap, rank_in);
2178#elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2179 return pseudoInverseOpenCV(Ap, rank_in);
2180#else
2181 (void)Ap;
2182 (void)svThreshold;
2183 throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2184 "Install Lapack, Eigen3 or OpenCV 3rd party"));
2185#endif
2186}
2187
2238vpMatrix vpMatrix::pseudoInverse(double svThreshold) const
2239{
2240#if defined(VISP_HAVE_LAPACK)
2241 return pseudoInverseLapack(svThreshold);
2242#elif defined(VISP_HAVE_EIGEN3)
2243 return pseudoInverseEigen3(svThreshold);
2244#elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2245 return pseudoInverseOpenCV(svThreshold);
2246#else
2247 (void)svThreshold;
2248 throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2249 "Install Lapack, Eigen3 or OpenCV 3rd party"));
2250#endif
2251}
2252
2304{
2305#if defined(VISP_HAVE_LAPACK)
2306 return pseudoInverseLapack(rank_in);
2307#elif defined(VISP_HAVE_EIGEN3)
2308 return pseudoInverseEigen3(rank_in);
2309#elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2310 return pseudoInverseOpenCV(rank_in);
2311#else
2312 (void)svThreshold;
2313 throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2314 "Install Lapack, Eigen3 or OpenCV 3rd party"));
2315#endif
2316}
2317
2318#ifndef DOXYGEN_SHOULD_SKIP_THIS
2319#if defined(VISP_HAVE_LAPACK)
2356vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const
2357{
2358 unsigned int nrows = getRows();
2359 unsigned int ncols = getCols();
2360 int rank_out;
2361
2362 vpMatrix U, V, Ap;
2363 vpColVector sv;
2364
2365 Ap.resize(ncols, nrows, false, false);
2366
2367 if (nrows < ncols) {
2368 U.resize(ncols, ncols, true);
2369 sv.resize(nrows, false);
2370 }
2371 else {
2372 U.resize(nrows, ncols, false);
2373 sv.resize(ncols, false);
2374 }
2375
2376 U.insert(*this, 0, 0);
2377 U.svdLapack(sv, V);
2378
2379 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
2380
2381 return Ap;
2382}
2383
2424unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) const
2425{
2426 unsigned int nrows = getRows();
2427 unsigned int ncols = getCols();
2428 int rank_out;
2429
2430 vpMatrix U, V;
2431 vpColVector sv;
2432
2433 Ap.resize(ncols, nrows, false, false);
2434
2435 if (nrows < ncols) {
2436 U.resize(ncols, ncols, true);
2437 sv.resize(nrows, false);
2438 }
2439 else {
2440 U.resize(nrows, ncols, false);
2441 sv.resize(ncols, false);
2442 }
2443
2444 U.insert(*this, 0, 0);
2445 U.svdLapack(sv, V);
2446
2447 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
2448
2449 return static_cast<unsigned int>(rank_out);
2450}
2498unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2499{
2500 unsigned int nrows = getRows();
2501 unsigned int ncols = getCols();
2502 int rank_out;
2503
2504 vpMatrix U, V;
2505 vpColVector sv_;
2506
2507 Ap.resize(ncols, nrows, false, false);
2508
2509 if (nrows < ncols) {
2510 U.resize(ncols, ncols, true);
2511 sv.resize(nrows, false);
2512 }
2513 else {
2514 U.resize(nrows, ncols, false);
2515 sv.resize(ncols, false);
2516 }
2517
2518 U.insert(*this, 0, 0);
2519 U.svdLapack(sv_, V);
2520
2521 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
2522
2523 // Remove singular values equal to the one that corresponds to the lines of 0
2524 // introduced when m < n
2525 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2526
2527 return static_cast<unsigned int>(rank_out);
2528}
2529
2636unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
2637 vpMatrix &imAt, vpMatrix &kerAt) const
2638{
2639 unsigned int nrows = getRows();
2640 unsigned int ncols = getCols();
2641 int rank_out;
2642 vpMatrix U, V;
2643 vpColVector sv_;
2644
2645 if (nrows < ncols) {
2646 U.resize(ncols, ncols, true);
2647 sv.resize(nrows, false);
2648 }
2649 else {
2650 U.resize(nrows, ncols, false);
2651 sv.resize(ncols, false);
2652 }
2653
2654 U.insert(*this, 0, 0);
2655 U.svdLapack(sv_, V);
2656
2657 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerAt);
2658
2659 // Remove singular values equal to the one that corresponds to the lines of 0
2660 // introduced when m < n
2661 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2662
2663 return static_cast<unsigned int>(rank_out);
2664}
2665
2702vpMatrix vpMatrix::pseudoInverseLapack(int rank_in) const
2703{
2704 unsigned int nrows = getRows();
2705 unsigned int ncols = getCols();
2706 int rank_out;
2707 double svThreshold = 1e-26;
2708
2709 vpMatrix U, V, Ap;
2710 vpColVector sv;
2711
2712 Ap.resize(ncols, nrows, false, false);
2713
2714 if (nrows < ncols) {
2715 U.resize(ncols, ncols, true);
2716 sv.resize(nrows, false);
2717 }
2718 else {
2719 U.resize(nrows, ncols, false);
2720 sv.resize(ncols, false);
2721 }
2722
2723 U.insert(*this, 0, 0);
2724 U.svdLapack(sv, V);
2725
2726 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
2727
2728 return Ap;
2729}
2730
2777int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, int rank_in) const
2778{
2779 unsigned int nrows = getRows();
2780 unsigned int ncols = getCols();
2781 int rank_out;
2782 double svThreshold = 1e-26;
2783
2784 vpMatrix U, V;
2785 vpColVector sv;
2786
2787 Ap.resize(ncols, nrows, false, false);
2788
2789 if (nrows < ncols) {
2790 U.resize(ncols, ncols, true);
2791 sv.resize(nrows, false);
2792 }
2793 else {
2794 U.resize(nrows, ncols, false);
2795 sv.resize(ncols, false);
2796 }
2797
2798 U.insert(*this, 0, 0);
2799 U.svdLapack(sv, V);
2800
2801 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
2802
2803 return rank_out;
2804}
2805
2859int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in) const
2860{
2861 unsigned int nrows = getRows();
2862 unsigned int ncols = getCols();
2863 int rank_out;
2864 double svThreshold = 1e-26;
2865
2866 vpMatrix U, V;
2867 vpColVector sv_;
2868
2869 Ap.resize(ncols, nrows, false, false);
2870
2871 if (nrows < ncols) {
2872 U.resize(ncols, ncols, true);
2873 sv.resize(nrows, false);
2874 }
2875 else {
2876 U.resize(nrows, ncols, false);
2877 sv.resize(ncols, false);
2878 }
2879
2880 U.insert(*this, 0, 0);
2881 U.svdLapack(sv_, V);
2882
2883 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
2884
2885 // Remove singular values equal to the one that corresponds to the lines of 0
2886 // introduced when m < n
2887 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2888
2889 return rank_out;
2890}
2891
3003int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
3004 vpMatrix &kerAt) const
3005{
3006 unsigned int nrows = getRows();
3007 unsigned int ncols = getCols();
3008 int rank_out;
3009 double svThreshold = 1e-26;
3010 vpMatrix U, V;
3011 vpColVector sv_;
3012
3013 if (nrows < ncols) {
3014 U.resize(ncols, ncols, true);
3015 sv.resize(nrows, false);
3016 }
3017 else {
3018 U.resize(nrows, ncols, false);
3019 sv.resize(ncols, false);
3020 }
3021
3022 U.insert(*this, 0, 0);
3023 U.svdLapack(sv_, V);
3024
3025 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
3026
3027 // Remove singular values equal to the one that corresponds to the lines of 0
3028 // introduced when m < n
3029 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3030
3031 return rank_out;
3032}
3033
3034#endif
3035#if defined(VISP_HAVE_EIGEN3)
3072vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const
3073{
3074 unsigned int nrows = getRows();
3075 unsigned int ncols = getCols();
3076 int rank_out;
3077
3078 vpMatrix U, V, Ap;
3079 vpColVector sv;
3080
3081 Ap.resize(ncols, nrows, false, false);
3082
3083 if (nrows < ncols) {
3084 U.resize(ncols, ncols, true);
3085 sv.resize(nrows, false);
3086 }
3087 else {
3088 U.resize(nrows, ncols, false);
3089 sv.resize(ncols, false);
3090 }
3091
3092 U.insert(*this, 0, 0);
3093 U.svdEigen3(sv, V);
3094
3095 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3096
3097 return Ap;
3098}
3099
3140unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) const
3141{
3142 unsigned int nrows = getRows();
3143 unsigned int ncols = getCols();
3144 int rank_out;
3145
3146 vpMatrix U, V;
3147 vpColVector sv;
3148
3149 Ap.resize(ncols, nrows, false, false);
3150
3151 if (nrows < ncols) {
3152 U.resize(ncols, ncols, true);
3153 sv.resize(nrows, false);
3154 }
3155 else {
3156 U.resize(nrows, ncols, false);
3157 sv.resize(ncols, false);
3158 }
3159
3160 U.insert(*this, 0, 0);
3161 U.svdEigen3(sv, V);
3162
3163 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3164
3165 return static_cast<unsigned int>(rank_out);
3166}
3214unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3215{
3216 unsigned int nrows = getRows();
3217 unsigned int ncols = getCols();
3218 int rank_out;
3219
3220 vpMatrix U, V;
3221 vpColVector sv_;
3222
3223 Ap.resize(ncols, nrows, false, false);
3224
3225 if (nrows < ncols) {
3226 U.resize(ncols, ncols, true);
3227 sv.resize(nrows, false);
3228 }
3229 else {
3230 U.resize(nrows, ncols, false);
3231 sv.resize(ncols, false);
3232 }
3233
3234 U.insert(*this, 0, 0);
3235 U.svdEigen3(sv_, V);
3236
3237 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3238
3239 // Remove singular values equal to the one that corresponds to the lines of 0
3240 // introduced when m < n
3241 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3242
3243 return static_cast<unsigned int>(rank_out);
3244}
3245
3352unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3353 vpMatrix &imAt, vpMatrix &kerAt) const
3354{
3355 unsigned int nrows = getRows();
3356 unsigned int ncols = getCols();
3357 int rank_out;
3358 vpMatrix U, V;
3359 vpColVector sv_;
3360
3361 if (nrows < ncols) {
3362 U.resize(ncols, ncols, true);
3363 sv.resize(nrows, false);
3364 }
3365 else {
3366 U.resize(nrows, ncols, false);
3367 sv.resize(ncols, false);
3368 }
3369
3370 U.insert(*this, 0, 0);
3371 U.svdEigen3(sv_, V);
3372
3373 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerAt);
3374
3375 // Remove singular values equal to the one that corresponds to the lines of 0
3376 // introduced when m < n
3377 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3378
3379 return static_cast<unsigned int>(rank_out);
3380}
3381
3418vpMatrix vpMatrix::pseudoInverseEigen3(int rank_in) const
3419{
3420 unsigned int nrows = getRows();
3421 unsigned int ncols = getCols();
3422 int rank_out;
3423 double svThreshold = 1e-26;
3424
3425 vpMatrix U, V, Ap;
3426 vpColVector sv;
3427
3428 Ap.resize(ncols, nrows, false, false);
3429
3430 if (nrows < ncols) {
3431 U.resize(ncols, ncols, true);
3432 sv.resize(nrows, false);
3433 }
3434 else {
3435 U.resize(nrows, ncols, false);
3436 sv.resize(ncols, false);
3437 }
3438
3439 U.insert(*this, 0, 0);
3440 U.svdEigen3(sv, V);
3441
3442 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3443
3444 return Ap;
3445}
3446
3493int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, int rank_in) const
3494{
3495 unsigned int nrows = getRows();
3496 unsigned int ncols = getCols();
3497 int rank_out;
3498 double svThreshold = 1e-26;
3499
3500 vpMatrix U, V;
3501 vpColVector sv;
3502
3503 Ap.resize(ncols, nrows, false, false);
3504
3505 if (nrows < ncols) {
3506 U.resize(ncols, ncols, true);
3507 sv.resize(nrows, false);
3508 }
3509 else {
3510 U.resize(nrows, ncols, false);
3511 sv.resize(ncols, false);
3512 }
3513
3514 U.insert(*this, 0, 0);
3515 U.svdEigen3(sv, V);
3516
3517 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3518
3519 return rank_out;
3520}
3521
3575int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in) const
3576{
3577 unsigned int nrows = getRows();
3578 unsigned int ncols = getCols();
3579 int rank_out;
3580 double svThreshold = 1e-26;
3581
3582 vpMatrix U, V;
3583 vpColVector sv_;
3584
3585 Ap.resize(ncols, nrows, false, false);
3586
3587 if (nrows < ncols) {
3588 U.resize(ncols, ncols, true);
3589 sv.resize(nrows, false);
3590 }
3591 else {
3592 U.resize(nrows, ncols, false);
3593 sv.resize(ncols, false);
3594 }
3595
3596 U.insert(*this, 0, 0);
3597 U.svdEigen3(sv_, V);
3598
3599 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3600
3601 // Remove singular values equal to the one that corresponds to the lines of 0
3602 // introduced when m < n
3603 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3604
3605 return rank_out;
3606}
3607
3719int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
3720 vpMatrix &kerAt) const
3721{
3722 unsigned int nrows = getRows();
3723 unsigned int ncols = getCols();
3724 int rank_out;
3725 double svThreshold = 1e-26;
3726 vpMatrix U, V;
3727 vpColVector sv_;
3728
3729 if (nrows < ncols) {
3730 U.resize(ncols, ncols, true);
3731 sv.resize(nrows, false);
3732 }
3733 else {
3734 U.resize(nrows, ncols, false);
3735 sv.resize(ncols, false);
3736 }
3737
3738 U.insert(*this, 0, 0);
3739 U.svdEigen3(sv_, V);
3740
3741 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
3742
3743 // Remove singular values equal to the one that corresponds to the lines of 0
3744 // introduced when m < n
3745 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3746
3747 return rank_out;
3748}
3749
3750#endif
3751#if defined(VISP_HAVE_OPENCV)
3788vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const
3789{
3790 unsigned int nrows = getRows();
3791 unsigned int ncols = getCols();
3792 int rank_out;
3793
3794 vpMatrix U, V, Ap;
3795 vpColVector sv;
3796
3797 Ap.resize(ncols, nrows, false, false);
3798
3799 if (nrows < ncols) {
3800 U.resize(ncols, ncols, true);
3801 sv.resize(nrows, false);
3802 }
3803 else {
3804 U.resize(nrows, ncols, false);
3805 sv.resize(ncols, false);
3806 }
3807
3808 U.insert(*this, 0, 0);
3809 U.svdOpenCV(sv, V);
3810
3811 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3812
3813 return Ap;
3814}
3815
3856unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) const
3857{
3858 unsigned int nrows = getRows();
3859 unsigned int ncols = getCols();
3860 int rank_out;
3861
3862 vpMatrix U, V;
3863 vpColVector sv;
3864
3865 Ap.resize(ncols, nrows, false, false);
3866
3867 if (nrows < ncols) {
3868 U.resize(ncols, ncols, true);
3869 sv.resize(nrows, false);
3870 }
3871 else {
3872 U.resize(nrows, ncols, false);
3873 sv.resize(ncols, false);
3874 }
3875
3876 U.insert(*this, 0, 0);
3877 U.svdOpenCV(sv, V);
3878
3879 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3880
3881 return static_cast<unsigned int>(rank_out);
3882}
3930unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3931{
3932 unsigned int nrows = getRows();
3933 unsigned int ncols = getCols();
3934 int rank_out;
3935
3936 vpMatrix U, V;
3937 vpColVector sv_;
3938
3939 Ap.resize(ncols, nrows, false, false);
3940
3941 if (nrows < ncols) {
3942 U.resize(ncols, ncols, true);
3943 sv.resize(nrows, false);
3944 }
3945 else {
3946 U.resize(nrows, ncols, false);
3947 sv.resize(ncols, false);
3948 }
3949
3950 U.insert(*this, 0, 0);
3951 U.svdOpenCV(sv_, V);
3952
3953 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3954
3955 // Remove singular values equal to the one that corresponds to the lines of 0
3956 // introduced when m < n
3957 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3958
3959 return static_cast<unsigned int>(rank_out);
3960}
3961
4068unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
4069 vpMatrix &imAt, vpMatrix &kerAt) const
4070{
4071 unsigned int nrows = getRows();
4072 unsigned int ncols = getCols();
4073 int rank_out;
4074 vpMatrix U, V;
4075 vpColVector sv_;
4076
4077 if (nrows < ncols) {
4078 U.resize(ncols, ncols, true);
4079 sv.resize(nrows, false);
4080 }
4081 else {
4082 U.resize(nrows, ncols, false);
4083 sv.resize(ncols, false);
4084 }
4085
4086 U.insert(*this, 0, 0);
4087 U.svdOpenCV(sv_, V);
4088
4089 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerAt);
4090
4091 // Remove singular values equal to the one that corresponds to the lines of 0
4092 // introduced when m < n
4093 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4094
4095 return static_cast<unsigned int>(rank_out);
4096}
4097
4134vpMatrix vpMatrix::pseudoInverseOpenCV(int rank_in) const
4135{
4136 unsigned int nrows = getRows();
4137 unsigned int ncols = getCols();
4138 int rank_out;
4139 double svThreshold = 1e-26;
4140
4141 vpMatrix U, V, Ap;
4142 vpColVector sv;
4143
4144 Ap.resize(ncols, nrows, false, false);
4145
4146 if (nrows < ncols) {
4147 U.resize(ncols, ncols, true);
4148 sv.resize(nrows, false);
4149 }
4150 else {
4151 U.resize(nrows, ncols, false);
4152 sv.resize(ncols, false);
4153 }
4154
4155 U.insert(*this, 0, 0);
4156 U.svdOpenCV(sv, V);
4157
4158 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4159
4160 return Ap;
4161}
4162
4209int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, int rank_in) const
4210{
4211 unsigned int nrows = getRows();
4212 unsigned int ncols = getCols();
4213 int rank_out;
4214 double svThreshold = 1e-26;
4215
4216 vpMatrix U, V;
4217 vpColVector sv;
4218
4219 Ap.resize(ncols, nrows, false, false);
4220
4221 if (nrows < ncols) {
4222 U.resize(ncols, ncols, true);
4223 sv.resize(nrows, false);
4224 }
4225 else {
4226 U.resize(nrows, ncols, false);
4227 sv.resize(ncols, false);
4228 }
4229
4230 U.insert(*this, 0, 0);
4231 U.svdOpenCV(sv, V);
4232
4233 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4234
4235 return rank_out;
4236}
4237
4291int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4292{
4293 unsigned int nrows = getRows();
4294 unsigned int ncols = getCols();
4295 int rank_out;
4296 double svThreshold = 1e-26;
4297
4298 vpMatrix U, V;
4299 vpColVector sv_;
4300
4301 Ap.resize(ncols, nrows, false, false);
4302
4303 if (nrows < ncols) {
4304 U.resize(ncols, ncols, true);
4305 sv.resize(nrows, false);
4306 }
4307 else {
4308 U.resize(nrows, ncols, false);
4309 sv.resize(ncols, false);
4310 }
4311
4312 U.insert(*this, 0, 0);
4313 U.svdOpenCV(sv_, V);
4314
4315 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4316
4317 // Remove singular values equal to the one that corresponds to the lines of 0
4318 // introduced when m < n
4319 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4320
4321 return rank_out;
4322}
4323
4435int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
4436 vpMatrix &kerAt) const
4437{
4438 unsigned int nrows = getRows();
4439 unsigned int ncols = getCols();
4440 int rank_out;
4441 double svThreshold = 1e-26;
4442 vpMatrix U, V;
4443 vpColVector sv_;
4444
4445 if (nrows < ncols) {
4446 U.resize(ncols, ncols, true);
4447 sv.resize(nrows, false);
4448 }
4449 else {
4450 U.resize(nrows, ncols, false);
4451 sv.resize(ncols, false);
4452 }
4453
4454 U.insert(*this, 0, 0);
4455 U.svdOpenCV(sv_, V);
4456
4457 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
4458
4459 // Remove singular values equal to the one that corresponds to the lines of 0
4460 // introduced when m < n
4461 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4462
4463 return rank_out;
4464}
4465
4466#endif
4467#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
4468
4530unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
4531{
4532#if defined(VISP_HAVE_LAPACK)
4533 return pseudoInverseLapack(Ap, sv, svThreshold);
4534#elif defined(VISP_HAVE_EIGEN3)
4535 return pseudoInverseEigen3(Ap, sv, svThreshold);
4536#elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
4537 return pseudoInverseOpenCV(Ap, sv, svThreshold);
4538#else
4539 (void)Ap;
4540 (void)sv;
4541 (void)svThreshold;
4542 throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4543 "Install Lapack, Eigen3 or OpenCV 3rd party"));
4544#endif
4545}
4546
4613int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4614{
4615#if defined(VISP_HAVE_LAPACK)
4616 return pseudoInverseLapack(Ap, sv, rank_in);
4617#elif defined(VISP_HAVE_EIGEN3)
4618 return pseudoInverseEigen3(Ap, sv, rank_in);
4619#elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
4620 return pseudoInverseOpenCV(Ap, sv, rank_in);
4621#else
4622 (void)Ap;
4623 (void)sv;
4624 (void)svThreshold;
4625 throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4626 "Install Lapack, Eigen3 or OpenCV 3rd party"));
4627#endif
4628}
4703unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
4704 vpMatrix &imAt) const
4705{
4706 vpMatrix kerAt;
4707 return pseudoInverse(Ap, sv, svThreshold, imA, imAt, kerAt);
4708}
4709
4788int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt) const
4789{
4790 vpMatrix kerAt;
4791 return pseudoInverse(Ap, sv, rank_in, imA, imAt, kerAt);
4792}
4793
4928unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt,
4929 vpMatrix &kerAt) const
4930{
4931#if defined(VISP_HAVE_LAPACK)
4932 return pseudoInverseLapack(Ap, sv, svThreshold, imA, imAt, kerAt);
4933#elif defined(VISP_HAVE_EIGEN3)
4934 return pseudoInverseEigen3(Ap, sv, svThreshold, imA, imAt, kerAt);
4935#elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
4936 return pseudoInverseOpenCV(Ap, sv, svThreshold, imA, imAt, kerAt);
4937#else
4938 (void)Ap;
4939 (void)sv;
4940 (void)svThreshold;
4941 (void)imA;
4942 (void)imAt;
4943 (void)kerAt;
4944 throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4945 "Install Lapack, Eigen3 or OpenCV 3rd party"));
4946#endif
4947}
4948
5088int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
5089 vpMatrix &kerAt) const
5090{
5091#if defined(VISP_HAVE_LAPACK)
5092 return pseudoInverseLapack(Ap, sv, rank_in, imA, imAt, kerAt);
5093#elif defined(VISP_HAVE_EIGEN3)
5094 return pseudoInverseEigen3(Ap, sv, rank_in, imA, imAt, kerAt);
5095#elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
5096 return pseudoInverseOpenCV(Ap, sv, rank_in, imA, imAt, kerAt);
5097#else
5098 (void)Ap;
5099 (void)sv;
5100 (void)svThreshold;
5101 (void)imA;
5102 (void)imAt;
5103 (void)kerAt;
5104 throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
5105 "Install Lapack, Eigen3 or OpenCV 3rd party"));
5106#endif
5107}
5108
5150vpColVector vpMatrix::getCol(unsigned int j, unsigned int i_begin, unsigned int column_size) const
5151{
5152 if (i_begin + column_size > getRows() || j >= getCols())
5153 throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(),
5154 getCols()));
5155 vpColVector c(column_size);
5156 for (unsigned int i = 0; i < column_size; i++)
5157 c[i] = (*this)[i_begin + i][j];
5158 return c;
5159}
5160
5200vpColVector vpMatrix::getCol(unsigned int j) const { return getCol(j, 0, rowNum); }
5201
5237vpRowVector vpMatrix::getRow(unsigned int i) const { return getRow(i, 0, colNum); }
5238
5278vpRowVector vpMatrix::getRow(unsigned int i, unsigned int j_begin, unsigned int row_size) const
5279{
5280 if (j_begin + row_size > colNum || i >= rowNum)
5281 throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
5282
5283 vpRowVector r(row_size);
5284 if (r.data != NULL && data != NULL) {
5285 memcpy(r.data, (*this)[i] + j_begin, row_size * sizeof(double));
5286 }
5287
5288 return r;
5289}
5290
5327{
5328 unsigned int min_size = std::min(rowNum, colNum);
5330
5331 if (min_size > 0) {
5332 diag.resize(min_size, false);
5333
5334 for (unsigned int i = 0; i < min_size; i++) {
5335 diag[i] = (*this)[i][i];
5336 }
5337 }
5338
5339 return diag;
5340}
5341
5353{
5354 vpMatrix C;
5355
5356 vpMatrix::stack(A, B, C);
5357
5358 return C;
5359}
5360
5372void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
5373{
5374 unsigned int nra = A.getRows();
5375 unsigned int nrb = B.getRows();
5376
5377 if (nra != 0) {
5378 if (A.getCols() != B.getCols()) {
5379 throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5380 A.getCols(), B.getRows(), B.getCols()));
5381 }
5382 }
5383
5384 if (A.data != NULL && A.data == C.data) {
5385 std::cerr << "A and C must be two different objects!" << std::endl;
5386 return;
5387 }
5388
5389 if (B.data != NULL && B.data == C.data) {
5390 std::cerr << "B and C must be two different objects!" << std::endl;
5391 return;
5392 }
5393
5394 C.resize(nra + nrb, B.getCols(), false, false);
5395
5396 if (C.data != NULL && A.data != NULL && A.size() > 0) {
5397 // Copy A in C
5398 memcpy(C.data, A.data, sizeof(double) * A.size());
5399 }
5400
5401 if (C.data != NULL && B.data != NULL && B.size() > 0) {
5402 // Copy B in C
5403 memcpy(C.data + A.size(), B.data, sizeof(double) * B.size());
5404 }
5405}
5406
5417{
5418 vpMatrix C;
5419 vpMatrix::stack(A, r, C);
5420
5421 return C;
5422}
5423
5435void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
5436{
5437 if (A.data != NULL && A.data == C.data) {
5438 std::cerr << "A and C must be two different objects!" << std::endl;
5439 return;
5440 }
5441
5442 C = A;
5443 C.stack(r);
5444}
5445
5456{
5457 vpMatrix C;
5458 vpMatrix::stack(A, c, C);
5459
5460 return C;
5461}
5462
5474void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C)
5475{
5476 if (A.data != NULL && A.data == C.data) {
5477 std::cerr << "A and C must be two different objects!" << std::endl;
5478 return;
5479 }
5480
5481 C = A;
5482 C.stack(c);
5483}
5484
5497vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, unsigned int r, unsigned int c)
5498{
5500
5501 vpArray2D<double>::insert(A, B, C, r, c);
5502
5503 return vpMatrix(C);
5504}
5505
5519void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, unsigned int r, unsigned int c)
5520{
5521 vpArray2D<double> C_array;
5522
5523 vpArray2D<double>::insert(A, B, C_array, r, c);
5524
5525 C = C_array;
5526}
5527
5540{
5541 vpMatrix C;
5542
5543 juxtaposeMatrices(A, B, C);
5544
5545 return C;
5546}
5547
5561{
5562 unsigned int nca = A.getCols();
5563 unsigned int ncb = B.getCols();
5564
5565 if (nca != 0) {
5566 if (A.getRows() != B.getRows()) {
5567 throw(vpException(vpException::dimensionError, "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5568 A.getCols(), B.getRows(), B.getCols()));
5569 }
5570 }
5571
5572 if (B.getRows() == 0 || nca + ncb == 0) {
5573 std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl;
5574 return;
5575 }
5576
5577 C.resize(B.getRows(), nca + ncb, false, false);
5578
5579 C.insert(A, 0, 0);
5580 C.insert(B, 0, nca);
5581}
5582
5583//--------------------------------------------------------------------
5584// Output
5585//--------------------------------------------------------------------
5586
5606int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &intro) const
5607{
5608 typedef std::string::size_type size_type;
5609
5610 unsigned int m = getRows();
5611 unsigned int n = getCols();
5612
5613 std::vector<std::string> values(m * n);
5614 std::ostringstream oss;
5615 std::ostringstream ossFixed;
5616 std::ios_base::fmtflags original_flags = oss.flags();
5617
5618 ossFixed.setf(std::ios::fixed, std::ios::floatfield);
5619
5620 size_type maxBefore = 0; // the length of the integral part
5621 size_type maxAfter = 0; // number of decimals plus
5622 // one place for the decimal point
5623 for (unsigned int i = 0; i < m; ++i) {
5624 for (unsigned int j = 0; j < n; ++j) {
5625 oss.str("");
5626 oss << (*this)[i][j];
5627 if (oss.str().find("e") != std::string::npos) {
5628 ossFixed.str("");
5629 ossFixed << (*this)[i][j];
5630 oss.str(ossFixed.str());
5631 }
5632
5633 values[i * n + j] = oss.str();
5634 size_type thislen = values[i * n + j].size();
5635 size_type p = values[i * n + j].find('.');
5636
5637 if (p == std::string::npos) {
5638 maxBefore = vpMath::maximum(maxBefore, thislen);
5639 // maxAfter remains the same
5640 }
5641 else {
5642 maxBefore = vpMath::maximum(maxBefore, p);
5643 maxAfter = vpMath::maximum(maxAfter, thislen - p);
5644 }
5645 }
5646 }
5647
5648 size_type totalLength = length;
5649 // increase totalLength according to maxBefore
5650 totalLength = vpMath::maximum(totalLength, maxBefore);
5651 // decrease maxAfter according to totalLength
5652 maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
5653
5654 if (!intro.empty())
5655 s << intro;
5656 s << "[" << m << "," << n << "]=\n";
5657
5658 for (unsigned int i = 0; i < m; i++) {
5659 s << " ";
5660 for (unsigned int j = 0; j < n; j++) {
5661 size_type p = values[i * n + j].find('.');
5662 s.setf(std::ios::right, std::ios::adjustfield);
5663 s.width((std::streamsize)maxBefore);
5664 s << values[i * n + j].substr(0, p).c_str();
5665
5666 if (maxAfter > 0) {
5667 s.setf(std::ios::left, std::ios::adjustfield);
5668 if (p != std::string::npos) {
5669 s.width((std::streamsize)maxAfter);
5670 s << values[i * n + j].substr(p, maxAfter).c_str();
5671 }
5672 else {
5673 s.width((std::streamsize)maxAfter);
5674 s << ".0";
5675 }
5676 }
5677
5678 s << ' ';
5679 }
5680 s << std::endl;
5681 }
5682
5683 s.flags(original_flags); // restore s to standard state
5684
5685 return (int)(maxBefore + maxAfter);
5686}
5687
5724std::ostream &vpMatrix::matlabPrint(std::ostream &os) const
5725{
5726 os << "[ ";
5727 for (unsigned int i = 0; i < this->getRows(); ++i) {
5728 for (unsigned int j = 0; j < this->getCols(); ++j) {
5729 os << (*this)[i][j] << ", ";
5730 }
5731 if (this->getRows() != i + 1) {
5732 os << ";" << std::endl;
5733 }
5734 else {
5735 os << "]" << std::endl;
5736 }
5737 }
5738 return os;
5739}
5740
5769std::ostream &vpMatrix::maplePrint(std::ostream &os) const
5770{
5771 os << "([ " << std::endl;
5772 for (unsigned int i = 0; i < this->getRows(); ++i) {
5773 os << "[";
5774 for (unsigned int j = 0; j < this->getCols(); ++j) {
5775 os << (*this)[i][j] << ", ";
5776 }
5777 os << "]," << std::endl;
5778 }
5779 os << "])" << std::endl;
5780 return os;
5781}
5782
5810std::ostream &vpMatrix::csvPrint(std::ostream &os) const
5811{
5812 for (unsigned int i = 0; i < this->getRows(); ++i) {
5813 for (unsigned int j = 0; j < this->getCols(); ++j) {
5814 os << (*this)[i][j];
5815 if (!(j == (this->getCols() - 1)))
5816 os << ", ";
5817 }
5818 os << std::endl;
5819 }
5820 return os;
5821}
5822
5859std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
5860{
5861 os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " << this->getCols() << "); " << std::endl;
5862
5863 for (unsigned int i = 0; i < this->getRows(); ++i) {
5864 for (unsigned int j = 0; j < this->getCols(); ++j) {
5865 if (!octet) {
5866 os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl;
5867 }
5868 else {
5869 for (unsigned int k = 0; k < sizeof(double); ++k) {
5870 os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex
5871 << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl;
5872 }
5873 }
5874 }
5875 os << std::endl;
5876 }
5877 return os;
5878}
5879
5885{
5886 if (rowNum == 0) {
5887 *this = A;
5888 }
5889 else if (A.getRows() > 0) {
5890 if (colNum != A.getCols()) {
5891 throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum,
5892 A.getRows(), A.getCols()));
5893 }
5894
5895 unsigned int rowNumOld = rowNum;
5896 resize(rowNum + A.getRows(), colNum, false, false);
5897 insert(A, rowNumOld, 0);
5898 }
5899}
5900
5917{
5918 if (rowNum == 0) {
5919 *this = r;
5920 }
5921 else {
5922 if (colNum != r.getCols()) {
5923 throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum,
5924 colNum, r.getCols()));
5925 }
5926
5927 if (r.size() == 0) {
5928 return;
5929 }
5930
5931 unsigned int oldSize = size();
5932 resize(rowNum + 1, colNum, false, false);
5933
5934 if (data != NULL && r.data != NULL && data != r.data) {
5935 // Copy r in data
5936 memcpy(data + oldSize, r.data, sizeof(double) * r.size());
5937 }
5938 }
5939}
5940
5958{
5959 if (colNum == 0) {
5960 *this = c;
5961 }
5962 else {
5963 if (rowNum != c.getRows()) {
5964 throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum,
5965 colNum, c.getRows()));
5966 }
5967
5968 if (c.size() == 0) {
5969 return;
5970 }
5971
5972 vpMatrix tmp = *this;
5973 unsigned int oldColNum = colNum;
5974 resize(rowNum, colNum + 1, false, false);
5975
5976 if (data != NULL && tmp.data != NULL && data != tmp.data) {
5977 // Copy c in data
5978 for (unsigned int i = 0; i < rowNum; i++) {
5979 memcpy(data + i * colNum, tmp.data + i * oldColNum, sizeof(double) * oldColNum);
5980 rowPtrs[i][oldColNum] = c[i];
5981 }
5982 }
5983 }
5984}
5985
5996void vpMatrix::insert(const vpMatrix &A, unsigned int r, unsigned int c)
5997{
5998 if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) {
5999 if (A.colNum == colNum && data != NULL && A.data != NULL && A.data != data) {
6000 memcpy(data + r * colNum, A.data, sizeof(double) * A.size());
6001 }
6002 else if (data != NULL && A.data != NULL && A.data != data) {
6003 for (unsigned int i = r; i < (r + A.getRows()); i++) {
6004 memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum);
6005 }
6006 }
6007 }
6008 else {
6009 throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
6010 A.getRows(), A.getCols(), rowNum, colNum, r, c);
6011 }
6012}
6013
6051{
6052 vpColVector evalue(rowNum); // Eigen values
6053
6054 if (rowNum != colNum) {
6055 throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
6056 colNum));
6057 }
6058
6059 // Check if the matrix is symmetric: At - A = 0
6060 vpMatrix At_A = (*this).t() - (*this);
6061 for (unsigned int i = 0; i < rowNum; i++) {
6062 for (unsigned int j = 0; j < rowNum; j++) {
6063 // if (At_A[i][j] != 0) {
6064 if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
6065 throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix"));
6066 }
6067 }
6068 }
6069
6070#if defined(VISP_HAVE_LAPACK)
6071#if defined(VISP_HAVE_GSL) /* be careful of the copy below */
6072 {
6073 gsl_vector *eval = gsl_vector_alloc(rowNum);
6074 gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
6075
6076 gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
6077 gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
6078
6079 unsigned int Atda = (unsigned int)m->tda;
6080 for (unsigned int i = 0; i < rowNum; i++) {
6081 unsigned int k = i * Atda;
6082 for (unsigned int j = 0; j < colNum; j++)
6083 m->data[k + j] = (*this)[i][j];
6084 }
6085 gsl_eigen_symmv(m, eval, evec, w);
6086
6087 gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
6088
6089 for (unsigned int i = 0; i < rowNum; i++) {
6090 evalue[i] = gsl_vector_get(eval, i);
6091 }
6092
6093 gsl_eigen_symmv_free(w);
6094 gsl_vector_free(eval);
6095 gsl_matrix_free(m);
6096 gsl_matrix_free(evec);
6097 }
6098#else
6099 {
6100 const char jobz = 'N';
6101 const char uplo = 'U';
6102 vpMatrix A = (*this);
6103 vpColVector WORK;
6104 int lwork = -1;
6105 int info = 0;
6106 double wkopt;
6107 vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
6108 lwork = static_cast<int>(wkopt);
6109 WORK.resize(lwork);
6110 vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
6111 }
6112#endif
6113#else
6114 {
6115 throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. "
6116 "You should install Lapack 3rd party"));
6117 }
6118#endif
6119 return evalue;
6120}
6121
6171void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const
6172{
6173 if (rowNum != colNum) {
6174 throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
6175 colNum));
6176 }
6177
6178 // Check if the matrix is symmetric: At - A = 0
6179 vpMatrix At_A = (*this).t() - (*this);
6180 for (unsigned int i = 0; i < rowNum; i++) {
6181 for (unsigned int j = 0; j < rowNum; j++) {
6182 // if (At_A[i][j] != 0) {
6183 if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
6184 throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix"));
6185 }
6186 }
6187 }
6188
6189 // Resize the output matrices
6190 evalue.resize(rowNum);
6191 evector.resize(rowNum, colNum);
6192
6193#if defined(VISP_HAVE_LAPACK)
6194#if defined(VISP_HAVE_GSL) /* be careful of the copy below */
6195 {
6196 gsl_vector *eval = gsl_vector_alloc(rowNum);
6197 gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
6198
6199 gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
6200 gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
6201
6202 unsigned int Atda = (unsigned int)m->tda;
6203 for (unsigned int i = 0; i < rowNum; i++) {
6204 unsigned int k = i * Atda;
6205 for (unsigned int j = 0; j < colNum; j++)
6206 m->data[k + j] = (*this)[i][j];
6207 }
6208 gsl_eigen_symmv(m, eval, evec, w);
6209
6210 gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
6211
6212 for (unsigned int i = 0; i < rowNum; i++) {
6213 evalue[i] = gsl_vector_get(eval, i);
6214 }
6215 Atda = (unsigned int)evec->tda;
6216 for (unsigned int i = 0; i < rowNum; i++) {
6217 unsigned int k = i * Atda;
6218 for (unsigned int j = 0; j < rowNum; j++) {
6219 evector[i][j] = evec->data[k + j];
6220 }
6221 }
6222
6223 gsl_eigen_symmv_free(w);
6224 gsl_vector_free(eval);
6225 gsl_matrix_free(m);
6226 gsl_matrix_free(evec);
6227 }
6228#else // defined(VISP_HAVE_GSL)
6229 {
6230 const char jobz = 'V';
6231 const char uplo = 'U';
6232 vpMatrix A = (*this);
6233 vpColVector WORK;
6234 int lwork = -1;
6235 int info = 0;
6236 double wkopt;
6237 vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
6238 lwork = static_cast<int>(wkopt);
6239 WORK.resize(lwork);
6240 vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
6241 evector = A.t();
6242 }
6243#endif // defined(VISP_HAVE_GSL)
6244#else
6245 {
6246 throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. "
6247 "You should install Lapack 3rd party"));
6248 }
6249#endif
6250}
6251
6270unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const
6271{
6272 unsigned int nbline = getRows();
6273 unsigned int nbcol = getCols();
6274
6275 vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6276 vpColVector sv;
6277 sv.resize(nbcol, false); // singular values
6278 V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6279
6280 // Copy and resize matrix to have at least as many rows as columns
6281 // kernel is computed in svd method only if the matrix has more rows than
6282 // columns
6283
6284 if (nbline < nbcol)
6285 U.resize(nbcol, nbcol, true);
6286 else
6287 U.resize(nbline, nbcol, false);
6288
6289 U.insert(*this, 0, 0);
6290
6291 U.svd(sv, V);
6292
6293 // Compute the highest singular value and rank of the matrix
6294 double maxsv = 0;
6295 for (unsigned int i = 0; i < nbcol; i++) {
6296 if (sv[i] > maxsv) {
6297 maxsv = sv[i];
6298 }
6299 }
6300
6301 unsigned int rank = 0;
6302 for (unsigned int i = 0; i < nbcol; i++) {
6303 if (sv[i] > maxsv * svThreshold) {
6304 rank++;
6305 }
6306 }
6307
6308 kerAt.resize(nbcol - rank, nbcol);
6309 if (rank != nbcol) {
6310 for (unsigned int j = 0, k = 0; j < nbcol; j++) {
6311 // if( v.col(j) in kernel and non zero )
6312 if ((sv[j] <= maxsv * svThreshold) &&
6313 (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
6314 for (unsigned int i = 0; i < V.getRows(); i++) {
6315 kerAt[k][i] = V[i][j];
6316 }
6317 k++;
6318 }
6319 }
6320 }
6321
6322 return rank;
6323}
6324
6341unsigned int vpMatrix::nullSpace(vpMatrix &kerA, double svThreshold) const
6342{
6343 unsigned int nbrow = getRows();
6344 unsigned int nbcol = getCols();
6345
6346 vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6347 vpColVector sv;
6348 sv.resize(nbcol, false); // singular values
6349 V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6350
6351 // Copy and resize matrix to have at least as many rows as columns
6352 // kernel is computed in svd method only if the matrix has more rows than
6353 // columns
6354
6355 if (nbrow < nbcol)
6356 U.resize(nbcol, nbcol, true);
6357 else
6358 U.resize(nbrow, nbcol, false);
6359
6360 U.insert(*this, 0, 0);
6361
6362 U.svd(sv, V);
6363
6364 // Compute the highest singular value and rank of the matrix
6365 double maxsv = sv[0];
6366
6367 unsigned int rank = 0;
6368 for (unsigned int i = 0; i < nbcol; i++) {
6369 if (sv[i] > maxsv * svThreshold) {
6370 rank++;
6371 }
6372 }
6373
6374 kerA.resize(nbcol, nbcol - rank);
6375 if (rank != nbcol) {
6376 for (unsigned int j = 0, k = 0; j < nbcol; j++) {
6377 // if( v.col(j) in kernel and non zero )
6378 if (sv[j] <= maxsv * svThreshold) {
6379 for (unsigned int i = 0; i < nbcol; i++) {
6380 kerA[i][k] = V[i][j];
6381 }
6382 k++;
6383 }
6384 }
6385 }
6386
6387 return (nbcol - rank);
6388}
6389
6406unsigned int vpMatrix::nullSpace(vpMatrix &kerA, int dim) const
6407{
6408 unsigned int nbrow = getRows();
6409 unsigned int nbcol = getCols();
6410 unsigned int dim_ = static_cast<unsigned int>(dim);
6411
6412 vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6413 vpColVector sv;
6414 sv.resize(nbcol, false); // singular values
6415 V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6416
6417 // Copy and resize matrix to have at least as many rows as columns
6418 // kernel is computed in svd method only if the matrix has more rows than
6419 // columns
6420
6421 if (nbrow < nbcol)
6422 U.resize(nbcol, nbcol, true);
6423 else
6424 U.resize(nbrow, nbcol, false);
6425
6426 U.insert(*this, 0, 0);
6427
6428 U.svd(sv, V);
6429
6430 kerA.resize(nbcol, dim_);
6431 if (dim_ != 0) {
6432 unsigned int rank = nbcol - dim_;
6433 for (unsigned int k = 0; k < dim_; k++) {
6434 unsigned int j = k + rank;
6435 for (unsigned int i = 0; i < nbcol; i++) {
6436 kerA[i][k] = V[i][j];
6437 }
6438 }
6439 }
6440
6441 double maxsv = sv[0];
6442 unsigned int rank = 0;
6443 for (unsigned int i = 0; i < nbcol; i++) {
6444 if (sv[i] > maxsv * 1e-6) {
6445 rank++;
6446 }
6447 }
6448 return (nbcol - rank);
6449}
6450
6482double vpMatrix::det(vpDetMethod method) const
6483{
6484 double det = 0.;
6485
6486 if (method == LU_DECOMPOSITION) {
6487 det = this->detByLU();
6488 }
6489
6490 return (det);
6491}
6492
6501{
6502 if (colNum != rowNum) {
6503 throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix",
6504 rowNum, colNum));
6505 }
6506 else {
6507#ifdef VISP_HAVE_GSL
6508 size_t size_ = rowNum * colNum;
6509 double *b = new double[size_];
6510 for (size_t i = 0; i < size_; i++)
6511 b[i] = 0.;
6512 gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum);
6513 gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum);
6514 gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
6515 // gsl_matrix_fprintf(stdout, &em.matrix, "%g");
6516 vpMatrix expA;
6517 expA.resize(rowNum, colNum, false);
6518 memcpy(expA.data, b, size_ * sizeof(double));
6519
6520 delete[] b;
6521 return expA;
6522#else
6523 vpMatrix _expE(rowNum, colNum, false);
6524 vpMatrix _expD(rowNum, colNum, false);
6525 vpMatrix _expX(rowNum, colNum, false);
6526 vpMatrix _expcX(rowNum, colNum, false);
6527 vpMatrix _eye(rowNum, colNum, false);
6528
6529 _eye.eye();
6530 vpMatrix exp(*this);
6531
6532 // double f;
6533 int e;
6534 double c = 0.5;
6535 int q = 6;
6536 int p = 1;
6537
6538 double nA = 0;
6539 for (unsigned int i = 0; i < rowNum; i++) {
6540 double sum = 0;
6541 for (unsigned int j = 0; j < colNum; j++) {
6542 sum += fabs((*this)[i][j]);
6543 }
6544 if (sum > nA || i == 0) {
6545 nA = sum;
6546 }
6547 }
6548
6549 /* f = */ frexp(nA, &e);
6550 // double s = (0 > e+1)?0:e+1;
6551 double s = e + 1;
6552
6553 double sca = 1.0 / pow(2.0, s);
6554 exp = sca * exp;
6555 _expX = *this;
6556 _expE = c * exp + _eye;
6557 _expD = -c * exp + _eye;
6558 for (int k = 2; k <= q; k++) {
6559 c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1)));
6560 _expcX = exp * _expX;
6561 _expX = _expcX;
6562 _expcX = c * _expX;
6563 _expE = _expE + _expcX;
6564 if (p)
6565 _expD = _expD + _expcX;
6566 else
6567 _expD = _expD - _expcX;
6568 p = !p;
6569 }
6570 _expX = _expD.inverseByLU();
6571 exp = _expX * _expE;
6572 for (int k = 1; k <= s; k++) {
6573 _expE = exp * exp;
6574 exp = _expE;
6575 }
6576 return exp;
6577#endif
6578 }
6579}
6580
6581/**************************************************************************************************************/
6582/**************************************************************************************************************/
6583
6584// Specific functions
6585
6586/*
6587input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.
6588
6589output:: the complement matrix of the element (rowNo,colNo).
6590This is the matrix obtained from M after elimenating the row rowNo and column
6591colNo
6592
6593example:
65941 2 3
6595M = 4 5 6
65967 8 9
65971 3
6598subblock(M, 1, 1) give the matrix 7 9
6599*/
6600vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row)
6601{
6602 vpMatrix M_comp;
6603 M_comp.resize(M.getRows() - 1, M.getCols() - 1, false);
6604
6605 for (unsigned int i = 0; i < col; i++) {
6606 for (unsigned int j = 0; j < row; j++)
6607 M_comp[i][j] = M[i][j];
6608 for (unsigned int j = row + 1; j < M.getRows(); j++)
6609 M_comp[i][j - 1] = M[i][j];
6610 }
6611 for (unsigned int i = col + 1; i < M.getCols(); i++) {
6612 for (unsigned int j = 0; j < row; j++)
6613 M_comp[i - 1][j] = M[i][j];
6614 for (unsigned int j = row + 1; j < M.getRows(); j++)
6615 M_comp[i - 1][j - 1] = M[i][j];
6616 }
6617 return M_comp;
6618}
6619
6629double vpMatrix::cond(double svThreshold) const
6630{
6631 unsigned int nbline = getRows();
6632 unsigned int nbcol = getCols();
6633
6634 vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6635 vpColVector sv;
6636 sv.resize(nbcol); // singular values
6637 V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6638
6639 // Copy and resize matrix to have at least as many rows as columns
6640 // kernel is computed in svd method only if the matrix has more rows than
6641 // columns
6642
6643 if (nbline < nbcol)
6644 U.resize(nbcol, nbcol, true);
6645 else
6646 U.resize(nbline, nbcol, false);
6647
6648 U.insert(*this, 0, 0);
6649
6650 U.svd(sv, V);
6651
6652 // Compute the highest singular value
6653 double maxsv = 0;
6654 for (unsigned int i = 0; i < nbcol; i++) {
6655 if (sv[i] > maxsv) {
6656 maxsv = sv[i];
6657 }
6658 }
6659
6660 // Compute the rank of the matrix
6661 unsigned int rank = 0;
6662 for (unsigned int i = 0; i < nbcol; i++) {
6663 if (sv[i] > maxsv * svThreshold) {
6664 rank++;
6665 }
6666 }
6667
6668 // Compute the lowest singular value
6669 double minsv = maxsv;
6670 for (unsigned int i = 0; i < rank; i++) {
6671 if (sv[i] < minsv) {
6672 minsv = sv[i];
6673 }
6674 }
6675
6676 if (std::fabs(minsv) > std::numeric_limits<double>::epsilon()) {
6677 return maxsv / minsv;
6678 }
6679 else {
6680 return std::numeric_limits<double>::infinity();
6681 }
6682}
6683
6690void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
6691{
6692 if (H.getCols() != H.getRows()) {
6693 throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(),
6694 H.getCols()));
6695 }
6696
6697 HLM = H;
6698 for (unsigned int i = 0; i < H.getCols(); i++) {
6699 HLM[i][i] += alpha * H[i][i];
6700 }
6701}
6702
6711{
6712 double norm = 0.0;
6713 for (unsigned int i = 0; i < dsize; i++) {
6714 double x = *(data + i);
6715 norm += x * x;
6716 }
6717
6718 return sqrt(norm);
6719}
6720
6730{
6731 if (this->dsize != 0) {
6732 vpMatrix v;
6733 vpColVector w;
6734
6735 vpMatrix M = *this;
6736
6737 M.svd(w, v);
6738
6739 double max = w[0];
6740 unsigned int maxRank = std::min(this->getCols(), this->getRows());
6741 // The maximum reachable rank is either the number of columns or the number of rows
6742 // of the matrix.
6743 unsigned int boundary = std::min(maxRank, w.size());
6744 // boundary is here to ensure that the number of singular values used for the com-
6745 // putation of the euclidean norm of the matrix is not greater than the maximum
6746 // reachable rank. Indeed, some svd library pad the singular values vector with 0s
6747 // if the input matrix is non-square.
6748 for (unsigned int i = 0; i < boundary; i++) {
6749 if (max < w[i]) {
6750 max = w[i];
6751 }
6752 }
6753 return max;
6754 }
6755 else {
6756 return 0.;
6757 }
6758}
6759
6771{
6772 double norm = 0.0;
6773 for (unsigned int i = 0; i < rowNum; i++) {
6774 double x = 0;
6775 for (unsigned int j = 0; j < colNum; j++) {
6776 x += fabs(*(*(rowPtrs + i) + j));
6777 }
6778 if (x > norm) {
6779 norm = x;
6780 }
6781 }
6782 return norm;
6783}
6784
6792{
6793 double sum_square = 0.0;
6794 double x;
6795
6796 for (unsigned int i = 0; i < rowNum; i++) {
6797 for (unsigned int j = 0; j < colNum; j++) {
6798 x = rowPtrs[i][j];
6799 sum_square += x * x;
6800 }
6801 }
6802
6803 return sum_square;
6804}
6805#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
6815double vpMatrix::euclideanNorm() const { return frobeniusNorm(); }
6816
6818{
6819 return (vpMatrix)(vpColVector::stack(A, B));
6820}
6821
6823{
6824 vpColVector::stack(A, B, C);
6825}
6826
6828
6829void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); }
6830
6850{
6851 vpRowVector c(getCols());
6852
6853 for (unsigned int j = 0; j < getCols(); j++)
6854 c[j] = (*this)[i - 1][j];
6855 return c;
6856}
6857
6876{
6877 vpColVector c(getRows());
6878
6879 for (unsigned int i = 0; i < getRows(); i++)
6880 c[i] = (*this)[i][j - 1];
6881 return c;
6882}
6883
6890void vpMatrix::setIdentity(const double &val)
6891{
6892 for (unsigned int i = 0; i < rowNum; i++)
6893 for (unsigned int j = 0; j < colNum; j++)
6894 if (i == j)
6895 (*this)[i][j] = val;
6896 else
6897 (*this)[i][j] = 0;
6898}
6899
6900#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition vpArray2D.h:131
unsigned int getCols() const
Definition vpArray2D.h:280
void insert(const vpArray2D< Type > &A, unsigned int r, unsigned int c)
Definition vpArray2D.h:417
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int rowNum
Number of rows in the array.
Definition vpArray2D.h:134
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< double > &A)
Definition vpArray2D.h:529
unsigned int dsize
Definition vpArray2D.h:140
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
unsigned int getRows() const
Definition vpArray2D.h:290
unsigned int colNum
Number of columns in the array.
Definition vpArray2D.h:136
Implementation of column vector and the associated operations.
double sumSquare() const
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ functionNotImplementedError
Function not implemented.
Definition vpException.h:78
@ dimensionError
Bad dimension.
Definition vpException.h:83
@ fatalError
Fatal error.
Definition vpException.h:84
@ divideByZeroError
Division by zero.
Definition vpException.h:82
Implementation of an homogeneous matrix and operations on such kind of matrices.
static Type maximum(const Type &a, const Type &b)
Definition vpMath.h:172
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void svdLapack(vpColVector &w, vpMatrix &V)
vpColVector eigenValues() const
vpMatrix pseudoInverseOpenCV(double svThreshold=1e-6) const
std::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
double cond(double svThreshold=1e-6) const
vpMatrix expm() const
vpMatrix pseudoInverseEigen3(double svThreshold=1e-6) const
vpMatrix hadamard(const vpMatrix &m) const
vpMatrix operator-() const
vp_deprecated void setIdentity(const double &val=1.0)
vpMatrix & operator/=(double x)
Divide all the element of the matrix by x : Aij = Aij / x.
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
std::ostream & maplePrint(std::ostream &os) const
void eye()
Definition vpMatrix.cpp:446
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
vpMatrix inverseByLU() const
vpMatrix operator*(const vpMatrix &B) const
vpMatrix operator/(double x) const
Cij = Aij / x (A is unchanged)
void stack(const vpMatrix &A)
vp_deprecated void stackMatrices(const vpMatrix &A)
Definition vpMatrix.h:1009
vpMatrix & operator-=(const vpMatrix &B)
Operation A = A - B.
vpMatrix & operator*=(double x)
Multiply all the element of the matrix by x : Aij = Aij * x.
vpMatrix operator*(const double &x, const vpMatrix &B)
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
vp_deprecated double euclideanNorm() const
vpColVector stackColumns()
vp_deprecated vpColVector column(unsigned int j)
static void sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
void svd(vpColVector &w, vpMatrix &V)
vpRowVector stackRows()
vpMatrix operator+(const vpMatrix &B) const
double sum() const
void diag(const double &val=1.0)
Definition vpMatrix.cpp:881
vpRowVector getRow(unsigned int i) const
void svdOpenCV(vpColVector &w, vpMatrix &V)
vpMatrix t() const
Definition vpMatrix.cpp:461
vpMatrix & operator+=(const vpMatrix &B)
Operation A = A + B.
static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C)
double inducedL2Norm() const
double infinityNorm() const
vpMatrix & operator=(const vpArray2D< double > &A)
Definition vpMatrix.cpp:650
double frobeniusNorm() const
vpColVector getDiag() const
vpMatrix AtA() const
Definition vpMatrix.cpp:625
static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
Definition vpMatrix.cpp:900
void solveBySVD(const vpColVector &B, vpColVector &x) const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
vpMatrix pseudoInverseLapack(double svThreshold=1e-6) const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
vpMatrix & operator,(double val)
Definition vpMatrix.cpp:799
static void multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
Definition vpMatrix.cpp:953
vp_deprecated vpRowVector row(unsigned int i)
vpColVector getCol(unsigned int j) const
double detByLU() const
double det(vpDetMethod method=LU_DECOMPOSITION) const
double sumSquare() const
vp_deprecated void init()
Definition vpMatrix.h:1004
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
void svdEigen3(vpColVector &w, vpMatrix &V)
void kron(const vpMatrix &m1, vpMatrix &out) const
vpMatrix AAt() const
Definition vpMatrix.cpp:501
vpMatrix transpose() const
Definition vpMatrix.cpp:468
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
static void negateMatrix(const vpMatrix &A, vpMatrix &C)
@ LU_DECOMPOSITION
Definition vpMatrix.h:160
std::ostream & csvPrint(std::ostream &os) const
unsigned int nullSpace(vpMatrix &kerA, double svThreshold=1e-6) const
std::ostream & matlabPrint(std::ostream &os) const
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition vpMatrix.cpp:404
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
Class that consider the case of a translation vector.