Visual Servoing Platform version 3.5.0
vpMatrix.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Matrix manipulation.
33 *
34 * Authors:
35 * Eric Marchand
36 *
37 *****************************************************************************/
43#include <algorithm>
44#include <assert.h>
45#include <cmath> // std::fabs
46#include <fstream>
47#include <limits> // numeric_limits
48#include <sstream>
49#include <stdio.h>
50#include <stdlib.h>
51#include <string.h>
52#include <string>
53#include <vector>
54
55#include <visp3/core/vpConfig.h>
56#include <visp3/core/vpCPUFeatures.h>
57#include <visp3/core/vpColVector.h>
58#include <visp3/core/vpDebug.h>
59#include <visp3/core/vpException.h>
60#include <visp3/core/vpMath.h>
61#include <visp3/core/vpMatrix.h>
62#include <visp3/core/vpTranslationVector.h>
63
64#include <Simd/SimdLib.hpp>
65
66#ifdef VISP_HAVE_LAPACK
67# ifdef VISP_HAVE_GSL
68# include <gsl/gsl_eigen.h>
69# include <gsl/gsl_math.h>
70# include <gsl/gsl_linalg.h>
71# elif defined(VISP_HAVE_MKL)
72# include <mkl.h>
73typedef MKL_INT integer;
74
75void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_,
76 double *w_data, double *work_data, int lwork_, int &info_)
77{
78 MKL_INT n = static_cast<MKL_INT>(n_);
79 MKL_INT lda = static_cast<MKL_INT>(lda_);
80 MKL_INT lwork = static_cast<MKL_INT>(lwork_);
81 MKL_INT info = static_cast<MKL_INT>(info_);
82
83 dsyev(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
84}
85
86# else
87# if defined(VISP_HAVE_LAPACK_BUILT_IN)
88typedef long int integer;
89# else
90typedef int integer;
91# endif
92extern "C" integer dsyev_(char *jobz, char *uplo, integer *n, double *a, integer *lda,
93 double *w, double *WORK, integer *lwork, integer *info);
94
95void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_,
96 double *w_data, double *work_data, int lwork_, int &info_)
97{
98 integer n = static_cast<integer>(n_);
99 integer lda = static_cast<integer>(lda_);
100 integer lwork = static_cast<integer>(lwork_);
101 integer info = static_cast<integer>(info_);
102
103 dsyev_(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
104
105 lwork_ = static_cast<int>(lwork);
106 info_ = static_cast<int>(info);
107}
108# endif
109#endif
110
111#if !defined(VISP_USE_MSVC) || (defined(VISP_USE_MSVC) && !defined(VISP_BUILD_SHARED_LIBS))
112const unsigned int vpMatrix::m_lapack_min_size_default = 0;
113unsigned int vpMatrix::m_lapack_min_size = vpMatrix::m_lapack_min_size_default;
114#endif
115
116// Prototypes of specific functions
117vpMatrix subblock(const vpMatrix &, unsigned int, unsigned int);
118
119void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows,
120 unsigned int ncols, double svThreshold, vpMatrix &Ap, int &rank_out,
121 int *rank_in, vpMatrix *imA, vpMatrix *imAt, vpMatrix *kerAt)
122{
123 Ap.resize(ncols, nrows, true, false);
124
125 // compute the highest singular value and the rank of h
126 double maxsv = sv[0];
127
128 rank_out = 0;
129
130 for (unsigned int i = 0; i < ncols; i++) {
131 if (sv[i] > maxsv * svThreshold) {
132 rank_out++;
133 }
134 }
135
136 unsigned int rank = static_cast<unsigned int>(rank_out);
137 if (rank_in) {
138 rank = static_cast<unsigned int>(*rank_in);
139 }
140
141 for (unsigned int i = 0; i < ncols; i++) {
142 for (unsigned int j = 0; j < nrows; j++) {
143 for (unsigned int k = 0; k < rank; k++) {
144 Ap[i][j] += V[i][k] * U[j][k] / sv[k];
145 }
146 }
147 }
148
149 // Compute im(A)
150 if (imA) {
151 imA->resize(nrows, rank);
152
153 for (unsigned int i = 0; i < nrows; i++) {
154 for (unsigned int j = 0; j < rank; j++) {
155 (*imA)[i][j] = U[i][j];
156 }
157 }
158 }
159
160 // Compute im(At)
161 if (imAt) {
162 imAt->resize(ncols, rank);
163 for (unsigned int i = 0; i < ncols; i++) {
164 for (unsigned int j = 0; j < rank; j++) {
165 (*imAt)[i][j] = V[i][j];
166 }
167 }
168 }
169
170 // Compute ker(At)
171 if (kerAt) {
172 kerAt->resize(ncols - rank, ncols);
173 if (rank != ncols) {
174 for (unsigned int k = 0; k < (ncols - rank); k++) {
175 unsigned j = k + rank;
176 for (unsigned int i = 0; i < V.getRows(); i++) {
177 (*kerAt)[k][i] = V[i][j];
178 }
179 }
180 }
181 }
182}
183
189vpMatrix::vpMatrix(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
190 : vpArray2D<double>()
191{
192 if (((r + nrows) > M.rowNum) || ((c + ncols) > M.colNum)) {
194 "Cannot construct a sub matrix (%dx%d) starting at "
195 "position (%d,%d) that is not contained in the "
196 "original matrix (%dx%d)",
197 nrows, ncols, r, c, M.rowNum, M.colNum));
198 }
199
200 init(M, r, c, nrows, ncols);
201}
202
203#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
205{
206 rowNum = A.rowNum;
207 colNum = A.colNum;
208 rowPtrs = A.rowPtrs;
209 dsize = A.dsize;
210 data = A.data;
211
212 A.rowNum = 0;
213 A.colNum = 0;
214 A.rowPtrs = NULL;
215 A.dsize = 0;
216 A.data = NULL;
217}
218
244vpMatrix::vpMatrix(const std::initializer_list<double> &list) : vpArray2D<double>(list) { }
245
246
271vpMatrix::vpMatrix(unsigned int nrows, unsigned int ncols, const std::initializer_list<double> &list)
272 : vpArray2D<double>(nrows, ncols, list) {}
273
296vpMatrix::vpMatrix(const std::initializer_list<std::initializer_list<double> > &lists) : vpArray2D<double>(lists) { }
297
298#endif
299
346void vpMatrix::init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
347{
348 unsigned int rnrows = r + nrows;
349 unsigned int cncols = c + ncols;
350
351 if (rnrows > M.getRows())
352 throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
353 M.getRows()));
354 if (cncols > M.getCols())
355 throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
356 M.getCols()));
357 resize(nrows, ncols, false, false);
358
359 if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
360 return; // Noting to do
361 for (unsigned int i = 0; i < nrows; i++) {
362 memcpy((*this)[i], &M[i + r][c], ncols * sizeof(double));
363 }
364}
365
407vpMatrix vpMatrix::extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
408{
409 unsigned int rnrows = r + nrows;
410 unsigned int cncols = c + ncols;
411
412 if (rnrows > getRows())
413 throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
414 getRows()));
415 if (cncols > getCols())
416 throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
417 getCols()));
418
419 vpMatrix M;
420 M.resize(nrows, ncols, false, false);
421 for (unsigned int i = 0; i < nrows; i++) {
422 memcpy(M[i], &(*this)[i + r][c], ncols * sizeof(double));
423 }
424
425 return M;
426}
427
432void vpMatrix::eye(unsigned int n) { eye(n, n); }
433
438void vpMatrix::eye(unsigned int m, unsigned int n)
439{
440 resize(m, n);
441
442 eye();
443}
444
450{
451 for (unsigned int i = 0; i < rowNum; i++) {
452 for (unsigned int j = 0; j < colNum; j++) {
453 if (i == j)
454 (*this)[i][j] = 1.0;
455 else
456 (*this)[i][j] = 0;
457 }
458 }
459}
460
465{
466 return transpose();
467}
468
475{
476 vpMatrix At;
477 transpose(At);
478 return At;
479}
480
487{
488 At.resize(colNum, rowNum, false, false);
489
490 if (rowNum <= 16 || colNum <= 16) {
491 for (unsigned int i = 0; i < rowNum; i++) {
492 for (unsigned int j = 0; j < colNum; j++) {
493 At[j][i] = (*this)[i][j];
494 }
495 }
496 }
497 else {
498 SimdMatTranspose(data, rowNum, colNum, At.data);
499 }
500}
501
508{
509 vpMatrix B;
510
511 AAt(B);
512
513 return B;
514}
515
528{
529 if ((B.rowNum != rowNum) || (B.colNum != rowNum))
530 B.resize(rowNum, rowNum, false, false);
531
532 // If available use Lapack only for large matrices
533 bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
534#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
535 useLapack = false;
536#endif
537
538 if (useLapack) {
539#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
540 const double alpha = 1.0;
541 const double beta = 0.0;
542 const char transa = 't';
543 const char transb = 'n';
544
545 vpMatrix::blas_dgemm(transa, transb, rowNum, rowNum, colNum, alpha, data, colNum, data, colNum, beta, B.data, rowNum);
546#endif
547 }
548 else {
549 // compute A*A^T
550 for (unsigned int i = 0; i < rowNum; i++) {
551 for (unsigned int j = i; j < rowNum; j++) {
552 double *pi = rowPtrs[i]; // row i
553 double *pj = rowPtrs[j]; // row j
554
555 // sum (row i .* row j)
556 double ssum = 0;
557 for (unsigned int k = 0; k < colNum; k++)
558 ssum += *(pi++) * *(pj++);
559
560 B[i][j] = ssum; // upper triangle
561 if (i != j)
562 B[j][i] = ssum; // lower triangle
563 }
564 }
565 }
566}
567
580{
581 if ((B.rowNum != colNum) || (B.colNum != colNum))
582 B.resize(colNum, colNum, false, false);
583
584 // If available use Lapack only for large matrices
585 bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
586#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
587 useLapack = false;
588#endif
589
590 if (useLapack) {
591#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
592 const double alpha = 1.0;
593 const double beta = 0.0;
594 const char transa = 'n';
595 const char transb = 't';
596
597 vpMatrix::blas_dgemm(transa, transb, colNum, colNum, rowNum, alpha, data, colNum, data, colNum, beta, B.data, colNum);
598#endif
599 }
600 else {
601 for (unsigned int i = 0; i < colNum; i++) {
602 double *Bi = B[i];
603 for (unsigned int j = 0; j < i; j++) {
604 double *ptr = data;
605 double s = 0;
606 for (unsigned int k = 0; k < rowNum; k++) {
607 s += (*(ptr + i)) * (*(ptr + j));
608 ptr += colNum;
609 }
610 *Bi++ = s;
611 B[j][i] = s;
612 }
613 double *ptr = data;
614 double s = 0;
615 for (unsigned int k = 0; k < rowNum; k++) {
616 s += (*(ptr + i)) * (*(ptr + i));
617 ptr += colNum;
618 }
619 *Bi = s;
620 }
621 }
622}
623
630{
631 vpMatrix B;
632
633 AtA(B);
634
635 return B;
636}
637
655{
656 resize(A.getRows(), A.getCols(), false, false);
657
658 if (data != NULL && A.data != NULL && data != A.data) {
659 memcpy(data, A.data, dsize * sizeof(double));
660 }
661
662 return *this;
663}
664
665#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
667{
668 resize(A.getRows(), A.getCols(), false, false);
669
670 if (data != NULL && A.data != NULL && data != A.data) {
671 memcpy(data, A.data, dsize * sizeof(double));
672 }
673
674 return *this;
675}
676
678{
679 if (this != &other) {
680 free(data);
681 free(rowPtrs);
682
683 rowNum = other.rowNum;
684 colNum = other.colNum;
685 rowPtrs = other.rowPtrs;
686 dsize = other.dsize;
687 data = other.data;
688
689 other.rowNum = 0;
690 other.colNum = 0;
691 other.rowPtrs = NULL;
692 other.dsize = 0;
693 other.data = NULL;
694 }
695
696 return *this;
697}
698
723vpMatrix& vpMatrix::operator=(const std::initializer_list<double> &list)
724{
725 if (dsize != static_cast<unsigned int>(list.size())) {
726 resize(1, static_cast<unsigned int>(list.size()), false, false);
727 }
728
729 std::copy(list.begin(), list.end(), data);
730
731 return *this;
732}
733
757vpMatrix& vpMatrix::operator=(const std::initializer_list<std::initializer_list<double> > &lists)
758{
759 unsigned int nrows = static_cast<unsigned int>(lists.size()), ncols = 0;
760 for (auto& l : lists) {
761 if (static_cast<unsigned int>(l.size()) > ncols) {
762 ncols = static_cast<unsigned int>(l.size());
763 }
764 }
765
766 resize(nrows, ncols, false, false);
767 auto it = lists.begin();
768 for (unsigned int i = 0; i < rowNum; i++, ++it) {
769 std::copy(it->begin(), it->end(), rowPtrs[i]);
770 }
771
772 return *this;
773}
774#endif
775
778{
779 std::fill(data, data + rowNum*colNum, x);
780 return *this;
781}
782
789{
790 for (unsigned int i = 0; i < rowNum; i++) {
791 for (unsigned int j = 0; j < colNum; j++) {
792 rowPtrs[i][j] = *x++;
793 }
794 }
795 return *this;
796}
797
799{
800 resize(1, 1, false, false);
801 rowPtrs[0][0] = val;
802 return *this;
803}
804
806{
807 resize(1, colNum + 1, false, false);
808 rowPtrs[0][colNum - 1] = val;
809 return *this;
810}
811
848{
849 unsigned int rows = A.getRows();
850 this->resize(rows, rows);
851
852 (*this) = 0;
853 for (unsigned int i = 0; i < rows; i++)
854 (*this)[i][i] = A[i];
855}
856
887void vpMatrix::diag(const double &val)
888{
889 (*this) = 0;
890 unsigned int min_ = (rowNum < colNum) ? rowNum : colNum;
891 for (unsigned int i = 0; i < min_; i++)
892 (*this)[i][i] = val;
893}
894
907{
908 unsigned int rows = A.getRows();
909 DA.resize(rows, rows, true);
910
911 for (unsigned int i = 0; i < rows; i++)
912 DA[i][i] = A[i];
913}
914
920{
922
923 if (rowNum != 3 || colNum != 3) {
924 throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector",
925 rowNum, colNum, tv.getRows(), tv.getCols()));
926 }
927
928 for (unsigned int j = 0; j < 3; j++)
929 t_out[j] = 0;
930
931 for (unsigned int j = 0; j < 3; j++) {
932 double tj = tv[j]; // optimization em 5/12/2006
933 for (unsigned int i = 0; i < 3; i++) {
934 t_out[i] += rowPtrs[i][j] * tj;
935 }
936 }
937 return t_out;
938}
939
945{
946 vpColVector v_out;
947 vpMatrix::multMatrixVector(*this, v, v_out);
948 return v_out;
949}
950
960{
961 if (A.colNum != v.getRows()) {
962 throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
963 A.getRows(), A.getCols(), v.getRows()));
964 }
965
966 if (A.rowNum != w.rowNum)
967 w.resize(A.rowNum, false);
968
969 // If available use Lapack only for large matrices
970 bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size);
971#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
972 useLapack = false;
973#endif
974
975 if (useLapack) {
976#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
977 double alpha = 1.0;
978 double beta = 0.0;
979 char trans = 't';
980 int incr = 1;
981
982 vpMatrix::blas_dgemv(trans, A.colNum, A.rowNum, alpha, A.data, A.colNum, v.data, incr, beta, w.data, incr);
983#endif
984 }
985 else {
986 w = 0.0;
987 for (unsigned int j = 0; j < A.colNum; j++) {
988 double vj = v[j]; // optimization em 5/12/2006
989 for (unsigned int i = 0; i < A.rowNum; i++) {
990 w[i] += A.rowPtrs[i][j] * vj;
991 }
992 }
993 }
994}
995
996//---------------------------------
997// Matrix operations.
998//---------------------------------
999
1010{
1011 if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1012 C.resize(A.rowNum, B.colNum, false, false);
1013
1014 if (A.colNum != B.rowNum) {
1015 throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (%dx%d) matrix", A.getRows(),
1016 A.getCols(), B.getRows(), B.getCols()));
1017 }
1018
1019 // If available use Lapack only for large matrices
1020 bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size || B.colNum > vpMatrix::m_lapack_min_size);
1021#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1022 useLapack = false;
1023#endif
1024
1025 if (useLapack) {
1026#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1027 const double alpha = 1.0;
1028 const double beta = 0.0;
1029 const char trans = 'n';
1030 vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1031 C.data, B.colNum);
1032#endif
1033 }
1034 else {
1035 // 5/12/06 some "very" simple optimization to avoid indexation
1036 const unsigned int BcolNum = B.colNum;
1037 const unsigned int BrowNum = B.rowNum;
1038 double **BrowPtrs = B.rowPtrs;
1039 for (unsigned int i = 0; i < A.rowNum; i++) {
1040 const double *rowptri = A.rowPtrs[i];
1041 double *ci = C[i];
1042 for (unsigned int j = 0; j < BcolNum; j++) {
1043 double s = 0;
1044 for (unsigned int k = 0; k < BrowNum; k++)
1045 s += rowptri[k] * BrowPtrs[k][j];
1046 ci[j] = s;
1047 }
1048 }
1049 }
1050}
1051
1066{
1067 if (A.colNum != 3 || A.rowNum != 3 || B.colNum != 3 || B.rowNum != 3) {
1069 "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1070 "rotation matrix",
1071 A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1072 }
1073 // 5/12/06 some "very" simple optimization to avoid indexation
1074 const unsigned int BcolNum = B.colNum;
1075 const unsigned int BrowNum = B.rowNum;
1076 double **BrowPtrs = B.rowPtrs;
1077 for (unsigned int i = 0; i < A.rowNum; i++) {
1078 const double *rowptri = A.rowPtrs[i];
1079 double *ci = C[i];
1080 for (unsigned int j = 0; j < BcolNum; j++) {
1081 double s = 0;
1082 for (unsigned int k = 0; k < BrowNum; k++)
1083 s += rowptri[k] * BrowPtrs[k][j];
1084 ci[j] = s;
1085 }
1086 }
1087}
1088
1103{
1104 if (A.colNum != 4 || A.rowNum != 4 || B.colNum != 4 || B.rowNum != 4) {
1106 "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1107 "rotation matrix",
1108 A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1109 }
1110 // Considering perfMatrixMultiplication.cpp benchmark,
1111 // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1112 // Lapack usage needs to be validated again.
1113 // If available use Lapack only for large matrices.
1114 // Using SSE2 doesn't speed up.
1115 bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size || B.colNum > vpMatrix::m_lapack_min_size);
1116#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1117 useLapack = false;
1118#endif
1119
1120 if (useLapack) {
1121#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1122 const double alpha = 1.0;
1123 const double beta = 0.0;
1124 const char trans = 'n';
1125 vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1126 C.data, B.colNum);
1127#endif
1128 }
1129 else {
1130 // 5/12/06 some "very" simple optimization to avoid indexation
1131 const unsigned int BcolNum = B.colNum;
1132 const unsigned int BrowNum = B.rowNum;
1133 double **BrowPtrs = B.rowPtrs;
1134 for (unsigned int i = 0; i < A.rowNum; i++) {
1135 const double *rowptri = A.rowPtrs[i];
1136 double *ci = C[i];
1137 for (unsigned int j = 0; j < BcolNum; j++) {
1138 double s = 0;
1139 for (unsigned int k = 0; k < BrowNum; k++)
1140 s += rowptri[k] * BrowPtrs[k][j];
1141 ci[j] = s;
1142 }
1143 }
1144 }
1145}
1146
1160{
1162}
1163
1169{
1170 vpMatrix C;
1171
1172 vpMatrix::mult2Matrices(*this, B, C);
1173
1174 return C;
1175}
1176
1182{
1183 if (colNum != R.getRows()) {
1184 throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1185 colNum));
1186 }
1187 vpMatrix C;
1188 C.resize(rowNum, 3, false, false);
1189
1190 unsigned int RcolNum = R.getCols();
1191 unsigned int RrowNum = R.getRows();
1192 for (unsigned int i = 0; i < rowNum; i++) {
1193 double *rowptri = rowPtrs[i];
1194 double *ci = C[i];
1195 for (unsigned int j = 0; j < RcolNum; j++) {
1196 double s = 0;
1197 for (unsigned int k = 0; k < RrowNum; k++)
1198 s += rowptri[k] * R[k][j];
1199 ci[j] = s;
1200 }
1201 }
1202
1203 return C;
1204}
1205
1211{
1212 if (colNum != M.getRows()) {
1213 throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1214 colNum));
1215 }
1216 vpMatrix C;
1217 C.resize(rowNum, 4, false, false);
1218
1219 const unsigned int McolNum = M.getCols();
1220 const unsigned int MrowNum = M.getRows();
1221 for (unsigned int i = 0; i < rowNum; i++) {
1222 const double *rowptri = rowPtrs[i];
1223 double *ci = C[i];
1224 for (unsigned int j = 0; j < McolNum; j++) {
1225 double s = 0;
1226 for (unsigned int k = 0; k < MrowNum; k++)
1227 s += rowptri[k] * M[k][j];
1228 ci[j] = s;
1229 }
1230 }
1231
1232 return C;
1233}
1234
1240{
1241 if (colNum != V.getRows()) {
1242 throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix",
1243 rowNum, colNum));
1244 }
1245 vpMatrix M;
1246 M.resize(rowNum, 6, false, false);
1247
1248 // Considering perfMatrixMultiplication.cpp benchmark,
1249 // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1250 // Lapack usage needs to be validated again.
1251 // If available use Lapack only for large matrices.
1252 // Speed up obtained using SSE2.
1253 bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size || V.colNum > vpMatrix::m_lapack_min_size);
1254#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1255 useLapack = false;
1256#endif
1257
1258 if (useLapack) {
1259#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1260 const double alpha = 1.0;
1261 const double beta = 0.0;
1262 const char trans = 'n';
1263 vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, V.colNum, data, colNum, beta,
1264 M.data, M.colNum);
1265#endif
1266 }
1267 else {
1268 SimdMatMulTwist(data, rowNum, V.data, M.data);
1269 }
1270
1271 return M;
1272}
1273
1279{
1280 if (colNum != V.getRows()) {
1281 throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) force/torque twist matrix",
1282 rowNum, colNum));
1283 }
1284 vpMatrix M;
1285 M.resize(rowNum, 6, false, false);
1286
1287 // Considering perfMatrixMultiplication.cpp benchmark,
1288 // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1289 // Lapack usage needs to be validated again.
1290 // If available use Lapack only for large matrices.
1291 // Speed up obtained using SSE2.
1292 bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size || V.getCols() > vpMatrix::m_lapack_min_size);
1293#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1294 useLapack = false;
1295#endif
1296
1297 if (useLapack) {
1298#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1299 const double alpha = 1.0;
1300 const double beta = 0.0;
1301 const char trans = 'n';
1302 vpMatrix::blas_dgemm(trans, trans, V.getCols(), rowNum, colNum, alpha, V.data, V.getCols(), data, colNum, beta,
1303 M.data, M.colNum);
1304#endif
1305 }
1306 else {
1307 SimdMatMulTwist(data, rowNum, V.data, M.data);
1308 }
1309
1310 return M;
1311}
1312
1323void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB,
1324 vpMatrix &C)
1325{
1326 if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1327 C.resize(A.rowNum, B.colNum, false, false);
1328
1329 if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1330 throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1331 A.getCols(), B.getRows(), B.getCols()));
1332 }
1333
1334 double **ArowPtrs = A.rowPtrs;
1335 double **BrowPtrs = B.rowPtrs;
1336 double **CrowPtrs = C.rowPtrs;
1337
1338 for (unsigned int i = 0; i < A.rowNum; i++)
1339 for (unsigned int j = 0; j < A.colNum; j++)
1340 CrowPtrs[i][j] = wB * BrowPtrs[i][j] + wA * ArowPtrs[i][j];
1341}
1342
1353{
1354 if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1355 C.resize(A.rowNum, B.colNum, false, false);
1356
1357 if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1358 throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1359 A.getCols(), B.getRows(), B.getCols()));
1360 }
1361
1362 double **ArowPtrs = A.rowPtrs;
1363 double **BrowPtrs = B.rowPtrs;
1364 double **CrowPtrs = C.rowPtrs;
1365
1366 for (unsigned int i = 0; i < A.rowNum; i++) {
1367 for (unsigned int j = 0; j < A.colNum; j++) {
1368 CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1369 }
1370 }
1371}
1372
1386{
1387 if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1388 C.resize(A.rowNum);
1389
1390 if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1391 throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1392 A.getCols(), B.getRows(), B.getCols()));
1393 }
1394
1395 double **ArowPtrs = A.rowPtrs;
1396 double **BrowPtrs = B.rowPtrs;
1397 double **CrowPtrs = C.rowPtrs;
1398
1399 for (unsigned int i = 0; i < A.rowNum; i++) {
1400 for (unsigned int j = 0; j < A.colNum; j++) {
1401 CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1402 }
1403 }
1404}
1405
1411{
1412 vpMatrix C;
1413 vpMatrix::add2Matrices(*this, B, C);
1414 return C;
1415}
1416
1433{
1434 if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1435 C.resize(A.rowNum);
1436
1437 if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1438 throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1439 A.getCols(), B.getRows(), B.getCols()));
1440 }
1441
1442 double **ArowPtrs = A.rowPtrs;
1443 double **BrowPtrs = B.rowPtrs;
1444 double **CrowPtrs = C.rowPtrs;
1445
1446 for (unsigned int i = 0; i < A.rowNum; i++) {
1447 for (unsigned int j = 0; j < A.colNum; j++) {
1448 CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1449 }
1450 }
1451}
1452
1466{
1467 if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1468 C.resize(A.rowNum, A.colNum, false, false);
1469
1470 if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1471 throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1472 A.getCols(), B.getRows(), B.getCols()));
1473 }
1474
1475 double **ArowPtrs = A.rowPtrs;
1476 double **BrowPtrs = B.rowPtrs;
1477 double **CrowPtrs = C.rowPtrs;
1478
1479 for (unsigned int i = 0; i < A.rowNum; i++) {
1480 for (unsigned int j = 0; j < A.colNum; j++) {
1481 CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1482 }
1483 }
1484}
1485
1491{
1492 vpMatrix C;
1493 vpMatrix::sub2Matrices(*this, B, C);
1494 return C;
1495}
1496
1498
1500{
1501 if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1502 throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1503 B.getRows(), B.getCols()));
1504 }
1505
1506 double **BrowPtrs = B.rowPtrs;
1507
1508 for (unsigned int i = 0; i < rowNum; i++)
1509 for (unsigned int j = 0; j < colNum; j++)
1510 rowPtrs[i][j] += BrowPtrs[i][j];
1511
1512 return *this;
1513}
1514
1517{
1518 if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1519 throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1520 B.getRows(), B.getCols()));
1521 }
1522
1523 double **BrowPtrs = B.rowPtrs;
1524 for (unsigned int i = 0; i < rowNum; i++)
1525 for (unsigned int j = 0; j < colNum; j++)
1526 rowPtrs[i][j] -= BrowPtrs[i][j];
1527
1528 return *this;
1529}
1530
1541{
1542 if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1543 C.resize(A.rowNum, A.colNum, false, false);
1544
1545 double **ArowPtrs = A.rowPtrs;
1546 double **CrowPtrs = C.rowPtrs;
1547
1548 // t0=vpTime::measureTimeMicros();
1549 for (unsigned int i = 0; i < A.rowNum; i++)
1550 for (unsigned int j = 0; j < A.colNum; j++)
1551 CrowPtrs[i][j] = -ArowPtrs[i][j];
1552}
1553
1559{
1560 vpMatrix C;
1561 vpMatrix::negateMatrix(*this, C);
1562 return C;
1563}
1564
1565double vpMatrix::sum() const
1566{
1567 double s = 0.0;
1568 for (unsigned int i = 0; i < rowNum; i++) {
1569 for (unsigned int j = 0; j < colNum; j++) {
1570 s += rowPtrs[i][j];
1571 }
1572 }
1573
1574 return s;
1575}
1576
1577//---------------------------------
1578// Matrix/vector operations.
1579//---------------------------------
1580
1581//---------------------------------
1582// Matrix/real operations.
1583//---------------------------------
1584
1589vpMatrix operator*(const double &x, const vpMatrix &B)
1590{
1591 if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1592 return B;
1593 }
1594
1595 unsigned int Brow = B.getRows();
1596 unsigned int Bcol = B.getCols();
1597
1598 vpMatrix C;
1599 C.resize(Brow, Bcol, false, false);
1600
1601 for (unsigned int i = 0; i < Brow; i++)
1602 for (unsigned int j = 0; j < Bcol; j++)
1603 C[i][j] = B[i][j] * x;
1604
1605 return C;
1606}
1607
1613{
1614 if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1615 return (*this);
1616 }
1617
1618 vpMatrix M;
1619 M.resize(rowNum, colNum, false, false);
1620
1621 for (unsigned int i = 0; i < rowNum; i++)
1622 for (unsigned int j = 0; j < colNum; j++)
1623 M[i][j] = rowPtrs[i][j] * x;
1624
1625 return M;
1626}
1627
1630{
1631 if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1632 return (*this);
1633 }
1634
1635 if (std::fabs(x) < std::numeric_limits<double>::epsilon()) {
1636 throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1637 }
1638
1639 vpMatrix C;
1640 C.resize(rowNum, colNum, false, false);
1641
1642 double xinv = 1 / x;
1643
1644 for (unsigned int i = 0; i < rowNum; i++)
1645 for (unsigned int j = 0; j < colNum; j++)
1646 C[i][j] = rowPtrs[i][j] * xinv;
1647
1648 return C;
1649}
1650
1653{
1654 for (unsigned int i = 0; i < rowNum; i++)
1655 for (unsigned int j = 0; j < colNum; j++)
1656 rowPtrs[i][j] += x;
1657
1658 return *this;
1659}
1660
1663{
1664 for (unsigned int i = 0; i < rowNum; i++)
1665 for (unsigned int j = 0; j < colNum; j++)
1666 rowPtrs[i][j] -= x;
1667
1668 return *this;
1669}
1670
1676{
1677 if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1678 return *this;
1679 }
1680
1681 for (unsigned int i = 0; i < rowNum; i++)
1682 for (unsigned int j = 0; j < colNum; j++)
1683 rowPtrs[i][j] *= x;
1684
1685 return *this;
1686}
1687
1690{
1691 if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1692 return *this;
1693 }
1694
1695 if (std::fabs(x) < std::numeric_limits<double>::epsilon())
1696 throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1697
1698 double xinv = 1 / x;
1699
1700 for (unsigned int i = 0; i < rowNum; i++)
1701 for (unsigned int j = 0; j < colNum; j++)
1702 rowPtrs[i][j] *= xinv;
1703
1704 return *this;
1705}
1706
1707//----------------------------------------------------------------
1708// Matrix Operation
1709//----------------------------------------------------------------
1710
1716{
1717 if ((out.rowNum != colNum * rowNum) || (out.colNum != 1))
1718 out.resize(colNum * rowNum, false, false);
1719
1720 double *optr = out.data;
1721 for (unsigned int j = 0; j < colNum; j++) {
1722 for (unsigned int i = 0; i < rowNum; i++) {
1723 *(optr++) = rowPtrs[i][j];
1724 }
1725 }
1726}
1727
1733{
1734 vpColVector out(colNum * rowNum);
1735 stackColumns(out);
1736 return out;
1737}
1738
1744{
1745 if ((out.getRows() != 1) || (out.getCols() != colNum * rowNum))
1746 out.resize(colNum * rowNum, false, false);
1747
1748 memcpy(out.data, data, sizeof(double)*out.getCols());
1749}
1755{
1756 vpRowVector out(colNum * rowNum);
1757 stackRows(out);
1758 return out;
1759}
1760
1768{
1769 if (m.getRows() != rowNum || m.getCols() != colNum) {
1770 throw(vpException(vpException::dimensionError, "In Hadamard product: bad dimension of input matrix"));
1771 }
1772
1773 vpMatrix out;
1774 out.resize(rowNum, colNum, false, false);
1775
1776 SimdVectorHadamard(data, m.data, dsize, out.data);
1777
1778 return out;
1779}
1780
1787void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out)
1788{
1789 unsigned int r1 = m1.getRows();
1790 unsigned int c1 = m1.getCols();
1791 unsigned int r2 = m2.getRows();
1792 unsigned int c2 = m2.getCols();
1793
1794 out.resize(r1*r2, c1*c2, false, false);
1795
1796 for (unsigned int r = 0; r < r1; r++) {
1797 for (unsigned int c = 0; c < c1; c++) {
1798 double alpha = m1[r][c];
1799 double *m2ptr = m2[0];
1800 unsigned int roffset = r * r2;
1801 unsigned int coffset = c * c2;
1802 for (unsigned int rr = 0; rr < r2; rr++) {
1803 for (unsigned int cc = 0; cc < c2; cc++) {
1804 out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1805 }
1806 }
1807 }
1808 }
1809}
1810
1817void vpMatrix::kron(const vpMatrix &m, vpMatrix &out) const { kron(*this, m, out); }
1818
1826{
1827 unsigned int r1 = m1.getRows();
1828 unsigned int c1 = m1.getCols();
1829 unsigned int r2 = m2.getRows();
1830 unsigned int c2 = m2.getCols();
1831
1832 vpMatrix out;
1833 out.resize(r1 * r2, c1 * c2, false, false);
1834
1835 for (unsigned int r = 0; r < r1; r++) {
1836 for (unsigned int c = 0; c < c1; c++) {
1837 double alpha = m1[r][c];
1838 double *m2ptr = m2[0];
1839 unsigned int roffset = r * r2;
1840 unsigned int coffset = c * c2;
1841 for (unsigned int rr = 0; rr < r2; rr++) {
1842 for (unsigned int cc = 0; cc < c2; cc++) {
1843 out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1844 }
1845 }
1846 }
1847 }
1848 return out;
1849}
1850
1856vpMatrix vpMatrix::kron(const vpMatrix &m) const { return kron(*this, m); }
1857
1908void vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const { x = pseudoInverse(1e-6) * b; }
1909
1960{
1962
1963 solveBySVD(B, X);
1964 return X;
1965}
1966
2031{
2032#if defined(VISP_HAVE_LAPACK)
2033 svdLapack(w, V);
2034#elif defined(VISP_HAVE_EIGEN3)
2035 svdEigen3(w, V);
2036#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2037 svdOpenCV(w, V);
2038#else
2039 (void)w;
2040 (void)V;
2041 throw(vpException(vpException::fatalError, "Cannot compute SVD. Install Lapack, Eigen3 or OpenCV 3rd party"));
2042#endif
2043}
2044
2099unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
2100{
2101#if defined(VISP_HAVE_LAPACK)
2102 return pseudoInverseLapack(Ap, svThreshold);
2103#elif defined(VISP_HAVE_EIGEN3)
2104 return pseudoInverseEigen3(Ap, svThreshold);
2105#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2106 return pseudoInverseOpenCV(Ap, svThreshold);
2107#else
2108 (void)Ap;
2109 (void)svThreshold;
2110 throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2111 "Install Lapack, Eigen3 or OpenCV 3rd party"));
2112#endif
2113}
2114
2175int vpMatrix::pseudoInverse(vpMatrix &Ap, int rank_in) const
2176{
2177#if defined(VISP_HAVE_LAPACK)
2178 return pseudoInverseLapack(Ap, rank_in);
2179#elif defined(VISP_HAVE_EIGEN3)
2180 return pseudoInverseEigen3(Ap, rank_in);
2181#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2182 return pseudoInverseOpenCV(Ap, rank_in);
2183#else
2184 (void)Ap;
2185 (void)svThreshold;
2186 throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2187 "Install Lapack, Eigen3 or OpenCV 3rd party"));
2188#endif
2189}
2190
2241vpMatrix vpMatrix::pseudoInverse(double svThreshold) const
2242{
2243#if defined(VISP_HAVE_LAPACK)
2244 return pseudoInverseLapack(svThreshold);
2245#elif defined(VISP_HAVE_EIGEN3)
2246 return pseudoInverseEigen3(svThreshold);
2247#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2248 return pseudoInverseOpenCV(svThreshold);
2249#else
2250 (void)svThreshold;
2251 throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2252 "Install Lapack, Eigen3 or OpenCV 3rd party"));
2253#endif
2254}
2255
2307{
2308#if defined(VISP_HAVE_LAPACK)
2309 return pseudoInverseLapack(rank_in);
2310#elif defined(VISP_HAVE_EIGEN3)
2311 return pseudoInverseEigen3(rank_in);
2312#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2313 return pseudoInverseOpenCV(rank_in);
2314#else
2315 (void)svThreshold;
2316 throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2317 "Install Lapack, Eigen3 or OpenCV 3rd party"));
2318#endif
2319}
2320
2321#ifndef DOXYGEN_SHOULD_SKIP_THIS
2322#if defined(VISP_HAVE_LAPACK)
2359vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const
2360{
2361 unsigned int nrows = getRows();
2362 unsigned int ncols = getCols();
2363 int rank_out;
2364
2365 vpMatrix U, V, Ap;
2366 vpColVector sv;
2367
2368 Ap.resize(ncols, nrows, false, false);
2369
2370 if (nrows < ncols) {
2371 U.resize(ncols, ncols, true);
2372 sv.resize(nrows, false);
2373 } else {
2374 U.resize(nrows, ncols, false);
2375 sv.resize(ncols, false);
2376 }
2377
2378 U.insert(*this, 0, 0);
2379 U.svdLapack(sv, V);
2380
2381 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
2382
2383 return Ap;
2384}
2385
2426unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) const
2427{
2428 unsigned int nrows = getRows();
2429 unsigned int ncols = getCols();
2430 int rank_out;
2431
2432 vpMatrix U, V;
2433 vpColVector sv;
2434
2435 Ap.resize(ncols, nrows, false, false);
2436
2437 if (nrows < ncols) {
2438 U.resize(ncols, ncols, true);
2439 sv.resize(nrows, false);
2440 } else {
2441 U.resize(nrows, ncols, false);
2442 sv.resize(ncols, false);
2443 }
2444
2445 U.insert(*this, 0, 0);
2446 U.svdLapack(sv, V);
2447
2448 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
2449
2450 return static_cast<unsigned int>(rank_out);
2451}
2499unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2500{
2501 unsigned int nrows = getRows();
2502 unsigned int ncols = getCols();
2503 int rank_out;
2504
2505 vpMatrix U, V;
2506 vpColVector sv_;
2507
2508 Ap.resize(ncols, nrows, false, false);
2509
2510 if (nrows < ncols) {
2511 U.resize(ncols, ncols, true);
2512 sv.resize(nrows, false);
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 } else {
2649 U.resize(nrows, ncols, false);
2650 sv.resize(ncols, false);
2651 }
2652
2653 U.insert(*this, 0, 0);
2654 U.svdLapack(sv_, V);
2655
2656 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerAt);
2657
2658 // Remove singular values equal to the one that corresponds to the lines of 0
2659 // introduced when m < n
2660 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2661
2662 return static_cast<unsigned int>(rank_out);
2663}
2664
2701vpMatrix vpMatrix::pseudoInverseLapack(int rank_in) const
2702{
2703 unsigned int nrows = getRows();
2704 unsigned int ncols = getCols();
2705 int rank_out;
2706 double svThreshold = 1e-26;
2707
2708 vpMatrix U, V, Ap;
2709 vpColVector sv;
2710
2711 Ap.resize(ncols, nrows, false, false);
2712
2713 if (nrows < ncols) {
2714 U.resize(ncols, ncols, true);
2715 sv.resize(nrows, false);
2716 } else {
2717 U.resize(nrows, ncols, false);
2718 sv.resize(ncols, false);
2719 }
2720
2721 U.insert(*this, 0, 0);
2722 U.svdLapack(sv, V);
2723
2724 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
2725
2726 return Ap;
2727}
2728
2775int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, int rank_in) const
2776{
2777 unsigned int nrows = getRows();
2778 unsigned int ncols = getCols();
2779 int rank_out;
2780 double svThreshold = 1e-26;
2781
2782 vpMatrix U, V;
2783 vpColVector sv;
2784
2785 Ap.resize(ncols, nrows, false, false);
2786
2787 if (nrows < ncols) {
2788 U.resize(ncols, ncols, true);
2789 sv.resize(nrows, false);
2790 } else {
2791 U.resize(nrows, ncols, false);
2792 sv.resize(ncols, false);
2793 }
2794
2795 U.insert(*this, 0, 0);
2796 U.svdLapack(sv, V);
2797
2798 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
2799
2800 return rank_out;
2801}
2802
2856int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in) const
2857{
2858 unsigned int nrows = getRows();
2859 unsigned int ncols = getCols();
2860 int rank_out;
2861 double svThreshold = 1e-26;
2862
2863 vpMatrix U, V;
2864 vpColVector sv_;
2865
2866 Ap.resize(ncols, nrows, false, false);
2867
2868 if (nrows < ncols) {
2869 U.resize(ncols, ncols, true);
2870 sv.resize(nrows, false);
2871 } else {
2872 U.resize(nrows, ncols, false);
2873 sv.resize(ncols, false);
2874 }
2875
2876 U.insert(*this, 0, 0);
2877 U.svdLapack(sv_, V);
2878
2879 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
2880
2881 // Remove singular values equal to the one that corresponds to the lines of 0
2882 // introduced when m < n
2883 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2884
2885 return rank_out;
2886}
2887
2999int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA,
3000 vpMatrix &imAt, vpMatrix &kerAt) const
3001{
3002 unsigned int nrows = getRows();
3003 unsigned int ncols = getCols();
3004 int rank_out;
3005 double svThreshold = 1e-26;
3006 vpMatrix U, V;
3007 vpColVector sv_;
3008
3009 if (nrows < ncols) {
3010 U.resize(ncols, ncols, true);
3011 sv.resize(nrows, false);
3012 } else {
3013 U.resize(nrows, ncols, false);
3014 sv.resize(ncols, false);
3015 }
3016
3017 U.insert(*this, 0, 0);
3018 U.svdLapack(sv_, V);
3019
3020 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
3021
3022 // Remove singular values equal to the one that corresponds to the lines of 0
3023 // introduced when m < n
3024 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3025
3026 return rank_out;
3027}
3028
3029#endif
3030#if defined(VISP_HAVE_EIGEN3)
3067vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const
3068{
3069 unsigned int nrows = getRows();
3070 unsigned int ncols = getCols();
3071 int rank_out;
3072
3073 vpMatrix U, V, Ap;
3074 vpColVector sv;
3075
3076 Ap.resize(ncols, nrows, false, false);
3077
3078 if (nrows < ncols) {
3079 U.resize(ncols, ncols, true);
3080 sv.resize(nrows, false);
3081 } else {
3082 U.resize(nrows, ncols, false);
3083 sv.resize(ncols, false);
3084 }
3085
3086 U.insert(*this, 0, 0);
3087 U.svdEigen3(sv, V);
3088
3089 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3090
3091 return Ap;
3092}
3093
3134unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) const
3135{
3136 unsigned int nrows = getRows();
3137 unsigned int ncols = getCols();
3138 int rank_out;
3139
3140 vpMatrix U, V;
3141 vpColVector sv;
3142
3143 Ap.resize(ncols, nrows, false, false);
3144
3145 if (nrows < ncols) {
3146 U.resize(ncols, ncols, true);
3147 sv.resize(nrows, false);
3148 } else {
3149 U.resize(nrows, ncols, false);
3150 sv.resize(ncols, false);
3151 }
3152
3153 U.insert(*this, 0, 0);
3154 U.svdEigen3(sv, V);
3155
3156 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3157
3158 return static_cast<unsigned int>(rank_out);
3159}
3207unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3208{
3209 unsigned int nrows = getRows();
3210 unsigned int ncols = getCols();
3211 int rank_out;
3212
3213 vpMatrix U, V;
3214 vpColVector sv_;
3215
3216 Ap.resize(ncols, nrows, false, false);
3217
3218 if (nrows < ncols) {
3219 U.resize(ncols, ncols, true);
3220 sv.resize(nrows, false);
3221 } else {
3222 U.resize(nrows, ncols, false);
3223 sv.resize(ncols, false);
3224 }
3225
3226 U.insert(*this, 0, 0);
3227 U.svdEigen3(sv_, V);
3228
3229 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3230
3231 // Remove singular values equal to the one that corresponds to the lines of 0
3232 // introduced when m < n
3233 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3234
3235 return static_cast<unsigned int>(rank_out);
3236}
3237
3344unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3345 vpMatrix &imAt, vpMatrix &kerAt) const
3346{
3347 unsigned int nrows = getRows();
3348 unsigned int ncols = getCols();
3349 int rank_out;
3350 vpMatrix U, V;
3351 vpColVector sv_;
3352
3353 if (nrows < ncols) {
3354 U.resize(ncols, ncols, true);
3355 sv.resize(nrows, false);
3356 } else {
3357 U.resize(nrows, ncols, false);
3358 sv.resize(ncols, false);
3359 }
3360
3361 U.insert(*this, 0, 0);
3362 U.svdEigen3(sv_, V);
3363
3364 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerAt);
3365
3366 // Remove singular values equal to the one that corresponds to the lines of 0
3367 // introduced when m < n
3368 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3369
3370 return static_cast<unsigned int>(rank_out);
3371}
3372
3409vpMatrix vpMatrix::pseudoInverseEigen3(int rank_in) const
3410{
3411 unsigned int nrows = getRows();
3412 unsigned int ncols = getCols();
3413 int rank_out;
3414 double svThreshold = 1e-26;
3415
3416 vpMatrix U, V, Ap;
3417 vpColVector sv;
3418
3419 Ap.resize(ncols, nrows, false, false);
3420
3421 if (nrows < ncols) {
3422 U.resize(ncols, ncols, true);
3423 sv.resize(nrows, false);
3424 } else {
3425 U.resize(nrows, ncols, false);
3426 sv.resize(ncols, false);
3427 }
3428
3429 U.insert(*this, 0, 0);
3430 U.svdEigen3(sv, V);
3431
3432 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3433
3434 return Ap;
3435}
3436
3483int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, int rank_in) const
3484{
3485 unsigned int nrows = getRows();
3486 unsigned int ncols = getCols();
3487 int rank_out;
3488 double svThreshold = 1e-26;
3489
3490 vpMatrix U, V;
3491 vpColVector sv;
3492
3493 Ap.resize(ncols, nrows, false, false);
3494
3495 if (nrows < ncols) {
3496 U.resize(ncols, ncols, true);
3497 sv.resize(nrows, false);
3498 } else {
3499 U.resize(nrows, ncols, false);
3500 sv.resize(ncols, false);
3501 }
3502
3503 U.insert(*this, 0, 0);
3504 U.svdEigen3(sv, V);
3505
3506 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3507
3508 return rank_out;
3509}
3510
3564int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in) const
3565{
3566 unsigned int nrows = getRows();
3567 unsigned int ncols = getCols();
3568 int rank_out;
3569 double svThreshold = 1e-26;
3570
3571 vpMatrix U, V;
3572 vpColVector sv_;
3573
3574 Ap.resize(ncols, nrows, false, false);
3575
3576 if (nrows < ncols) {
3577 U.resize(ncols, ncols, true);
3578 sv.resize(nrows, false);
3579 } else {
3580 U.resize(nrows, ncols, false);
3581 sv.resize(ncols, false);
3582 }
3583
3584 U.insert(*this, 0, 0);
3585 U.svdEigen3(sv_, V);
3586
3587 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3588
3589 // Remove singular values equal to the one that corresponds to the lines of 0
3590 // introduced when m < n
3591 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3592
3593 return rank_out;
3594}
3595
3707int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA,
3708 vpMatrix &imAt, vpMatrix &kerAt) const
3709{
3710 unsigned int nrows = getRows();
3711 unsigned int ncols = getCols();
3712 int rank_out;
3713 double svThreshold = 1e-26;
3714 vpMatrix U, V;
3715 vpColVector sv_;
3716
3717 if (nrows < ncols) {
3718 U.resize(ncols, ncols, true);
3719 sv.resize(nrows, false);
3720 } else {
3721 U.resize(nrows, ncols, false);
3722 sv.resize(ncols, false);
3723 }
3724
3725 U.insert(*this, 0, 0);
3726 U.svdEigen3(sv_, V);
3727
3728 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
3729
3730 // Remove singular values equal to the one that corresponds to the lines of 0
3731 // introduced when m < n
3732 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3733
3734 return rank_out;
3735}
3736
3737#endif
3738#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
3775vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const
3776{
3777 unsigned int nrows = getRows();
3778 unsigned int ncols = getCols();
3779 int rank_out;
3780
3781 vpMatrix U, V, Ap;
3782 vpColVector sv;
3783
3784 Ap.resize(ncols, nrows, false, false);
3785
3786 if (nrows < ncols) {
3787 U.resize(ncols, ncols, true);
3788 sv.resize(nrows, false);
3789 } else {
3790 U.resize(nrows, ncols, false);
3791 sv.resize(ncols, false);
3792 }
3793
3794 U.insert(*this, 0, 0);
3795 U.svdOpenCV(sv, V);
3796
3797 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3798
3799 return Ap;
3800}
3801
3842unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) const
3843{
3844 unsigned int nrows = getRows();
3845 unsigned int ncols = getCols();
3846 int rank_out;
3847
3848 vpMatrix U, V;
3849 vpColVector sv;
3850
3851 Ap.resize(ncols, nrows, false, false);
3852
3853 if (nrows < ncols) {
3854 U.resize(ncols, ncols, true);
3855 sv.resize(nrows, false);
3856 } else {
3857 U.resize(nrows, ncols, false);
3858 sv.resize(ncols, false);
3859 }
3860
3861 U.insert(*this, 0, 0);
3862 U.svdOpenCV(sv, V);
3863
3864 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3865
3866 return static_cast<unsigned int>(rank_out);
3867}
3915unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3916{
3917 unsigned int nrows = getRows();
3918 unsigned int ncols = getCols();
3919 int rank_out;
3920
3921 vpMatrix U, V;
3922 vpColVector sv_;
3923
3924 Ap.resize(ncols, nrows, false, false);
3925
3926 if (nrows < ncols) {
3927 U.resize(ncols, ncols, true);
3928 sv.resize(nrows, false);
3929 } else {
3930 U.resize(nrows, ncols, false);
3931 sv.resize(ncols, false);
3932 }
3933
3934 U.insert(*this, 0, 0);
3935 U.svdOpenCV(sv_, V);
3936
3937 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3938
3939 // Remove singular values equal to the one that corresponds to the lines of 0
3940 // introduced when m < n
3941 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3942
3943 return static_cast<unsigned int>(rank_out);
3944}
3945
4052unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
4053 vpMatrix &imAt, vpMatrix &kerAt) const
4054{
4055 unsigned int nrows = getRows();
4056 unsigned int ncols = getCols();
4057 int rank_out;
4058 vpMatrix U, V;
4059 vpColVector sv_;
4060
4061 if (nrows < ncols) {
4062 U.resize(ncols, ncols, true);
4063 sv.resize(nrows, false);
4064 } else {
4065 U.resize(nrows, ncols, false);
4066 sv.resize(ncols, false);
4067 }
4068
4069 U.insert(*this, 0, 0);
4070 U.svdOpenCV(sv_, V);
4071
4072 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerAt);
4073
4074 // Remove singular values equal to the one that corresponds to the lines of 0
4075 // introduced when m < n
4076 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4077
4078 return static_cast<unsigned int>(rank_out);
4079}
4080
4117vpMatrix vpMatrix::pseudoInverseOpenCV(int rank_in) const
4118{
4119 unsigned int nrows = getRows();
4120 unsigned int ncols = getCols();
4121 int rank_out;
4122 double svThreshold = 1e-26;
4123
4124 vpMatrix U, V, Ap;
4125 vpColVector sv;
4126
4127 Ap.resize(ncols, nrows, false, false);
4128
4129 if (nrows < ncols) {
4130 U.resize(ncols, ncols, true);
4131 sv.resize(nrows, false);
4132 } else {
4133 U.resize(nrows, ncols, false);
4134 sv.resize(ncols, false);
4135 }
4136
4137 U.insert(*this, 0, 0);
4138 U.svdOpenCV(sv, V);
4139
4140 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4141
4142 return Ap;
4143}
4144
4191int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, int rank_in) const
4192{
4193 unsigned int nrows = getRows();
4194 unsigned int ncols = getCols();
4195 int rank_out;
4196 double svThreshold = 1e-26;
4197
4198 vpMatrix U, V;
4199 vpColVector sv;
4200
4201 Ap.resize(ncols, nrows, false, false);
4202
4203 if (nrows < ncols) {
4204 U.resize(ncols, ncols, true);
4205 sv.resize(nrows, false);
4206 } else {
4207 U.resize(nrows, ncols, false);
4208 sv.resize(ncols, false);
4209 }
4210
4211 U.insert(*this, 0, 0);
4212 U.svdOpenCV(sv, V);
4213
4214 compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4215
4216 return rank_out;
4217}
4218
4272int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4273{
4274 unsigned int nrows = getRows();
4275 unsigned int ncols = getCols();
4276 int rank_out;
4277 double svThreshold = 1e-26;
4278
4279 vpMatrix U, V;
4280 vpColVector sv_;
4281
4282 Ap.resize(ncols, nrows, false, false);
4283
4284 if (nrows < ncols) {
4285 U.resize(ncols, ncols, true);
4286 sv.resize(nrows, false);
4287 } else {
4288 U.resize(nrows, ncols, false);
4289 sv.resize(ncols, false);
4290 }
4291
4292 U.insert(*this, 0, 0);
4293 U.svdOpenCV(sv_, V);
4294
4295 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4296
4297 // Remove singular values equal to the one that corresponds to the lines of 0
4298 // introduced when m < n
4299 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4300
4301 return rank_out;
4302}
4303
4415int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA,
4416 vpMatrix &imAt, vpMatrix &kerAt) const
4417{
4418 unsigned int nrows = getRows();
4419 unsigned int ncols = getCols();
4420 int rank_out;
4421 double svThreshold = 1e-26;
4422 vpMatrix U, V;
4423 vpColVector sv_;
4424
4425 if (nrows < ncols) {
4426 U.resize(ncols, ncols, true);
4427 sv.resize(nrows, false);
4428 } else {
4429 U.resize(nrows, ncols, false);
4430 sv.resize(ncols, false);
4431 }
4432
4433 U.insert(*this, 0, 0);
4434 U.svdOpenCV(sv_, V);
4435
4436 compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
4437
4438 // Remove singular values equal to the one that corresponds to the lines of 0
4439 // introduced when m < n
4440 memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4441
4442 return rank_out;
4443}
4444
4445#endif
4446#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
4447
4509unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
4510{
4511#if defined(VISP_HAVE_LAPACK)
4512 return pseudoInverseLapack(Ap, sv, svThreshold);
4513#elif defined(VISP_HAVE_EIGEN3)
4514 return pseudoInverseEigen3(Ap, sv, svThreshold);
4515#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
4516 return pseudoInverseOpenCV(Ap, sv, svThreshold);
4517#else
4518 (void)Ap;
4519 (void)sv;
4520 (void)svThreshold;
4521 throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4522 "Install Lapack, Eigen3 or OpenCV 3rd party"));
4523#endif
4524}
4525
4592int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4593{
4594#if defined(VISP_HAVE_LAPACK)
4595 return pseudoInverseLapack(Ap, sv, rank_in);
4596#elif defined(VISP_HAVE_EIGEN3)
4597 return pseudoInverseEigen3(Ap, sv, rank_in);
4598#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
4599 return pseudoInverseOpenCV(Ap, sv, rank_in);
4600#else
4601 (void)Ap;
4602 (void)sv;
4603 (void)svThreshold;
4604 throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4605 "Install Lapack, Eigen3 or OpenCV 3rd party"));
4606#endif
4607}
4682unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt) const
4683{
4684 vpMatrix kerAt;
4685 return pseudoInverse(Ap, sv, svThreshold, imA, imAt, kerAt);
4686}
4687
4766int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt) const
4767{
4768 vpMatrix kerAt;
4769 return pseudoInverse(Ap, sv, rank_in, imA, imAt, kerAt);
4770}
4771
4906unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt, vpMatrix &kerAt) const
4907{
4908#if defined(VISP_HAVE_LAPACK)
4909 return pseudoInverseLapack(Ap, sv, svThreshold, imA, imAt, kerAt);
4910#elif defined(VISP_HAVE_EIGEN3)
4911 return pseudoInverseEigen3(Ap, sv, svThreshold, imA, imAt, kerAt);
4912#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
4913 return pseudoInverseOpenCV(Ap, sv, svThreshold, imA, imAt, kerAt);
4914#else
4915 (void)Ap;
4916 (void)sv;
4917 (void)svThreshold;
4918 (void)imA;
4919 (void)imAt;
4920 (void)kerAt;
4921 throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4922 "Install Lapack, Eigen3 or OpenCV 3rd party"));
4923#endif
4924}
4925
5065int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt, vpMatrix &kerAt) const
5066{
5067#if defined(VISP_HAVE_LAPACK)
5068 return pseudoInverseLapack(Ap, sv, rank_in, imA, imAt, kerAt);
5069#elif defined(VISP_HAVE_EIGEN3)
5070 return pseudoInverseEigen3(Ap, sv, rank_in, imA, imAt, kerAt);
5071#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
5072 return pseudoInverseOpenCV(Ap, sv, rank_in, imA, imAt, kerAt);
5073#else
5074 (void)Ap;
5075 (void)sv;
5076 (void)svThreshold;
5077 (void)imA;
5078 (void)imAt;
5079 (void)kerAt;
5080 throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
5081 "Install Lapack, Eigen3 or OpenCV 3rd party"));
5082#endif
5083}
5084
5126vpColVector vpMatrix::getCol(unsigned int j, unsigned int i_begin, unsigned int column_size) const
5127{
5128 if (i_begin + column_size > getRows() || j >= getCols())
5129 throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(), getCols()));
5130 vpColVector c(column_size);
5131 for (unsigned int i = 0; i < column_size; i++)
5132 c[i] = (*this)[i_begin + i][j];
5133 return c;
5134}
5135
5175vpColVector vpMatrix::getCol(unsigned int j) const
5176{
5177 return getCol(j, 0, rowNum);
5178}
5179
5215vpRowVector vpMatrix::getRow(unsigned int i) const
5216{
5217 return getRow(i, 0, colNum);
5218}
5219
5259vpRowVector vpMatrix::getRow(unsigned int i, unsigned int j_begin, unsigned int row_size) const
5260{
5261 if (j_begin + row_size > colNum || i >= rowNum)
5262 throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
5263
5264 vpRowVector r(row_size);
5265 if (r.data != NULL && data != NULL) {
5266 memcpy(r.data, (*this)[i] + j_begin, row_size*sizeof(double));
5267 }
5268
5269 return r;
5270}
5271
5308{
5309 unsigned int min_size = std::min(rowNum, colNum);
5311
5312 if (min_size > 0) {
5313 diag.resize(min_size, false);
5314
5315 for (unsigned int i = 0; i < min_size; i++) {
5316 diag[i] = (*this)[i][i];
5317 }
5318 }
5319
5320 return diag;
5321}
5322
5334{
5335 vpMatrix C;
5336
5337 vpMatrix::stack(A, B, C);
5338
5339 return C;
5340}
5341
5353void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
5354{
5355 unsigned int nra = A.getRows();
5356 unsigned int nrb = B.getRows();
5357
5358 if (nra != 0) {
5359 if (A.getCols() != B.getCols()) {
5360 throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5361 A.getCols(), B.getRows(), B.getCols()));
5362 }
5363 }
5364
5365 if (A.data != NULL && A.data == C.data) {
5366 std::cerr << "A and C must be two different objects!" << std::endl;
5367 return;
5368 }
5369
5370 if (B.data != NULL && B.data == C.data) {
5371 std::cerr << "B and C must be two different objects!" << std::endl;
5372 return;
5373 }
5374
5375 C.resize(nra + nrb, B.getCols(), false, false);
5376
5377 if (C.data != NULL && A.data != NULL && A.size() > 0) {
5378 // Copy A in C
5379 memcpy(C.data, A.data, sizeof(double) * A.size());
5380 }
5381
5382 if (C.data != NULL && B.data != NULL && B.size() > 0) {
5383 // Copy B in C
5384 memcpy(C.data + A.size(), B.data, sizeof(double) * B.size());
5385 }
5386}
5387
5398{
5399 vpMatrix C;
5400 vpMatrix::stack(A, r, C);
5401
5402 return C;
5403}
5404
5416void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
5417{
5418 if (A.data != NULL && A.data == C.data) {
5419 std::cerr << "A and C must be two different objects!" << std::endl;
5420 return;
5421 }
5422
5423 C = A;
5424 C.stack(r);
5425}
5426
5437{
5438 vpMatrix C;
5439 vpMatrix::stack(A, c, C);
5440
5441 return C;
5442}
5443
5455void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C)
5456{
5457 if (A.data != NULL && A.data == C.data) {
5458 std::cerr << "A and C must be two different objects!" << std::endl;
5459 return;
5460 }
5461
5462 C = A;
5463 C.stack(c);
5464}
5465
5478vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, unsigned int r, unsigned int c)
5479{
5480 vpMatrix C;
5481
5482 insert(A, B, C, r, c);
5483
5484 return C;
5485}
5486
5500void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, unsigned int r, unsigned int c)
5501{
5502 if (((r + B.getRows()) <= A.getRows()) && ((c + B.getCols()) <= A.getCols())) {
5503 C.resize(A.getRows(), A.getCols(), false, false);
5504
5505 for (unsigned int i = 0; i < A.getRows(); i++) {
5506 for (unsigned int j = 0; j < A.getCols(); j++) {
5507 if (i >= r && i < (r + B.getRows()) && j >= c && j < (c + B.getCols())) {
5508 C[i][j] = B[i - r][j - c];
5509 } else {
5510 C[i][j] = A[i][j];
5511 }
5512 }
5513 }
5514 } else {
5515 throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
5516 B.getRows(), B.getCols(), A.getCols(), A.getRows(), r, c);
5517 }
5518}
5519
5532{
5533 vpMatrix C;
5534
5535 juxtaposeMatrices(A, B, C);
5536
5537 return C;
5538}
5539
5553{
5554 unsigned int nca = A.getCols();
5555 unsigned int ncb = B.getCols();
5556
5557 if (nca != 0) {
5558 if (A.getRows() != B.getRows()) {
5559 throw(vpException(vpException::dimensionError, "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5560 A.getCols(), B.getRows(), B.getCols()));
5561 }
5562 }
5563
5564 if (B.getRows() == 0 || nca + ncb == 0) {
5565 std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl;
5566 return;
5567 }
5568
5569 C.resize(B.getRows(), nca + ncb, false, false);
5570
5571 C.insert(A, 0, 0);
5572 C.insert(B, 0, nca);
5573}
5574
5575//--------------------------------------------------------------------
5576// Output
5577//--------------------------------------------------------------------
5578
5598int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &intro) const
5599{
5600 typedef std::string::size_type size_type;
5601
5602 unsigned int m = getRows();
5603 unsigned int n = getCols();
5604
5605 std::vector<std::string> values(m * n);
5606 std::ostringstream oss;
5607 std::ostringstream ossFixed;
5608 std::ios_base::fmtflags original_flags = oss.flags();
5609
5610 // ossFixed <<std::fixed;
5611 ossFixed.setf(std::ios::fixed, std::ios::floatfield);
5612
5613 size_type maxBefore = 0; // the length of the integral part
5614 size_type maxAfter = 0; // number of decimals plus
5615 // one place for the decimal point
5616 for (unsigned int i = 0; i < m; ++i) {
5617 for (unsigned int j = 0; j < n; ++j) {
5618 oss.str("");
5619 oss << (*this)[i][j];
5620 if (oss.str().find("e") != std::string::npos) {
5621 ossFixed.str("");
5622 ossFixed << (*this)[i][j];
5623 oss.str(ossFixed.str());
5624 }
5625
5626 values[i * n + j] = oss.str();
5627 size_type thislen = values[i * n + j].size();
5628 size_type p = values[i * n + j].find('.');
5629
5630 if (p == std::string::npos) {
5631 maxBefore = vpMath::maximum(maxBefore, thislen);
5632 // maxAfter remains the same
5633 } else {
5634 maxBefore = vpMath::maximum(maxBefore, p);
5635 maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
5636 }
5637 }
5638 }
5639
5640 size_type totalLength = length;
5641 // increase totalLength according to maxBefore
5642 totalLength = vpMath::maximum(totalLength, maxBefore);
5643 // decrease maxAfter according to totalLength
5644 maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
5645 if (maxAfter == 1)
5646 maxAfter = 0;
5647
5648 // the following line is useful for debugging
5649 // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
5650
5651 if (! intro.empty())
5652 s << intro;
5653 s << "[" << m << "," << n << "]=\n";
5654
5655 for (unsigned int i = 0; i < m; i++) {
5656 s << " ";
5657 for (unsigned int j = 0; j < n; j++) {
5658 size_type p = values[i * n + j].find('.');
5659 s.setf(std::ios::right, std::ios::adjustfield);
5660 s.width((std::streamsize)maxBefore);
5661 s << values[i * n + j].substr(0, p).c_str();
5662
5663 if (maxAfter > 0) {
5664 s.setf(std::ios::left, std::ios::adjustfield);
5665 if (p != std::string::npos) {
5666 s.width((std::streamsize)maxAfter);
5667 s << values[i * n + j].substr(p, maxAfter).c_str();
5668 } else {
5669 assert(maxAfter > 1);
5670 s.width((std::streamsize)maxAfter);
5671 s << ".0";
5672 }
5673 }
5674
5675 s << ' ';
5676 }
5677 s << std::endl;
5678 }
5679
5680 s.flags(original_flags); // restore s to standard state
5681
5682 return (int)(maxBefore + maxAfter);
5683}
5684
5721std::ostream &vpMatrix::matlabPrint(std::ostream &os) const
5722{
5723 os << "[ ";
5724 for (unsigned int i = 0; i < this->getRows(); ++i) {
5725 for (unsigned int j = 0; j < this->getCols(); ++j) {
5726 os << (*this)[i][j] << ", ";
5727 }
5728 if (this->getRows() != i + 1) {
5729 os << ";" << std::endl;
5730 } else {
5731 os << "]" << std::endl;
5732 }
5733 }
5734 return os;
5735}
5736
5765std::ostream &vpMatrix::maplePrint(std::ostream &os) const
5766{
5767 os << "([ " << std::endl;
5768 for (unsigned int i = 0; i < this->getRows(); ++i) {
5769 os << "[";
5770 for (unsigned int j = 0; j < this->getCols(); ++j) {
5771 os << (*this)[i][j] << ", ";
5772 }
5773 os << "]," << std::endl;
5774 }
5775 os << "])" << std::endl;
5776 return os;
5777}
5778
5806std::ostream &vpMatrix::csvPrint(std::ostream &os) const
5807{
5808 for (unsigned int i = 0; i < this->getRows(); ++i) {
5809 for (unsigned int j = 0; j < this->getCols(); ++j) {
5810 os << (*this)[i][j];
5811 if (!(j == (this->getCols() - 1)))
5812 os << ", ";
5813 }
5814 os << std::endl;
5815 }
5816 return os;
5817}
5818
5855std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
5856{
5857 os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " << this->getCols() << "); " << std::endl;
5858
5859 for (unsigned int i = 0; i < this->getRows(); ++i) {
5860 for (unsigned int j = 0; j < this->getCols(); ++j) {
5861 if (!octet) {
5862 os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl;
5863 } else {
5864 for (unsigned int k = 0; k < sizeof(double); ++k) {
5865 os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex
5866 << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl;
5867 }
5868 }
5869 }
5870 os << std::endl;
5871 }
5872 return os;
5873}
5874
5880{
5881 if (rowNum == 0) {
5882 *this = A;
5883 } else if (A.getRows() > 0) {
5884 if (colNum != A.getCols()) {
5885 throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum,
5886 A.getRows(), A.getCols()));
5887 }
5888
5889 unsigned int rowNumOld = rowNum;
5890 resize(rowNum + A.getRows(), colNum, false, false);
5891 insert(A, rowNumOld, 0);
5892 }
5893}
5894
5911{
5912 if (rowNum == 0) {
5913 *this = r;
5914 } else {
5915 if (colNum != r.getCols()) {
5916 throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum,
5917 colNum, r.getCols()));
5918 }
5919
5920 if (r.size() == 0) {
5921 return;
5922 }
5923
5924 unsigned int oldSize = size();
5925 resize(rowNum + 1, colNum, false, false);
5926
5927 if (data != NULL && r.data != NULL && data != r.data) {
5928 // Copy r in data
5929 memcpy(data + oldSize, r.data, sizeof(double) * r.size());
5930 }
5931 }
5932}
5933
5951{
5952 if (colNum == 0) {
5953 *this = c;
5954 } else {
5955 if (rowNum != c.getRows()) {
5956 throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum,
5957 colNum, c.getRows()));
5958 }
5959
5960 if (c.size() == 0) {
5961 return;
5962 }
5963
5964 vpMatrix tmp = *this;
5965 unsigned int oldColNum = colNum;
5966 resize(rowNum, colNum + 1, false, false);
5967
5968 if (data != NULL && tmp.data != NULL && data != tmp.data) {
5969 // Copy c in data
5970 for (unsigned int i = 0; i < rowNum; i++) {
5971 memcpy(data + i*colNum, tmp.data + i*oldColNum, sizeof(double) * oldColNum);
5972 rowPtrs[i][oldColNum] = c[i];
5973 }
5974 }
5975 }
5976}
5977
5988void vpMatrix::insert(const vpMatrix &A, unsigned int r, unsigned int c)
5989{
5990 if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) {
5991 if (A.colNum == colNum && data != NULL && A.data != NULL && A.data != data) {
5992 memcpy(data + r * colNum, A.data, sizeof(double) * A.size());
5993 } else if (data != NULL && A.data != NULL && A.data != data) {
5994 for (unsigned int i = r; i < (r + A.getRows()); i++) {
5995 memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum);
5996 }
5997 }
5998 } else {
5999 throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
6000 A.getRows(), A.getCols(), rowNum, colNum, r, c);
6001 }
6002}
6003
6041{
6042 vpColVector evalue(rowNum); // Eigen values
6043
6044 if (rowNum != colNum) {
6046 "Cannot compute eigen values on a non square matrix (%dx%d)",
6047 rowNum, colNum));
6048 }
6049
6050 // Check if the matrix is symmetric: At - A = 0
6051 vpMatrix At_A = (*this).t() - (*this);
6052 for (unsigned int i = 0; i < rowNum; i++) {
6053 for (unsigned int j = 0; j < rowNum; j++) {
6054 // if (At_A[i][j] != 0) {
6055 if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
6056 throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix"));
6057 }
6058 }
6059 }
6060
6061#if defined(VISP_HAVE_LAPACK)
6062#if defined(VISP_HAVE_GSL) /* be careful of the copy below */
6063 {
6064 gsl_vector *eval = gsl_vector_alloc(rowNum);
6065 gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
6066
6067 gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
6068 gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
6069
6070 unsigned int Atda = (unsigned int)m->tda;
6071 for (unsigned int i = 0; i < rowNum; i++) {
6072 unsigned int k = i * Atda;
6073 for (unsigned int j = 0; j < colNum; j++)
6074 m->data[k + j] = (*this)[i][j];
6075 }
6076 gsl_eigen_symmv(m, eval, evec, w);
6077
6078 gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
6079
6080 for (unsigned int i = 0; i < rowNum; i++) {
6081 evalue[i] = gsl_vector_get(eval, i);
6082 }
6083
6084 gsl_eigen_symmv_free(w);
6085 gsl_vector_free(eval);
6086 gsl_matrix_free(m);
6087 gsl_matrix_free(evec);
6088 }
6089#else
6090 {
6091 const char jobz = 'N';
6092 const char uplo = 'U';
6093 vpMatrix A = (*this);
6094 vpColVector WORK;
6095 int lwork = -1;
6096 int info = 0;
6097 double wkopt;
6098 vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
6099 lwork = static_cast<int>(wkopt);
6100 WORK.resize(lwork);
6101 vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
6102 }
6103#endif
6104#else
6105 {
6107 "Eigen values computation is not implemented. "
6108 "You should install Lapack 3rd party"));
6109 }
6110#endif
6111 return evalue;
6112}
6113
6163void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const
6164{
6165 if (rowNum != colNum) {
6167 "Cannot compute eigen values on a non square matrix (%dx%d)",
6168 rowNum, colNum));
6169 }
6170
6171 // Check if the matrix is symmetric: At - A = 0
6172 vpMatrix At_A = (*this).t() - (*this);
6173 for (unsigned int i = 0; i < rowNum; i++) {
6174 for (unsigned int j = 0; j < rowNum; j++) {
6175 // if (At_A[i][j] != 0) {
6176 if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
6177 throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix"));
6178 }
6179 }
6180 }
6181
6182 // Resize the output matrices
6183 evalue.resize(rowNum);
6184 evector.resize(rowNum, colNum);
6185
6186#if defined(VISP_HAVE_LAPACK)
6187#if defined(VISP_HAVE_GSL) /* be careful of the copy below */
6188 {
6189 gsl_vector *eval = gsl_vector_alloc(rowNum);
6190 gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
6191
6192 gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
6193 gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
6194
6195 unsigned int Atda = (unsigned int)m->tda;
6196 for (unsigned int i = 0; i < rowNum; i++) {
6197 unsigned int k = i * Atda;
6198 for (unsigned int j = 0; j < colNum; j++)
6199 m->data[k + j] = (*this)[i][j];
6200 }
6201 gsl_eigen_symmv(m, eval, evec, w);
6202
6203 gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
6204
6205 for (unsigned int i = 0; i < rowNum; i++) {
6206 evalue[i] = gsl_vector_get(eval, i);
6207 }
6208 Atda = (unsigned int)evec->tda;
6209 for (unsigned int i = 0; i < rowNum; i++) {
6210 unsigned int k = i * Atda;
6211 for (unsigned int j = 0; j < rowNum; j++) {
6212 evector[i][j] = evec->data[k + j];
6213 }
6214 }
6215
6216 gsl_eigen_symmv_free(w);
6217 gsl_vector_free(eval);
6218 gsl_matrix_free(m);
6219 gsl_matrix_free(evec);
6220 }
6221#else // defined(VISP_HAVE_GSL)
6222 {
6223 const char jobz = 'V';
6224 const char uplo = 'U';
6225 vpMatrix A = (*this);
6226 vpColVector WORK;
6227 int lwork = -1;
6228 int info = 0;
6229 double wkopt;
6230 vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
6231 lwork = static_cast<int>(wkopt);
6232 WORK.resize(lwork);
6233 vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
6234 evector = A.t();
6235 }
6236#endif // defined(VISP_HAVE_GSL)
6237#else
6238 {
6240 "Eigen values computation is not implemented. "
6241 "You should install Lapack 3rd party"));
6242 }
6243#endif
6244}
6245
6264unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const
6265{
6266 unsigned int nbline = getRows();
6267 unsigned int nbcol = getCols();
6268
6269 vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6270 vpColVector sv;
6271 sv.resize(nbcol, false); // singular values
6272 V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6273
6274 // Copy and resize matrix to have at least as many rows as columns
6275 // kernel is computed in svd method only if the matrix has more rows than
6276 // columns
6277
6278 if (nbline < nbcol)
6279 U.resize(nbcol, nbcol, true);
6280 else
6281 U.resize(nbline, nbcol, false);
6282
6283 U.insert(*this, 0, 0);
6284
6285 U.svd(sv, V);
6286
6287 // Compute the highest singular value and rank of the matrix
6288 double maxsv = 0;
6289 for (unsigned int i = 0; i < nbcol; i++) {
6290 if (sv[i] > maxsv) {
6291 maxsv = sv[i];
6292 }
6293 }
6294
6295 unsigned int rank = 0;
6296 for (unsigned int i = 0; i < nbcol; i++) {
6297 if (sv[i] > maxsv * svThreshold) {
6298 rank++;
6299 }
6300 }
6301
6302 kerAt.resize(nbcol - rank, nbcol);
6303 if (rank != nbcol) {
6304 for (unsigned int j = 0, k = 0; j < nbcol; j++) {
6305 // if( v.col(j) in kernel and non zero )
6306 if ((sv[j] <= maxsv * svThreshold) &&
6307 (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
6308 for (unsigned int i = 0; i < V.getRows(); i++) {
6309 kerAt[k][i] = V[i][j];
6310 }
6311 k++;
6312 }
6313 }
6314 }
6315
6316 return rank;
6317}
6318
6335unsigned int vpMatrix::nullSpace(vpMatrix &kerA, double svThreshold) const
6336{
6337 unsigned int nbrow = getRows();
6338 unsigned int nbcol = getCols();
6339
6340 vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6341 vpColVector sv;
6342 sv.resize(nbcol, false); // singular values
6343 V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6344
6345 // Copy and resize matrix to have at least as many rows as columns
6346 // kernel is computed in svd method only if the matrix has more rows than
6347 // columns
6348
6349 if (nbrow < nbcol)
6350 U.resize(nbcol, nbcol, true);
6351 else
6352 U.resize(nbrow, nbcol, false);
6353
6354 U.insert(*this, 0, 0);
6355
6356 U.svd(sv, V);
6357
6358 // Compute the highest singular value and rank of the matrix
6359 double maxsv = sv[0];
6360
6361 unsigned int rank = 0;
6362 for (unsigned int i = 0; i < nbcol; i++) {
6363 if (sv[i] > maxsv * svThreshold) {
6364 rank++;
6365 }
6366 }
6367
6368 kerA.resize(nbcol, nbcol - rank);
6369 if (rank != nbcol) {
6370 for (unsigned int j = 0, k = 0; j < nbcol; j++) {
6371 // if( v.col(j) in kernel and non zero )
6372 if (sv[j] <= maxsv * svThreshold) {
6373 for (unsigned int i = 0; i < nbcol; i++) {
6374 kerA[i][k] = V[i][j];
6375 }
6376 k++;
6377 }
6378 }
6379 }
6380
6381 return (nbcol - rank);
6382}
6383
6400unsigned int vpMatrix::nullSpace(vpMatrix &kerA, int dim) const
6401{
6402 unsigned int nbrow = getRows();
6403 unsigned int nbcol = getCols();
6404 unsigned int dim_ = static_cast<unsigned int>(dim);
6405
6406 vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6407 vpColVector sv;
6408 sv.resize(nbcol, false); // singular values
6409 V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6410
6411 // Copy and resize matrix to have at least as many rows as columns
6412 // kernel is computed in svd method only if the matrix has more rows than
6413 // columns
6414
6415 if (nbrow < nbcol)
6416 U.resize(nbcol, nbcol, true);
6417 else
6418 U.resize(nbrow, nbcol, false);
6419
6420 U.insert(*this, 0, 0);
6421
6422 U.svd(sv, V);
6423
6424 kerA.resize(nbcol, dim_);
6425 if (dim_ != 0) {
6426 unsigned int rank = nbcol - dim_;
6427 for (unsigned int k = 0; k < dim_; k++) {
6428 unsigned int j = k + rank;
6429 for (unsigned int i = 0; i < nbcol; i++) {
6430 kerA[i][k] = V[i][j];
6431 }
6432 }
6433 }
6434
6435 double maxsv = sv[0];
6436 unsigned int rank = 0;
6437 for (unsigned int i = 0; i < nbcol; i++) {
6438 if (sv[i] > maxsv * 1e-6) {
6439 rank++;
6440 }
6441 }
6442 return (nbcol - rank);
6443}
6444
6476double vpMatrix::det(vpDetMethod method) const
6477{
6478 double det = 0.;
6479
6480 if (method == LU_DECOMPOSITION) {
6481 det = this->detByLU();
6482 }
6483
6484 return (det);
6485}
6486
6495{
6496 if (colNum != rowNum) {
6497 throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix",
6498 rowNum, colNum));
6499 } else {
6500#ifdef VISP_HAVE_GSL
6501 size_t size_ = rowNum * colNum;
6502 double *b = new double[size_];
6503 for (size_t i = 0; i < size_; i++)
6504 b[i] = 0.;
6505 gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum);
6506 gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum);
6507 gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
6508 // gsl_matrix_fprintf(stdout, &em.matrix, "%g");
6509 vpMatrix expA;
6510 expA.resize(rowNum, colNum, false);
6511 memcpy(expA.data, b, size_ * sizeof(double));
6512
6513 delete[] b;
6514 return expA;
6515#else
6516 vpMatrix _expE(rowNum, colNum, false);
6517 vpMatrix _expD(rowNum, colNum, false);
6518 vpMatrix _expX(rowNum, colNum, false);
6519 vpMatrix _expcX(rowNum, colNum, false);
6520 vpMatrix _eye(rowNum, colNum, false);
6521
6522 _eye.eye();
6523 vpMatrix exp(*this);
6524
6525 // double f;
6526 int e;
6527 double c = 0.5;
6528 int q = 6;
6529 int p = 1;
6530
6531 double nA = 0;
6532 for (unsigned int i = 0; i < rowNum; i++) {
6533 double sum = 0;
6534 for (unsigned int j = 0; j < colNum; j++) {
6535 sum += fabs((*this)[i][j]);
6536 }
6537 if (sum > nA || i == 0) {
6538 nA = sum;
6539 }
6540 }
6541
6542 /* f = */ frexp(nA, &e);
6543 // double s = (0 > e+1)?0:e+1;
6544 double s = e + 1;
6545
6546 double sca = 1.0 / pow(2.0, s);
6547 exp = sca * exp;
6548 _expX = *this;
6549 _expE = c * exp + _eye;
6550 _expD = -c * exp + _eye;
6551 for (int k = 2; k <= q; k++) {
6552 c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1)));
6553 _expcX = exp * _expX;
6554 _expX = _expcX;
6555 _expcX = c * _expX;
6556 _expE = _expE + _expcX;
6557 if (p)
6558 _expD = _expD + _expcX;
6559 else
6560 _expD = _expD - _expcX;
6561 p = !p;
6562 }
6563 _expX = _expD.inverseByLU();
6564 exp = _expX * _expE;
6565 for (int k = 1; k <= s; k++) {
6566 _expE = exp * exp;
6567 exp = _expE;
6568 }
6569 return exp;
6570#endif
6571 }
6572}
6573
6574/**************************************************************************************************************/
6575/**************************************************************************************************************/
6576
6577// Specific functions
6578
6579/*
6580input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.
6581
6582output:: the complement matrix of the element (rowNo,colNo).
6583This is the matrix obtained from M after elimenating the row rowNo and column
6584colNo
6585
6586example:
65871 2 3
6588M = 4 5 6
65897 8 9
65901 3
6591subblock(M, 1, 1) give the matrix 7 9
6592*/
6593vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row)
6594{
6595 vpMatrix M_comp;
6596 M_comp.resize(M.getRows() - 1, M.getCols() - 1, false);
6597
6598 for (unsigned int i = 0; i < col; i++) {
6599 for (unsigned int j = 0; j < row; j++)
6600 M_comp[i][j] = M[i][j];
6601 for (unsigned int j = row + 1; j < M.getRows(); j++)
6602 M_comp[i][j - 1] = M[i][j];
6603 }
6604 for (unsigned int i = col + 1; i < M.getCols(); i++) {
6605 for (unsigned int j = 0; j < row; j++)
6606 M_comp[i - 1][j] = M[i][j];
6607 for (unsigned int j = row + 1; j < M.getRows(); j++)
6608 M_comp[i - 1][j - 1] = M[i][j];
6609 }
6610 return M_comp;
6611}
6612
6622double vpMatrix::cond(double svThreshold) const
6623{
6624 unsigned int nbline = getRows();
6625 unsigned int nbcol = getCols();
6626
6627 vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6628 vpColVector sv;
6629 sv.resize(nbcol); // singular values
6630 V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6631
6632 // Copy and resize matrix to have at least as many rows as columns
6633 // kernel is computed in svd method only if the matrix has more rows than
6634 // columns
6635
6636 if (nbline < nbcol)
6637 U.resize(nbcol, nbcol, true);
6638 else
6639 U.resize(nbline, nbcol, false);
6640
6641 U.insert(*this, 0, 0);
6642
6643 U.svd(sv, V);
6644
6645 // Compute the highest singular value
6646 double maxsv = 0;
6647 for (unsigned int i = 0; i < nbcol; i++) {
6648 if (sv[i] > maxsv) {
6649 maxsv = sv[i];
6650 }
6651 }
6652
6653 // Compute the rank of the matrix
6654 unsigned int rank = 0;
6655 for (unsigned int i = 0; i < nbcol; i++) {
6656 if (sv[i] > maxsv * svThreshold) {
6657 rank++;
6658 }
6659 }
6660
6661 // Compute the lowest singular value
6662 double minsv = maxsv;
6663 for (unsigned int i = 0; i < rank; i++) {
6664 if (sv[i] < minsv) {
6665 minsv = sv[i];
6666 }
6667 }
6668
6669 if (std::fabs(minsv) > std::numeric_limits<double>::epsilon()) {
6670 return maxsv / minsv;
6671 }
6672 else {
6673 return std::numeric_limits<double>::infinity();
6674 }
6675}
6676
6683void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
6684{
6685 if (H.getCols() != H.getRows()) {
6686 throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(),
6687 H.getCols()));
6688 }
6689
6690 HLM = H;
6691 for (unsigned int i = 0; i < H.getCols(); i++) {
6692 HLM[i][i] += alpha * H[i][i];
6693 }
6694}
6695
6704{
6705 double norm = 0.0;
6706 for (unsigned int i = 0; i < dsize; i++) {
6707 double x = *(data + i);
6708 norm += x * x;
6709 }
6710
6711 return sqrt(norm);
6712}
6713
6723{
6724 if(this->dsize != 0){
6725 vpMatrix v;
6726 vpColVector w;
6727
6728 vpMatrix M = *this;
6729
6730 M.svd(w, v);
6731
6732 double max = w[0];
6733 unsigned int maxRank = std::min(this->getCols(), this->getRows());
6734 // The maximum reachable rank is either the number of columns or the number of rows
6735 // of the matrix.
6736 unsigned int boundary = std::min(maxRank, w.size());
6737 // boundary is here to ensure that the number of singular values used for the com-
6738 // putation of the euclidean norm of the matrix is not greater than the maximum
6739 // reachable rank. Indeed, some svd library pad the singular values vector with 0s
6740 // if the input matrix is non-square.
6741 for (unsigned int i = 0; i < boundary; i++) {
6742 if (max < w[i]) {
6743 max = w[i];
6744 }
6745 }
6746 return max;
6747 }
6748 else {
6749 return 0.;
6750 }
6751}
6752
6764{
6765 double norm = 0.0;
6766 for (unsigned int i = 0; i < rowNum; i++) {
6767 double x = 0;
6768 for (unsigned int j = 0; j < colNum; j++) {
6769 x += fabs(*(*(rowPtrs + i) + j));
6770 }
6771 if (x > norm) {
6772 norm = x;
6773 }
6774 }
6775 return norm;
6776}
6777
6785{
6786 double sum_square = 0.0;
6787 double x;
6788
6789 for (unsigned int i = 0; i < rowNum; i++) {
6790 for (unsigned int j = 0; j < colNum; j++) {
6791 x = rowPtrs[i][j];
6792 sum_square += x * x;
6793 }
6794 }
6795
6796 return sum_square;
6797}
6798
6810vpMatrix vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode)
6811{
6812 vpMatrix res;
6813 conv2(M, kernel, res, mode);
6814 return res;
6815}
6816
6829void vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, vpMatrix &res, const std::string &mode)
6830{
6831 if (M.getRows()*M.getCols() == 0 || kernel.getRows()*kernel.getCols() == 0)
6832 return;
6833
6834 if (mode == "valid") {
6835 if (kernel.getRows() > M.getRows() || kernel.getCols() > M.getCols())
6836 return;
6837 }
6838
6839 vpMatrix M_padded, res_same;
6840
6841 if (mode == "full" || mode == "same") {
6842 const unsigned int pad_x = kernel.getCols()-1;
6843 const unsigned int pad_y = kernel.getRows()-1;
6844 M_padded.resize(M.getRows() + 2*pad_y, M.getCols() + 2*pad_x, true, false);
6845 M_padded.insert(M, pad_y, pad_x);
6846
6847 if (mode == "same") {
6848 res.resize(M.getRows(), M.getCols(), false, false);
6849 res_same.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
6850 } else {
6851 res.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
6852 }
6853 } else if (mode == "valid") {
6854 M_padded = M;
6855 res.resize(M.getRows()-kernel.getRows()+1, M.getCols()-kernel.getCols()+1);
6856 } else {
6857 return;
6858 }
6859
6860 if (mode == "same") {
6861 for (unsigned int i = 0; i < res_same.getRows(); i++) {
6862 for (unsigned int j = 0; j < res_same.getCols(); j++) {
6863 for (unsigned int k = 0; k < kernel.getRows(); k++) {
6864 for (unsigned int l = 0; l < kernel.getCols(); l++) {
6865 res_same[i][j] += M_padded[i+k][j+l] * kernel[kernel.getRows()-k-1][kernel.getCols()-l-1];
6866 }
6867 }
6868 }
6869 }
6870
6871 const unsigned int start_i = kernel.getRows()/2;
6872 const unsigned int start_j = kernel.getCols()/2;
6873 for (unsigned int i = 0; i < M.getRows(); i++) {
6874 memcpy(res.data + i*M.getCols(), res_same.data + (i+start_i)*res_same.getCols() + start_j, sizeof(double)*M.getCols());
6875 }
6876 } else {
6877 for (unsigned int i = 0; i < res.getRows(); i++) {
6878 for (unsigned int j = 0; j < res.getCols(); j++) {
6879 for (unsigned int k = 0; k < kernel.getRows(); k++) {
6880 for (unsigned int l = 0; l < kernel.getCols(); l++) {
6881 res[i][j] += M_padded[i+k][j+l] * kernel[kernel.getRows()-k-1][kernel.getCols()-l-1];
6882 }
6883 }
6884 }
6885 }
6886 }
6887}
6888
6889#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
6899vp_deprecated double vpMatrix::euclideanNorm() const
6900{
6901 return frobeniusNorm();
6902}
6903
6905{
6906 return (vpMatrix)(vpColVector::stack(A, B));
6907}
6908
6910{
6911 vpColVector::stack(A, B, C);
6912}
6913
6915
6916void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); }
6917
6937{
6938 vpRowVector c(getCols());
6939
6940 for (unsigned int j = 0; j < getCols(); j++)
6941 c[j] = (*this)[i - 1][j];
6942 return c;
6943}
6944
6963{
6964 vpColVector c(getRows());
6965
6966 for (unsigned int i = 0; i < getRows(); i++)
6967 c[i] = (*this)[i][j - 1];
6968 return c;
6969}
6970
6977void vpMatrix::setIdentity(const double &val)
6978{
6979 for (unsigned int i = 0; i < rowNum; i++)
6980 for (unsigned int j = 0; j < colNum; j++)
6981 if (i == j)
6982 (*this)[i][j] = val;
6983 else
6984 (*this)[i][j] = 0;
6985}
6986
6987#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:132
unsigned int getCols() const
Definition: vpArray2D.h:279
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:139
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:141
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
unsigned int getRows() const
Definition: vpArray2D.h:289
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
double sumSquare() const
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:90
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
@ divideByZeroError
Division by zero.
Definition: vpException.h:94
Implementation of an homogeneous matrix and operations on such kind of matrices.
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void svdLapack(vpColVector &w, vpMatrix &V)
static vpMatrix conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode="full")
Definition: vpMatrix.cpp:6810
vpColVector eigenValues() const
Definition: vpMatrix.cpp:6040
vpMatrix pseudoInverseOpenCV(double svThreshold=1e-6) const
std::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
Definition: vpMatrix.cpp:5855
vpMatrix & operator<<(double *)
Definition: vpMatrix.cpp:788
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6622
vpMatrix expm() const
Definition: vpMatrix.cpp:6494
vpMatrix pseudoInverseEigen3(double svThreshold=1e-6) const
vpMatrix hadamard(const vpMatrix &m) const
Definition: vpMatrix.cpp:1767
vpMatrix operator-() const
Definition: vpMatrix.cpp:1558
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:6977
vpMatrix & operator/=(double x)
Divide all the element of the matrix by x : Aij = Aij / x.
Definition: vpMatrix.cpp:1689
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5598
std::ostream & maplePrint(std::ostream &os) const
Definition: vpMatrix.cpp:5765
void eye()
Definition: vpMatrix.cpp:449
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6264
vpMatrix inverseByLU() const
vpMatrix operator*(const vpMatrix &B) const
Definition: vpMatrix.cpp:1168
vpMatrix operator/(double x) const
Cij = Aij / x (A is unchanged)
Definition: vpMatrix.cpp:1629
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5879
vp_deprecated void stackMatrices(const vpMatrix &A)
Definition: vpMatrix.h:786
vpMatrix & operator-=(const vpMatrix &B)
Operation A = A - B.
Definition: vpMatrix.cpp:1516
vpMatrix & operator*=(double x)
Multiply all the element of the matrix by x : Aij = Aij * x.
Definition: vpMatrix.cpp:1675
vpMatrix operator*(const double &x, const vpMatrix &B)
Definition: vpMatrix.cpp:1589
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:5531
vpColVector stackColumns()
Definition: vpMatrix.cpp:1732
vp_deprecated vpColVector column(unsigned int j)
Definition: vpMatrix.cpp:6962
static void sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1465
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2030
vpRowVector stackRows()
Definition: vpMatrix.cpp:1754
vpMatrix operator+(const vpMatrix &B) const
Definition: vpMatrix.cpp:1410
double sum() const
Definition: vpMatrix.cpp:1565
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:887
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:5215
void svdOpenCV(vpColVector &w, vpMatrix &V)
vpMatrix t() const
Definition: vpMatrix.cpp:464
vpMatrix()
Definition: vpMatrix.h:169
vpMatrix & operator+=(const vpMatrix &B)
Operation A = A + B.
Definition: vpMatrix.cpp:1499
static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C)
Definition: vpMatrix.cpp:1323
double inducedL2Norm() const
Definition: vpMatrix.cpp:6722
double infinityNorm() const
Definition: vpMatrix.cpp:6763
vpMatrix & operator=(const vpArray2D< double > &A)
Definition: vpMatrix.cpp:654
double frobeniusNorm() const
Definition: vpMatrix.cpp:6703
vpColVector getDiag() const
Definition: vpMatrix.cpp:5307
vpMatrix AtA() const
Definition: vpMatrix.cpp:629
static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
Definition: vpMatrix.cpp:906
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:1908
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6683
vpMatrix pseudoInverseLapack(double svThreshold=1e-6) const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
vpMatrix & operator,(double val)
Definition: vpMatrix.cpp:805
static void multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
Definition: vpMatrix.cpp:959
vp_deprecated vpRowVector row(unsigned int i)
Definition: vpMatrix.cpp:6936
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5175
double detByLU() const
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6476
double sumSquare() const
Definition: vpMatrix.cpp:6784
vp_deprecated void init()
Definition: vpMatrix.h:781
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1352
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5988
void svdEigen3(vpColVector &w, vpMatrix &V)
void kron(const vpMatrix &m1, vpMatrix &out) const
Definition: vpMatrix.cpp:1817
vpMatrix AAt() const
Definition: vpMatrix.cpp:507
vpMatrix transpose() const
Definition: vpMatrix.cpp:474
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1009
static void negateMatrix(const vpMatrix &A, vpMatrix &C)
Definition: vpMatrix.cpp:1540
@ LU_DECOMPOSITION
Definition: vpMatrix.h:161
std::ostream & csvPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5806
unsigned int nullSpace(vpMatrix &kerA, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6335
std::ostream & matlabPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5721
double euclideanNorm() const
Definition: vpMatrix.cpp:6899
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:407
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:116
void resize(unsigned int i, bool flagNullify=true)
Definition: vpRowVector.h:271
Class that consider the case of a translation vector.