Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpMatrix.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Matrix manipulation.
33  *
34 *****************************************************************************/
40 #include <algorithm>
41 #include <assert.h>
42 #include <cmath> // std::fabs
43 #include <fstream>
44 #include <limits> // numeric_limits
45 #include <sstream>
46 #include <stdio.h>
47 #include <stdlib.h>
48 #include <string.h>
49 #include <string>
50 #include <vector>
51 
52 #include <visp3/core/vpCPUFeatures.h>
53 #include <visp3/core/vpColVector.h>
54 #include <visp3/core/vpConfig.h>
55 #include <visp3/core/vpDebug.h>
56 #include <visp3/core/vpException.h>
57 #include <visp3/core/vpMath.h>
58 #include <visp3/core/vpMatrix.h>
59 #include <visp3/core/vpTranslationVector.h>
60 
61 #if defined(VISP_HAVE_SIMDLIB)
62 #include <Simd/SimdLib.h>
63 #endif
64 
65 #ifdef VISP_HAVE_LAPACK
66 #ifdef VISP_HAVE_GSL
67 #include <gsl/gsl_eigen.h>
68 #include <gsl/gsl_linalg.h>
69 #include <gsl/gsl_math.h>
70 #elif defined(VISP_HAVE_MKL)
71 #include <mkl.h>
72 typedef MKL_INT integer;
73 
74 void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_, double *w_data,
75  double *work_data, int lwork_, int &info_)
76 {
77  MKL_INT n = static_cast<MKL_INT>(n_);
78  MKL_INT lda = static_cast<MKL_INT>(lda_);
79  MKL_INT lwork = static_cast<MKL_INT>(lwork_);
80  MKL_INT info = static_cast<MKL_INT>(info_);
81 
82  dsyev(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
83 }
84 
85 #else
86 #if defined(VISP_HAVE_LAPACK_BUILT_IN)
87 typedef long int integer;
88 #else
89 typedef int integer;
90 #endif
91 extern "C" integer dsyev_(char *jobz, char *uplo, integer *n, double *a, integer *lda, double *w, double *WORK,
92  integer *lwork, integer *info);
93 
94 void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_, double *w_data,
95  double *work_data, int lwork_, int &info_)
96 {
97  integer n = static_cast<integer>(n_);
98  integer lda = static_cast<integer>(lda_);
99  integer lwork = static_cast<integer>(lwork_);
100  integer info = static_cast<integer>(info_);
101 
102  dsyev_(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
103 
104  lwork_ = static_cast<int>(lwork);
105  info_ = static_cast<int>(info);
106 }
107 #endif
108 #endif
109 
110 #if !defined(VISP_USE_MSVC) || (defined(VISP_USE_MSVC) && !defined(VISP_BUILD_SHARED_LIBS))
111 const unsigned int vpMatrix::m_lapack_min_size_default = 0;
112 unsigned int vpMatrix::m_lapack_min_size = vpMatrix::m_lapack_min_size_default;
113 #endif
114 
115 // Prototypes of specific functions
116 vpMatrix subblock(const vpMatrix &, unsigned int, unsigned int);
117 
118 void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows,
119  unsigned int ncols, double svThreshold, vpMatrix &Ap, int &rank_out, int *rank_in,
120  vpMatrix *imA, vpMatrix *imAt, vpMatrix *kerAt)
121 {
122  Ap.resize(ncols, nrows, true, false);
123 
124  // compute the highest singular value and the rank of h
125  double maxsv = sv[0];
126 
127  rank_out = 0;
128 
129  for (unsigned int i = 0; i < ncols; i++) {
130  if (sv[i] > maxsv * svThreshold) {
131  rank_out++;
132  }
133  }
134 
135  unsigned int rank = static_cast<unsigned int>(rank_out);
136  if (rank_in) {
137  rank = static_cast<unsigned int>(*rank_in);
138  }
139 
140  for (unsigned int i = 0; i < ncols; i++) {
141  for (unsigned int j = 0; j < nrows; j++) {
142  for (unsigned int k = 0; k < rank; k++) {
143  Ap[i][j] += V[i][k] * U[j][k] / sv[k];
144  }
145  }
146  }
147 
148  // Compute im(A)
149  if (imA) {
150  imA->resize(nrows, rank);
151 
152  for (unsigned int i = 0; i < nrows; i++) {
153  for (unsigned int j = 0; j < rank; j++) {
154  (*imA)[i][j] = U[i][j];
155  }
156  }
157  }
158 
159  // Compute im(At)
160  if (imAt) {
161  imAt->resize(ncols, rank);
162  for (unsigned int i = 0; i < ncols; i++) {
163  for (unsigned int j = 0; j < rank; j++) {
164  (*imAt)[i][j] = V[i][j];
165  }
166  }
167  }
168 
169  // Compute ker(At)
170  if (kerAt) {
171  kerAt->resize(ncols - rank, ncols);
172  if (rank != ncols) {
173  for (unsigned int k = 0; k < (ncols - rank); k++) {
174  unsigned j = k + rank;
175  for (unsigned int i = 0; i < V.getRows(); i++) {
176  (*kerAt)[k][i] = V[i][j];
177  }
178  }
179  }
180  }
181 }
182 
188 vpMatrix::vpMatrix(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
189  : vpArray2D<double>()
190 {
191  if (((r + nrows) > M.rowNum) || ((c + ncols) > M.colNum)) {
193  "Cannot construct a sub matrix (%dx%d) starting at "
194  "position (%d,%d) that is not contained in the "
195  "original matrix (%dx%d)",
196  nrows, ncols, r, c, M.rowNum, M.colNum));
197  }
198 
199  init(M, r, c, nrows, ncols);
200 }
201 
202 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
204 {
205  rowNum = A.rowNum;
206  colNum = A.colNum;
207  rowPtrs = A.rowPtrs;
208  dsize = A.dsize;
209  data = A.data;
210 
211  A.rowNum = 0;
212  A.colNum = 0;
213  A.rowPtrs = nullptr;
214  A.dsize = 0;
215  A.data = nullptr;
216 }
217 
241 vpMatrix::vpMatrix(const std::initializer_list<double> &list) : vpArray2D<double>(list) { }
242 
265 vpMatrix::vpMatrix(unsigned int nrows, unsigned int ncols, const std::initializer_list<double> &list)
266  : vpArray2D<double>(nrows, ncols, list)
267 { }
268 
289 vpMatrix::vpMatrix(const std::initializer_list<std::initializer_list<double> > &lists) : vpArray2D<double>(lists) { }
290 #endif
291 
338 void vpMatrix::init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
339 {
340  unsigned int rnrows = r + nrows;
341  unsigned int cncols = c + ncols;
342 
343  if (rnrows > M.getRows())
344  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
345  M.getRows()));
346  if (cncols > M.getCols())
347  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
348  M.getCols()));
349  resize(nrows, ncols, false, false);
350 
351  if (this->rowPtrs == nullptr) { // Fix coverity scan: explicit null dereferenced
352  return; // Noting to do
353  }
354  for (unsigned int i = 0; i < nrows; i++) {
355  memcpy((*this)[i], &M[i + r][c], ncols * sizeof(double));
356  }
357 }
358 
400 vpMatrix vpMatrix::extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
401 {
402  unsigned int rnrows = r + nrows;
403  unsigned int cncols = c + ncols;
404 
405  if (rnrows > getRows())
406  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
407  getRows()));
408  if (cncols > getCols())
409  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
410  getCols()));
411 
412  vpMatrix M;
413  M.resize(nrows, ncols, false, false);
414  for (unsigned int i = 0; i < nrows; i++) {
415  memcpy(M[i], &(*this)[i + r][c], ncols * sizeof(double));
416  }
417 
418  return M;
419 }
420 
425 void vpMatrix::eye(unsigned int n) { eye(n, n); }
426 
431 void vpMatrix::eye(unsigned int m, unsigned int n)
432 {
433  resize(m, n);
434 
435  eye();
436 }
437 
443 {
444  for (unsigned int i = 0; i < rowNum; i++) {
445  for (unsigned int j = 0; j < colNum; j++) {
446  if (i == j)
447  (*this)[i][j] = 1.0;
448  else
449  (*this)[i][j] = 0;
450  }
451  }
452 }
453 
457 vpMatrix vpMatrix::t() const { return transpose(); }
458 
465 {
466  vpMatrix At;
467  transpose(At);
468  return At;
469 }
470 
477 {
478  At.resize(colNum, rowNum, false, false);
479 
480  if (rowNum <= 16 || colNum <= 16) {
481  for (unsigned int i = 0; i < rowNum; i++) {
482  for (unsigned int j = 0; j < colNum; j++) {
483  At[j][i] = (*this)[i][j];
484  }
485  }
486  }
487  else {
488 #if defined(VISP_HAVE_SIMDLIB)
489  SimdMatTranspose(data, rowNum, colNum, At.data);
490 #else
491  // https://stackoverflow.com/a/21548079
492  const int tileSize = 32;
493  for (unsigned int i = 0; i < rowNum; i += tileSize) {
494  for (unsigned int j = 0; j < colNum; j++) {
495  for (unsigned int b = 0; b < static_cast<unsigned int>(tileSize) && i + b < rowNum; b++) {
496  At[j][i + b] = (*this)[i + b][j];
497  }
498  }
499  }
500 #endif
501  }
502 }
503 
510 {
511  vpMatrix B;
512 
513  AAt(B);
514 
515  return B;
516 }
517 
529 void vpMatrix::AAt(vpMatrix &B) const
530 {
531  if ((B.rowNum != rowNum) || (B.colNum != rowNum))
532  B.resize(rowNum, rowNum, false, false);
533 
534  // If available use Lapack only for large matrices
535  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
536 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
537  useLapack = false;
538 #endif
539 
540  if (useLapack) {
541 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
542  const double alpha = 1.0;
543  const double beta = 0.0;
544  const char transa = 't';
545  const char transb = 'n';
546 
547  vpMatrix::blas_dgemm(transa, transb, rowNum, rowNum, colNum, alpha, data, colNum, data, colNum, beta, B.data,
548  rowNum);
549 #endif
550  }
551  else {
552  // compute A*A^T
553  for (unsigned int i = 0; i < rowNum; i++) {
554  for (unsigned int j = i; j < rowNum; j++) {
555  double *pi = rowPtrs[i]; // row i
556  double *pj = rowPtrs[j]; // row j
557 
558  // sum (row i .* row j)
559  double ssum = 0;
560  for (unsigned int k = 0; k < colNum; k++)
561  ssum += *(pi++) * *(pj++);
562 
563  B[i][j] = ssum; // upper triangle
564  if (i != j)
565  B[j][i] = ssum; // lower triangle
566  }
567  }
568  }
569 }
570 
582 void vpMatrix::AtA(vpMatrix &B) const
583 {
584  if ((B.rowNum != colNum) || (B.colNum != colNum))
585  B.resize(colNum, colNum, false, false);
586 
587  // If available use Lapack only for large matrices
588  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
589 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
590  useLapack = false;
591 #endif
592 
593  if (useLapack) {
594 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
595  const double alpha = 1.0;
596  const double beta = 0.0;
597  const char transa = 'n';
598  const char transb = 't';
599 
600  vpMatrix::blas_dgemm(transa, transb, colNum, colNum, rowNum, alpha, data, colNum, data, colNum, beta, B.data,
601  colNum);
602 #endif
603  }
604  else {
605  for (unsigned int i = 0; i < colNum; i++) {
606  double *Bi = B[i];
607  for (unsigned int j = 0; j < i; j++) {
608  double *ptr = data;
609  double s = 0;
610  for (unsigned int k = 0; k < rowNum; k++) {
611  s += (*(ptr + i)) * (*(ptr + j));
612  ptr += colNum;
613  }
614  *Bi++ = s;
615  B[j][i] = s;
616  }
617  double *ptr = data;
618  double s = 0;
619  for (unsigned int k = 0; k < rowNum; k++) {
620  s += (*(ptr + i)) * (*(ptr + i));
621  ptr += colNum;
622  }
623  *Bi = s;
624  }
625  }
626 }
627 
634 {
635  vpMatrix B;
636 
637  AtA(B);
638 
639  return B;
640 }
641 
659 {
660  resize(A.getRows(), A.getCols(), false, false);
661 
662  if (data != nullptr && A.data != nullptr && data != A.data) {
663  memcpy(data, A.data, dsize * sizeof(double));
664  }
665 
666  return *this;
667 }
668 
670 {
671  resize(A.getRows(), A.getCols(), false, false);
672 
673  if (data != nullptr && A.data != nullptr && data != A.data) {
674  memcpy(data, A.data, dsize * sizeof(double));
675  }
676 
677  return *this;
678 }
679 
680 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
682 {
683  if (this != &other) {
684  free(data);
685  free(rowPtrs);
686 
687  rowNum = other.rowNum;
688  colNum = other.colNum;
689  rowPtrs = other.rowPtrs;
690  dsize = other.dsize;
691  data = other.data;
692 
693  other.rowNum = 0;
694  other.colNum = 0;
695  other.rowPtrs = nullptr;
696  other.dsize = 0;
697  other.data = nullptr;
698  }
699 
700  return *this;
701 }
702 
729 vpMatrix &vpMatrix::operator=(const std::initializer_list<double> &list)
730 {
731  if (dsize != static_cast<unsigned int>(list.size())) {
732  resize(1, static_cast<unsigned int>(list.size()), false, false);
733  }
734 
735  std::copy(list.begin(), list.end(), data);
736 
737  return *this;
738 }
739 
763 vpMatrix &vpMatrix::operator=(const std::initializer_list<std::initializer_list<double> > &lists)
764 {
765  unsigned int nrows = static_cast<unsigned int>(lists.size()), ncols = 0;
766  for (auto &l : lists) {
767  if (static_cast<unsigned int>(l.size()) > ncols) {
768  ncols = static_cast<unsigned int>(l.size());
769  }
770  }
771 
772  resize(nrows, ncols, false, false);
773  auto it = lists.begin();
774  for (unsigned int i = 0; i < rowNum; i++, ++it) {
775  std::copy(it->begin(), it->end(), rowPtrs[i]);
776  }
777 
778  return *this;
779 }
780 #endif
781 
784 {
785  std::fill(data, data + rowNum * colNum, x);
786  return *this;
787 }
788 
795 {
796  for (unsigned int i = 0; i < rowNum; i++) {
797  for (unsigned int j = 0; j < colNum; j++) {
798  rowPtrs[i][j] = *x++;
799  }
800  }
801  return *this;
802 }
803 
805 {
806  resize(1, 1, false, false);
807  rowPtrs[0][0] = val;
808  return *this;
809 }
810 
812 {
813  resize(1, colNum + 1, false, false);
814  rowPtrs[0][colNum - 1] = val;
815  return *this;
816 }
817 
854 {
855  unsigned int rows = A.getRows();
856  this->resize(rows, rows);
857 
858  (*this) = 0;
859  for (unsigned int i = 0; i < rows; i++)
860  (*this)[i][i] = A[i];
861 }
862 
893 void vpMatrix::diag(const double &val)
894 {
895  (*this) = 0;
896  unsigned int min_ = (rowNum < colNum) ? rowNum : colNum;
897  for (unsigned int i = 0; i < min_; i++)
898  (*this)[i][i] = val;
899 }
900 
913 {
914  unsigned int rows = A.getRows();
915  DA.resize(rows, rows, true);
916 
917  for (unsigned int i = 0; i < rows; i++)
918  DA[i][i] = A[i];
919 }
920 
926 {
927  vpTranslationVector t_out;
928 
929  if (rowNum != 3 || colNum != 3) {
930  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector",
931  rowNum, colNum, tv.getRows(), tv.getCols()));
932  }
933 
934  for (unsigned int j = 0; j < 3; j++)
935  t_out[j] = 0;
936 
937  for (unsigned int j = 0; j < 3; j++) {
938  double tj = tv[j]; // optimization em 5/12/2006
939  for (unsigned int i = 0; i < 3; i++) {
940  t_out[i] += rowPtrs[i][j] * tj;
941  }
942  }
943  return t_out;
944 }
945 
951 {
952  vpColVector v_out;
953  vpMatrix::multMatrixVector(*this, v, v_out);
954  return v_out;
955 }
956 
966 {
967  if (A.colNum != v.getRows()) {
968  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
969  A.getRows(), A.getCols(), v.getRows()));
970  }
971 
972  if (A.rowNum != w.rowNum)
973  w.resize(A.rowNum, false);
974 
975  // If available use Lapack only for large matrices
976  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size);
977 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
978  useLapack = false;
979 #endif
980 
981  if (useLapack) {
982 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
983  double alpha = 1.0;
984  double beta = 0.0;
985  char trans = 't';
986  int incr = 1;
987 
988  vpMatrix::blas_dgemv(trans, A.colNum, A.rowNum, alpha, A.data, A.colNum, v.data, incr, beta, w.data, incr);
989 #endif
990  }
991  else {
992  w = 0.0;
993  for (unsigned int j = 0; j < A.colNum; j++) {
994  double vj = v[j]; // optimization em 5/12/2006
995  for (unsigned int i = 0; i < A.rowNum; i++) {
996  w[i] += A.rowPtrs[i][j] * vj;
997  }
998  }
999  }
1000 }
1001 
1002 //---------------------------------
1003 // Matrix operations.
1004 //---------------------------------
1005 
1016 {
1017  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1018  C.resize(A.rowNum, B.colNum, false, false);
1019 
1020  if (A.colNum != B.rowNum) {
1021  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (%dx%d) matrix", A.getRows(),
1022  A.getCols(), B.getRows(), B.getCols()));
1023  }
1024 
1025  // If available use Lapack only for large matrices
1026  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size ||
1027  B.colNum > vpMatrix::m_lapack_min_size);
1028 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1029  useLapack = false;
1030 #endif
1031 
1032  if (useLapack) {
1033 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1034  const double alpha = 1.0;
1035  const double beta = 0.0;
1036  const char trans = 'n';
1037  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1038  C.data, B.colNum);
1039 #endif
1040  }
1041  else {
1042  // 5/12/06 some "very" simple optimization to avoid indexation
1043  const unsigned int BcolNum = B.colNum;
1044  const unsigned int BrowNum = B.rowNum;
1045  double **BrowPtrs = B.rowPtrs;
1046  for (unsigned int i = 0; i < A.rowNum; i++) {
1047  const double *rowptri = A.rowPtrs[i];
1048  double *ci = C[i];
1049  for (unsigned int j = 0; j < BcolNum; j++) {
1050  double s = 0;
1051  for (unsigned int k = 0; k < BrowNum; k++)
1052  s += rowptri[k] * BrowPtrs[k][j];
1053  ci[j] = s;
1054  }
1055  }
1056  }
1057 }
1058 
1073 {
1074  if (A.colNum != 3 || A.rowNum != 3 || B.colNum != 3 || B.rowNum != 3) {
1076  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1077  "rotation matrix",
1078  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1079  }
1080  // 5/12/06 some "very" simple optimization to avoid indexation
1081  const unsigned int BcolNum = B.colNum;
1082  const unsigned int BrowNum = B.rowNum;
1083  double **BrowPtrs = B.rowPtrs;
1084  for (unsigned int i = 0; i < A.rowNum; i++) {
1085  const double *rowptri = A.rowPtrs[i];
1086  double *ci = C[i];
1087  for (unsigned int j = 0; j < BcolNum; j++) {
1088  double s = 0;
1089  for (unsigned int k = 0; k < BrowNum; k++)
1090  s += rowptri[k] * BrowPtrs[k][j];
1091  ci[j] = s;
1092  }
1093  }
1094 }
1095 
1110 {
1111  if (A.colNum != 4 || A.rowNum != 4 || B.colNum != 4 || B.rowNum != 4) {
1113  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1114  "rotation matrix",
1115  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1116  }
1117  // Considering perfMatrixMultiplication.cpp benchmark,
1118  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1119  // Lapack usage needs to be validated again.
1120  // If available use Lapack only for large matrices.
1121  // Using SSE2 doesn't speed up.
1122  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size ||
1123  B.colNum > vpMatrix::m_lapack_min_size);
1124 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1125  useLapack = false;
1126 #endif
1127 
1128  if (useLapack) {
1129 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1130  const double alpha = 1.0;
1131  const double beta = 0.0;
1132  const char trans = 'n';
1133  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1134  C.data, B.colNum);
1135 #endif
1136  }
1137  else {
1138  // 5/12/06 some "very" simple optimization to avoid indexation
1139  const unsigned int BcolNum = B.colNum;
1140  const unsigned int BrowNum = B.rowNum;
1141  double **BrowPtrs = B.rowPtrs;
1142  for (unsigned int i = 0; i < A.rowNum; i++) {
1143  const double *rowptri = A.rowPtrs[i];
1144  double *ci = C[i];
1145  for (unsigned int j = 0; j < BcolNum; j++) {
1146  double s = 0;
1147  for (unsigned int k = 0; k < BrowNum; k++)
1148  s += rowptri[k] * BrowPtrs[k][j];
1149  ci[j] = s;
1150  }
1151  }
1152  }
1153 }
1154 
1168 {
1169  vpMatrix::multMatrixVector(A, B, C);
1170 }
1171 
1177 {
1178  vpMatrix C;
1179 
1180  vpMatrix::mult2Matrices(*this, B, C);
1181 
1182  return C;
1183 }
1184 
1190 {
1191  if (colNum != R.getRows()) {
1192  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1193  colNum));
1194  }
1195  vpMatrix C;
1196  C.resize(rowNum, 3, false, false);
1197 
1198  unsigned int RcolNum = R.getCols();
1199  unsigned int RrowNum = R.getRows();
1200  for (unsigned int i = 0; i < rowNum; i++) {
1201  double *rowptri = rowPtrs[i];
1202  double *ci = C[i];
1203  for (unsigned int j = 0; j < RcolNum; j++) {
1204  double s = 0;
1205  for (unsigned int k = 0; k < RrowNum; k++)
1206  s += rowptri[k] * R[k][j];
1207  ci[j] = s;
1208  }
1209  }
1210 
1211  return C;
1212 }
1213 
1219 {
1220  if (colNum != M.getRows()) {
1221  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1222  colNum));
1223  }
1224  vpMatrix C;
1225  C.resize(rowNum, 4, false, false);
1226 
1227  const unsigned int McolNum = M.getCols();
1228  const unsigned int MrowNum = M.getRows();
1229  for (unsigned int i = 0; i < rowNum; i++) {
1230  const double *rowptri = rowPtrs[i];
1231  double *ci = C[i];
1232  for (unsigned int j = 0; j < McolNum; j++) {
1233  double s = 0;
1234  for (unsigned int k = 0; k < MrowNum; k++)
1235  s += rowptri[k] * M[k][j];
1236  ci[j] = s;
1237  }
1238  }
1239 
1240  return C;
1241 }
1242 
1248 {
1249  if (colNum != V.getRows()) {
1250  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix",
1251  rowNum, colNum));
1252  }
1253  vpMatrix M;
1254  M.resize(rowNum, 6, false, false);
1255 
1256  // Considering perfMatrixMultiplication.cpp benchmark,
1257  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1258  // Lapack usage needs to be validated again.
1259  // If available use Lapack only for large matrices.
1260  // Speed up obtained using SSE2.
1261  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size ||
1262  V.colNum > vpMatrix::m_lapack_min_size);
1263 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1264  useLapack = false;
1265 #endif
1266 
1267  if (useLapack) {
1268 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1269  const double alpha = 1.0;
1270  const double beta = 0.0;
1271  const char trans = 'n';
1272  vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, V.colNum, data, colNum, beta, M.data,
1273  M.colNum);
1274 #endif
1275  }
1276  else {
1277 #if defined(VISP_HAVE_SIMDLIB)
1278  SimdMatMulTwist(data, rowNum, V.data, M.data);
1279 #else
1280  unsigned int VcolNum = V.getCols();
1281  unsigned int VrowNum = V.getRows();
1282  for (unsigned int i = 0; i < rowNum; i++) {
1283  double *rowptri = rowPtrs[i];
1284  double *ci = M[i];
1285  for (unsigned int j = 0; j < VcolNum; j++) {
1286  double s = 0;
1287  for (unsigned int k = 0; k < VrowNum; k++)
1288  s += rowptri[k] * V[k][j];
1289  ci[j] = s;
1290  }
1291  }
1292 #endif
1293  }
1294 
1295  return M;
1296 }
1297 
1303 {
1304  if (colNum != V.getRows()) {
1305  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) force/torque twist matrix",
1306  rowNum, colNum));
1307  }
1308  vpMatrix M;
1309  M.resize(rowNum, 6, false, false);
1310 
1311  // Considering perfMatrixMultiplication.cpp benchmark,
1312  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1313  // Lapack usage needs to be validated again.
1314  // If available use Lapack only for large matrices.
1315  // Speed up obtained using SSE2.
1316  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size ||
1317  V.getCols() > vpMatrix::m_lapack_min_size);
1318 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1319  useLapack = false;
1320 #endif
1321 
1322  if (useLapack) {
1323 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1324  const double alpha = 1.0;
1325  const double beta = 0.0;
1326  const char trans = 'n';
1327  vpMatrix::blas_dgemm(trans, trans, V.getCols(), rowNum, colNum, alpha, V.data, V.getCols(), data, colNum, beta,
1328  M.data, M.colNum);
1329 #endif
1330  }
1331  else {
1332 #if defined(VISP_HAVE_SIMDLIB)
1333  SimdMatMulTwist(data, rowNum, V.data, M.data);
1334 #else
1335  unsigned int VcolNum = V.getCols();
1336  unsigned int VrowNum = V.getRows();
1337  for (unsigned int i = 0; i < rowNum; i++) {
1338  double *rowptri = rowPtrs[i];
1339  double *ci = M[i];
1340  for (unsigned int j = 0; j < VcolNum; j++) {
1341  double s = 0;
1342  for (unsigned int k = 0; k < VrowNum; k++)
1343  s += rowptri[k] * V[k][j];
1344  ci[j] = s;
1345  }
1346  }
1347 #endif
1348  }
1349 
1350  return M;
1351 }
1352 
1363 void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB,
1364  vpMatrix &C)
1365 {
1366  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1367  C.resize(A.rowNum, B.colNum, false, false);
1368 
1369  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1370  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1371  A.getCols(), B.getRows(), B.getCols()));
1372  }
1373 
1374  double **ArowPtrs = A.rowPtrs;
1375  double **BrowPtrs = B.rowPtrs;
1376  double **CrowPtrs = C.rowPtrs;
1377 
1378  for (unsigned int i = 0; i < A.rowNum; i++)
1379  for (unsigned int j = 0; j < A.colNum; j++)
1380  CrowPtrs[i][j] = wB * BrowPtrs[i][j] + wA * ArowPtrs[i][j];
1381 }
1382 
1392 void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1393 {
1394  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1395  C.resize(A.rowNum, B.colNum, false, false);
1396 
1397  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1398  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1399  A.getCols(), B.getRows(), B.getCols()));
1400  }
1401 
1402  double **ArowPtrs = A.rowPtrs;
1403  double **BrowPtrs = B.rowPtrs;
1404  double **CrowPtrs = C.rowPtrs;
1405 
1406  for (unsigned int i = 0; i < A.rowNum; i++) {
1407  for (unsigned int j = 0; j < A.colNum; j++) {
1408  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1409  }
1410  }
1411 }
1412 
1426 {
1427  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1428  C.resize(A.rowNum);
1429 
1430  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1431  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1432  A.getCols(), B.getRows(), B.getCols()));
1433  }
1434 
1435  double **ArowPtrs = A.rowPtrs;
1436  double **BrowPtrs = B.rowPtrs;
1437  double **CrowPtrs = C.rowPtrs;
1438 
1439  for (unsigned int i = 0; i < A.rowNum; i++) {
1440  for (unsigned int j = 0; j < A.colNum; j++) {
1441  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1442  }
1443  }
1444 }
1445 
1451 {
1452  vpMatrix C;
1453  vpMatrix::add2Matrices(*this, B, C);
1454  return C;
1455 }
1456 
1473 {
1474  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1475  C.resize(A.rowNum);
1476 
1477  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1478  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1479  A.getCols(), B.getRows(), B.getCols()));
1480  }
1481 
1482  double **ArowPtrs = A.rowPtrs;
1483  double **BrowPtrs = B.rowPtrs;
1484  double **CrowPtrs = C.rowPtrs;
1485 
1486  for (unsigned int i = 0; i < A.rowNum; i++) {
1487  for (unsigned int j = 0; j < A.colNum; j++) {
1488  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1489  }
1490  }
1491 }
1492 
1505 void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1506 {
1507  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1508  C.resize(A.rowNum, A.colNum, false, false);
1509 
1510  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1511  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1512  A.getCols(), B.getRows(), B.getCols()));
1513  }
1514 
1515  double **ArowPtrs = A.rowPtrs;
1516  double **BrowPtrs = B.rowPtrs;
1517  double **CrowPtrs = C.rowPtrs;
1518 
1519  for (unsigned int i = 0; i < A.rowNum; i++) {
1520  for (unsigned int j = 0; j < A.colNum; j++) {
1521  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1522  }
1523  }
1524 }
1525 
1531 {
1532  vpMatrix C;
1533  vpMatrix::sub2Matrices(*this, B, C);
1534  return C;
1535 }
1536 
1538 
1540 {
1541  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1542  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1543  B.getRows(), B.getCols()));
1544  }
1545 
1546  double **BrowPtrs = B.rowPtrs;
1547 
1548  for (unsigned int i = 0; i < rowNum; i++)
1549  for (unsigned int j = 0; j < colNum; j++)
1550  rowPtrs[i][j] += BrowPtrs[i][j];
1551 
1552  return *this;
1553 }
1554 
1557 {
1558  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1559  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1560  B.getRows(), B.getCols()));
1561  }
1562 
1563  double **BrowPtrs = B.rowPtrs;
1564  for (unsigned int i = 0; i < rowNum; i++)
1565  for (unsigned int j = 0; j < colNum; j++)
1566  rowPtrs[i][j] -= BrowPtrs[i][j];
1567 
1568  return *this;
1569 }
1570 
1581 {
1582  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1583  C.resize(A.rowNum, A.colNum, false, false);
1584 
1585  double **ArowPtrs = A.rowPtrs;
1586  double **CrowPtrs = C.rowPtrs;
1587 
1588  for (unsigned int i = 0; i < A.rowNum; i++)
1589  for (unsigned int j = 0; j < A.colNum; j++)
1590  CrowPtrs[i][j] = -ArowPtrs[i][j];
1591 }
1592 
1598 {
1599  vpMatrix C;
1600  vpMatrix::negateMatrix(*this, C);
1601  return C;
1602 }
1603 
1604 double vpMatrix::sum() const
1605 {
1606  double s = 0.0;
1607  for (unsigned int i = 0; i < rowNum; i++) {
1608  for (unsigned int j = 0; j < colNum; j++) {
1609  s += rowPtrs[i][j];
1610  }
1611  }
1612 
1613  return s;
1614 }
1615 
1616 //---------------------------------
1617 // Matrix/vector operations.
1618 //---------------------------------
1619 
1620 //---------------------------------
1621 // Matrix/real operations.
1622 //---------------------------------
1623 
1628 vpMatrix operator*(const double &x, const vpMatrix &B)
1629 {
1630  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1631  return B;
1632  }
1633 
1634  unsigned int Brow = B.getRows();
1635  unsigned int Bcol = B.getCols();
1636 
1637  vpMatrix C;
1638  C.resize(Brow, Bcol, false, false);
1639 
1640  for (unsigned int i = 0; i < Brow; i++)
1641  for (unsigned int j = 0; j < Bcol; j++)
1642  C[i][j] = B[i][j] * x;
1643 
1644  return C;
1645 }
1646 
1652 {
1653  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1654  return (*this);
1655  }
1656 
1657  vpMatrix M;
1658  M.resize(rowNum, colNum, false, false);
1659 
1660  for (unsigned int i = 0; i < rowNum; i++)
1661  for (unsigned int j = 0; j < colNum; j++)
1662  M[i][j] = rowPtrs[i][j] * x;
1663 
1664  return M;
1665 }
1666 
1669 {
1670  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1671  return (*this);
1672  }
1673 
1674  if (std::fabs(x) < std::numeric_limits<double>::epsilon()) {
1675  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1676  }
1677 
1678  vpMatrix C;
1679  C.resize(rowNum, colNum, false, false);
1680 
1681  double xinv = 1 / x;
1682 
1683  for (unsigned int i = 0; i < rowNum; i++)
1684  for (unsigned int j = 0; j < colNum; j++)
1685  C[i][j] = rowPtrs[i][j] * xinv;
1686 
1687  return C;
1688 }
1689 
1692 {
1693  for (unsigned int i = 0; i < rowNum; i++)
1694  for (unsigned int j = 0; j < colNum; j++)
1695  rowPtrs[i][j] += x;
1696 
1697  return *this;
1698 }
1699 
1702 {
1703  for (unsigned int i = 0; i < rowNum; i++)
1704  for (unsigned int j = 0; j < colNum; j++)
1705  rowPtrs[i][j] -= x;
1706 
1707  return *this;
1708 }
1709 
1715 {
1716  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1717  return *this;
1718  }
1719 
1720  for (unsigned int i = 0; i < rowNum; i++)
1721  for (unsigned int j = 0; j < colNum; j++)
1722  rowPtrs[i][j] *= x;
1723 
1724  return *this;
1725 }
1726 
1729 {
1730  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1731  return *this;
1732  }
1733 
1734  if (std::fabs(x) < std::numeric_limits<double>::epsilon())
1735  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1736 
1737  double xinv = 1 / x;
1738 
1739  for (unsigned int i = 0; i < rowNum; i++)
1740  for (unsigned int j = 0; j < colNum; j++)
1741  rowPtrs[i][j] *= xinv;
1742 
1743  return *this;
1744 }
1745 
1746 //----------------------------------------------------------------
1747 // Matrix Operation
1748 //----------------------------------------------------------------
1749 
1755 {
1756  if ((out.rowNum != colNum * rowNum) || (out.colNum != 1))
1757  out.resize(colNum * rowNum, false, false);
1758 
1759  double *optr = out.data;
1760  for (unsigned int j = 0; j < colNum; j++) {
1761  for (unsigned int i = 0; i < rowNum; i++) {
1762  *(optr++) = rowPtrs[i][j];
1763  }
1764  }
1765 }
1766 
1772 {
1773  vpColVector out(colNum * rowNum);
1774  stackColumns(out);
1775  return out;
1776 }
1777 
1783 {
1784  if ((out.getRows() != 1) || (out.getCols() != colNum * rowNum))
1785  out.resize(colNum * rowNum, false, false);
1786 
1787  memcpy(out.data, data, sizeof(double) * out.getCols());
1788 }
1794 {
1795  vpRowVector out(colNum * rowNum);
1796  stackRows(out);
1797  return out;
1798 }
1799 
1807 {
1808  if (m.getRows() != rowNum || m.getCols() != colNum) {
1809  throw(vpException(vpException::dimensionError, "In Hadamard product: bad dimension of input matrix"));
1810  }
1811 
1812  vpMatrix out;
1813  out.resize(rowNum, colNum, false, false);
1814 
1815 #if defined(VISP_HAVE_SIMDLIB)
1816  SimdVectorHadamard(data, m.data, dsize, out.data);
1817 #else
1818  for (unsigned int i = 0; i < dsize; ++i) {
1819  out.data[i] = data[i] * m.data[i];
1820  }
1821 #endif
1822 
1823  return out;
1824 }
1825 
1832 void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out)
1833 {
1834  unsigned int r1 = m1.getRows();
1835  unsigned int c1 = m1.getCols();
1836  unsigned int r2 = m2.getRows();
1837  unsigned int c2 = m2.getCols();
1838 
1839  out.resize(r1 * r2, c1 * c2, false, false);
1840 
1841  for (unsigned int r = 0; r < r1; r++) {
1842  for (unsigned int c = 0; c < c1; c++) {
1843  double alpha = m1[r][c];
1844  double *m2ptr = m2[0];
1845  unsigned int roffset = r * r2;
1846  unsigned int coffset = c * c2;
1847  for (unsigned int rr = 0; rr < r2; rr++) {
1848  for (unsigned int cc = 0; cc < c2; cc++) {
1849  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1850  }
1851  }
1852  }
1853  }
1854 }
1855 
1862 void vpMatrix::kron(const vpMatrix &m, vpMatrix &out) const { kron(*this, m, out); }
1863 
1871 {
1872  unsigned int r1 = m1.getRows();
1873  unsigned int c1 = m1.getCols();
1874  unsigned int r2 = m2.getRows();
1875  unsigned int c2 = m2.getCols();
1876 
1877  vpMatrix out;
1878  out.resize(r1 * r2, c1 * c2, false, false);
1879 
1880  for (unsigned int r = 0; r < r1; r++) {
1881  for (unsigned int c = 0; c < c1; c++) {
1882  double alpha = m1[r][c];
1883  double *m2ptr = m2[0];
1884  unsigned int roffset = r * r2;
1885  unsigned int coffset = c * c2;
1886  for (unsigned int rr = 0; rr < r2; rr++) {
1887  for (unsigned int cc = 0; cc < c2; cc++) {
1888  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1889  }
1890  }
1891  }
1892  }
1893  return out;
1894 }
1895 
1901 vpMatrix vpMatrix::kron(const vpMatrix &m) const { return kron(*this, m); }
1902 
1953 void vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const { x = pseudoInverse(1e-6) * b; }
1954 
2005 {
2006  vpColVector X(colNum);
2007 
2008  solveBySVD(B, X);
2009  return X;
2010 }
2011 
2076 {
2077 #if defined(VISP_HAVE_LAPACK)
2078  svdLapack(w, V);
2079 #elif defined(VISP_HAVE_EIGEN3)
2080  svdEigen3(w, V);
2081 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2082  svdOpenCV(w, V);
2083 #else
2084  (void)w;
2085  (void)V;
2086  throw(vpException(vpException::fatalError, "Cannot compute SVD. Install Lapack, Eigen3 or OpenCV 3rd party"));
2087 #endif
2088 }
2089 
2144 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
2145 {
2146 #if defined(VISP_HAVE_LAPACK)
2147  return pseudoInverseLapack(Ap, svThreshold);
2148 #elif defined(VISP_HAVE_EIGEN3)
2149  return pseudoInverseEigen3(Ap, svThreshold);
2150 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2151  return pseudoInverseOpenCV(Ap, svThreshold);
2152 #else
2153  (void)Ap;
2154  (void)svThreshold;
2155  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2156  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2157 #endif
2158 }
2159 
2220 int vpMatrix::pseudoInverse(vpMatrix &Ap, int rank_in) const
2221 {
2222 #if defined(VISP_HAVE_LAPACK)
2223  return pseudoInverseLapack(Ap, rank_in);
2224 #elif defined(VISP_HAVE_EIGEN3)
2225  return pseudoInverseEigen3(Ap, rank_in);
2226 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2227  return pseudoInverseOpenCV(Ap, rank_in);
2228 #else
2229  (void)Ap;
2230  (void)rank_in;
2231  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2232  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2233 #endif
2234 }
2235 
2286 vpMatrix vpMatrix::pseudoInverse(double svThreshold) const
2287 {
2288 #if defined(VISP_HAVE_LAPACK)
2289  return pseudoInverseLapack(svThreshold);
2290 #elif defined(VISP_HAVE_EIGEN3)
2291  return pseudoInverseEigen3(svThreshold);
2292 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2293  return pseudoInverseOpenCV(svThreshold);
2294 #else
2295  (void)svThreshold;
2296  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2297  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2298 #endif
2299 }
2300 
2352 {
2353 #if defined(VISP_HAVE_LAPACK)
2354  return pseudoInverseLapack(rank_in);
2355 #elif defined(VISP_HAVE_EIGEN3)
2356  return pseudoInverseEigen3(rank_in);
2357 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2358  return pseudoInverseOpenCV(rank_in);
2359 #else
2360  (void)rank_in;
2361  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2362  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2363 #endif
2364 }
2365 
2366 #ifndef DOXYGEN_SHOULD_SKIP_THIS
2367 #if defined(VISP_HAVE_LAPACK)
2404 vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const
2405 {
2406  unsigned int nrows = getRows();
2407  unsigned int ncols = getCols();
2408  int rank_out;
2409 
2410  vpMatrix U, V, Ap;
2411  vpColVector sv;
2412 
2413  Ap.resize(ncols, nrows, false, false);
2414 
2415  if (nrows < ncols) {
2416  U.resize(ncols, ncols, true);
2417  sv.resize(nrows, false);
2418  }
2419  else {
2420  U.resize(nrows, ncols, false);
2421  sv.resize(ncols, false);
2422  }
2423 
2424  U.insert(*this, 0, 0);
2425  U.svdLapack(sv, V);
2426 
2427  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
2428 
2429  return Ap;
2430 }
2431 
2472 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) const
2473 {
2474  unsigned int nrows = getRows();
2475  unsigned int ncols = getCols();
2476  int rank_out;
2477 
2478  vpMatrix U, V;
2479  vpColVector sv;
2480 
2481  Ap.resize(ncols, nrows, false, false);
2482 
2483  if (nrows < ncols) {
2484  U.resize(ncols, ncols, true);
2485  sv.resize(nrows, false);
2486  }
2487  else {
2488  U.resize(nrows, ncols, false);
2489  sv.resize(ncols, false);
2490  }
2491 
2492  U.insert(*this, 0, 0);
2493  U.svdLapack(sv, V);
2494 
2495  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
2496 
2497  return static_cast<unsigned int>(rank_out);
2498 }
2546 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2547 {
2548  unsigned int nrows = getRows();
2549  unsigned int ncols = getCols();
2550  int rank_out;
2551 
2552  vpMatrix U, V;
2553  vpColVector sv_;
2554 
2555  Ap.resize(ncols, nrows, false, false);
2556 
2557  if (nrows < ncols) {
2558  U.resize(ncols, ncols, true);
2559  sv.resize(nrows, false);
2560  }
2561  else {
2562  U.resize(nrows, ncols, false);
2563  sv.resize(ncols, false);
2564  }
2565 
2566  U.insert(*this, 0, 0);
2567  U.svdLapack(sv_, V);
2568 
2569  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
2570 
2571  // Remove singular values equal to the one that corresponds to the lines of 0
2572  // introduced when m < n
2573  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2574 
2575  return static_cast<unsigned int>(rank_out);
2576 }
2577 
2684 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
2685  vpMatrix &imAt, vpMatrix &kerAt) const
2686 {
2687  unsigned int nrows = getRows();
2688  unsigned int ncols = getCols();
2689  int rank_out;
2690  vpMatrix U, V;
2691  vpColVector sv_;
2692 
2693  if (nrows < ncols) {
2694  U.resize(ncols, ncols, true);
2695  sv.resize(nrows, false);
2696  }
2697  else {
2698  U.resize(nrows, ncols, false);
2699  sv.resize(ncols, false);
2700  }
2701 
2702  U.insert(*this, 0, 0);
2703  U.svdLapack(sv_, V);
2704 
2705  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt);
2706 
2707  // Remove singular values equal to the one that corresponds to the lines of 0
2708  // introduced when m < n
2709  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2710 
2711  return static_cast<unsigned int>(rank_out);
2712 }
2713 
2750 vpMatrix vpMatrix::pseudoInverseLapack(int rank_in) const
2751 {
2752  unsigned int nrows = getRows();
2753  unsigned int ncols = getCols();
2754  int rank_out;
2755  double svThreshold = 1e-26;
2756 
2757  vpMatrix U, V, Ap;
2758  vpColVector sv;
2759 
2760  Ap.resize(ncols, nrows, false, false);
2761 
2762  if (nrows < ncols) {
2763  U.resize(ncols, ncols, true);
2764  sv.resize(nrows, false);
2765  }
2766  else {
2767  U.resize(nrows, ncols, false);
2768  sv.resize(ncols, false);
2769  }
2770 
2771  U.insert(*this, 0, 0);
2772  U.svdLapack(sv, V);
2773 
2774  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
2775 
2776  return Ap;
2777 }
2778 
2825 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, int rank_in) const
2826 {
2827  unsigned int nrows = getRows();
2828  unsigned int ncols = getCols();
2829  int rank_out;
2830  double svThreshold = 1e-26;
2831 
2832  vpMatrix U, V;
2833  vpColVector sv;
2834 
2835  Ap.resize(ncols, nrows, false, false);
2836 
2837  if (nrows < ncols) {
2838  U.resize(ncols, ncols, true);
2839  sv.resize(nrows, false);
2840  }
2841  else {
2842  U.resize(nrows, ncols, false);
2843  sv.resize(ncols, false);
2844  }
2845 
2846  U.insert(*this, 0, 0);
2847  U.svdLapack(sv, V);
2848 
2849  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
2850 
2851  return rank_out;
2852 }
2853 
2907 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in) const
2908 {
2909  unsigned int nrows = getRows();
2910  unsigned int ncols = getCols();
2911  int rank_out;
2912  double svThreshold = 1e-26;
2913 
2914  vpMatrix U, V;
2915  vpColVector sv_;
2916 
2917  Ap.resize(ncols, nrows, false, false);
2918 
2919  if (nrows < ncols) {
2920  U.resize(ncols, ncols, true);
2921  sv.resize(nrows, false);
2922  }
2923  else {
2924  U.resize(nrows, ncols, false);
2925  sv.resize(ncols, false);
2926  }
2927 
2928  U.insert(*this, 0, 0);
2929  U.svdLapack(sv_, V);
2930 
2931  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
2932 
2933  // Remove singular values equal to the one that corresponds to the lines of 0
2934  // introduced when m < n
2935  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2936 
2937  return rank_out;
2938 }
2939 
3051 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
3052  vpMatrix &kerAt) const
3053 {
3054  unsigned int nrows = getRows();
3055  unsigned int ncols = getCols();
3056  int rank_out;
3057  double svThreshold = 1e-26;
3058  vpMatrix U, V;
3059  vpColVector sv_;
3060 
3061  if (nrows < ncols) {
3062  U.resize(ncols, ncols, true);
3063  sv.resize(nrows, false);
3064  }
3065  else {
3066  U.resize(nrows, ncols, false);
3067  sv.resize(ncols, false);
3068  }
3069 
3070  U.insert(*this, 0, 0);
3071  U.svdLapack(sv_, V);
3072 
3073  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
3074 
3075  // Remove singular values equal to the one that corresponds to the lines of 0
3076  // introduced when m < n
3077  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3078 
3079  return rank_out;
3080 }
3081 
3082 #endif
3083 #if defined(VISP_HAVE_EIGEN3)
3120 vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const
3121 {
3122  unsigned int nrows = getRows();
3123  unsigned int ncols = getCols();
3124  int rank_out;
3125 
3126  vpMatrix U, V, Ap;
3127  vpColVector sv;
3128 
3129  Ap.resize(ncols, nrows, false, false);
3130 
3131  if (nrows < ncols) {
3132  U.resize(ncols, ncols, true);
3133  sv.resize(nrows, false);
3134  }
3135  else {
3136  U.resize(nrows, ncols, false);
3137  sv.resize(ncols, false);
3138  }
3139 
3140  U.insert(*this, 0, 0);
3141  U.svdEigen3(sv, V);
3142 
3143  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
3144 
3145  return Ap;
3146 }
3147 
3188 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) const
3189 {
3190  unsigned int nrows = getRows();
3191  unsigned int ncols = getCols();
3192  int rank_out;
3193 
3194  vpMatrix U, V;
3195  vpColVector sv;
3196 
3197  Ap.resize(ncols, nrows, false, false);
3198 
3199  if (nrows < ncols) {
3200  U.resize(ncols, ncols, true);
3201  sv.resize(nrows, false);
3202  }
3203  else {
3204  U.resize(nrows, ncols, false);
3205  sv.resize(ncols, false);
3206  }
3207 
3208  U.insert(*this, 0, 0);
3209  U.svdEigen3(sv, V);
3210 
3211  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
3212 
3213  return static_cast<unsigned int>(rank_out);
3214 }
3262 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3263 {
3264  unsigned int nrows = getRows();
3265  unsigned int ncols = getCols();
3266  int rank_out;
3267 
3268  vpMatrix U, V;
3269  vpColVector sv_;
3270 
3271  Ap.resize(ncols, nrows, false, false);
3272 
3273  if (nrows < ncols) {
3274  U.resize(ncols, ncols, true);
3275  sv.resize(nrows, false);
3276  }
3277  else {
3278  U.resize(nrows, ncols, false);
3279  sv.resize(ncols, false);
3280  }
3281 
3282  U.insert(*this, 0, 0);
3283  U.svdEigen3(sv_, V);
3284 
3285  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
3286 
3287  // Remove singular values equal to the one that corresponds to the lines of 0
3288  // introduced when m < n
3289  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3290 
3291  return static_cast<unsigned int>(rank_out);
3292 }
3293 
3400 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3401  vpMatrix &imAt, vpMatrix &kerAt) const
3402 {
3403  unsigned int nrows = getRows();
3404  unsigned int ncols = getCols();
3405  int rank_out;
3406  vpMatrix U, V;
3407  vpColVector sv_;
3408 
3409  if (nrows < ncols) {
3410  U.resize(ncols, ncols, true);
3411  sv.resize(nrows, false);
3412  }
3413  else {
3414  U.resize(nrows, ncols, false);
3415  sv.resize(ncols, false);
3416  }
3417 
3418  U.insert(*this, 0, 0);
3419  U.svdEigen3(sv_, V);
3420 
3421  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt);
3422 
3423  // Remove singular values equal to the one that corresponds to the lines of 0
3424  // introduced when m < n
3425  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3426 
3427  return static_cast<unsigned int>(rank_out);
3428 }
3429 
3466 vpMatrix vpMatrix::pseudoInverseEigen3(int rank_in) const
3467 {
3468  unsigned int nrows = getRows();
3469  unsigned int ncols = getCols();
3470  int rank_out;
3471  double svThreshold = 1e-26;
3472 
3473  vpMatrix U, V, Ap;
3474  vpColVector sv;
3475 
3476  Ap.resize(ncols, nrows, false, false);
3477 
3478  if (nrows < ncols) {
3479  U.resize(ncols, ncols, true);
3480  sv.resize(nrows, false);
3481  }
3482  else {
3483  U.resize(nrows, ncols, false);
3484  sv.resize(ncols, false);
3485  }
3486 
3487  U.insert(*this, 0, 0);
3488  U.svdEigen3(sv, V);
3489 
3490  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
3491 
3492  return Ap;
3493 }
3494 
3541 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, int rank_in) const
3542 {
3543  unsigned int nrows = getRows();
3544  unsigned int ncols = getCols();
3545  int rank_out;
3546  double svThreshold = 1e-26;
3547 
3548  vpMatrix U, V;
3549  vpColVector sv;
3550 
3551  Ap.resize(ncols, nrows, false, false);
3552 
3553  if (nrows < ncols) {
3554  U.resize(ncols, ncols, true);
3555  sv.resize(nrows, false);
3556  }
3557  else {
3558  U.resize(nrows, ncols, false);
3559  sv.resize(ncols, false);
3560  }
3561 
3562  U.insert(*this, 0, 0);
3563  U.svdEigen3(sv, V);
3564 
3565  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
3566 
3567  return rank_out;
3568 }
3569 
3623 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in) const
3624 {
3625  unsigned int nrows = getRows();
3626  unsigned int ncols = getCols();
3627  int rank_out;
3628  double svThreshold = 1e-26;
3629 
3630  vpMatrix U, V;
3631  vpColVector sv_;
3632 
3633  Ap.resize(ncols, nrows, false, false);
3634 
3635  if (nrows < ncols) {
3636  U.resize(ncols, ncols, true);
3637  sv.resize(nrows, false);
3638  }
3639  else {
3640  U.resize(nrows, ncols, false);
3641  sv.resize(ncols, false);
3642  }
3643 
3644  U.insert(*this, 0, 0);
3645  U.svdEigen3(sv_, V);
3646 
3647  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
3648 
3649  // Remove singular values equal to the one that corresponds to the lines of 0
3650  // introduced when m < n
3651  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3652 
3653  return rank_out;
3654 }
3655 
3767 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
3768  vpMatrix &kerAt) const
3769 {
3770  unsigned int nrows = getRows();
3771  unsigned int ncols = getCols();
3772  int rank_out;
3773  double svThreshold = 1e-26;
3774  vpMatrix U, V;
3775  vpColVector sv_;
3776 
3777  if (nrows < ncols) {
3778  U.resize(ncols, ncols, true);
3779  sv.resize(nrows, false);
3780  }
3781  else {
3782  U.resize(nrows, ncols, false);
3783  sv.resize(ncols, false);
3784  }
3785 
3786  U.insert(*this, 0, 0);
3787  U.svdEigen3(sv_, V);
3788 
3789  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
3790 
3791  // Remove singular values equal to the one that corresponds to the lines of 0
3792  // introduced when m < n
3793  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3794 
3795  return rank_out;
3796 }
3797 
3798 #endif
3799 #if defined(VISP_HAVE_OPENCV)
3836 vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const
3837 {
3838  unsigned int nrows = getRows();
3839  unsigned int ncols = getCols();
3840  int rank_out;
3841 
3842  vpMatrix U, V, Ap;
3843  vpColVector sv;
3844 
3845  Ap.resize(ncols, nrows, false, false);
3846 
3847  if (nrows < ncols) {
3848  U.resize(ncols, ncols, true);
3849  sv.resize(nrows, false);
3850  }
3851  else {
3852  U.resize(nrows, ncols, false);
3853  sv.resize(ncols, false);
3854  }
3855 
3856  U.insert(*this, 0, 0);
3857  U.svdOpenCV(sv, V);
3858 
3859  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
3860 
3861  return Ap;
3862 }
3863 
3904 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) const
3905 {
3906  unsigned int nrows = getRows();
3907  unsigned int ncols = getCols();
3908  int rank_out;
3909 
3910  vpMatrix U, V;
3911  vpColVector sv;
3912 
3913  Ap.resize(ncols, nrows, false, false);
3914 
3915  if (nrows < ncols) {
3916  U.resize(ncols, ncols, true);
3917  sv.resize(nrows, false);
3918  }
3919  else {
3920  U.resize(nrows, ncols, false);
3921  sv.resize(ncols, false);
3922  }
3923 
3924  U.insert(*this, 0, 0);
3925  U.svdOpenCV(sv, V);
3926 
3927  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
3928 
3929  return static_cast<unsigned int>(rank_out);
3930 }
3978 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3979 {
3980  unsigned int nrows = getRows();
3981  unsigned int ncols = getCols();
3982  int rank_out;
3983 
3984  vpMatrix U, V;
3985  vpColVector sv_;
3986 
3987  Ap.resize(ncols, nrows, false, false);
3988 
3989  if (nrows < ncols) {
3990  U.resize(ncols, ncols, true);
3991  sv.resize(nrows, false);
3992  }
3993  else {
3994  U.resize(nrows, ncols, false);
3995  sv.resize(ncols, false);
3996  }
3997 
3998  U.insert(*this, 0, 0);
3999  U.svdOpenCV(sv_, V);
4000 
4001  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
4002 
4003  // Remove singular values equal to the one that corresponds to the lines of 0
4004  // introduced when m < n
4005  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4006 
4007  return static_cast<unsigned int>(rank_out);
4008 }
4009 
4116 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
4117  vpMatrix &imAt, vpMatrix &kerAt) const
4118 {
4119  unsigned int nrows = getRows();
4120  unsigned int ncols = getCols();
4121  int rank_out;
4122  vpMatrix U, V;
4123  vpColVector sv_;
4124 
4125  if (nrows < ncols) {
4126  U.resize(ncols, ncols, true);
4127  sv.resize(nrows, false);
4128  }
4129  else {
4130  U.resize(nrows, ncols, false);
4131  sv.resize(ncols, false);
4132  }
4133 
4134  U.insert(*this, 0, 0);
4135  U.svdOpenCV(sv_, V);
4136 
4137  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt);
4138 
4139  // Remove singular values equal to the one that corresponds to the lines of 0
4140  // introduced when m < n
4141  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4142 
4143  return static_cast<unsigned int>(rank_out);
4144 }
4145 
4182 vpMatrix vpMatrix::pseudoInverseOpenCV(int rank_in) const
4183 {
4184  unsigned int nrows = getRows();
4185  unsigned int ncols = getCols();
4186  int rank_out;
4187  double svThreshold = 1e-26;
4188 
4189  vpMatrix U, V, Ap;
4190  vpColVector sv;
4191 
4192  Ap.resize(ncols, nrows, false, false);
4193 
4194  if (nrows < ncols) {
4195  U.resize(ncols, ncols, true);
4196  sv.resize(nrows, false);
4197  }
4198  else {
4199  U.resize(nrows, ncols, false);
4200  sv.resize(ncols, false);
4201  }
4202 
4203  U.insert(*this, 0, 0);
4204  U.svdOpenCV(sv, V);
4205 
4206  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
4207 
4208  return Ap;
4209 }
4210 
4257 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, int rank_in) const
4258 {
4259  unsigned int nrows = getRows();
4260  unsigned int ncols = getCols();
4261  int rank_out;
4262  double svThreshold = 1e-26;
4263 
4264  vpMatrix U, V;
4265  vpColVector sv;
4266 
4267  Ap.resize(ncols, nrows, false, false);
4268 
4269  if (nrows < ncols) {
4270  U.resize(ncols, ncols, true);
4271  sv.resize(nrows, false);
4272  }
4273  else {
4274  U.resize(nrows, ncols, false);
4275  sv.resize(ncols, false);
4276  }
4277 
4278  U.insert(*this, 0, 0);
4279  U.svdOpenCV(sv, V);
4280 
4281  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
4282 
4283  return rank_out;
4284 }
4285 
4339 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4340 {
4341  unsigned int nrows = getRows();
4342  unsigned int ncols = getCols();
4343  int rank_out;
4344  double svThreshold = 1e-26;
4345 
4346  vpMatrix U, V;
4347  vpColVector sv_;
4348 
4349  Ap.resize(ncols, nrows, false, false);
4350 
4351  if (nrows < ncols) {
4352  U.resize(ncols, ncols, true);
4353  sv.resize(nrows, false);
4354  }
4355  else {
4356  U.resize(nrows, ncols, false);
4357  sv.resize(ncols, false);
4358  }
4359 
4360  U.insert(*this, 0, 0);
4361  U.svdOpenCV(sv_, V);
4362 
4363  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
4364 
4365  // Remove singular values equal to the one that corresponds to the lines of 0
4366  // introduced when m < n
4367  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4368 
4369  return rank_out;
4370 }
4371 
4483 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
4484  vpMatrix &kerAt) const
4485 {
4486  unsigned int nrows = getRows();
4487  unsigned int ncols = getCols();
4488  int rank_out;
4489  double svThreshold = 1e-26;
4490  vpMatrix U, V;
4491  vpColVector sv_;
4492 
4493  if (nrows < ncols) {
4494  U.resize(ncols, ncols, true);
4495  sv.resize(nrows, false);
4496  }
4497  else {
4498  U.resize(nrows, ncols, false);
4499  sv.resize(ncols, false);
4500  }
4501 
4502  U.insert(*this, 0, 0);
4503  U.svdOpenCV(sv_, V);
4504 
4505  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
4506 
4507  // Remove singular values equal to the one that corresponds to the lines of 0
4508  // introduced when m < n
4509  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4510 
4511  return rank_out;
4512 }
4513 
4514 #endif
4515 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
4516 
4578 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
4579 {
4580 #if defined(VISP_HAVE_LAPACK)
4581  return pseudoInverseLapack(Ap, sv, svThreshold);
4582 #elif defined(VISP_HAVE_EIGEN3)
4583  return pseudoInverseEigen3(Ap, sv, svThreshold);
4584 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
4585  return pseudoInverseOpenCV(Ap, sv, svThreshold);
4586 #else
4587  (void)Ap;
4588  (void)sv;
4589  (void)svThreshold;
4590  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4591  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4592 #endif
4593 }
4594 
4661 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4662 {
4663 #if defined(VISP_HAVE_LAPACK)
4664  return pseudoInverseLapack(Ap, sv, rank_in);
4665 #elif defined(VISP_HAVE_EIGEN3)
4666  return pseudoInverseEigen3(Ap, sv, rank_in);
4667 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
4668  return pseudoInverseOpenCV(Ap, sv, rank_in);
4669 #else
4670  (void)Ap;
4671  (void)sv;
4672  (void)rank_in;
4673  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4674  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4675 #endif
4676 }
4751 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt) const
4752 {
4753  vpMatrix kerAt;
4754  return pseudoInverse(Ap, sv, svThreshold, imA, imAt, kerAt);
4755 }
4756 
4835 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt) const
4836 {
4837  vpMatrix kerAt;
4838  return pseudoInverse(Ap, sv, rank_in, imA, imAt, kerAt);
4839 }
4840 
4942 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt,
4943  vpMatrix &kerAt) const
4944 {
4945 #if defined(VISP_HAVE_LAPACK)
4946  return pseudoInverseLapack(Ap, sv, svThreshold, imA, imAt, kerAt);
4947 #elif defined(VISP_HAVE_EIGEN3)
4948  return pseudoInverseEigen3(Ap, sv, svThreshold, imA, imAt, kerAt);
4949 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
4950  return pseudoInverseOpenCV(Ap, sv, svThreshold, imA, imAt, kerAt);
4951 #else
4952  (void)Ap;
4953  (void)sv;
4954  (void)svThreshold;
4955  (void)imA;
4956  (void)imAt;
4957  (void)kerAt;
4958  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4959  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4960 #endif
4961 }
4962 
5069  int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
5070  vpMatrix &kerAt) const
5071  {
5072 #if defined(VISP_HAVE_LAPACK)
5073  return pseudoInverseLapack(Ap, sv, rank_in, imA, imAt, kerAt);
5074 #elif defined(VISP_HAVE_EIGEN3)
5075  return pseudoInverseEigen3(Ap, sv, rank_in, imA, imAt, kerAt);
5076 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
5077  return pseudoInverseOpenCV(Ap, sv, rank_in, imA, imAt, kerAt);
5078 #else
5079  (void)Ap;
5080  (void)sv;
5081  (void)rank_in;
5082  (void)imA;
5083  (void)imAt;
5084  (void)kerAt;
5085  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
5086  "Install Lapack, Eigen3 or OpenCV 3rd party"));
5087 #endif
5088  }
5089 
5131  vpColVector vpMatrix::getCol(unsigned int j, unsigned int i_begin, unsigned int column_size) const
5132  {
5133  if (i_begin + column_size > getRows() || j >= getCols())
5134  throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(),
5135  getCols()));
5136  vpColVector c(column_size);
5137  for (unsigned int i = 0; i < column_size; i++)
5138  c[i] = (*this)[i_begin + i][j];
5139  return c;
5140  }
5141 
5181  vpColVector vpMatrix::getCol(unsigned int j) const { return getCol(j, 0, rowNum); }
5182 
5219  vpRowVector vpMatrix::getRow(unsigned int i) const { return getRow(i, 0, colNum); }
5220 
5260  vpRowVector vpMatrix::getRow(unsigned int i, unsigned int j_begin, unsigned int row_size) const
5261  {
5262  if (j_begin + row_size > colNum || i >= rowNum)
5263  throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
5264 
5265  vpRowVector r(row_size);
5266  if (r.data != nullptr && data != nullptr) {
5267  memcpy(r.data, (*this)[i] + j_begin, row_size * sizeof(double));
5268  }
5269 
5270  return r;
5271  }
5272 
5309  {
5310  unsigned int min_size = std::min<unsigned int>(rowNum, colNum);
5311  vpColVector diag;
5312 
5313  if (min_size > 0) {
5314  diag.resize(min_size, false);
5315 
5316  for (unsigned int i = 0; i < min_size; i++) {
5317  diag[i] = (*this)[i][i];
5318  }
5319  }
5320 
5321  return diag;
5322  }
5323 
5335  {
5336  vpMatrix C;
5337 
5338  vpMatrix::stack(A, B, C);
5339 
5340  return C;
5341  }
5342 
5354  void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
5355  {
5356  unsigned int nra = A.getRows();
5357  unsigned int nrb = B.getRows();
5358 
5359  if (nra != 0) {
5360  if (A.getCols() != B.getCols()) {
5361  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5362  A.getCols(), B.getRows(), B.getCols()));
5363  }
5364  }
5365 
5366  if (A.data != nullptr && A.data == C.data) {
5367  std::cerr << "A and C must be two different objects!" << std::endl;
5368  return;
5369  }
5370 
5371  if (B.data != nullptr && B.data == C.data) {
5372  std::cerr << "B and C must be two different objects!" << std::endl;
5373  return;
5374  }
5375 
5376  C.resize(nra + nrb, B.getCols(), false, false);
5377 
5378  if (C.data != nullptr && A.data != nullptr && A.size() > 0) {
5379  // Copy A in C
5380  memcpy(C.data, A.data, sizeof(double) * A.size());
5381  }
5382 
5383  if (C.data != nullptr && B.data != nullptr && B.size() > 0) {
5384  // Copy B in C
5385  memcpy(C.data + A.size(), B.data, sizeof(double) * B.size());
5386  }
5387  }
5388 
5399  {
5400  vpMatrix C;
5401  vpMatrix::stack(A, r, C);
5402 
5403  return C;
5404  }
5405 
5417  void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
5418  {
5419  if (A.data != nullptr && A.data == C.data) {
5420  std::cerr << "A and C must be two different objects!" << std::endl;
5421  return;
5422  }
5423 
5424  C = A;
5425  C.stack(r);
5426  }
5427 
5438  {
5439  vpMatrix C;
5440  vpMatrix::stack(A, c, C);
5441 
5442  return C;
5443  }
5444 
5456  void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C)
5457  {
5458  if (A.data != nullptr && A.data == C.data) {
5459  std::cerr << "A and C must be two different objects!" << std::endl;
5460  return;
5461  }
5462 
5463  C = A;
5464  C.stack(c);
5465  }
5466 
5479  vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, unsigned int r, unsigned int c)
5480  {
5482 
5483  vpArray2D<double>::insert(A, B, C, r, c);
5484 
5485  return vpMatrix(C);
5486  }
5487 
5501  void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, unsigned int r, unsigned int c)
5502  {
5503  vpArray2D<double> C_array;
5504 
5505  vpArray2D<double>::insert(A, B, C_array, r, c);
5506 
5507  C = C_array;
5508  }
5509 
5522  {
5523  vpMatrix C;
5524 
5525  juxtaposeMatrices(A, B, C);
5526 
5527  return C;
5528  }
5529 
5543  {
5544  unsigned int nca = A.getCols();
5545  unsigned int ncb = B.getCols();
5546 
5547  if (nca != 0) {
5548  if (A.getRows() != B.getRows()) {
5549  throw(vpException(vpException::dimensionError, "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5550  A.getCols(), B.getRows(), B.getCols()));
5551  }
5552  }
5553 
5554  if (B.getRows() == 0 || nca + ncb == 0) {
5555  std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl;
5556  return;
5557  }
5558 
5559  C.resize(B.getRows(), nca + ncb, false, false);
5560 
5561  C.insert(A, 0, 0);
5562  C.insert(B, 0, nca);
5563  }
5564 
5565  //--------------------------------------------------------------------
5566  // Output
5567  //--------------------------------------------------------------------
5568 
5588  int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &intro) const
5589  {
5590  typedef std::string::size_type size_type;
5591 
5592  unsigned int m = getRows();
5593  unsigned int n = getCols();
5594 
5595  std::vector<std::string> values(m * n);
5596  std::ostringstream oss;
5597  std::ostringstream ossFixed;
5598  std::ios_base::fmtflags original_flags = oss.flags();
5599 
5600  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
5601 
5602  size_type maxBefore = 0; // the length of the integral part
5603  size_type maxAfter = 0; // number of decimals plus
5604  // one place for the decimal point
5605  for (unsigned int i = 0; i < m; ++i) {
5606  for (unsigned int j = 0; j < n; ++j) {
5607  oss.str("");
5608  oss << (*this)[i][j];
5609  if (oss.str().find("e") != std::string::npos) {
5610  ossFixed.str("");
5611  ossFixed << (*this)[i][j];
5612  oss.str(ossFixed.str());
5613  }
5614 
5615  values[i * n + j] = oss.str();
5616  size_type thislen = values[i * n + j].size();
5617  size_type p = values[i * n + j].find('.');
5618 
5619  if (p == std::string::npos) {
5620  maxBefore = vpMath::maximum(maxBefore, thislen);
5621  // maxAfter remains the same
5622  }
5623  else {
5624  maxBefore = vpMath::maximum(maxBefore, p);
5625  maxAfter = vpMath::maximum(maxAfter, thislen - p);
5626  }
5627  }
5628  }
5629 
5630  size_type totalLength = length;
5631  // increase totalLength according to maxBefore
5632  totalLength = vpMath::maximum(totalLength, maxBefore);
5633  // decrease maxAfter according to totalLength
5634  maxAfter = std::min<size_type>(maxAfter, totalLength - maxBefore);
5635 
5636  if (!intro.empty())
5637  s << intro;
5638  s << "[" << m << "," << n << "]=\n";
5639 
5640  for (unsigned int i = 0; i < m; i++) {
5641  s << " ";
5642  for (unsigned int j = 0; j < n; j++) {
5643  size_type p = values[i * n + j].find('.');
5644  s.setf(std::ios::right, std::ios::adjustfield);
5645  s.width((std::streamsize)maxBefore);
5646  s << values[i * n + j].substr(0, p).c_str();
5647 
5648  if (maxAfter > 0) {
5649  s.setf(std::ios::left, std::ios::adjustfield);
5650  if (p != std::string::npos) {
5651  s.width((std::streamsize)maxAfter);
5652  s << values[i * n + j].substr(p, maxAfter).c_str();
5653  }
5654  else {
5655  s.width((std::streamsize)maxAfter);
5656  s << ".0";
5657  }
5658  }
5659 
5660  s << ' ';
5661  }
5662  s << std::endl;
5663  }
5664 
5665  s.flags(original_flags); // restore s to standard state
5666 
5667  return (int)(maxBefore + maxAfter);
5668  }
5669 
5706  std::ostream &vpMatrix::matlabPrint(std::ostream &os) const
5707  {
5708  os << "[ ";
5709  for (unsigned int i = 0; i < this->getRows(); ++i) {
5710  for (unsigned int j = 0; j < this->getCols(); ++j) {
5711  os << (*this)[i][j] << ", ";
5712  }
5713  if (this->getRows() != i + 1) {
5714  os << ";" << std::endl;
5715  }
5716  else {
5717  os << "]" << std::endl;
5718  }
5719  }
5720  return os;
5721  }
5722 
5751  std::ostream &vpMatrix::maplePrint(std::ostream &os) const
5752  {
5753  os << "([ " << std::endl;
5754  for (unsigned int i = 0; i < this->getRows(); ++i) {
5755  os << "[";
5756  for (unsigned int j = 0; j < this->getCols(); ++j) {
5757  os << (*this)[i][j] << ", ";
5758  }
5759  os << "]," << std::endl;
5760  }
5761  os << "])" << std::endl;
5762  return os;
5763  }
5764 
5792  std::ostream &vpMatrix::csvPrint(std::ostream &os) const
5793  {
5794  for (unsigned int i = 0; i < this->getRows(); ++i) {
5795  for (unsigned int j = 0; j < this->getCols(); ++j) {
5796  os << (*this)[i][j];
5797  if (!(j == (this->getCols() - 1)))
5798  os << ", ";
5799  }
5800  os << std::endl;
5801  }
5802  return os;
5803  }
5804 
5840  std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
5841  {
5842  os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " << this->getCols() << "); " << std::endl;
5843 
5844  for (unsigned int i = 0; i < this->getRows(); ++i) {
5845  for (unsigned int j = 0; j < this->getCols(); ++j) {
5846  if (!octet) {
5847  os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl;
5848  }
5849  else {
5850  for (unsigned int k = 0; k < sizeof(double); ++k) {
5851  os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex
5852  << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl;
5853  }
5854  }
5855  }
5856  os << std::endl;
5857  }
5858  return os;
5859  }
5860 
5865  void vpMatrix::stack(const vpMatrix &A)
5866  {
5867  if (rowNum == 0) {
5868  *this = A;
5869  }
5870  else if (A.getRows() > 0) {
5871  if (colNum != A.getCols()) {
5872  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum,
5873  A.getRows(), A.getCols()));
5874  }
5875 
5876  unsigned int rowNumOld = rowNum;
5877  resize(rowNum + A.getRows(), colNum, false, false);
5878  insert(A, rowNumOld, 0);
5879  }
5880  }
5881 
5898  {
5899  if (rowNum == 0) {
5900  *this = r;
5901  }
5902  else {
5903  if (colNum != r.getCols()) {
5904  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum,
5905  colNum, r.getCols()));
5906  }
5907 
5908  if (r.size() == 0) {
5909  return;
5910  }
5911 
5912  unsigned int oldSize = size();
5913  resize(rowNum + 1, colNum, false, false);
5914 
5915  if (data != nullptr && r.data != nullptr && data != r.data) {
5916  // Copy r in data
5917  memcpy(data + oldSize, r.data, sizeof(double) * r.size());
5918  }
5919  }
5920  }
5921 
5939  {
5940  if (colNum == 0) {
5941  *this = c;
5942  }
5943  else {
5944  if (rowNum != c.getRows()) {
5945  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum,
5946  colNum, c.getRows()));
5947  }
5948 
5949  if (c.size() == 0) {
5950  return;
5951  }
5952 
5953  vpMatrix tmp = *this;
5954  unsigned int oldColNum = colNum;
5955  resize(rowNum, colNum + 1, false, false);
5956 
5957  if (data != nullptr && tmp.data != nullptr && data != tmp.data) {
5958  // Copy c in data
5959  for (unsigned int i = 0; i < rowNum; i++) {
5960  memcpy(data + i * colNum, tmp.data + i * oldColNum, sizeof(double) * oldColNum);
5961  rowPtrs[i][oldColNum] = c[i];
5962  }
5963  }
5964  }
5965  }
5966 
5977  void vpMatrix::insert(const vpMatrix &A, unsigned int r, unsigned int c)
5978  {
5979  if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) {
5980  if (A.colNum == colNum && data != nullptr && A.data != nullptr && A.data != data) {
5981  memcpy(data + r * colNum, A.data, sizeof(double) * A.size());
5982  }
5983  else if (data != nullptr && A.data != nullptr && A.data != data) {
5984  for (unsigned int i = r; i < (r + A.getRows()); i++) {
5985  memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum);
5986  }
5987  }
5988  }
5989  else {
5990  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
5991  A.getRows(), A.getCols(), rowNum, colNum, r, c);
5992  }
5993  }
5994 
6032  {
6033  vpColVector evalue(rowNum); // Eigen values
6034 
6035  if (rowNum != colNum) {
6036  throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
6037  colNum));
6038  }
6039 
6040  // Check if the matrix is symmetric: At - A = 0
6041  vpMatrix At_A = (*this).t() - (*this);
6042  for (unsigned int i = 0; i < rowNum; i++) {
6043  for (unsigned int j = 0; j < rowNum; j++) {
6044  // if (At_A[i][j] != 0) {
6045  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
6046  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix"));
6047  }
6048  }
6049  }
6050 
6051 #if defined(VISP_HAVE_LAPACK)
6052 #if defined(VISP_HAVE_GSL) /* be careful of the copy below */
6053  {
6054  gsl_vector *eval = gsl_vector_alloc(rowNum);
6055  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
6056 
6057  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
6058  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
6059 
6060  unsigned int Atda = (unsigned int)m->tda;
6061  for (unsigned int i = 0; i < rowNum; i++) {
6062  unsigned int k = i * Atda;
6063  for (unsigned int j = 0; j < colNum; j++)
6064  m->data[k + j] = (*this)[i][j];
6065  }
6066  gsl_eigen_symmv(m, eval, evec, w);
6067 
6068  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
6069 
6070  for (unsigned int i = 0; i < rowNum; i++) {
6071  evalue[i] = gsl_vector_get(eval, i);
6072  }
6073 
6074  gsl_eigen_symmv_free(w);
6075  gsl_vector_free(eval);
6076  gsl_matrix_free(m);
6077  gsl_matrix_free(evec);
6078  }
6079 #else
6080  {
6081  const char jobz = 'N';
6082  const char uplo = 'U';
6083  vpMatrix A = (*this);
6084  vpColVector WORK;
6085  int lwork = -1;
6086  int info = 0;
6087  double wkopt;
6088  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
6089  lwork = static_cast<int>(wkopt);
6090  WORK.resize(lwork);
6091  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
6092  }
6093 #endif
6094 #else
6095  {
6096  throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. "
6097  "You should install Lapack 3rd party"));
6098  }
6099 #endif
6100  return evalue;
6101  }
6102 
6152  void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const
6153  {
6154  if (rowNum != colNum) {
6155  throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
6156  colNum));
6157  }
6158 
6159  // Check if the matrix is symmetric: At - A = 0
6160  vpMatrix At_A = (*this).t() - (*this);
6161  for (unsigned int i = 0; i < rowNum; i++) {
6162  for (unsigned int j = 0; j < rowNum; j++) {
6163  // if (At_A[i][j] != 0) {
6164  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
6165  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix"));
6166  }
6167  }
6168  }
6169 
6170  // Resize the output matrices
6171  evalue.resize(rowNum);
6172  evector.resize(rowNum, colNum);
6173 
6174 #if defined(VISP_HAVE_LAPACK)
6175 #if defined(VISP_HAVE_GSL) /* be careful of the copy below */
6176  {
6177  gsl_vector *eval = gsl_vector_alloc(rowNum);
6178  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
6179 
6180  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
6181  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
6182 
6183  unsigned int Atda = (unsigned int)m->tda;
6184  for (unsigned int i = 0; i < rowNum; i++) {
6185  unsigned int k = i * Atda;
6186  for (unsigned int j = 0; j < colNum; j++)
6187  m->data[k + j] = (*this)[i][j];
6188  }
6189  gsl_eigen_symmv(m, eval, evec, w);
6190 
6191  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
6192 
6193  for (unsigned int i = 0; i < rowNum; i++) {
6194  evalue[i] = gsl_vector_get(eval, i);
6195  }
6196  Atda = (unsigned int)evec->tda;
6197  for (unsigned int i = 0; i < rowNum; i++) {
6198  unsigned int k = i * Atda;
6199  for (unsigned int j = 0; j < rowNum; j++) {
6200  evector[i][j] = evec->data[k + j];
6201  }
6202  }
6203 
6204  gsl_eigen_symmv_free(w);
6205  gsl_vector_free(eval);
6206  gsl_matrix_free(m);
6207  gsl_matrix_free(evec);
6208  }
6209 #else // defined(VISP_HAVE_GSL)
6210  {
6211  const char jobz = 'V';
6212  const char uplo = 'U';
6213  vpMatrix A = (*this);
6214  vpColVector WORK;
6215  int lwork = -1;
6216  int info = 0;
6217  double wkopt;
6218  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
6219  lwork = static_cast<int>(wkopt);
6220  WORK.resize(lwork);
6221  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
6222  evector = A.t();
6223  }
6224 #endif // defined(VISP_HAVE_GSL)
6225 #else
6226  {
6227  throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. "
6228  "You should install Lapack 3rd party"));
6229  }
6230 #endif
6231  }
6232 
6251  unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const
6252  {
6253  unsigned int nbline = getRows();
6254  unsigned int nbcol = getCols();
6255 
6256  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6257  vpColVector sv;
6258  sv.resize(nbcol, false); // singular values
6259  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6260 
6261  // Copy and resize matrix to have at least as many rows as columns
6262  // kernel is computed in svd method only if the matrix has more rows than
6263  // columns
6264 
6265  if (nbline < nbcol)
6266  U.resize(nbcol, nbcol, true);
6267  else
6268  U.resize(nbline, nbcol, false);
6269 
6270  U.insert(*this, 0, 0);
6271 
6272  U.svd(sv, V);
6273 
6274  // Compute the highest singular value and rank of the matrix
6275  double maxsv = 0;
6276  for (unsigned int i = 0; i < nbcol; i++) {
6277  if (sv[i] > maxsv) {
6278  maxsv = sv[i];
6279  }
6280  }
6281 
6282  unsigned int rank = 0;
6283  for (unsigned int i = 0; i < nbcol; i++) {
6284  if (sv[i] > maxsv * svThreshold) {
6285  rank++;
6286  }
6287  }
6288 
6289  kerAt.resize(nbcol - rank, nbcol);
6290  if (rank != nbcol) {
6291  for (unsigned int j = 0, k = 0; j < nbcol; j++) {
6292  // if( v.col(j) in kernel and non zero )
6293  if ((sv[j] <= maxsv * svThreshold) &&
6294  (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
6295  for (unsigned int i = 0; i < V.getRows(); i++) {
6296  kerAt[k][i] = V[i][j];
6297  }
6298  k++;
6299  }
6300  }
6301  }
6302 
6303  return rank;
6304  }
6305 
6322  unsigned int vpMatrix::nullSpace(vpMatrix &kerA, double svThreshold) const
6323  {
6324  unsigned int nbrow = getRows();
6325  unsigned int nbcol = getCols();
6326 
6327  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6328  vpColVector sv;
6329  sv.resize(nbcol, false); // singular values
6330  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6331 
6332  // Copy and resize matrix to have at least as many rows as columns
6333  // kernel is computed in svd method only if the matrix has more rows than
6334  // columns
6335 
6336  if (nbrow < nbcol)
6337  U.resize(nbcol, nbcol, true);
6338  else
6339  U.resize(nbrow, nbcol, false);
6340 
6341  U.insert(*this, 0, 0);
6342 
6343  U.svd(sv, V);
6344 
6345  // Compute the highest singular value and rank of the matrix
6346  double maxsv = sv[0];
6347 
6348  unsigned int rank = 0;
6349  for (unsigned int i = 0; i < nbcol; i++) {
6350  if (sv[i] > maxsv * svThreshold) {
6351  rank++;
6352  }
6353  }
6354 
6355  kerA.resize(nbcol, nbcol - rank);
6356  if (rank != nbcol) {
6357  for (unsigned int j = 0, k = 0; j < nbcol; j++) {
6358  // if( v.col(j) in kernel and non zero )
6359  if (sv[j] <= maxsv * svThreshold) {
6360  for (unsigned int i = 0; i < nbcol; i++) {
6361  kerA[i][k] = V[i][j];
6362  }
6363  k++;
6364  }
6365  }
6366  }
6367 
6368  return (nbcol - rank);
6369  }
6370 
6387  unsigned int vpMatrix::nullSpace(vpMatrix &kerA, int dim) const
6388  {
6389  unsigned int nbrow = getRows();
6390  unsigned int nbcol = getCols();
6391  unsigned int dim_ = static_cast<unsigned int>(dim);
6392 
6393  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6394  vpColVector sv;
6395  sv.resize(nbcol, false); // singular values
6396  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6397 
6398  // Copy and resize matrix to have at least as many rows as columns
6399  // kernel is computed in svd method only if the matrix has more rows than
6400  // columns
6401 
6402  if (nbrow < nbcol)
6403  U.resize(nbcol, nbcol, true);
6404  else
6405  U.resize(nbrow, nbcol, false);
6406 
6407  U.insert(*this, 0, 0);
6408 
6409  U.svd(sv, V);
6410 
6411  kerA.resize(nbcol, dim_);
6412  if (dim_ != 0) {
6413  unsigned int rank = nbcol - dim_;
6414  for (unsigned int k = 0; k < dim_; k++) {
6415  unsigned int j = k + rank;
6416  for (unsigned int i = 0; i < nbcol; i++) {
6417  kerA[i][k] = V[i][j];
6418  }
6419  }
6420  }
6421 
6422  double maxsv = sv[0];
6423  unsigned int rank = 0;
6424  for (unsigned int i = 0; i < nbcol; i++) {
6425  if (sv[i] > maxsv * 1e-6) {
6426  rank++;
6427  }
6428  }
6429  return (nbcol - rank);
6430  }
6431 
6463  double vpMatrix::det(vpDetMethod method) const
6464  {
6465  double det = 0.;
6466 
6467  if (method == LU_DECOMPOSITION) {
6468  det = this->detByLU();
6469  }
6470 
6471  return (det);
6472  }
6473 
6482  {
6483  if (colNum != rowNum) {
6484  throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix",
6485  rowNum, colNum));
6486  }
6487  else {
6488 #ifdef VISP_HAVE_GSL
6489  size_t size_ = rowNum * colNum;
6490  double *b = new double[size_];
6491  for (size_t i = 0; i < size_; i++)
6492  b[i] = 0.;
6493  gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum);
6494  gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum);
6495  gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
6496  // gsl_matrix_fprintf(stdout, &em.matrix, "%g");
6497  vpMatrix expA;
6498  expA.resize(rowNum, colNum, false);
6499  memcpy(expA.data, b, size_ * sizeof(double));
6500 
6501  delete[] b;
6502  return expA;
6503 #else
6504  vpMatrix _expE(rowNum, colNum, false);
6505  vpMatrix _expD(rowNum, colNum, false);
6506  vpMatrix _expX(rowNum, colNum, false);
6507  vpMatrix _expcX(rowNum, colNum, false);
6508  vpMatrix _eye(rowNum, colNum, false);
6509 
6510  _eye.eye();
6511  vpMatrix exp(*this);
6512 
6513  // double f;
6514  int e;
6515  double c = 0.5;
6516  int q = 6;
6517  int p = 1;
6518 
6519  double nA = 0;
6520  for (unsigned int i = 0; i < rowNum; i++) {
6521  double sum = 0;
6522  for (unsigned int j = 0; j < colNum; j++) {
6523  sum += fabs((*this)[i][j]);
6524  }
6525  if (sum > nA || i == 0) {
6526  nA = sum;
6527  }
6528  }
6529 
6530  /* f = */ frexp(nA, &e);
6531  // double s = (0 > e+1)?0:e+1;
6532  double s = e + 1;
6533 
6534  double sca = 1.0 / pow(2.0, s);
6535  exp = sca * exp;
6536  _expX = *this;
6537  _expE = c * exp + _eye;
6538  _expD = -c * exp + _eye;
6539  for (int k = 2; k <= q; k++) {
6540  c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1)));
6541  _expcX = exp * _expX;
6542  _expX = _expcX;
6543  _expcX = c * _expX;
6544  _expE = _expE + _expcX;
6545  if (p)
6546  _expD = _expD + _expcX;
6547  else
6548  _expD = _expD - _expcX;
6549  p = !p;
6550  }
6551  _expX = _expD.inverseByLU();
6552  exp = _expX * _expE;
6553  for (int k = 1; k <= s; k++) {
6554  _expE = exp * exp;
6555  exp = _expE;
6556  }
6557  return exp;
6558 #endif
6559  }
6560  }
6561 
6562  /**************************************************************************************************************/
6563  /**************************************************************************************************************/
6564 
6565  // Specific functions
6566 
6567  /*
6568  input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.
6569 
6570  output:: the complement matrix of the element (rowNo,colNo).
6571  This is the matrix obtained from M after elimenating the row rowNo and column
6572  colNo
6573 
6574  example:
6575  1 2 3
6576  M = 4 5 6
6577  7 8 9
6578  1 3
6579  subblock(M, 1, 1) give the matrix 7 9
6580  */
6581  vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row)
6582  {
6583  vpMatrix M_comp;
6584  M_comp.resize(M.getRows() - 1, M.getCols() - 1, false);
6585 
6586  for (unsigned int i = 0; i < col; i++) {
6587  for (unsigned int j = 0; j < row; j++)
6588  M_comp[i][j] = M[i][j];
6589  for (unsigned int j = row + 1; j < M.getRows(); j++)
6590  M_comp[i][j - 1] = M[i][j];
6591  }
6592  for (unsigned int i = col + 1; i < M.getCols(); i++) {
6593  for (unsigned int j = 0; j < row; j++)
6594  M_comp[i - 1][j] = M[i][j];
6595  for (unsigned int j = row + 1; j < M.getRows(); j++)
6596  M_comp[i - 1][j - 1] = M[i][j];
6597  }
6598  return M_comp;
6599  }
6600 
6610  double vpMatrix::cond(double svThreshold) const
6611  {
6612  unsigned int nbline = getRows();
6613  unsigned int nbcol = getCols();
6614 
6615  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6616  vpColVector sv;
6617  sv.resize(nbcol); // singular values
6618  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6619 
6620  // Copy and resize matrix to have at least as many rows as columns
6621  // kernel is computed in svd method only if the matrix has more rows than
6622  // columns
6623 
6624  if (nbline < nbcol)
6625  U.resize(nbcol, nbcol, true);
6626  else
6627  U.resize(nbline, nbcol, false);
6628 
6629  U.insert(*this, 0, 0);
6630 
6631  U.svd(sv, V);
6632 
6633  // Compute the highest singular value
6634  double maxsv = 0;
6635  for (unsigned int i = 0; i < nbcol; i++) {
6636  if (sv[i] > maxsv) {
6637  maxsv = sv[i];
6638  }
6639  }
6640 
6641  // Compute the rank of the matrix
6642  unsigned int rank = 0;
6643  for (unsigned int i = 0; i < nbcol; i++) {
6644  if (sv[i] > maxsv * svThreshold) {
6645  rank++;
6646  }
6647  }
6648 
6649  // Compute the lowest singular value
6650  double minsv = maxsv;
6651  for (unsigned int i = 0; i < rank; i++) {
6652  if (sv[i] < minsv) {
6653  minsv = sv[i];
6654  }
6655  }
6656 
6657  if (std::fabs(minsv) > std::numeric_limits<double>::epsilon()) {
6658  return maxsv / minsv;
6659  }
6660  else {
6661  return std::numeric_limits<double>::infinity();
6662  }
6663  }
6664 
6671  void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
6672  {
6673  if (H.getCols() != H.getRows()) {
6674  throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(),
6675  H.getCols()));
6676  }
6677 
6678  HLM = H;
6679  for (unsigned int i = 0; i < H.getCols(); i++) {
6680  HLM[i][i] += alpha * H[i][i];
6681  }
6682  }
6683 
6692  {
6693  double norm = 0.0;
6694  for (unsigned int i = 0; i < dsize; i++) {
6695  double x = *(data + i);
6696  norm += x * x;
6697  }
6698 
6699  return sqrt(norm);
6700  }
6701 
6711  {
6712  if (this->dsize != 0) {
6713  vpMatrix v;
6714  vpColVector w;
6715 
6716  vpMatrix M = *this;
6717 
6718  M.svd(w, v);
6719 
6720  double max = w[0];
6721  unsigned int maxRank = std::min<unsigned int>(this->getCols(), this->getRows());
6722  // The maximum reachable rank is either the number of columns or the number of rows
6723  // of the matrix.
6724  unsigned int boundary = std::min<unsigned int>(maxRank, w.size());
6725  // boundary is here to ensure that the number of singular values used for the com-
6726  // putation of the euclidean norm of the matrix is not greater than the maximum
6727  // reachable rank. Indeed, some svd library pad the singular values vector with 0s
6728  // if the input matrix is non-square.
6729  for (unsigned int i = 0; i < boundary; i++) {
6730  if (max < w[i]) {
6731  max = w[i];
6732  }
6733  }
6734  return max;
6735  }
6736  else {
6737  return 0.;
6738  }
6739  }
6740 
6751  double vpMatrix::infinityNorm() const
6752  {
6753  double norm = 0.0;
6754  for (unsigned int i = 0; i < rowNum; i++) {
6755  double x = 0;
6756  for (unsigned int j = 0; j < colNum; j++) {
6757  x += fabs(*(*(rowPtrs + i) + j));
6758  }
6759  if (x > norm) {
6760  norm = x;
6761  }
6762  }
6763  return norm;
6764  }
6765 
6772  double vpMatrix::sumSquare() const
6773  {
6774  double sum_square = 0.0;
6775  double x;
6776 
6777  for (unsigned int i = 0; i < rowNum; i++) {
6778  for (unsigned int j = 0; j < colNum; j++) {
6779  x = rowPtrs[i][j];
6780  sum_square += x * x;
6781  }
6782  }
6783 
6784  return sum_square;
6785  }
6786 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
6796  double vpMatrix::euclideanNorm() const { return frobeniusNorm(); }
6797 
6799  {
6800  return (vpMatrix)(vpColVector::stack(A, B));
6801  }
6802 
6804  {
6805  vpColVector::stack(A, B, C);
6806  }
6807 
6809 
6810  void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); }
6811 
6830  vpRowVector vpMatrix::row(unsigned int i)
6831  {
6832  vpRowVector c(getCols());
6833 
6834  for (unsigned int j = 0; j < getCols(); j++)
6835  c[j] = (*this)[i - 1][j];
6836  return c;
6837  }
6838 
6857  {
6858  vpColVector c(getRows());
6859 
6860  for (unsigned int i = 0; i < getRows(); i++)
6861  c[i] = (*this)[i][j - 1];
6862  return c;
6863  }
6864 
6871  void vpMatrix::setIdentity(const double &val)
6872  {
6873  for (unsigned int i = 0; i < rowNum; i++)
6874  for (unsigned int j = 0; j < colNum; j++)
6875  if (i == j)
6876  (*this)[i][j] = val;
6877  else
6878  (*this)[i][j] = 0;
6879  }
6880 
6881 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:125
unsigned int getCols() const
Definition: vpArray2D.h:274
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:138
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:132
void insert(const vpArray2D< Type > &A, unsigned int r, unsigned int c)
Definition: vpArray2D.h:411
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:299
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:128
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:134
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:286
unsigned int getRows() const
Definition: vpArray2D.h:284
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:130
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
double sumSquare() const
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:78
@ dimensionError
Bad dimension.
Definition: vpException.h:83
@ fatalError
Fatal error.
Definition: vpException.h:84
@ divideByZeroError
Division by zero.
Definition: vpException.h:82
Implementation of an homogeneous matrix and operations on such kind of matrices.
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:252
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void svdLapack(vpColVector &w, vpMatrix &V)
vpColVector eigenValues() const
Definition: vpMatrix.cpp:6031
vpMatrix pseudoInverseOpenCV(double svThreshold=1e-6) const
std::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
Definition: vpMatrix.cpp:5840
vpMatrix & operator<<(double *)
Definition: vpMatrix.cpp:794
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6610
vpMatrix expm() const
Definition: vpMatrix.cpp:6481
vpMatrix pseudoInverseEigen3(double svThreshold=1e-6) const
vpMatrix hadamard(const vpMatrix &m) const
Definition: vpMatrix.cpp:1806
vpMatrix operator-() const
Definition: vpMatrix.cpp:1597
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:6871
vpMatrix & operator/=(double x)
Divide all the element of the matrix by x : Aij = Aij / x.
Definition: vpMatrix.cpp:1728
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5588
std::ostream & maplePrint(std::ostream &os) const
Definition: vpMatrix.cpp:5751
void eye()
Definition: vpMatrix.cpp:442
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6251
vpMatrix inverseByLU() const
vpMatrix operator*(const vpMatrix &B) const
Definition: vpMatrix.cpp:1176
vpMatrix operator/(double x) const
Cij = Aij / x (A is unchanged)
Definition: vpMatrix.cpp:1668
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5865
vp_deprecated void stackMatrices(const vpMatrix &A)
Definition: vpMatrix.h:994
vpMatrix & operator-=(const vpMatrix &B)
Operation A = A - B.
Definition: vpMatrix.cpp:1556
vpMatrix & operator*=(double x)
Multiply all the element of the matrix by x : Aij = Aij * x.
Definition: vpMatrix.cpp:1714
vpMatrix operator*(const double &x, const vpMatrix &B)
Definition: vpMatrix.cpp:1628
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:5521
vp_deprecated double euclideanNorm() const
Definition: vpMatrix.cpp:6796
vpColVector stackColumns()
Definition: vpMatrix.cpp:1771
vp_deprecated vpColVector column(unsigned int j)
Definition: vpMatrix.cpp:6856
static void sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1505
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2075
vpRowVector stackRows()
Definition: vpMatrix.cpp:1793
vpMatrix operator+(const vpMatrix &B) const
Definition: vpMatrix.cpp:1450
double sum() const
Definition: vpMatrix.cpp:1604
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:893
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:5219
void svdOpenCV(vpColVector &w, vpMatrix &V)
vpMatrix t() const
Definition: vpMatrix.cpp:457
vpMatrix()
Definition: vpMatrix.h:162
vpMatrix & operator+=(const vpMatrix &B)
Operation A = A + B.
Definition: vpMatrix.cpp:1539
static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C)
Definition: vpMatrix.cpp:1363
double inducedL2Norm() const
Definition: vpMatrix.cpp:6710
double infinityNorm() const
Definition: vpMatrix.cpp:6751
vpMatrix & operator=(const vpArray2D< double > &A)
Definition: vpMatrix.cpp:658
double frobeniusNorm() const
Definition: vpMatrix.cpp:6691
vpColVector getDiag() const
Definition: vpMatrix.cpp:5308
vpMatrix AtA() const
Definition: vpMatrix.cpp:633
static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
Definition: vpMatrix.cpp:912
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:1953
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6671
vpMatrix pseudoInverseLapack(double svThreshold=1e-6) const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2286
vpMatrix & operator,(double val)
Definition: vpMatrix.cpp:811
static void multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
Definition: vpMatrix.cpp:965
vp_deprecated vpRowVector row(unsigned int i)
Definition: vpMatrix.cpp:6830
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5181
double detByLU() const
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6463
double sumSquare() const
Definition: vpMatrix.cpp:6772
vp_deprecated void init()
Definition: vpMatrix.h:989
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1392
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5977
void svdEigen3(vpColVector &w, vpMatrix &V)
void kron(const vpMatrix &m1, vpMatrix &out) const
Definition: vpMatrix.cpp:1862
vpMatrix AAt() const
Definition: vpMatrix.cpp:509
vpMatrix transpose() const
Definition: vpMatrix.cpp:464
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1015
static void negateMatrix(const vpMatrix &A, vpMatrix &C)
Definition: vpMatrix.cpp:1580
@ LU_DECOMPOSITION
Definition: vpMatrix.h:154
std::ostream & csvPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5792
unsigned int nullSpace(vpMatrix &kerA, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6322
std::ostream & matlabPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5706
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:400
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:107
void resize(unsigned int i, bool flagNullify=true)
Definition: vpRowVector.h:259
Class that consider the case of a translation vector.