Visual Servoing Platform  version 3.5.1 under development (2022-07-05)
vpMatrix.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Matrix manipulation.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
43 #include <algorithm>
44 #include <assert.h>
45 #include <cmath> // std::fabs
46 #include <fstream>
47 #include <limits> // numeric_limits
48 #include <sstream>
49 #include <stdio.h>
50 #include <stdlib.h>
51 #include <string.h>
52 #include <string>
53 #include <vector>
54 
55 #include <visp3/core/vpCPUFeatures.h>
56 #include <visp3/core/vpColVector.h>
57 #include <visp3/core/vpConfig.h>
58 #include <visp3/core/vpDebug.h>
59 #include <visp3/core/vpException.h>
60 #include <visp3/core/vpMath.h>
61 #include <visp3/core/vpMatrix.h>
62 #include <visp3/core/vpTranslationVector.h>
63 
64 #include <Simd/SimdLib.hpp>
65 
66 #ifdef VISP_HAVE_LAPACK
67 #ifdef VISP_HAVE_GSL
68 #include <gsl/gsl_eigen.h>
69 #include <gsl/gsl_linalg.h>
70 #include <gsl/gsl_math.h>
71 #elif defined(VISP_HAVE_MKL)
72 #include <mkl.h>
73 typedef MKL_INT integer;
74 
75 void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_, double *w_data,
76  double *work_data, int lwork_, int &info_)
77 {
78  MKL_INT n = static_cast<MKL_INT>(n_);
79  MKL_INT lda = static_cast<MKL_INT>(lda_);
80  MKL_INT lwork = static_cast<MKL_INT>(lwork_);
81  MKL_INT info = static_cast<MKL_INT>(info_);
82 
83  dsyev(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
84 }
85 
86 #else
87 #if defined(VISP_HAVE_LAPACK_BUILT_IN)
88 typedef long int integer;
89 #else
90 typedef int integer;
91 #endif
92 extern "C" integer dsyev_(char *jobz, char *uplo, integer *n, double *a, integer *lda, double *w, double *WORK,
93  integer *lwork, integer *info);
94 
95 void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_, double *w_data,
96  double *work_data, int lwork_, int &info_)
97 {
98  integer n = static_cast<integer>(n_);
99  integer lda = static_cast<integer>(lda_);
100  integer lwork = static_cast<integer>(lwork_);
101  integer info = static_cast<integer>(info_);
102 
103  dsyev_(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
104 
105  lwork_ = static_cast<int>(lwork);
106  info_ = static_cast<int>(info);
107 }
108 #endif
109 #endif
110 
111 #if !defined(VISP_USE_MSVC) || (defined(VISP_USE_MSVC) && !defined(VISP_BUILD_SHARED_LIBS))
112 const unsigned int vpMatrix::m_lapack_min_size_default = 0;
113 unsigned int vpMatrix::m_lapack_min_size = vpMatrix::m_lapack_min_size_default;
114 #endif
115 
116 // Prototypes of specific functions
117 vpMatrix subblock(const vpMatrix &, unsigned int, unsigned int);
118 
119 void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows,
120  unsigned int ncols, double svThreshold, vpMatrix &Ap, int &rank_out, int *rank_in,
121  vpMatrix *imA, vpMatrix *imAt, vpMatrix *kerAt)
122 {
123  Ap.resize(ncols, nrows, true, false);
124 
125  // compute the highest singular value and the rank of h
126  double maxsv = sv[0];
127 
128  rank_out = 0;
129 
130  for (unsigned int i = 0; i < ncols; i++) {
131  if (sv[i] > maxsv * svThreshold) {
132  rank_out++;
133  }
134  }
135 
136  unsigned int rank = static_cast<unsigned int>(rank_out);
137  if (rank_in) {
138  rank = static_cast<unsigned int>(*rank_in);
139  }
140 
141  for (unsigned int i = 0; i < ncols; i++) {
142  for (unsigned int j = 0; j < nrows; j++) {
143  for (unsigned int k = 0; k < rank; k++) {
144  Ap[i][j] += V[i][k] * U[j][k] / sv[k];
145  }
146  }
147  }
148 
149  // Compute im(A)
150  if (imA) {
151  imA->resize(nrows, rank);
152 
153  for (unsigned int i = 0; i < nrows; i++) {
154  for (unsigned int j = 0; j < rank; j++) {
155  (*imA)[i][j] = U[i][j];
156  }
157  }
158  }
159 
160  // Compute im(At)
161  if (imAt) {
162  imAt->resize(ncols, rank);
163  for (unsigned int i = 0; i < ncols; i++) {
164  for (unsigned int j = 0; j < rank; j++) {
165  (*imAt)[i][j] = V[i][j];
166  }
167  }
168  }
169 
170  // Compute ker(At)
171  if (kerAt) {
172  kerAt->resize(ncols - rank, ncols);
173  if (rank != ncols) {
174  for (unsigned int k = 0; k < (ncols - rank); k++) {
175  unsigned j = k + rank;
176  for (unsigned int i = 0; i < V.getRows(); i++) {
177  (*kerAt)[k][i] = V[i][j];
178  }
179  }
180  }
181  }
182 }
183 
189 vpMatrix::vpMatrix(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
190  : vpArray2D<double>()
191 {
192  if (((r + nrows) > M.rowNum) || ((c + ncols) > M.colNum)) {
194  "Cannot construct a sub matrix (%dx%d) starting at "
195  "position (%d,%d) that is not contained in the "
196  "original matrix (%dx%d)",
197  nrows, ncols, r, c, M.rowNum, M.colNum));
198  }
199 
200  init(M, r, c, nrows, ncols);
201 }
202 
203 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
205 {
206  rowNum = A.rowNum;
207  colNum = A.colNum;
208  rowPtrs = A.rowPtrs;
209  dsize = A.dsize;
210  data = A.data;
211 
212  A.rowNum = 0;
213  A.colNum = 0;
214  A.rowPtrs = NULL;
215  A.dsize = 0;
216  A.data = NULL;
217 }
218 
244 vpMatrix::vpMatrix(const std::initializer_list<double> &list) : vpArray2D<double>(list) {}
245 
270 vpMatrix::vpMatrix(unsigned int nrows, unsigned int ncols, const std::initializer_list<double> &list)
271  : vpArray2D<double>(nrows, ncols, list)
272 {
273 }
274 
297 vpMatrix::vpMatrix(const std::initializer_list<std::initializer_list<double> > &lists) : vpArray2D<double>(lists) {}
298 
299 #endif
300 
347 void vpMatrix::init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
348 {
349  unsigned int rnrows = r + nrows;
350  unsigned int cncols = c + ncols;
351 
352  if (rnrows > M.getRows())
353  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
354  M.getRows()));
355  if (cncols > M.getCols())
356  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
357  M.getCols()));
358  resize(nrows, ncols, false, false);
359 
360  if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
361  return; // Noting to do
362  for (unsigned int i = 0; i < nrows; i++) {
363  memcpy((*this)[i], &M[i + r][c], ncols * sizeof(double));
364  }
365 }
366 
408 vpMatrix vpMatrix::extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
409 {
410  unsigned int rnrows = r + nrows;
411  unsigned int cncols = c + ncols;
412 
413  if (rnrows > getRows())
414  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
415  getRows()));
416  if (cncols > getCols())
417  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
418  getCols()));
419 
420  vpMatrix M;
421  M.resize(nrows, ncols, false, false);
422  for (unsigned int i = 0; i < nrows; i++) {
423  memcpy(M[i], &(*this)[i + r][c], ncols * sizeof(double));
424  }
425 
426  return M;
427 }
428 
433 void vpMatrix::eye(unsigned int n) { eye(n, n); }
434 
439 void vpMatrix::eye(unsigned int m, unsigned int n)
440 {
441  resize(m, n);
442 
443  eye();
444 }
445 
451 {
452  for (unsigned int i = 0; i < rowNum; i++) {
453  for (unsigned int j = 0; j < colNum; j++) {
454  if (i == j)
455  (*this)[i][j] = 1.0;
456  else
457  (*this)[i][j] = 0;
458  }
459  }
460 }
461 
465 vpMatrix vpMatrix::t() const { return transpose(); }
466 
473 {
474  vpMatrix At;
475  transpose(At);
476  return At;
477 }
478 
485 {
486  At.resize(colNum, rowNum, false, false);
487 
488  if (rowNum <= 16 || colNum <= 16) {
489  for (unsigned int i = 0; i < rowNum; i++) {
490  for (unsigned int j = 0; j < colNum; j++) {
491  At[j][i] = (*this)[i][j];
492  }
493  }
494  } else {
495  SimdMatTranspose(data, rowNum, colNum, At.data);
496  }
497 }
498 
505 {
506  vpMatrix B;
507 
508  AAt(B);
509 
510  return B;
511 }
512 
524 void vpMatrix::AAt(vpMatrix &B) const
525 {
526  if ((B.rowNum != rowNum) || (B.colNum != rowNum))
527  B.resize(rowNum, rowNum, false, false);
528 
529  // If available use Lapack only for large matrices
530  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
531 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
532  useLapack = false;
533 #endif
534 
535  if (useLapack) {
536 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
537  const double alpha = 1.0;
538  const double beta = 0.0;
539  const char transa = 't';
540  const char transb = 'n';
541 
542  vpMatrix::blas_dgemm(transa, transb, rowNum, rowNum, colNum, alpha, data, colNum, data, colNum, beta, B.data,
543  rowNum);
544 #endif
545  } else {
546  // compute A*A^T
547  for (unsigned int i = 0; i < rowNum; i++) {
548  for (unsigned int j = i; j < rowNum; j++) {
549  double *pi = rowPtrs[i]; // row i
550  double *pj = rowPtrs[j]; // row j
551 
552  // sum (row i .* row j)
553  double ssum = 0;
554  for (unsigned int k = 0; k < colNum; k++)
555  ssum += *(pi++) * *(pj++);
556 
557  B[i][j] = ssum; // upper triangle
558  if (i != j)
559  B[j][i] = ssum; // lower triangle
560  }
561  }
562  }
563 }
564 
576 void vpMatrix::AtA(vpMatrix &B) const
577 {
578  if ((B.rowNum != colNum) || (B.colNum != colNum))
579  B.resize(colNum, colNum, false, false);
580 
581  // If available use Lapack only for large matrices
582  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
583 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
584  useLapack = false;
585 #endif
586 
587  if (useLapack) {
588 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
589  const double alpha = 1.0;
590  const double beta = 0.0;
591  const char transa = 'n';
592  const char transb = 't';
593 
594  vpMatrix::blas_dgemm(transa, transb, colNum, colNum, rowNum, alpha, data, colNum, data, colNum, beta, B.data,
595  colNum);
596 #endif
597  } else {
598  for (unsigned int i = 0; i < colNum; i++) {
599  double *Bi = B[i];
600  for (unsigned int j = 0; j < i; j++) {
601  double *ptr = data;
602  double s = 0;
603  for (unsigned int k = 0; k < rowNum; k++) {
604  s += (*(ptr + i)) * (*(ptr + j));
605  ptr += colNum;
606  }
607  *Bi++ = s;
608  B[j][i] = s;
609  }
610  double *ptr = data;
611  double s = 0;
612  for (unsigned int k = 0; k < rowNum; k++) {
613  s += (*(ptr + i)) * (*(ptr + i));
614  ptr += colNum;
615  }
616  *Bi = s;
617  }
618  }
619 }
620 
627 {
628  vpMatrix B;
629 
630  AtA(B);
631 
632  return B;
633 }
634 
652 {
653  resize(A.getRows(), A.getCols(), false, false);
654 
655  if (data != NULL && A.data != NULL && data != A.data) {
656  memcpy(data, A.data, dsize * sizeof(double));
657  }
658 
659  return *this;
660 }
661 
662 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
664 {
665  resize(A.getRows(), A.getCols(), false, false);
666 
667  if (data != NULL && A.data != NULL && data != A.data) {
668  memcpy(data, A.data, dsize * sizeof(double));
669  }
670 
671  return *this;
672 }
673 
675 {
676  if (this != &other) {
677  free(data);
678  free(rowPtrs);
679 
680  rowNum = other.rowNum;
681  colNum = other.colNum;
682  rowPtrs = other.rowPtrs;
683  dsize = other.dsize;
684  data = other.data;
685 
686  other.rowNum = 0;
687  other.colNum = 0;
688  other.rowPtrs = NULL;
689  other.dsize = 0;
690  other.data = NULL;
691  }
692 
693  return *this;
694 }
695 
718 vpMatrix &vpMatrix::operator=(const std::initializer_list<double> &list)
719 {
720  if (dsize != static_cast<unsigned int>(list.size())) {
721  resize(1, static_cast<unsigned int>(list.size()), false, false);
722  }
723 
724  std::copy(list.begin(), list.end(), data);
725 
726  return *this;
727 }
728 
752 vpMatrix &vpMatrix::operator=(const std::initializer_list<std::initializer_list<double> > &lists)
753 {
754  unsigned int nrows = static_cast<unsigned int>(lists.size()), ncols = 0;
755  for (auto &l : lists) {
756  if (static_cast<unsigned int>(l.size()) > ncols) {
757  ncols = static_cast<unsigned int>(l.size());
758  }
759  }
760 
761  resize(nrows, ncols, false, false);
762  auto it = lists.begin();
763  for (unsigned int i = 0; i < rowNum; i++, ++it) {
764  std::copy(it->begin(), it->end(), rowPtrs[i]);
765  }
766 
767  return *this;
768 }
769 #endif
770 
773 {
774  std::fill(data, data + rowNum * colNum, x);
775  return *this;
776 }
777 
784 {
785  for (unsigned int i = 0; i < rowNum; i++) {
786  for (unsigned int j = 0; j < colNum; j++) {
787  rowPtrs[i][j] = *x++;
788  }
789  }
790  return *this;
791 }
792 
794 {
795  resize(1, 1, false, false);
796  rowPtrs[0][0] = val;
797  return *this;
798 }
799 
801 {
802  resize(1, colNum + 1, false, false);
803  rowPtrs[0][colNum - 1] = val;
804  return *this;
805 }
806 
843 {
844  unsigned int rows = A.getRows();
845  this->resize(rows, rows);
846 
847  (*this) = 0;
848  for (unsigned int i = 0; i < rows; i++)
849  (*this)[i][i] = A[i];
850 }
851 
882 void vpMatrix::diag(const double &val)
883 {
884  (*this) = 0;
885  unsigned int min_ = (rowNum < colNum) ? rowNum : colNum;
886  for (unsigned int i = 0; i < min_; i++)
887  (*this)[i][i] = val;
888 }
889 
902 {
903  unsigned int rows = A.getRows();
904  DA.resize(rows, rows, true);
905 
906  for (unsigned int i = 0; i < rows; i++)
907  DA[i][i] = A[i];
908 }
909 
915 {
916  vpTranslationVector t_out;
917 
918  if (rowNum != 3 || colNum != 3) {
919  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector",
920  rowNum, colNum, tv.getRows(), tv.getCols()));
921  }
922 
923  for (unsigned int j = 0; j < 3; j++)
924  t_out[j] = 0;
925 
926  for (unsigned int j = 0; j < 3; j++) {
927  double tj = tv[j]; // optimization em 5/12/2006
928  for (unsigned int i = 0; i < 3; i++) {
929  t_out[i] += rowPtrs[i][j] * tj;
930  }
931  }
932  return t_out;
933 }
934 
940 {
941  vpColVector v_out;
942  vpMatrix::multMatrixVector(*this, v, v_out);
943  return v_out;
944 }
945 
955 {
956  if (A.colNum != v.getRows()) {
957  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
958  A.getRows(), A.getCols(), v.getRows()));
959  }
960 
961  if (A.rowNum != w.rowNum)
962  w.resize(A.rowNum, false);
963 
964  // If available use Lapack only for large matrices
965  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size);
966 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
967  useLapack = false;
968 #endif
969 
970  if (useLapack) {
971 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
972  double alpha = 1.0;
973  double beta = 0.0;
974  char trans = 't';
975  int incr = 1;
976 
977  vpMatrix::blas_dgemv(trans, A.colNum, A.rowNum, alpha, A.data, A.colNum, v.data, incr, beta, w.data, incr);
978 #endif
979  } else {
980  w = 0.0;
981  for (unsigned int j = 0; j < A.colNum; j++) {
982  double vj = v[j]; // optimization em 5/12/2006
983  for (unsigned int i = 0; i < A.rowNum; i++) {
984  w[i] += A.rowPtrs[i][j] * vj;
985  }
986  }
987  }
988 }
989 
990 //---------------------------------
991 // Matrix operations.
992 //---------------------------------
993 
1004 {
1005  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1006  C.resize(A.rowNum, B.colNum, false, false);
1007 
1008  if (A.colNum != B.rowNum) {
1009  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (%dx%d) matrix", A.getRows(),
1010  A.getCols(), B.getRows(), B.getCols()));
1011  }
1012 
1013  // If available use Lapack only for large matrices
1014  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size ||
1015  B.colNum > vpMatrix::m_lapack_min_size);
1016 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1017  useLapack = false;
1018 #endif
1019 
1020  if (useLapack) {
1021 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1022  const double alpha = 1.0;
1023  const double beta = 0.0;
1024  const char trans = 'n';
1025  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1026  C.data, B.colNum);
1027 #endif
1028  } else {
1029  // 5/12/06 some "very" simple optimization to avoid indexation
1030  const unsigned int BcolNum = B.colNum;
1031  const unsigned int BrowNum = B.rowNum;
1032  double **BrowPtrs = B.rowPtrs;
1033  for (unsigned int i = 0; i < A.rowNum; i++) {
1034  const double *rowptri = A.rowPtrs[i];
1035  double *ci = C[i];
1036  for (unsigned int j = 0; j < BcolNum; j++) {
1037  double s = 0;
1038  for (unsigned int k = 0; k < BrowNum; k++)
1039  s += rowptri[k] * BrowPtrs[k][j];
1040  ci[j] = s;
1041  }
1042  }
1043  }
1044 }
1045 
1060 {
1061  if (A.colNum != 3 || A.rowNum != 3 || B.colNum != 3 || B.rowNum != 3) {
1063  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1064  "rotation matrix",
1065  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1066  }
1067  // 5/12/06 some "very" simple optimization to avoid indexation
1068  const unsigned int BcolNum = B.colNum;
1069  const unsigned int BrowNum = B.rowNum;
1070  double **BrowPtrs = B.rowPtrs;
1071  for (unsigned int i = 0; i < A.rowNum; i++) {
1072  const double *rowptri = A.rowPtrs[i];
1073  double *ci = C[i];
1074  for (unsigned int j = 0; j < BcolNum; j++) {
1075  double s = 0;
1076  for (unsigned int k = 0; k < BrowNum; k++)
1077  s += rowptri[k] * BrowPtrs[k][j];
1078  ci[j] = s;
1079  }
1080  }
1081 }
1082 
1097 {
1098  if (A.colNum != 4 || A.rowNum != 4 || B.colNum != 4 || B.rowNum != 4) {
1100  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1101  "rotation matrix",
1102  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1103  }
1104  // Considering perfMatrixMultiplication.cpp benchmark,
1105  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1106  // Lapack usage needs to be validated again.
1107  // If available use Lapack only for large matrices.
1108  // Using SSE2 doesn't speed up.
1109  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size ||
1110  B.colNum > vpMatrix::m_lapack_min_size);
1111 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1112  useLapack = false;
1113 #endif
1114 
1115  if (useLapack) {
1116 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1117  const double alpha = 1.0;
1118  const double beta = 0.0;
1119  const char trans = 'n';
1120  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1121  C.data, B.colNum);
1122 #endif
1123  } else {
1124  // 5/12/06 some "very" simple optimization to avoid indexation
1125  const unsigned int BcolNum = B.colNum;
1126  const unsigned int BrowNum = B.rowNum;
1127  double **BrowPtrs = B.rowPtrs;
1128  for (unsigned int i = 0; i < A.rowNum; i++) {
1129  const double *rowptri = A.rowPtrs[i];
1130  double *ci = C[i];
1131  for (unsigned int j = 0; j < BcolNum; j++) {
1132  double s = 0;
1133  for (unsigned int k = 0; k < BrowNum; k++)
1134  s += rowptri[k] * BrowPtrs[k][j];
1135  ci[j] = s;
1136  }
1137  }
1138  }
1139 }
1140 
1154 {
1155  vpMatrix::multMatrixVector(A, B, C);
1156 }
1157 
1163 {
1164  vpMatrix C;
1165 
1166  vpMatrix::mult2Matrices(*this, B, C);
1167 
1168  return C;
1169 }
1170 
1176 {
1177  if (colNum != R.getRows()) {
1178  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1179  colNum));
1180  }
1181  vpMatrix C;
1182  C.resize(rowNum, 3, false, false);
1183 
1184  unsigned int RcolNum = R.getCols();
1185  unsigned int RrowNum = R.getRows();
1186  for (unsigned int i = 0; i < rowNum; i++) {
1187  double *rowptri = rowPtrs[i];
1188  double *ci = C[i];
1189  for (unsigned int j = 0; j < RcolNum; j++) {
1190  double s = 0;
1191  for (unsigned int k = 0; k < RrowNum; k++)
1192  s += rowptri[k] * R[k][j];
1193  ci[j] = s;
1194  }
1195  }
1196 
1197  return C;
1198 }
1199 
1205 {
1206  if (colNum != M.getRows()) {
1207  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1208  colNum));
1209  }
1210  vpMatrix C;
1211  C.resize(rowNum, 4, false, false);
1212 
1213  const unsigned int McolNum = M.getCols();
1214  const unsigned int MrowNum = M.getRows();
1215  for (unsigned int i = 0; i < rowNum; i++) {
1216  const double *rowptri = rowPtrs[i];
1217  double *ci = C[i];
1218  for (unsigned int j = 0; j < McolNum; j++) {
1219  double s = 0;
1220  for (unsigned int k = 0; k < MrowNum; k++)
1221  s += rowptri[k] * M[k][j];
1222  ci[j] = s;
1223  }
1224  }
1225 
1226  return C;
1227 }
1228 
1234 {
1235  if (colNum != V.getRows()) {
1236  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix",
1237  rowNum, colNum));
1238  }
1239  vpMatrix M;
1240  M.resize(rowNum, 6, false, false);
1241 
1242  // Considering perfMatrixMultiplication.cpp benchmark,
1243  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1244  // Lapack usage needs to be validated again.
1245  // If available use Lapack only for large matrices.
1246  // Speed up obtained using SSE2.
1247  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size ||
1248  V.colNum > vpMatrix::m_lapack_min_size);
1249 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1250  useLapack = false;
1251 #endif
1252 
1253  if (useLapack) {
1254 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1255  const double alpha = 1.0;
1256  const double beta = 0.0;
1257  const char trans = 'n';
1258  vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, V.colNum, data, colNum, beta, M.data,
1259  M.colNum);
1260 #endif
1261  } else {
1262  SimdMatMulTwist(data, rowNum, V.data, M.data);
1263  }
1264 
1265  return M;
1266 }
1267 
1273 {
1274  if (colNum != V.getRows()) {
1275  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) force/torque twist matrix",
1276  rowNum, colNum));
1277  }
1278  vpMatrix M;
1279  M.resize(rowNum, 6, false, false);
1280 
1281  // Considering perfMatrixMultiplication.cpp benchmark,
1282  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1283  // Lapack usage needs to be validated again.
1284  // If available use Lapack only for large matrices.
1285  // Speed up obtained using SSE2.
1286  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size ||
1287  V.getCols() > vpMatrix::m_lapack_min_size);
1288 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1289  useLapack = false;
1290 #endif
1291 
1292  if (useLapack) {
1293 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1294  const double alpha = 1.0;
1295  const double beta = 0.0;
1296  const char trans = 'n';
1297  vpMatrix::blas_dgemm(trans, trans, V.getCols(), rowNum, colNum, alpha, V.data, V.getCols(), data, colNum, beta,
1298  M.data, M.colNum);
1299 #endif
1300  } else {
1301  SimdMatMulTwist(data, rowNum, V.data, M.data);
1302  }
1303 
1304  return M;
1305 }
1306 
1317 void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB,
1318  vpMatrix &C)
1319 {
1320  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1321  C.resize(A.rowNum, B.colNum, false, false);
1322 
1323  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1324  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1325  A.getCols(), B.getRows(), B.getCols()));
1326  }
1327 
1328  double **ArowPtrs = A.rowPtrs;
1329  double **BrowPtrs = B.rowPtrs;
1330  double **CrowPtrs = C.rowPtrs;
1331 
1332  for (unsigned int i = 0; i < A.rowNum; i++)
1333  for (unsigned int j = 0; j < A.colNum; j++)
1334  CrowPtrs[i][j] = wB * BrowPtrs[i][j] + wA * ArowPtrs[i][j];
1335 }
1336 
1346 void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1347 {
1348  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1349  C.resize(A.rowNum, B.colNum, false, false);
1350 
1351  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1352  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1353  A.getCols(), B.getRows(), B.getCols()));
1354  }
1355 
1356  double **ArowPtrs = A.rowPtrs;
1357  double **BrowPtrs = B.rowPtrs;
1358  double **CrowPtrs = C.rowPtrs;
1359 
1360  for (unsigned int i = 0; i < A.rowNum; i++) {
1361  for (unsigned int j = 0; j < A.colNum; j++) {
1362  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1363  }
1364  }
1365 }
1366 
1380 {
1381  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1382  C.resize(A.rowNum);
1383 
1384  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1385  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1386  A.getCols(), B.getRows(), B.getCols()));
1387  }
1388 
1389  double **ArowPtrs = A.rowPtrs;
1390  double **BrowPtrs = B.rowPtrs;
1391  double **CrowPtrs = C.rowPtrs;
1392 
1393  for (unsigned int i = 0; i < A.rowNum; i++) {
1394  for (unsigned int j = 0; j < A.colNum; j++) {
1395  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1396  }
1397  }
1398 }
1399 
1405 {
1406  vpMatrix C;
1407  vpMatrix::add2Matrices(*this, B, C);
1408  return C;
1409 }
1410 
1427 {
1428  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1429  C.resize(A.rowNum);
1430 
1431  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1432  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1433  A.getCols(), B.getRows(), B.getCols()));
1434  }
1435 
1436  double **ArowPtrs = A.rowPtrs;
1437  double **BrowPtrs = B.rowPtrs;
1438  double **CrowPtrs = C.rowPtrs;
1439 
1440  for (unsigned int i = 0; i < A.rowNum; i++) {
1441  for (unsigned int j = 0; j < A.colNum; j++) {
1442  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1443  }
1444  }
1445 }
1446 
1459 void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1460 {
1461  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1462  C.resize(A.rowNum, A.colNum, false, false);
1463 
1464  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1465  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1466  A.getCols(), B.getRows(), B.getCols()));
1467  }
1468 
1469  double **ArowPtrs = A.rowPtrs;
1470  double **BrowPtrs = B.rowPtrs;
1471  double **CrowPtrs = C.rowPtrs;
1472 
1473  for (unsigned int i = 0; i < A.rowNum; i++) {
1474  for (unsigned int j = 0; j < A.colNum; j++) {
1475  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1476  }
1477  }
1478 }
1479 
1485 {
1486  vpMatrix C;
1487  vpMatrix::sub2Matrices(*this, B, C);
1488  return C;
1489 }
1490 
1492 
1494 {
1495  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1496  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1497  B.getRows(), B.getCols()));
1498  }
1499 
1500  double **BrowPtrs = B.rowPtrs;
1501 
1502  for (unsigned int i = 0; i < rowNum; i++)
1503  for (unsigned int j = 0; j < colNum; j++)
1504  rowPtrs[i][j] += BrowPtrs[i][j];
1505 
1506  return *this;
1507 }
1508 
1511 {
1512  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1513  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1514  B.getRows(), B.getCols()));
1515  }
1516 
1517  double **BrowPtrs = B.rowPtrs;
1518  for (unsigned int i = 0; i < rowNum; i++)
1519  for (unsigned int j = 0; j < colNum; j++)
1520  rowPtrs[i][j] -= BrowPtrs[i][j];
1521 
1522  return *this;
1523 }
1524 
1535 {
1536  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1537  C.resize(A.rowNum, A.colNum, false, false);
1538 
1539  double **ArowPtrs = A.rowPtrs;
1540  double **CrowPtrs = C.rowPtrs;
1541 
1542  // t0=vpTime::measureTimeMicros();
1543  for (unsigned int i = 0; i < A.rowNum; i++)
1544  for (unsigned int j = 0; j < A.colNum; j++)
1545  CrowPtrs[i][j] = -ArowPtrs[i][j];
1546 }
1547 
1553 {
1554  vpMatrix C;
1555  vpMatrix::negateMatrix(*this, C);
1556  return C;
1557 }
1558 
1559 double vpMatrix::sum() const
1560 {
1561  double s = 0.0;
1562  for (unsigned int i = 0; i < rowNum; i++) {
1563  for (unsigned int j = 0; j < colNum; j++) {
1564  s += rowPtrs[i][j];
1565  }
1566  }
1567 
1568  return s;
1569 }
1570 
1571 //---------------------------------
1572 // Matrix/vector operations.
1573 //---------------------------------
1574 
1575 //---------------------------------
1576 // Matrix/real operations.
1577 //---------------------------------
1578 
1583 vpMatrix operator*(const double &x, const vpMatrix &B)
1584 {
1585  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1586  return B;
1587  }
1588 
1589  unsigned int Brow = B.getRows();
1590  unsigned int Bcol = B.getCols();
1591 
1592  vpMatrix C;
1593  C.resize(Brow, Bcol, false, false);
1594 
1595  for (unsigned int i = 0; i < Brow; i++)
1596  for (unsigned int j = 0; j < Bcol; j++)
1597  C[i][j] = B[i][j] * x;
1598 
1599  return C;
1600 }
1601 
1607 {
1608  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1609  return (*this);
1610  }
1611 
1612  vpMatrix M;
1613  M.resize(rowNum, colNum, false, false);
1614 
1615  for (unsigned int i = 0; i < rowNum; i++)
1616  for (unsigned int j = 0; j < colNum; j++)
1617  M[i][j] = rowPtrs[i][j] * x;
1618 
1619  return M;
1620 }
1621 
1624 {
1625  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1626  return (*this);
1627  }
1628 
1629  if (std::fabs(x) < std::numeric_limits<double>::epsilon()) {
1630  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1631  }
1632 
1633  vpMatrix C;
1634  C.resize(rowNum, colNum, false, false);
1635 
1636  double xinv = 1 / x;
1637 
1638  for (unsigned int i = 0; i < rowNum; i++)
1639  for (unsigned int j = 0; j < colNum; j++)
1640  C[i][j] = rowPtrs[i][j] * xinv;
1641 
1642  return C;
1643 }
1644 
1647 {
1648  for (unsigned int i = 0; i < rowNum; i++)
1649  for (unsigned int j = 0; j < colNum; j++)
1650  rowPtrs[i][j] += x;
1651 
1652  return *this;
1653 }
1654 
1657 {
1658  for (unsigned int i = 0; i < rowNum; i++)
1659  for (unsigned int j = 0; j < colNum; j++)
1660  rowPtrs[i][j] -= x;
1661 
1662  return *this;
1663 }
1664 
1670 {
1671  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1672  return *this;
1673  }
1674 
1675  for (unsigned int i = 0; i < rowNum; i++)
1676  for (unsigned int j = 0; j < colNum; j++)
1677  rowPtrs[i][j] *= x;
1678 
1679  return *this;
1680 }
1681 
1684 {
1685  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1686  return *this;
1687  }
1688 
1689  if (std::fabs(x) < std::numeric_limits<double>::epsilon())
1690  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1691 
1692  double xinv = 1 / x;
1693 
1694  for (unsigned int i = 0; i < rowNum; i++)
1695  for (unsigned int j = 0; j < colNum; j++)
1696  rowPtrs[i][j] *= xinv;
1697 
1698  return *this;
1699 }
1700 
1701 //----------------------------------------------------------------
1702 // Matrix Operation
1703 //----------------------------------------------------------------
1704 
1710 {
1711  if ((out.rowNum != colNum * rowNum) || (out.colNum != 1))
1712  out.resize(colNum * rowNum, false, false);
1713 
1714  double *optr = out.data;
1715  for (unsigned int j = 0; j < colNum; j++) {
1716  for (unsigned int i = 0; i < rowNum; i++) {
1717  *(optr++) = rowPtrs[i][j];
1718  }
1719  }
1720 }
1721 
1727 {
1728  vpColVector out(colNum * rowNum);
1729  stackColumns(out);
1730  return out;
1731 }
1732 
1738 {
1739  if ((out.getRows() != 1) || (out.getCols() != colNum * rowNum))
1740  out.resize(colNum * rowNum, false, false);
1741 
1742  memcpy(out.data, data, sizeof(double) * out.getCols());
1743 }
1749 {
1750  vpRowVector out(colNum * rowNum);
1751  stackRows(out);
1752  return out;
1753 }
1754 
1762 {
1763  if (m.getRows() != rowNum || m.getCols() != colNum) {
1764  throw(vpException(vpException::dimensionError, "In Hadamard product: bad dimension of input matrix"));
1765  }
1766 
1767  vpMatrix out;
1768  out.resize(rowNum, colNum, false, false);
1769 
1770  SimdVectorHadamard(data, m.data, dsize, out.data);
1771 
1772  return out;
1773 }
1774 
1781 void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out)
1782 {
1783  unsigned int r1 = m1.getRows();
1784  unsigned int c1 = m1.getCols();
1785  unsigned int r2 = m2.getRows();
1786  unsigned int c2 = m2.getCols();
1787 
1788  out.resize(r1 * r2, c1 * c2, false, false);
1789 
1790  for (unsigned int r = 0; r < r1; r++) {
1791  for (unsigned int c = 0; c < c1; c++) {
1792  double alpha = m1[r][c];
1793  double *m2ptr = m2[0];
1794  unsigned int roffset = r * r2;
1795  unsigned int coffset = c * c2;
1796  for (unsigned int rr = 0; rr < r2; rr++) {
1797  for (unsigned int cc = 0; cc < c2; cc++) {
1798  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1799  }
1800  }
1801  }
1802  }
1803 }
1804 
1811 void vpMatrix::kron(const vpMatrix &m, vpMatrix &out) const { kron(*this, m, out); }
1812 
1820 {
1821  unsigned int r1 = m1.getRows();
1822  unsigned int c1 = m1.getCols();
1823  unsigned int r2 = m2.getRows();
1824  unsigned int c2 = m2.getCols();
1825 
1826  vpMatrix out;
1827  out.resize(r1 * r2, c1 * c2, false, false);
1828 
1829  for (unsigned int r = 0; r < r1; r++) {
1830  for (unsigned int c = 0; c < c1; c++) {
1831  double alpha = m1[r][c];
1832  double *m2ptr = m2[0];
1833  unsigned int roffset = r * r2;
1834  unsigned int coffset = c * c2;
1835  for (unsigned int rr = 0; rr < r2; rr++) {
1836  for (unsigned int cc = 0; cc < c2; cc++) {
1837  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1838  }
1839  }
1840  }
1841  }
1842  return out;
1843 }
1844 
1850 vpMatrix vpMatrix::kron(const vpMatrix &m) const { return kron(*this, m); }
1851 
1902 void vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const { x = pseudoInverse(1e-6) * b; }
1903 
1954 {
1955  vpColVector X(colNum);
1956 
1957  solveBySVD(B, X);
1958  return X;
1959 }
1960 
2025 {
2026 #if defined(VISP_HAVE_LAPACK)
2027  svdLapack(w, V);
2028 #elif defined(VISP_HAVE_EIGEN3)
2029  svdEigen3(w, V);
2030 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2031  svdOpenCV(w, V);
2032 #else
2033  (void)w;
2034  (void)V;
2035  throw(vpException(vpException::fatalError, "Cannot compute SVD. Install Lapack, Eigen3 or OpenCV 3rd party"));
2036 #endif
2037 }
2038 
2093 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
2094 {
2095 #if defined(VISP_HAVE_LAPACK)
2096  return pseudoInverseLapack(Ap, svThreshold);
2097 #elif defined(VISP_HAVE_EIGEN3)
2098  return pseudoInverseEigen3(Ap, svThreshold);
2099 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2100  return pseudoInverseOpenCV(Ap, svThreshold);
2101 #else
2102  (void)Ap;
2103  (void)svThreshold;
2104  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2105  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2106 #endif
2107 }
2108 
2169 int vpMatrix::pseudoInverse(vpMatrix &Ap, int rank_in) const
2170 {
2171 #if defined(VISP_HAVE_LAPACK)
2172  return pseudoInverseLapack(Ap, rank_in);
2173 #elif defined(VISP_HAVE_EIGEN3)
2174  return pseudoInverseEigen3(Ap, rank_in);
2175 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2176  return pseudoInverseOpenCV(Ap, rank_in);
2177 #else
2178  (void)Ap;
2179  (void)svThreshold;
2180  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2181  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2182 #endif
2183 }
2184 
2235 vpMatrix vpMatrix::pseudoInverse(double svThreshold) const
2236 {
2237 #if defined(VISP_HAVE_LAPACK)
2238  return pseudoInverseLapack(svThreshold);
2239 #elif defined(VISP_HAVE_EIGEN3)
2240  return pseudoInverseEigen3(svThreshold);
2241 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2242  return pseudoInverseOpenCV(svThreshold);
2243 #else
2244  (void)svThreshold;
2245  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2246  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2247 #endif
2248 }
2249 
2301 {
2302 #if defined(VISP_HAVE_LAPACK)
2303  return pseudoInverseLapack(rank_in);
2304 #elif defined(VISP_HAVE_EIGEN3)
2305  return pseudoInverseEigen3(rank_in);
2306 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2307  return pseudoInverseOpenCV(rank_in);
2308 #else
2309  (void)svThreshold;
2310  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2311  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2312 #endif
2313 }
2314 
2315 #ifndef DOXYGEN_SHOULD_SKIP_THIS
2316 #if defined(VISP_HAVE_LAPACK)
2353 vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const
2354 {
2355  unsigned int nrows = getRows();
2356  unsigned int ncols = getCols();
2357  int rank_out;
2358 
2359  vpMatrix U, V, Ap;
2360  vpColVector sv;
2361 
2362  Ap.resize(ncols, nrows, false, false);
2363 
2364  if (nrows < ncols) {
2365  U.resize(ncols, ncols, true);
2366  sv.resize(nrows, false);
2367  } else {
2368  U.resize(nrows, ncols, false);
2369  sv.resize(ncols, false);
2370  }
2371 
2372  U.insert(*this, 0, 0);
2373  U.svdLapack(sv, V);
2374 
2375  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
2376 
2377  return Ap;
2378 }
2379 
2420 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) const
2421 {
2422  unsigned int nrows = getRows();
2423  unsigned int ncols = getCols();
2424  int rank_out;
2425 
2426  vpMatrix U, V;
2427  vpColVector sv;
2428 
2429  Ap.resize(ncols, nrows, false, false);
2430 
2431  if (nrows < ncols) {
2432  U.resize(ncols, ncols, true);
2433  sv.resize(nrows, false);
2434  } else {
2435  U.resize(nrows, ncols, false);
2436  sv.resize(ncols, false);
2437  }
2438 
2439  U.insert(*this, 0, 0);
2440  U.svdLapack(sv, V);
2441 
2442  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
2443 
2444  return static_cast<unsigned int>(rank_out);
2445 }
2493 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2494 {
2495  unsigned int nrows = getRows();
2496  unsigned int ncols = getCols();
2497  int rank_out;
2498 
2499  vpMatrix U, V;
2500  vpColVector sv_;
2501 
2502  Ap.resize(ncols, nrows, false, false);
2503 
2504  if (nrows < ncols) {
2505  U.resize(ncols, ncols, true);
2506  sv.resize(nrows, false);
2507  } else {
2508  U.resize(nrows, ncols, false);
2509  sv.resize(ncols, false);
2510  }
2511 
2512  U.insert(*this, 0, 0);
2513  U.svdLapack(sv_, V);
2514 
2515  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
2516 
2517  // Remove singular values equal to the one that corresponds to the lines of 0
2518  // introduced when m < n
2519  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2520 
2521  return static_cast<unsigned int>(rank_out);
2522 }
2523 
2630 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
2631  vpMatrix &imAt, vpMatrix &kerAt) const
2632 {
2633  unsigned int nrows = getRows();
2634  unsigned int ncols = getCols();
2635  int rank_out;
2636  vpMatrix U, V;
2637  vpColVector sv_;
2638 
2639  if (nrows < ncols) {
2640  U.resize(ncols, ncols, true);
2641  sv.resize(nrows, false);
2642  } else {
2643  U.resize(nrows, ncols, false);
2644  sv.resize(ncols, false);
2645  }
2646 
2647  U.insert(*this, 0, 0);
2648  U.svdLapack(sv_, V);
2649 
2650  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerAt);
2651 
2652  // Remove singular values equal to the one that corresponds to the lines of 0
2653  // introduced when m < n
2654  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2655 
2656  return static_cast<unsigned int>(rank_out);
2657 }
2658 
2695 vpMatrix vpMatrix::pseudoInverseLapack(int rank_in) const
2696 {
2697  unsigned int nrows = getRows();
2698  unsigned int ncols = getCols();
2699  int rank_out;
2700  double svThreshold = 1e-26;
2701 
2702  vpMatrix U, V, Ap;
2703  vpColVector sv;
2704 
2705  Ap.resize(ncols, nrows, false, false);
2706 
2707  if (nrows < ncols) {
2708  U.resize(ncols, ncols, true);
2709  sv.resize(nrows, false);
2710  } else {
2711  U.resize(nrows, ncols, false);
2712  sv.resize(ncols, false);
2713  }
2714 
2715  U.insert(*this, 0, 0);
2716  U.svdLapack(sv, V);
2717 
2718  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
2719 
2720  return Ap;
2721 }
2722 
2769 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, int rank_in) const
2770 {
2771  unsigned int nrows = getRows();
2772  unsigned int ncols = getCols();
2773  int rank_out;
2774  double svThreshold = 1e-26;
2775 
2776  vpMatrix U, V;
2777  vpColVector sv;
2778 
2779  Ap.resize(ncols, nrows, false, false);
2780 
2781  if (nrows < ncols) {
2782  U.resize(ncols, ncols, true);
2783  sv.resize(nrows, false);
2784  } else {
2785  U.resize(nrows, ncols, false);
2786  sv.resize(ncols, false);
2787  }
2788 
2789  U.insert(*this, 0, 0);
2790  U.svdLapack(sv, V);
2791 
2792  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
2793 
2794  return rank_out;
2795 }
2796 
2850 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in) const
2851 {
2852  unsigned int nrows = getRows();
2853  unsigned int ncols = getCols();
2854  int rank_out;
2855  double svThreshold = 1e-26;
2856 
2857  vpMatrix U, V;
2858  vpColVector sv_;
2859 
2860  Ap.resize(ncols, nrows, false, false);
2861 
2862  if (nrows < ncols) {
2863  U.resize(ncols, ncols, true);
2864  sv.resize(nrows, false);
2865  } else {
2866  U.resize(nrows, ncols, false);
2867  sv.resize(ncols, false);
2868  }
2869 
2870  U.insert(*this, 0, 0);
2871  U.svdLapack(sv_, V);
2872 
2873  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
2874 
2875  // Remove singular values equal to the one that corresponds to the lines of 0
2876  // introduced when m < n
2877  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2878 
2879  return rank_out;
2880 }
2881 
2993 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
2994  vpMatrix &kerAt) const
2995 {
2996  unsigned int nrows = getRows();
2997  unsigned int ncols = getCols();
2998  int rank_out;
2999  double svThreshold = 1e-26;
3000  vpMatrix U, V;
3001  vpColVector sv_;
3002 
3003  if (nrows < ncols) {
3004  U.resize(ncols, ncols, true);
3005  sv.resize(nrows, false);
3006  } else {
3007  U.resize(nrows, ncols, false);
3008  sv.resize(ncols, false);
3009  }
3010 
3011  U.insert(*this, 0, 0);
3012  U.svdLapack(sv_, V);
3013 
3014  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
3015 
3016  // Remove singular values equal to the one that corresponds to the lines of 0
3017  // introduced when m < n
3018  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3019 
3020  return rank_out;
3021 }
3022 
3023 #endif
3024 #if defined(VISP_HAVE_EIGEN3)
3061 vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const
3062 {
3063  unsigned int nrows = getRows();
3064  unsigned int ncols = getCols();
3065  int rank_out;
3066 
3067  vpMatrix U, V, Ap;
3068  vpColVector sv;
3069 
3070  Ap.resize(ncols, nrows, false, false);
3071 
3072  if (nrows < ncols) {
3073  U.resize(ncols, ncols, true);
3074  sv.resize(nrows, false);
3075  } else {
3076  U.resize(nrows, ncols, false);
3077  sv.resize(ncols, false);
3078  }
3079 
3080  U.insert(*this, 0, 0);
3081  U.svdEigen3(sv, V);
3082 
3083  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3084 
3085  return Ap;
3086 }
3087 
3128 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) const
3129 {
3130  unsigned int nrows = getRows();
3131  unsigned int ncols = getCols();
3132  int rank_out;
3133 
3134  vpMatrix U, V;
3135  vpColVector sv;
3136 
3137  Ap.resize(ncols, nrows, false, false);
3138 
3139  if (nrows < ncols) {
3140  U.resize(ncols, ncols, true);
3141  sv.resize(nrows, false);
3142  } else {
3143  U.resize(nrows, ncols, false);
3144  sv.resize(ncols, false);
3145  }
3146 
3147  U.insert(*this, 0, 0);
3148  U.svdEigen3(sv, V);
3149 
3150  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3151 
3152  return static_cast<unsigned int>(rank_out);
3153 }
3201 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3202 {
3203  unsigned int nrows = getRows();
3204  unsigned int ncols = getCols();
3205  int rank_out;
3206 
3207  vpMatrix U, V;
3208  vpColVector sv_;
3209 
3210  Ap.resize(ncols, nrows, false, false);
3211 
3212  if (nrows < ncols) {
3213  U.resize(ncols, ncols, true);
3214  sv.resize(nrows, false);
3215  } else {
3216  U.resize(nrows, ncols, false);
3217  sv.resize(ncols, false);
3218  }
3219 
3220  U.insert(*this, 0, 0);
3221  U.svdEigen3(sv_, V);
3222 
3223  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3224 
3225  // Remove singular values equal to the one that corresponds to the lines of 0
3226  // introduced when m < n
3227  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3228 
3229  return static_cast<unsigned int>(rank_out);
3230 }
3231 
3338 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3339  vpMatrix &imAt, vpMatrix &kerAt) const
3340 {
3341  unsigned int nrows = getRows();
3342  unsigned int ncols = getCols();
3343  int rank_out;
3344  vpMatrix U, V;
3345  vpColVector sv_;
3346 
3347  if (nrows < ncols) {
3348  U.resize(ncols, ncols, true);
3349  sv.resize(nrows, false);
3350  } else {
3351  U.resize(nrows, ncols, false);
3352  sv.resize(ncols, false);
3353  }
3354 
3355  U.insert(*this, 0, 0);
3356  U.svdEigen3(sv_, V);
3357 
3358  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerAt);
3359 
3360  // Remove singular values equal to the one that corresponds to the lines of 0
3361  // introduced when m < n
3362  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3363 
3364  return static_cast<unsigned int>(rank_out);
3365 }
3366 
3403 vpMatrix vpMatrix::pseudoInverseEigen3(int rank_in) const
3404 {
3405  unsigned int nrows = getRows();
3406  unsigned int ncols = getCols();
3407  int rank_out;
3408  double svThreshold = 1e-26;
3409 
3410  vpMatrix U, V, Ap;
3411  vpColVector sv;
3412 
3413  Ap.resize(ncols, nrows, false, false);
3414 
3415  if (nrows < ncols) {
3416  U.resize(ncols, ncols, true);
3417  sv.resize(nrows, false);
3418  } else {
3419  U.resize(nrows, ncols, false);
3420  sv.resize(ncols, false);
3421  }
3422 
3423  U.insert(*this, 0, 0);
3424  U.svdEigen3(sv, V);
3425 
3426  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3427 
3428  return Ap;
3429 }
3430 
3477 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, int rank_in) const
3478 {
3479  unsigned int nrows = getRows();
3480  unsigned int ncols = getCols();
3481  int rank_out;
3482  double svThreshold = 1e-26;
3483 
3484  vpMatrix U, V;
3485  vpColVector sv;
3486 
3487  Ap.resize(ncols, nrows, false, false);
3488 
3489  if (nrows < ncols) {
3490  U.resize(ncols, ncols, true);
3491  sv.resize(nrows, false);
3492  } else {
3493  U.resize(nrows, ncols, false);
3494  sv.resize(ncols, false);
3495  }
3496 
3497  U.insert(*this, 0, 0);
3498  U.svdEigen3(sv, V);
3499 
3500  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3501 
3502  return rank_out;
3503 }
3504 
3558 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in) const
3559 {
3560  unsigned int nrows = getRows();
3561  unsigned int ncols = getCols();
3562  int rank_out;
3563  double svThreshold = 1e-26;
3564 
3565  vpMatrix U, V;
3566  vpColVector sv_;
3567 
3568  Ap.resize(ncols, nrows, false, false);
3569 
3570  if (nrows < ncols) {
3571  U.resize(ncols, ncols, true);
3572  sv.resize(nrows, false);
3573  } else {
3574  U.resize(nrows, ncols, false);
3575  sv.resize(ncols, false);
3576  }
3577 
3578  U.insert(*this, 0, 0);
3579  U.svdEigen3(sv_, V);
3580 
3581  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3582 
3583  // Remove singular values equal to the one that corresponds to the lines of 0
3584  // introduced when m < n
3585  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3586 
3587  return rank_out;
3588 }
3589 
3701 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
3702  vpMatrix &kerAt) const
3703 {
3704  unsigned int nrows = getRows();
3705  unsigned int ncols = getCols();
3706  int rank_out;
3707  double svThreshold = 1e-26;
3708  vpMatrix U, V;
3709  vpColVector sv_;
3710 
3711  if (nrows < ncols) {
3712  U.resize(ncols, ncols, true);
3713  sv.resize(nrows, false);
3714  } else {
3715  U.resize(nrows, ncols, false);
3716  sv.resize(ncols, false);
3717  }
3718 
3719  U.insert(*this, 0, 0);
3720  U.svdEigen3(sv_, V);
3721 
3722  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
3723 
3724  // Remove singular values equal to the one that corresponds to the lines of 0
3725  // introduced when m < n
3726  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3727 
3728  return rank_out;
3729 }
3730 
3731 #endif
3732 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
3769 vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const
3770 {
3771  unsigned int nrows = getRows();
3772  unsigned int ncols = getCols();
3773  int rank_out;
3774 
3775  vpMatrix U, V, Ap;
3776  vpColVector sv;
3777 
3778  Ap.resize(ncols, nrows, false, false);
3779 
3780  if (nrows < ncols) {
3781  U.resize(ncols, ncols, true);
3782  sv.resize(nrows, false);
3783  } else {
3784  U.resize(nrows, ncols, false);
3785  sv.resize(ncols, false);
3786  }
3787 
3788  U.insert(*this, 0, 0);
3789  U.svdOpenCV(sv, V);
3790 
3791  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3792 
3793  return Ap;
3794 }
3795 
3836 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) const
3837 {
3838  unsigned int nrows = getRows();
3839  unsigned int ncols = getCols();
3840  int rank_out;
3841 
3842  vpMatrix U, V;
3843  vpColVector sv;
3844 
3845  Ap.resize(ncols, nrows, false, false);
3846 
3847  if (nrows < ncols) {
3848  U.resize(ncols, ncols, true);
3849  sv.resize(nrows, false);
3850  } else {
3851  U.resize(nrows, ncols, false);
3852  sv.resize(ncols, false);
3853  }
3854 
3855  U.insert(*this, 0, 0);
3856  U.svdOpenCV(sv, V);
3857 
3858  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3859 
3860  return static_cast<unsigned int>(rank_out);
3861 }
3909 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3910 {
3911  unsigned int nrows = getRows();
3912  unsigned int ncols = getCols();
3913  int rank_out;
3914 
3915  vpMatrix U, V;
3916  vpColVector sv_;
3917 
3918  Ap.resize(ncols, nrows, false, false);
3919 
3920  if (nrows < ncols) {
3921  U.resize(ncols, ncols, true);
3922  sv.resize(nrows, false);
3923  } else {
3924  U.resize(nrows, ncols, false);
3925  sv.resize(ncols, false);
3926  }
3927 
3928  U.insert(*this, 0, 0);
3929  U.svdOpenCV(sv_, V);
3930 
3931  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3932 
3933  // Remove singular values equal to the one that corresponds to the lines of 0
3934  // introduced when m < n
3935  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3936 
3937  return static_cast<unsigned int>(rank_out);
3938 }
3939 
4046 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
4047  vpMatrix &imAt, vpMatrix &kerAt) const
4048 {
4049  unsigned int nrows = getRows();
4050  unsigned int ncols = getCols();
4051  int rank_out;
4052  vpMatrix U, V;
4053  vpColVector sv_;
4054 
4055  if (nrows < ncols) {
4056  U.resize(ncols, ncols, true);
4057  sv.resize(nrows, false);
4058  } else {
4059  U.resize(nrows, ncols, false);
4060  sv.resize(ncols, false);
4061  }
4062 
4063  U.insert(*this, 0, 0);
4064  U.svdOpenCV(sv_, V);
4065 
4066  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerAt);
4067 
4068  // Remove singular values equal to the one that corresponds to the lines of 0
4069  // introduced when m < n
4070  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4071 
4072  return static_cast<unsigned int>(rank_out);
4073 }
4074 
4111 vpMatrix vpMatrix::pseudoInverseOpenCV(int rank_in) const
4112 {
4113  unsigned int nrows = getRows();
4114  unsigned int ncols = getCols();
4115  int rank_out;
4116  double svThreshold = 1e-26;
4117 
4118  vpMatrix U, V, Ap;
4119  vpColVector sv;
4120 
4121  Ap.resize(ncols, nrows, false, false);
4122 
4123  if (nrows < ncols) {
4124  U.resize(ncols, ncols, true);
4125  sv.resize(nrows, false);
4126  } else {
4127  U.resize(nrows, ncols, false);
4128  sv.resize(ncols, false);
4129  }
4130 
4131  U.insert(*this, 0, 0);
4132  U.svdOpenCV(sv, V);
4133 
4134  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4135 
4136  return Ap;
4137 }
4138 
4185 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, int rank_in) const
4186 {
4187  unsigned int nrows = getRows();
4188  unsigned int ncols = getCols();
4189  int rank_out;
4190  double svThreshold = 1e-26;
4191 
4192  vpMatrix U, V;
4193  vpColVector sv;
4194 
4195  Ap.resize(ncols, nrows, false, false);
4196 
4197  if (nrows < ncols) {
4198  U.resize(ncols, ncols, true);
4199  sv.resize(nrows, false);
4200  } else {
4201  U.resize(nrows, ncols, false);
4202  sv.resize(ncols, false);
4203  }
4204 
4205  U.insert(*this, 0, 0);
4206  U.svdOpenCV(sv, V);
4207 
4208  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4209 
4210  return rank_out;
4211 }
4212 
4266 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4267 {
4268  unsigned int nrows = getRows();
4269  unsigned int ncols = getCols();
4270  int rank_out;
4271  double svThreshold = 1e-26;
4272 
4273  vpMatrix U, V;
4274  vpColVector sv_;
4275 
4276  Ap.resize(ncols, nrows, false, false);
4277 
4278  if (nrows < ncols) {
4279  U.resize(ncols, ncols, true);
4280  sv.resize(nrows, false);
4281  } else {
4282  U.resize(nrows, ncols, false);
4283  sv.resize(ncols, false);
4284  }
4285 
4286  U.insert(*this, 0, 0);
4287  U.svdOpenCV(sv_, V);
4288 
4289  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4290 
4291  // Remove singular values equal to the one that corresponds to the lines of 0
4292  // introduced when m < n
4293  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4294 
4295  return rank_out;
4296 }
4297 
4409 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
4410  vpMatrix &kerAt) const
4411 {
4412  unsigned int nrows = getRows();
4413  unsigned int ncols = getCols();
4414  int rank_out;
4415  double svThreshold = 1e-26;
4416  vpMatrix U, V;
4417  vpColVector sv_;
4418 
4419  if (nrows < ncols) {
4420  U.resize(ncols, ncols, true);
4421  sv.resize(nrows, false);
4422  } else {
4423  U.resize(nrows, ncols, false);
4424  sv.resize(ncols, false);
4425  }
4426 
4427  U.insert(*this, 0, 0);
4428  U.svdOpenCV(sv_, V);
4429 
4430  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
4431 
4432  // Remove singular values equal to the one that corresponds to the lines of 0
4433  // introduced when m < n
4434  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4435 
4436  return rank_out;
4437 }
4438 
4439 #endif
4440 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
4441 
4503 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
4504 {
4505 #if defined(VISP_HAVE_LAPACK)
4506  return pseudoInverseLapack(Ap, sv, svThreshold);
4507 #elif defined(VISP_HAVE_EIGEN3)
4508  return pseudoInverseEigen3(Ap, sv, svThreshold);
4509 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
4510  return pseudoInverseOpenCV(Ap, sv, svThreshold);
4511 #else
4512  (void)Ap;
4513  (void)sv;
4514  (void)svThreshold;
4515  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4516  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4517 #endif
4518 }
4519 
4586 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4587 {
4588 #if defined(VISP_HAVE_LAPACK)
4589  return pseudoInverseLapack(Ap, sv, rank_in);
4590 #elif defined(VISP_HAVE_EIGEN3)
4591  return pseudoInverseEigen3(Ap, sv, rank_in);
4592 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
4593  return pseudoInverseOpenCV(Ap, sv, rank_in);
4594 #else
4595  (void)Ap;
4596  (void)sv;
4597  (void)svThreshold;
4598  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4599  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4600 #endif
4601 }
4676 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
4677  vpMatrix &imAt) const
4678 {
4679  vpMatrix kerAt;
4680  return pseudoInverse(Ap, sv, svThreshold, imA, imAt, kerAt);
4681 }
4682 
4761 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt) const
4762 {
4763  vpMatrix kerAt;
4764  return pseudoInverse(Ap, sv, rank_in, imA, imAt, kerAt);
4765 }
4766 
4901 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt,
4902  vpMatrix &kerAt) const
4903 {
4904 #if defined(VISP_HAVE_LAPACK)
4905  return pseudoInverseLapack(Ap, sv, svThreshold, imA, imAt, kerAt);
4906 #elif defined(VISP_HAVE_EIGEN3)
4907  return pseudoInverseEigen3(Ap, sv, svThreshold, imA, imAt, kerAt);
4908 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
4909  return pseudoInverseOpenCV(Ap, sv, svThreshold, imA, imAt, kerAt);
4910 #else
4911  (void)Ap;
4912  (void)sv;
4913  (void)svThreshold;
4914  (void)imA;
4915  (void)imAt;
4916  (void)kerAt;
4917  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4918  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4919 #endif
4920 }
4921 
5061 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
5062  vpMatrix &kerAt) const
5063 {
5064 #if defined(VISP_HAVE_LAPACK)
5065  return pseudoInverseLapack(Ap, sv, rank_in, imA, imAt, kerAt);
5066 #elif defined(VISP_HAVE_EIGEN3)
5067  return pseudoInverseEigen3(Ap, sv, rank_in, imA, imAt, kerAt);
5068 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
5069  return pseudoInverseOpenCV(Ap, sv, rank_in, imA, imAt, kerAt);
5070 #else
5071  (void)Ap;
5072  (void)sv;
5073  (void)svThreshold;
5074  (void)imA;
5075  (void)imAt;
5076  (void)kerAt;
5077  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
5078  "Install Lapack, Eigen3 or OpenCV 3rd party"));
5079 #endif
5080 }
5081 
5123 vpColVector vpMatrix::getCol(unsigned int j, unsigned int i_begin, unsigned int column_size) const
5124 {
5125  if (i_begin + column_size > getRows() || j >= getCols())
5126  throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(),
5127  getCols()));
5128  vpColVector c(column_size);
5129  for (unsigned int i = 0; i < column_size; i++)
5130  c[i] = (*this)[i_begin + i][j];
5131  return c;
5132 }
5133 
5173 vpColVector vpMatrix::getCol(unsigned int j) const { return getCol(j, 0, rowNum); }
5174 
5210 vpRowVector vpMatrix::getRow(unsigned int i) const { return getRow(i, 0, colNum); }
5211 
5251 vpRowVector vpMatrix::getRow(unsigned int i, unsigned int j_begin, unsigned int row_size) const
5252 {
5253  if (j_begin + row_size > colNum || i >= rowNum)
5254  throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
5255 
5256  vpRowVector r(row_size);
5257  if (r.data != NULL && data != NULL) {
5258  memcpy(r.data, (*this)[i] + j_begin, row_size * sizeof(double));
5259  }
5260 
5261  return r;
5262 }
5263 
5300 {
5301  unsigned int min_size = std::min(rowNum, colNum);
5302  vpColVector diag;
5303 
5304  if (min_size > 0) {
5305  diag.resize(min_size, false);
5306 
5307  for (unsigned int i = 0; i < min_size; i++) {
5308  diag[i] = (*this)[i][i];
5309  }
5310  }
5311 
5312  return diag;
5313 }
5314 
5326 {
5327  vpMatrix C;
5328 
5329  vpMatrix::stack(A, B, C);
5330 
5331  return C;
5332 }
5333 
5345 void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
5346 {
5347  unsigned int nra = A.getRows();
5348  unsigned int nrb = B.getRows();
5349 
5350  if (nra != 0) {
5351  if (A.getCols() != B.getCols()) {
5352  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5353  A.getCols(), B.getRows(), B.getCols()));
5354  }
5355  }
5356 
5357  if (A.data != NULL && A.data == C.data) {
5358  std::cerr << "A and C must be two different objects!" << std::endl;
5359  return;
5360  }
5361 
5362  if (B.data != NULL && B.data == C.data) {
5363  std::cerr << "B and C must be two different objects!" << std::endl;
5364  return;
5365  }
5366 
5367  C.resize(nra + nrb, B.getCols(), false, false);
5368 
5369  if (C.data != NULL && A.data != NULL && A.size() > 0) {
5370  // Copy A in C
5371  memcpy(C.data, A.data, sizeof(double) * A.size());
5372  }
5373 
5374  if (C.data != NULL && B.data != NULL && B.size() > 0) {
5375  // Copy B in C
5376  memcpy(C.data + A.size(), B.data, sizeof(double) * B.size());
5377  }
5378 }
5379 
5390 {
5391  vpMatrix C;
5392  vpMatrix::stack(A, r, C);
5393 
5394  return C;
5395 }
5396 
5408 void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
5409 {
5410  if (A.data != NULL && A.data == C.data) {
5411  std::cerr << "A and C must be two different objects!" << std::endl;
5412  return;
5413  }
5414 
5415  C = A;
5416  C.stack(r);
5417 }
5418 
5429 {
5430  vpMatrix C;
5431  vpMatrix::stack(A, c, C);
5432 
5433  return C;
5434 }
5435 
5447 void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C)
5448 {
5449  if (A.data != NULL && A.data == C.data) {
5450  std::cerr << "A and C must be two different objects!" << std::endl;
5451  return;
5452  }
5453 
5454  C = A;
5455  C.stack(c);
5456 }
5457 
5470 vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, unsigned int r, unsigned int c)
5471 {
5472  vpMatrix C;
5473 
5474  insert(A, B, C, r, c);
5475 
5476  return C;
5477 }
5478 
5492 void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, unsigned int r, unsigned int c)
5493 {
5494  if (((r + B.getRows()) <= A.getRows()) && ((c + B.getCols()) <= A.getCols())) {
5495  C.resize(A.getRows(), A.getCols(), false, false);
5496 
5497  for (unsigned int i = 0; i < A.getRows(); i++) {
5498  for (unsigned int j = 0; j < A.getCols(); j++) {
5499  if (i >= r && i < (r + B.getRows()) && j >= c && j < (c + B.getCols())) {
5500  C[i][j] = B[i - r][j - c];
5501  } else {
5502  C[i][j] = A[i][j];
5503  }
5504  }
5505  }
5506  } else {
5507  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
5508  B.getRows(), B.getCols(), A.getCols(), A.getRows(), r, c);
5509  }
5510 }
5511 
5524 {
5525  vpMatrix C;
5526 
5527  juxtaposeMatrices(A, B, C);
5528 
5529  return C;
5530 }
5531 
5545 {
5546  unsigned int nca = A.getCols();
5547  unsigned int ncb = B.getCols();
5548 
5549  if (nca != 0) {
5550  if (A.getRows() != B.getRows()) {
5551  throw(vpException(vpException::dimensionError, "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5552  A.getCols(), B.getRows(), B.getCols()));
5553  }
5554  }
5555 
5556  if (B.getRows() == 0 || nca + ncb == 0) {
5557  std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl;
5558  return;
5559  }
5560 
5561  C.resize(B.getRows(), nca + ncb, false, false);
5562 
5563  C.insert(A, 0, 0);
5564  C.insert(B, 0, nca);
5565 }
5566 
5567 //--------------------------------------------------------------------
5568 // Output
5569 //--------------------------------------------------------------------
5570 
5590 int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &intro) const
5591 {
5592  typedef std::string::size_type size_type;
5593 
5594  unsigned int m = getRows();
5595  unsigned int n = getCols();
5596 
5597  std::vector<std::string> values(m * n);
5598  std::ostringstream oss;
5599  std::ostringstream ossFixed;
5600  std::ios_base::fmtflags original_flags = oss.flags();
5601 
5602  // ossFixed <<std::fixed;
5603  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
5604 
5605  size_type maxBefore = 0; // the length of the integral part
5606  size_type maxAfter = 0; // number of decimals plus
5607  // one place for the decimal point
5608  for (unsigned int i = 0; i < m; ++i) {
5609  for (unsigned int j = 0; j < n; ++j) {
5610  oss.str("");
5611  oss << (*this)[i][j];
5612  if (oss.str().find("e") != std::string::npos) {
5613  ossFixed.str("");
5614  ossFixed << (*this)[i][j];
5615  oss.str(ossFixed.str());
5616  }
5617 
5618  values[i * n + j] = oss.str();
5619  size_type thislen = values[i * n + j].size();
5620  size_type p = values[i * n + j].find('.');
5621 
5622  if (p == std::string::npos) {
5623  maxBefore = vpMath::maximum(maxBefore, thislen);
5624  // maxAfter remains the same
5625  } else {
5626  maxBefore = vpMath::maximum(maxBefore, p);
5627  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
5628  }
5629  }
5630  }
5631 
5632  size_type totalLength = length;
5633  // increase totalLength according to maxBefore
5634  totalLength = vpMath::maximum(totalLength, maxBefore);
5635  // decrease maxAfter according to totalLength
5636  maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
5637  if (maxAfter == 1)
5638  maxAfter = 0;
5639 
5640  // the following line is useful for debugging
5641  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
5642 
5643  if (!intro.empty())
5644  s << intro;
5645  s << "[" << m << "," << n << "]=\n";
5646 
5647  for (unsigned int i = 0; i < m; i++) {
5648  s << " ";
5649  for (unsigned int j = 0; j < n; j++) {
5650  size_type p = values[i * n + j].find('.');
5651  s.setf(std::ios::right, std::ios::adjustfield);
5652  s.width((std::streamsize)maxBefore);
5653  s << values[i * n + j].substr(0, p).c_str();
5654 
5655  if (maxAfter > 0) {
5656  s.setf(std::ios::left, std::ios::adjustfield);
5657  if (p != std::string::npos) {
5658  s.width((std::streamsize)maxAfter);
5659  s << values[i * n + j].substr(p, maxAfter).c_str();
5660  } else {
5661  assert(maxAfter > 1);
5662  s.width((std::streamsize)maxAfter);
5663  s << ".0";
5664  }
5665  }
5666 
5667  s << ' ';
5668  }
5669  s << std::endl;
5670  }
5671 
5672  s.flags(original_flags); // restore s to standard state
5673 
5674  return (int)(maxBefore + maxAfter);
5675 }
5676 
5713 std::ostream &vpMatrix::matlabPrint(std::ostream &os) const
5714 {
5715  os << "[ ";
5716  for (unsigned int i = 0; i < this->getRows(); ++i) {
5717  for (unsigned int j = 0; j < this->getCols(); ++j) {
5718  os << (*this)[i][j] << ", ";
5719  }
5720  if (this->getRows() != i + 1) {
5721  os << ";" << std::endl;
5722  } else {
5723  os << "]" << std::endl;
5724  }
5725  }
5726  return os;
5727 }
5728 
5757 std::ostream &vpMatrix::maplePrint(std::ostream &os) const
5758 {
5759  os << "([ " << std::endl;
5760  for (unsigned int i = 0; i < this->getRows(); ++i) {
5761  os << "[";
5762  for (unsigned int j = 0; j < this->getCols(); ++j) {
5763  os << (*this)[i][j] << ", ";
5764  }
5765  os << "]," << std::endl;
5766  }
5767  os << "])" << std::endl;
5768  return os;
5769 }
5770 
5798 std::ostream &vpMatrix::csvPrint(std::ostream &os) const
5799 {
5800  for (unsigned int i = 0; i < this->getRows(); ++i) {
5801  for (unsigned int j = 0; j < this->getCols(); ++j) {
5802  os << (*this)[i][j];
5803  if (!(j == (this->getCols() - 1)))
5804  os << ", ";
5805  }
5806  os << std::endl;
5807  }
5808  return os;
5809 }
5810 
5847 std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
5848 {
5849  os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " << this->getCols() << "); " << std::endl;
5850 
5851  for (unsigned int i = 0; i < this->getRows(); ++i) {
5852  for (unsigned int j = 0; j < this->getCols(); ++j) {
5853  if (!octet) {
5854  os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl;
5855  } else {
5856  for (unsigned int k = 0; k < sizeof(double); ++k) {
5857  os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex
5858  << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl;
5859  }
5860  }
5861  }
5862  os << std::endl;
5863  }
5864  return os;
5865 }
5866 
5872 {
5873  if (rowNum == 0) {
5874  *this = A;
5875  } else if (A.getRows() > 0) {
5876  if (colNum != A.getCols()) {
5877  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum,
5878  A.getRows(), A.getCols()));
5879  }
5880 
5881  unsigned int rowNumOld = rowNum;
5882  resize(rowNum + A.getRows(), colNum, false, false);
5883  insert(A, rowNumOld, 0);
5884  }
5885 }
5886 
5903 {
5904  if (rowNum == 0) {
5905  *this = r;
5906  } else {
5907  if (colNum != r.getCols()) {
5908  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum,
5909  colNum, r.getCols()));
5910  }
5911 
5912  if (r.size() == 0) {
5913  return;
5914  }
5915 
5916  unsigned int oldSize = size();
5917  resize(rowNum + 1, colNum, false, false);
5918 
5919  if (data != NULL && r.data != NULL && data != r.data) {
5920  // Copy r in data
5921  memcpy(data + oldSize, r.data, sizeof(double) * r.size());
5922  }
5923  }
5924 }
5925 
5943 {
5944  if (colNum == 0) {
5945  *this = c;
5946  } else {
5947  if (rowNum != c.getRows()) {
5948  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum,
5949  colNum, c.getRows()));
5950  }
5951 
5952  if (c.size() == 0) {
5953  return;
5954  }
5955 
5956  vpMatrix tmp = *this;
5957  unsigned int oldColNum = colNum;
5958  resize(rowNum, colNum + 1, false, false);
5959 
5960  if (data != NULL && tmp.data != NULL && data != tmp.data) {
5961  // Copy c in data
5962  for (unsigned int i = 0; i < rowNum; i++) {
5963  memcpy(data + i * colNum, tmp.data + i * oldColNum, sizeof(double) * oldColNum);
5964  rowPtrs[i][oldColNum] = c[i];
5965  }
5966  }
5967  }
5968 }
5969 
5980 void vpMatrix::insert(const vpMatrix &A, unsigned int r, unsigned int c)
5981 {
5982  if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) {
5983  if (A.colNum == colNum && data != NULL && A.data != NULL && A.data != data) {
5984  memcpy(data + r * colNum, A.data, sizeof(double) * A.size());
5985  } else if (data != NULL && A.data != NULL && A.data != data) {
5986  for (unsigned int i = r; i < (r + A.getRows()); i++) {
5987  memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum);
5988  }
5989  }
5990  } else {
5991  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
5992  A.getRows(), A.getCols(), rowNum, colNum, r, c);
5993  }
5994 }
5995 
6033 {
6034  vpColVector evalue(rowNum); // Eigen values
6035 
6036  if (rowNum != colNum) {
6037  throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
6038  colNum));
6039  }
6040 
6041  // Check if the matrix is symmetric: At - A = 0
6042  vpMatrix At_A = (*this).t() - (*this);
6043  for (unsigned int i = 0; i < rowNum; i++) {
6044  for (unsigned int j = 0; j < rowNum; j++) {
6045  // if (At_A[i][j] != 0) {
6046  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
6047  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix"));
6048  }
6049  }
6050  }
6051 
6052 #if defined(VISP_HAVE_LAPACK)
6053 #if defined(VISP_HAVE_GSL) /* be careful of the copy below */
6054  {
6055  gsl_vector *eval = gsl_vector_alloc(rowNum);
6056  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
6057 
6058  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
6059  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
6060 
6061  unsigned int Atda = (unsigned int)m->tda;
6062  for (unsigned int i = 0; i < rowNum; i++) {
6063  unsigned int k = i * Atda;
6064  for (unsigned int j = 0; j < colNum; j++)
6065  m->data[k + j] = (*this)[i][j];
6066  }
6067  gsl_eigen_symmv(m, eval, evec, w);
6068 
6069  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
6070 
6071  for (unsigned int i = 0; i < rowNum; i++) {
6072  evalue[i] = gsl_vector_get(eval, i);
6073  }
6074 
6075  gsl_eigen_symmv_free(w);
6076  gsl_vector_free(eval);
6077  gsl_matrix_free(m);
6078  gsl_matrix_free(evec);
6079  }
6080 #else
6081  {
6082  const char jobz = 'N';
6083  const char uplo = 'U';
6084  vpMatrix A = (*this);
6085  vpColVector WORK;
6086  int lwork = -1;
6087  int info = 0;
6088  double wkopt;
6089  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
6090  lwork = static_cast<int>(wkopt);
6091  WORK.resize(lwork);
6092  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
6093  }
6094 #endif
6095 #else
6096  {
6097  throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. "
6098  "You should install Lapack 3rd party"));
6099  }
6100 #endif
6101  return evalue;
6102 }
6103 
6153 void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const
6154 {
6155  if (rowNum != colNum) {
6156  throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
6157  colNum));
6158  }
6159 
6160  // Check if the matrix is symmetric: At - A = 0
6161  vpMatrix At_A = (*this).t() - (*this);
6162  for (unsigned int i = 0; i < rowNum; i++) {
6163  for (unsigned int j = 0; j < rowNum; j++) {
6164  // if (At_A[i][j] != 0) {
6165  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
6166  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix"));
6167  }
6168  }
6169  }
6170 
6171  // Resize the output matrices
6172  evalue.resize(rowNum);
6173  evector.resize(rowNum, colNum);
6174 
6175 #if defined(VISP_HAVE_LAPACK)
6176 #if defined(VISP_HAVE_GSL) /* be careful of the copy below */
6177  {
6178  gsl_vector *eval = gsl_vector_alloc(rowNum);
6179  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
6180 
6181  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
6182  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
6183 
6184  unsigned int Atda = (unsigned int)m->tda;
6185  for (unsigned int i = 0; i < rowNum; i++) {
6186  unsigned int k = i * Atda;
6187  for (unsigned int j = 0; j < colNum; j++)
6188  m->data[k + j] = (*this)[i][j];
6189  }
6190  gsl_eigen_symmv(m, eval, evec, w);
6191 
6192  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
6193 
6194  for (unsigned int i = 0; i < rowNum; i++) {
6195  evalue[i] = gsl_vector_get(eval, i);
6196  }
6197  Atda = (unsigned int)evec->tda;
6198  for (unsigned int i = 0; i < rowNum; i++) {
6199  unsigned int k = i * Atda;
6200  for (unsigned int j = 0; j < rowNum; j++) {
6201  evector[i][j] = evec->data[k + j];
6202  }
6203  }
6204 
6205  gsl_eigen_symmv_free(w);
6206  gsl_vector_free(eval);
6207  gsl_matrix_free(m);
6208  gsl_matrix_free(evec);
6209  }
6210 #else // defined(VISP_HAVE_GSL)
6211  {
6212  const char jobz = 'V';
6213  const char uplo = 'U';
6214  vpMatrix A = (*this);
6215  vpColVector WORK;
6216  int lwork = -1;
6217  int info = 0;
6218  double wkopt;
6219  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
6220  lwork = static_cast<int>(wkopt);
6221  WORK.resize(lwork);
6222  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
6223  evector = A.t();
6224  }
6225 #endif // defined(VISP_HAVE_GSL)
6226 #else
6227  {
6228  throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. "
6229  "You should install Lapack 3rd party"));
6230  }
6231 #endif
6232 }
6233 
6252 unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const
6253 {
6254  unsigned int nbline = getRows();
6255  unsigned int nbcol = getCols();
6256 
6257  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6258  vpColVector sv;
6259  sv.resize(nbcol, false); // singular values
6260  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6261 
6262  // Copy and resize matrix to have at least as many rows as columns
6263  // kernel is computed in svd method only if the matrix has more rows than
6264  // columns
6265 
6266  if (nbline < nbcol)
6267  U.resize(nbcol, nbcol, true);
6268  else
6269  U.resize(nbline, nbcol, false);
6270 
6271  U.insert(*this, 0, 0);
6272 
6273  U.svd(sv, V);
6274 
6275  // Compute the highest singular value and rank of the matrix
6276  double maxsv = 0;
6277  for (unsigned int i = 0; i < nbcol; i++) {
6278  if (sv[i] > maxsv) {
6279  maxsv = sv[i];
6280  }
6281  }
6282 
6283  unsigned int rank = 0;
6284  for (unsigned int i = 0; i < nbcol; i++) {
6285  if (sv[i] > maxsv * svThreshold) {
6286  rank++;
6287  }
6288  }
6289 
6290  kerAt.resize(nbcol - rank, nbcol);
6291  if (rank != nbcol) {
6292  for (unsigned int j = 0, k = 0; j < nbcol; j++) {
6293  // if( v.col(j) in kernel and non zero )
6294  if ((sv[j] <= maxsv * svThreshold) &&
6295  (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
6296  for (unsigned int i = 0; i < V.getRows(); i++) {
6297  kerAt[k][i] = V[i][j];
6298  }
6299  k++;
6300  }
6301  }
6302  }
6303 
6304  return rank;
6305 }
6306 
6323 unsigned int vpMatrix::nullSpace(vpMatrix &kerA, double svThreshold) const
6324 {
6325  unsigned int nbrow = getRows();
6326  unsigned int nbcol = getCols();
6327 
6328  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6329  vpColVector sv;
6330  sv.resize(nbcol, false); // singular values
6331  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6332 
6333  // Copy and resize matrix to have at least as many rows as columns
6334  // kernel is computed in svd method only if the matrix has more rows than
6335  // columns
6336 
6337  if (nbrow < nbcol)
6338  U.resize(nbcol, nbcol, true);
6339  else
6340  U.resize(nbrow, nbcol, false);
6341 
6342  U.insert(*this, 0, 0);
6343 
6344  U.svd(sv, V);
6345 
6346  // Compute the highest singular value and rank of the matrix
6347  double maxsv = sv[0];
6348 
6349  unsigned int rank = 0;
6350  for (unsigned int i = 0; i < nbcol; i++) {
6351  if (sv[i] > maxsv * svThreshold) {
6352  rank++;
6353  }
6354  }
6355 
6356  kerA.resize(nbcol, nbcol - rank);
6357  if (rank != nbcol) {
6358  for (unsigned int j = 0, k = 0; j < nbcol; j++) {
6359  // if( v.col(j) in kernel and non zero )
6360  if (sv[j] <= maxsv * svThreshold) {
6361  for (unsigned int i = 0; i < nbcol; i++) {
6362  kerA[i][k] = V[i][j];
6363  }
6364  k++;
6365  }
6366  }
6367  }
6368 
6369  return (nbcol - rank);
6370 }
6371 
6388 unsigned int vpMatrix::nullSpace(vpMatrix &kerA, int dim) const
6389 {
6390  unsigned int nbrow = getRows();
6391  unsigned int nbcol = getCols();
6392  unsigned int dim_ = static_cast<unsigned int>(dim);
6393 
6394  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6395  vpColVector sv;
6396  sv.resize(nbcol, false); // singular values
6397  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6398 
6399  // Copy and resize matrix to have at least as many rows as columns
6400  // kernel is computed in svd method only if the matrix has more rows than
6401  // columns
6402 
6403  if (nbrow < nbcol)
6404  U.resize(nbcol, nbcol, true);
6405  else
6406  U.resize(nbrow, nbcol, false);
6407 
6408  U.insert(*this, 0, 0);
6409 
6410  U.svd(sv, V);
6411 
6412  kerA.resize(nbcol, dim_);
6413  if (dim_ != 0) {
6414  unsigned int rank = nbcol - dim_;
6415  for (unsigned int k = 0; k < dim_; k++) {
6416  unsigned int j = k + rank;
6417  for (unsigned int i = 0; i < nbcol; i++) {
6418  kerA[i][k] = V[i][j];
6419  }
6420  }
6421  }
6422 
6423  double maxsv = sv[0];
6424  unsigned int rank = 0;
6425  for (unsigned int i = 0; i < nbcol; i++) {
6426  if (sv[i] > maxsv * 1e-6) {
6427  rank++;
6428  }
6429  }
6430  return (nbcol - rank);
6431 }
6432 
6464 double vpMatrix::det(vpDetMethod method) const
6465 {
6466  double det = 0.;
6467 
6468  if (method == LU_DECOMPOSITION) {
6469  det = this->detByLU();
6470  }
6471 
6472  return (det);
6473 }
6474 
6483 {
6484  if (colNum != rowNum) {
6485  throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix",
6486  rowNum, colNum));
6487  } else {
6488 #ifdef VISP_HAVE_GSL
6489  size_t size_ = rowNum * colNum;
6490  double *b = new double[size_];
6491  for (size_t i = 0; i < size_; i++)
6492  b[i] = 0.;
6493  gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum);
6494  gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum);
6495  gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
6496  // gsl_matrix_fprintf(stdout, &em.matrix, "%g");
6497  vpMatrix expA;
6498  expA.resize(rowNum, colNum, false);
6499  memcpy(expA.data, b, size_ * sizeof(double));
6500 
6501  delete[] b;
6502  return expA;
6503 #else
6504  vpMatrix _expE(rowNum, colNum, false);
6505  vpMatrix _expD(rowNum, colNum, false);
6506  vpMatrix _expX(rowNum, colNum, false);
6507  vpMatrix _expcX(rowNum, colNum, false);
6508  vpMatrix _eye(rowNum, colNum, false);
6509 
6510  _eye.eye();
6511  vpMatrix exp(*this);
6512 
6513  // double f;
6514  int e;
6515  double c = 0.5;
6516  int q = 6;
6517  int p = 1;
6518 
6519  double nA = 0;
6520  for (unsigned int i = 0; i < rowNum; i++) {
6521  double sum = 0;
6522  for (unsigned int j = 0; j < colNum; j++) {
6523  sum += fabs((*this)[i][j]);
6524  }
6525  if (sum > nA || i == 0) {
6526  nA = sum;
6527  }
6528  }
6529 
6530  /* f = */ frexp(nA, &e);
6531  // double s = (0 > e+1)?0:e+1;
6532  double s = e + 1;
6533 
6534  double sca = 1.0 / pow(2.0, s);
6535  exp = sca * exp;
6536  _expX = *this;
6537  _expE = c * exp + _eye;
6538  _expD = -c * exp + _eye;
6539  for (int k = 2; k <= q; k++) {
6540  c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1)));
6541  _expcX = exp * _expX;
6542  _expX = _expcX;
6543  _expcX = c * _expX;
6544  _expE = _expE + _expcX;
6545  if (p)
6546  _expD = _expD + _expcX;
6547  else
6548  _expD = _expD - _expcX;
6549  p = !p;
6550  }
6551  _expX = _expD.inverseByLU();
6552  exp = _expX * _expE;
6553  for (int k = 1; k <= s; k++) {
6554  _expE = exp * exp;
6555  exp = _expE;
6556  }
6557  return exp;
6558 #endif
6559  }
6560 }
6561 
6562 /**************************************************************************************************************/
6563 /**************************************************************************************************************/
6564 
6565 // Specific functions
6566 
6567 /*
6568 input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.
6569 
6570 output:: the complement matrix of the element (rowNo,colNo).
6571 This is the matrix obtained from M after elimenating the row rowNo and column
6572 colNo
6573 
6574 example:
6575 1 2 3
6576 M = 4 5 6
6577 7 8 9
6578 1 3
6579 subblock(M, 1, 1) give the matrix 7 9
6580 */
6581 vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row)
6582 {
6583  vpMatrix M_comp;
6584  M_comp.resize(M.getRows() - 1, M.getCols() - 1, false);
6585 
6586  for (unsigned int i = 0; i < col; i++) {
6587  for (unsigned int j = 0; j < row; j++)
6588  M_comp[i][j] = M[i][j];
6589  for (unsigned int j = row + 1; j < M.getRows(); j++)
6590  M_comp[i][j - 1] = M[i][j];
6591  }
6592  for (unsigned int i = col + 1; i < M.getCols(); i++) {
6593  for (unsigned int j = 0; j < row; j++)
6594  M_comp[i - 1][j] = M[i][j];
6595  for (unsigned int j = row + 1; j < M.getRows(); j++)
6596  M_comp[i - 1][j - 1] = M[i][j];
6597  }
6598  return M_comp;
6599 }
6600 
6610 double vpMatrix::cond(double svThreshold) const
6611 {
6612  unsigned int nbline = getRows();
6613  unsigned int nbcol = getCols();
6614 
6615  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6616  vpColVector sv;
6617  sv.resize(nbcol); // singular values
6618  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6619 
6620  // Copy and resize matrix to have at least as many rows as columns
6621  // kernel is computed in svd method only if the matrix has more rows than
6622  // columns
6623 
6624  if (nbline < nbcol)
6625  U.resize(nbcol, nbcol, true);
6626  else
6627  U.resize(nbline, nbcol, false);
6628 
6629  U.insert(*this, 0, 0);
6630 
6631  U.svd(sv, V);
6632 
6633  // Compute the highest singular value
6634  double maxsv = 0;
6635  for (unsigned int i = 0; i < nbcol; i++) {
6636  if (sv[i] > maxsv) {
6637  maxsv = sv[i];
6638  }
6639  }
6640 
6641  // Compute the rank of the matrix
6642  unsigned int rank = 0;
6643  for (unsigned int i = 0; i < nbcol; i++) {
6644  if (sv[i] > maxsv * svThreshold) {
6645  rank++;
6646  }
6647  }
6648 
6649  // Compute the lowest singular value
6650  double minsv = maxsv;
6651  for (unsigned int i = 0; i < rank; i++) {
6652  if (sv[i] < minsv) {
6653  minsv = sv[i];
6654  }
6655  }
6656 
6657  if (std::fabs(minsv) > std::numeric_limits<double>::epsilon()) {
6658  return maxsv / minsv;
6659  } else {
6660  return std::numeric_limits<double>::infinity();
6661  }
6662 }
6663 
6670 void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
6671 {
6672  if (H.getCols() != H.getRows()) {
6673  throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(),
6674  H.getCols()));
6675  }
6676 
6677  HLM = H;
6678  for (unsigned int i = 0; i < H.getCols(); i++) {
6679  HLM[i][i] += alpha * H[i][i];
6680  }
6681 }
6682 
6691 {
6692  double norm = 0.0;
6693  for (unsigned int i = 0; i < dsize; i++) {
6694  double x = *(data + i);
6695  norm += x * x;
6696  }
6697 
6698  return sqrt(norm);
6699 }
6700 
6710 {
6711  if (this->dsize != 0) {
6712  vpMatrix v;
6713  vpColVector w;
6714 
6715  vpMatrix M = *this;
6716 
6717  M.svd(w, v);
6718 
6719  double max = w[0];
6720  unsigned int maxRank = std::min(this->getCols(), this->getRows());
6721  // The maximum reachable rank is either the number of columns or the number of rows
6722  // of the matrix.
6723  unsigned int boundary = std::min(maxRank, w.size());
6724  // boundary is here to ensure that the number of singular values used for the com-
6725  // putation of the euclidean norm of the matrix is not greater than the maximum
6726  // reachable rank. Indeed, some svd library pad the singular values vector with 0s
6727  // if the input matrix is non-square.
6728  for (unsigned int i = 0; i < boundary; i++) {
6729  if (max < w[i]) {
6730  max = w[i];
6731  }
6732  }
6733  return max;
6734  } else {
6735  return 0.;
6736  }
6737 }
6738 
6750 {
6751  double norm = 0.0;
6752  for (unsigned int i = 0; i < rowNum; i++) {
6753  double x = 0;
6754  for (unsigned int j = 0; j < colNum; j++) {
6755  x += fabs(*(*(rowPtrs + i) + j));
6756  }
6757  if (x > norm) {
6758  norm = x;
6759  }
6760  }
6761  return norm;
6762 }
6763 
6770 double vpMatrix::sumSquare() const
6771 {
6772  double sum_square = 0.0;
6773  double x;
6774 
6775  for (unsigned int i = 0; i < rowNum; i++) {
6776  for (unsigned int j = 0; j < colNum; j++) {
6777  x = rowPtrs[i][j];
6778  sum_square += x * x;
6779  }
6780  }
6781 
6782  return sum_square;
6783 }
6784 
6796 vpMatrix vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode)
6797 {
6798  vpMatrix res;
6799  conv2(M, kernel, res, mode);
6800  return res;
6801 }
6802 
6815 void vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, vpMatrix &res, const std::string &mode)
6816 {
6817  if (M.getRows() * M.getCols() == 0 || kernel.getRows() * kernel.getCols() == 0)
6818  return;
6819 
6820  if (mode == "valid") {
6821  if (kernel.getRows() > M.getRows() || kernel.getCols() > M.getCols())
6822  return;
6823  }
6824 
6825  vpMatrix M_padded, res_same;
6826 
6827  if (mode == "full" || mode == "same") {
6828  const unsigned int pad_x = kernel.getCols() - 1;
6829  const unsigned int pad_y = kernel.getRows() - 1;
6830  M_padded.resize(M.getRows() + 2 * pad_y, M.getCols() + 2 * pad_x, true, false);
6831  M_padded.insert(M, pad_y, pad_x);
6832 
6833  if (mode == "same") {
6834  res.resize(M.getRows(), M.getCols(), false, false);
6835  res_same.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
6836  } else {
6837  res.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
6838  }
6839  } else if (mode == "valid") {
6840  M_padded = M;
6841  res.resize(M.getRows() - kernel.getRows() + 1, M.getCols() - kernel.getCols() + 1);
6842  } else {
6843  return;
6844  }
6845 
6846  if (mode == "same") {
6847  for (unsigned int i = 0; i < res_same.getRows(); i++) {
6848  for (unsigned int j = 0; j < res_same.getCols(); j++) {
6849  for (unsigned int k = 0; k < kernel.getRows(); k++) {
6850  for (unsigned int l = 0; l < kernel.getCols(); l++) {
6851  res_same[i][j] += M_padded[i + k][j + l] * kernel[kernel.getRows() - k - 1][kernel.getCols() - l - 1];
6852  }
6853  }
6854  }
6855  }
6856 
6857  const unsigned int start_i = kernel.getRows() / 2;
6858  const unsigned int start_j = kernel.getCols() / 2;
6859  for (unsigned int i = 0; i < M.getRows(); i++) {
6860  memcpy(res.data + i * M.getCols(), res_same.data + (i + start_i) * res_same.getCols() + start_j,
6861  sizeof(double) * M.getCols());
6862  }
6863  } else {
6864  for (unsigned int i = 0; i < res.getRows(); i++) {
6865  for (unsigned int j = 0; j < res.getCols(); j++) {
6866  for (unsigned int k = 0; k < kernel.getRows(); k++) {
6867  for (unsigned int l = 0; l < kernel.getCols(); l++) {
6868  res[i][j] += M_padded[i + k][j + l] * kernel[kernel.getRows() - k - 1][kernel.getCols() - l - 1];
6869  }
6870  }
6871  }
6872  }
6873  }
6874 }
6875 
6876 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
6886 vp_deprecated double vpMatrix::euclideanNorm() const { return frobeniusNorm(); }
6887 
6889 {
6890  return (vpMatrix)(vpColVector::stack(A, B));
6891 }
6892 
6894 {
6895  vpColVector::stack(A, B, C);
6896 }
6897 
6899 
6900 void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); }
6901 
6921 {
6922  vpRowVector c(getCols());
6923 
6924  for (unsigned int j = 0; j < getCols(); j++)
6925  c[j] = (*this)[i - 1][j];
6926  return c;
6927 }
6928 
6947 {
6948  vpColVector c(getRows());
6949 
6950  for (unsigned int i = 0; i < getRows(); i++)
6951  c[i] = (*this)[i][j - 1];
6952  return c;
6953 }
6954 
6961 void vpMatrix::setIdentity(const double &val)
6962 {
6963  for (unsigned int i = 0; i < rowNum; i++)
6964  for (unsigned int j = 0; j < colNum; j++)
6965  if (i == j)
6966  (*this)[i][j] = val;
6967  else
6968  (*this)[i][j] = 0;
6969 }
6970 
6971 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:132
unsigned int getCols() const
Definition: vpArray2D.h:281
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:139
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:306
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:141
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:293
unsigned int getRows() const
Definition: vpArray2D.h:291
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
double sumSquare() const
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h: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:171
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:6796
vpColVector eigenValues() const
Definition: vpMatrix.cpp:6032
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:5847
vpMatrix & operator<<(double *)
Definition: vpMatrix.cpp:783
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6610
vpMatrix expm() const
Definition: vpMatrix.cpp:6482
vpMatrix pseudoInverseEigen3(double svThreshold=1e-6) const
vpMatrix hadamard(const vpMatrix &m) const
Definition: vpMatrix.cpp:1761
vpMatrix operator-() const
Definition: vpMatrix.cpp:1552
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:6961
vpMatrix & operator/=(double x)
Divide all the element of the matrix by x : Aij = Aij / x.
Definition: vpMatrix.cpp:1683
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5590
std::ostream & maplePrint(std::ostream &os) const
Definition: vpMatrix.cpp:5757
void eye()
Definition: vpMatrix.cpp:450
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6252
vpMatrix inverseByLU() const
vpMatrix operator*(const vpMatrix &B) const
Definition: vpMatrix.cpp:1162
vpMatrix operator/(double x) const
Cij = Aij / x (A is unchanged)
Definition: vpMatrix.cpp:1623
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5871
vp_deprecated void stackMatrices(const vpMatrix &A)
Definition: vpMatrix.h:790
vpMatrix & operator-=(const vpMatrix &B)
Operation A = A - B.
Definition: vpMatrix.cpp:1510
vpMatrix & operator*=(double x)
Multiply all the element of the matrix by x : Aij = Aij * x.
Definition: vpMatrix.cpp:1669
vpMatrix operator*(const double &x, const vpMatrix &B)
Definition: vpMatrix.cpp:1583
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:5523
vpColVector stackColumns()
Definition: vpMatrix.cpp:1726
vp_deprecated vpColVector column(unsigned int j)
Definition: vpMatrix.cpp:6946
static void sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1459
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2024
vpRowVector stackRows()
Definition: vpMatrix.cpp:1748
vpMatrix operator+(const vpMatrix &B) const
Definition: vpMatrix.cpp:1404
double sum() const
Definition: vpMatrix.cpp:1559
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:882
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:5210
void svdOpenCV(vpColVector &w, vpMatrix &V)
vpMatrix t() const
Definition: vpMatrix.cpp:465
vpMatrix()
Definition: vpMatrix.h:169
vpMatrix & operator+=(const vpMatrix &B)
Operation A = A + B.
Definition: vpMatrix.cpp:1493
static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C)
Definition: vpMatrix.cpp:1317
double inducedL2Norm() const
Definition: vpMatrix.cpp:6709
double infinityNorm() const
Definition: vpMatrix.cpp:6749
vpMatrix & operator=(const vpArray2D< double > &A)
Definition: vpMatrix.cpp:651
double frobeniusNorm() const
Definition: vpMatrix.cpp:6690
vpColVector getDiag() const
Definition: vpMatrix.cpp:5299
vpMatrix AtA() const
Definition: vpMatrix.cpp:626
static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
Definition: vpMatrix.cpp:901
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:1902
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6670
vpMatrix pseudoInverseLapack(double svThreshold=1e-6) const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2235
vpMatrix & operator,(double val)
Definition: vpMatrix.cpp:800
static void multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
Definition: vpMatrix.cpp:954
vp_deprecated vpRowVector row(unsigned int i)
Definition: vpMatrix.cpp:6920
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5173
double detByLU() const
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6464
double sumSquare() const
Definition: vpMatrix.cpp:6770
vp_deprecated void init()
Definition: vpMatrix.h:785
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1346
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5980
void svdEigen3(vpColVector &w, vpMatrix &V)
void kron(const vpMatrix &m1, vpMatrix &out) const
Definition: vpMatrix.cpp:1811
vpMatrix AAt() const
Definition: vpMatrix.cpp:504
vpMatrix transpose() const
Definition: vpMatrix.cpp:472
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1003
static void negateMatrix(const vpMatrix &A, vpMatrix &C)
Definition: vpMatrix.cpp:1534
@ LU_DECOMPOSITION
Definition: vpMatrix.h:161
std::ostream & csvPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5798
unsigned int nullSpace(vpMatrix &kerA, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6323
std::ostream & matlabPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5713
double euclideanNorm() const
Definition: vpMatrix.cpp:6886
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:408
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.