Visual Servoing Platform  version 3.3.1 under development (2021-01-18)
vpMatrix.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Matrix manipulation.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
43 #include <algorithm>
44 #include <assert.h>
45 #include <cmath> // std::fabs
46 #include <fstream>
47 #include <limits> // numeric_limits
48 #include <sstream>
49 #include <stdio.h>
50 #include <stdlib.h>
51 #include <string.h>
52 #include <string>
53 #include <vector>
54 
55 #include <visp3/core/vpConfig.h>
56 #include <visp3/core/vpCPUFeatures.h>
57 #include <visp3/core/vpColVector.h>
58 #include <visp3/core/vpDebug.h>
59 #include <visp3/core/vpException.h>
60 #include <visp3/core/vpMath.h>
61 #include <visp3/core/vpMatrix.h>
62 #include <visp3/core/vpTranslationVector.h>
63 
64 #include <Simd/SimdLib.hpp>
65 
66 #ifdef VISP_HAVE_LAPACK
67 # ifdef VISP_HAVE_GSL
68 # include <gsl/gsl_eigen.h>
69 # include <gsl/gsl_math.h>
70 # include <gsl/gsl_linalg.h>
71 # elif defined(VISP_HAVE_MKL)
72 # include <mkl.h>
73 typedef MKL_INT integer;
74 
75 void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_,
76  double *w_data, double *work_data, int lwork_, int &info_)
77 {
78  MKL_INT n = static_cast<MKL_INT>(n_);
79  MKL_INT lda = static_cast<MKL_INT>(lda_);
80  MKL_INT lwork = static_cast<MKL_INT>(lwork_);
81  MKL_INT info = static_cast<MKL_INT>(info_);
82 
83  dsyev(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
84 }
85 
86 # else
87 # if defined(VISP_HAVE_LAPACK_BUILT_IN)
88 typedef long int integer;
89 # else
90 typedef int integer;
91 # endif
92 extern "C" integer dsyev_(char *jobz, char *uplo, integer *n, double *a, integer *lda,
93  double *w, double *WORK, integer *lwork, integer *info);
94 
95 void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_,
96  double *w_data, double *work_data, int lwork_, int &info_)
97 {
98  integer n = static_cast<integer>(n_);
99  integer lda = static_cast<integer>(lda_);
100  integer lwork = static_cast<integer>(lwork_);
101  integer info = static_cast<integer>(info_);
102 
103  dsyev_(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
104 
105  lwork_ = static_cast<int>(lwork);
106  info_ = static_cast<int>(info);
107 }
108 # endif
109 #endif
110 
111 #if !defined(VISP_USE_MSVC) || (defined(VISP_USE_MSVC) && !defined(VISP_BUILD_SHARED_LIBS))
112 const unsigned int vpMatrix::m_lapack_min_size_default = 0;
113 unsigned int vpMatrix::m_lapack_min_size = vpMatrix::m_lapack_min_size_default;
114 #endif
115 
116 // Prototypes of specific functions
117 vpMatrix subblock(const vpMatrix &, unsigned int, unsigned int);
118 
119 void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows,
120  unsigned int ncols, double svThreshold, vpMatrix &Ap, int &rank_out,
121  int *rank_in, vpMatrix *imA, vpMatrix *imAt, vpMatrix *kerAt)
122 {
123  Ap.resize(ncols, nrows, true, false);
124 
125  // compute the highest singular value and the rank of h
126  double maxsv = sv[0];
127 
128  rank_out = 0;
129 
130  for (unsigned int i = 0; i < ncols; i++) {
131  if (sv[i] > maxsv * svThreshold) {
132  rank_out++;
133  }
134  }
135 
136  unsigned int rank = static_cast<unsigned int>(rank_out);
137  if (rank_in) {
138  rank = static_cast<unsigned int>(*rank_in);
139  }
140 
141  for (unsigned int i = 0; i < ncols; i++) {
142  for (unsigned int j = 0; j < nrows; j++) {
143  for (unsigned int k = 0; k < rank; k++) {
144  Ap[i][j] += V[i][k] * U[j][k] / sv[k];
145  }
146  }
147  }
148 
149  // Compute im(A)
150  if (imA) {
151  imA->resize(nrows, rank);
152 
153  for (unsigned int i = 0; i < nrows; i++) {
154  for (unsigned int j = 0; j < rank; j++) {
155  (*imA)[i][j] = U[i][j];
156  }
157  }
158  }
159 
160  // Compute im(At)
161  if (imAt) {
162  imAt->resize(ncols, rank);
163  for (unsigned int i = 0; i < ncols; i++) {
164  for (unsigned int j = 0; j < rank; j++) {
165  (*imAt)[i][j] = V[i][j];
166  }
167  }
168  }
169 
170  // Compute ker(At)
171  if (kerAt) {
172  kerAt->resize(ncols - rank, ncols);
173  if (rank != ncols) {
174  for (unsigned int k = 0; k < (ncols - rank); k++) {
175  unsigned j = k + rank;
176  for (unsigned int i = 0; i < V.getRows(); i++) {
177  (*kerAt)[k][i] = V[i][j];
178  }
179  }
180  }
181  }
182 }
183 
189 vpMatrix::vpMatrix(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
190  : vpArray2D<double>()
191 {
192  if (((r + nrows) > M.rowNum) || ((c + ncols) > M.colNum)) {
194  "Cannot construct a sub matrix (%dx%d) starting at "
195  "position (%d,%d) that is not contained in the "
196  "original matrix (%dx%d)",
197  nrows, ncols, r, c, M.rowNum, M.colNum));
198  }
199 
200  init(M, r, c, nrows, ncols);
201 }
202 
203 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
205 {
206  rowNum = A.rowNum;
207  colNum = A.colNum;
208  rowPtrs = A.rowPtrs;
209  dsize = A.dsize;
210  data = A.data;
211 
212  A.rowNum = 0;
213  A.colNum = 0;
214  A.rowPtrs = NULL;
215  A.dsize = 0;
216  A.data = NULL;
217 }
218 
244 vpMatrix::vpMatrix(const std::initializer_list<double> &list) : vpArray2D<double>(list) { }
245 
246 
271 vpMatrix::vpMatrix(unsigned int nrows, unsigned int ncols, const std::initializer_list<double> &list)
272  : vpArray2D<double>(nrows, ncols, list) {}
273 
296 vpMatrix::vpMatrix(const std::initializer_list<std::initializer_list<double> > &lists) : vpArray2D<double>(lists) { }
297 
298 #endif
299 
346 void vpMatrix::init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
347 {
348  unsigned int rnrows = r + nrows;
349  unsigned int cncols = c + ncols;
350 
351  if (rnrows > M.getRows())
352  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
353  M.getRows()));
354  if (cncols > M.getCols())
355  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
356  M.getCols()));
357  resize(nrows, ncols, false, false);
358 
359  if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
360  return; // Noting to do
361  for (unsigned int i = 0; i < nrows; i++) {
362  memcpy((*this)[i], &M[i + r][c], ncols * sizeof(double));
363  }
364 }
365 
407 vpMatrix vpMatrix::extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
408 {
409  unsigned int rnrows = r + nrows;
410  unsigned int cncols = c + ncols;
411 
412  if (rnrows > getRows())
413  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
414  getRows()));
415  if (cncols > getCols())
416  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
417  getCols()));
418 
419  vpMatrix M;
420  M.resize(nrows, ncols, false, false);
421  for (unsigned int i = 0; i < nrows; i++) {
422  memcpy(M[i], &(*this)[i + r][c], ncols * sizeof(double));
423  }
424 
425  return M;
426 }
427 
432 void vpMatrix::eye(unsigned int n) { eye(n, n); }
433 
438 void vpMatrix::eye(unsigned int m, unsigned int n)
439 {
440  resize(m, n);
441 
442  eye();
443 }
444 
450 {
451  for (unsigned int i = 0; i < rowNum; i++) {
452  for (unsigned int j = 0; j < colNum; j++) {
453  if (i == j)
454  (*this)[i][j] = 1.0;
455  else
456  (*this)[i][j] = 0;
457  }
458  }
459 }
460 
465 {
466  return transpose();
467 }
468 
475 {
476  vpMatrix At;
477  transpose(At);
478  return At;
479 }
480 
487 {
488  At.resize(colNum, rowNum, false, false);
489 
490  if (rowNum <= 16 || colNum <= 16) {
491  for (unsigned int i = 0; i < rowNum; i++) {
492  for (unsigned int j = 0; j < colNum; j++) {
493  At[j][i] = (*this)[i][j];
494  }
495  }
496  }
497  else {
498  SimdMatTranspose(data, rowNum, colNum, At.data);
499  }
500 }
501 
508 {
509  vpMatrix B;
510 
511  AAt(B);
512 
513  return B;
514 }
515 
527 void vpMatrix::AAt(vpMatrix &B) const
528 {
529  if ((B.rowNum != rowNum) || (B.colNum != rowNum))
530  B.resize(rowNum, rowNum, false, false);
531 
532  // If available use Lapack only for large matrices
533  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
534 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
535  useLapack = false;
536 #endif
537 
538  if (useLapack) {
539 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
540  const double alpha = 1.0;
541  const double beta = 0.0;
542  const char transa = 't';
543  const char transb = 'n';
544 
545  vpMatrix::blas_dgemm(transa, transb, rowNum, rowNum, colNum, alpha, data, colNum, data, colNum, beta, B.data, rowNum);
546 #endif
547  }
548  else {
549  // compute A*A^T
550  for (unsigned int i = 0; i < rowNum; i++) {
551  for (unsigned int j = i; j < rowNum; j++) {
552  double *pi = rowPtrs[i]; // row i
553  double *pj = rowPtrs[j]; // row j
554 
555  // sum (row i .* row j)
556  double ssum = 0;
557  for (unsigned int k = 0; k < colNum; k++)
558  ssum += *(pi++) * *(pj++);
559 
560  B[i][j] = ssum; // upper triangle
561  if (i != j)
562  B[j][i] = ssum; // lower triangle
563  }
564  }
565  }
566 }
567 
579 void vpMatrix::AtA(vpMatrix &B) const
580 {
581  if ((B.rowNum != colNum) || (B.colNum != colNum))
582  B.resize(colNum, colNum, false, false);
583 
584  // If available use Lapack only for large matrices
585  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
586 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
587  useLapack = false;
588 #endif
589 
590  if (useLapack) {
591 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
592  const double alpha = 1.0;
593  const double beta = 0.0;
594  const char transa = 'n';
595  const char transb = 't';
596 
597  vpMatrix::blas_dgemm(transa, transb, colNum, colNum, rowNum, alpha, data, colNum, data, colNum, beta, B.data, colNum);
598 #endif
599  }
600  else {
601  for (unsigned int i = 0; i < colNum; i++) {
602  double *Bi = B[i];
603  for (unsigned int j = 0; j < i; j++) {
604  double *ptr = data;
605  double s = 0;
606  for (unsigned int k = 0; k < rowNum; k++) {
607  s += (*(ptr + i)) * (*(ptr + j));
608  ptr += colNum;
609  }
610  *Bi++ = s;
611  B[j][i] = s;
612  }
613  double *ptr = data;
614  double s = 0;
615  for (unsigned int k = 0; k < rowNum; k++) {
616  s += (*(ptr + i)) * (*(ptr + i));
617  ptr += colNum;
618  }
619  *Bi = s;
620  }
621  }
622 }
623 
630 {
631  vpMatrix B;
632 
633  AtA(B);
634 
635  return B;
636 }
637 
655 {
656  resize(A.getRows(), A.getCols(), false, false);
657 
658  if (data != NULL && A.data != NULL && data != A.data) {
659  memcpy(data, A.data, dsize * sizeof(double));
660  }
661 
662  return *this;
663 }
664 
665 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
667 {
668  resize(A.getRows(), A.getCols(), false, false);
669 
670  if (data != NULL && A.data != NULL && data != A.data) {
671  memcpy(data, A.data, dsize * sizeof(double));
672  }
673 
674  return *this;
675 }
676 
678 {
679  if (this != &other) {
680  free(data);
681  free(rowPtrs);
682 
683  rowNum = other.rowNum;
684  colNum = other.colNum;
685  rowPtrs = other.rowPtrs;
686  dsize = other.dsize;
687  data = other.data;
688 
689  other.rowNum = 0;
690  other.colNum = 0;
691  other.rowPtrs = NULL;
692  other.dsize = 0;
693  other.data = NULL;
694  }
695 
696  return *this;
697 }
698 
723 vpMatrix& vpMatrix::operator=(const std::initializer_list<double> &list)
724 {
725  if (dsize != static_cast<unsigned int>(list.size())) {
726  resize(1, static_cast<unsigned int>(list.size()), false, false);
727  }
728 
729  std::copy(list.begin(), list.end(), data);
730 
731  return *this;
732 }
733 
757 vpMatrix& vpMatrix::operator=(const std::initializer_list<std::initializer_list<double> > &lists)
758 {
759  unsigned int nrows = static_cast<unsigned int>(lists.size()), ncols = 0;
760  for (auto& l : lists) {
761  if (static_cast<unsigned int>(l.size()) > ncols) {
762  ncols = static_cast<unsigned int>(l.size());
763  }
764  }
765 
766  resize(nrows, ncols, false, false);
767  auto it = lists.begin();
768  for (unsigned int i = 0; i < rowNum; i++, ++it) {
769  std::copy(it->begin(), it->end(), rowPtrs[i]);
770  }
771 
772  return *this;
773 }
774 #endif
775 
778 {
779  std::fill(data, data + rowNum*colNum, x);
780  return *this;
781 }
782 
789 {
790  for (unsigned int i = 0; i < rowNum; i++) {
791  for (unsigned int j = 0; j < colNum; j++) {
792  rowPtrs[i][j] = *x++;
793  }
794  }
795  return *this;
796 }
797 
799 {
800  resize(1, 1, false, false);
801  rowPtrs[0][0] = val;
802  return *this;
803 }
804 
806 {
807  resize(1, colNum + 1, false, false);
808  rowPtrs[0][colNum - 1] = val;
809  return *this;
810 }
811 
848 {
849  unsigned int rows = A.getRows();
850  this->resize(rows, rows);
851 
852  (*this) = 0;
853  for (unsigned int i = 0; i < rows; i++)
854  (*this)[i][i] = A[i];
855 }
856 
887 void vpMatrix::diag(const double &val)
888 {
889  (*this) = 0;
890  unsigned int min_ = (rowNum < colNum) ? rowNum : colNum;
891  for (unsigned int i = 0; i < min_; i++)
892  (*this)[i][i] = val;
893 }
894 
907 {
908  unsigned int rows = A.getRows();
909  DA.resize(rows, rows, true);
910 
911  for (unsigned int i = 0; i < rows; i++)
912  DA[i][i] = A[i];
913 }
914 
920 {
921  vpTranslationVector t_out;
922 
923  if (rowNum != 3 || colNum != 3) {
924  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector",
925  rowNum, colNum, tv.getRows(), tv.getCols()));
926  }
927 
928  for (unsigned int j = 0; j < 3; j++)
929  t_out[j] = 0;
930 
931  for (unsigned int j = 0; j < 3; j++) {
932  double tj = tv[j]; // optimization em 5/12/2006
933  for (unsigned int i = 0; i < 3; i++) {
934  t_out[i] += rowPtrs[i][j] * tj;
935  }
936  }
937  return t_out;
938 }
939 
945 {
946  vpColVector v_out;
947  vpMatrix::multMatrixVector(*this, v, v_out);
948  return v_out;
949 }
950 
960 {
961  if (A.colNum != v.getRows()) {
962  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
963  A.getRows(), A.getCols(), v.getRows()));
964  }
965 
966  if (A.rowNum != w.rowNum)
967  w.resize(A.rowNum, false);
968 
969  // If available use Lapack only for large matrices
970  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size);
971 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
972  useLapack = false;
973 #endif
974 
975  if (useLapack) {
976 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
977  double alpha = 1.0;
978  double beta = 0.0;
979  char trans = 't';
980  int incr = 1;
981 
982  vpMatrix::blas_dgemv(trans, A.colNum, A.rowNum, alpha, A.data, A.colNum, v.data, incr, beta, w.data, incr);
983 #endif
984  }
985  else {
986  w = 0.0;
987  for (unsigned int j = 0; j < A.colNum; j++) {
988  double vj = v[j]; // optimization em 5/12/2006
989  for (unsigned int i = 0; i < A.rowNum; i++) {
990  w[i] += A.rowPtrs[i][j] * vj;
991  }
992  }
993  }
994 }
995 
996 //---------------------------------
997 // Matrix operations.
998 //---------------------------------
999 
1010 {
1011  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1012  C.resize(A.rowNum, B.colNum, false, false);
1013 
1014  if (A.colNum != B.rowNum) {
1015  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (%dx%d) matrix", A.getRows(),
1016  A.getCols(), B.getRows(), B.getCols()));
1017  }
1018 
1019  // If available use Lapack only for large matrices
1020  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size || B.colNum > vpMatrix::m_lapack_min_size);
1021 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1022  useLapack = false;
1023 #endif
1024 
1025  if (useLapack) {
1026 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1027  const double alpha = 1.0;
1028  const double beta = 0.0;
1029  const char trans = 'n';
1030  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1031  C.data, B.colNum);
1032 #endif
1033  }
1034  else {
1035  // 5/12/06 some "very" simple optimization to avoid indexation
1036  const unsigned int BcolNum = B.colNum;
1037  const unsigned int BrowNum = B.rowNum;
1038  double **BrowPtrs = B.rowPtrs;
1039  for (unsigned int i = 0; i < A.rowNum; i++) {
1040  const double *rowptri = A.rowPtrs[i];
1041  double *ci = C[i];
1042  for (unsigned int j = 0; j < BcolNum; j++) {
1043  double s = 0;
1044  for (unsigned int k = 0; k < BrowNum; k++)
1045  s += rowptri[k] * BrowPtrs[k][j];
1046  ci[j] = s;
1047  }
1048  }
1049  }
1050 }
1051 
1066 {
1067  if (A.colNum != 3 || A.rowNum != 3 || B.colNum != 3 || B.rowNum != 3) {
1069  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1070  "rotation matrix",
1071  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1072  }
1073  // 5/12/06 some "very" simple optimization to avoid indexation
1074  const unsigned int BcolNum = B.colNum;
1075  const unsigned int BrowNum = B.rowNum;
1076  double **BrowPtrs = B.rowPtrs;
1077  for (unsigned int i = 0; i < A.rowNum; i++) {
1078  const double *rowptri = A.rowPtrs[i];
1079  double *ci = C[i];
1080  for (unsigned int j = 0; j < BcolNum; j++) {
1081  double s = 0;
1082  for (unsigned int k = 0; k < BrowNum; k++)
1083  s += rowptri[k] * BrowPtrs[k][j];
1084  ci[j] = s;
1085  }
1086  }
1087 }
1088 
1103 {
1104  if (A.colNum != 4 || A.rowNum != 4 || B.colNum != 4 || B.rowNum != 4) {
1106  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1107  "rotation matrix",
1108  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1109  }
1110  // Considering perfMatrixMultiplication.cpp benchmark,
1111  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1112  // Lapack usage needs to be validated again.
1113  // If available use Lapack only for large matrices.
1114  // Using SSE2 doesn't speed up.
1115  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size || B.colNum > vpMatrix::m_lapack_min_size);
1116 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1117  useLapack = false;
1118 #endif
1119 
1120  if (useLapack) {
1121 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1122  const double alpha = 1.0;
1123  const double beta = 0.0;
1124  const char trans = 'n';
1125  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1126  C.data, B.colNum);
1127 #endif
1128  }
1129  else {
1130  // 5/12/06 some "very" simple optimization to avoid indexation
1131  const unsigned int BcolNum = B.colNum;
1132  const unsigned int BrowNum = B.rowNum;
1133  double **BrowPtrs = B.rowPtrs;
1134  for (unsigned int i = 0; i < A.rowNum; i++) {
1135  const double *rowptri = A.rowPtrs[i];
1136  double *ci = C[i];
1137  for (unsigned int j = 0; j < BcolNum; j++) {
1138  double s = 0;
1139  for (unsigned int k = 0; k < BrowNum; k++)
1140  s += rowptri[k] * BrowPtrs[k][j];
1141  ci[j] = s;
1142  }
1143  }
1144  }
1145 }
1146 
1160 {
1161  vpMatrix::multMatrixVector(A, B, C);
1162 }
1163 
1169 {
1170  vpMatrix C;
1171 
1172  vpMatrix::mult2Matrices(*this, B, C);
1173 
1174  return C;
1175 }
1176 
1182 {
1183  if (colNum != R.getRows()) {
1184  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1185  colNum));
1186  }
1187  vpMatrix C;
1188  C.resize(rowNum, 3, false, false);
1189 
1190  unsigned int RcolNum = R.getCols();
1191  unsigned int RrowNum = R.getRows();
1192  for (unsigned int i = 0; i < rowNum; i++) {
1193  double *rowptri = rowPtrs[i];
1194  double *ci = C[i];
1195  for (unsigned int j = 0; j < RcolNum; j++) {
1196  double s = 0;
1197  for (unsigned int k = 0; k < RrowNum; k++)
1198  s += rowptri[k] * R[k][j];
1199  ci[j] = s;
1200  }
1201  }
1202 
1203  return C;
1204 }
1205 
1211 {
1212  if (colNum != M.getRows()) {
1213  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1214  colNum));
1215  }
1216  vpMatrix C;
1217  C.resize(rowNum, 4, false, false);
1218 
1219  const unsigned int McolNum = M.getCols();
1220  const unsigned int MrowNum = M.getRows();
1221  for (unsigned int i = 0; i < rowNum; i++) {
1222  const double *rowptri = rowPtrs[i];
1223  double *ci = C[i];
1224  for (unsigned int j = 0; j < McolNum; j++) {
1225  double s = 0;
1226  for (unsigned int k = 0; k < MrowNum; k++)
1227  s += rowptri[k] * M[k][j];
1228  ci[j] = s;
1229  }
1230  }
1231 
1232  return C;
1233 }
1234 
1240 {
1241  if (colNum != V.getRows()) {
1242  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix",
1243  rowNum, colNum));
1244  }
1245  vpMatrix M;
1246  M.resize(rowNum, 6, false, false);
1247 
1248  // Considering perfMatrixMultiplication.cpp benchmark,
1249  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1250  // Lapack usage needs to be validated again.
1251  // If available use Lapack only for large matrices.
1252  // Speed up obtained using SSE2.
1253  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size || V.colNum > vpMatrix::m_lapack_min_size);
1254 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1255  useLapack = false;
1256 #endif
1257 
1258  if (useLapack) {
1259 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1260  const double alpha = 1.0;
1261  const double beta = 0.0;
1262  const char trans = 'n';
1263  vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, V.colNum, data, colNum, beta,
1264  M.data, M.colNum);
1265 #endif
1266  }
1267  else {
1268  SimdMatMulTwist(data, rowNum, V.data, M.data);
1269  }
1270 
1271  return M;
1272 }
1273 
1279 {
1280  if (colNum != V.getRows()) {
1281  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) force/torque twist matrix",
1282  rowNum, colNum));
1283  }
1284  vpMatrix M;
1285  M.resize(rowNum, 6, false, false);
1286 
1287  // Considering perfMatrixMultiplication.cpp benchmark,
1288  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1289  // Lapack usage needs to be validated again.
1290  // If available use Lapack only for large matrices.
1291  // Speed up obtained using SSE2.
1292  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size || V.getCols() > vpMatrix::m_lapack_min_size);
1293 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1294  useLapack = false;
1295 #endif
1296 
1297  if (useLapack) {
1298 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1299  const double alpha = 1.0;
1300  const double beta = 0.0;
1301  const char trans = 'n';
1302  vpMatrix::blas_dgemm(trans, trans, V.getCols(), rowNum, colNum, alpha, V.data, V.getCols(), data, colNum, beta,
1303  M.data, M.colNum);
1304 #endif
1305  }
1306  else {
1307  SimdMatMulTwist(data, rowNum, V.data, M.data);
1308  }
1309 
1310  return M;
1311 }
1312 
1323 void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB,
1324  vpMatrix &C)
1325 {
1326  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1327  C.resize(A.rowNum, B.colNum, false, false);
1328 
1329  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1330  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1331  A.getCols(), B.getRows(), B.getCols()));
1332  }
1333 
1334  double **ArowPtrs = A.rowPtrs;
1335  double **BrowPtrs = B.rowPtrs;
1336  double **CrowPtrs = C.rowPtrs;
1337 
1338  for (unsigned int i = 0; i < A.rowNum; i++)
1339  for (unsigned int j = 0; j < A.colNum; j++)
1340  CrowPtrs[i][j] = wB * BrowPtrs[i][j] + wA * ArowPtrs[i][j];
1341 }
1342 
1352 void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1353 {
1354  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1355  C.resize(A.rowNum, B.colNum, false, false);
1356 
1357  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1358  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1359  A.getCols(), B.getRows(), B.getCols()));
1360  }
1361 
1362  double **ArowPtrs = A.rowPtrs;
1363  double **BrowPtrs = B.rowPtrs;
1364  double **CrowPtrs = C.rowPtrs;
1365 
1366  for (unsigned int i = 0; i < A.rowNum; i++) {
1367  for (unsigned int j = 0; j < A.colNum; j++) {
1368  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1369  }
1370  }
1371 }
1372 
1386 {
1387  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1388  C.resize(A.rowNum);
1389 
1390  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1391  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1392  A.getCols(), B.getRows(), B.getCols()));
1393  }
1394 
1395  double **ArowPtrs = A.rowPtrs;
1396  double **BrowPtrs = B.rowPtrs;
1397  double **CrowPtrs = C.rowPtrs;
1398 
1399  for (unsigned int i = 0; i < A.rowNum; i++) {
1400  for (unsigned int j = 0; j < A.colNum; j++) {
1401  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1402  }
1403  }
1404 }
1405 
1411 {
1412  vpMatrix C;
1413  vpMatrix::add2Matrices(*this, B, C);
1414  return C;
1415 }
1416 
1433 {
1434  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1435  C.resize(A.rowNum);
1436 
1437  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1438  throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1439  A.getCols(), B.getRows(), B.getCols()));
1440  }
1441 
1442  double **ArowPtrs = A.rowPtrs;
1443  double **BrowPtrs = B.rowPtrs;
1444  double **CrowPtrs = C.rowPtrs;
1445 
1446  for (unsigned int i = 0; i < A.rowNum; i++) {
1447  for (unsigned int j = 0; j < A.colNum; j++) {
1448  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1449  }
1450  }
1451 }
1452 
1465 void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1466 {
1467  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1468  C.resize(A.rowNum, A.colNum, false, false);
1469 
1470  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1471  throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1472  A.getCols(), B.getRows(), B.getCols()));
1473  }
1474 
1475  double **ArowPtrs = A.rowPtrs;
1476  double **BrowPtrs = B.rowPtrs;
1477  double **CrowPtrs = C.rowPtrs;
1478 
1479  for (unsigned int i = 0; i < A.rowNum; i++) {
1480  for (unsigned int j = 0; j < A.colNum; j++) {
1481  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1482  }
1483  }
1484 }
1485 
1491 {
1492  vpMatrix C;
1493  vpMatrix::sub2Matrices(*this, B, C);
1494  return C;
1495 }
1496 
1498 
1500 {
1501  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1502  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1503  B.getRows(), B.getCols()));
1504  }
1505 
1506  double **BrowPtrs = B.rowPtrs;
1507 
1508  for (unsigned int i = 0; i < rowNum; i++)
1509  for (unsigned int j = 0; j < colNum; j++)
1510  rowPtrs[i][j] += BrowPtrs[i][j];
1511 
1512  return *this;
1513 }
1514 
1517 {
1518  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1519  throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1520  B.getRows(), B.getCols()));
1521  }
1522 
1523  double **BrowPtrs = B.rowPtrs;
1524  for (unsigned int i = 0; i < rowNum; i++)
1525  for (unsigned int j = 0; j < colNum; j++)
1526  rowPtrs[i][j] -= BrowPtrs[i][j];
1527 
1528  return *this;
1529 }
1530 
1541 {
1542  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1543  C.resize(A.rowNum, A.colNum, false, false);
1544 
1545  double **ArowPtrs = A.rowPtrs;
1546  double **CrowPtrs = C.rowPtrs;
1547 
1548  // t0=vpTime::measureTimeMicros();
1549  for (unsigned int i = 0; i < A.rowNum; i++)
1550  for (unsigned int j = 0; j < A.colNum; j++)
1551  CrowPtrs[i][j] = -ArowPtrs[i][j];
1552 }
1553 
1559 {
1560  vpMatrix C;
1561  vpMatrix::negateMatrix(*this, C);
1562  return C;
1563 }
1564 
1565 double vpMatrix::sum() const
1566 {
1567  double s = 0.0;
1568  for (unsigned int i = 0; i < rowNum; i++) {
1569  for (unsigned int j = 0; j < colNum; j++) {
1570  s += rowPtrs[i][j];
1571  }
1572  }
1573 
1574  return s;
1575 }
1576 
1577 //---------------------------------
1578 // Matrix/vector operations.
1579 //---------------------------------
1580 
1581 //---------------------------------
1582 // Matrix/real operations.
1583 //---------------------------------
1584 
1589 vpMatrix operator*(const double &x, const vpMatrix &B)
1590 {
1591  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1592  return B;
1593  }
1594 
1595  unsigned int Brow = B.getRows();
1596  unsigned int Bcol = B.getCols();
1597 
1598  vpMatrix C;
1599  C.resize(Brow, Bcol, false, false);
1600 
1601  for (unsigned int i = 0; i < Brow; i++)
1602  for (unsigned int j = 0; j < Bcol; j++)
1603  C[i][j] = B[i][j] * x;
1604 
1605  return C;
1606 }
1607 
1613 {
1614  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1615  return (*this);
1616  }
1617 
1618  vpMatrix M;
1619  M.resize(rowNum, colNum, false, false);
1620 
1621  for (unsigned int i = 0; i < rowNum; i++)
1622  for (unsigned int j = 0; j < colNum; j++)
1623  M[i][j] = rowPtrs[i][j] * x;
1624 
1625  return M;
1626 }
1627 
1630 {
1631  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1632  return (*this);
1633  }
1634 
1635  if (std::fabs(x) < std::numeric_limits<double>::epsilon()) {
1636  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1637  }
1638 
1639  vpMatrix C;
1640  C.resize(rowNum, colNum, false, false);
1641 
1642  double xinv = 1 / x;
1643 
1644  for (unsigned int i = 0; i < rowNum; i++)
1645  for (unsigned int j = 0; j < colNum; j++)
1646  C[i][j] = rowPtrs[i][j] * xinv;
1647 
1648  return C;
1649 }
1650 
1653 {
1654  for (unsigned int i = 0; i < rowNum; i++)
1655  for (unsigned int j = 0; j < colNum; j++)
1656  rowPtrs[i][j] += x;
1657 
1658  return *this;
1659 }
1660 
1663 {
1664  for (unsigned int i = 0; i < rowNum; i++)
1665  for (unsigned int j = 0; j < colNum; j++)
1666  rowPtrs[i][j] -= x;
1667 
1668  return *this;
1669 }
1670 
1676 {
1677  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1678  return *this;
1679  }
1680 
1681  for (unsigned int i = 0; i < rowNum; i++)
1682  for (unsigned int j = 0; j < colNum; j++)
1683  rowPtrs[i][j] *= x;
1684 
1685  return *this;
1686 }
1687 
1690 {
1691  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1692  return *this;
1693  }
1694 
1695  if (std::fabs(x) < std::numeric_limits<double>::epsilon())
1696  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1697 
1698  double xinv = 1 / x;
1699 
1700  for (unsigned int i = 0; i < rowNum; i++)
1701  for (unsigned int j = 0; j < colNum; j++)
1702  rowPtrs[i][j] *= xinv;
1703 
1704  return *this;
1705 }
1706 
1707 //----------------------------------------------------------------
1708 // Matrix Operation
1709 //----------------------------------------------------------------
1710 
1716 {
1717  if ((out.rowNum != colNum * rowNum) || (out.colNum != 1))
1718  out.resize(colNum * rowNum, false, false);
1719 
1720  double *optr = out.data;
1721  for (unsigned int j = 0; j < colNum; j++) {
1722  for (unsigned int i = 0; i < rowNum; i++) {
1723  *(optr++) = rowPtrs[i][j];
1724  }
1725  }
1726 }
1727 
1733 {
1734  vpColVector out(colNum * rowNum);
1735  stackColumns(out);
1736  return out;
1737 }
1738 
1744 {
1745  if ((out.getRows() != 1) || (out.getCols() != colNum * rowNum))
1746  out.resize(colNum * rowNum, false, false);
1747 
1748  memcpy(out.data, data, sizeof(double)*out.getCols());
1749 }
1755 {
1756  vpRowVector out(colNum * rowNum);
1757  stackRows(out);
1758  return out;
1759 }
1760 
1768 {
1769  if (m.getRows() != rowNum || m.getCols() != colNum) {
1770  throw(vpException(vpException::dimensionError, "In Hadamard product: bad dimension of input matrix"));
1771  }
1772 
1773  vpMatrix out;
1774  out.resize(rowNum, colNum, false, false);
1775 
1776  SimdVectorHadamard(data, m.data, dsize, out.data);
1777 
1778  return out;
1779 }
1780 
1787 void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out)
1788 {
1789  unsigned int r1 = m1.getRows();
1790  unsigned int c1 = m1.getCols();
1791  unsigned int r2 = m2.getRows();
1792  unsigned int c2 = m2.getCols();
1793 
1794  out.resize(r1*r2, c1*c2, false, false);
1795 
1796  for (unsigned int r = 0; r < r1; r++) {
1797  for (unsigned int c = 0; c < c1; c++) {
1798  double alpha = m1[r][c];
1799  double *m2ptr = m2[0];
1800  unsigned int roffset = r * r2;
1801  unsigned int coffset = c * c2;
1802  for (unsigned int rr = 0; rr < r2; rr++) {
1803  for (unsigned int cc = 0; cc < c2; cc++) {
1804  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1805  }
1806  }
1807  }
1808  }
1809 }
1810 
1817 void vpMatrix::kron(const vpMatrix &m, vpMatrix &out) const { kron(*this, m, out); }
1818 
1826 {
1827  unsigned int r1 = m1.getRows();
1828  unsigned int c1 = m1.getCols();
1829  unsigned int r2 = m2.getRows();
1830  unsigned int c2 = m2.getCols();
1831 
1832  vpMatrix out;
1833  out.resize(r1 * r2, c1 * c2, false, false);
1834 
1835  for (unsigned int r = 0; r < r1; r++) {
1836  for (unsigned int c = 0; c < c1; c++) {
1837  double alpha = m1[r][c];
1838  double *m2ptr = m2[0];
1839  unsigned int roffset = r * r2;
1840  unsigned int coffset = c * c2;
1841  for (unsigned int rr = 0; rr < r2; rr++) {
1842  for (unsigned int cc = 0; cc < c2; cc++) {
1843  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1844  }
1845  }
1846  }
1847  }
1848  return out;
1849 }
1850 
1856 vpMatrix vpMatrix::kron(const vpMatrix &m) const { return kron(*this, m); }
1857 
1908 void vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const { x = pseudoInverse(1e-6) * b; }
1909 
1960 {
1961  vpColVector X(colNum);
1962 
1963  solveBySVD(B, X);
1964  return X;
1965 }
1966 
2031 {
2032 #if defined(VISP_HAVE_LAPACK)
2033  svdLapack(w, V);
2034 #elif defined(VISP_HAVE_EIGEN3)
2035  svdEigen3(w, V);
2036 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2037  svdOpenCV(w, V);
2038 #else
2039  (void)w;
2040  (void)V;
2041  throw(vpException(vpException::fatalError, "Cannot compute SVD. Install Lapack, Eigen3 or OpenCV 3rd party"));
2042 #endif
2043 }
2044 
2099 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
2100 {
2101 #if defined(VISP_HAVE_LAPACK)
2102  return pseudoInverseLapack(Ap, svThreshold);
2103 #elif defined(VISP_HAVE_EIGEN3)
2104  return pseudoInverseEigen3(Ap, svThreshold);
2105 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2106  return pseudoInverseOpenCV(Ap, svThreshold);
2107 #else
2108  (void)Ap;
2109  (void)svThreshold;
2110  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2111  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2112 #endif
2113 }
2114 
2175 int vpMatrix::pseudoInverse(vpMatrix &Ap, int rank_in) const
2176 {
2177 #if defined(VISP_HAVE_LAPACK)
2178  return pseudoInverseLapack(Ap, rank_in);
2179 #elif defined(VISP_HAVE_EIGEN3)
2180  return pseudoInverseEigen3(Ap, rank_in);
2181 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2182  return pseudoInverseOpenCV(Ap, rank_in);
2183 #else
2184  (void)Ap;
2185  (void)svThreshold;
2186  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2187  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2188 #endif
2189 }
2190 
2241 vpMatrix vpMatrix::pseudoInverse(double svThreshold) const
2242 {
2243 #if defined(VISP_HAVE_LAPACK)
2244  return pseudoInverseLapack(svThreshold);
2245 #elif defined(VISP_HAVE_EIGEN3)
2246  return pseudoInverseEigen3(svThreshold);
2247 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2248  return pseudoInverseOpenCV(svThreshold);
2249 #else
2250  (void)svThreshold;
2251  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2252  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2253 #endif
2254 }
2255 
2307 {
2308 #if defined(VISP_HAVE_LAPACK)
2309  return pseudoInverseLapack(rank_in);
2310 #elif defined(VISP_HAVE_EIGEN3)
2311  return pseudoInverseEigen3(rank_in);
2312 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2313  return pseudoInverseOpenCV(rank_in);
2314 #else
2315  (void)svThreshold;
2316  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2317  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2318 #endif
2319 }
2320 
2321 #ifndef DOXYGEN_SHOULD_SKIP_THIS
2322 #if defined(VISP_HAVE_LAPACK)
2323 
2359 vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const
2360 {
2361  unsigned int nrows = getRows();
2362  unsigned int ncols = getCols();
2363  int rank_out;
2364 
2365  vpMatrix U, V, Ap;
2366  vpColVector sv;
2367 
2368  Ap.resize(ncols, nrows, false, false);
2369 
2370  if (nrows < ncols) {
2371  U.resize(ncols, ncols, true);
2372  sv.resize(nrows, false);
2373  } else {
2374  U.resize(nrows, ncols, false);
2375  sv.resize(ncols, false);
2376  }
2377 
2378  U.insert(*this, 0, 0);
2379  U.svdLapack(sv, V);
2380 
2381  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
2382 
2383  return Ap;
2384 }
2385 
2426 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) const
2427 {
2428  unsigned int nrows = getRows();
2429  unsigned int ncols = getCols();
2430  int rank_out;
2431 
2432  vpMatrix U, V;
2433  vpColVector sv;
2434 
2435  Ap.resize(ncols, nrows, false, false);
2436 
2437  if (nrows < ncols) {
2438  U.resize(ncols, ncols, true);
2439  sv.resize(nrows, false);
2440  } else {
2441  U.resize(nrows, ncols, false);
2442  sv.resize(ncols, false);
2443  }
2444 
2445  U.insert(*this, 0, 0);
2446  U.svdLapack(sv, V);
2447 
2448  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
2449 
2450  return static_cast<unsigned int>(rank_out);
2451 }
2499 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2500 {
2501  unsigned int nrows = getRows();
2502  unsigned int ncols = getCols();
2503  int rank_out;
2504 
2505  vpMatrix U, V;
2506  vpColVector sv_;
2507 
2508  Ap.resize(ncols, nrows, false, false);
2509 
2510  if (nrows < ncols) {
2511  U.resize(ncols, ncols, true);
2512  sv.resize(nrows, false);
2513  } else {
2514  U.resize(nrows, ncols, false);
2515  sv.resize(ncols, false);
2516  }
2517 
2518  U.insert(*this, 0, 0);
2519  U.svdLapack(sv_, V);
2520 
2521  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
2522 
2523  // Remove singular values equal to the one that corresponds to the lines of 0
2524  // introduced when m < n
2525  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2526 
2527  return static_cast<unsigned int>(rank_out);
2528 }
2529 
2636 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
2637  vpMatrix &imAt, vpMatrix &kerA) const
2638 {
2639  unsigned int nrows = getRows();
2640  unsigned int ncols = getCols();
2641  int rank_out;
2642  vpMatrix U, V;
2643  vpColVector sv_;
2644 
2645  if (nrows < ncols) {
2646  U.resize(ncols, ncols, true);
2647  sv.resize(nrows, false);
2648  } else {
2649  U.resize(nrows, ncols, false);
2650  sv.resize(ncols, false);
2651  }
2652 
2653  U.insert(*this, 0, 0);
2654  U.svdLapack(sv_, V);
2655 
2656  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerA);
2657 
2658  // Remove singular values equal to the one that corresponds to the lines of 0
2659  // introduced when m < n
2660  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2661 
2662  return static_cast<unsigned int>(rank_out);
2663 }
2664 
2705 vpMatrix vpMatrix::pseudoInverseLapack(int rank_in) const
2706 {
2707  unsigned int nrows = getRows();
2708  unsigned int ncols = getCols();
2709  int rank_out;
2710  double svThreshold = 1e-26;
2711 
2712  vpMatrix U, V, Ap;
2713  vpColVector sv;
2714 
2715  Ap.resize(ncols, nrows, false, false);
2716 
2717  if (nrows < ncols) {
2718  U.resize(ncols, ncols, true);
2719  sv.resize(nrows, false);
2720  } else {
2721  U.resize(nrows, ncols, false);
2722  sv.resize(ncols, false);
2723  }
2724 
2725  U.insert(*this, 0, 0);
2726  U.svdLapack(sv, V);
2727 
2728  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
2729 
2730  return Ap;
2731 }
2732 
2779 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, int rank_in) const
2780 {
2781  unsigned int nrows = getRows();
2782  unsigned int ncols = getCols();
2783  int rank_out;
2784  double svThreshold = 1e-26;
2785 
2786  vpMatrix U, V;
2787  vpColVector sv;
2788 
2789  Ap.resize(ncols, nrows, false, false);
2790 
2791  if (nrows < ncols) {
2792  U.resize(ncols, ncols, true);
2793  sv.resize(nrows, false);
2794  } else {
2795  U.resize(nrows, ncols, false);
2796  sv.resize(ncols, false);
2797  }
2798 
2799  U.insert(*this, 0, 0);
2800  U.svdLapack(sv, V);
2801 
2802  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
2803 
2804  return rank_out;
2805 }
2806 
2860 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in) const
2861 {
2862  unsigned int nrows = getRows();
2863  unsigned int ncols = getCols();
2864  int rank_out;
2865  double svThreshold = 1e-26;
2866 
2867  vpMatrix U, V;
2868  vpColVector sv_;
2869 
2870  Ap.resize(ncols, nrows, false, false);
2871 
2872  if (nrows < ncols) {
2873  U.resize(ncols, ncols, true);
2874  sv.resize(nrows, false);
2875  } else {
2876  U.resize(nrows, ncols, false);
2877  sv.resize(ncols, false);
2878  }
2879 
2880  U.insert(*this, 0, 0);
2881  U.svdLapack(sv_, V);
2882 
2883  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
2884 
2885  // Remove singular values equal to the one that corresponds to the lines of 0
2886  // introduced when m < n
2887  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2888 
2889  return rank_out;
2890 }
2891 
3003 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA,
3004  vpMatrix &imAt, vpMatrix &kerA) const
3005 {
3006  unsigned int nrows = getRows();
3007  unsigned int ncols = getCols();
3008  int rank_out;
3009  double svThreshold = 1e-26;
3010  vpMatrix U, V;
3011  vpColVector sv_;
3012 
3013  if (nrows < ncols) {
3014  U.resize(ncols, ncols, true);
3015  sv.resize(nrows, false);
3016  } else {
3017  U.resize(nrows, ncols, false);
3018  sv.resize(ncols, false);
3019  }
3020 
3021  U.insert(*this, 0, 0);
3022  U.svdLapack(sv_, V);
3023 
3024  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerA);
3025 
3026  // Remove singular values equal to the one that corresponds to the lines of 0
3027  // introduced when m < n
3028  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3029 
3030  return rank_out;
3031 }
3032 
3033 #endif
3034 #if defined(VISP_HAVE_EIGEN3)
3035 
3071 vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const
3072 {
3073  unsigned int nrows = getRows();
3074  unsigned int ncols = getCols();
3075  int rank_out;
3076 
3077  vpMatrix U, V, Ap;
3078  vpColVector sv;
3079 
3080  Ap.resize(ncols, nrows, false, false);
3081 
3082  if (nrows < ncols) {
3083  U.resize(ncols, ncols, true);
3084  sv.resize(nrows, false);
3085  } else {
3086  U.resize(nrows, ncols, false);
3087  sv.resize(ncols, false);
3088  }
3089 
3090  U.insert(*this, 0, 0);
3091  U.svdEigen3(sv, V);
3092 
3093  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3094 
3095  return Ap;
3096 }
3097 
3138 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) const
3139 {
3140  unsigned int nrows = getRows();
3141  unsigned int ncols = getCols();
3142  int rank_out;
3143 
3144  vpMatrix U, V;
3145  vpColVector sv;
3146 
3147  Ap.resize(ncols, nrows, false, false);
3148 
3149  if (nrows < ncols) {
3150  U.resize(ncols, ncols, true);
3151  sv.resize(nrows, false);
3152  } else {
3153  U.resize(nrows, ncols, false);
3154  sv.resize(ncols, false);
3155  }
3156 
3157  U.insert(*this, 0, 0);
3158  U.svdEigen3(sv, V);
3159 
3160  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3161 
3162  return static_cast<unsigned int>(rank_out);
3163 }
3211 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3212 {
3213  unsigned int nrows = getRows();
3214  unsigned int ncols = getCols();
3215  int rank_out;
3216 
3217  vpMatrix U, V;
3218  vpColVector sv_;
3219 
3220  Ap.resize(ncols, nrows, false, false);
3221 
3222  if (nrows < ncols) {
3223  U.resize(ncols, ncols, true);
3224  sv.resize(nrows, false);
3225  } else {
3226  U.resize(nrows, ncols, false);
3227  sv.resize(ncols, false);
3228  }
3229 
3230  U.insert(*this, 0, 0);
3231  U.svdEigen3(sv_, V);
3232 
3233  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3234 
3235  // Remove singular values equal to the one that corresponds to the lines of 0
3236  // introduced when m < n
3237  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3238 
3239  return static_cast<unsigned int>(rank_out);
3240 }
3241 
3348 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3349  vpMatrix &imAt, vpMatrix &kerA) const
3350 {
3351  unsigned int nrows = getRows();
3352  unsigned int ncols = getCols();
3353  int rank_out;
3354  vpMatrix U, V;
3355  vpColVector sv_;
3356 
3357  if (nrows < ncols) {
3358  U.resize(ncols, ncols, true);
3359  sv.resize(nrows, false);
3360  } else {
3361  U.resize(nrows, ncols, false);
3362  sv.resize(ncols, false);
3363  }
3364 
3365  U.insert(*this, 0, 0);
3366  U.svdEigen3(sv_, V);
3367 
3368  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerA);
3369 
3370  // Remove singular values equal to the one that corresponds to the lines of 0
3371  // introduced when m < n
3372  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3373 
3374  return static_cast<unsigned int>(rank_out);
3375 }
3376 
3417 vpMatrix vpMatrix::pseudoInverseEigen3(int rank_in) const
3418 {
3419  unsigned int nrows = getRows();
3420  unsigned int ncols = getCols();
3421  int rank_out;
3422  double svThreshold = 1e-26;
3423 
3424  vpMatrix U, V, Ap;
3425  vpColVector sv;
3426 
3427  Ap.resize(ncols, nrows, false, false);
3428 
3429  if (nrows < ncols) {
3430  U.resize(ncols, ncols, true);
3431  sv.resize(nrows, false);
3432  } else {
3433  U.resize(nrows, ncols, false);
3434  sv.resize(ncols, false);
3435  }
3436 
3437  U.insert(*this, 0, 0);
3438  U.svdEigen3(sv, V);
3439 
3440  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3441 
3442  return Ap;
3443 }
3444 
3491 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, int rank_in) const
3492 {
3493  unsigned int nrows = getRows();
3494  unsigned int ncols = getCols();
3495  int rank_out;
3496  double svThreshold = 1e-26;
3497 
3498  vpMatrix U, V;
3499  vpColVector sv;
3500 
3501  Ap.resize(ncols, nrows, false, false);
3502 
3503  if (nrows < ncols) {
3504  U.resize(ncols, ncols, true);
3505  sv.resize(nrows, false);
3506  } else {
3507  U.resize(nrows, ncols, false);
3508  sv.resize(ncols, false);
3509  }
3510 
3511  U.insert(*this, 0, 0);
3512  U.svdEigen3(sv, V);
3513 
3514  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3515 
3516  return rank_out;
3517 }
3518 
3572 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in) const
3573 {
3574  unsigned int nrows = getRows();
3575  unsigned int ncols = getCols();
3576  int rank_out;
3577  double svThreshold = 1e-26;
3578 
3579  vpMatrix U, V;
3580  vpColVector sv_;
3581 
3582  Ap.resize(ncols, nrows, false, false);
3583 
3584  if (nrows < ncols) {
3585  U.resize(ncols, ncols, true);
3586  sv.resize(nrows, false);
3587  } else {
3588  U.resize(nrows, ncols, false);
3589  sv.resize(ncols, false);
3590  }
3591 
3592  U.insert(*this, 0, 0);
3593  U.svdEigen3(sv_, V);
3594 
3595  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3596 
3597  // Remove singular values equal to the one that corresponds to the lines of 0
3598  // introduced when m < n
3599  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3600 
3601  return rank_out;
3602 }
3603 
3715 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA,
3716  vpMatrix &imAt, vpMatrix &kerA) const
3717 {
3718  unsigned int nrows = getRows();
3719  unsigned int ncols = getCols();
3720  int rank_out;
3721  double svThreshold = 1e-26;
3722  vpMatrix U, V;
3723  vpColVector sv_;
3724 
3725  if (nrows < ncols) {
3726  U.resize(ncols, ncols, true);
3727  sv.resize(nrows, false);
3728  } else {
3729  U.resize(nrows, ncols, false);
3730  sv.resize(ncols, false);
3731  }
3732 
3733  U.insert(*this, 0, 0);
3734  U.svdEigen3(sv_, V);
3735 
3736  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerA);
3737 
3738  // Remove singular values equal to the one that corresponds to the lines of 0
3739  // introduced when m < n
3740  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3741 
3742  return rank_out;
3743 }
3744 
3745 #endif
3746 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
3747 
3783 vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const
3784 {
3785  unsigned int nrows = getRows();
3786  unsigned int ncols = getCols();
3787  int rank_out;
3788 
3789  vpMatrix U, V, Ap;
3790  vpColVector sv;
3791 
3792  Ap.resize(ncols, nrows, false, false);
3793 
3794  if (nrows < ncols) {
3795  U.resize(ncols, ncols, true);
3796  sv.resize(nrows, false);
3797  } else {
3798  U.resize(nrows, ncols, false);
3799  sv.resize(ncols, false);
3800  }
3801 
3802  U.insert(*this, 0, 0);
3803  U.svdOpenCV(sv, V);
3804 
3805  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3806 
3807  return Ap;
3808 }
3809 
3850 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) const
3851 {
3852  unsigned int nrows = getRows();
3853  unsigned int ncols = getCols();
3854  int rank_out;
3855 
3856  vpMatrix U, V;
3857  vpColVector sv;
3858 
3859  Ap.resize(ncols, nrows, false, false);
3860 
3861  if (nrows < ncols) {
3862  U.resize(ncols, ncols, true);
3863  sv.resize(nrows, false);
3864  } else {
3865  U.resize(nrows, ncols, false);
3866  sv.resize(ncols, false);
3867  }
3868 
3869  U.insert(*this, 0, 0);
3870  U.svdOpenCV(sv, V);
3871 
3872  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3873 
3874  return static_cast<unsigned int>(rank_out);
3875 }
3923 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3924 {
3925  unsigned int nrows = getRows();
3926  unsigned int ncols = getCols();
3927  int rank_out;
3928 
3929  vpMatrix U, V;
3930  vpColVector sv_;
3931 
3932  Ap.resize(ncols, nrows, false, false);
3933 
3934  if (nrows < ncols) {
3935  U.resize(ncols, ncols, true);
3936  sv.resize(nrows, false);
3937  } else {
3938  U.resize(nrows, ncols, false);
3939  sv.resize(ncols, false);
3940  }
3941 
3942  U.insert(*this, 0, 0);
3943  U.svdOpenCV(sv_, V);
3944 
3945  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3946 
3947  // Remove singular values equal to the one that corresponds to the lines of 0
3948  // introduced when m < n
3949  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3950 
3951  return static_cast<unsigned int>(rank_out);
3952 }
3953 
4060 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
4061  vpMatrix &imAt, vpMatrix &kerA) const
4062 {
4063  unsigned int nrows = getRows();
4064  unsigned int ncols = getCols();
4065  int rank_out;
4066  vpMatrix U, V;
4067  vpColVector sv_;
4068 
4069  if (nrows < ncols) {
4070  U.resize(ncols, ncols, true);
4071  sv.resize(nrows, false);
4072  } else {
4073  U.resize(nrows, ncols, false);
4074  sv.resize(ncols, false);
4075  }
4076 
4077  U.insert(*this, 0, 0);
4078  U.svdOpenCV(sv_, V);
4079 
4080  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerA);
4081 
4082  // Remove singular values equal to the one that corresponds to the lines of 0
4083  // introduced when m < n
4084  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4085 
4086  return static_cast<unsigned int>(rank_out);
4087 }
4088 
4129 vpMatrix vpMatrix::pseudoInverseOpenCV(int rank_in) const
4130 {
4131  unsigned int nrows = getRows();
4132  unsigned int ncols = getCols();
4133  int rank_out;
4134  double svThreshold = 1e-26;
4135 
4136  vpMatrix U, V, Ap;
4137  vpColVector sv;
4138 
4139  Ap.resize(ncols, nrows, false, false);
4140 
4141  if (nrows < ncols) {
4142  U.resize(ncols, ncols, true);
4143  sv.resize(nrows, false);
4144  } else {
4145  U.resize(nrows, ncols, false);
4146  sv.resize(ncols, false);
4147  }
4148 
4149  U.insert(*this, 0, 0);
4150  U.svdOpenCV(sv, V);
4151 
4152  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4153 
4154  return Ap;
4155 }
4156 
4203 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, int rank_in) const
4204 {
4205  unsigned int nrows = getRows();
4206  unsigned int ncols = getCols();
4207  int rank_out;
4208  double svThreshold = 1e-26;
4209 
4210  vpMatrix U, V;
4211  vpColVector sv;
4212 
4213  Ap.resize(ncols, nrows, false, false);
4214 
4215  if (nrows < ncols) {
4216  U.resize(ncols, ncols, true);
4217  sv.resize(nrows, false);
4218  } else {
4219  U.resize(nrows, ncols, false);
4220  sv.resize(ncols, false);
4221  }
4222 
4223  U.insert(*this, 0, 0);
4224  U.svdOpenCV(sv, V);
4225 
4226  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4227 
4228  return rank_out;
4229 }
4230 
4284 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4285 {
4286  unsigned int nrows = getRows();
4287  unsigned int ncols = getCols();
4288  int rank_out;
4289  double svThreshold = 1e-26;
4290 
4291  vpMatrix U, V;
4292  vpColVector sv_;
4293 
4294  Ap.resize(ncols, nrows, false, false);
4295 
4296  if (nrows < ncols) {
4297  U.resize(ncols, ncols, true);
4298  sv.resize(nrows, false);
4299  } else {
4300  U.resize(nrows, ncols, false);
4301  sv.resize(ncols, false);
4302  }
4303 
4304  U.insert(*this, 0, 0);
4305  U.svdOpenCV(sv_, V);
4306 
4307  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4308 
4309  // Remove singular values equal to the one that corresponds to the lines of 0
4310  // introduced when m < n
4311  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4312 
4313  return rank_out;
4314 }
4315 
4427 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA,
4428  vpMatrix &imAt, vpMatrix &kerA) const
4429 {
4430  unsigned int nrows = getRows();
4431  unsigned int ncols = getCols();
4432  int rank_out;
4433  double svThreshold = 1e-26;
4434  vpMatrix U, V;
4435  vpColVector sv_;
4436 
4437  if (nrows < ncols) {
4438  U.resize(ncols, ncols, true);
4439  sv.resize(nrows, false);
4440  } else {
4441  U.resize(nrows, ncols, false);
4442  sv.resize(ncols, false);
4443  }
4444 
4445  U.insert(*this, 0, 0);
4446  U.svdOpenCV(sv_, V);
4447 
4448  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerA);
4449 
4450  // Remove singular values equal to the one that corresponds to the lines of 0
4451  // introduced when m < n
4452  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4453 
4454  return rank_out;
4455 }
4456 
4457 #endif
4458 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
4459 
4521 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
4522 {
4523 #if defined(VISP_HAVE_LAPACK)
4524  return pseudoInverseLapack(Ap, sv, svThreshold);
4525 #elif defined(VISP_HAVE_EIGEN3)
4526  return pseudoInverseEigen3(Ap, sv, svThreshold);
4527 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
4528  return pseudoInverseOpenCV(Ap, sv, svThreshold);
4529 #else
4530  (void)Ap;
4531  (void)sv;
4532  (void)svThreshold;
4533  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4534  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4535 #endif
4536 }
4537 
4604 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4605 {
4606 #if defined(VISP_HAVE_LAPACK)
4607  return pseudoInverseLapack(Ap, sv, rank_in);
4608 #elif defined(VISP_HAVE_EIGEN3)
4609  return pseudoInverseEigen3(Ap, sv, rank_in);
4610 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
4611  return pseudoInverseOpenCV(Ap, sv, rank_in);
4612 #else
4613  (void)Ap;
4614  (void)sv;
4615  (void)svThreshold;
4616  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4617  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4618 #endif
4619 }
4694 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt) const
4695 {
4696  vpMatrix kerAt;
4697  return pseudoInverse(Ap, sv, svThreshold, imA, imAt, kerAt);
4698 }
4699 
4778 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt) const
4779 {
4780  vpMatrix kerAt;
4781  return pseudoInverse(Ap, sv, rank_in, imA, imAt, kerAt);
4782 }
4783 
4918 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt, vpMatrix &kerAt) const
4919 {
4920 #if defined(VISP_HAVE_LAPACK)
4921  return pseudoInverseLapack(Ap, sv, svThreshold, imA, imAt, kerAt);
4922 #elif defined(VISP_HAVE_EIGEN3)
4923  return pseudoInverseEigen3(Ap, sv, svThreshold, imA, imAt, kerAt);
4924 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
4925  return pseudoInverseOpenCV(Ap, sv, svThreshold, imA, imAt, kerAt);
4926 #else
4927  (void)Ap;
4928  (void)sv;
4929  (void)svThreshold;
4930  (void)imA;
4931  (void)imAt;
4932  (void)kerAt;
4933  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4934  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4935 #endif
4936 }
4937 
5077 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt, vpMatrix &kerAt) const
5078 {
5079 #if defined(VISP_HAVE_LAPACK)
5080  return pseudoInverseLapack(Ap, sv, rank_in, imA, imAt, kerAt);
5081 #elif defined(VISP_HAVE_EIGEN3)
5082  return pseudoInverseEigen3(Ap, sv, rank_in, imA, imAt, kerAt);
5083 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
5084  return pseudoInverseOpenCV(Ap, sv, rank_in, imA, imAt, kerAt);
5085 #else
5086  (void)Ap;
5087  (void)sv;
5088  (void)svThreshold;
5089  (void)imA;
5090  (void)imAt;
5091  (void)kerAt;
5092  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
5093  "Install Lapack, Eigen3 or OpenCV 3rd party"));
5094 #endif
5095 }
5096 
5138 vpColVector vpMatrix::getCol(unsigned int j, unsigned int i_begin, unsigned int column_size) const
5139 {
5140  if (i_begin + column_size > getRows() || j >= getCols())
5141  throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(), getCols()));
5142  vpColVector c(column_size);
5143  for (unsigned int i = 0; i < column_size; i++)
5144  c[i] = (*this)[i_begin + i][j];
5145  return c;
5146 }
5147 
5187 vpColVector vpMatrix::getCol(unsigned int j) const
5188 {
5189  return getCol(j, 0, rowNum);
5190 }
5191 
5227 vpRowVector vpMatrix::getRow(unsigned int i) const
5228 {
5229  return getRow(i, 0, colNum);
5230 }
5231 
5271 vpRowVector vpMatrix::getRow(unsigned int i, unsigned int j_begin, unsigned int row_size) const
5272 {
5273  if (j_begin + row_size > colNum || i >= rowNum)
5274  throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
5275 
5276  vpRowVector r(row_size);
5277  if (r.data != NULL && data != NULL) {
5278  memcpy(r.data, (*this)[i] + j_begin, row_size*sizeof(double));
5279  }
5280 
5281  return r;
5282 }
5283 
5320 {
5321  unsigned int min_size = std::min(rowNum, colNum);
5322  vpColVector diag;
5323 
5324  if (min_size > 0) {
5325  diag.resize(min_size, false);
5326 
5327  for (unsigned int i = 0; i < min_size; i++) {
5328  diag[i] = (*this)[i][i];
5329  }
5330  }
5331 
5332  return diag;
5333 }
5334 
5346 {
5347  vpMatrix C;
5348 
5349  vpMatrix::stack(A, B, C);
5350 
5351  return C;
5352 }
5353 
5365 void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
5366 {
5367  unsigned int nra = A.getRows();
5368  unsigned int nrb = B.getRows();
5369 
5370  if (nra != 0) {
5371  if (A.getCols() != B.getCols()) {
5372  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5373  A.getCols(), B.getRows(), B.getCols()));
5374  }
5375  }
5376 
5377  if (A.data != NULL && A.data == C.data) {
5378  std::cerr << "A and C must be two different objects!" << std::endl;
5379  return;
5380  }
5381 
5382  if (B.data != NULL && B.data == C.data) {
5383  std::cerr << "B and C must be two different objects!" << std::endl;
5384  return;
5385  }
5386 
5387  C.resize(nra + nrb, B.getCols(), false, false);
5388 
5389  if (C.data != NULL && A.data != NULL && A.size() > 0) {
5390  // Copy A in C
5391  memcpy(C.data, A.data, sizeof(double) * A.size());
5392  }
5393 
5394  if (C.data != NULL && B.data != NULL && B.size() > 0) {
5395  // Copy B in C
5396  memcpy(C.data + A.size(), B.data, sizeof(double) * B.size());
5397  }
5398 }
5399 
5410 {
5411  vpMatrix C;
5412  vpMatrix::stack(A, r, C);
5413 
5414  return C;
5415 }
5416 
5428 void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
5429 {
5430  if (A.data != NULL && A.data == C.data) {
5431  std::cerr << "A and C must be two different objects!" << std::endl;
5432  return;
5433  }
5434 
5435  C = A;
5436  C.stack(r);
5437 }
5438 
5449 {
5450  vpMatrix C;
5451  vpMatrix::stack(A, c, C);
5452 
5453  return C;
5454 }
5455 
5467 void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C)
5468 {
5469  if (A.data != NULL && A.data == C.data) {
5470  std::cerr << "A and C must be two different objects!" << std::endl;
5471  return;
5472  }
5473 
5474  C = A;
5475  C.stack(c);
5476 }
5477 
5490 vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, unsigned int r, unsigned int c)
5491 {
5492  vpMatrix C;
5493 
5494  insert(A, B, C, r, c);
5495 
5496  return C;
5497 }
5498 
5512 void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, unsigned int r, unsigned int c)
5513 {
5514  if (((r + B.getRows()) <= A.getRows()) && ((c + B.getCols()) <= A.getCols())) {
5515  C.resize(A.getRows(), A.getCols(), false, false);
5516 
5517  for (unsigned int i = 0; i < A.getRows(); i++) {
5518  for (unsigned int j = 0; j < A.getCols(); j++) {
5519  if (i >= r && i < (r + B.getRows()) && j >= c && j < (c + B.getCols())) {
5520  C[i][j] = B[i - r][j - c];
5521  } else {
5522  C[i][j] = A[i][j];
5523  }
5524  }
5525  }
5526  } else {
5527  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
5528  B.getRows(), B.getCols(), A.getCols(), A.getRows(), r, c);
5529  }
5530 }
5531 
5544 {
5545  vpMatrix C;
5546 
5547  juxtaposeMatrices(A, B, C);
5548 
5549  return C;
5550 }
5551 
5565 {
5566  unsigned int nca = A.getCols();
5567  unsigned int ncb = B.getCols();
5568 
5569  if (nca != 0) {
5570  if (A.getRows() != B.getRows()) {
5571  throw(vpException(vpException::dimensionError, "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5572  A.getCols(), B.getRows(), B.getCols()));
5573  }
5574  }
5575 
5576  if (B.getRows() == 0 || nca + ncb == 0) {
5577  std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl;
5578  return;
5579  }
5580 
5581  C.resize(B.getRows(), nca + ncb, false, false);
5582 
5583  C.insert(A, 0, 0);
5584  C.insert(B, 0, nca);
5585 }
5586 
5587 //--------------------------------------------------------------------
5588 // Output
5589 //--------------------------------------------------------------------
5590 
5610 int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &intro) const
5611 {
5612  typedef std::string::size_type size_type;
5613 
5614  unsigned int m = getRows();
5615  unsigned int n = getCols();
5616 
5617  std::vector<std::string> values(m * n);
5618  std::ostringstream oss;
5619  std::ostringstream ossFixed;
5620  std::ios_base::fmtflags original_flags = oss.flags();
5621 
5622  // ossFixed <<std::fixed;
5623  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
5624 
5625  size_type maxBefore = 0; // the length of the integral part
5626  size_type maxAfter = 0; // number of decimals plus
5627  // one place for the decimal point
5628  for (unsigned int i = 0; i < m; ++i) {
5629  for (unsigned int j = 0; j < n; ++j) {
5630  oss.str("");
5631  oss << (*this)[i][j];
5632  if (oss.str().find("e") != std::string::npos) {
5633  ossFixed.str("");
5634  ossFixed << (*this)[i][j];
5635  oss.str(ossFixed.str());
5636  }
5637 
5638  values[i * n + j] = oss.str();
5639  size_type thislen = values[i * n + j].size();
5640  size_type p = values[i * n + j].find('.');
5641 
5642  if (p == std::string::npos) {
5643  maxBefore = vpMath::maximum(maxBefore, thislen);
5644  // maxAfter remains the same
5645  } else {
5646  maxBefore = vpMath::maximum(maxBefore, p);
5647  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
5648  }
5649  }
5650  }
5651 
5652  size_type totalLength = length;
5653  // increase totalLength according to maxBefore
5654  totalLength = vpMath::maximum(totalLength, maxBefore);
5655  // decrease maxAfter according to totalLength
5656  maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
5657  if (maxAfter == 1)
5658  maxAfter = 0;
5659 
5660  // the following line is useful for debugging
5661  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
5662 
5663  if (! intro.empty())
5664  s << intro;
5665  s << "[" << m << "," << n << "]=\n";
5666 
5667  for (unsigned int i = 0; i < m; i++) {
5668  s << " ";
5669  for (unsigned int j = 0; j < n; j++) {
5670  size_type p = values[i * n + j].find('.');
5671  s.setf(std::ios::right, std::ios::adjustfield);
5672  s.width((std::streamsize)maxBefore);
5673  s << values[i * n + j].substr(0, p).c_str();
5674 
5675  if (maxAfter > 0) {
5676  s.setf(std::ios::left, std::ios::adjustfield);
5677  if (p != std::string::npos) {
5678  s.width((std::streamsize)maxAfter);
5679  s << values[i * n + j].substr(p, maxAfter).c_str();
5680  } else {
5681  assert(maxAfter > 1);
5682  s.width((std::streamsize)maxAfter);
5683  s << ".0";
5684  }
5685  }
5686 
5687  s << ' ';
5688  }
5689  s << std::endl;
5690  }
5691 
5692  s.flags(original_flags); // restore s to standard state
5693 
5694  return (int)(maxBefore + maxAfter);
5695 }
5696 
5733 std::ostream &vpMatrix::matlabPrint(std::ostream &os) const
5734 {
5735  os << "[ ";
5736  for (unsigned int i = 0; i < this->getRows(); ++i) {
5737  for (unsigned int j = 0; j < this->getCols(); ++j) {
5738  os << (*this)[i][j] << ", ";
5739  }
5740  if (this->getRows() != i + 1) {
5741  os << ";" << std::endl;
5742  } else {
5743  os << "]" << std::endl;
5744  }
5745  }
5746  return os;
5747 }
5748 
5777 std::ostream &vpMatrix::maplePrint(std::ostream &os) const
5778 {
5779  os << "([ " << std::endl;
5780  for (unsigned int i = 0; i < this->getRows(); ++i) {
5781  os << "[";
5782  for (unsigned int j = 0; j < this->getCols(); ++j) {
5783  os << (*this)[i][j] << ", ";
5784  }
5785  os << "]," << std::endl;
5786  }
5787  os << "])" << std::endl;
5788  return os;
5789 }
5790 
5818 std::ostream &vpMatrix::csvPrint(std::ostream &os) const
5819 {
5820  for (unsigned int i = 0; i < this->getRows(); ++i) {
5821  for (unsigned int j = 0; j < this->getCols(); ++j) {
5822  os << (*this)[i][j];
5823  if (!(j == (this->getCols() - 1)))
5824  os << ", ";
5825  }
5826  os << std::endl;
5827  }
5828  return os;
5829 }
5830 
5867 std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
5868 {
5869  os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " << this->getCols() << "); " << std::endl;
5870 
5871  for (unsigned int i = 0; i < this->getRows(); ++i) {
5872  for (unsigned int j = 0; j < this->getCols(); ++j) {
5873  if (!octet) {
5874  os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl;
5875  } else {
5876  for (unsigned int k = 0; k < sizeof(double); ++k) {
5877  os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex
5878  << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl;
5879  }
5880  }
5881  }
5882  os << std::endl;
5883  }
5884  return os;
5885 }
5886 
5892 {
5893  if (rowNum == 0) {
5894  *this = A;
5895  } else if (A.getRows() > 0) {
5896  if (colNum != A.getCols()) {
5897  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum,
5898  A.getRows(), A.getCols()));
5899  }
5900 
5901  unsigned int rowNumOld = rowNum;
5902  resize(rowNum + A.getRows(), colNum, false, false);
5903  insert(A, rowNumOld, 0);
5904  }
5905 }
5906 
5923 {
5924  if (rowNum == 0) {
5925  *this = r;
5926  } else {
5927  if (colNum != r.getCols()) {
5928  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum,
5929  colNum, r.getCols()));
5930  }
5931 
5932  if (r.size() == 0) {
5933  return;
5934  }
5935 
5936  unsigned int oldSize = size();
5937  resize(rowNum + 1, colNum, false, false);
5938 
5939  if (data != NULL && r.data != NULL && data != r.data) {
5940  // Copy r in data
5941  memcpy(data + oldSize, r.data, sizeof(double) * r.size());
5942  }
5943  }
5944 }
5945 
5963 {
5964  if (colNum == 0) {
5965  *this = c;
5966  } else {
5967  if (rowNum != c.getRows()) {
5968  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum,
5969  colNum, c.getRows()));
5970  }
5971 
5972  if (c.size() == 0) {
5973  return;
5974  }
5975 
5976  vpMatrix tmp = *this;
5977  unsigned int oldColNum = colNum;
5978  resize(rowNum, colNum + 1, false, false);
5979 
5980  if (data != NULL && tmp.data != NULL && data != tmp.data) {
5981  // Copy c in data
5982  for (unsigned int i = 0; i < rowNum; i++) {
5983  memcpy(data + i*colNum, tmp.data + i*oldColNum, sizeof(double) * oldColNum);
5984  rowPtrs[i][oldColNum] = c[i];
5985  }
5986  }
5987  }
5988 }
5989 
6000 void vpMatrix::insert(const vpMatrix &A, unsigned int r, unsigned int c)
6001 {
6002  if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) {
6003  if (A.colNum == colNum && data != NULL && A.data != NULL && A.data != data) {
6004  memcpy(data + r * colNum, A.data, sizeof(double) * A.size());
6005  } else if (data != NULL && A.data != NULL && A.data != data) {
6006  for (unsigned int i = r; i < (r + A.getRows()); i++) {
6007  memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum);
6008  }
6009  }
6010  } else {
6011  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
6012  A.getRows(), A.getCols(), rowNum, colNum, r, c);
6013  }
6014 }
6015 
6053 {
6054  vpColVector evalue(rowNum); // Eigen values
6055 
6056  if (rowNum != colNum) {
6058  "Cannot compute eigen values on a non square matrix (%dx%d)",
6059  rowNum, colNum));
6060  }
6061 
6062  // Check if the matrix is symetric: At - A = 0
6063  vpMatrix At_A = (*this).t() - (*this);
6064  for (unsigned int i = 0; i < rowNum; i++) {
6065  for (unsigned int j = 0; j < rowNum; j++) {
6066  // if (At_A[i][j] != 0) {
6067  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
6068  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symetric matrix"));
6069  }
6070  }
6071  }
6072 
6073 #if defined(VISP_HAVE_LAPACK)
6074 #if defined(VISP_HAVE_GSL) /* be careful of the copy below */
6075  {
6076  gsl_vector *eval = gsl_vector_alloc(rowNum);
6077  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
6078 
6079  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
6080  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
6081 
6082  unsigned int Atda = (unsigned int)m->tda;
6083  for (unsigned int i = 0; i < rowNum; i++) {
6084  unsigned int k = i * Atda;
6085  for (unsigned int j = 0; j < colNum; j++)
6086  m->data[k + j] = (*this)[i][j];
6087  }
6088  gsl_eigen_symmv(m, eval, evec, w);
6089 
6090  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
6091 
6092  for (unsigned int i = 0; i < rowNum; i++) {
6093  evalue[i] = gsl_vector_get(eval, i);
6094  }
6095 
6096  gsl_eigen_symmv_free(w);
6097  gsl_vector_free(eval);
6098  gsl_matrix_free(m);
6099  gsl_matrix_free(evec);
6100  }
6101 #else
6102  {
6103  const char jobz = 'N';
6104  const char uplo = 'U';
6105  vpMatrix A = (*this);
6106  vpColVector WORK;
6107  int lwork = -1;
6108  int info;
6109  double wkopt;
6110  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
6111  lwork = static_cast<int>(wkopt);
6112  WORK.resize(lwork);
6113  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
6114  }
6115 #endif
6116 #else
6117  {
6119  "Eigen values computation is not implemented. "
6120  "You should install Lapack 3rd party"));
6121  }
6122 #endif
6123  return evalue;
6124 }
6125 
6176 void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const
6177 {
6178  if (rowNum != colNum) {
6180  "Cannot compute eigen values on a non square matrix (%dx%d)",
6181  rowNum, colNum));
6182  }
6183 
6184  // Check if the matrix is symetric: At - A = 0
6185  vpMatrix At_A = (*this).t() - (*this);
6186  for (unsigned int i = 0; i < rowNum; i++) {
6187  for (unsigned int j = 0; j < rowNum; j++) {
6188  // if (At_A[i][j] != 0) {
6189  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
6190  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symetric matrix"));
6191  }
6192  }
6193  }
6194 
6195  // Resize the output matrices
6196  evalue.resize(rowNum);
6197  evector.resize(rowNum, colNum);
6198 
6199 #if defined(VISP_HAVE_LAPACK)
6200 #if defined(VISP_HAVE_GSL) /* be careful of the copy below */
6201  {
6202  gsl_vector *eval = gsl_vector_alloc(rowNum);
6203  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
6204 
6205  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
6206  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
6207 
6208  unsigned int Atda = (unsigned int)m->tda;
6209  for (unsigned int i = 0; i < rowNum; i++) {
6210  unsigned int k = i * Atda;
6211  for (unsigned int j = 0; j < colNum; j++)
6212  m->data[k + j] = (*this)[i][j];
6213  }
6214  gsl_eigen_symmv(m, eval, evec, w);
6215 
6216  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
6217 
6218  for (unsigned int i = 0; i < rowNum; i++) {
6219  evalue[i] = gsl_vector_get(eval, i);
6220  }
6221  Atda = (unsigned int)evec->tda;
6222  for (unsigned int i = 0; i < rowNum; i++) {
6223  unsigned int k = i * Atda;
6224  for (unsigned int j = 0; j < rowNum; j++) {
6225  evector[i][j] = evec->data[k + j];
6226  }
6227  }
6228 
6229  gsl_eigen_symmv_free(w);
6230  gsl_vector_free(eval);
6231  gsl_matrix_free(m);
6232  gsl_matrix_free(evec);
6233  }
6234 #else // defined(VISP_HAVE_GSL)
6235  {
6236  const char jobz = 'V';
6237  const char uplo = 'U';
6238  vpMatrix A = (*this);
6239  vpColVector WORK;
6240  int lwork = -1;
6241  int info;
6242  double wkopt;
6243  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
6244  lwork = static_cast<int>(wkopt);
6245  WORK.resize(lwork);
6246  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
6247  evector = A.t();
6248  }
6249 #endif // defined(VISP_HAVE_GSL)
6250 #else
6251  {
6253  "Eigen values computation is not implemented. "
6254  "You should install Lapack 3rd party"));
6255  }
6256 #endif
6257 }
6258 
6278 unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const
6279 {
6280  unsigned int nbline = getRows();
6281  unsigned int nbcol = getCols();
6282 
6283  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6284  vpColVector sv;
6285  sv.resize(nbcol, false); // singular values
6286  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6287 
6288  // Copy and resize matrix to have at least as many rows as columns
6289  // kernel is computed in svd method only if the matrix has more rows than
6290  // columns
6291 
6292  if (nbline < nbcol)
6293  U.resize(nbcol, nbcol, true);
6294  else
6295  U.resize(nbline, nbcol, false);
6296 
6297  U.insert(*this, 0, 0);
6298 
6299  U.svd(sv, V);
6300 
6301  // Compute the highest singular value and rank of the matrix
6302  double maxsv = 0;
6303  for (unsigned int i = 0; i < nbcol; i++) {
6304  if (sv[i] > maxsv) {
6305  maxsv = sv[i];
6306  }
6307  }
6308 
6309  unsigned int rank = 0;
6310  for (unsigned int i = 0; i < nbcol; i++) {
6311  if (sv[i] > maxsv * svThreshold) {
6312  rank++;
6313  }
6314  }
6315 
6316  kerAt.resize(nbcol - rank, nbcol);
6317  if (rank != nbcol) {
6318  for (unsigned int j = 0, k = 0; j < nbcol; j++) {
6319  // if( v.col(j) in kernel and non zero )
6320  if ((sv[j] <= maxsv * svThreshold) &&
6321  (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
6322  for (unsigned int i = 0; i < V.getRows(); i++) {
6323  kerAt[k][i] = V[i][j];
6324  }
6325  k++;
6326  }
6327  }
6328  }
6329 
6330  return rank;
6331 }
6332 
6364 double vpMatrix::det(vpDetMethod method) const
6365 {
6366  double det = 0.;
6367 
6368  if (method == LU_DECOMPOSITION) {
6369  det = this->detByLU();
6370  }
6371 
6372  return (det);
6373 }
6374 
6383 {
6384  if (colNum != rowNum) {
6385  throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix",
6386  rowNum, colNum));
6387  } else {
6388 #ifdef VISP_HAVE_GSL
6389  size_t size_ = rowNum * colNum;
6390  double *b = new double[size_];
6391  for (size_t i = 0; i < size_; i++)
6392  b[i] = 0.;
6393  gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum);
6394  gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum);
6395  gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
6396  // gsl_matrix_fprintf(stdout, &em.matrix, "%g");
6397  vpMatrix expA;
6398  expA.resize(rowNum, colNum, false);
6399  memcpy(expA.data, b, size_ * sizeof(double));
6400 
6401  delete[] b;
6402  return expA;
6403 #else
6404  vpMatrix _expE(rowNum, colNum, false);
6405  vpMatrix _expD(rowNum, colNum, false);
6406  vpMatrix _expX(rowNum, colNum, false);
6407  vpMatrix _expcX(rowNum, colNum, false);
6408  vpMatrix _eye(rowNum, colNum, false);
6409 
6410  _eye.eye();
6411  vpMatrix exp(*this);
6412 
6413  // double f;
6414  int e;
6415  double c = 0.5;
6416  int q = 6;
6417  int p = 1;
6418 
6419  double nA = 0;
6420  for (unsigned int i = 0; i < rowNum; i++) {
6421  double sum = 0;
6422  for (unsigned int j = 0; j < colNum; j++) {
6423  sum += fabs((*this)[i][j]);
6424  }
6425  if (sum > nA || i == 0) {
6426  nA = sum;
6427  }
6428  }
6429 
6430  /* f = */ frexp(nA, &e);
6431  // double s = (0 > e+1)?0:e+1;
6432  double s = e + 1;
6433 
6434  double sca = 1.0 / pow(2.0, s);
6435  exp = sca * exp;
6436  _expX = *this;
6437  _expE = c * exp + _eye;
6438  _expD = -c * exp + _eye;
6439  for (int k = 2; k <= q; k++) {
6440  c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1)));
6441  _expcX = exp * _expX;
6442  _expX = _expcX;
6443  _expcX = c * _expX;
6444  _expE = _expE + _expcX;
6445  if (p)
6446  _expD = _expD + _expcX;
6447  else
6448  _expD = _expD - _expcX;
6449  p = !p;
6450  }
6451  _expX = _expD.inverseByLU();
6452  exp = _expX * _expE;
6453  for (int k = 1; k <= s; k++) {
6454  _expE = exp * exp;
6455  exp = _expE;
6456  }
6457  return exp;
6458 #endif
6459  }
6460 }
6461 
6462 /**************************************************************************************************************/
6463 /**************************************************************************************************************/
6464 
6465 // Specific functions
6466 
6467 /*
6468 input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.
6469 
6470 output:: the complement matrix of the element (rowNo,colNo).
6471 This is the matrix obtained from M after elimenating the row rowNo and column
6472 colNo
6473 
6474 example:
6475 1 2 3
6476 M = 4 5 6
6477 7 8 9
6478 1 3
6479 subblock(M, 1, 1) give the matrix 7 9
6480 */
6481 vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row)
6482 {
6483  vpMatrix M_comp;
6484  M_comp.resize(M.getRows() - 1, M.getCols() - 1, false);
6485 
6486  for (unsigned int i = 0; i < col; i++) {
6487  for (unsigned int j = 0; j < row; j++)
6488  M_comp[i][j] = M[i][j];
6489  for (unsigned int j = row + 1; j < M.getRows(); j++)
6490  M_comp[i][j - 1] = M[i][j];
6491  }
6492  for (unsigned int i = col + 1; i < M.getCols(); i++) {
6493  for (unsigned int j = 0; j < row; j++)
6494  M_comp[i - 1][j] = M[i][j];
6495  for (unsigned int j = row + 1; j < M.getRows(); j++)
6496  M_comp[i - 1][j - 1] = M[i][j];
6497  }
6498  return M_comp;
6499 }
6500 
6510 double vpMatrix::cond(double svThreshold) const
6511 {
6512  unsigned int nbline = getRows();
6513  unsigned int nbcol = getCols();
6514 
6515  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6516  vpColVector sv;
6517  sv.resize(nbcol); // singular values
6518  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6519 
6520  // Copy and resize matrix to have at least as many rows as columns
6521  // kernel is computed in svd method only if the matrix has more rows than
6522  // columns
6523 
6524  if (nbline < nbcol)
6525  U.resize(nbcol, nbcol, true);
6526  else
6527  U.resize(nbline, nbcol, false);
6528 
6529  U.insert(*this, 0, 0);
6530 
6531  U.svd(sv, V);
6532 
6533  // Compute the highest singular value
6534  double maxsv = 0;
6535  for (unsigned int i = 0; i < nbcol; i++) {
6536  if (sv[i] > maxsv) {
6537  maxsv = sv[i];
6538  }
6539  }
6540 
6541  // Compute the rank of the matrix
6542  unsigned int rank = 0;
6543  for (unsigned int i = 0; i < nbcol; i++) {
6544  if (sv[i] > maxsv * svThreshold) {
6545  rank++;
6546  }
6547  }
6548 
6549  // Compute the lowest singular value
6550  double minsv = maxsv;
6551  for (unsigned int i = 0; i < rank; i++) {
6552  if (sv[i] < minsv) {
6553  minsv = sv[i];
6554  }
6555  }
6556 
6557  if (std::fabs(minsv) > std::numeric_limits<double>::epsilon()) {
6558  return maxsv / minsv;
6559  }
6560  else {
6561  return std::numeric_limits<double>::infinity();
6562  }
6563 }
6564 
6571 void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
6572 {
6573  if (H.getCols() != H.getRows()) {
6574  throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(),
6575  H.getCols()));
6576  }
6577 
6578  HLM = H;
6579  for (unsigned int i = 0; i < H.getCols(); i++) {
6580  HLM[i][i] += alpha * H[i][i];
6581  }
6582 }
6583 
6593 vp_deprecated double vpMatrix::euclideanNorm() const
6594 {
6595  return frobeniusNorm();
6596 }
6597 
6606 {
6607  double norm = 0.0;
6608  for (unsigned int i = 0; i < dsize; i++) {
6609  double x = *(data + i);
6610  norm += x * x;
6611  }
6612 
6613  return sqrt(norm);
6614 }
6615 
6625 {
6626  if(this->dsize != 0){
6627  vpMatrix v;
6628  vpColVector w;
6629 
6630  vpMatrix M = *this;
6631 
6632  M.svd(w, v);
6633 
6634  double max = w[0];
6635  unsigned int maxRank = std::min(this->getCols(), this->getRows());
6636  // The maximum reachable rank is either the number of columns or the number of rows
6637  // of the matrix.
6638  unsigned int boundary = std::min(maxRank, w.size());
6639  // boundary is here to ensure that the number of singular values used for the com-
6640  // putation of the euclidean norm of the matrix is not greater than the maximum
6641  // reachable rank. Indeed, some svd library pad the singular values vector with 0s
6642  // if the input matrix is non-square.
6643  for (unsigned int i = 0; i < boundary; i++) {
6644  if (max < w[i]) {
6645  max = w[i];
6646  }
6647  }
6648  return max;
6649  }
6650  else {
6651  return 0.;
6652  }
6653 }
6654 
6666 {
6667  double norm = 0.0;
6668  for (unsigned int i = 0; i < rowNum; i++) {
6669  double x = 0;
6670  for (unsigned int j = 0; j < colNum; j++) {
6671  x += fabs(*(*(rowPtrs + i) + j));
6672  }
6673  if (x > norm) {
6674  norm = x;
6675  }
6676  }
6677  return norm;
6678 }
6679 
6686 double vpMatrix::sumSquare() const
6687 {
6688  double sum_square = 0.0;
6689  double x;
6690 
6691  for (unsigned int i = 0; i < rowNum; i++) {
6692  for (unsigned int j = 0; j < colNum; j++) {
6693  x = rowPtrs[i][j];
6694  sum_square += x * x;
6695  }
6696  }
6697 
6698  return sum_square;
6699 }
6700 
6712 vpMatrix vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode)
6713 {
6714  vpMatrix res;
6715  conv2(M, kernel, res, mode);
6716  return res;
6717 }
6718 
6731 void vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, vpMatrix &res, const std::string &mode)
6732 {
6733  if (M.getRows()*M.getCols() == 0 || kernel.getRows()*kernel.getCols() == 0)
6734  return;
6735 
6736  if (mode == "valid") {
6737  if (kernel.getRows() > M.getRows() || kernel.getCols() > M.getCols())
6738  return;
6739  }
6740 
6741  vpMatrix M_padded, res_same;
6742 
6743  if (mode == "full" || mode == "same") {
6744  const unsigned int pad_x = kernel.getCols()-1;
6745  const unsigned int pad_y = kernel.getRows()-1;
6746  M_padded.resize(M.getRows() + 2*pad_y, M.getCols() + 2*pad_x, true, false);
6747  M_padded.insert(M, pad_y, pad_x);
6748 
6749  if (mode == "same") {
6750  res.resize(M.getRows(), M.getCols(), false, false);
6751  res_same.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
6752  } else {
6753  res.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
6754  }
6755  } else if (mode == "valid") {
6756  M_padded = M;
6757  res.resize(M.getRows()-kernel.getRows()+1, M.getCols()-kernel.getCols()+1);
6758  } else {
6759  return;
6760  }
6761 
6762  if (mode == "same") {
6763  for (unsigned int i = 0; i < res_same.getRows(); i++) {
6764  for (unsigned int j = 0; j < res_same.getCols(); j++) {
6765  for (unsigned int k = 0; k < kernel.getRows(); k++) {
6766  for (unsigned int l = 0; l < kernel.getCols(); l++) {
6767  res_same[i][j] += M_padded[i+k][j+l] * kernel[kernel.getRows()-k-1][kernel.getCols()-l-1];
6768  }
6769  }
6770  }
6771  }
6772 
6773  const unsigned int start_i = kernel.getRows()/2;
6774  const unsigned int start_j = kernel.getCols()/2;
6775  for (unsigned int i = 0; i < M.getRows(); i++) {
6776  memcpy(res.data + i*M.getCols(), res_same.data + (i+start_i)*res_same.getCols() + start_j, sizeof(double)*M.getCols());
6777  }
6778  } else {
6779  for (unsigned int i = 0; i < res.getRows(); i++) {
6780  for (unsigned int j = 0; j < res.getCols(); j++) {
6781  for (unsigned int k = 0; k < kernel.getRows(); k++) {
6782  for (unsigned int l = 0; l < kernel.getCols(); l++) {
6783  res[i][j] += M_padded[i+k][j+l] * kernel[kernel.getRows()-k-1][kernel.getCols()-l-1];
6784  }
6785  }
6786  }
6787  }
6788  }
6789 }
6790 
6791 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
6793 {
6794  return (vpMatrix)(vpColVector::stack(A, B));
6795 }
6796 
6798 {
6799  vpColVector::stack(A, B, C);
6800 }
6801 
6803 
6804 void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); }
6805 
6825 {
6826  vpRowVector c(getCols());
6827 
6828  for (unsigned int j = 0; j < getCols(); j++)
6829  c[j] = (*this)[i - 1][j];
6830  return c;
6831 }
6832 
6851 {
6852  vpColVector c(getRows());
6853 
6854  for (unsigned int i = 0; i < getRows(); i++)
6855  c[i] = (*this)[i][j - 1];
6856  return c;
6857 }
6858 
6865 void vpMatrix::setIdentity(const double &val)
6866 {
6867  for (unsigned int i = 0; i < rowNum; i++)
6868  for (unsigned int j = 0; j < colNum; j++)
6869  if (i == j)
6870  (*this)[i][j] = val;
6871  else
6872  (*this)[i][j] = 0;
6873 }
6874 
6875 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:1908
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2030
vpMatrix operator*(const double &x, const vpMatrix &B)
Definition: vpMatrix.cpp:1589
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:407
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:5543
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
vpColVector getDiag() const
Definition: vpMatrix.cpp:5319
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6364
static vpMatrix conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode="full")
Definition: vpMatrix.cpp:6712
void kron(const vpMatrix &m1, vpMatrix &out) const
Definition: vpMatrix.cpp:1817
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpMatrix operator-() const
Definition: vpMatrix.cpp:1558
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:115
std::ostream & csvPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5818
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1352
vpMatrix AtA() const
Definition: vpMatrix.cpp:629
double sum() const
Definition: vpMatrix.cpp:1565
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5891
vp_deprecated vpColVector column(unsigned int j)
Definition: vpMatrix.cpp:6850
vpMatrix inverseByLU() const
vpMatrix & operator/=(double x)
Divide all the element of the matrix by x : Aij = Aij / x.
Definition: vpMatrix.cpp:1689
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
vp_deprecated void stackMatrices(const vpMatrix &A)
Definition: vpMatrix.h:787
Implementation of a generic 2D array used as base class for matrices and vectors. ...
Definition: vpArray2D.h:131
static void sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1465
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
vpMatrix pseudoInverseLapack(double svThreshold=1e-6) const
vpMatrix & operator+=(const vpMatrix &B)
Operation A = A + B.
Definition: vpMatrix.cpp:1499
vpMatrix()
Definition: vpMatrix.h:172
vpMatrix operator/(double x) const
Cij = Aij / x (A is unchanged)
Definition: vpMatrix.cpp:1629
double infinityNorm() const
Definition: vpMatrix.cpp:6665
void svdLapack(vpColVector &w, vpMatrix &V)
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6510
vpMatrix & operator-=(const vpMatrix &B)
Operation A = A - B.
Definition: vpMatrix.cpp:1516
vpMatrix operator+(const vpMatrix &B) const
Definition: vpMatrix.cpp:1410
vpMatrix pseudoInverseEigen3(double svThreshold=1e-6) const
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
Implementation of a rotation matrix and operations on such kind of matrices.
vpMatrix & operator*=(double x)
Multiply all the element of the matrix by x : Aij = Aij * x.
Definition: vpMatrix.cpp:1675
unsigned int getCols() const
Definition: vpArray2D.h:279
std::ostream & maplePrint(std::ostream &os) const
Definition: vpMatrix.cpp:5777
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
double sumSquare() const
Definition: vpMatrix.cpp:6686
vpMatrix hadamard(const vpMatrix &m) const
Definition: vpMatrix.cpp:1767
vpMatrix t() const
Definition: vpMatrix.cpp:464
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:6865
std::ostream & matlabPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5733
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6278
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:887
vp_deprecated void init()
Definition: vpMatrix.h:782
static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C)
Definition: vpMatrix.cpp:1323
vpMatrix pseudoInverseOpenCV(double svThreshold=1e-6) const
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:5227
static void multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
Definition: vpMatrix.cpp:959
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1009
std::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
Definition: vpMatrix.cpp:5867
vpColVector eigenValues() const
Definition: vpMatrix.cpp:6052
double euclideanNorm() const
Definition: vpMatrix.cpp:6593
void svdOpenCV(vpColVector &w, vpMatrix &V)
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5610
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5187
vpMatrix AAt() const
Definition: vpMatrix.cpp:507
vpMatrix transpose() const
Definition: vpMatrix.cpp:474
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
double inducedL2Norm() const
Definition: vpMatrix.cpp:6624
vpMatrix operator*(const vpMatrix &B) const
Definition: vpMatrix.cpp:1168
vp_deprecated vpRowVector row(unsigned int i)
Definition: vpMatrix.cpp:6824
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
vpRowVector stackRows()
Definition: vpMatrix.cpp:1754
double sumSquare() const
static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
Definition: vpMatrix.cpp:906
vpColVector stackColumns()
Definition: vpMatrix.cpp:1732
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:141
double detByLU() const
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:6000
static void negateMatrix(const vpMatrix &A, vpMatrix &C)
Definition: vpMatrix.cpp:1540
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6571
vpMatrix & operator,(double val)
Definition: vpMatrix.cpp:805
double frobeniusNorm() const
Definition: vpMatrix.cpp:6605
Function not implemented.
Definition: vpException.h:90
void resize(unsigned int i, bool flagNullify=true)
Definition: vpRowVector.h:271
Class that consider the case of a translation vector.
void eye()
Definition: vpMatrix.cpp:449
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:139
void svdEigen3(vpColVector &w, vpMatrix &V)
vpMatrix expm() const
Definition: vpMatrix.cpp:6382
vpMatrix & operator=(const vpArray2D< double > &A)
Definition: vpMatrix.cpp:654
vpMatrix & operator<<(double *)
Definition: vpMatrix.cpp:788