Visual Servoing Platform  version 3.6.1 under development (2024-03-18)
vpMatrix.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Matrix manipulation.
33  */
34 
40 #include <algorithm>
41 #include <assert.h>
42 #include <cmath> // std::fabs
43 #include <fstream>
44 #include <limits> // numeric_limits
45 #include <sstream>
46 #include <stdio.h>
47 #include <stdlib.h>
48 #include <string.h>
49 #include <string>
50 #include <vector>
51 
52 #include <visp3/core/vpCPUFeatures.h>
53 #include <visp3/core/vpColVector.h>
54 #include <visp3/core/vpConfig.h>
55 #include <visp3/core/vpDebug.h>
56 #include <visp3/core/vpException.h>
57 #include <visp3/core/vpMath.h>
58 #include <visp3/core/vpMatrix.h>
59 #include <visp3/core/vpTranslationVector.h>
60 
61 #if defined(VISP_HAVE_SIMDLIB)
62 #include <Simd/SimdLib.h>
63 #endif
64 
65 #ifdef VISP_HAVE_LAPACK
66 #ifdef VISP_HAVE_GSL
67 #include <gsl/gsl_eigen.h>
68 #include <gsl/gsl_linalg.h>
69 #include <gsl/gsl_math.h>
70 #elif defined(VISP_HAVE_MKL)
71 #include <mkl.h>
72 typedef MKL_INT integer;
73 
74 void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_, double *w_data,
75  double *work_data, int lwork_, int &info_)
76 {
77  MKL_INT n = static_cast<MKL_INT>(n_);
78  MKL_INT lda = static_cast<MKL_INT>(lda_);
79  MKL_INT lwork = static_cast<MKL_INT>(lwork_);
80  MKL_INT info = static_cast<MKL_INT>(info_);
81 
82  dsyev(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
83 }
84 
85 #else
86 #if defined(VISP_HAVE_LAPACK_BUILT_IN)
87 typedef long int integer;
88 #else
89 typedef int integer;
90 #endif
91 extern "C" integer dsyev_(char *jobz, char *uplo, integer *n, double *a, integer *lda, double *w, double *WORK,
92  integer *lwork, integer *info);
93 
94 void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_, double *w_data,
95  double *work_data, int lwork_, int &info_)
96 {
97  integer n = static_cast<integer>(n_);
98  integer lda = static_cast<integer>(lda_);
99  integer lwork = static_cast<integer>(lwork_);
100  integer info = static_cast<integer>(info_);
101 
102  dsyev_(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
103 
104  lwork_ = static_cast<int>(lwork);
105  info_ = static_cast<int>(info);
106 }
107 #endif
108 #endif
109 
110 #if !defined(VISP_USE_MSVC) || (defined(VISP_USE_MSVC) && !defined(VISP_BUILD_SHARED_LIBS))
111 const unsigned int vpMatrix::m_lapack_min_size_default = 0;
112 unsigned int vpMatrix::m_lapack_min_size = vpMatrix::m_lapack_min_size_default;
113 #endif
114 
115 // Prototypes of specific functions
116 vpMatrix subblock(const vpMatrix &, unsigned int, unsigned int);
117 
118 void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows,
119  unsigned int ncols, double svThreshold, vpMatrix &Ap, int &rank_out, int *rank_in,
120  vpMatrix *imA, vpMatrix *imAt, vpMatrix *kerAt)
121 {
122  Ap.resize(ncols, nrows, true, false);
123 
124  // compute the highest singular value and the rank of h
125  double maxsv = sv[0];
126 
127  rank_out = 0;
128 
129  for (unsigned int i = 0; i < sv.size(); i++) {
130  if (sv[i] > maxsv * svThreshold) {
131  rank_out++;
132  }
133  }
134 
135  unsigned int rank = static_cast<unsigned int>(rank_out);
136  if (rank_in) {
137  rank = static_cast<unsigned int>(*rank_in);
138  }
139 
140  for (unsigned int i = 0; i < ncols; i++) {
141  for (unsigned int j = 0; j < nrows; j++) {
142  for (unsigned int k = 0; k < rank; k++) {
143  Ap[i][j] += V[i][k] * U[j][k] / sv[k];
144  }
145  }
146  }
147 
148  // Compute im(A)
149  if (imA) {
150  imA->resize(nrows, rank);
151 
152  for (unsigned int i = 0; i < nrows; i++) {
153  for (unsigned int j = 0; j < rank; j++) {
154  (*imA)[i][j] = U[i][j];
155  }
156  }
157  }
158 
159  // Compute im(At)
160  if (imAt) {
161  imAt->resize(ncols, rank);
162  for (unsigned int i = 0; i < ncols; i++) {
163  for (unsigned int j = 0; j < rank; j++) {
164  (*imAt)[i][j] = V[i][j];
165  }
166  }
167  }
168 
169  // Compute ker(At)
170  if (kerAt) {
171  kerAt->resize(ncols - rank, ncols);
172  if (rank != ncols) {
173  for (unsigned int k = 0; k < (ncols - rank); k++) {
174  unsigned j = k + rank;
175  for (unsigned int i = 0; i < V.getRows(); i++) {
176  (*kerAt)[k][i] = V[i][j];
177  }
178  }
179  }
180  }
181 }
182 
188 vpMatrix::vpMatrix(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
189  : vpArray2D<double>()
190 {
191  if (((r + nrows) > M.rowNum) || ((c + ncols) > M.colNum)) {
193  "Cannot construct a sub matrix (%dx%d) starting at "
194  "position (%d,%d) that is not contained in the "
195  "original matrix (%dx%d)",
196  nrows, ncols, r, c, M.rowNum, M.colNum));
197  }
198 
199  init(M, r, c, nrows, ncols);
200 }
201 
202 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
204 {
205  rowNum = A.rowNum;
206  colNum = A.colNum;
207  rowPtrs = A.rowPtrs;
208  dsize = A.dsize;
209  data = A.data;
210 
211  A.rowNum = 0;
212  A.colNum = 0;
213  A.rowPtrs = nullptr;
214  A.dsize = 0;
215  A.data = nullptr;
216 }
217 
241 vpMatrix::vpMatrix(const std::initializer_list<double> &list) : vpArray2D<double>(list) { }
242 
265 vpMatrix::vpMatrix(unsigned int nrows, unsigned int ncols, const std::initializer_list<double> &list)
266  : vpArray2D<double>(nrows, ncols, list)
267 { }
268 
289 vpMatrix::vpMatrix(const std::initializer_list<std::initializer_list<double> > &lists) : vpArray2D<double>(lists) { }
290 #endif
291 
338 void vpMatrix::init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
339 {
340  unsigned int rnrows = r + nrows;
341  unsigned int cncols = c + ncols;
342 
343  if (rnrows > M.getRows())
344  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
345  M.getRows()));
346  if (cncols > M.getCols())
347  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
348  M.getCols()));
349  resize(nrows, ncols, false, false);
350 
351  if (this->rowPtrs == nullptr) { // Fix coverity scan: explicit null dereferenced
352  return; // Noting to do
353  }
354  for (unsigned int i = 0; i < nrows; i++) {
355  memcpy((*this)[i], &M[i + r][c], ncols * sizeof(double));
356  }
357 }
358 
400 vpMatrix vpMatrix::extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
401 {
402  unsigned int rnrows = r + nrows;
403  unsigned int cncols = c + ncols;
404 
405  if (rnrows > getRows())
406  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
407  getRows()));
408  if (cncols > getCols())
409  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
410  getCols()));
411 
412  vpMatrix M;
413  M.resize(nrows, ncols, false, false);
414  for (unsigned int i = 0; i < nrows; i++) {
415  memcpy(M[i], &(*this)[i + r][c], ncols * sizeof(double));
416  }
417 
418  return M;
419 }
420 
425 void vpMatrix::eye(unsigned int n) { eye(n, n); }
426 
431 void vpMatrix::eye(unsigned int m, unsigned int n)
432 {
433  resize(m, n);
434 
435  eye();
436 }
437 
443 {
444  for (unsigned int i = 0; i < rowNum; i++) {
445  for (unsigned int j = 0; j < colNum; j++) {
446  if (i == j)
447  (*this)[i][j] = 1.0;
448  else
449  (*this)[i][j] = 0;
450  }
451  }
452 }
453 
457 vpMatrix vpMatrix::t() const { return transpose(); }
458 
465 {
466  vpMatrix At;
467  transpose(At);
468  return At;
469 }
470 
477 {
478  At.resize(colNum, rowNum, false, false);
479 
480  if (rowNum <= 16 || colNum <= 16) {
481  for (unsigned int i = 0; i < rowNum; i++) {
482  for (unsigned int j = 0; j < colNum; j++) {
483  At[j][i] = (*this)[i][j];
484  }
485  }
486  }
487  else {
488 #if defined(VISP_HAVE_SIMDLIB)
489  SimdMatTranspose(data, rowNum, colNum, At.data);
490 #else
491  // https://stackoverflow.com/a/21548079
492  const int tileSize = 32;
493  for (unsigned int i = 0; i < rowNum; i += tileSize) {
494  for (unsigned int j = 0; j < colNum; j++) {
495  for (unsigned int b = 0; b < static_cast<unsigned int>(tileSize) && i + b < rowNum; b++) {
496  At[j][i + b] = (*this)[i + b][j];
497  }
498  }
499  }
500 #endif
501  }
502 }
503 
510 {
511  vpMatrix B;
512 
513  AAt(B);
514 
515  return B;
516 }
517 
529 void vpMatrix::AAt(vpMatrix &B) const
530 {
531  if ((B.rowNum != rowNum) || (B.colNum != rowNum))
532  B.resize(rowNum, rowNum, false, false);
533 
534  // If available use Lapack only for large matrices
535  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
536 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
537  useLapack = false;
538 #endif
539 
540  if (useLapack) {
541 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
542  const double alpha = 1.0;
543  const double beta = 0.0;
544  const char transa = 't';
545  const char transb = 'n';
546 
547  vpMatrix::blas_dgemm(transa, transb, rowNum, rowNum, colNum, alpha, data, colNum, data, colNum, beta, B.data,
548  rowNum);
549 #endif
550  }
551  else {
552  // compute A*A^T
553  for (unsigned int i = 0; i < rowNum; i++) {
554  for (unsigned int j = i; j < rowNum; j++) {
555  double *pi = rowPtrs[i]; // row i
556  double *pj = rowPtrs[j]; // row j
557 
558  // sum (row i .* row j)
559  double ssum = 0;
560  for (unsigned int k = 0; k < colNum; k++)
561  ssum += *(pi++) * *(pj++);
562 
563  B[i][j] = ssum; // upper triangle
564  if (i != j)
565  B[j][i] = ssum; // lower triangle
566  }
567  }
568  }
569 }
570 
582 void vpMatrix::AtA(vpMatrix &B) const
583 {
584  if ((B.rowNum != colNum) || (B.colNum != colNum))
585  B.resize(colNum, colNum, false, false);
586 
587  // If available use Lapack only for large matrices
588  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
589 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
590  useLapack = false;
591 #endif
592 
593  if (useLapack) {
594 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
595  const double alpha = 1.0;
596  const double beta = 0.0;
597  const char transa = 'n';
598  const char transb = 't';
599 
600  vpMatrix::blas_dgemm(transa, transb, colNum, colNum, rowNum, alpha, data, colNum, data, colNum, beta, B.data,
601  colNum);
602 #endif
603  }
604  else {
605  for (unsigned int i = 0; i < colNum; i++) {
606  double *Bi = B[i];
607  for (unsigned int j = 0; j < i; j++) {
608  double *ptr = data;
609  double s = 0;
610  for (unsigned int k = 0; k < rowNum; k++) {
611  s += (*(ptr + i)) * (*(ptr + j));
612  ptr += colNum;
613  }
614  *Bi++ = s;
615  B[j][i] = s;
616  }
617  double *ptr = data;
618  double s = 0;
619  for (unsigned int k = 0; k < rowNum; k++) {
620  s += (*(ptr + i)) * (*(ptr + i));
621  ptr += colNum;
622  }
623  *Bi = s;
624  }
625  }
626 }
627 
634 {
635  vpMatrix B;
636 
637  AtA(B);
638 
639  return B;
640 }
641 
659 {
660  resize(A.getRows(), A.getCols(), false, false);
661 
662  if (data != nullptr && A.data != nullptr && data != A.data) {
663  memcpy(data, A.data, dsize * sizeof(double));
664  }
665 
666  return *this;
667 }
668 
670 {
671  resize(A.getRows(), A.getCols(), false, false);
672 
673  if (data != nullptr && A.data != nullptr && data != A.data) {
674  memcpy(data, A.data, dsize * sizeof(double));
675  }
676 
677  return *this;
678 }
679 
680 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
682 {
683  if (this != &other) {
684  if (data) {
685  free(data);
686  }
687  if (rowPtrs) {
688  free(rowPtrs);
689  }
690 
691  rowNum = other.rowNum;
692  colNum = other.colNum;
693  rowPtrs = other.rowPtrs;
694  dsize = other.dsize;
695  data = other.data;
696 
697  other.rowNum = 0;
698  other.colNum = 0;
699  other.rowPtrs = nullptr;
700  other.dsize = 0;
701  other.data = nullptr;
702  }
703 
704  return *this;
705 }
706 
733 vpMatrix &vpMatrix::operator=(const std::initializer_list<double> &list)
734 {
735  if (dsize != static_cast<unsigned int>(list.size())) {
736  resize(1, static_cast<unsigned int>(list.size()), false, false);
737  }
738 
739  std::copy(list.begin(), list.end(), data);
740 
741  return *this;
742 }
743 
767 vpMatrix &vpMatrix::operator=(const std::initializer_list<std::initializer_list<double> > &lists)
768 {
769  unsigned int nrows = static_cast<unsigned int>(lists.size()), ncols = 0;
770  for (auto &l : lists) {
771  if (static_cast<unsigned int>(l.size()) > ncols) {
772  ncols = static_cast<unsigned int>(l.size());
773  }
774  }
775 
776  resize(nrows, ncols, false, false);
777  auto it = lists.begin();
778  for (unsigned int i = 0; i < rowNum; i++, ++it) {
779  std::copy(it->begin(), it->end(), rowPtrs[i]);
780  }
781 
782  return *this;
783 }
784 #endif
785 
788 {
789  std::fill(data, data + rowNum * colNum, x);
790  return *this;
791 }
792 
799 {
800  for (unsigned int i = 0; i < rowNum; i++) {
801  for (unsigned int j = 0; j < colNum; j++) {
802  rowPtrs[i][j] = *x++;
803  }
804  }
805  return *this;
806 }
807 
809 {
810  resize(1, 1, false, false);
811  rowPtrs[0][0] = val;
812  return *this;
813 }
814 
816 {
817  resize(1, colNum + 1, false, false);
818  rowPtrs[0][colNum - 1] = val;
819  return *this;
820 }
821 
858 {
859  unsigned int rows = A.getRows();
860  this->resize(rows, rows);
861 
862  (*this) = 0;
863  for (unsigned int i = 0; i < rows; i++)
864  (*this)[i][i] = A[i];
865 }
866 
897 void vpMatrix::diag(const double &val)
898 {
899  (*this) = 0;
900  unsigned int min_ = (rowNum < colNum) ? rowNum : colNum;
901  for (unsigned int i = 0; i < min_; i++)
902  (*this)[i][i] = val;
903 }
904 
916 {
917  unsigned int rows = A.getRows();
918  DA.resize(rows, rows, true);
919 
920  for (unsigned int i = 0; i < rows; i++)
921  DA[i][i] = A[i];
922 }
923 
929 {
930  vpTranslationVector t_out;
931 
932  if (rowNum != 3 || colNum != 3) {
933  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector",
934  rowNum, colNum, tv.getRows(), tv.getCols()));
935  }
936 
937  for (unsigned int j = 0; j < 3; j++)
938  t_out[j] = 0;
939 
940  for (unsigned int j = 0; j < 3; j++) {
941  double tj = tv[j]; // optimization em 5/12/2006
942  for (unsigned int i = 0; i < 3; i++) {
943  t_out[i] += rowPtrs[i][j] * tj;
944  }
945  }
946  return t_out;
947 }
948 
954 {
955  vpColVector v_out;
956  vpMatrix::multMatrixVector(*this, v, v_out);
957  return v_out;
958 }
959 
969 {
970  if (A.colNum != v.getRows()) {
971  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
972  A.getRows(), A.getCols(), v.getRows()));
973  }
974 
975  if (A.rowNum != w.rowNum)
976  w.resize(A.rowNum, false);
977 
978  // If available use Lapack only for large matrices
979  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size);
980 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
981  useLapack = false;
982 #endif
983 
984  if (useLapack) {
985 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
986  double alpha = 1.0;
987  double beta = 0.0;
988  char trans = 't';
989  int incr = 1;
990 
991  vpMatrix::blas_dgemv(trans, A.colNum, A.rowNum, alpha, A.data, A.colNum, v.data, incr, beta, w.data, incr);
992 #endif
993  }
994  else {
995  w = 0.0;
996  for (unsigned int j = 0; j < A.colNum; j++) {
997  double vj = v[j]; // optimization em 5/12/2006
998  for (unsigned int i = 0; i < A.rowNum; i++) {
999  w[i] += A.rowPtrs[i][j] * vj;
1000  }
1001  }
1002  }
1003 }
1004 
1005 //---------------------------------
1006 // Matrix operations.
1007 //---------------------------------
1008 
1019 {
1020  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1021  C.resize(A.rowNum, B.colNum, false, false);
1022 
1023  if (A.colNum != B.rowNum) {
1024  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (%dx%d) matrix", A.getRows(),
1025  A.getCols(), B.getRows(), B.getCols()));
1026  }
1027 
1028  // If available use Lapack only for large matrices
1029  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size ||
1030  B.colNum > vpMatrix::m_lapack_min_size);
1031 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1032  useLapack = false;
1033 #endif
1034 
1035  if (useLapack) {
1036 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1037  const double alpha = 1.0;
1038  const double beta = 0.0;
1039  const char trans = 'n';
1040  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1041  C.data, B.colNum);
1042 #endif
1043  }
1044  else {
1045  // 5/12/06 some "very" simple optimization to avoid indexation
1046  const unsigned int BcolNum = B.colNum;
1047  const unsigned int BrowNum = B.rowNum;
1048  double **BrowPtrs = B.rowPtrs;
1049  for (unsigned int i = 0; i < A.rowNum; i++) {
1050  const double *rowptri = A.rowPtrs[i];
1051  double *ci = C[i];
1052  for (unsigned int j = 0; j < BcolNum; j++) {
1053  double s = 0;
1054  for (unsigned int k = 0; k < BrowNum; k++)
1055  s += rowptri[k] * BrowPtrs[k][j];
1056  ci[j] = s;
1057  }
1058  }
1059  }
1060 }
1061 
1075 {
1076  if (A.colNum != 3 || A.rowNum != 3 || B.colNum != 3 || B.rowNum != 3) {
1078  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1079  "rotation matrix",
1080  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1081  }
1082  // 5/12/06 some "very" simple optimization to avoid indexation
1083  const unsigned int BcolNum = B.colNum;
1084  const unsigned int BrowNum = B.rowNum;
1085  double **BrowPtrs = B.rowPtrs;
1086  for (unsigned int i = 0; i < A.rowNum; i++) {
1087  const double *rowptri = A.rowPtrs[i];
1088  double *ci = C[i];
1089  for (unsigned int j = 0; j < BcolNum; j++) {
1090  double s = 0;
1091  for (unsigned int k = 0; k < BrowNum; k++)
1092  s += rowptri[k] * BrowPtrs[k][j];
1093  ci[j] = s;
1094  }
1095  }
1096 }
1097 
1111 {
1112  if (A.colNum != 4 || A.rowNum != 4 || B.colNum != 4 || B.rowNum != 4) {
1114  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1115  "rotation matrix",
1116  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1117  }
1118  // Considering perfMatrixMultiplication.cpp benchmark,
1119  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1120  // Lapack usage needs to be validated again.
1121  // If available use Lapack only for large matrices.
1122  // Using SSE2 doesn't speed up.
1123  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size ||
1124  B.colNum > vpMatrix::m_lapack_min_size);
1125 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1126  useLapack = false;
1127 #endif
1128 
1129  if (useLapack) {
1130 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1131  const double alpha = 1.0;
1132  const double beta = 0.0;
1133  const char trans = 'n';
1134  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1135  C.data, B.colNum);
1136 #endif
1137  }
1138  else {
1139  // 5/12/06 some "very" simple optimization to avoid indexation
1140  const unsigned int BcolNum = B.colNum;
1141  const unsigned int BrowNum = B.rowNum;
1142  double **BrowPtrs = B.rowPtrs;
1143  for (unsigned int i = 0; i < A.rowNum; i++) {
1144  const double *rowptri = A.rowPtrs[i];
1145  double *ci = C[i];
1146  for (unsigned int j = 0; j < BcolNum; j++) {
1147  double s = 0;
1148  for (unsigned int k = 0; k < BrowNum; k++)
1149  s += rowptri[k] * BrowPtrs[k][j];
1150  ci[j] = s;
1151  }
1152  }
1153  }
1154 }
1155 
1169 {
1170  vpMatrix::multMatrixVector(A, B, C);
1171 }
1172 
1178 {
1179  vpMatrix C;
1180 
1181  vpMatrix::mult2Matrices(*this, B, C);
1182 
1183  return C;
1184 }
1185 
1191 {
1192  if (colNum != R.getRows()) {
1193  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1194  colNum));
1195  }
1196  vpMatrix C;
1197  C.resize(rowNum, 3, false, false);
1198 
1199  unsigned int RcolNum = R.getCols();
1200  unsigned int RrowNum = R.getRows();
1201  for (unsigned int i = 0; i < rowNum; i++) {
1202  double *rowptri = rowPtrs[i];
1203  double *ci = C[i];
1204  for (unsigned int j = 0; j < RcolNum; j++) {
1205  double s = 0;
1206  for (unsigned int k = 0; k < RrowNum; k++)
1207  s += rowptri[k] * R[k][j];
1208  ci[j] = s;
1209  }
1210  }
1211 
1212  return C;
1213 }
1214 
1220 {
1221  if (colNum != M.getRows()) {
1222  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1223  colNum));
1224  }
1225  vpMatrix C;
1226  C.resize(rowNum, 4, false, false);
1227 
1228  const unsigned int McolNum = M.getCols();
1229  const unsigned int MrowNum = M.getRows();
1230  for (unsigned int i = 0; i < rowNum; i++) {
1231  const double *rowptri = rowPtrs[i];
1232  double *ci = C[i];
1233  for (unsigned int j = 0; j < McolNum; j++) {
1234  double s = 0;
1235  for (unsigned int k = 0; k < MrowNum; k++)
1236  s += rowptri[k] * M[k][j];
1237  ci[j] = s;
1238  }
1239  }
1240 
1241  return C;
1242 }
1243 
1249 {
1250  if (colNum != V.getRows()) {
1251  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix",
1252  rowNum, colNum));
1253  }
1254  vpMatrix M;
1255  M.resize(rowNum, 6, false, false);
1256 
1257  // Considering perfMatrixMultiplication.cpp benchmark,
1258  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1259  // Lapack usage needs to be validated again.
1260  // If available use Lapack only for large matrices.
1261  // Speed up obtained using SSE2.
1262  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size ||
1263  V.colNum > vpMatrix::m_lapack_min_size);
1264 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1265  useLapack = false;
1266 #endif
1267 
1268  if (useLapack) {
1269 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1270  const double alpha = 1.0;
1271  const double beta = 0.0;
1272  const char trans = 'n';
1273  vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, V.colNum, data, colNum, beta, M.data,
1274  M.colNum);
1275 #endif
1276  }
1277  else {
1278 #if defined(VISP_HAVE_SIMDLIB)
1279  SimdMatMulTwist(data, rowNum, V.data, M.data);
1280 #else
1281  unsigned int VcolNum = V.getCols();
1282  unsigned int VrowNum = V.getRows();
1283  for (unsigned int i = 0; i < rowNum; i++) {
1284  double *rowptri = rowPtrs[i];
1285  double *ci = M[i];
1286  for (unsigned int j = 0; j < VcolNum; j++) {
1287  double s = 0;
1288  for (unsigned int k = 0; k < VrowNum; k++)
1289  s += rowptri[k] * V[k][j];
1290  ci[j] = s;
1291  }
1292  }
1293 #endif
1294  }
1295 
1296  return M;
1297 }
1298 
1304 {
1305  if (colNum != V.getRows()) {
1306  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) force/torque twist matrix",
1307  rowNum, colNum));
1308  }
1309  vpMatrix M;
1310  M.resize(rowNum, 6, false, false);
1311 
1312  // Considering perfMatrixMultiplication.cpp benchmark,
1313  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1314  // Lapack usage needs to be validated again.
1315  // If available use Lapack only for large matrices.
1316  // Speed up obtained using SSE2.
1317  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size ||
1318  V.getCols() > vpMatrix::m_lapack_min_size);
1319 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1320  useLapack = false;
1321 #endif
1322 
1323  if (useLapack) {
1324 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1325  const double alpha = 1.0;
1326  const double beta = 0.0;
1327  const char trans = 'n';
1328  vpMatrix::blas_dgemm(trans, trans, V.getCols(), rowNum, colNum, alpha, V.data, V.getCols(), data, colNum, beta,
1329  M.data, M.colNum);
1330 #endif
1331  }
1332  else {
1333 #if defined(VISP_HAVE_SIMDLIB)
1334  SimdMatMulTwist(data, rowNum, V.data, M.data);
1335 #else
1336  unsigned int VcolNum = V.getCols();
1337  unsigned int VrowNum = V.getRows();
1338  for (unsigned int i = 0; i < rowNum; i++) {
1339  double *rowptri = rowPtrs[i];
1340  double *ci = M[i];
1341  for (unsigned int j = 0; j < VcolNum; j++) {
1342  double s = 0;
1343  for (unsigned int k = 0; k < VrowNum; k++)
1344  s += rowptri[k] * V[k][j];
1345  ci[j] = s;
1346  }
1347  }
1348 #endif
1349  }
1350 
1351  return M;
1352 }
1353 
1364 void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB,
1365  vpMatrix &C)
1366 {
1367  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1368  C.resize(A.rowNum, B.colNum, false, false);
1369 
1370  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1371  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1372  A.getCols(), B.getRows(), B.getCols()));
1373  }
1374 
1375  double **ArowPtrs = A.rowPtrs;
1376  double **BrowPtrs = B.rowPtrs;
1377  double **CrowPtrs = C.rowPtrs;
1378 
1379  for (unsigned int i = 0; i < A.rowNum; i++)
1380  for (unsigned int j = 0; j < A.colNum; j++)
1381  CrowPtrs[i][j] = wB * BrowPtrs[i][j] + wA * ArowPtrs[i][j];
1382 }
1383 
1393 void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1394 {
1395  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1396  C.resize(A.rowNum, B.colNum, false, false);
1397 
1398  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1399  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1400  A.getCols(), B.getRows(), B.getCols()));
1401  }
1402 
1403  double **ArowPtrs = A.rowPtrs;
1404  double **BrowPtrs = B.rowPtrs;
1405  double **CrowPtrs = C.rowPtrs;
1406 
1407  for (unsigned int i = 0; i < A.rowNum; i++) {
1408  for (unsigned int j = 0; j < A.colNum; j++) {
1409  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1410  }
1411  }
1412 }
1413 
1427 {
1428  if ((A.rowNum != C.rowNum) || (B.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 add (%dx%d) matrix with (%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] = BrowPtrs[i][j] + ArowPtrs[i][j];
1443  }
1444  }
1445 }
1446 
1452 {
1453  vpMatrix C;
1454  vpMatrix::add2Matrices(*this, B, C);
1455  return C;
1456 }
1457 
1474 {
1475  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1476  C.resize(A.rowNum);
1477 
1478  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1479  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1480  A.getCols(), B.getRows(), B.getCols()));
1481  }
1482 
1483  double **ArowPtrs = A.rowPtrs;
1484  double **BrowPtrs = B.rowPtrs;
1485  double **CrowPtrs = C.rowPtrs;
1486 
1487  for (unsigned int i = 0; i < A.rowNum; i++) {
1488  for (unsigned int j = 0; j < A.colNum; j++) {
1489  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1490  }
1491  }
1492 }
1493 
1506 void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1507 {
1508  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1509  C.resize(A.rowNum, A.colNum, false, false);
1510 
1511  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1512  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1513  A.getCols(), B.getRows(), B.getCols()));
1514  }
1515 
1516  double **ArowPtrs = A.rowPtrs;
1517  double **BrowPtrs = B.rowPtrs;
1518  double **CrowPtrs = C.rowPtrs;
1519 
1520  for (unsigned int i = 0; i < A.rowNum; i++) {
1521  for (unsigned int j = 0; j < A.colNum; j++) {
1522  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1523  }
1524  }
1525 }
1526 
1532 {
1533  vpMatrix C;
1534  vpMatrix::sub2Matrices(*this, B, C);
1535  return C;
1536 }
1537 
1539 
1541 {
1542  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1543  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1544  B.getRows(), B.getCols()));
1545  }
1546 
1547  double **BrowPtrs = B.rowPtrs;
1548 
1549  for (unsigned int i = 0; i < rowNum; i++)
1550  for (unsigned int j = 0; j < colNum; j++)
1551  rowPtrs[i][j] += BrowPtrs[i][j];
1552 
1553  return *this;
1554 }
1555 
1558 {
1559  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1560  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1561  B.getRows(), B.getCols()));
1562  }
1563 
1564  double **BrowPtrs = B.rowPtrs;
1565  for (unsigned int i = 0; i < rowNum; i++)
1566  for (unsigned int j = 0; j < colNum; j++)
1567  rowPtrs[i][j] -= BrowPtrs[i][j];
1568 
1569  return *this;
1570 }
1571 
1582 {
1583  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1584  C.resize(A.rowNum, A.colNum, false, false);
1585 
1586  double **ArowPtrs = A.rowPtrs;
1587  double **CrowPtrs = C.rowPtrs;
1588 
1589  for (unsigned int i = 0; i < A.rowNum; i++)
1590  for (unsigned int j = 0; j < A.colNum; j++)
1591  CrowPtrs[i][j] = -ArowPtrs[i][j];
1592 }
1593 
1599 {
1600  vpMatrix C;
1601  vpMatrix::negateMatrix(*this, C);
1602  return C;
1603 }
1604 
1605 double vpMatrix::sum() const
1606 {
1607  double s = 0.0;
1608  for (unsigned int i = 0; i < rowNum; i++) {
1609  for (unsigned int j = 0; j < colNum; j++) {
1610  s += rowPtrs[i][j];
1611  }
1612  }
1613 
1614  return s;
1615 }
1616 
1617 //---------------------------------
1618 // Matrix/vector operations.
1619 //---------------------------------
1620 
1621 //---------------------------------
1622 // Matrix/real operations.
1623 //---------------------------------
1624 
1629 vpMatrix operator*(const double &x, const vpMatrix &B)
1630 {
1631  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1632  return B;
1633  }
1634 
1635  unsigned int Brow = B.getRows();
1636  unsigned int Bcol = B.getCols();
1637 
1638  vpMatrix C;
1639  C.resize(Brow, Bcol, false, false);
1640 
1641  for (unsigned int i = 0; i < Brow; i++)
1642  for (unsigned int j = 0; j < Bcol; j++)
1643  C[i][j] = B[i][j] * x;
1644 
1645  return C;
1646 }
1647 
1653 {
1654  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1655  return (*this);
1656  }
1657 
1658  vpMatrix M;
1659  M.resize(rowNum, colNum, false, false);
1660 
1661  for (unsigned int i = 0; i < rowNum; i++)
1662  for (unsigned int j = 0; j < colNum; j++)
1663  M[i][j] = rowPtrs[i][j] * x;
1664 
1665  return M;
1666 }
1667 
1670 {
1671  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1672  return (*this);
1673  }
1674 
1675  if (std::fabs(x) < std::numeric_limits<double>::epsilon()) {
1676  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1677  }
1678 
1679  vpMatrix C;
1680  C.resize(rowNum, colNum, false, false);
1681 
1682  double xinv = 1 / x;
1683 
1684  for (unsigned int i = 0; i < rowNum; i++)
1685  for (unsigned int j = 0; j < colNum; j++)
1686  C[i][j] = rowPtrs[i][j] * xinv;
1687 
1688  return C;
1689 }
1690 
1693 {
1694  for (unsigned int i = 0; i < rowNum; i++)
1695  for (unsigned int j = 0; j < colNum; j++)
1696  rowPtrs[i][j] += x;
1697 
1698  return *this;
1699 }
1700 
1703 {
1704  for (unsigned int i = 0; i < rowNum; i++)
1705  for (unsigned int j = 0; j < colNum; j++)
1706  rowPtrs[i][j] -= x;
1707 
1708  return *this;
1709 }
1710 
1716 {
1717  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1718  return *this;
1719  }
1720 
1721  for (unsigned int i = 0; i < rowNum; i++)
1722  for (unsigned int j = 0; j < colNum; j++)
1723  rowPtrs[i][j] *= x;
1724 
1725  return *this;
1726 }
1727 
1730 {
1731  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1732  return *this;
1733  }
1734 
1735  if (std::fabs(x) < std::numeric_limits<double>::epsilon())
1736  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1737 
1738  double xinv = 1 / x;
1739 
1740  for (unsigned int i = 0; i < rowNum; i++)
1741  for (unsigned int j = 0; j < colNum; j++)
1742  rowPtrs[i][j] *= xinv;
1743 
1744  return *this;
1745 }
1746 
1747 //----------------------------------------------------------------
1748 // Matrix Operation
1749 //----------------------------------------------------------------
1750 
1756 {
1757  if ((out.rowNum != colNum * rowNum) || (out.colNum != 1))
1758  out.resize(colNum * rowNum, false, false);
1759 
1760  double *optr = out.data;
1761  for (unsigned int j = 0; j < colNum; j++) {
1762  for (unsigned int i = 0; i < rowNum; i++) {
1763  *(optr++) = rowPtrs[i][j];
1764  }
1765  }
1766 }
1767 
1773 {
1774  vpColVector out(colNum * rowNum);
1775  stackColumns(out);
1776  return out;
1777 }
1778 
1784 {
1785  if ((out.getRows() != 1) || (out.getCols() != colNum * rowNum))
1786  out.resize(colNum * rowNum, false, false);
1787 
1788  memcpy(out.data, data, sizeof(double) * out.getCols());
1789 }
1795 {
1796  vpRowVector out(colNum * rowNum);
1797  stackRows(out);
1798  return out;
1799 }
1800 
1808 {
1809  if (m.getRows() != rowNum || m.getCols() != colNum) {
1810  throw(vpException(vpException::dimensionError, "In Hadamard product: bad dimension of input matrix"));
1811  }
1812 
1813  vpMatrix out;
1814  out.resize(rowNum, colNum, false, false);
1815 
1816 #if defined(VISP_HAVE_SIMDLIB)
1817  SimdVectorHadamard(data, m.data, dsize, out.data);
1818 #else
1819  for (unsigned int i = 0; i < dsize; ++i) {
1820  out.data[i] = data[i] * m.data[i];
1821  }
1822 #endif
1823 
1824  return out;
1825 }
1826 
1833 void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out)
1834 {
1835  unsigned int r1 = m1.getRows();
1836  unsigned int c1 = m1.getCols();
1837  unsigned int r2 = m2.getRows();
1838  unsigned int c2 = m2.getCols();
1839 
1840  out.resize(r1 * r2, c1 * c2, false, false);
1841 
1842  for (unsigned int r = 0; r < r1; r++) {
1843  for (unsigned int c = 0; c < c1; c++) {
1844  double alpha = m1[r][c];
1845  double *m2ptr = m2[0];
1846  unsigned int roffset = r * r2;
1847  unsigned int coffset = c * c2;
1848  for (unsigned int rr = 0; rr < r2; rr++) {
1849  for (unsigned int cc = 0; cc < c2; cc++) {
1850  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1851  }
1852  }
1853  }
1854  }
1855 }
1856 
1863 void vpMatrix::kron(const vpMatrix &m, vpMatrix &out) const { kron(*this, m, out); }
1864 
1872 {
1873  unsigned int r1 = m1.getRows();
1874  unsigned int c1 = m1.getCols();
1875  unsigned int r2 = m2.getRows();
1876  unsigned int c2 = m2.getCols();
1877 
1878  vpMatrix out;
1879  out.resize(r1 * r2, c1 * c2, false, false);
1880 
1881  for (unsigned int r = 0; r < r1; r++) {
1882  for (unsigned int c = 0; c < c1; c++) {
1883  double alpha = m1[r][c];
1884  double *m2ptr = m2[0];
1885  unsigned int roffset = r * r2;
1886  unsigned int coffset = c * c2;
1887  for (unsigned int rr = 0; rr < r2; rr++) {
1888  for (unsigned int cc = 0; cc < c2; cc++) {
1889  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1890  }
1891  }
1892  }
1893  }
1894  return out;
1895 }
1896 
1902 vpMatrix vpMatrix::kron(const vpMatrix &m) const { return kron(*this, m); }
1903 
1954 void vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const { x = pseudoInverse(1e-6) * b; }
1955 
2006 {
2007  vpColVector X(colNum);
2008 
2009  solveBySVD(B, X);
2010  return X;
2011 }
2012 
2077 {
2078 #if defined(VISP_HAVE_LAPACK)
2079  svdLapack(w, V);
2080 #elif defined(VISP_HAVE_EIGEN3)
2081  svdEigen3(w, V);
2082 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2083  svdOpenCV(w, V);
2084 #else
2085  (void)w;
2086  (void)V;
2087  throw(vpException(vpException::fatalError, "Cannot compute SVD. Install Lapack, Eigen3 or OpenCV 3rd party"));
2088 #endif
2089 }
2090 
2145 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
2146 {
2147 #if defined(VISP_HAVE_LAPACK)
2148  return pseudoInverseLapack(Ap, svThreshold);
2149 #elif defined(VISP_HAVE_EIGEN3)
2150  return pseudoInverseEigen3(Ap, svThreshold);
2151 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2152  return pseudoInverseOpenCV(Ap, svThreshold);
2153 #else
2154  (void)Ap;
2155  (void)svThreshold;
2156  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2157  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2158 #endif
2159 }
2160 
2221 int vpMatrix::pseudoInverse(vpMatrix &Ap, int rank_in) const
2222 {
2223 #if defined(VISP_HAVE_LAPACK)
2224  return pseudoInverseLapack(Ap, rank_in);
2225 #elif defined(VISP_HAVE_EIGEN3)
2226  return pseudoInverseEigen3(Ap, rank_in);
2227 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2228  return pseudoInverseOpenCV(Ap, rank_in);
2229 #else
2230  (void)Ap;
2231  (void)rank_in;
2232  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2233  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2234 #endif
2235 }
2236 
2287 vpMatrix vpMatrix::pseudoInverse(double svThreshold) const
2288 {
2289 #if defined(VISP_HAVE_LAPACK)
2290  return pseudoInverseLapack(svThreshold);
2291 #elif defined(VISP_HAVE_EIGEN3)
2292  return pseudoInverseEigen3(svThreshold);
2293 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2294  return pseudoInverseOpenCV(svThreshold);
2295 #else
2296  (void)svThreshold;
2297  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2298  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2299 #endif
2300 }
2301 
2353 {
2354 #if defined(VISP_HAVE_LAPACK)
2355  return pseudoInverseLapack(rank_in);
2356 #elif defined(VISP_HAVE_EIGEN3)
2357  return pseudoInverseEigen3(rank_in);
2358 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2359  return pseudoInverseOpenCV(rank_in);
2360 #else
2361  (void)rank_in;
2362  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2363  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2364 #endif
2365 }
2366 
2367 #ifndef DOXYGEN_SHOULD_SKIP_THIS
2368 #if defined(VISP_HAVE_LAPACK)
2405 vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const
2406 {
2407  unsigned int nrows = getRows();
2408  unsigned int ncols = getCols();
2409  int rank_out;
2410 
2411  vpMatrix U, V, Ap;
2412  vpColVector sv;
2413 
2414  U = *this;
2415  U.svdLapack(sv, V);
2416 
2417  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
2418 
2419  return Ap;
2420 }
2421 
2462 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) const
2463 {
2464  unsigned int nrows = getRows();
2465  unsigned int ncols = getCols();
2466  int rank_out;
2467 
2468  vpMatrix U, V;
2469  vpColVector sv;
2470 
2471  U = *this;
2472  U.svdLapack(sv, V);
2473 
2474  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
2475 
2476  return static_cast<unsigned int>(rank_out);
2477 }
2525 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2526 {
2527  unsigned int nrows = getRows();
2528  unsigned int ncols = getCols();
2529  int rank_out;
2530 
2531  vpMatrix U, V;
2532 
2533  Ap.resize(ncols, nrows, false, false);
2534 
2535  U = *this;
2536  U.svdLapack(sv, V);
2537 
2538  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
2539 
2540  return static_cast<unsigned int>(rank_out);
2541 }
2542 
2649 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
2650  vpMatrix &imAt, vpMatrix &kerAt) const
2651 {
2652  unsigned int nrows = getRows();
2653  unsigned int ncols = getCols();
2654  int rank_out;
2655  vpMatrix U, V;
2656 
2657  if (nrows < ncols) {
2658  U.resize(ncols, ncols, true);
2659  U.insert(*this, 0, 0);
2660  }
2661  else {
2662  U = *this;
2663  }
2664 
2665  U.svdLapack(sv, V);
2666 
2667  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt);
2668 
2669  return static_cast<unsigned int>(rank_out);
2670 }
2671 
2708 vpMatrix vpMatrix::pseudoInverseLapack(int rank_in) const
2709 {
2710  unsigned int nrows = getRows();
2711  unsigned int ncols = getCols();
2712  int rank_out;
2713  double svThreshold = 1e-26;
2714 
2715  vpMatrix U, V, Ap;
2716  vpColVector sv;
2717 
2718  U = *this;
2719  U.svdLapack(sv, V);
2720 
2721  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
2722 
2723  return Ap;
2724 }
2725 
2772 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, int rank_in) const
2773 {
2774  unsigned int nrows = getRows();
2775  unsigned int ncols = getCols();
2776  int rank_out;
2777  double svThreshold = 1e-26;
2778 
2779  vpMatrix U, V;
2780  vpColVector sv;
2781 
2782  U = *this;
2783  U.svdLapack(sv, V);
2784 
2785  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
2786 
2787  return rank_out;
2788 }
2789 
2843 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in) const
2844 {
2845  unsigned int nrows = getRows();
2846  unsigned int ncols = getCols();
2847  int rank_out;
2848  double svThreshold = 1e-26;
2849  vpMatrix U, V;
2850 
2851  Ap.resize(ncols, nrows, false, false);
2852 
2853  U = *this;
2854  U.svdLapack(sv, V);
2855 
2856  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
2857 
2858  return rank_out;
2859 }
2860 
2972 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
2973  vpMatrix &kerAt) const
2974 {
2975  unsigned int nrows = getRows();
2976  unsigned int ncols = getCols();
2977  int rank_out;
2978  double svThreshold = 1e-26;
2979  vpMatrix U, V;
2980 
2981  if (nrows < ncols) {
2982  U.resize(ncols, ncols, true);
2983  U.insert(*this, 0, 0);
2984  }
2985  else {
2986  U = *this;
2987  }
2988 
2989  U.svdLapack(sv, V);
2990 
2991  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
2992 
2993  return rank_out;
2994 }
2995 
2996 #endif
2997 #if defined(VISP_HAVE_EIGEN3)
3034 vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const
3035 {
3036  unsigned int nrows = getRows();
3037  unsigned int ncols = getCols();
3038  int rank_out;
3039  vpMatrix U, V, Ap;
3040  vpColVector sv;
3041 
3042  U = *this;
3043  U.svdEigen3(sv, V);
3044 
3045  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
3046 
3047  return Ap;
3048 }
3049 
3090 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) const
3091 {
3092  unsigned int nrows = getRows();
3093  unsigned int ncols = getCols();
3094  int rank_out;
3095  vpMatrix U, V;
3096  vpColVector sv;
3097 
3098  U = *this;
3099  U.svdEigen3(sv, V);
3100 
3101  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
3102 
3103  return static_cast<unsigned int>(rank_out);
3104 }
3105 
3153 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3154 {
3155  unsigned int nrows = getRows();
3156  unsigned int ncols = getCols();
3157  int rank_out;
3158  vpMatrix U, V;
3159 
3160  Ap.resize(ncols, nrows, false, false);
3161 
3162  U = *this;
3163  U.svdEigen3(sv, V);
3164 
3165  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
3166 
3167  return static_cast<unsigned int>(rank_out);
3168 }
3169 
3276 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3277  vpMatrix &imAt, vpMatrix &kerAt) const
3278 {
3279  unsigned int nrows = getRows();
3280  unsigned int ncols = getCols();
3281  int rank_out;
3282  vpMatrix U, V;
3283 
3284  if (nrows < ncols) {
3285  U.resize(ncols, ncols, true);
3286  U.insert(*this, 0, 0);
3287  }
3288  else {
3289  U = *this;
3290  }
3291 
3292  U.svdEigen3(sv, V);
3293 
3294  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt);
3295 
3296  return static_cast<unsigned int>(rank_out);
3297 }
3298 
3335 vpMatrix vpMatrix::pseudoInverseEigen3(int rank_in) const
3336 {
3337  unsigned int nrows = getRows();
3338  unsigned int ncols = getCols();
3339  int rank_out;
3340  double svThreshold = 1e-26;
3341  vpMatrix U, V, Ap;
3342  vpColVector sv;
3343 
3344  U = *this;
3345  U.svdEigen3(sv, V);
3346 
3347  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
3348 
3349  return Ap;
3350 }
3351 
3398 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, int rank_in) const
3399 {
3400  unsigned int nrows = getRows();
3401  unsigned int ncols = getCols();
3402  int rank_out;
3403  double svThreshold = 1e-26;
3404  vpMatrix U, V;
3405  vpColVector sv;
3406 
3407  Ap.resize(ncols, nrows, false, false);
3408 
3409  U = *this;
3410  U.svdEigen3(sv, V);
3411 
3412  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
3413 
3414  return rank_out;
3415 }
3416 
3470 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in) const
3471 {
3472  unsigned int nrows = getRows();
3473  unsigned int ncols = getCols();
3474  int rank_out;
3475  double svThreshold = 1e-26;
3476  vpMatrix U, V;
3477 
3478  Ap.resize(ncols, nrows, false, false);
3479 
3480  U = *this;
3481  U.svdEigen3(sv, V);
3482 
3483  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
3484 
3485  return rank_out;
3486 }
3487 
3599 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
3600  vpMatrix &kerAt) const
3601 {
3602  unsigned int nrows = getRows();
3603  unsigned int ncols = getCols();
3604  int rank_out;
3605  double svThreshold = 1e-26;
3606  vpMatrix U, V;
3607 
3608  if (nrows < ncols) {
3609  U.resize(ncols, ncols, true);
3610  U.insert(*this, 0, 0);
3611  }
3612  else {
3613  U = *this;
3614  }
3615  U.svdEigen3(sv, V);
3616 
3617  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
3618 
3619  return rank_out;
3620 }
3621 
3622 #endif
3623 #if defined(VISP_HAVE_OPENCV)
3660 vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const
3661 {
3662  unsigned int nrows = getRows();
3663  unsigned int ncols = getCols();
3664  int rank_out;
3665  vpMatrix U, V, Ap;
3666  vpColVector sv;
3667 
3668  U = *this;
3669  U.svdOpenCV(sv, V);
3670 
3671  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
3672 
3673  return Ap;
3674 }
3675 
3716 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) const
3717 {
3718  unsigned int nrows = getRows();
3719  unsigned int ncols = getCols();
3720  int rank_out;
3721  vpMatrix U, V;
3722  vpColVector sv;
3723 
3724  Ap.resize(ncols, nrows, false, false);
3725 
3726  U = *this;
3727  U.svdOpenCV(sv, V);
3728 
3729  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
3730 
3731  return static_cast<unsigned int>(rank_out);
3732 }
3780 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3781 {
3782  unsigned int nrows = getRows();
3783  unsigned int ncols = getCols();
3784  int rank_out;
3785  vpMatrix U, V;
3786 
3787  Ap.resize(ncols, nrows, false, false);
3788 
3789  U = *this;
3790  U.svdOpenCV(sv, V);
3791 
3792  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
3793 
3794  return static_cast<unsigned int>(rank_out);
3795 }
3796 
3903 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3904  vpMatrix &imAt, vpMatrix &kerAt) const
3905 {
3906  unsigned int nrows = getRows();
3907  unsigned int ncols = getCols();
3908  int rank_out;
3909  vpMatrix U, V;
3910 
3911  if (nrows < ncols) {
3912  U.resize(ncols, ncols, true);
3913  U.insert(*this, 0, 0);
3914  }
3915  else {
3916  U = *this;
3917  }
3918 
3919  U.svdOpenCV(sv, V);
3920 
3921  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt);
3922 
3923  return static_cast<unsigned int>(rank_out);
3924 }
3925 
3962 vpMatrix vpMatrix::pseudoInverseOpenCV(int rank_in) const
3963 {
3964  unsigned int nrows = getRows();
3965  unsigned int ncols = getCols();
3966  int rank_out;
3967  double svThreshold = 1e-26;
3968  vpMatrix U, V, Ap;
3969  vpColVector sv;
3970 
3971  U = *this;
3972  U.svdOpenCV(sv, V);
3973 
3974  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
3975 
3976  return Ap;
3977 }
3978 
4025 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, int rank_in) const
4026 {
4027  unsigned int nrows = getRows();
4028  unsigned int ncols = getCols();
4029  int rank_out;
4030  double svThreshold = 1e-26;
4031  vpMatrix U, V;
4032  vpColVector sv;
4033 
4034  Ap.resize(ncols, nrows, false, false);
4035 
4036  U = *this;
4037  U.svdOpenCV(sv, V);
4038 
4039  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
4040 
4041  return rank_out;
4042 }
4043 
4097 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4098 {
4099  unsigned int nrows = getRows();
4100  unsigned int ncols = getCols();
4101  int rank_out;
4102  double svThreshold = 1e-26;
4103  vpMatrix U, V;
4104 
4105  Ap.resize(ncols, nrows, false, false);
4106 
4107  U = *this;
4108  U.svdOpenCV(sv, V);
4109 
4110  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
4111 
4112  return rank_out;
4113 }
4114 
4226 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
4227  vpMatrix &kerAt) const
4228 {
4229  unsigned int nrows = getRows();
4230  unsigned int ncols = getCols();
4231  int rank_out;
4232  double svThreshold = 1e-26;
4233  vpMatrix U, V;
4234 
4235  if (nrows < ncols) {
4236  U.resize(ncols, ncols, true);
4237  U.insert(*this, 0, 0);
4238  }
4239  else {
4240  U = *this;
4241  }
4242 
4243  U.svdOpenCV(sv, V);
4244 
4245  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
4246 
4247  return rank_out;
4248 }
4249 
4250 #endif
4251 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
4252 
4314 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
4315 {
4316 #if defined(VISP_HAVE_LAPACK)
4317  return pseudoInverseLapack(Ap, sv, svThreshold);
4318 #elif defined(VISP_HAVE_EIGEN3)
4319  return pseudoInverseEigen3(Ap, sv, svThreshold);
4320 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
4321  return pseudoInverseOpenCV(Ap, sv, svThreshold);
4322 #else
4323  (void)Ap;
4324  (void)sv;
4325  (void)svThreshold;
4326  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4327  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4328 #endif
4329 }
4330 
4397 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4398 {
4399 #if defined(VISP_HAVE_LAPACK)
4400  return pseudoInverseLapack(Ap, sv, rank_in);
4401 #elif defined(VISP_HAVE_EIGEN3)
4402  return pseudoInverseEigen3(Ap, sv, rank_in);
4403 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
4404  return pseudoInverseOpenCV(Ap, sv, rank_in);
4405 #else
4406  (void)Ap;
4407  (void)sv;
4408  (void)rank_in;
4409  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4410  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4411 #endif
4412 }
4487 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt) const
4488 {
4489  vpMatrix kerAt;
4490  return pseudoInverse(Ap, sv, svThreshold, imA, imAt, kerAt);
4491 }
4492 
4571 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt) const
4572 {
4573  vpMatrix kerAt;
4574  return pseudoInverse(Ap, sv, rank_in, imA, imAt, kerAt);
4575 }
4576 
4678 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt,
4679  vpMatrix &kerAt) const
4680 {
4681 #if defined(VISP_HAVE_LAPACK)
4682  return pseudoInverseLapack(Ap, sv, svThreshold, imA, imAt, kerAt);
4683 #elif defined(VISP_HAVE_EIGEN3)
4684  return pseudoInverseEigen3(Ap, sv, svThreshold, imA, imAt, kerAt);
4685 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
4686  return pseudoInverseOpenCV(Ap, sv, svThreshold, imA, imAt, kerAt);
4687 #else
4688  (void)Ap;
4689  (void)sv;
4690  (void)svThreshold;
4691  (void)imA;
4692  (void)imAt;
4693  (void)kerAt;
4694  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4695  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4696 #endif
4697 }
4698 
4805 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
4806  vpMatrix &kerAt) const
4807 {
4808 #if defined(VISP_HAVE_LAPACK)
4809  return pseudoInverseLapack(Ap, sv, rank_in, imA, imAt, kerAt);
4810 #elif defined(VISP_HAVE_EIGEN3)
4811  return pseudoInverseEigen3(Ap, sv, rank_in, imA, imAt, kerAt);
4812 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
4813  return pseudoInverseOpenCV(Ap, sv, rank_in, imA, imAt, kerAt);
4814 #else
4815  (void)Ap;
4816  (void)sv;
4817  (void)rank_in;
4818  (void)imA;
4819  (void)imAt;
4820  (void)kerAt;
4821  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4822  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4823 #endif
4824 }
4825 
4867 vpColVector vpMatrix::getCol(unsigned int j, unsigned int i_begin, unsigned int column_size) const
4868 {
4869  if (i_begin + column_size > getRows() || j >= getCols())
4870  throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(),
4871  getCols()));
4872  vpColVector c(column_size);
4873  for (unsigned int i = 0; i < column_size; i++)
4874  c[i] = (*this)[i_begin + i][j];
4875  return c;
4876 }
4877 
4917 vpColVector vpMatrix::getCol(unsigned int j) const { return getCol(j, 0, rowNum); }
4918 
4955 vpRowVector vpMatrix::getRow(unsigned int i) const { return getRow(i, 0, colNum); }
4956 
4996 vpRowVector vpMatrix::getRow(unsigned int i, unsigned int j_begin, unsigned int row_size) const
4997 {
4998  if (j_begin + row_size > colNum || i >= rowNum)
4999  throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
5000 
5001  vpRowVector r(row_size);
5002  if (r.data != nullptr && data != nullptr) {
5003  memcpy(r.data, (*this)[i] + j_begin, row_size * sizeof(double));
5004  }
5005 
5006  return r;
5007 }
5008 
5045 {
5046  unsigned int min_size = std::min<unsigned int>(rowNum, colNum);
5047  vpColVector diag;
5048 
5049  if (min_size > 0) {
5050  diag.resize(min_size, false);
5051 
5052  for (unsigned int i = 0; i < min_size; i++) {
5053  diag[i] = (*this)[i][i];
5054  }
5055  }
5056 
5057  return diag;
5058 }
5059 
5071 {
5072  vpMatrix C;
5073 
5074  vpMatrix::stack(A, B, C);
5075 
5076  return C;
5077 }
5078 
5090 void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
5091 {
5092  unsigned int nra = A.getRows();
5093  unsigned int nrb = B.getRows();
5094 
5095  if (nra != 0) {
5096  if (A.getCols() != B.getCols()) {
5097  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5098  A.getCols(), B.getRows(), B.getCols()));
5099  }
5100  }
5101 
5102  if (A.data != nullptr && A.data == C.data) {
5103  std::cerr << "A and C must be two different objects!" << std::endl;
5104  return;
5105  }
5106 
5107  if (B.data != nullptr && B.data == C.data) {
5108  std::cerr << "B and C must be two different objects!" << std::endl;
5109  return;
5110  }
5111 
5112  C.resize(nra + nrb, B.getCols(), false, false);
5113 
5114  if (C.data != nullptr && A.data != nullptr && A.size() > 0) {
5115  // Copy A in C
5116  memcpy(C.data, A.data, sizeof(double) * A.size());
5117  }
5118 
5119  if (C.data != nullptr && B.data != nullptr && B.size() > 0) {
5120  // Copy B in C
5121  memcpy(C.data + A.size(), B.data, sizeof(double) * B.size());
5122  }
5123 }
5124 
5135 {
5136  vpMatrix C;
5137  vpMatrix::stack(A, r, C);
5138 
5139  return C;
5140 }
5141 
5153 void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
5154 {
5155  if (A.data != nullptr && A.data == C.data) {
5156  std::cerr << "A and C must be two different objects!" << std::endl;
5157  return;
5158  }
5159 
5160  C = A;
5161  C.stack(r);
5162 }
5163 
5174 {
5175  vpMatrix C;
5176  vpMatrix::stack(A, c, C);
5177 
5178  return C;
5179 }
5180 
5192 void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C)
5193 {
5194  if (A.data != nullptr && A.data == C.data) {
5195  std::cerr << "A and C must be two different objects!" << std::endl;
5196  return;
5197  }
5198 
5199  C = A;
5200  C.stack(c);
5201 }
5202 
5215 vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, unsigned int r, unsigned int c)
5216 {
5218 
5219  vpArray2D<double>::insert(A, B, C, r, c);
5220 
5221  return vpMatrix(C);
5222 }
5223 
5237 void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, unsigned int r, unsigned int c)
5238 {
5239  vpArray2D<double> C_array;
5240 
5241  vpArray2D<double>::insert(A, B, C_array, r, c);
5242 
5243  C = C_array;
5244 }
5245 
5258 {
5259  vpMatrix C;
5260 
5261  juxtaposeMatrices(A, B, C);
5262 
5263  return C;
5264 }
5265 
5279 {
5280  unsigned int nca = A.getCols();
5281  unsigned int ncb = B.getCols();
5282 
5283  if (nca != 0) {
5284  if (A.getRows() != B.getRows()) {
5285  throw(vpException(vpException::dimensionError, "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5286  A.getCols(), B.getRows(), B.getCols()));
5287  }
5288  }
5289 
5290  if (B.getRows() == 0 || nca + ncb == 0) {
5291  std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl;
5292  return;
5293  }
5294 
5295  C.resize(B.getRows(), nca + ncb, false, false);
5296 
5297  C.insert(A, 0, 0);
5298  C.insert(B, 0, nca);
5299 }
5300 
5301 //--------------------------------------------------------------------
5302 // Output
5303 //--------------------------------------------------------------------
5304 
5324 int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &intro) const
5325 {
5326  typedef std::string::size_type size_type;
5327 
5328  unsigned int m = getRows();
5329  unsigned int n = getCols();
5330 
5331  std::vector<std::string> values(m * n);
5332  std::ostringstream oss;
5333  std::ostringstream ossFixed;
5334  std::ios_base::fmtflags original_flags = oss.flags();
5335 
5336  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
5337 
5338  size_type maxBefore = 0; // the length of the integral part
5339  size_type maxAfter = 0; // number of decimals plus
5340  // one place for the decimal point
5341  for (unsigned int i = 0; i < m; ++i) {
5342  for (unsigned int j = 0; j < n; ++j) {
5343  oss.str("");
5344  oss << (*this)[i][j];
5345  if (oss.str().find("e") != std::string::npos) {
5346  ossFixed.str("");
5347  ossFixed << (*this)[i][j];
5348  oss.str(ossFixed.str());
5349  }
5350 
5351  values[i * n + j] = oss.str();
5352  size_type thislen = values[i * n + j].size();
5353  size_type p = values[i * n + j].find('.');
5354 
5355  if (p == std::string::npos) {
5356  maxBefore = vpMath::maximum(maxBefore, thislen);
5357  // maxAfter remains the same
5358  }
5359  else {
5360  maxBefore = vpMath::maximum(maxBefore, p);
5361  maxAfter = vpMath::maximum(maxAfter, thislen - p);
5362  }
5363  }
5364  }
5365 
5366  size_type totalLength = length;
5367  // increase totalLength according to maxBefore
5368  totalLength = vpMath::maximum(totalLength, maxBefore);
5369  // decrease maxAfter according to totalLength
5370  maxAfter = std::min<size_type>(maxAfter, totalLength - maxBefore);
5371 
5372  if (!intro.empty())
5373  s << intro;
5374  s << "[" << m << "," << n << "]=\n";
5375 
5376  for (unsigned int i = 0; i < m; i++) {
5377  s << " ";
5378  for (unsigned int j = 0; j < n; j++) {
5379  size_type p = values[i * n + j].find('.');
5380  s.setf(std::ios::right, std::ios::adjustfield);
5381  s.width((std::streamsize)maxBefore);
5382  s << values[i * n + j].substr(0, p).c_str();
5383 
5384  if (maxAfter > 0) {
5385  s.setf(std::ios::left, std::ios::adjustfield);
5386  if (p != std::string::npos) {
5387  s.width((std::streamsize)maxAfter);
5388  s << values[i * n + j].substr(p, maxAfter).c_str();
5389  }
5390  else {
5391  s.width((std::streamsize)maxAfter);
5392  s << ".0";
5393  }
5394  }
5395 
5396  s << ' ';
5397  }
5398  s << std::endl;
5399  }
5400 
5401  s.flags(original_flags); // restore s to standard state
5402 
5403  return (int)(maxBefore + maxAfter);
5404 }
5405 
5442 std::ostream &vpMatrix::matlabPrint(std::ostream &os) const
5443 {
5444  os << "[ ";
5445  for (unsigned int i = 0; i < this->getRows(); ++i) {
5446  for (unsigned int j = 0; j < this->getCols(); ++j) {
5447  os << (*this)[i][j] << ", ";
5448  }
5449  if (this->getRows() != i + 1) {
5450  os << ";" << std::endl;
5451  }
5452  else {
5453  os << "]" << std::endl;
5454  }
5455  }
5456  return os;
5457 }
5458 
5487 std::ostream &vpMatrix::maplePrint(std::ostream &os) const
5488 {
5489  os << "([ " << std::endl;
5490  for (unsigned int i = 0; i < this->getRows(); ++i) {
5491  os << "[";
5492  for (unsigned int j = 0; j < this->getCols(); ++j) {
5493  os << (*this)[i][j] << ", ";
5494  }
5495  os << "]," << std::endl;
5496  }
5497  os << "])" << std::endl;
5498  return os;
5499 }
5500 
5528 std::ostream &vpMatrix::csvPrint(std::ostream &os) const
5529 {
5530  for (unsigned int i = 0; i < this->getRows(); ++i) {
5531  for (unsigned int j = 0; j < this->getCols(); ++j) {
5532  os << (*this)[i][j];
5533  if (!(j == (this->getCols() - 1)))
5534  os << ", ";
5535  }
5536  os << std::endl;
5537  }
5538  return os;
5539 }
5540 
5576 std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
5577 {
5578  os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " << this->getCols() << "); " << std::endl;
5579 
5580  for (unsigned int i = 0; i < this->getRows(); ++i) {
5581  for (unsigned int j = 0; j < this->getCols(); ++j) {
5582  if (!octet) {
5583  os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl;
5584  }
5585  else {
5586  for (unsigned int k = 0; k < sizeof(double); ++k) {
5587  os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex
5588  << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl;
5589  }
5590  }
5591  }
5592  os << std::endl;
5593  }
5594  return os;
5595 }
5596 
5602 {
5603  if (rowNum == 0) {
5604  *this = A;
5605  }
5606  else if (A.getRows() > 0) {
5607  if (colNum != A.getCols()) {
5608  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum,
5609  A.getRows(), A.getCols()));
5610  }
5611 
5612  unsigned int rowNumOld = rowNum;
5613  resize(rowNum + A.getRows(), colNum, false, false);
5614  insert(A, rowNumOld, 0);
5615  }
5616 }
5617 
5634 {
5635  if (rowNum == 0) {
5636  *this = r;
5637  }
5638  else {
5639  if (colNum != r.getCols()) {
5640  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum,
5641  colNum, r.getCols()));
5642  }
5643 
5644  if (r.size() == 0) {
5645  return;
5646  }
5647 
5648  unsigned int oldSize = size();
5649  resize(rowNum + 1, colNum, false, false);
5650 
5651  if (data != nullptr && r.data != nullptr && data != r.data) {
5652  // Copy r in data
5653  memcpy(data + oldSize, r.data, sizeof(double) * r.size());
5654  }
5655  }
5656 }
5657 
5675 {
5676  if (colNum == 0) {
5677  *this = c;
5678  }
5679  else {
5680  if (rowNum != c.getRows()) {
5681  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum,
5682  colNum, c.getRows()));
5683  }
5684 
5685  if (c.size() == 0) {
5686  return;
5687  }
5688 
5689  vpMatrix tmp = *this;
5690  unsigned int oldColNum = colNum;
5691  resize(rowNum, colNum + 1, false, false);
5692 
5693  if (data != nullptr && tmp.data != nullptr && data != tmp.data) {
5694  // Copy c in data
5695  for (unsigned int i = 0; i < rowNum; i++) {
5696  memcpy(data + i * colNum, tmp.data + i * oldColNum, sizeof(double) * oldColNum);
5697  rowPtrs[i][oldColNum] = c[i];
5698  }
5699  }
5700  }
5701 }
5702 
5713 void vpMatrix::insert(const vpMatrix &A, unsigned int r, unsigned int c)
5714 {
5715  if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) {
5716  if (A.colNum == colNum && data != nullptr && A.data != nullptr && A.data != data) {
5717  memcpy(data + r * colNum, A.data, sizeof(double) * A.size());
5718  }
5719  else if (data != nullptr && A.data != nullptr && A.data != data) {
5720  for (unsigned int i = r; i < (r + A.getRows()); i++) {
5721  memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum);
5722  }
5723  }
5724  }
5725  else {
5726  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
5727  A.getRows(), A.getCols(), rowNum, colNum, r, c);
5728  }
5729 }
5730 
5768 {
5769  vpColVector evalue(rowNum); // Eigen values
5770 
5771  if (rowNum != colNum) {
5772  throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
5773  colNum));
5774  }
5775 
5776  // Check if the matrix is symmetric: At - A = 0
5777  vpMatrix At_A = (*this).t() - (*this);
5778  for (unsigned int i = 0; i < rowNum; i++) {
5779  for (unsigned int j = 0; j < rowNum; j++) {
5780  // if (At_A[i][j] != 0) {
5781  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
5782  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix"));
5783  }
5784  }
5785  }
5786 
5787 #if defined(VISP_HAVE_LAPACK)
5788 #if defined(VISP_HAVE_GSL) /* be careful of the copy below */
5789  {
5790  gsl_vector *eval = gsl_vector_alloc(rowNum);
5791  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
5792 
5793  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
5794  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
5795 
5796  unsigned int Atda = (unsigned int)m->tda;
5797  for (unsigned int i = 0; i < rowNum; i++) {
5798  unsigned int k = i * Atda;
5799  for (unsigned int j = 0; j < colNum; j++)
5800  m->data[k + j] = (*this)[i][j];
5801  }
5802  gsl_eigen_symmv(m, eval, evec, w);
5803 
5804  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
5805 
5806  for (unsigned int i = 0; i < rowNum; i++) {
5807  evalue[i] = gsl_vector_get(eval, i);
5808  }
5809 
5810  gsl_eigen_symmv_free(w);
5811  gsl_vector_free(eval);
5812  gsl_matrix_free(m);
5813  gsl_matrix_free(evec);
5814  }
5815 #else
5816  {
5817  const char jobz = 'N';
5818  const char uplo = 'U';
5819  vpMatrix A = (*this);
5820  vpColVector WORK;
5821  int lwork = -1;
5822  int info = 0;
5823  double wkopt;
5824  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
5825  lwork = static_cast<int>(wkopt);
5826  WORK.resize(lwork);
5827  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
5828  }
5829 #endif
5830 #else
5831  {
5832  throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. "
5833  "You should install Lapack 3rd party"));
5834  }
5835 #endif
5836  return evalue;
5837 }
5838 
5888 void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const
5889 {
5890  if (rowNum != colNum) {
5891  throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
5892  colNum));
5893  }
5894 
5895  // Check if the matrix is symmetric: At - A = 0
5896  vpMatrix At_A = (*this).t() - (*this);
5897  for (unsigned int i = 0; i < rowNum; i++) {
5898  for (unsigned int j = 0; j < rowNum; j++) {
5899  // if (At_A[i][j] != 0) {
5900  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
5901  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix"));
5902  }
5903  }
5904  }
5905 
5906  // Resize the output matrices
5907  evalue.resize(rowNum);
5908  evector.resize(rowNum, colNum);
5909 
5910 #if defined(VISP_HAVE_LAPACK)
5911 #if defined(VISP_HAVE_GSL) /* be careful of the copy below */
5912  {
5913  gsl_vector *eval = gsl_vector_alloc(rowNum);
5914  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
5915 
5916  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
5917  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
5918 
5919  unsigned int Atda = (unsigned int)m->tda;
5920  for (unsigned int i = 0; i < rowNum; i++) {
5921  unsigned int k = i * Atda;
5922  for (unsigned int j = 0; j < colNum; j++)
5923  m->data[k + j] = (*this)[i][j];
5924  }
5925  gsl_eigen_symmv(m, eval, evec, w);
5926 
5927  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
5928 
5929  for (unsigned int i = 0; i < rowNum; i++) {
5930  evalue[i] = gsl_vector_get(eval, i);
5931  }
5932  Atda = (unsigned int)evec->tda;
5933  for (unsigned int i = 0; i < rowNum; i++) {
5934  unsigned int k = i * Atda;
5935  for (unsigned int j = 0; j < rowNum; j++) {
5936  evector[i][j] = evec->data[k + j];
5937  }
5938  }
5939 
5940  gsl_eigen_symmv_free(w);
5941  gsl_vector_free(eval);
5942  gsl_matrix_free(m);
5943  gsl_matrix_free(evec);
5944  }
5945 #else // defined(VISP_HAVE_GSL)
5946  {
5947  const char jobz = 'V';
5948  const char uplo = 'U';
5949  vpMatrix A = (*this);
5950  vpColVector WORK;
5951  int lwork = -1;
5952  int info = 0;
5953  double wkopt;
5954  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
5955  lwork = static_cast<int>(wkopt);
5956  WORK.resize(lwork);
5957  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
5958  evector = A.t();
5959  }
5960 #endif // defined(VISP_HAVE_GSL)
5961 #else
5962  {
5963  throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. "
5964  "You should install Lapack 3rd party"));
5965  }
5966 #endif
5967 }
5968 
5987 unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const
5988 {
5989  unsigned int nbline = getRows();
5990  unsigned int nbcol = getCols();
5991 
5992  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
5993  vpColVector sv;
5994  sv.resize(nbcol, false); // singular values
5995  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
5996 
5997  // Copy and resize matrix to have at least as many rows as columns
5998  // kernel is computed in svd method only if the matrix has more rows than
5999  // columns
6000 
6001  if (nbline < nbcol)
6002  U.resize(nbcol, nbcol, true);
6003  else
6004  U.resize(nbline, nbcol, false);
6005 
6006  U.insert(*this, 0, 0);
6007 
6008  U.svd(sv, V);
6009 
6010  // Compute the highest singular value and rank of the matrix
6011  double maxsv = 0;
6012  for (unsigned int i = 0; i < nbcol; i++) {
6013  if (sv[i] > maxsv) {
6014  maxsv = sv[i];
6015  }
6016  }
6017 
6018  unsigned int rank = 0;
6019  for (unsigned int i = 0; i < nbcol; i++) {
6020  if (sv[i] > maxsv * svThreshold) {
6021  rank++;
6022  }
6023  }
6024 
6025  kerAt.resize(nbcol - rank, nbcol);
6026  if (rank != nbcol) {
6027  for (unsigned int j = 0, k = 0; j < nbcol; j++) {
6028  // if( v.col(j) in kernel and non zero )
6029  if ((sv[j] <= maxsv * svThreshold) &&
6030  (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
6031  for (unsigned int i = 0; i < V.getRows(); i++) {
6032  kerAt[k][i] = V[i][j];
6033  }
6034  k++;
6035  }
6036  }
6037  }
6038 
6039  return rank;
6040 }
6041 
6058 unsigned int vpMatrix::nullSpace(vpMatrix &kerA, double svThreshold) const
6059 {
6060  unsigned int nbrow = getRows();
6061  unsigned int nbcol = getCols();
6062 
6063  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6064  vpColVector sv;
6065  sv.resize(nbcol, false); // singular values
6066  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6067 
6068  // Copy and resize matrix to have at least as many rows as columns
6069  // kernel is computed in svd method only if the matrix has more rows than
6070  // columns
6071 
6072  if (nbrow < nbcol)
6073  U.resize(nbcol, nbcol, true);
6074  else
6075  U.resize(nbrow, nbcol, false);
6076 
6077  U.insert(*this, 0, 0);
6078 
6079  U.svd(sv, V);
6080 
6081  // Compute the highest singular value and rank of the matrix
6082  double maxsv = sv[0];
6083 
6084  unsigned int rank = 0;
6085  for (unsigned int i = 0; i < nbcol; i++) {
6086  if (sv[i] > maxsv * svThreshold) {
6087  rank++;
6088  }
6089  }
6090 
6091  kerA.resize(nbcol, nbcol - rank);
6092  if (rank != nbcol) {
6093  for (unsigned int j = 0, k = 0; j < nbcol; j++) {
6094  // if( v.col(j) in kernel and non zero )
6095  if (sv[j] <= maxsv * svThreshold) {
6096  for (unsigned int i = 0; i < nbcol; i++) {
6097  kerA[i][k] = V[i][j];
6098  }
6099  k++;
6100  }
6101  }
6102  }
6103 
6104  return (nbcol - rank);
6105 }
6106 
6123 unsigned int vpMatrix::nullSpace(vpMatrix &kerA, int dim) const
6124 {
6125  unsigned int nbrow = getRows();
6126  unsigned int nbcol = getCols();
6127  unsigned int dim_ = static_cast<unsigned int>(dim);
6128 
6129  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6130  vpColVector sv;
6131  sv.resize(nbcol, false); // singular values
6132  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6133 
6134  // Copy and resize matrix to have at least as many rows as columns
6135  // kernel is computed in svd method only if the matrix has more rows than
6136  // columns
6137 
6138  if (nbrow < nbcol)
6139  U.resize(nbcol, nbcol, true);
6140  else
6141  U.resize(nbrow, nbcol, false);
6142 
6143  U.insert(*this, 0, 0);
6144 
6145  U.svd(sv, V);
6146 
6147  kerA.resize(nbcol, dim_);
6148  if (dim_ != 0) {
6149  unsigned int rank = nbcol - dim_;
6150  for (unsigned int k = 0; k < dim_; k++) {
6151  unsigned int j = k + rank;
6152  for (unsigned int i = 0; i < nbcol; i++) {
6153  kerA[i][k] = V[i][j];
6154  }
6155  }
6156  }
6157 
6158  double maxsv = sv[0];
6159  unsigned int rank = 0;
6160  for (unsigned int i = 0; i < nbcol; i++) {
6161  if (sv[i] > maxsv * 1e-6) {
6162  rank++;
6163  }
6164  }
6165  return (nbcol - rank);
6166 }
6167 
6199 double vpMatrix::det(vpDetMethod method) const
6200 {
6201  double det = 0.;
6202 
6203  if (method == LU_DECOMPOSITION) {
6204  det = this->detByLU();
6205  }
6206 
6207  return (det);
6208 }
6209 
6218 {
6219  if (colNum != rowNum) {
6220  throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix",
6221  rowNum, colNum));
6222  }
6223  else {
6224 #ifdef VISP_HAVE_GSL
6225  size_t size_ = rowNum * colNum;
6226  double *b = new double[size_];
6227  for (size_t i = 0; i < size_; i++)
6228  b[i] = 0.;
6229  gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum);
6230  gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum);
6231  gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
6232  // gsl_matrix_fprintf(stdout, &em.matrix, "%g");
6233  vpMatrix expA;
6234  expA.resize(rowNum, colNum, false);
6235  memcpy(expA.data, b, size_ * sizeof(double));
6236 
6237  delete[] b;
6238  return expA;
6239 #else
6240  vpMatrix _expE(rowNum, colNum, false);
6241  vpMatrix _expD(rowNum, colNum, false);
6242  vpMatrix _expX(rowNum, colNum, false);
6243  vpMatrix _expcX(rowNum, colNum, false);
6244  vpMatrix _eye(rowNum, colNum, false);
6245 
6246  _eye.eye();
6247  vpMatrix exp(*this);
6248 
6249  // double f;
6250  int e;
6251  double c = 0.5;
6252  int q = 6;
6253  int p = 1;
6254 
6255  double nA = 0;
6256  for (unsigned int i = 0; i < rowNum; i++) {
6257  double sum = 0;
6258  for (unsigned int j = 0; j < colNum; j++) {
6259  sum += fabs((*this)[i][j]);
6260  }
6261  if (sum > nA || i == 0) {
6262  nA = sum;
6263  }
6264  }
6265 
6266  /* f = */ frexp(nA, &e);
6267  // double s = (0 > e+1)?0:e+1;
6268  double s = e + 1;
6269 
6270  double sca = 1.0 / pow(2.0, s);
6271  exp = sca * exp;
6272  _expX = *this;
6273  _expE = c * exp + _eye;
6274  _expD = -c * exp + _eye;
6275  for (int k = 2; k <= q; k++) {
6276  c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1)));
6277  _expcX = exp * _expX;
6278  _expX = _expcX;
6279  _expcX = c * _expX;
6280  _expE = _expE + _expcX;
6281  if (p)
6282  _expD = _expD + _expcX;
6283  else
6284  _expD = _expD - _expcX;
6285  p = !p;
6286  }
6287  _expX = _expD.inverseByLU();
6288  exp = _expX * _expE;
6289  for (int k = 1; k <= s; k++) {
6290  _expE = exp * exp;
6291  exp = _expE;
6292  }
6293  return exp;
6294 #endif
6295  }
6296 }
6297 
6298 /**************************************************************************************************************/
6299 /**************************************************************************************************************/
6300 
6301 // Specific functions
6302 
6303 /*
6304  input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.
6305 
6306  output:: the complement matrix of the element (rowNo,colNo).
6307  This is the matrix obtained from M after elimenating the row rowNo and column
6308  colNo
6309 
6310  example:
6311  1 2 3
6312  M = 4 5 6
6313  7 8 9
6314  1 3
6315  subblock(M, 1, 1) give the matrix 7 9
6316 */
6317 vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row)
6318 {
6319  vpMatrix M_comp;
6320  M_comp.resize(M.getRows() - 1, M.getCols() - 1, false);
6321 
6322  for (unsigned int i = 0; i < col; i++) {
6323  for (unsigned int j = 0; j < row; j++)
6324  M_comp[i][j] = M[i][j];
6325  for (unsigned int j = row + 1; j < M.getRows(); j++)
6326  M_comp[i][j - 1] = M[i][j];
6327  }
6328  for (unsigned int i = col + 1; i < M.getCols(); i++) {
6329  for (unsigned int j = 0; j < row; j++)
6330  M_comp[i - 1][j] = M[i][j];
6331  for (unsigned int j = row + 1; j < M.getRows(); j++)
6332  M_comp[i - 1][j - 1] = M[i][j];
6333  }
6334  return M_comp;
6335 }
6336 
6346 double vpMatrix::cond(double svThreshold) const
6347 {
6348  unsigned int nbline = getRows();
6349  unsigned int nbcol = getCols();
6350 
6351  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6352  vpColVector sv;
6353  sv.resize(nbcol); // singular values
6354  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6355 
6356  // Copy and resize matrix to have at least as many rows as columns
6357  // kernel is computed in svd method only if the matrix has more rows than
6358  // columns
6359 
6360  if (nbline < nbcol)
6361  U.resize(nbcol, nbcol, true);
6362  else
6363  U.resize(nbline, nbcol, false);
6364 
6365  U.insert(*this, 0, 0);
6366 
6367  U.svd(sv, V);
6368 
6369  // Compute the highest singular value
6370  double maxsv = 0;
6371  for (unsigned int i = 0; i < nbcol; i++) {
6372  if (sv[i] > maxsv) {
6373  maxsv = sv[i];
6374  }
6375  }
6376 
6377  // Compute the rank of the matrix
6378  unsigned int rank = 0;
6379  for (unsigned int i = 0; i < nbcol; i++) {
6380  if (sv[i] > maxsv * svThreshold) {
6381  rank++;
6382  }
6383  }
6384 
6385  // Compute the lowest singular value
6386  double minsv = maxsv;
6387  for (unsigned int i = 0; i < rank; i++) {
6388  if (sv[i] < minsv) {
6389  minsv = sv[i];
6390  }
6391  }
6392 
6393  if (std::fabs(minsv) > std::numeric_limits<double>::epsilon()) {
6394  return maxsv / minsv;
6395  }
6396  else {
6397  return std::numeric_limits<double>::infinity();
6398  }
6399 }
6400 
6407 void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
6408 {
6409  if (H.getCols() != H.getRows()) {
6410  throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(),
6411  H.getCols()));
6412  }
6413 
6414  HLM = H;
6415  for (unsigned int i = 0; i < H.getCols(); i++) {
6416  HLM[i][i] += alpha * H[i][i];
6417  }
6418 }
6419 
6428 {
6429  double norm = 0.0;
6430  for (unsigned int i = 0; i < dsize; i++) {
6431  double x = *(data + i);
6432  norm += x * x;
6433  }
6434 
6435  return sqrt(norm);
6436 }
6437 
6447 {
6448  if (this->dsize != 0) {
6449  vpMatrix v;
6450  vpColVector w;
6451 
6452  vpMatrix M = *this;
6453 
6454  M.svd(w, v);
6455 
6456  double max = w[0];
6457  unsigned int maxRank = std::min<unsigned int>(this->getCols(), this->getRows());
6458  // The maximum reachable rank is either the number of columns or the number of rows
6459  // of the matrix.
6460  unsigned int boundary = std::min<unsigned int>(maxRank, w.size());
6461  // boundary is here to ensure that the number of singular values used for the com-
6462  // putation of the euclidean norm of the matrix is not greater than the maximum
6463  // reachable rank. Indeed, some svd library pad the singular values vector with 0s
6464  // if the input matrix is non-square.
6465  for (unsigned int i = 0; i < boundary; i++) {
6466  if (max < w[i]) {
6467  max = w[i];
6468  }
6469  }
6470  return max;
6471  }
6472  else {
6473  return 0.;
6474  }
6475 }
6476 
6488 {
6489  double norm = 0.0;
6490  for (unsigned int i = 0; i < rowNum; i++) {
6491  double x = 0;
6492  for (unsigned int j = 0; j < colNum; j++) {
6493  x += fabs(*(*(rowPtrs + i) + j));
6494  }
6495  if (x > norm) {
6496  norm = x;
6497  }
6498  }
6499  return norm;
6500 }
6501 
6508 double vpMatrix::sumSquare() const
6509 {
6510  double sum_square = 0.0;
6511  double x;
6512 
6513  for (unsigned int i = 0; i < rowNum; i++) {
6514  for (unsigned int j = 0; j < colNum; j++) {
6515  x = rowPtrs[i][j];
6516  sum_square += x * x;
6517  }
6518  }
6519 
6520  return sum_square;
6521 }
6522 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
6532 double vpMatrix::euclideanNorm() const { return frobeniusNorm(); }
6533 
6535 {
6536  return (vpMatrix)(vpColVector::stack(A, B));
6537 }
6538 
6540 {
6541  vpColVector::stack(A, B, C);
6542 }
6543 
6545 
6546 void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); }
6547 
6567 {
6568  vpRowVector c(getCols());
6569 
6570  for (unsigned int j = 0; j < getCols(); j++)
6571  c[j] = (*this)[i - 1][j];
6572  return c;
6573 }
6574 
6593 {
6594  vpColVector c(getRows());
6595 
6596  for (unsigned int i = 0; i < getRows(); i++)
6597  c[i] = (*this)[i][j - 1];
6598  return c;
6599 }
6600 
6607 void vpMatrix::setIdentity(const double &val)
6608 {
6609  for (unsigned int i = 0; i < rowNum; i++)
6610  for (unsigned int j = 0; j < colNum; j++)
6611  if (i == j)
6612  (*this)[i][j] = val;
6613  else
6614  (*this)[i][j] = 0;
6615 }
6616 
6617 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:125
unsigned int getCols() const
Definition: vpArray2D.h:274
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:138
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:132
void insert(const vpArray2D< Type > &A, unsigned int r, unsigned int c)
Definition: vpArray2D.h:431
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:299
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:128
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:134
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:286
unsigned int getRows() const
Definition: vpArray2D.h:284
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:130
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
double sumSquare() const
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:78
@ dimensionError
Bad dimension.
Definition: vpException.h:83
@ fatalError
Fatal error.
Definition: vpException.h:84
@ divideByZeroError
Division by zero.
Definition: vpException.h:82
Implementation of an homogeneous matrix and operations on such kind of matrices.
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:252
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void svdLapack(vpColVector &w, vpMatrix &V)
vpColVector eigenValues() const
Definition: vpMatrix.cpp:5767
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:5576
vpMatrix & operator<<(double *)
Definition: vpMatrix.cpp:798
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6346
vpMatrix expm() const
Definition: vpMatrix.cpp:6217
vpMatrix pseudoInverseEigen3(double svThreshold=1e-6) const
vpMatrix hadamard(const vpMatrix &m) const
Definition: vpMatrix.cpp:1807
vpMatrix operator-() const
Definition: vpMatrix.cpp:1598
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:6607
vpMatrix & operator/=(double x)
Divide all the element of the matrix by x : Aij = Aij / x.
Definition: vpMatrix.cpp:1729
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5324
std::ostream & maplePrint(std::ostream &os) const
Definition: vpMatrix.cpp:5487
void eye()
Definition: vpMatrix.cpp:442
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:5987
vpMatrix inverseByLU() const
vpMatrix operator*(const vpMatrix &B) const
Definition: vpMatrix.cpp:1177
vpMatrix operator/(double x) const
Cij = Aij / x (A is unchanged)
Definition: vpMatrix.cpp:1669
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5601
vp_deprecated void stackMatrices(const vpMatrix &A)
Definition: vpMatrix.h:994
vpMatrix & operator-=(const vpMatrix &B)
Operation A = A - B.
Definition: vpMatrix.cpp:1557
vpMatrix & operator*=(double x)
Multiply all the element of the matrix by x : Aij = Aij * x.
Definition: vpMatrix.cpp:1715
vpMatrix operator*(const double &x, const vpMatrix &B)
Definition: vpMatrix.cpp:1629
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:5257
vp_deprecated double euclideanNorm() const
Definition: vpMatrix.cpp:6532
vpColVector stackColumns()
Definition: vpMatrix.cpp:1772
vp_deprecated vpColVector column(unsigned int j)
Definition: vpMatrix.cpp:6592
static void sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1506
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2076
vpRowVector stackRows()
Definition: vpMatrix.cpp:1794
vpMatrix operator+(const vpMatrix &B) const
Definition: vpMatrix.cpp:1451
double sum() const
Definition: vpMatrix.cpp:1605
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:897
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:4955
void svdOpenCV(vpColVector &w, vpMatrix &V)
vpMatrix t() const
Definition: vpMatrix.cpp:457
vpMatrix()
Definition: vpMatrix.h:162
vpMatrix & operator+=(const vpMatrix &B)
Operation A = A + B.
Definition: vpMatrix.cpp:1540
static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C)
Definition: vpMatrix.cpp:1364
double inducedL2Norm() const
Definition: vpMatrix.cpp:6446
double infinityNorm() const
Definition: vpMatrix.cpp:6487
vpMatrix & operator=(const vpArray2D< double > &A)
Definition: vpMatrix.cpp:658
double frobeniusNorm() const
Definition: vpMatrix.cpp:6427
vpColVector getDiag() const
Definition: vpMatrix.cpp:5044
vpMatrix AtA() const
Definition: vpMatrix.cpp:633
static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
Definition: vpMatrix.cpp:915
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:1954
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6407
vpMatrix pseudoInverseLapack(double svThreshold=1e-6) const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2287
vpMatrix & operator,(double val)
Definition: vpMatrix.cpp:815
static void multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
Definition: vpMatrix.cpp:968
vp_deprecated vpRowVector row(unsigned int i)
Definition: vpMatrix.cpp:6566
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:4917
double detByLU() const
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6199
double sumSquare() const
Definition: vpMatrix.cpp:6508
vp_deprecated void init()
Definition: vpMatrix.h:989
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1393
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5713
void svdEigen3(vpColVector &w, vpMatrix &V)
void kron(const vpMatrix &m1, vpMatrix &out) const
Definition: vpMatrix.cpp:1863
vpMatrix AAt() const
Definition: vpMatrix.cpp:509
vpMatrix transpose() const
Definition: vpMatrix.cpp:464
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1018
static void negateMatrix(const vpMatrix &A, vpMatrix &C)
Definition: vpMatrix.cpp:1581
@ LU_DECOMPOSITION
Definition: vpMatrix.h:154
std::ostream & csvPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5528
unsigned int nullSpace(vpMatrix &kerA, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6058
std::ostream & matlabPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5442
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:400
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:107
void resize(unsigned int i, bool flagNullify=true)
Definition: vpRowVector.h:259
Class that consider the case of a translation vector.