Visual Servoing Platform  version 3.5.1 under development (2023-03-14)
vpMatrix.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2022 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  *****************************************************************************/
40 #include <algorithm>
41 #include <assert.h>
42 #include <cmath> // std::fabs
43 #include <fstream>
44 #include <limits> // numeric_limits
45 #include <sstream>
46 #include <stdio.h>
47 #include <stdlib.h>
48 #include <string.h>
49 #include <string>
50 #include <vector>
51 
52 #include <visp3/core/vpCPUFeatures.h>
53 #include <visp3/core/vpColVector.h>
54 #include <visp3/core/vpConfig.h>
55 #include <visp3/core/vpDebug.h>
56 #include <visp3/core/vpException.h>
57 #include <visp3/core/vpMath.h>
58 #include <visp3/core/vpMatrix.h>
59 #include <visp3/core/vpTranslationVector.h>
60 
61 #include <Simd/SimdLib.hpp>
62 
63 #ifdef VISP_HAVE_LAPACK
64 #ifdef VISP_HAVE_GSL
65 #include <gsl/gsl_eigen.h>
66 #include <gsl/gsl_linalg.h>
67 #include <gsl/gsl_math.h>
68 #elif defined(VISP_HAVE_MKL)
69 #include <mkl.h>
70 typedef MKL_INT integer;
71 
72 void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_, double *w_data,
73  double *work_data, int lwork_, int &info_)
74 {
75  MKL_INT n = static_cast<MKL_INT>(n_);
76  MKL_INT lda = static_cast<MKL_INT>(lda_);
77  MKL_INT lwork = static_cast<MKL_INT>(lwork_);
78  MKL_INT info = static_cast<MKL_INT>(info_);
79 
80  dsyev(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
81 }
82 
83 #else
84 #if defined(VISP_HAVE_LAPACK_BUILT_IN)
85 typedef long int integer;
86 #else
87 typedef int integer;
88 #endif
89 extern "C" integer dsyev_(char *jobz, char *uplo, integer *n, double *a, integer *lda, double *w, double *WORK,
90  integer *lwork, integer *info);
91 
92 void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_, double *w_data,
93  double *work_data, int lwork_, int &info_)
94 {
95  integer n = static_cast<integer>(n_);
96  integer lda = static_cast<integer>(lda_);
97  integer lwork = static_cast<integer>(lwork_);
98  integer info = static_cast<integer>(info_);
99 
100  dsyev_(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
101 
102  lwork_ = static_cast<int>(lwork);
103  info_ = static_cast<int>(info);
104 }
105 #endif
106 #endif
107 
108 #if !defined(VISP_USE_MSVC) || (defined(VISP_USE_MSVC) && !defined(VISP_BUILD_SHARED_LIBS))
109 const unsigned int vpMatrix::m_lapack_min_size_default = 0;
110 unsigned int vpMatrix::m_lapack_min_size = vpMatrix::m_lapack_min_size_default;
111 #endif
112 
113 // Prototypes of specific functions
114 vpMatrix subblock(const vpMatrix &, unsigned int, unsigned int);
115 
116 void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows,
117  unsigned int ncols, double svThreshold, vpMatrix &Ap, int &rank_out, int *rank_in,
118  vpMatrix *imA, vpMatrix *imAt, vpMatrix *kerAt)
119 {
120  Ap.resize(ncols, nrows, true, false);
121 
122  // compute the highest singular value and the rank of h
123  double maxsv = sv[0];
124 
125  rank_out = 0;
126 
127  for (unsigned int i = 0; i < ncols; i++) {
128  if (sv[i] > maxsv * svThreshold) {
129  rank_out++;
130  }
131  }
132 
133  unsigned int rank = static_cast<unsigned int>(rank_out);
134  if (rank_in) {
135  rank = static_cast<unsigned int>(*rank_in);
136  }
137 
138  for (unsigned int i = 0; i < ncols; i++) {
139  for (unsigned int j = 0; j < nrows; j++) {
140  for (unsigned int k = 0; k < rank; k++) {
141  Ap[i][j] += V[i][k] * U[j][k] / sv[k];
142  }
143  }
144  }
145 
146  // Compute im(A)
147  if (imA) {
148  imA->resize(nrows, rank);
149 
150  for (unsigned int i = 0; i < nrows; i++) {
151  for (unsigned int j = 0; j < rank; j++) {
152  (*imA)[i][j] = U[i][j];
153  }
154  }
155  }
156 
157  // Compute im(At)
158  if (imAt) {
159  imAt->resize(ncols, rank);
160  for (unsigned int i = 0; i < ncols; i++) {
161  for (unsigned int j = 0; j < rank; j++) {
162  (*imAt)[i][j] = V[i][j];
163  }
164  }
165  }
166 
167  // Compute ker(At)
168  if (kerAt) {
169  kerAt->resize(ncols - rank, ncols);
170  if (rank != ncols) {
171  for (unsigned int k = 0; k < (ncols - rank); k++) {
172  unsigned j = k + rank;
173  for (unsigned int i = 0; i < V.getRows(); i++) {
174  (*kerAt)[k][i] = V[i][j];
175  }
176  }
177  }
178  }
179 }
180 
186 vpMatrix::vpMatrix(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
187  : vpArray2D<double>()
188 {
189  if (((r + nrows) > M.rowNum) || ((c + ncols) > M.colNum)) {
191  "Cannot construct a sub matrix (%dx%d) starting at "
192  "position (%d,%d) that is not contained in the "
193  "original matrix (%dx%d)",
194  nrows, ncols, r, c, M.rowNum, M.colNum));
195  }
196 
197  init(M, r, c, nrows, ncols);
198 }
199 
200 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
202 {
203  rowNum = A.rowNum;
204  colNum = A.colNum;
205  rowPtrs = A.rowPtrs;
206  dsize = A.dsize;
207  data = A.data;
208 
209  A.rowNum = 0;
210  A.colNum = 0;
211  A.rowPtrs = NULL;
212  A.dsize = 0;
213  A.data = NULL;
214 }
215 
241 vpMatrix::vpMatrix(const std::initializer_list<double> &list) : vpArray2D<double>(list) {}
242 
267 vpMatrix::vpMatrix(unsigned int nrows, unsigned int ncols, const std::initializer_list<double> &list)
268  : vpArray2D<double>(nrows, ncols, list)
269 {
270 }
271 
294 vpMatrix::vpMatrix(const std::initializer_list<std::initializer_list<double> > &lists) : vpArray2D<double>(lists) {}
295 
296 #endif
297 
344 void vpMatrix::init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
345 {
346  unsigned int rnrows = r + nrows;
347  unsigned int cncols = c + ncols;
348 
349  if (rnrows > M.getRows())
350  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
351  M.getRows()));
352  if (cncols > M.getCols())
353  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
354  M.getCols()));
355  resize(nrows, ncols, false, false);
356 
357  if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
358  return; // Noting to do
359  for (unsigned int i = 0; i < nrows; i++) {
360  memcpy((*this)[i], &M[i + r][c], ncols * sizeof(double));
361  }
362 }
363 
405 vpMatrix vpMatrix::extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
406 {
407  unsigned int rnrows = r + nrows;
408  unsigned int cncols = c + ncols;
409 
410  if (rnrows > getRows())
411  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
412  getRows()));
413  if (cncols > getCols())
414  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
415  getCols()));
416 
417  vpMatrix M;
418  M.resize(nrows, ncols, false, false);
419  for (unsigned int i = 0; i < nrows; i++) {
420  memcpy(M[i], &(*this)[i + r][c], ncols * sizeof(double));
421  }
422 
423  return M;
424 }
425 
430 void vpMatrix::eye(unsigned int n) { eye(n, n); }
431 
436 void vpMatrix::eye(unsigned int m, unsigned int n)
437 {
438  resize(m, n);
439 
440  eye();
441 }
442 
448 {
449  for (unsigned int i = 0; i < rowNum; i++) {
450  for (unsigned int j = 0; j < colNum; j++) {
451  if (i == j)
452  (*this)[i][j] = 1.0;
453  else
454  (*this)[i][j] = 0;
455  }
456  }
457 }
458 
462 vpMatrix vpMatrix::t() const { return transpose(); }
463 
470 {
471  vpMatrix At;
472  transpose(At);
473  return At;
474 }
475 
482 {
483  At.resize(colNum, rowNum, false, false);
484 
485  if (rowNum <= 16 || colNum <= 16) {
486  for (unsigned int i = 0; i < rowNum; i++) {
487  for (unsigned int j = 0; j < colNum; j++) {
488  At[j][i] = (*this)[i][j];
489  }
490  }
491  } else {
492  SimdMatTranspose(data, rowNum, colNum, At.data);
493  }
494 }
495 
502 {
503  vpMatrix B;
504 
505  AAt(B);
506 
507  return B;
508 }
509 
521 void vpMatrix::AAt(vpMatrix &B) const
522 {
523  if ((B.rowNum != rowNum) || (B.colNum != rowNum))
524  B.resize(rowNum, rowNum, false, false);
525 
526  // If available use Lapack only for large matrices
527  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
528 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
529  useLapack = false;
530 #endif
531 
532  if (useLapack) {
533 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
534  const double alpha = 1.0;
535  const double beta = 0.0;
536  const char transa = 't';
537  const char transb = 'n';
538 
539  vpMatrix::blas_dgemm(transa, transb, rowNum, rowNum, colNum, alpha, data, colNum, data, colNum, beta, B.data,
540  rowNum);
541 #endif
542  } else {
543  // compute A*A^T
544  for (unsigned int i = 0; i < rowNum; i++) {
545  for (unsigned int j = i; j < rowNum; j++) {
546  double *pi = rowPtrs[i]; // row i
547  double *pj = rowPtrs[j]; // row j
548 
549  // sum (row i .* row j)
550  double ssum = 0;
551  for (unsigned int k = 0; k < colNum; k++)
552  ssum += *(pi++) * *(pj++);
553 
554  B[i][j] = ssum; // upper triangle
555  if (i != j)
556  B[j][i] = ssum; // lower triangle
557  }
558  }
559  }
560 }
561 
573 void vpMatrix::AtA(vpMatrix &B) const
574 {
575  if ((B.rowNum != colNum) || (B.colNum != colNum))
576  B.resize(colNum, colNum, false, false);
577 
578  // If available use Lapack only for large matrices
579  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
580 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
581  useLapack = false;
582 #endif
583 
584  if (useLapack) {
585 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
586  const double alpha = 1.0;
587  const double beta = 0.0;
588  const char transa = 'n';
589  const char transb = 't';
590 
591  vpMatrix::blas_dgemm(transa, transb, colNum, colNum, rowNum, alpha, data, colNum, data, colNum, beta, B.data,
592  colNum);
593 #endif
594  } else {
595  for (unsigned int i = 0; i < colNum; i++) {
596  double *Bi = B[i];
597  for (unsigned int j = 0; j < i; j++) {
598  double *ptr = data;
599  double s = 0;
600  for (unsigned int k = 0; k < rowNum; k++) {
601  s += (*(ptr + i)) * (*(ptr + j));
602  ptr += colNum;
603  }
604  *Bi++ = s;
605  B[j][i] = s;
606  }
607  double *ptr = data;
608  double s = 0;
609  for (unsigned int k = 0; k < rowNum; k++) {
610  s += (*(ptr + i)) * (*(ptr + i));
611  ptr += colNum;
612  }
613  *Bi = s;
614  }
615  }
616 }
617 
624 {
625  vpMatrix B;
626 
627  AtA(B);
628 
629  return B;
630 }
631 
649 {
650  resize(A.getRows(), A.getCols(), false, false);
651 
652  if (data != NULL && A.data != NULL && data != A.data) {
653  memcpy(data, A.data, dsize * sizeof(double));
654  }
655 
656  return *this;
657 }
658 
659 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
661 {
662  resize(A.getRows(), A.getCols(), false, false);
663 
664  if (data != NULL && A.data != NULL && data != A.data) {
665  memcpy(data, A.data, dsize * sizeof(double));
666  }
667 
668  return *this;
669 }
670 
672 {
673  if (this != &other) {
674  free(data);
675  free(rowPtrs);
676 
677  rowNum = other.rowNum;
678  colNum = other.colNum;
679  rowPtrs = other.rowPtrs;
680  dsize = other.dsize;
681  data = other.data;
682 
683  other.rowNum = 0;
684  other.colNum = 0;
685  other.rowPtrs = NULL;
686  other.dsize = 0;
687  other.data = NULL;
688  }
689 
690  return *this;
691 }
692 
715 vpMatrix &vpMatrix::operator=(const std::initializer_list<double> &list)
716 {
717  if (dsize != static_cast<unsigned int>(list.size())) {
718  resize(1, static_cast<unsigned int>(list.size()), false, false);
719  }
720 
721  std::copy(list.begin(), list.end(), data);
722 
723  return *this;
724 }
725 
749 vpMatrix &vpMatrix::operator=(const std::initializer_list<std::initializer_list<double> > &lists)
750 {
751  unsigned int nrows = static_cast<unsigned int>(lists.size()), ncols = 0;
752  for (auto &l : lists) {
753  if (static_cast<unsigned int>(l.size()) > ncols) {
754  ncols = static_cast<unsigned int>(l.size());
755  }
756  }
757 
758  resize(nrows, ncols, false, false);
759  auto it = lists.begin();
760  for (unsigned int i = 0; i < rowNum; i++, ++it) {
761  std::copy(it->begin(), it->end(), rowPtrs[i]);
762  }
763 
764  return *this;
765 }
766 #endif
767 
770 {
771  std::fill(data, data + rowNum * colNum, x);
772  return *this;
773 }
774 
781 {
782  for (unsigned int i = 0; i < rowNum; i++) {
783  for (unsigned int j = 0; j < colNum; j++) {
784  rowPtrs[i][j] = *x++;
785  }
786  }
787  return *this;
788 }
789 
791 {
792  resize(1, 1, false, false);
793  rowPtrs[0][0] = val;
794  return *this;
795 }
796 
798 {
799  resize(1, colNum + 1, false, false);
800  rowPtrs[0][colNum - 1] = val;
801  return *this;
802 }
803 
840 {
841  unsigned int rows = A.getRows();
842  this->resize(rows, rows);
843 
844  (*this) = 0;
845  for (unsigned int i = 0; i < rows; i++)
846  (*this)[i][i] = A[i];
847 }
848 
879 void vpMatrix::diag(const double &val)
880 {
881  (*this) = 0;
882  unsigned int min_ = (rowNum < colNum) ? rowNum : colNum;
883  for (unsigned int i = 0; i < min_; i++)
884  (*this)[i][i] = val;
885 }
886 
899 {
900  unsigned int rows = A.getRows();
901  DA.resize(rows, rows, true);
902 
903  for (unsigned int i = 0; i < rows; i++)
904  DA[i][i] = A[i];
905 }
906 
912 {
913  vpTranslationVector t_out;
914 
915  if (rowNum != 3 || colNum != 3) {
916  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector",
917  rowNum, colNum, tv.getRows(), tv.getCols()));
918  }
919 
920  for (unsigned int j = 0; j < 3; j++)
921  t_out[j] = 0;
922 
923  for (unsigned int j = 0; j < 3; j++) {
924  double tj = tv[j]; // optimization em 5/12/2006
925  for (unsigned int i = 0; i < 3; i++) {
926  t_out[i] += rowPtrs[i][j] * tj;
927  }
928  }
929  return t_out;
930 }
931 
937 {
938  vpColVector v_out;
939  vpMatrix::multMatrixVector(*this, v, v_out);
940  return v_out;
941 }
942 
952 {
953  if (A.colNum != v.getRows()) {
954  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
955  A.getRows(), A.getCols(), v.getRows()));
956  }
957 
958  if (A.rowNum != w.rowNum)
959  w.resize(A.rowNum, false);
960 
961  // If available use Lapack only for large matrices
962  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size);
963 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
964  useLapack = false;
965 #endif
966 
967  if (useLapack) {
968 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
969  double alpha = 1.0;
970  double beta = 0.0;
971  char trans = 't';
972  int incr = 1;
973 
974  vpMatrix::blas_dgemv(trans, A.colNum, A.rowNum, alpha, A.data, A.colNum, v.data, incr, beta, w.data, incr);
975 #endif
976  } else {
977  w = 0.0;
978  for (unsigned int j = 0; j < A.colNum; j++) {
979  double vj = v[j]; // optimization em 5/12/2006
980  for (unsigned int i = 0; i < A.rowNum; i++) {
981  w[i] += A.rowPtrs[i][j] * vj;
982  }
983  }
984  }
985 }
986 
987 //---------------------------------
988 // Matrix operations.
989 //---------------------------------
990 
1001 {
1002  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1003  C.resize(A.rowNum, B.colNum, false, false);
1004 
1005  if (A.colNum != B.rowNum) {
1006  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (%dx%d) matrix", A.getRows(),
1007  A.getCols(), B.getRows(), B.getCols()));
1008  }
1009 
1010  // If available use Lapack only for large matrices
1011  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size ||
1012  B.colNum > vpMatrix::m_lapack_min_size);
1013 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1014  useLapack = false;
1015 #endif
1016 
1017  if (useLapack) {
1018 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1019  const double alpha = 1.0;
1020  const double beta = 0.0;
1021  const char trans = 'n';
1022  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1023  C.data, B.colNum);
1024 #endif
1025  } else {
1026  // 5/12/06 some "very" simple optimization to avoid indexation
1027  const unsigned int BcolNum = B.colNum;
1028  const unsigned int BrowNum = B.rowNum;
1029  double **BrowPtrs = B.rowPtrs;
1030  for (unsigned int i = 0; i < A.rowNum; i++) {
1031  const double *rowptri = A.rowPtrs[i];
1032  double *ci = C[i];
1033  for (unsigned int j = 0; j < BcolNum; j++) {
1034  double s = 0;
1035  for (unsigned int k = 0; k < BrowNum; k++)
1036  s += rowptri[k] * BrowPtrs[k][j];
1037  ci[j] = s;
1038  }
1039  }
1040  }
1041 }
1042 
1057 {
1058  if (A.colNum != 3 || A.rowNum != 3 || B.colNum != 3 || B.rowNum != 3) {
1060  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1061  "rotation matrix",
1062  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1063  }
1064  // 5/12/06 some "very" simple optimization to avoid indexation
1065  const unsigned int BcolNum = B.colNum;
1066  const unsigned int BrowNum = B.rowNum;
1067  double **BrowPtrs = B.rowPtrs;
1068  for (unsigned int i = 0; i < A.rowNum; i++) {
1069  const double *rowptri = A.rowPtrs[i];
1070  double *ci = C[i];
1071  for (unsigned int j = 0; j < BcolNum; j++) {
1072  double s = 0;
1073  for (unsigned int k = 0; k < BrowNum; k++)
1074  s += rowptri[k] * BrowPtrs[k][j];
1075  ci[j] = s;
1076  }
1077  }
1078 }
1079 
1094 {
1095  if (A.colNum != 4 || A.rowNum != 4 || B.colNum != 4 || B.rowNum != 4) {
1097  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1098  "rotation matrix",
1099  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1100  }
1101  // Considering perfMatrixMultiplication.cpp benchmark,
1102  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1103  // Lapack usage needs to be validated again.
1104  // If available use Lapack only for large matrices.
1105  // Using SSE2 doesn't speed up.
1106  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size ||
1107  B.colNum > vpMatrix::m_lapack_min_size);
1108 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1109  useLapack = false;
1110 #endif
1111 
1112  if (useLapack) {
1113 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1114  const double alpha = 1.0;
1115  const double beta = 0.0;
1116  const char trans = 'n';
1117  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1118  C.data, B.colNum);
1119 #endif
1120  } else {
1121  // 5/12/06 some "very" simple optimization to avoid indexation
1122  const unsigned int BcolNum = B.colNum;
1123  const unsigned int BrowNum = B.rowNum;
1124  double **BrowPtrs = B.rowPtrs;
1125  for (unsigned int i = 0; i < A.rowNum; i++) {
1126  const double *rowptri = A.rowPtrs[i];
1127  double *ci = C[i];
1128  for (unsigned int j = 0; j < BcolNum; j++) {
1129  double s = 0;
1130  for (unsigned int k = 0; k < BrowNum; k++)
1131  s += rowptri[k] * BrowPtrs[k][j];
1132  ci[j] = s;
1133  }
1134  }
1135  }
1136 }
1137 
1151 {
1152  vpMatrix::multMatrixVector(A, B, C);
1153 }
1154 
1160 {
1161  vpMatrix C;
1162 
1163  vpMatrix::mult2Matrices(*this, B, C);
1164 
1165  return C;
1166 }
1167 
1173 {
1174  if (colNum != R.getRows()) {
1175  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1176  colNum));
1177  }
1178  vpMatrix C;
1179  C.resize(rowNum, 3, false, false);
1180 
1181  unsigned int RcolNum = R.getCols();
1182  unsigned int RrowNum = R.getRows();
1183  for (unsigned int i = 0; i < rowNum; i++) {
1184  double *rowptri = rowPtrs[i];
1185  double *ci = C[i];
1186  for (unsigned int j = 0; j < RcolNum; j++) {
1187  double s = 0;
1188  for (unsigned int k = 0; k < RrowNum; k++)
1189  s += rowptri[k] * R[k][j];
1190  ci[j] = s;
1191  }
1192  }
1193 
1194  return C;
1195 }
1196 
1202 {
1203  if (colNum != M.getRows()) {
1204  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1205  colNum));
1206  }
1207  vpMatrix C;
1208  C.resize(rowNum, 4, false, false);
1209 
1210  const unsigned int McolNum = M.getCols();
1211  const unsigned int MrowNum = M.getRows();
1212  for (unsigned int i = 0; i < rowNum; i++) {
1213  const double *rowptri = rowPtrs[i];
1214  double *ci = C[i];
1215  for (unsigned int j = 0; j < McolNum; j++) {
1216  double s = 0;
1217  for (unsigned int k = 0; k < MrowNum; k++)
1218  s += rowptri[k] * M[k][j];
1219  ci[j] = s;
1220  }
1221  }
1222 
1223  return C;
1224 }
1225 
1231 {
1232  if (colNum != V.getRows()) {
1233  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix",
1234  rowNum, colNum));
1235  }
1236  vpMatrix M;
1237  M.resize(rowNum, 6, false, false);
1238 
1239  // Considering perfMatrixMultiplication.cpp benchmark,
1240  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1241  // Lapack usage needs to be validated again.
1242  // If available use Lapack only for large matrices.
1243  // Speed up obtained using SSE2.
1244  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size ||
1245  V.colNum > vpMatrix::m_lapack_min_size);
1246 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1247  useLapack = false;
1248 #endif
1249 
1250  if (useLapack) {
1251 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1252  const double alpha = 1.0;
1253  const double beta = 0.0;
1254  const char trans = 'n';
1255  vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, V.colNum, data, colNum, beta, M.data,
1256  M.colNum);
1257 #endif
1258  } else {
1259  SimdMatMulTwist(data, rowNum, V.data, M.data);
1260  }
1261 
1262  return M;
1263 }
1264 
1270 {
1271  if (colNum != V.getRows()) {
1272  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) force/torque twist matrix",
1273  rowNum, colNum));
1274  }
1275  vpMatrix M;
1276  M.resize(rowNum, 6, false, false);
1277 
1278  // Considering perfMatrixMultiplication.cpp benchmark,
1279  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1280  // Lapack usage needs to be validated again.
1281  // If available use Lapack only for large matrices.
1282  // Speed up obtained using SSE2.
1283  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size ||
1284  V.getCols() > vpMatrix::m_lapack_min_size);
1285 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1286  useLapack = false;
1287 #endif
1288 
1289  if (useLapack) {
1290 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1291  const double alpha = 1.0;
1292  const double beta = 0.0;
1293  const char trans = 'n';
1294  vpMatrix::blas_dgemm(trans, trans, V.getCols(), rowNum, colNum, alpha, V.data, V.getCols(), data, colNum, beta,
1295  M.data, M.colNum);
1296 #endif
1297  } else {
1298  SimdMatMulTwist(data, rowNum, V.data, M.data);
1299  }
1300 
1301  return M;
1302 }
1303 
1314 void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB,
1315  vpMatrix &C)
1316 {
1317  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1318  C.resize(A.rowNum, B.colNum, false, false);
1319 
1320  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1321  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1322  A.getCols(), B.getRows(), B.getCols()));
1323  }
1324 
1325  double **ArowPtrs = A.rowPtrs;
1326  double **BrowPtrs = B.rowPtrs;
1327  double **CrowPtrs = C.rowPtrs;
1328 
1329  for (unsigned int i = 0; i < A.rowNum; i++)
1330  for (unsigned int j = 0; j < A.colNum; j++)
1331  CrowPtrs[i][j] = wB * BrowPtrs[i][j] + wA * ArowPtrs[i][j];
1332 }
1333 
1343 void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1344 {
1345  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1346  C.resize(A.rowNum, B.colNum, false, false);
1347 
1348  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1349  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1350  A.getCols(), B.getRows(), B.getCols()));
1351  }
1352 
1353  double **ArowPtrs = A.rowPtrs;
1354  double **BrowPtrs = B.rowPtrs;
1355  double **CrowPtrs = C.rowPtrs;
1356 
1357  for (unsigned int i = 0; i < A.rowNum; i++) {
1358  for (unsigned int j = 0; j < A.colNum; j++) {
1359  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1360  }
1361  }
1362 }
1363 
1377 {
1378  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1379  C.resize(A.rowNum);
1380 
1381  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1382  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1383  A.getCols(), B.getRows(), B.getCols()));
1384  }
1385 
1386  double **ArowPtrs = A.rowPtrs;
1387  double **BrowPtrs = B.rowPtrs;
1388  double **CrowPtrs = C.rowPtrs;
1389 
1390  for (unsigned int i = 0; i < A.rowNum; i++) {
1391  for (unsigned int j = 0; j < A.colNum; j++) {
1392  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1393  }
1394  }
1395 }
1396 
1402 {
1403  vpMatrix C;
1404  vpMatrix::add2Matrices(*this, B, C);
1405  return C;
1406 }
1407 
1424 {
1425  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1426  C.resize(A.rowNum);
1427 
1428  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1429  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1430  A.getCols(), B.getRows(), B.getCols()));
1431  }
1432 
1433  double **ArowPtrs = A.rowPtrs;
1434  double **BrowPtrs = B.rowPtrs;
1435  double **CrowPtrs = C.rowPtrs;
1436 
1437  for (unsigned int i = 0; i < A.rowNum; i++) {
1438  for (unsigned int j = 0; j < A.colNum; j++) {
1439  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1440  }
1441  }
1442 }
1443 
1456 void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1457 {
1458  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1459  C.resize(A.rowNum, A.colNum, false, false);
1460 
1461  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1462  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1463  A.getCols(), B.getRows(), B.getCols()));
1464  }
1465 
1466  double **ArowPtrs = A.rowPtrs;
1467  double **BrowPtrs = B.rowPtrs;
1468  double **CrowPtrs = C.rowPtrs;
1469 
1470  for (unsigned int i = 0; i < A.rowNum; i++) {
1471  for (unsigned int j = 0; j < A.colNum; j++) {
1472  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1473  }
1474  }
1475 }
1476 
1482 {
1483  vpMatrix C;
1484  vpMatrix::sub2Matrices(*this, B, C);
1485  return C;
1486 }
1487 
1489 
1491 {
1492  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1493  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1494  B.getRows(), B.getCols()));
1495  }
1496 
1497  double **BrowPtrs = B.rowPtrs;
1498 
1499  for (unsigned int i = 0; i < rowNum; i++)
1500  for (unsigned int j = 0; j < colNum; j++)
1501  rowPtrs[i][j] += BrowPtrs[i][j];
1502 
1503  return *this;
1504 }
1505 
1508 {
1509  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1510  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1511  B.getRows(), B.getCols()));
1512  }
1513 
1514  double **BrowPtrs = B.rowPtrs;
1515  for (unsigned int i = 0; i < rowNum; i++)
1516  for (unsigned int j = 0; j < colNum; j++)
1517  rowPtrs[i][j] -= BrowPtrs[i][j];
1518 
1519  return *this;
1520 }
1521 
1532 {
1533  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1534  C.resize(A.rowNum, A.colNum, false, false);
1535 
1536  double **ArowPtrs = A.rowPtrs;
1537  double **CrowPtrs = C.rowPtrs;
1538 
1539  // t0=vpTime::measureTimeMicros();
1540  for (unsigned int i = 0; i < A.rowNum; i++)
1541  for (unsigned int j = 0; j < A.colNum; j++)
1542  CrowPtrs[i][j] = -ArowPtrs[i][j];
1543 }
1544 
1550 {
1551  vpMatrix C;
1552  vpMatrix::negateMatrix(*this, C);
1553  return C;
1554 }
1555 
1556 double vpMatrix::sum() const
1557 {
1558  double s = 0.0;
1559  for (unsigned int i = 0; i < rowNum; i++) {
1560  for (unsigned int j = 0; j < colNum; j++) {
1561  s += rowPtrs[i][j];
1562  }
1563  }
1564 
1565  return s;
1566 }
1567 
1568 //---------------------------------
1569 // Matrix/vector operations.
1570 //---------------------------------
1571 
1572 //---------------------------------
1573 // Matrix/real operations.
1574 //---------------------------------
1575 
1580 vpMatrix operator*(const double &x, const vpMatrix &B)
1581 {
1582  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1583  return B;
1584  }
1585 
1586  unsigned int Brow = B.getRows();
1587  unsigned int Bcol = B.getCols();
1588 
1589  vpMatrix C;
1590  C.resize(Brow, Bcol, false, false);
1591 
1592  for (unsigned int i = 0; i < Brow; i++)
1593  for (unsigned int j = 0; j < Bcol; j++)
1594  C[i][j] = B[i][j] * x;
1595 
1596  return C;
1597 }
1598 
1604 {
1605  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1606  return (*this);
1607  }
1608 
1609  vpMatrix M;
1610  M.resize(rowNum, colNum, false, false);
1611 
1612  for (unsigned int i = 0; i < rowNum; i++)
1613  for (unsigned int j = 0; j < colNum; j++)
1614  M[i][j] = rowPtrs[i][j] * x;
1615 
1616  return M;
1617 }
1618 
1621 {
1622  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1623  return (*this);
1624  }
1625 
1626  if (std::fabs(x) < std::numeric_limits<double>::epsilon()) {
1627  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1628  }
1629 
1630  vpMatrix C;
1631  C.resize(rowNum, colNum, false, false);
1632 
1633  double xinv = 1 / x;
1634 
1635  for (unsigned int i = 0; i < rowNum; i++)
1636  for (unsigned int j = 0; j < colNum; j++)
1637  C[i][j] = rowPtrs[i][j] * xinv;
1638 
1639  return C;
1640 }
1641 
1644 {
1645  for (unsigned int i = 0; i < rowNum; i++)
1646  for (unsigned int j = 0; j < colNum; j++)
1647  rowPtrs[i][j] += x;
1648 
1649  return *this;
1650 }
1651 
1654 {
1655  for (unsigned int i = 0; i < rowNum; i++)
1656  for (unsigned int j = 0; j < colNum; j++)
1657  rowPtrs[i][j] -= x;
1658 
1659  return *this;
1660 }
1661 
1667 {
1668  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1669  return *this;
1670  }
1671 
1672  for (unsigned int i = 0; i < rowNum; i++)
1673  for (unsigned int j = 0; j < colNum; j++)
1674  rowPtrs[i][j] *= x;
1675 
1676  return *this;
1677 }
1678 
1681 {
1682  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1683  return *this;
1684  }
1685 
1686  if (std::fabs(x) < std::numeric_limits<double>::epsilon())
1687  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1688 
1689  double xinv = 1 / x;
1690 
1691  for (unsigned int i = 0; i < rowNum; i++)
1692  for (unsigned int j = 0; j < colNum; j++)
1693  rowPtrs[i][j] *= xinv;
1694 
1695  return *this;
1696 }
1697 
1698 //----------------------------------------------------------------
1699 // Matrix Operation
1700 //----------------------------------------------------------------
1701 
1707 {
1708  if ((out.rowNum != colNum * rowNum) || (out.colNum != 1))
1709  out.resize(colNum * rowNum, false, false);
1710 
1711  double *optr = out.data;
1712  for (unsigned int j = 0; j < colNum; j++) {
1713  for (unsigned int i = 0; i < rowNum; i++) {
1714  *(optr++) = rowPtrs[i][j];
1715  }
1716  }
1717 }
1718 
1724 {
1725  vpColVector out(colNum * rowNum);
1726  stackColumns(out);
1727  return out;
1728 }
1729 
1735 {
1736  if ((out.getRows() != 1) || (out.getCols() != colNum * rowNum))
1737  out.resize(colNum * rowNum, false, false);
1738 
1739  memcpy(out.data, data, sizeof(double) * out.getCols());
1740 }
1746 {
1747  vpRowVector out(colNum * rowNum);
1748  stackRows(out);
1749  return out;
1750 }
1751 
1759 {
1760  if (m.getRows() != rowNum || m.getCols() != colNum) {
1761  throw(vpException(vpException::dimensionError, "In Hadamard product: bad dimension of input matrix"));
1762  }
1763 
1764  vpMatrix out;
1765  out.resize(rowNum, colNum, false, false);
1766 
1767  SimdVectorHadamard(data, m.data, dsize, out.data);
1768 
1769  return out;
1770 }
1771 
1778 void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out)
1779 {
1780  unsigned int r1 = m1.getRows();
1781  unsigned int c1 = m1.getCols();
1782  unsigned int r2 = m2.getRows();
1783  unsigned int c2 = m2.getCols();
1784 
1785  out.resize(r1 * r2, c1 * c2, false, false);
1786 
1787  for (unsigned int r = 0; r < r1; r++) {
1788  for (unsigned int c = 0; c < c1; c++) {
1789  double alpha = m1[r][c];
1790  double *m2ptr = m2[0];
1791  unsigned int roffset = r * r2;
1792  unsigned int coffset = c * c2;
1793  for (unsigned int rr = 0; rr < r2; rr++) {
1794  for (unsigned int cc = 0; cc < c2; cc++) {
1795  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1796  }
1797  }
1798  }
1799  }
1800 }
1801 
1808 void vpMatrix::kron(const vpMatrix &m, vpMatrix &out) const { kron(*this, m, out); }
1809 
1817 {
1818  unsigned int r1 = m1.getRows();
1819  unsigned int c1 = m1.getCols();
1820  unsigned int r2 = m2.getRows();
1821  unsigned int c2 = m2.getCols();
1822 
1823  vpMatrix out;
1824  out.resize(r1 * r2, c1 * c2, false, false);
1825 
1826  for (unsigned int r = 0; r < r1; r++) {
1827  for (unsigned int c = 0; c < c1; c++) {
1828  double alpha = m1[r][c];
1829  double *m2ptr = m2[0];
1830  unsigned int roffset = r * r2;
1831  unsigned int coffset = c * c2;
1832  for (unsigned int rr = 0; rr < r2; rr++) {
1833  for (unsigned int cc = 0; cc < c2; cc++) {
1834  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1835  }
1836  }
1837  }
1838  }
1839  return out;
1840 }
1841 
1847 vpMatrix vpMatrix::kron(const vpMatrix &m) const { return kron(*this, m); }
1848 
1899 void vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const { x = pseudoInverse(1e-6) * b; }
1900 
1951 {
1952  vpColVector X(colNum);
1953 
1954  solveBySVD(B, X);
1955  return X;
1956 }
1957 
2022 {
2023 #if defined(VISP_HAVE_LAPACK)
2024  svdLapack(w, V);
2025 #elif defined(VISP_HAVE_EIGEN3)
2026  svdEigen3(w, V);
2027 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2028  svdOpenCV(w, V);
2029 #else
2030  (void)w;
2031  (void)V;
2032  throw(vpException(vpException::fatalError, "Cannot compute SVD. Install Lapack, Eigen3 or OpenCV 3rd party"));
2033 #endif
2034 }
2035 
2090 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
2091 {
2092 #if defined(VISP_HAVE_LAPACK)
2093  return pseudoInverseLapack(Ap, svThreshold);
2094 #elif defined(VISP_HAVE_EIGEN3)
2095  return pseudoInverseEigen3(Ap, svThreshold);
2096 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2097  return pseudoInverseOpenCV(Ap, svThreshold);
2098 #else
2099  (void)Ap;
2100  (void)svThreshold;
2101  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2102  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2103 #endif
2104 }
2105 
2166 int vpMatrix::pseudoInverse(vpMatrix &Ap, int rank_in) const
2167 {
2168 #if defined(VISP_HAVE_LAPACK)
2169  return pseudoInverseLapack(Ap, rank_in);
2170 #elif defined(VISP_HAVE_EIGEN3)
2171  return pseudoInverseEigen3(Ap, rank_in);
2172 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2173  return pseudoInverseOpenCV(Ap, rank_in);
2174 #else
2175  (void)Ap;
2176  (void)svThreshold;
2177  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2178  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2179 #endif
2180 }
2181 
2232 vpMatrix vpMatrix::pseudoInverse(double svThreshold) const
2233 {
2234 #if defined(VISP_HAVE_LAPACK)
2235  return pseudoInverseLapack(svThreshold);
2236 #elif defined(VISP_HAVE_EIGEN3)
2237  return pseudoInverseEigen3(svThreshold);
2238 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2239  return pseudoInverseOpenCV(svThreshold);
2240 #else
2241  (void)svThreshold;
2242  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2243  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2244 #endif
2245 }
2246 
2298 {
2299 #if defined(VISP_HAVE_LAPACK)
2300  return pseudoInverseLapack(rank_in);
2301 #elif defined(VISP_HAVE_EIGEN3)
2302  return pseudoInverseEigen3(rank_in);
2303 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2304  return pseudoInverseOpenCV(rank_in);
2305 #else
2306  (void)svThreshold;
2307  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2308  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2309 #endif
2310 }
2311 
2312 #ifndef DOXYGEN_SHOULD_SKIP_THIS
2313 #if defined(VISP_HAVE_LAPACK)
2350 vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const
2351 {
2352  unsigned int nrows = getRows();
2353  unsigned int ncols = getCols();
2354  int rank_out;
2355 
2356  vpMatrix U, V, Ap;
2357  vpColVector sv;
2358 
2359  Ap.resize(ncols, nrows, false, false);
2360 
2361  if (nrows < ncols) {
2362  U.resize(ncols, ncols, true);
2363  sv.resize(nrows, false);
2364  } else {
2365  U.resize(nrows, ncols, false);
2366  sv.resize(ncols, false);
2367  }
2368 
2369  U.insert(*this, 0, 0);
2370  U.svdLapack(sv, V);
2371 
2372  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
2373 
2374  return Ap;
2375 }
2376 
2417 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) const
2418 {
2419  unsigned int nrows = getRows();
2420  unsigned int ncols = getCols();
2421  int rank_out;
2422 
2423  vpMatrix U, V;
2424  vpColVector sv;
2425 
2426  Ap.resize(ncols, nrows, false, false);
2427 
2428  if (nrows < ncols) {
2429  U.resize(ncols, ncols, true);
2430  sv.resize(nrows, false);
2431  } else {
2432  U.resize(nrows, ncols, false);
2433  sv.resize(ncols, false);
2434  }
2435 
2436  U.insert(*this, 0, 0);
2437  U.svdLapack(sv, V);
2438 
2439  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
2440 
2441  return static_cast<unsigned int>(rank_out);
2442 }
2490 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2491 {
2492  unsigned int nrows = getRows();
2493  unsigned int ncols = getCols();
2494  int rank_out;
2495 
2496  vpMatrix U, V;
2497  vpColVector sv_;
2498 
2499  Ap.resize(ncols, nrows, false, false);
2500 
2501  if (nrows < ncols) {
2502  U.resize(ncols, ncols, true);
2503  sv.resize(nrows, false);
2504  } else {
2505  U.resize(nrows, ncols, false);
2506  sv.resize(ncols, false);
2507  }
2508 
2509  U.insert(*this, 0, 0);
2510  U.svdLapack(sv_, V);
2511 
2512  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
2513 
2514  // Remove singular values equal to the one that corresponds to the lines of 0
2515  // introduced when m < n
2516  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2517 
2518  return static_cast<unsigned int>(rank_out);
2519 }
2520 
2627 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
2628  vpMatrix &imAt, vpMatrix &kerAt) const
2629 {
2630  unsigned int nrows = getRows();
2631  unsigned int ncols = getCols();
2632  int rank_out;
2633  vpMatrix U, V;
2634  vpColVector sv_;
2635 
2636  if (nrows < ncols) {
2637  U.resize(ncols, ncols, true);
2638  sv.resize(nrows, false);
2639  } else {
2640  U.resize(nrows, ncols, false);
2641  sv.resize(ncols, false);
2642  }
2643 
2644  U.insert(*this, 0, 0);
2645  U.svdLapack(sv_, V);
2646 
2647  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerAt);
2648 
2649  // Remove singular values equal to the one that corresponds to the lines of 0
2650  // introduced when m < n
2651  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2652 
2653  return static_cast<unsigned int>(rank_out);
2654 }
2655 
2692 vpMatrix vpMatrix::pseudoInverseLapack(int rank_in) const
2693 {
2694  unsigned int nrows = getRows();
2695  unsigned int ncols = getCols();
2696  int rank_out;
2697  double svThreshold = 1e-26;
2698 
2699  vpMatrix U, V, Ap;
2700  vpColVector sv;
2701 
2702  Ap.resize(ncols, nrows, false, false);
2703 
2704  if (nrows < ncols) {
2705  U.resize(ncols, ncols, true);
2706  sv.resize(nrows, false);
2707  } else {
2708  U.resize(nrows, ncols, false);
2709  sv.resize(ncols, false);
2710  }
2711 
2712  U.insert(*this, 0, 0);
2713  U.svdLapack(sv, V);
2714 
2715  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
2716 
2717  return Ap;
2718 }
2719 
2766 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, int rank_in) const
2767 {
2768  unsigned int nrows = getRows();
2769  unsigned int ncols = getCols();
2770  int rank_out;
2771  double svThreshold = 1e-26;
2772 
2773  vpMatrix U, V;
2774  vpColVector sv;
2775 
2776  Ap.resize(ncols, nrows, false, false);
2777 
2778  if (nrows < ncols) {
2779  U.resize(ncols, ncols, true);
2780  sv.resize(nrows, false);
2781  } else {
2782  U.resize(nrows, ncols, false);
2783  sv.resize(ncols, false);
2784  }
2785 
2786  U.insert(*this, 0, 0);
2787  U.svdLapack(sv, V);
2788 
2789  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
2790 
2791  return rank_out;
2792 }
2793 
2847 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in) const
2848 {
2849  unsigned int nrows = getRows();
2850  unsigned int ncols = getCols();
2851  int rank_out;
2852  double svThreshold = 1e-26;
2853 
2854  vpMatrix U, V;
2855  vpColVector sv_;
2856 
2857  Ap.resize(ncols, nrows, false, false);
2858 
2859  if (nrows < ncols) {
2860  U.resize(ncols, ncols, true);
2861  sv.resize(nrows, false);
2862  } else {
2863  U.resize(nrows, ncols, false);
2864  sv.resize(ncols, false);
2865  }
2866 
2867  U.insert(*this, 0, 0);
2868  U.svdLapack(sv_, V);
2869 
2870  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
2871 
2872  // Remove singular values equal to the one that corresponds to the lines of 0
2873  // introduced when m < n
2874  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2875 
2876  return rank_out;
2877 }
2878 
2990 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
2991  vpMatrix &kerAt) const
2992 {
2993  unsigned int nrows = getRows();
2994  unsigned int ncols = getCols();
2995  int rank_out;
2996  double svThreshold = 1e-26;
2997  vpMatrix U, V;
2998  vpColVector sv_;
2999 
3000  if (nrows < ncols) {
3001  U.resize(ncols, ncols, true);
3002  sv.resize(nrows, false);
3003  } else {
3004  U.resize(nrows, ncols, false);
3005  sv.resize(ncols, false);
3006  }
3007 
3008  U.insert(*this, 0, 0);
3009  U.svdLapack(sv_, V);
3010 
3011  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
3012 
3013  // Remove singular values equal to the one that corresponds to the lines of 0
3014  // introduced when m < n
3015  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3016 
3017  return rank_out;
3018 }
3019 
3020 #endif
3021 #if defined(VISP_HAVE_EIGEN3)
3058 vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const
3059 {
3060  unsigned int nrows = getRows();
3061  unsigned int ncols = getCols();
3062  int rank_out;
3063 
3064  vpMatrix U, V, Ap;
3065  vpColVector sv;
3066 
3067  Ap.resize(ncols, nrows, false, false);
3068 
3069  if (nrows < ncols) {
3070  U.resize(ncols, ncols, true);
3071  sv.resize(nrows, false);
3072  } else {
3073  U.resize(nrows, ncols, false);
3074  sv.resize(ncols, false);
3075  }
3076 
3077  U.insert(*this, 0, 0);
3078  U.svdEigen3(sv, V);
3079 
3080  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3081 
3082  return Ap;
3083 }
3084 
3125 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) const
3126 {
3127  unsigned int nrows = getRows();
3128  unsigned int ncols = getCols();
3129  int rank_out;
3130 
3131  vpMatrix U, V;
3132  vpColVector sv;
3133 
3134  Ap.resize(ncols, nrows, false, false);
3135 
3136  if (nrows < ncols) {
3137  U.resize(ncols, ncols, true);
3138  sv.resize(nrows, false);
3139  } else {
3140  U.resize(nrows, ncols, false);
3141  sv.resize(ncols, false);
3142  }
3143 
3144  U.insert(*this, 0, 0);
3145  U.svdEigen3(sv, V);
3146 
3147  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3148 
3149  return static_cast<unsigned int>(rank_out);
3150 }
3198 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3199 {
3200  unsigned int nrows = getRows();
3201  unsigned int ncols = getCols();
3202  int rank_out;
3203 
3204  vpMatrix U, V;
3205  vpColVector sv_;
3206 
3207  Ap.resize(ncols, nrows, false, false);
3208 
3209  if (nrows < ncols) {
3210  U.resize(ncols, ncols, true);
3211  sv.resize(nrows, false);
3212  } else {
3213  U.resize(nrows, ncols, false);
3214  sv.resize(ncols, false);
3215  }
3216 
3217  U.insert(*this, 0, 0);
3218  U.svdEigen3(sv_, V);
3219 
3220  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3221 
3222  // Remove singular values equal to the one that corresponds to the lines of 0
3223  // introduced when m < n
3224  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3225 
3226  return static_cast<unsigned int>(rank_out);
3227 }
3228 
3335 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3336  vpMatrix &imAt, vpMatrix &kerAt) const
3337 {
3338  unsigned int nrows = getRows();
3339  unsigned int ncols = getCols();
3340  int rank_out;
3341  vpMatrix U, V;
3342  vpColVector sv_;
3343 
3344  if (nrows < ncols) {
3345  U.resize(ncols, ncols, true);
3346  sv.resize(nrows, false);
3347  } else {
3348  U.resize(nrows, ncols, false);
3349  sv.resize(ncols, false);
3350  }
3351 
3352  U.insert(*this, 0, 0);
3353  U.svdEigen3(sv_, V);
3354 
3355  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerAt);
3356 
3357  // Remove singular values equal to the one that corresponds to the lines of 0
3358  // introduced when m < n
3359  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3360 
3361  return static_cast<unsigned int>(rank_out);
3362 }
3363 
3400 vpMatrix vpMatrix::pseudoInverseEigen3(int rank_in) const
3401 {
3402  unsigned int nrows = getRows();
3403  unsigned int ncols = getCols();
3404  int rank_out;
3405  double svThreshold = 1e-26;
3406 
3407  vpMatrix U, V, Ap;
3408  vpColVector sv;
3409 
3410  Ap.resize(ncols, nrows, false, false);
3411 
3412  if (nrows < ncols) {
3413  U.resize(ncols, ncols, true);
3414  sv.resize(nrows, false);
3415  } else {
3416  U.resize(nrows, ncols, false);
3417  sv.resize(ncols, false);
3418  }
3419 
3420  U.insert(*this, 0, 0);
3421  U.svdEigen3(sv, V);
3422 
3423  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3424 
3425  return Ap;
3426 }
3427 
3474 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, int rank_in) const
3475 {
3476  unsigned int nrows = getRows();
3477  unsigned int ncols = getCols();
3478  int rank_out;
3479  double svThreshold = 1e-26;
3480 
3481  vpMatrix U, V;
3482  vpColVector sv;
3483 
3484  Ap.resize(ncols, nrows, false, false);
3485 
3486  if (nrows < ncols) {
3487  U.resize(ncols, ncols, true);
3488  sv.resize(nrows, false);
3489  } else {
3490  U.resize(nrows, ncols, false);
3491  sv.resize(ncols, false);
3492  }
3493 
3494  U.insert(*this, 0, 0);
3495  U.svdEigen3(sv, V);
3496 
3497  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3498 
3499  return rank_out;
3500 }
3501 
3555 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in) const
3556 {
3557  unsigned int nrows = getRows();
3558  unsigned int ncols = getCols();
3559  int rank_out;
3560  double svThreshold = 1e-26;
3561 
3562  vpMatrix U, V;
3563  vpColVector sv_;
3564 
3565  Ap.resize(ncols, nrows, false, false);
3566 
3567  if (nrows < ncols) {
3568  U.resize(ncols, ncols, true);
3569  sv.resize(nrows, false);
3570  } else {
3571  U.resize(nrows, ncols, false);
3572  sv.resize(ncols, false);
3573  }
3574 
3575  U.insert(*this, 0, 0);
3576  U.svdEigen3(sv_, V);
3577 
3578  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3579 
3580  // Remove singular values equal to the one that corresponds to the lines of 0
3581  // introduced when m < n
3582  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3583 
3584  return rank_out;
3585 }
3586 
3698 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
3699  vpMatrix &kerAt) const
3700 {
3701  unsigned int nrows = getRows();
3702  unsigned int ncols = getCols();
3703  int rank_out;
3704  double svThreshold = 1e-26;
3705  vpMatrix U, V;
3706  vpColVector sv_;
3707 
3708  if (nrows < ncols) {
3709  U.resize(ncols, ncols, true);
3710  sv.resize(nrows, false);
3711  } else {
3712  U.resize(nrows, ncols, false);
3713  sv.resize(ncols, false);
3714  }
3715 
3716  U.insert(*this, 0, 0);
3717  U.svdEigen3(sv_, V);
3718 
3719  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
3720 
3721  // Remove singular values equal to the one that corresponds to the lines of 0
3722  // introduced when m < n
3723  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3724 
3725  return rank_out;
3726 }
3727 
3728 #endif
3729 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
3766 vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const
3767 {
3768  unsigned int nrows = getRows();
3769  unsigned int ncols = getCols();
3770  int rank_out;
3771 
3772  vpMatrix U, V, Ap;
3773  vpColVector sv;
3774 
3775  Ap.resize(ncols, nrows, false, false);
3776 
3777  if (nrows < ncols) {
3778  U.resize(ncols, ncols, true);
3779  sv.resize(nrows, false);
3780  } else {
3781  U.resize(nrows, ncols, false);
3782  sv.resize(ncols, false);
3783  }
3784 
3785  U.insert(*this, 0, 0);
3786  U.svdOpenCV(sv, V);
3787 
3788  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3789 
3790  return Ap;
3791 }
3792 
3833 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) const
3834 {
3835  unsigned int nrows = getRows();
3836  unsigned int ncols = getCols();
3837  int rank_out;
3838 
3839  vpMatrix U, V;
3840  vpColVector sv;
3841 
3842  Ap.resize(ncols, nrows, false, false);
3843 
3844  if (nrows < ncols) {
3845  U.resize(ncols, ncols, true);
3846  sv.resize(nrows, false);
3847  } else {
3848  U.resize(nrows, ncols, false);
3849  sv.resize(ncols, false);
3850  }
3851 
3852  U.insert(*this, 0, 0);
3853  U.svdOpenCV(sv, V);
3854 
3855  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3856 
3857  return static_cast<unsigned int>(rank_out);
3858 }
3906 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3907 {
3908  unsigned int nrows = getRows();
3909  unsigned int ncols = getCols();
3910  int rank_out;
3911 
3912  vpMatrix U, V;
3913  vpColVector sv_;
3914 
3915  Ap.resize(ncols, nrows, false, false);
3916 
3917  if (nrows < ncols) {
3918  U.resize(ncols, ncols, true);
3919  sv.resize(nrows, false);
3920  } else {
3921  U.resize(nrows, ncols, false);
3922  sv.resize(ncols, false);
3923  }
3924 
3925  U.insert(*this, 0, 0);
3926  U.svdOpenCV(sv_, V);
3927 
3928  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3929 
3930  // Remove singular values equal to the one that corresponds to the lines of 0
3931  // introduced when m < n
3932  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3933 
3934  return static_cast<unsigned int>(rank_out);
3935 }
3936 
4043 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
4044  vpMatrix &imAt, vpMatrix &kerAt) const
4045 {
4046  unsigned int nrows = getRows();
4047  unsigned int ncols = getCols();
4048  int rank_out;
4049  vpMatrix U, V;
4050  vpColVector sv_;
4051 
4052  if (nrows < ncols) {
4053  U.resize(ncols, ncols, true);
4054  sv.resize(nrows, false);
4055  } else {
4056  U.resize(nrows, ncols, false);
4057  sv.resize(ncols, false);
4058  }
4059 
4060  U.insert(*this, 0, 0);
4061  U.svdOpenCV(sv_, V);
4062 
4063  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerAt);
4064 
4065  // Remove singular values equal to the one that corresponds to the lines of 0
4066  // introduced when m < n
4067  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4068 
4069  return static_cast<unsigned int>(rank_out);
4070 }
4071 
4108 vpMatrix vpMatrix::pseudoInverseOpenCV(int rank_in) const
4109 {
4110  unsigned int nrows = getRows();
4111  unsigned int ncols = getCols();
4112  int rank_out;
4113  double svThreshold = 1e-26;
4114 
4115  vpMatrix U, V, Ap;
4116  vpColVector sv;
4117 
4118  Ap.resize(ncols, nrows, false, false);
4119 
4120  if (nrows < ncols) {
4121  U.resize(ncols, ncols, true);
4122  sv.resize(nrows, false);
4123  } else {
4124  U.resize(nrows, ncols, false);
4125  sv.resize(ncols, false);
4126  }
4127 
4128  U.insert(*this, 0, 0);
4129  U.svdOpenCV(sv, V);
4130 
4131  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4132 
4133  return Ap;
4134 }
4135 
4182 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, int rank_in) const
4183 {
4184  unsigned int nrows = getRows();
4185  unsigned int ncols = getCols();
4186  int rank_out;
4187  double svThreshold = 1e-26;
4188 
4189  vpMatrix U, V;
4190  vpColVector sv;
4191 
4192  Ap.resize(ncols, nrows, false, false);
4193 
4194  if (nrows < ncols) {
4195  U.resize(ncols, ncols, true);
4196  sv.resize(nrows, false);
4197  } else {
4198  U.resize(nrows, ncols, false);
4199  sv.resize(ncols, false);
4200  }
4201 
4202  U.insert(*this, 0, 0);
4203  U.svdOpenCV(sv, V);
4204 
4205  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4206 
4207  return rank_out;
4208 }
4209 
4263 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4264 {
4265  unsigned int nrows = getRows();
4266  unsigned int ncols = getCols();
4267  int rank_out;
4268  double svThreshold = 1e-26;
4269 
4270  vpMatrix U, V;
4271  vpColVector sv_;
4272 
4273  Ap.resize(ncols, nrows, false, false);
4274 
4275  if (nrows < ncols) {
4276  U.resize(ncols, ncols, true);
4277  sv.resize(nrows, false);
4278  } else {
4279  U.resize(nrows, ncols, false);
4280  sv.resize(ncols, false);
4281  }
4282 
4283  U.insert(*this, 0, 0);
4284  U.svdOpenCV(sv_, V);
4285 
4286  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4287 
4288  // Remove singular values equal to the one that corresponds to the lines of 0
4289  // introduced when m < n
4290  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4291 
4292  return rank_out;
4293 }
4294 
4406 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
4407  vpMatrix &kerAt) const
4408 {
4409  unsigned int nrows = getRows();
4410  unsigned int ncols = getCols();
4411  int rank_out;
4412  double svThreshold = 1e-26;
4413  vpMatrix U, V;
4414  vpColVector sv_;
4415 
4416  if (nrows < ncols) {
4417  U.resize(ncols, ncols, true);
4418  sv.resize(nrows, false);
4419  } else {
4420  U.resize(nrows, ncols, false);
4421  sv.resize(ncols, false);
4422  }
4423 
4424  U.insert(*this, 0, 0);
4425  U.svdOpenCV(sv_, V);
4426 
4427  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
4428 
4429  // Remove singular values equal to the one that corresponds to the lines of 0
4430  // introduced when m < n
4431  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4432 
4433  return rank_out;
4434 }
4435 
4436 #endif
4437 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
4438 
4500 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
4501 {
4502 #if defined(VISP_HAVE_LAPACK)
4503  return pseudoInverseLapack(Ap, sv, svThreshold);
4504 #elif defined(VISP_HAVE_EIGEN3)
4505  return pseudoInverseEigen3(Ap, sv, svThreshold);
4506 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
4507  return pseudoInverseOpenCV(Ap, sv, svThreshold);
4508 #else
4509  (void)Ap;
4510  (void)sv;
4511  (void)svThreshold;
4512  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4513  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4514 #endif
4515 }
4516 
4583 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4584 {
4585 #if defined(VISP_HAVE_LAPACK)
4586  return pseudoInverseLapack(Ap, sv, rank_in);
4587 #elif defined(VISP_HAVE_EIGEN3)
4588  return pseudoInverseEigen3(Ap, sv, rank_in);
4589 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
4590  return pseudoInverseOpenCV(Ap, sv, rank_in);
4591 #else
4592  (void)Ap;
4593  (void)sv;
4594  (void)svThreshold;
4595  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4596  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4597 #endif
4598 }
4673 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
4674  vpMatrix &imAt) const
4675 {
4676  vpMatrix kerAt;
4677  return pseudoInverse(Ap, sv, svThreshold, imA, imAt, kerAt);
4678 }
4679 
4758 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt) const
4759 {
4760  vpMatrix kerAt;
4761  return pseudoInverse(Ap, sv, rank_in, imA, imAt, kerAt);
4762 }
4763 
4898 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt,
4899  vpMatrix &kerAt) const
4900 {
4901 #if defined(VISP_HAVE_LAPACK)
4902  return pseudoInverseLapack(Ap, sv, svThreshold, imA, imAt, kerAt);
4903 #elif defined(VISP_HAVE_EIGEN3)
4904  return pseudoInverseEigen3(Ap, sv, svThreshold, imA, imAt, kerAt);
4905 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
4906  return pseudoInverseOpenCV(Ap, sv, svThreshold, imA, imAt, kerAt);
4907 #else
4908  (void)Ap;
4909  (void)sv;
4910  (void)svThreshold;
4911  (void)imA;
4912  (void)imAt;
4913  (void)kerAt;
4914  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4915  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4916 #endif
4917 }
4918 
5058 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
5059  vpMatrix &kerAt) const
5060 {
5061 #if defined(VISP_HAVE_LAPACK)
5062  return pseudoInverseLapack(Ap, sv, rank_in, imA, imAt, kerAt);
5063 #elif defined(VISP_HAVE_EIGEN3)
5064  return pseudoInverseEigen3(Ap, sv, rank_in, imA, imAt, kerAt);
5065 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
5066  return pseudoInverseOpenCV(Ap, sv, rank_in, imA, imAt, kerAt);
5067 #else
5068  (void)Ap;
5069  (void)sv;
5070  (void)svThreshold;
5071  (void)imA;
5072  (void)imAt;
5073  (void)kerAt;
5074  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
5075  "Install Lapack, Eigen3 or OpenCV 3rd party"));
5076 #endif
5077 }
5078 
5120 vpColVector vpMatrix::getCol(unsigned int j, unsigned int i_begin, unsigned int column_size) const
5121 {
5122  if (i_begin + column_size > getRows() || j >= getCols())
5123  throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(),
5124  getCols()));
5125  vpColVector c(column_size);
5126  for (unsigned int i = 0; i < column_size; i++)
5127  c[i] = (*this)[i_begin + i][j];
5128  return c;
5129 }
5130 
5170 vpColVector vpMatrix::getCol(unsigned int j) const { return getCol(j, 0, rowNum); }
5171 
5207 vpRowVector vpMatrix::getRow(unsigned int i) const { return getRow(i, 0, colNum); }
5208 
5248 vpRowVector vpMatrix::getRow(unsigned int i, unsigned int j_begin, unsigned int row_size) const
5249 {
5250  if (j_begin + row_size > colNum || i >= rowNum)
5251  throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
5252 
5253  vpRowVector r(row_size);
5254  if (r.data != NULL && data != NULL) {
5255  memcpy(r.data, (*this)[i] + j_begin, row_size * sizeof(double));
5256  }
5257 
5258  return r;
5259 }
5260 
5297 {
5298  unsigned int min_size = std::min(rowNum, colNum);
5299  vpColVector diag;
5300 
5301  if (min_size > 0) {
5302  diag.resize(min_size, false);
5303 
5304  for (unsigned int i = 0; i < min_size; i++) {
5305  diag[i] = (*this)[i][i];
5306  }
5307  }
5308 
5309  return diag;
5310 }
5311 
5323 {
5324  vpMatrix C;
5325 
5326  vpMatrix::stack(A, B, C);
5327 
5328  return C;
5329 }
5330 
5342 void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
5343 {
5344  unsigned int nra = A.getRows();
5345  unsigned int nrb = B.getRows();
5346 
5347  if (nra != 0) {
5348  if (A.getCols() != B.getCols()) {
5349  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5350  A.getCols(), B.getRows(), B.getCols()));
5351  }
5352  }
5353 
5354  if (A.data != NULL && A.data == C.data) {
5355  std::cerr << "A and C must be two different objects!" << std::endl;
5356  return;
5357  }
5358 
5359  if (B.data != NULL && B.data == C.data) {
5360  std::cerr << "B and C must be two different objects!" << std::endl;
5361  return;
5362  }
5363 
5364  C.resize(nra + nrb, B.getCols(), false, false);
5365 
5366  if (C.data != NULL && A.data != NULL && A.size() > 0) {
5367  // Copy A in C
5368  memcpy(C.data, A.data, sizeof(double) * A.size());
5369  }
5370 
5371  if (C.data != NULL && B.data != NULL && B.size() > 0) {
5372  // Copy B in C
5373  memcpy(C.data + A.size(), B.data, sizeof(double) * B.size());
5374  }
5375 }
5376 
5387 {
5388  vpMatrix C;
5389  vpMatrix::stack(A, r, C);
5390 
5391  return C;
5392 }
5393 
5405 void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
5406 {
5407  if (A.data != NULL && A.data == C.data) {
5408  std::cerr << "A and C must be two different objects!" << std::endl;
5409  return;
5410  }
5411 
5412  C = A;
5413  C.stack(r);
5414 }
5415 
5426 {
5427  vpMatrix C;
5428  vpMatrix::stack(A, c, C);
5429 
5430  return C;
5431 }
5432 
5444 void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C)
5445 {
5446  if (A.data != NULL && A.data == C.data) {
5447  std::cerr << "A and C must be two different objects!" << std::endl;
5448  return;
5449  }
5450 
5451  C = A;
5452  C.stack(c);
5453 }
5454 
5467 vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, unsigned int r, unsigned int c)
5468 {
5469  vpMatrix C;
5470 
5471  insert(A, B, C, r, c);
5472 
5473  return C;
5474 }
5475 
5489 void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, unsigned int r, unsigned int c)
5490 {
5491  if (((r + B.getRows()) <= A.getRows()) && ((c + B.getCols()) <= A.getCols())) {
5492  C.resize(A.getRows(), A.getCols(), false, false);
5493 
5494  for (unsigned int i = 0; i < A.getRows(); i++) {
5495  for (unsigned int j = 0; j < A.getCols(); j++) {
5496  if (i >= r && i < (r + B.getRows()) && j >= c && j < (c + B.getCols())) {
5497  C[i][j] = B[i - r][j - c];
5498  } else {
5499  C[i][j] = A[i][j];
5500  }
5501  }
5502  }
5503  } else {
5504  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
5505  B.getRows(), B.getCols(), A.getCols(), A.getRows(), r, c);
5506  }
5507 }
5508 
5521 {
5522  vpMatrix C;
5523 
5524  juxtaposeMatrices(A, B, C);
5525 
5526  return C;
5527 }
5528 
5542 {
5543  unsigned int nca = A.getCols();
5544  unsigned int ncb = B.getCols();
5545 
5546  if (nca != 0) {
5547  if (A.getRows() != B.getRows()) {
5548  throw(vpException(vpException::dimensionError, "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5549  A.getCols(), B.getRows(), B.getCols()));
5550  }
5551  }
5552 
5553  if (B.getRows() == 0 || nca + ncb == 0) {
5554  std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl;
5555  return;
5556  }
5557 
5558  C.resize(B.getRows(), nca + ncb, false, false);
5559 
5560  C.insert(A, 0, 0);
5561  C.insert(B, 0, nca);
5562 }
5563 
5564 //--------------------------------------------------------------------
5565 // Output
5566 //--------------------------------------------------------------------
5567 
5587 int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &intro) const
5588 {
5589  typedef std::string::size_type size_type;
5590 
5591  unsigned int m = getRows();
5592  unsigned int n = getCols();
5593 
5594  std::vector<std::string> values(m * n);
5595  std::ostringstream oss;
5596  std::ostringstream ossFixed;
5597  std::ios_base::fmtflags original_flags = oss.flags();
5598 
5599  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
5600 
5601  size_type maxBefore = 0; // the length of the integral part
5602  size_type maxAfter = 0; // number of decimals plus
5603  // one place for the decimal point
5604  for (unsigned int i = 0; i < m; ++i) {
5605  for (unsigned int j = 0; j < n; ++j) {
5606  oss.str("");
5607  oss << (*this)[i][j];
5608  if (oss.str().find("e") != std::string::npos) {
5609  ossFixed.str("");
5610  ossFixed << (*this)[i][j];
5611  oss.str(ossFixed.str());
5612  }
5613 
5614  values[i * n + j] = oss.str();
5615  size_type thislen = values[i * n + j].size();
5616  size_type p = values[i * n + j].find('.');
5617 
5618  if (p == std::string::npos) {
5619  maxBefore = vpMath::maximum(maxBefore, thislen);
5620  // maxAfter remains the same
5621  } else {
5622  maxBefore = vpMath::maximum(maxBefore, p);
5623  maxAfter = vpMath::maximum(maxAfter, thislen - p);
5624  }
5625  }
5626  }
5627 
5628  size_type totalLength = length;
5629  // increase totalLength according to maxBefore
5630  totalLength = vpMath::maximum(totalLength, maxBefore);
5631  // decrease maxAfter according to totalLength
5632  maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
5633 
5634  if (!intro.empty())
5635  s << intro;
5636  s << "[" << m << "," << n << "]=\n";
5637 
5638  for (unsigned int i = 0; i < m; i++) {
5639  s << " ";
5640  for (unsigned int j = 0; j < n; j++) {
5641  size_type p = values[i * n + j].find('.');
5642  s.setf(std::ios::right, std::ios::adjustfield);
5643  s.width((std::streamsize)maxBefore);
5644  s << values[i * n + j].substr(0, p).c_str();
5645 
5646  if (maxAfter > 0) {
5647  s.setf(std::ios::left, std::ios::adjustfield);
5648  if (p != std::string::npos) {
5649  s.width((std::streamsize)maxAfter);
5650  s << values[i * n + j].substr(p, maxAfter).c_str();
5651  } else {
5652  s.width((std::streamsize)maxAfter);
5653  s << ".0";
5654  }
5655  }
5656 
5657  s << ' ';
5658  }
5659  s << std::endl;
5660  }
5661 
5662  s.flags(original_flags); // restore s to standard state
5663 
5664  return (int)(maxBefore + maxAfter);
5665 }
5666 
5703 std::ostream &vpMatrix::matlabPrint(std::ostream &os) const
5704 {
5705  os << "[ ";
5706  for (unsigned int i = 0; i < this->getRows(); ++i) {
5707  for (unsigned int j = 0; j < this->getCols(); ++j) {
5708  os << (*this)[i][j] << ", ";
5709  }
5710  if (this->getRows() != i + 1) {
5711  os << ";" << std::endl;
5712  } else {
5713  os << "]" << std::endl;
5714  }
5715  }
5716  return os;
5717 }
5718 
5747 std::ostream &vpMatrix::maplePrint(std::ostream &os) const
5748 {
5749  os << "([ " << std::endl;
5750  for (unsigned int i = 0; i < this->getRows(); ++i) {
5751  os << "[";
5752  for (unsigned int j = 0; j < this->getCols(); ++j) {
5753  os << (*this)[i][j] << ", ";
5754  }
5755  os << "]," << std::endl;
5756  }
5757  os << "])" << std::endl;
5758  return os;
5759 }
5760 
5788 std::ostream &vpMatrix::csvPrint(std::ostream &os) const
5789 {
5790  for (unsigned int i = 0; i < this->getRows(); ++i) {
5791  for (unsigned int j = 0; j < this->getCols(); ++j) {
5792  os << (*this)[i][j];
5793  if (!(j == (this->getCols() - 1)))
5794  os << ", ";
5795  }
5796  os << std::endl;
5797  }
5798  return os;
5799 }
5800 
5837 std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
5838 {
5839  os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " << this->getCols() << "); " << std::endl;
5840 
5841  for (unsigned int i = 0; i < this->getRows(); ++i) {
5842  for (unsigned int j = 0; j < this->getCols(); ++j) {
5843  if (!octet) {
5844  os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl;
5845  } else {
5846  for (unsigned int k = 0; k < sizeof(double); ++k) {
5847  os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex
5848  << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl;
5849  }
5850  }
5851  }
5852  os << std::endl;
5853  }
5854  return os;
5855 }
5856 
5862 {
5863  if (rowNum == 0) {
5864  *this = A;
5865  } else if (A.getRows() > 0) {
5866  if (colNum != A.getCols()) {
5867  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum,
5868  A.getRows(), A.getCols()));
5869  }
5870 
5871  unsigned int rowNumOld = rowNum;
5872  resize(rowNum + A.getRows(), colNum, false, false);
5873  insert(A, rowNumOld, 0);
5874  }
5875 }
5876 
5893 {
5894  if (rowNum == 0) {
5895  *this = r;
5896  } else {
5897  if (colNum != r.getCols()) {
5898  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum,
5899  colNum, r.getCols()));
5900  }
5901 
5902  if (r.size() == 0) {
5903  return;
5904  }
5905 
5906  unsigned int oldSize = size();
5907  resize(rowNum + 1, colNum, false, false);
5908 
5909  if (data != NULL && r.data != NULL && data != r.data) {
5910  // Copy r in data
5911  memcpy(data + oldSize, r.data, sizeof(double) * r.size());
5912  }
5913  }
5914 }
5915 
5933 {
5934  if (colNum == 0) {
5935  *this = c;
5936  } else {
5937  if (rowNum != c.getRows()) {
5938  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum,
5939  colNum, c.getRows()));
5940  }
5941 
5942  if (c.size() == 0) {
5943  return;
5944  }
5945 
5946  vpMatrix tmp = *this;
5947  unsigned int oldColNum = colNum;
5948  resize(rowNum, colNum + 1, false, false);
5949 
5950  if (data != NULL && tmp.data != NULL && data != tmp.data) {
5951  // Copy c in data
5952  for (unsigned int i = 0; i < rowNum; i++) {
5953  memcpy(data + i * colNum, tmp.data + i * oldColNum, sizeof(double) * oldColNum);
5954  rowPtrs[i][oldColNum] = c[i];
5955  }
5956  }
5957  }
5958 }
5959 
5970 void vpMatrix::insert(const vpMatrix &A, unsigned int r, unsigned int c)
5971 {
5972  if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) {
5973  if (A.colNum == colNum && data != NULL && A.data != NULL && A.data != data) {
5974  memcpy(data + r * colNum, A.data, sizeof(double) * A.size());
5975  } else if (data != NULL && A.data != NULL && A.data != data) {
5976  for (unsigned int i = r; i < (r + A.getRows()); i++) {
5977  memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum);
5978  }
5979  }
5980  } else {
5981  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
5982  A.getRows(), A.getCols(), rowNum, colNum, r, c);
5983  }
5984 }
5985 
6023 {
6024  vpColVector evalue(rowNum); // Eigen values
6025 
6026  if (rowNum != colNum) {
6027  throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
6028  colNum));
6029  }
6030 
6031  // Check if the matrix is symmetric: At - A = 0
6032  vpMatrix At_A = (*this).t() - (*this);
6033  for (unsigned int i = 0; i < rowNum; i++) {
6034  for (unsigned int j = 0; j < rowNum; j++) {
6035  // if (At_A[i][j] != 0) {
6036  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
6037  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix"));
6038  }
6039  }
6040  }
6041 
6042 #if defined(VISP_HAVE_LAPACK)
6043 #if defined(VISP_HAVE_GSL) /* be careful of the copy below */
6044  {
6045  gsl_vector *eval = gsl_vector_alloc(rowNum);
6046  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
6047 
6048  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
6049  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
6050 
6051  unsigned int Atda = (unsigned int)m->tda;
6052  for (unsigned int i = 0; i < rowNum; i++) {
6053  unsigned int k = i * Atda;
6054  for (unsigned int j = 0; j < colNum; j++)
6055  m->data[k + j] = (*this)[i][j];
6056  }
6057  gsl_eigen_symmv(m, eval, evec, w);
6058 
6059  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
6060 
6061  for (unsigned int i = 0; i < rowNum; i++) {
6062  evalue[i] = gsl_vector_get(eval, i);
6063  }
6064 
6065  gsl_eigen_symmv_free(w);
6066  gsl_vector_free(eval);
6067  gsl_matrix_free(m);
6068  gsl_matrix_free(evec);
6069  }
6070 #else
6071  {
6072  const char jobz = 'N';
6073  const char uplo = 'U';
6074  vpMatrix A = (*this);
6075  vpColVector WORK;
6076  int lwork = -1;
6077  int info = 0;
6078  double wkopt;
6079  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
6080  lwork = static_cast<int>(wkopt);
6081  WORK.resize(lwork);
6082  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
6083  }
6084 #endif
6085 #else
6086  {
6087  throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. "
6088  "You should install Lapack 3rd party"));
6089  }
6090 #endif
6091  return evalue;
6092 }
6093 
6143 void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const
6144 {
6145  if (rowNum != colNum) {
6146  throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
6147  colNum));
6148  }
6149 
6150  // Check if the matrix is symmetric: At - A = 0
6151  vpMatrix At_A = (*this).t() - (*this);
6152  for (unsigned int i = 0; i < rowNum; i++) {
6153  for (unsigned int j = 0; j < rowNum; j++) {
6154  // if (At_A[i][j] != 0) {
6155  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
6156  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix"));
6157  }
6158  }
6159  }
6160 
6161  // Resize the output matrices
6162  evalue.resize(rowNum);
6163  evector.resize(rowNum, colNum);
6164 
6165 #if defined(VISP_HAVE_LAPACK)
6166 #if defined(VISP_HAVE_GSL) /* be careful of the copy below */
6167  {
6168  gsl_vector *eval = gsl_vector_alloc(rowNum);
6169  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
6170 
6171  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
6172  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
6173 
6174  unsigned int Atda = (unsigned int)m->tda;
6175  for (unsigned int i = 0; i < rowNum; i++) {
6176  unsigned int k = i * Atda;
6177  for (unsigned int j = 0; j < colNum; j++)
6178  m->data[k + j] = (*this)[i][j];
6179  }
6180  gsl_eigen_symmv(m, eval, evec, w);
6181 
6182  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
6183 
6184  for (unsigned int i = 0; i < rowNum; i++) {
6185  evalue[i] = gsl_vector_get(eval, i);
6186  }
6187  Atda = (unsigned int)evec->tda;
6188  for (unsigned int i = 0; i < rowNum; i++) {
6189  unsigned int k = i * Atda;
6190  for (unsigned int j = 0; j < rowNum; j++) {
6191  evector[i][j] = evec->data[k + j];
6192  }
6193  }
6194 
6195  gsl_eigen_symmv_free(w);
6196  gsl_vector_free(eval);
6197  gsl_matrix_free(m);
6198  gsl_matrix_free(evec);
6199  }
6200 #else // defined(VISP_HAVE_GSL)
6201  {
6202  const char jobz = 'V';
6203  const char uplo = 'U';
6204  vpMatrix A = (*this);
6205  vpColVector WORK;
6206  int lwork = -1;
6207  int info = 0;
6208  double wkopt;
6209  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
6210  lwork = static_cast<int>(wkopt);
6211  WORK.resize(lwork);
6212  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
6213  evector = A.t();
6214  }
6215 #endif // defined(VISP_HAVE_GSL)
6216 #else
6217  {
6218  throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. "
6219  "You should install Lapack 3rd party"));
6220  }
6221 #endif
6222 }
6223 
6242 unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const
6243 {
6244  unsigned int nbline = getRows();
6245  unsigned int nbcol = getCols();
6246 
6247  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6248  vpColVector sv;
6249  sv.resize(nbcol, false); // singular values
6250  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6251 
6252  // Copy and resize matrix to have at least as many rows as columns
6253  // kernel is computed in svd method only if the matrix has more rows than
6254  // columns
6255 
6256  if (nbline < nbcol)
6257  U.resize(nbcol, nbcol, true);
6258  else
6259  U.resize(nbline, nbcol, false);
6260 
6261  U.insert(*this, 0, 0);
6262 
6263  U.svd(sv, V);
6264 
6265  // Compute the highest singular value and rank of the matrix
6266  double maxsv = 0;
6267  for (unsigned int i = 0; i < nbcol; i++) {
6268  if (sv[i] > maxsv) {
6269  maxsv = sv[i];
6270  }
6271  }
6272 
6273  unsigned int rank = 0;
6274  for (unsigned int i = 0; i < nbcol; i++) {
6275  if (sv[i] > maxsv * svThreshold) {
6276  rank++;
6277  }
6278  }
6279 
6280  kerAt.resize(nbcol - rank, nbcol);
6281  if (rank != nbcol) {
6282  for (unsigned int j = 0, k = 0; j < nbcol; j++) {
6283  // if( v.col(j) in kernel and non zero )
6284  if ((sv[j] <= maxsv * svThreshold) &&
6285  (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
6286  for (unsigned int i = 0; i < V.getRows(); i++) {
6287  kerAt[k][i] = V[i][j];
6288  }
6289  k++;
6290  }
6291  }
6292  }
6293 
6294  return rank;
6295 }
6296 
6313 unsigned int vpMatrix::nullSpace(vpMatrix &kerA, double svThreshold) const
6314 {
6315  unsigned int nbrow = getRows();
6316  unsigned int nbcol = getCols();
6317 
6318  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6319  vpColVector sv;
6320  sv.resize(nbcol, false); // singular values
6321  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6322 
6323  // Copy and resize matrix to have at least as many rows as columns
6324  // kernel is computed in svd method only if the matrix has more rows than
6325  // columns
6326 
6327  if (nbrow < nbcol)
6328  U.resize(nbcol, nbcol, true);
6329  else
6330  U.resize(nbrow, nbcol, false);
6331 
6332  U.insert(*this, 0, 0);
6333 
6334  U.svd(sv, V);
6335 
6336  // Compute the highest singular value and rank of the matrix
6337  double maxsv = sv[0];
6338 
6339  unsigned int rank = 0;
6340  for (unsigned int i = 0; i < nbcol; i++) {
6341  if (sv[i] > maxsv * svThreshold) {
6342  rank++;
6343  }
6344  }
6345 
6346  kerA.resize(nbcol, nbcol - rank);
6347  if (rank != nbcol) {
6348  for (unsigned int j = 0, k = 0; j < nbcol; j++) {
6349  // if( v.col(j) in kernel and non zero )
6350  if (sv[j] <= maxsv * svThreshold) {
6351  for (unsigned int i = 0; i < nbcol; i++) {
6352  kerA[i][k] = V[i][j];
6353  }
6354  k++;
6355  }
6356  }
6357  }
6358 
6359  return (nbcol - rank);
6360 }
6361 
6378 unsigned int vpMatrix::nullSpace(vpMatrix &kerA, int dim) const
6379 {
6380  unsigned int nbrow = getRows();
6381  unsigned int nbcol = getCols();
6382  unsigned int dim_ = static_cast<unsigned int>(dim);
6383 
6384  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6385  vpColVector sv;
6386  sv.resize(nbcol, false); // singular values
6387  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6388 
6389  // Copy and resize matrix to have at least as many rows as columns
6390  // kernel is computed in svd method only if the matrix has more rows than
6391  // columns
6392 
6393  if (nbrow < nbcol)
6394  U.resize(nbcol, nbcol, true);
6395  else
6396  U.resize(nbrow, nbcol, false);
6397 
6398  U.insert(*this, 0, 0);
6399 
6400  U.svd(sv, V);
6401 
6402  kerA.resize(nbcol, dim_);
6403  if (dim_ != 0) {
6404  unsigned int rank = nbcol - dim_;
6405  for (unsigned int k = 0; k < dim_; k++) {
6406  unsigned int j = k + rank;
6407  for (unsigned int i = 0; i < nbcol; i++) {
6408  kerA[i][k] = V[i][j];
6409  }
6410  }
6411  }
6412 
6413  double maxsv = sv[0];
6414  unsigned int rank = 0;
6415  for (unsigned int i = 0; i < nbcol; i++) {
6416  if (sv[i] > maxsv * 1e-6) {
6417  rank++;
6418  }
6419  }
6420  return (nbcol - rank);
6421 }
6422 
6454 double vpMatrix::det(vpDetMethod method) const
6455 {
6456  double det = 0.;
6457 
6458  if (method == LU_DECOMPOSITION) {
6459  det = this->detByLU();
6460  }
6461 
6462  return (det);
6463 }
6464 
6473 {
6474  if (colNum != rowNum) {
6475  throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix",
6476  rowNum, colNum));
6477  } else {
6478 #ifdef VISP_HAVE_GSL
6479  size_t size_ = rowNum * colNum;
6480  double *b = new double[size_];
6481  for (size_t i = 0; i < size_; i++)
6482  b[i] = 0.;
6483  gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum);
6484  gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum);
6485  gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
6486  // gsl_matrix_fprintf(stdout, &em.matrix, "%g");
6487  vpMatrix expA;
6488  expA.resize(rowNum, colNum, false);
6489  memcpy(expA.data, b, size_ * sizeof(double));
6490 
6491  delete[] b;
6492  return expA;
6493 #else
6494  vpMatrix _expE(rowNum, colNum, false);
6495  vpMatrix _expD(rowNum, colNum, false);
6496  vpMatrix _expX(rowNum, colNum, false);
6497  vpMatrix _expcX(rowNum, colNum, false);
6498  vpMatrix _eye(rowNum, colNum, false);
6499 
6500  _eye.eye();
6501  vpMatrix exp(*this);
6502 
6503  // double f;
6504  int e;
6505  double c = 0.5;
6506  int q = 6;
6507  int p = 1;
6508 
6509  double nA = 0;
6510  for (unsigned int i = 0; i < rowNum; i++) {
6511  double sum = 0;
6512  for (unsigned int j = 0; j < colNum; j++) {
6513  sum += fabs((*this)[i][j]);
6514  }
6515  if (sum > nA || i == 0) {
6516  nA = sum;
6517  }
6518  }
6519 
6520  /* f = */ frexp(nA, &e);
6521  // double s = (0 > e+1)?0:e+1;
6522  double s = e + 1;
6523 
6524  double sca = 1.0 / pow(2.0, s);
6525  exp = sca * exp;
6526  _expX = *this;
6527  _expE = c * exp + _eye;
6528  _expD = -c * exp + _eye;
6529  for (int k = 2; k <= q; k++) {
6530  c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1)));
6531  _expcX = exp * _expX;
6532  _expX = _expcX;
6533  _expcX = c * _expX;
6534  _expE = _expE + _expcX;
6535  if (p)
6536  _expD = _expD + _expcX;
6537  else
6538  _expD = _expD - _expcX;
6539  p = !p;
6540  }
6541  _expX = _expD.inverseByLU();
6542  exp = _expX * _expE;
6543  for (int k = 1; k <= s; k++) {
6544  _expE = exp * exp;
6545  exp = _expE;
6546  }
6547  return exp;
6548 #endif
6549  }
6550 }
6551 
6552 /**************************************************************************************************************/
6553 /**************************************************************************************************************/
6554 
6555 // Specific functions
6556 
6557 /*
6558 input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.
6559 
6560 output:: the complement matrix of the element (rowNo,colNo).
6561 This is the matrix obtained from M after elimenating the row rowNo and column
6562 colNo
6563 
6564 example:
6565 1 2 3
6566 M = 4 5 6
6567 7 8 9
6568 1 3
6569 subblock(M, 1, 1) give the matrix 7 9
6570 */
6571 vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row)
6572 {
6573  vpMatrix M_comp;
6574  M_comp.resize(M.getRows() - 1, M.getCols() - 1, false);
6575 
6576  for (unsigned int i = 0; i < col; i++) {
6577  for (unsigned int j = 0; j < row; j++)
6578  M_comp[i][j] = M[i][j];
6579  for (unsigned int j = row + 1; j < M.getRows(); j++)
6580  M_comp[i][j - 1] = M[i][j];
6581  }
6582  for (unsigned int i = col + 1; i < M.getCols(); i++) {
6583  for (unsigned int j = 0; j < row; j++)
6584  M_comp[i - 1][j] = M[i][j];
6585  for (unsigned int j = row + 1; j < M.getRows(); j++)
6586  M_comp[i - 1][j - 1] = M[i][j];
6587  }
6588  return M_comp;
6589 }
6590 
6600 double vpMatrix::cond(double svThreshold) const
6601 {
6602  unsigned int nbline = getRows();
6603  unsigned int nbcol = getCols();
6604 
6605  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6606  vpColVector sv;
6607  sv.resize(nbcol); // singular values
6608  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6609 
6610  // Copy and resize matrix to have at least as many rows as columns
6611  // kernel is computed in svd method only if the matrix has more rows than
6612  // columns
6613 
6614  if (nbline < nbcol)
6615  U.resize(nbcol, nbcol, true);
6616  else
6617  U.resize(nbline, nbcol, false);
6618 
6619  U.insert(*this, 0, 0);
6620 
6621  U.svd(sv, V);
6622 
6623  // Compute the highest singular value
6624  double maxsv = 0;
6625  for (unsigned int i = 0; i < nbcol; i++) {
6626  if (sv[i] > maxsv) {
6627  maxsv = sv[i];
6628  }
6629  }
6630 
6631  // Compute the rank of the matrix
6632  unsigned int rank = 0;
6633  for (unsigned int i = 0; i < nbcol; i++) {
6634  if (sv[i] > maxsv * svThreshold) {
6635  rank++;
6636  }
6637  }
6638 
6639  // Compute the lowest singular value
6640  double minsv = maxsv;
6641  for (unsigned int i = 0; i < rank; i++) {
6642  if (sv[i] < minsv) {
6643  minsv = sv[i];
6644  }
6645  }
6646 
6647  if (std::fabs(minsv) > std::numeric_limits<double>::epsilon()) {
6648  return maxsv / minsv;
6649  } else {
6650  return std::numeric_limits<double>::infinity();
6651  }
6652 }
6653 
6660 void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
6661 {
6662  if (H.getCols() != H.getRows()) {
6663  throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(),
6664  H.getCols()));
6665  }
6666 
6667  HLM = H;
6668  for (unsigned int i = 0; i < H.getCols(); i++) {
6669  HLM[i][i] += alpha * H[i][i];
6670  }
6671 }
6672 
6681 {
6682  double norm = 0.0;
6683  for (unsigned int i = 0; i < dsize; i++) {
6684  double x = *(data + i);
6685  norm += x * x;
6686  }
6687 
6688  return sqrt(norm);
6689 }
6690 
6700 {
6701  if (this->dsize != 0) {
6702  vpMatrix v;
6703  vpColVector w;
6704 
6705  vpMatrix M = *this;
6706 
6707  M.svd(w, v);
6708 
6709  double max = w[0];
6710  unsigned int maxRank = std::min(this->getCols(), this->getRows());
6711  // The maximum reachable rank is either the number of columns or the number of rows
6712  // of the matrix.
6713  unsigned int boundary = std::min(maxRank, w.size());
6714  // boundary is here to ensure that the number of singular values used for the com-
6715  // putation of the euclidean norm of the matrix is not greater than the maximum
6716  // reachable rank. Indeed, some svd library pad the singular values vector with 0s
6717  // if the input matrix is non-square.
6718  for (unsigned int i = 0; i < boundary; i++) {
6719  if (max < w[i]) {
6720  max = w[i];
6721  }
6722  }
6723  return max;
6724  } else {
6725  return 0.;
6726  }
6727 }
6728 
6740 {
6741  double norm = 0.0;
6742  for (unsigned int i = 0; i < rowNum; i++) {
6743  double x = 0;
6744  for (unsigned int j = 0; j < colNum; j++) {
6745  x += fabs(*(*(rowPtrs + i) + j));
6746  }
6747  if (x > norm) {
6748  norm = x;
6749  }
6750  }
6751  return norm;
6752 }
6753 
6760 double vpMatrix::sumSquare() const
6761 {
6762  double sum_square = 0.0;
6763  double x;
6764 
6765  for (unsigned int i = 0; i < rowNum; i++) {
6766  for (unsigned int j = 0; j < colNum; j++) {
6767  x = rowPtrs[i][j];
6768  sum_square += x * x;
6769  }
6770  }
6771 
6772  return sum_square;
6773 }
6774 
6786 vpMatrix vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode)
6787 {
6788  vpMatrix res;
6789  conv2(M, kernel, res, mode);
6790  return res;
6791 }
6792 
6805 void vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, vpMatrix &res, const std::string &mode)
6806 {
6807  if (M.getRows() * M.getCols() == 0 || kernel.getRows() * kernel.getCols() == 0)
6808  return;
6809 
6810  if (mode == "valid") {
6811  if (kernel.getRows() > M.getRows() || kernel.getCols() > M.getCols())
6812  return;
6813  }
6814 
6815  vpMatrix M_padded, res_same;
6816 
6817  if (mode == "full" || mode == "same") {
6818  const unsigned int pad_x = kernel.getCols() - 1;
6819  const unsigned int pad_y = kernel.getRows() - 1;
6820  M_padded.resize(M.getRows() + 2 * pad_y, M.getCols() + 2 * pad_x, true, false);
6821  M_padded.insert(M, pad_y, pad_x);
6822 
6823  if (mode == "same") {
6824  res.resize(M.getRows(), M.getCols(), false, false);
6825  res_same.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
6826  } else {
6827  res.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
6828  }
6829  } else if (mode == "valid") {
6830  M_padded = M;
6831  res.resize(M.getRows() - kernel.getRows() + 1, M.getCols() - kernel.getCols() + 1);
6832  } else {
6833  return;
6834  }
6835 
6836  if (mode == "same") {
6837  for (unsigned int i = 0; i < res_same.getRows(); i++) {
6838  for (unsigned int j = 0; j < res_same.getCols(); j++) {
6839  for (unsigned int k = 0; k < kernel.getRows(); k++) {
6840  for (unsigned int l = 0; l < kernel.getCols(); l++) {
6841  res_same[i][j] += M_padded[i + k][j + l] * kernel[kernel.getRows() - k - 1][kernel.getCols() - l - 1];
6842  }
6843  }
6844  }
6845  }
6846 
6847  const unsigned int start_i = kernel.getRows() / 2;
6848  const unsigned int start_j = kernel.getCols() / 2;
6849  for (unsigned int i = 0; i < M.getRows(); i++) {
6850  memcpy(res.data + i * M.getCols(), res_same.data + (i + start_i) * res_same.getCols() + start_j,
6851  sizeof(double) * M.getCols());
6852  }
6853  } else {
6854  for (unsigned int i = 0; i < res.getRows(); i++) {
6855  for (unsigned int j = 0; j < res.getCols(); j++) {
6856  for (unsigned int k = 0; k < kernel.getRows(); k++) {
6857  for (unsigned int l = 0; l < kernel.getCols(); l++) {
6858  res[i][j] += M_padded[i + k][j + l] * kernel[kernel.getRows() - k - 1][kernel.getCols() - l - 1];
6859  }
6860  }
6861  }
6862  }
6863  }
6864 }
6865 
6866 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
6876 vp_deprecated double vpMatrix::euclideanNorm() const { return frobeniusNorm(); }
6877 
6879 {
6880  return (vpMatrix)(vpColVector::stack(A, B));
6881 }
6882 
6884 {
6885  vpColVector::stack(A, B, C);
6886 }
6887 
6889 
6890 void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); }
6891 
6911 {
6912  vpRowVector c(getCols());
6913 
6914  for (unsigned int j = 0; j < getCols(); j++)
6915  c[j] = (*this)[i - 1][j];
6916  return c;
6917 }
6918 
6937 {
6938  vpColVector c(getRows());
6939 
6940  for (unsigned int i = 0; i < getRows(); i++)
6941  c[i] = (*this)[i][j - 1];
6942  return c;
6943 }
6944 
6951 void vpMatrix::setIdentity(const double &val)
6952 {
6953  for (unsigned int i = 0; i < rowNum; i++)
6954  for (unsigned int j = 0; j < colNum; j++)
6955  if (i == j)
6956  (*this)[i][j] = val;
6957  else
6958  (*this)[i][j] = 0;
6959 }
6960 
6961 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:129
unsigned int getCols() const
Definition: vpArray2D.h:278
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:142
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:136
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:303
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:132
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:138
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:290
unsigned int getRows() const
Definition: vpArray2D.h:288
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:134
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
double sumSquare() const
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:314
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:90
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
@ divideByZeroError
Division by zero.
Definition: vpException.h:94
Implementation of an homogeneous matrix and operations on such kind of matrices.
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:175
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void svdLapack(vpColVector &w, vpMatrix &V)
static vpMatrix conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode="full")
Definition: vpMatrix.cpp:6786
vpColVector eigenValues() const
Definition: vpMatrix.cpp:6022
vpMatrix pseudoInverseOpenCV(double svThreshold=1e-6) const
std::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
Definition: vpMatrix.cpp:5837
vpMatrix & operator<<(double *)
Definition: vpMatrix.cpp:780
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6600
vpMatrix expm() const
Definition: vpMatrix.cpp:6472
vpMatrix pseudoInverseEigen3(double svThreshold=1e-6) const
vpMatrix hadamard(const vpMatrix &m) const
Definition: vpMatrix.cpp:1758
vpMatrix operator-() const
Definition: vpMatrix.cpp:1549
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:6951
vpMatrix & operator/=(double x)
Divide all the element of the matrix by x : Aij = Aij / x.
Definition: vpMatrix.cpp:1680
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5587
std::ostream & maplePrint(std::ostream &os) const
Definition: vpMatrix.cpp:5747
void eye()
Definition: vpMatrix.cpp:447
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6242
vpMatrix inverseByLU() const
vpMatrix operator*(const vpMatrix &B) const
Definition: vpMatrix.cpp:1159
vpMatrix operator/(double x) const
Cij = Aij / x (A is unchanged)
Definition: vpMatrix.cpp:1620
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5861
vp_deprecated void stackMatrices(const vpMatrix &A)
Definition: vpMatrix.h:1016
vpMatrix & operator-=(const vpMatrix &B)
Operation A = A - B.
Definition: vpMatrix.cpp:1507
vpMatrix & operator*=(double x)
Multiply all the element of the matrix by x : Aij = Aij * x.
Definition: vpMatrix.cpp:1666
vpMatrix operator*(const double &x, const vpMatrix &B)
Definition: vpMatrix.cpp:1580
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:5520
vpColVector stackColumns()
Definition: vpMatrix.cpp:1723
vp_deprecated vpColVector column(unsigned int j)
Definition: vpMatrix.cpp:6936
static void sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1456
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2021
vpRowVector stackRows()
Definition: vpMatrix.cpp:1745
vpMatrix operator+(const vpMatrix &B) const
Definition: vpMatrix.cpp:1401
double sum() const
Definition: vpMatrix.cpp:1556
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:879
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:5207
void svdOpenCV(vpColVector &w, vpMatrix &V)
vpMatrix t() const
Definition: vpMatrix.cpp:462
vpMatrix()
Definition: vpMatrix.h:169
vpMatrix & operator+=(const vpMatrix &B)
Operation A = A + B.
Definition: vpMatrix.cpp:1490
static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C)
Definition: vpMatrix.cpp:1314
double inducedL2Norm() const
Definition: vpMatrix.cpp:6699
double infinityNorm() const
Definition: vpMatrix.cpp:6739
vpMatrix & operator=(const vpArray2D< double > &A)
Definition: vpMatrix.cpp:648
double frobeniusNorm() const
Definition: vpMatrix.cpp:6680
vpColVector getDiag() const
Definition: vpMatrix.cpp:5296
vpMatrix AtA() const
Definition: vpMatrix.cpp:623
static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
Definition: vpMatrix.cpp:898
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:1899
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6660
vpMatrix pseudoInverseLapack(double svThreshold=1e-6) const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2232
vpMatrix & operator,(double val)
Definition: vpMatrix.cpp:797
static void multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
Definition: vpMatrix.cpp:951
vp_deprecated vpRowVector row(unsigned int i)
Definition: vpMatrix.cpp:6910
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5170
double detByLU() const
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6454
double sumSquare() const
Definition: vpMatrix.cpp:6760
vp_deprecated void init()
Definition: vpMatrix.h:1011
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1343
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5970
void svdEigen3(vpColVector &w, vpMatrix &V)
void kron(const vpMatrix &m1, vpMatrix &out) const
Definition: vpMatrix.cpp:1808
vpMatrix AAt() const
Definition: vpMatrix.cpp:501
vpMatrix transpose() const
Definition: vpMatrix.cpp:469
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1000
static void negateMatrix(const vpMatrix &A, vpMatrix &C)
Definition: vpMatrix.cpp:1531
@ LU_DECOMPOSITION
Definition: vpMatrix.h:161
std::ostream & csvPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5788
unsigned int nullSpace(vpMatrix &kerA, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6313
std::ostream & matlabPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5703
double euclideanNorm() const
Definition: vpMatrix.cpp:6876
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:405
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:116
void resize(unsigned int i, bool flagNullify=true)
Definition: vpRowVector.h:271
Class that consider the case of a translation vector.