Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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>
73 typedef MKL_INT integer;
74 
75 void 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)
88 typedef long int integer;
89 # else
90 typedef int integer;
91 # endif
92 extern "C" integer dsyev_(char *jobz, char *uplo, integer *n, double *a, integer *lda,
93  double *w, double *WORK, integer *lwork, integer *info);
94 
95 void 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))
112 const unsigned int vpMatrix::m_lapack_min_size_default = 0;
113 unsigned int vpMatrix::m_lapack_min_size = vpMatrix::m_lapack_min_size_default;
114 #endif
115 
116 // Prototypes of specific functions
117 vpMatrix subblock(const vpMatrix &, unsigned int, unsigned int);
118 
119 void 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 
189 vpMatrix::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 
244 vpMatrix::vpMatrix(const std::initializer_list<double> &list) : vpArray2D<double>(list) { }
245 
246 
271 vpMatrix::vpMatrix(unsigned int nrows, unsigned int ncols, const std::initializer_list<double> &list)
272  : vpArray2D<double>(nrows, ncols, list) {}
273 
296 vpMatrix::vpMatrix(const std::initializer_list<std::initializer_list<double> > &lists) : vpArray2D<double>(lists) { }
297 
298 #endif
299 
346 void 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 
407 vpMatrix 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 
432 void vpMatrix::eye(unsigned int n) { eye(n, n); }
433 
438 void 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 
527 void vpMatrix::AAt(vpMatrix &B) const
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 
579 void vpMatrix::AtA(vpMatrix &B) const
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 
723 vpMatrix& 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 
757 vpMatrix& 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 
887 void 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 {
921  vpTranslationVector t_out;
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 {
1161  vpMatrix::multMatrixVector(A, B, C);
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 
1323 void 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 
1352 void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
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 
1465 void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
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 
1565 double 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 
1589 vpMatrix 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 
1787 void 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 
1817 void 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 
1856 vpMatrix vpMatrix::kron(const vpMatrix &m) const { return kron(*this, m); }
1857 
1908 void vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const { x = pseudoInverse(1e-6) * b; }
1909 
1960 {
1961  vpColVector X(colNum);
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 
2099 unsigned 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 
2175 int 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 
2241 vpMatrix 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)
2323 
2359 vpMatrix 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 
2426 unsigned 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 }
2499 unsigned 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 
2636 unsigned 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 
2701 vpMatrix 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 
2775 int 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 
2856 int 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 
2999 int 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)
3031 
3067 vpMatrix 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 
3134 unsigned 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 }
3207 unsigned 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 
3344 unsigned 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 
3409 vpMatrix 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 
3483 int 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 
3564 int 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 
3707 int 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)
3739 
3775 vpMatrix 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 
3842 unsigned 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 }
3915 unsigned 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 
4052 unsigned 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 
4117 vpMatrix 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 
4191 int 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 
4272 int 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 
4415 int 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 
4509 unsigned 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 
4592 int 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 }
4682 unsigned 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 
4766 int 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 
4906 unsigned 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 
5065 int 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 
5126 vpColVector 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 
5175 vpColVector vpMatrix::getCol(unsigned int j) const
5176 {
5177  return getCol(j, 0, rowNum);
5178 }
5179 
5215 vpRowVector vpMatrix::getRow(unsigned int i) const
5216 {
5217  return getRow(i, 0, colNum);
5218 }
5219 
5259 vpRowVector 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);
5310  vpColVector diag;
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 
5353 void 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 
5416 void 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 
5455 void 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 
5478 vpMatrix 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 
5500 void 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 
5598 int 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 
5721 std::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 
5765 std::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 
5806 std::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 
5855 std::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 
5988 void 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 
6163 void 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 
6264 unsigned 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 
6335 unsigned 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 
6400 unsigned 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 
6476 double 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 /*
6580 input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.
6581 
6582 output:: the complement matrix of the element (rowNo,colNo).
6583 This is the matrix obtained from M after elimenating the row rowNo and column
6584 colNo
6585 
6586 example:
6587 1 2 3
6588 M = 4 5 6
6589 7 8 9
6590 1 3
6591 subblock(M, 1, 1) give the matrix 7 9
6592 */
6593 vpMatrix 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 
6622 double 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 
6683 void 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 
6784 double vpMatrix::sumSquare() const
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 
6810 vpMatrix 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 
6829 void 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)
6890 
6899 vp_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 
6916 void 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 
6977 void 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)
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:1908
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2030
vpMatrix operator*(const double &x, const vpMatrix &B)
Definition: vpMatrix.cpp:1589
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:407
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:5531
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
vpColVector getDiag() const
Definition: vpMatrix.cpp:5307
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6476
static vpMatrix conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode="full")
Definition: vpMatrix.cpp:6810
void kron(const vpMatrix &m1, vpMatrix &out) const
Definition: vpMatrix.cpp:1817
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpMatrix operator-() const
Definition: vpMatrix.cpp:1558
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:115
std::ostream & csvPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5806
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1352
vpMatrix AtA() const
Definition: vpMatrix.cpp:629
double sum() const
Definition: vpMatrix.cpp:1565
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5879
vp_deprecated vpColVector column(unsigned int j)
Definition: vpMatrix.cpp:6962
vpMatrix inverseByLU() const
vpMatrix & operator/=(double x)
Divide all the element of the matrix by x : Aij = Aij / x.
Definition: vpMatrix.cpp:1689
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
vp_deprecated void stackMatrices(const vpMatrix &A)
Definition: vpMatrix.h:786
Implementation of a generic 2D array used as base class for matrices and vectors. ...
Definition: vpArray2D.h:131
static void sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1465
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
vpMatrix pseudoInverseLapack(double svThreshold=1e-6) const
vpMatrix & operator+=(const vpMatrix &B)
Operation A = A + B.
Definition: vpMatrix.cpp:1499
vpMatrix()
Definition: vpMatrix.h:169
vpMatrix operator/(double x) const
Cij = Aij / x (A is unchanged)
Definition: vpMatrix.cpp:1629
double infinityNorm() const
Definition: vpMatrix.cpp:6763
void svdLapack(vpColVector &w, vpMatrix &V)
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6622
vpMatrix & operator-=(const vpMatrix &B)
Operation A = A - B.
Definition: vpMatrix.cpp:1516
vpMatrix operator+(const vpMatrix &B) const
Definition: vpMatrix.cpp:1410
vpMatrix pseudoInverseEigen3(double svThreshold=1e-6) const
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
Implementation of a rotation matrix and operations on such kind of matrices.
vpMatrix & operator*=(double x)
Multiply all the element of the matrix by x : Aij = Aij * x.
Definition: vpMatrix.cpp:1675
unsigned int getCols() const
Definition: vpArray2D.h:279
std::ostream & maplePrint(std::ostream &os) const
Definition: vpMatrix.cpp:5765
unsigned int nullSpace(vpMatrix &kerA, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6335
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
double sumSquare() const
Definition: vpMatrix.cpp:6784
vpMatrix hadamard(const vpMatrix &m) const
Definition: vpMatrix.cpp:1767
vpMatrix t() const
Definition: vpMatrix.cpp:464
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:6977
std::ostream & matlabPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5721
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6264
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:887
vp_deprecated void init()
Definition: vpMatrix.h:781
static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C)
Definition: vpMatrix.cpp:1323
vpMatrix pseudoInverseOpenCV(double svThreshold=1e-6) const
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:5215
static void multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
Definition: vpMatrix.cpp:959
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1009
std::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
Definition: vpMatrix.cpp:5855
vpColVector eigenValues() const
Definition: vpMatrix.cpp:6040
double euclideanNorm() const
Definition: vpMatrix.cpp:6899
void svdOpenCV(vpColVector &w, vpMatrix &V)
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5598
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5175
vpMatrix AAt() const
Definition: vpMatrix.cpp:507
vpMatrix transpose() const
Definition: vpMatrix.cpp:474
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
double inducedL2Norm() const
Definition: vpMatrix.cpp:6722
vpMatrix operator*(const vpMatrix &B) const
Definition: vpMatrix.cpp:1168
vp_deprecated vpRowVector row(unsigned int i)
Definition: vpMatrix.cpp:6936
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
vpRowVector stackRows()
Definition: vpMatrix.cpp:1754
double sumSquare() const
static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
Definition: vpMatrix.cpp:906
vpColVector stackColumns()
Definition: vpMatrix.cpp:1732
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:141
double detByLU() const
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5988
static void negateMatrix(const vpMatrix &A, vpMatrix &C)
Definition: vpMatrix.cpp:1540
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6683
vpMatrix & operator,(double val)
Definition: vpMatrix.cpp:805
double frobeniusNorm() const
Definition: vpMatrix.cpp:6703
Function not implemented.
Definition: vpException.h:90
void resize(unsigned int i, bool flagNullify=true)
Definition: vpRowVector.h:271
Class that consider the case of a translation vector.
void eye()
Definition: vpMatrix.cpp:449
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:139
void svdEigen3(vpColVector &w, vpMatrix &V)
vpMatrix expm() const
Definition: vpMatrix.cpp:6494
vpMatrix & operator=(const vpArray2D< double > &A)
Definition: vpMatrix.cpp:654
vpMatrix & operator<<(double *)
Definition: vpMatrix.cpp:788