Visual Servoing Platform  version 3.6.1 under development (2024-06-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 #endif
73 #endif
74 
76 
77 #ifdef VISP_HAVE_LAPACK
78 #ifdef VISP_HAVE_GSL
79 #elif defined(VISP_HAVE_MKL)
80 typedef MKL_INT integer;
81 
82 void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_, double *w_data,
83  double *work_data, int lwork_, int &info_)
84 {
85  MKL_INT n = static_cast<MKL_INT>(n_);
86  MKL_INT lda = static_cast<MKL_INT>(lda_);
87  MKL_INT lwork = static_cast<MKL_INT>(lwork_);
88  MKL_INT info = static_cast<MKL_INT>(info_);
89 
90  dsyev(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
91 }
92 
93 #else
94 #if defined(VISP_HAVE_LAPACK_BUILT_IN)
95 typedef long int integer;
96 #else
97 typedef int integer;
98 #endif
99 extern "C" integer dsyev_(char *jobz, char *uplo, integer *n, double *a, integer *lda, double *w, double *WORK,
100  integer *lwork, integer *info);
101 
102 void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_, double *w_data,
103  double *work_data, int lwork_, int &info_)
104 {
105  integer n = static_cast<integer>(n_);
106  integer lda = static_cast<integer>(lda_);
107  integer lwork = static_cast<integer>(lwork_);
108  integer info = static_cast<integer>(info_);
109 
110  dsyev_(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
111 
112  lwork_ = static_cast<int>(lwork);
113  info_ = static_cast<int>(info);
114 }
115 #endif
116 #endif
117 
118 #if !defined(VISP_USE_MSVC) || (defined(VISP_USE_MSVC) && !defined(VISP_BUILD_SHARED_LIBS))
119 const unsigned int vpMatrix::m_lapack_min_size_default = 0;
120 unsigned int vpMatrix::m_lapack_min_size = vpMatrix::m_lapack_min_size_default;
121 #endif
122 
123 // Prototypes of specific functions
124 vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row);
125 
126 void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows,
127  unsigned int ncols, double svThreshold, vpMatrix &Ap, int &rank_out, int *rank_in,
128  vpMatrix *imA, vpMatrix *imAt, vpMatrix *kerAt)
129 {
130  Ap.resize(ncols, nrows, true, false);
131 
132  // compute the highest singular value and the rank of h
133  double maxsv = sv[0];
134 
135  rank_out = 0;
136 
137  unsigned int sv_size = sv.size();
138  for (unsigned int i = 0; i < sv_size; ++i) {
139  if (sv[i] >(maxsv * svThreshold)) {
140  ++rank_out;
141  }
142  }
143 
144  unsigned int rank = static_cast<unsigned int>(rank_out);
145  if (rank_in) {
146  rank = static_cast<unsigned int>(*rank_in);
147  }
148 
149  for (unsigned int i = 0; i < ncols; ++i) {
150  for (unsigned int j = 0; j < nrows; ++j) {
151  for (unsigned int k = 0; k < rank; ++k) {
152  Ap[i][j] += (V[i][k] * U[j][k]) / sv[k];
153  }
154  }
155  }
156 
157  // Compute im(A)
158  if (imA) {
159  imA->resize(nrows, rank);
160 
161  for (unsigned int i = 0; i < nrows; ++i) {
162  for (unsigned int j = 0; j < rank; ++j) {
163  (*imA)[i][j] = U[i][j];
164  }
165  }
166  }
167 
168  // Compute im(At)
169  if (imAt) {
170  imAt->resize(ncols, rank);
171  for (unsigned int i = 0; i < ncols; ++i) {
172  for (unsigned int j = 0; j < rank; ++j) {
173  (*imAt)[i][j] = V[i][j];
174  }
175  }
176  }
177 
178  // Compute ker(At)
179  if (kerAt) {
180  kerAt->resize(ncols - rank, ncols);
181  if (rank != ncols) {
182  unsigned int v_rows = V.getRows();
183  for (unsigned int k = 0; k < (ncols - rank); ++k) {
184  unsigned j = k + rank;
185  for (unsigned int i = 0; i < v_rows; ++i) {
186  (*kerAt)[k][i] = V[i][j];
187  }
188  }
189  }
190  }
191 }
192 
198 vpMatrix::vpMatrix(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
199  : vpArray2D<double>()
200 {
201  if (((r + nrows) > M.rowNum) || ((c + ncols) > M.colNum)) {
203  "Cannot construct a sub matrix (%dx%d) starting at "
204  "position (%d,%d) that is not contained in the "
205  "original matrix (%dx%d)",
206  nrows, ncols, r, c, M.rowNum, M.colNum));
207  }
208 
209  init(M, r, c, nrows, ncols);
210 }
211 
212 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
214 {
215  rowNum = A.rowNum;
216  colNum = A.colNum;
217  rowPtrs = A.rowPtrs;
218  dsize = A.dsize;
219  data = A.data;
220 
221  A.rowNum = 0;
222  A.colNum = 0;
223  A.rowPtrs = nullptr;
224  A.dsize = 0;
225  A.data = nullptr;
226 }
227 
251 vpMatrix::vpMatrix(const std::initializer_list<double> &list) : vpArray2D<double>(list) { }
252 
275 vpMatrix::vpMatrix(unsigned int nrows, unsigned int ncols, const std::initializer_list<double> &list)
276  : vpArray2D<double>(nrows, ncols, list)
277 { }
278 
299 vpMatrix::vpMatrix(const std::initializer_list<std::initializer_list<double> > &lists) : vpArray2D<double>(lists) { }
300 #endif
301 
348 void vpMatrix::init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
349 {
350  unsigned int rnrows = r + nrows;
351  unsigned int cncols = c + ncols;
352 
353  if (rnrows > M.getRows()) {
354  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
355  M.getRows()));
356  }
357  if (cncols > M.getCols()) {
358  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
359  M.getCols()));
360  }
361  resize(nrows, ncols, false, false);
362 
363  if (this->rowPtrs == nullptr) { // Fix coverity scan: explicit null dereferenced
364  return; // Noting to do
365  }
366  for (unsigned int i = 0; i < nrows; ++i) {
367  memcpy((*this)[i], &M[i + r][c], ncols * sizeof(double));
368  }
369 }
370 
412 vpMatrix vpMatrix::extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
413 {
414  unsigned int rnrows = r + nrows;
415  unsigned int cncols = c + ncols;
416 
417  if (rnrows > getRows()) {
418  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
419  getRows()));
420  }
421  if (cncols > getCols()) {
422  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
423  getCols()));
424  }
425 
426  vpMatrix M;
427  M.resize(nrows, ncols, false, false);
428  for (unsigned int i = 0; i < nrows; ++i) {
429  memcpy(M[i], &(*this)[i + r][c], ncols * sizeof(double));
430  }
431 
432  return M;
433 }
434 
439 void vpMatrix::eye(unsigned int n) { eye(n, n); }
440 
445 void vpMatrix::eye(unsigned int m, unsigned int n)
446 {
447  resize(m, n);
448 
449  eye();
450 }
451 
457 {
458  for (unsigned int i = 0; i < rowNum; ++i) {
459  for (unsigned int j = 0; j < colNum; ++j) {
460  if (i == j) {
461  (*this)[i][j] = 1.0;
462  }
463  else {
464  (*this)[i][j] = 0;
465  }
466  }
467  }
468 }
469 
473 vpMatrix vpMatrix::t() const { return transpose(); }
474 
481 {
482  vpMatrix At;
483  transpose(At);
484  return At;
485 }
486 
493 {
494  At.resize(colNum, rowNum, false, false);
495 
496  if ((rowNum <= 16) || (colNum <= 16)) {
497  for (unsigned int i = 0; i < rowNum; ++i) {
498  for (unsigned int j = 0; j < colNum; ++j) {
499  At[j][i] = (*this)[i][j];
500  }
501  }
502  }
503  else {
504 #if defined(VISP_HAVE_SIMDLIB)
505  SimdMatTranspose(data, rowNum, colNum, At.data);
506 #else
507  // https://stackoverflow.com/a/21548079
508  const int tileSize = 32;
509  for (unsigned int i = 0; i < rowNum; i += tileSize) {
510  for (unsigned int j = 0; j < colNum; ++j) {
511  for (unsigned int b = 0; ((b < static_cast<unsigned int>(tileSize)) && ((i + b) < rowNum)); ++b) {
512  At[j][i + b] = (*this)[i + b][j];
513  }
514  }
515  }
516 #endif
517  }
518 }
519 
526 {
527  vpMatrix B;
528 
529  AAt(B);
530 
531  return B;
532 }
533 
545 void vpMatrix::AAt(vpMatrix &B) const
546 {
547  if ((B.rowNum != rowNum) || (B.colNum != rowNum)) {
548  B.resize(rowNum, rowNum, false, false);
549  }
550 
551  // If available use Lapack only for large matrices
552  bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size));
553 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
554  useLapack = false;
555 #endif
556 
557  if (useLapack) {
558 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
559  const double alpha = 1.0;
560  const double beta = 0.0;
561  const char transa = 't';
562  const char transb = 'n';
563 
564  vpMatrix::blas_dgemm(transa, transb, rowNum, rowNum, colNum, alpha, data, colNum, data, colNum, beta, B.data,
565  rowNum);
566 #endif
567  }
568  else {
569  // compute A*A^T
570  for (unsigned int i = 0; i < rowNum; ++i) {
571  for (unsigned int j = i; j < rowNum; ++j) {
572  double *pi = rowPtrs[i]; // row i
573  double *pj = rowPtrs[j]; // row j
574 
575  // sum (row i .* row j)
576  double ssum = 0;
577  for (unsigned int k = 0; k < colNum; ++k) {
578  ssum += *(pi++) * *(pj++);
579  }
580 
581  B[i][j] = ssum; // upper triangle
582  if (i != j) {
583  B[j][i] = ssum; // lower triangle
584  }
585  }
586  }
587  }
588 }
589 
601 void vpMatrix::AtA(vpMatrix &B) const
602 {
603  if ((B.rowNum != colNum) || (B.colNum != colNum)) {
604  B.resize(colNum, colNum, false, false);
605  }
606 
607  // If available use Lapack only for large matrices
608  bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size));
609 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
610  useLapack = false;
611 #endif
612 
613  if (useLapack) {
614 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
615  const double alpha = 1.0;
616  const double beta = 0.0;
617  const char transa = 'n';
618  const char transb = 't';
619 
620  vpMatrix::blas_dgemm(transa, transb, colNum, colNum, rowNum, alpha, data, colNum, data, colNum, beta, B.data,
621  colNum);
622 #endif
623  }
624  else {
625  for (unsigned int i = 0; i < colNum; ++i) {
626  double *Bi = B[i];
627  for (unsigned int j = 0; j < i; ++j) {
628  double *ptr = data;
629  double s = 0;
630  for (unsigned int k = 0; k < rowNum; ++k) {
631  s += (*(ptr + i)) * (*(ptr + j));
632  ptr += colNum;
633  }
634  *Bi++ = s;
635  B[j][i] = s;
636  }
637  double *ptr = data;
638  double s = 0;
639  for (unsigned int k = 0; k < rowNum; ++k) {
640  s += (*(ptr + i)) * (*(ptr + i));
641  ptr += colNum;
642  }
643  *Bi = s;
644  }
645  }
646 }
647 
654 {
655  vpMatrix B;
656 
657  AtA(B);
658 
659  return B;
660 }
661 
679 {
680  resize(A.getRows(), A.getCols(), false, false);
681 
682  if ((data != nullptr) && (A.data != nullptr) && (data != A.data)) {
683  memcpy(data, A.data, dsize * sizeof(double));
684  }
685 
686  return *this;
687 }
688 
690 {
691  resize(A.getRows(), A.getCols(), false, false);
692 
693  if ((data != nullptr) && (A.data != nullptr) && (data != A.data)) {
694  memcpy(data, A.data, dsize * sizeof(double));
695  }
696 
697  return *this;
698 }
699 
700 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
702 {
703  if (this != &other) {
704  if (data) {
705  free(data);
706  }
707  if (rowPtrs) {
708  free(rowPtrs);
709  }
710 
711  rowNum = other.rowNum;
712  colNum = other.colNum;
713  rowPtrs = other.rowPtrs;
714  dsize = other.dsize;
715  data = other.data;
716 
717  other.rowNum = 0;
718  other.colNum = 0;
719  other.rowPtrs = nullptr;
720  other.dsize = 0;
721  other.data = nullptr;
722  }
723 
724  return *this;
725 }
726 
753 vpMatrix &vpMatrix::operator=(const std::initializer_list<double> &list)
754 {
755  if (dsize != static_cast<unsigned int>(list.size())) {
756  resize(1, static_cast<unsigned int>(list.size()), false, false);
757  }
758 
759  std::copy(list.begin(), list.end(), data);
760 
761  return *this;
762 }
763 
787 vpMatrix &vpMatrix::operator=(const std::initializer_list<std::initializer_list<double> > &lists)
788 {
789  unsigned int nrows = static_cast<unsigned int>(lists.size()), ncols = 0;
790  for (auto &l : lists) {
791  if (static_cast<unsigned int>(l.size()) > ncols) {
792  ncols = static_cast<unsigned int>(l.size());
793  }
794  }
795 
796  resize(nrows, ncols, false, false);
797  auto it = lists.begin();
798  for (unsigned int i = 0; i < rowNum; ++i, ++it) {
799  std::copy(it->begin(), it->end(), rowPtrs[i]);
800  }
801 
802  return *this;
803 }
804 #endif
805 
808 {
809  std::fill(data, data + (rowNum * colNum), x);
810  return *this;
811 }
812 
819 {
820  for (unsigned int i = 0; i < rowNum; ++i) {
821  for (unsigned int j = 0; j < colNum; ++j) {
822  rowPtrs[i][j] = *x++;
823  }
824  }
825  return *this;
826 }
827 
829 {
830  resize(1, 1, false, false);
831  rowPtrs[0][0] = val;
832  return *this;
833 }
834 
836 {
837  resize(1, colNum + 1, false, false);
838  rowPtrs[0][colNum - 1] = val;
839  return *this;
840 }
841 
878 {
879  unsigned int rows = A.getRows();
880  this->resize(rows, rows);
881 
882  (*this) = 0;
883  for (unsigned int i = 0; i < rows; ++i) {
884  (*this)[i][i] = A[i];
885  }
886 }
887 
918 void vpMatrix::diag(const double &val)
919 {
920  (*this) = 0;
921  unsigned int min_ = (rowNum < colNum) ? rowNum : colNum;
922  for (unsigned int i = 0; i < min_; ++i) {
923  (*this)[i][i] = val;
924  }
925 }
926 
938 {
939  unsigned int rows = A.getRows();
940  DA.resize(rows, rows, true);
941 
942  for (unsigned int i = 0; i < rows; ++i) {
943  DA[i][i] = A[i];
944  }
945 }
946 
952 {
953  vpTranslationVector t_out;
954 
955  if ((rowNum != 3) || (colNum != 3)) {
956  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector",
957  rowNum, colNum, tv.getRows(), tv.getCols()));
958  }
959 
960  for (unsigned int j = 0; j < 3; ++j) {
961  t_out[j] = 0;
962  }
963 
964  for (unsigned int j = 0; j < 3; ++j) {
965  double tj = tv[j]; // optimization em 5/12/2006
966  for (unsigned int i = 0; i < 3; ++i) {
967  t_out[i] += rowPtrs[i][j] * tj;
968  }
969  }
970  return t_out;
971 }
972 
978 {
979  vpColVector v_out;
980  vpMatrix::multMatrixVector(*this, v, v_out);
981  return v_out;
982 }
983 
993 {
994  if (A.colNum != v.getRows()) {
995  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
996  A.getRows(), A.getCols(), v.getRows()));
997  }
998 
999  if (A.rowNum != w.rowNum) {
1000  w.resize(A.rowNum, false);
1001  }
1002 
1003  // If available use Lapack only for large matrices
1004  bool useLapack = ((A.rowNum > vpMatrix::m_lapack_min_size) || (A.colNum > vpMatrix::m_lapack_min_size));
1005 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1006  useLapack = false;
1007 #endif
1008 
1009  if (useLapack) {
1010 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1011  double alpha = 1.0;
1012  double beta = 0.0;
1013  char trans = 't';
1014  int incr = 1;
1015 
1016  vpMatrix::blas_dgemv(trans, A.colNum, A.rowNum, alpha, A.data, A.colNum, v.data, incr, beta, w.data, incr);
1017 #endif
1018  }
1019  else {
1020  w = 0.0;
1021  for (unsigned int j = 0; j < A.colNum; ++j) {
1022  double vj = v[j]; // optimization em 5/12/2006
1023  for (unsigned int i = 0; i < A.rowNum; ++i) {
1024  w[i] += A.rowPtrs[i][j] * vj;
1025  }
1026  }
1027  }
1028 }
1029 
1030 //---------------------------------
1031 // Matrix operations.
1032 //---------------------------------
1033 
1044 {
1045  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) {
1046  C.resize(A.rowNum, B.colNum, false, false);
1047  }
1048 
1049  if (A.colNum != B.rowNum) {
1050  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (%dx%d) matrix", A.getRows(),
1051  A.getCols(), B.getRows(), B.getCols()));
1052  }
1053 
1054  // If available use Lapack only for large matrices
1055  bool useLapack = ((A.rowNum > vpMatrix::m_lapack_min_size) || (A.colNum > vpMatrix::m_lapack_min_size) ||
1056  (B.colNum > vpMatrix::m_lapack_min_size));
1057 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1058  useLapack = false;
1059 #endif
1060 
1061  if (useLapack) {
1062 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1063  const double alpha = 1.0;
1064  const double beta = 0.0;
1065  const char trans = 'n';
1066  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1067  C.data, B.colNum);
1068 #endif
1069  }
1070  else {
1071  // 5/12/06 some "very" simple optimization to avoid indexation
1072  const unsigned int BcolNum = B.colNum;
1073  const unsigned int BrowNum = B.rowNum;
1074  double **BrowPtrs = B.rowPtrs;
1075  for (unsigned int i = 0; i < A.rowNum; ++i) {
1076  const double *rowptri = A.rowPtrs[i];
1077  double *ci = C[i];
1078  for (unsigned int j = 0; j < BcolNum; ++j) {
1079  double s = 0;
1080  for (unsigned int k = 0; k < BrowNum; ++k) {
1081  s += rowptri[k] * BrowPtrs[k][j];
1082  }
1083  ci[j] = s;
1084  }
1085  }
1086  }
1087 }
1088 
1102 {
1103  if ((A.colNum != 3) || (A.rowNum != 3) || (B.colNum != 3) || (B.rowNum != 3)) {
1105  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1106  "rotation matrix",
1107  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1108  }
1109  // 5/12/06 some "very" simple optimization to avoid indexation
1110  const unsigned int BcolNum = B.colNum;
1111  const unsigned int BrowNum = B.rowNum;
1112  double **BrowPtrs = B.rowPtrs;
1113  for (unsigned int i = 0; i < A.rowNum; ++i) {
1114  const double *rowptri = A.rowPtrs[i];
1115  double *ci = C[i];
1116  for (unsigned int j = 0; j < BcolNum; ++j) {
1117  double s = 0;
1118  for (unsigned int k = 0; k < BrowNum; ++k) {
1119  s += rowptri[k] * BrowPtrs[k][j];
1120  }
1121  ci[j] = s;
1122  }
1123  }
1124 }
1125 
1139 {
1140  if ((A.colNum != 4) || (A.rowNum != 4) || (B.colNum != 4) || (B.rowNum != 4)) {
1142  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1143  "rotation matrix",
1144  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1145  }
1146  // Considering perfMatrixMultiplication.cpp benchmark,
1147  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1148  // Lapack usage needs to be validated again.
1149  // If available use Lapack only for large matrices.
1150  // Using SSE2 doesn't speed up.
1151  bool useLapack = ((A.rowNum > vpMatrix::m_lapack_min_size) || (A.colNum > vpMatrix::m_lapack_min_size) ||
1152  (B.colNum > vpMatrix::m_lapack_min_size));
1153 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1154  useLapack = false;
1155 #endif
1156 
1157  if (useLapack) {
1158 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1159  const double alpha = 1.0;
1160  const double beta = 0.0;
1161  const char trans = 'n';
1162  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1163  C.data, B.colNum);
1164 #endif
1165  }
1166  else {
1167  // 5/12/06 some "very" simple optimization to avoid indexation
1168  const unsigned int BcolNum = B.colNum;
1169  const unsigned int BrowNum = B.rowNum;
1170  double **BrowPtrs = B.rowPtrs;
1171  for (unsigned int i = 0; i < A.rowNum; ++i) {
1172  const double *rowptri = A.rowPtrs[i];
1173  double *ci = C[i];
1174  for (unsigned int j = 0; j < BcolNum; ++j) {
1175  double s = 0;
1176  for (unsigned int k = 0; k < BrowNum; ++k) {
1177  s += rowptri[k] * BrowPtrs[k][j];
1178  }
1179  ci[j] = s;
1180  }
1181  }
1182  }
1183 }
1184 
1198 {
1199  vpMatrix::multMatrixVector(A, B, C);
1200 }
1201 
1207 {
1208  vpMatrix C;
1209 
1210  vpMatrix::mult2Matrices(*this, B, C);
1211 
1212  return C;
1213 }
1214 
1220 {
1221  if (colNum != R.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, 3, false, false);
1227 
1228  unsigned int RcolNum = R.getCols();
1229  unsigned int RrowNum = R.getRows();
1230  for (unsigned int i = 0; i < rowNum; ++i) {
1231  double *rowptri = rowPtrs[i];
1232  double *ci = C[i];
1233  for (unsigned int j = 0; j < RcolNum; ++j) {
1234  double s = 0;
1235  for (unsigned int k = 0; k < RrowNum; ++k) {
1236  s += rowptri[k] * R[k][j];
1237  }
1238  ci[j] = s;
1239  }
1240  }
1241 
1242  return C;
1243 }
1244 
1250 {
1251  if (colNum != M.getRows()) {
1252  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1253  colNum));
1254  }
1255  vpMatrix C;
1256  C.resize(rowNum, 4, false, false);
1257 
1258  const unsigned int McolNum = M.getCols();
1259  const unsigned int MrowNum = M.getRows();
1260  for (unsigned int i = 0; i < rowNum; ++i) {
1261  const double *rowptri = rowPtrs[i];
1262  double *ci = C[i];
1263  for (unsigned int j = 0; j < McolNum; ++j) {
1264  double s = 0;
1265  for (unsigned int k = 0; k < MrowNum; ++k) {
1266  s += rowptri[k] * M[k][j];
1267  }
1268  ci[j] = s;
1269  }
1270  }
1271 
1272  return C;
1273 }
1274 
1280 {
1281  if (colNum != V.getRows()) {
1282  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix",
1283  rowNum, colNum));
1284  }
1285  vpMatrix M;
1286  M.resize(rowNum, 6, false, false);
1287 
1288  // Considering perfMatrixMultiplication.cpp benchmark,
1289  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1290  // Lapack usage needs to be validated again.
1291  // If available use Lapack only for large matrices.
1292  // Speed up obtained using SSE2.
1293  bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size) ||
1294  (V.colNum > vpMatrix::m_lapack_min_size));
1295 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1296  useLapack = false;
1297 #endif
1298 
1299  if (useLapack) {
1300 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1301  const double alpha = 1.0;
1302  const double beta = 0.0;
1303  const char trans = 'n';
1304  vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, V.colNum, data, colNum, beta, M.data,
1305  M.colNum);
1306 #endif
1307  }
1308  else {
1309 #if defined(VISP_HAVE_SIMDLIB)
1310  SimdMatMulTwist(data, rowNum, V.data, M.data);
1311 #else
1312  unsigned int VcolNum = V.getCols();
1313  unsigned int VrowNum = V.getRows();
1314  for (unsigned int i = 0; i < rowNum; ++i) {
1315  double *rowptri = rowPtrs[i];
1316  double *ci = M[i];
1317  for (unsigned int j = 0; j < VcolNum; ++j) {
1318  double s = 0;
1319  for (unsigned int k = 0; k < VrowNum; ++k) {
1320  s += rowptri[k] * V[k][j];
1321  }
1322  ci[j] = s;
1323  }
1324  }
1325 #endif
1326  }
1327 
1328  return M;
1329 }
1330 
1336 {
1337  if (colNum != V.getRows()) {
1338  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) force/torque twist matrix",
1339  rowNum, colNum));
1340  }
1341  vpMatrix M;
1342  M.resize(rowNum, 6, false, false);
1343 
1344  // Considering perfMatrixMultiplication.cpp benchmark,
1345  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1346  // Lapack usage needs to be validated again.
1347  // If available use Lapack only for large matrices.
1348  // Speed up obtained using SSE2.
1349  bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size) ||
1350  (V.getCols() > vpMatrix::m_lapack_min_size));
1351 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1352  useLapack = false;
1353 #endif
1354 
1355  if (useLapack) {
1356 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1357  const double alpha = 1.0;
1358  const double beta = 0.0;
1359  const char trans = 'n';
1360  vpMatrix::blas_dgemm(trans, trans, V.getCols(), rowNum, colNum, alpha, V.data, V.getCols(), data, colNum, beta,
1361  M.data, M.colNum);
1362 #endif
1363  }
1364  else {
1365 #if defined(VISP_HAVE_SIMDLIB)
1366  SimdMatMulTwist(data, rowNum, V.data, M.data);
1367 #else
1368  unsigned int VcolNum = V.getCols();
1369  unsigned int VrowNum = V.getRows();
1370  for (unsigned int i = 0; i < rowNum; ++i) {
1371  double *rowptri = rowPtrs[i];
1372  double *ci = M[i];
1373  for (unsigned int j = 0; j < VcolNum; ++j) {
1374  double s = 0;
1375  for (unsigned int k = 0; k < VrowNum; ++k) {
1376  s += rowptri[k] * V[k][j];
1377  }
1378  ci[j] = s;
1379  }
1380  }
1381 #endif
1382  }
1383 
1384  return M;
1385 }
1386 
1397 void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB,
1398  vpMatrix &C)
1399 {
1400  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) {
1401  C.resize(A.rowNum, B.colNum, false, false);
1402  }
1403 
1404  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1405  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1406  A.getCols(), B.getRows(), B.getCols()));
1407  }
1408 
1409  double **ArowPtrs = A.rowPtrs;
1410  double **BrowPtrs = B.rowPtrs;
1411  double **CrowPtrs = C.rowPtrs;
1412 
1413  for (unsigned int i = 0; i < A.rowNum; ++i) {
1414  for (unsigned int j = 0; j < A.colNum; ++j) {
1415  CrowPtrs[i][j] = (wB * BrowPtrs[i][j]) + (wA * ArowPtrs[i][j]);
1416  }
1417  }
1418 }
1419 
1429 void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1430 {
1431  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) {
1432  C.resize(A.rowNum, B.colNum, false, false);
1433  }
1434 
1435  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1436  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1437  A.getCols(), B.getRows(), B.getCols()));
1438  }
1439 
1440  double **ArowPtrs = A.rowPtrs;
1441  double **BrowPtrs = B.rowPtrs;
1442  double **CrowPtrs = C.rowPtrs;
1443 
1444  for (unsigned int i = 0; i < A.rowNum; ++i) {
1445  for (unsigned int j = 0; j < A.colNum; ++j) {
1446  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1447  }
1448  }
1449 }
1450 
1464 {
1465  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) {
1466  C.resize(A.rowNum);
1467  }
1468 
1469  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1470  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1471  A.getCols(), B.getRows(), B.getCols()));
1472  }
1473 
1474  double **ArowPtrs = A.rowPtrs;
1475  double **BrowPtrs = B.rowPtrs;
1476  double **CrowPtrs = C.rowPtrs;
1477 
1478  for (unsigned int i = 0; i < A.rowNum; ++i) {
1479  for (unsigned int j = 0; j < A.colNum; ++j) {
1480  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1481  }
1482  }
1483 }
1484 
1490 {
1491  vpMatrix C;
1492  vpMatrix::add2Matrices(*this, B, C);
1493  return C;
1494 }
1495 
1512 {
1513  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) {
1514  C.resize(A.rowNum);
1515  }
1516 
1517  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1518  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1519  A.getCols(), B.getRows(), B.getCols()));
1520  }
1521 
1522  double **ArowPtrs = A.rowPtrs;
1523  double **BrowPtrs = B.rowPtrs;
1524  double **CrowPtrs = C.rowPtrs;
1525 
1526  for (unsigned int i = 0; i < A.rowNum; ++i) {
1527  for (unsigned int j = 0; j < A.colNum; ++j) {
1528  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1529  }
1530  }
1531 }
1532 
1545 void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1546 {
1547  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) {
1548  C.resize(A.rowNum, A.colNum, false, false);
1549  }
1550 
1551  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1552  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1553  A.getCols(), B.getRows(), B.getCols()));
1554  }
1555 
1556  double **ArowPtrs = A.rowPtrs;
1557  double **BrowPtrs = B.rowPtrs;
1558  double **CrowPtrs = C.rowPtrs;
1559 
1560  for (unsigned int i = 0; i < A.rowNum; ++i) {
1561  for (unsigned int j = 0; j < A.colNum; ++j) {
1562  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1563  }
1564  }
1565 }
1566 
1572 {
1573  vpMatrix C;
1574  vpMatrix::sub2Matrices(*this, B, C);
1575  return C;
1576 }
1577 
1579 
1581 {
1582  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1583  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1584  B.getRows(), B.getCols()));
1585  }
1586 
1587  double **BrowPtrs = B.rowPtrs;
1588 
1589  for (unsigned int i = 0; i < rowNum; ++i) {
1590  for (unsigned int j = 0; j < colNum; ++j) {
1591  rowPtrs[i][j] += BrowPtrs[i][j];
1592  }
1593  }
1594 
1595  return *this;
1596 }
1597 
1600 {
1601  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1602  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1603  B.getRows(), B.getCols()));
1604  }
1605 
1606  double **BrowPtrs = B.rowPtrs;
1607  for (unsigned int i = 0; i < rowNum; ++i) {
1608  for (unsigned int j = 0; j < colNum; ++j) {
1609  rowPtrs[i][j] -= BrowPtrs[i][j];
1610  }
1611  }
1612 
1613  return *this;
1614 }
1615 
1626 {
1627  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) {
1628  C.resize(A.rowNum, A.colNum, false, false);
1629  }
1630 
1631  double **ArowPtrs = A.rowPtrs;
1632  double **CrowPtrs = C.rowPtrs;
1633 
1634  for (unsigned int i = 0; i < A.rowNum; ++i) {
1635  for (unsigned int j = 0; j < A.colNum; ++j) {
1636  CrowPtrs[i][j] = -ArowPtrs[i][j];
1637  }
1638  }
1639 }
1640 
1646 {
1647  vpMatrix C;
1648  vpMatrix::negateMatrix(*this, C);
1649  return C;
1650 }
1651 
1652 double vpMatrix::sum() const
1653 {
1654  double s = 0.0;
1655  for (unsigned int i = 0; i < rowNum; ++i) {
1656  for (unsigned int j = 0; j < colNum; ++j) {
1657  s += rowPtrs[i][j];
1658  }
1659  }
1660 
1661  return s;
1662 }
1663 
1664 //---------------------------------
1665 // Matrix/vector operations.
1666 //---------------------------------
1667 
1668 //---------------------------------
1669 // Matrix/real operations.
1670 //---------------------------------
1671 
1677 {
1678  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1679  return *this;
1680  }
1681 
1682  vpMatrix M;
1683  M.resize(rowNum, colNum, false, false);
1684 
1685  for (unsigned int i = 0; i < rowNum; ++i) {
1686  for (unsigned int j = 0; j < colNum; ++j) {
1687  M[i][j] = rowPtrs[i][j] * x;
1688  }
1689  }
1690 
1691  return M;
1692 }
1693 
1696 {
1697  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1698  return *this;
1699  }
1700 
1701  if (std::fabs(x) < std::numeric_limits<double>::epsilon()) {
1702  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1703  }
1704 
1705  vpMatrix C;
1706  C.resize(rowNum, colNum, false, false);
1707 
1708  double xinv = 1 / x;
1709 
1710  for (unsigned int i = 0; i < rowNum; ++i) {
1711  for (unsigned int j = 0; j < colNum; ++j) {
1712  C[i][j] = rowPtrs[i][j] * xinv;
1713  }
1714  }
1715 
1716  return C;
1717 }
1718 
1721 {
1722  for (unsigned int i = 0; i < rowNum; ++i) {
1723  for (unsigned int j = 0; j < colNum; ++j) {
1724  rowPtrs[i][j] += x;
1725  }
1726  }
1727 
1728  return *this;
1729 }
1730 
1733 {
1734  for (unsigned int i = 0; i < rowNum; ++i) {
1735  for (unsigned int j = 0; j < colNum; ++j) {
1736  rowPtrs[i][j] -= x;
1737  }
1738  }
1739 
1740  return *this;
1741 }
1742 
1748 {
1749  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1750  return *this;
1751  }
1752 
1753  for (unsigned int i = 0; i < rowNum; ++i) {
1754  for (unsigned int j = 0; j < colNum; ++j) {
1755  rowPtrs[i][j] *= x;
1756  }
1757  }
1758 
1759  return *this;
1760 }
1761 
1764 {
1765  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1766  return *this;
1767  }
1768 
1769  if (std::fabs(x) < std::numeric_limits<double>::epsilon()) {
1770  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1771  }
1772 
1773  double xinv = 1 / x;
1774 
1775  for (unsigned int i = 0; i < rowNum; ++i) {
1776  for (unsigned int j = 0; j < colNum; ++j) {
1777  rowPtrs[i][j] *= xinv;
1778  }
1779  }
1780 
1781  return *this;
1782 }
1783 
1784 //----------------------------------------------------------------
1785 // Matrix Operation
1786 //----------------------------------------------------------------
1787 
1793 {
1794  if ((out.rowNum != (colNum * rowNum)) || (out.colNum != 1)) {
1795  out.resize(colNum * rowNum, false, false);
1796  }
1797 
1798  double *optr = out.data;
1799  for (unsigned int j = 0; j < colNum; ++j) {
1800  for (unsigned int i = 0; i < rowNum; ++i) {
1801  *(optr++) = rowPtrs[i][j];
1802  }
1803  }
1804 }
1805 
1811 {
1812  vpColVector out(colNum * rowNum);
1813  stackColumns(out);
1814  return out;
1815 }
1816 
1822 {
1823  if ((out.getRows() != 1) || (out.getCols() != (colNum * rowNum))) {
1824  out.resize(colNum * rowNum, false, false);
1825  }
1826 
1827  memcpy(out.data, data, sizeof(double) * out.getCols());
1828 }
1834 {
1835  vpRowVector out(colNum * rowNum);
1836  stackRows(out);
1837  return out;
1838 }
1839 
1847 {
1848  if ((m.getRows() != rowNum) || (m.getCols() != colNum)) {
1849  throw(vpException(vpException::dimensionError, "In Hadamard product: bad dimension of input matrix"));
1850  }
1851 
1852  vpMatrix out;
1853  out.resize(rowNum, colNum, false, false);
1854 
1855 #if defined(VISP_HAVE_SIMDLIB)
1856  SimdVectorHadamard(data, m.data, dsize, out.data);
1857 #else
1858  for (unsigned int i = 0; i < dsize; ++i) {
1859  out.data[i] = data[i] * m.data[i];
1860  }
1861 #endif
1862 
1863  return out;
1864 }
1865 
1872 void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out)
1873 {
1874  unsigned int r1 = m1.getRows();
1875  unsigned int c1 = m1.getCols();
1876  unsigned int r2 = m2.getRows();
1877  unsigned int c2 = m2.getCols();
1878 
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 }
1895 
1902 void vpMatrix::kron(const vpMatrix &m, vpMatrix &out) const { kron(*this, m, out); }
1903 
1911 {
1912  unsigned int r1 = m1.getRows();
1913  unsigned int c1 = m1.getCols();
1914  unsigned int r2 = m2.getRows();
1915  unsigned int c2 = m2.getCols();
1916 
1917  vpMatrix out;
1918  out.resize(r1 * r2, c1 * c2, false, false);
1919 
1920  for (unsigned int r = 0; r < r1; ++r) {
1921  for (unsigned int c = 0; c < c1; ++c) {
1922  double alpha = m1[r][c];
1923  double *m2ptr = m2[0];
1924  unsigned int roffset = r * r2;
1925  unsigned int coffset = c * c2;
1926  for (unsigned int rr = 0; rr < r2; ++rr) {
1927  for (unsigned int cc = 0; cc < c2; ++cc) {
1928  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1929  }
1930  }
1931  }
1932  }
1933  return out;
1934 }
1935 
1941 vpMatrix vpMatrix::kron(const vpMatrix &m) const { return kron(*this, m); }
1942 
1993 void vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const { x = pseudoInverse(1e-6) * b; }
1994 
2045 {
2046  vpColVector X(colNum);
2047 
2048  solveBySVD(B, X);
2049  return X;
2050 }
2051 
2116 {
2117 #if defined(VISP_HAVE_LAPACK)
2118  svdLapack(w, V);
2119 #elif defined(VISP_HAVE_EIGEN3)
2120  svdEigen3(w, V);
2121 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2122  svdOpenCV(w, V);
2123 #else
2124  (void)w;
2125  (void)V;
2126  throw(vpException(vpException::fatalError, "Cannot compute SVD. Install Lapack, Eigen3 or OpenCV 3rd party"));
2127 #endif
2128 }
2129 
2184 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
2185 {
2186 #if defined(VISP_HAVE_LAPACK)
2187  return pseudoInverseLapack(Ap, svThreshold);
2188 #elif defined(VISP_HAVE_EIGEN3)
2189  return pseudoInverseEigen3(Ap, svThreshold);
2190 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2191  return pseudoInverseOpenCV(Ap, svThreshold);
2192 #else
2193  (void)Ap;
2194  (void)svThreshold;
2195  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2196  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2197 #endif
2198 }
2199 
2260 int vpMatrix::pseudoInverse(vpMatrix &Ap, int rank_in) const
2261 {
2262 #if defined(VISP_HAVE_LAPACK)
2263  return pseudoInverseLapack(Ap, rank_in);
2264 #elif defined(VISP_HAVE_EIGEN3)
2265  return pseudoInverseEigen3(Ap, rank_in);
2266 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2267  return pseudoInverseOpenCV(Ap, rank_in);
2268 #else
2269  (void)Ap;
2270  (void)rank_in;
2271  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2272  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2273 #endif
2274 }
2275 
2326 vpMatrix vpMatrix::pseudoInverse(double svThreshold) const
2327 {
2328 #if defined(VISP_HAVE_LAPACK)
2329  return pseudoInverseLapack(svThreshold);
2330 #elif defined(VISP_HAVE_EIGEN3)
2331  return pseudoInverseEigen3(svThreshold);
2332 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2333  return pseudoInverseOpenCV(svThreshold);
2334 #else
2335  (void)svThreshold;
2336  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2337  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2338 #endif
2339 }
2340 
2392 {
2393 #if defined(VISP_HAVE_LAPACK)
2394  return pseudoInverseLapack(rank_in);
2395 #elif defined(VISP_HAVE_EIGEN3)
2396  return pseudoInverseEigen3(rank_in);
2397 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
2398  return pseudoInverseOpenCV(rank_in);
2399 #else
2400  (void)rank_in;
2401  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2402  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2403 #endif
2404 }
2405 
2406 #ifndef DOXYGEN_SHOULD_SKIP_THIS
2407 #if defined(VISP_HAVE_LAPACK)
2444 vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const
2445 {
2446  unsigned int nrows = getRows();
2447  unsigned int ncols = getCols();
2448  int rank_out;
2449 
2450  vpMatrix U, V, Ap;
2451  vpColVector sv;
2452 
2453  U = *this;
2454  U.svdLapack(sv, V);
2455 
2456  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
2457 
2458  return Ap;
2459 }
2460 
2501 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) const
2502 {
2503  unsigned int nrows = getRows();
2504  unsigned int ncols = getCols();
2505  int rank_out;
2506 
2507  vpMatrix U, V;
2508  vpColVector sv;
2509 
2510  U = *this;
2511  U.svdLapack(sv, V);
2512 
2513  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
2514 
2515  return static_cast<unsigned int>(rank_out);
2516 }
2564 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2565 {
2566  unsigned int nrows = getRows();
2567  unsigned int ncols = getCols();
2568  int rank_out;
2569 
2570  vpMatrix U, V;
2571 
2572  Ap.resize(ncols, nrows, false, false);
2573 
2574  U = *this;
2575  U.svdLapack(sv, V);
2576 
2577  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
2578 
2579  return static_cast<unsigned int>(rank_out);
2580 }
2581 
2688 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
2689  vpMatrix &imAt, vpMatrix &kerAt) const
2690 {
2691  unsigned int nrows = getRows();
2692  unsigned int ncols = getCols();
2693  int rank_out;
2694  vpMatrix U, V;
2695 
2696  if (nrows < ncols) {
2697  U.resize(ncols, ncols, true);
2698  U.insert(*this, 0, 0);
2699  }
2700  else {
2701  U = *this;
2702  }
2703 
2704  U.svdLapack(sv, V);
2705 
2706  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt);
2707 
2708  return static_cast<unsigned int>(rank_out);
2709 }
2710 
2747 vpMatrix vpMatrix::pseudoInverseLapack(int rank_in) const
2748 {
2749  unsigned int nrows = getRows();
2750  unsigned int ncols = getCols();
2751  int rank_out;
2752  double svThreshold = 1e-26;
2753 
2754  vpMatrix U, V, Ap;
2755  vpColVector sv;
2756 
2757  U = *this;
2758  U.svdLapack(sv, V);
2759 
2760  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
2761 
2762  return Ap;
2763 }
2764 
2811 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, int rank_in) const
2812 {
2813  unsigned int nrows = getRows();
2814  unsigned int ncols = getCols();
2815  int rank_out;
2816  double svThreshold = 1e-26;
2817 
2818  vpMatrix U, V;
2819  vpColVector sv;
2820 
2821  U = *this;
2822  U.svdLapack(sv, V);
2823 
2824  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
2825 
2826  return rank_out;
2827 }
2828 
2882 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in) const
2883 {
2884  unsigned int nrows = getRows();
2885  unsigned int ncols = getCols();
2886  int rank_out;
2887  double svThreshold = 1e-26;
2888  vpMatrix U, V;
2889 
2890  Ap.resize(ncols, nrows, false, false);
2891 
2892  U = *this;
2893  U.svdLapack(sv, V);
2894 
2895  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
2896 
2897  return rank_out;
2898 }
2899 
3011 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
3012  vpMatrix &kerAt) const
3013 {
3014  unsigned int nrows = getRows();
3015  unsigned int ncols = getCols();
3016  int rank_out;
3017  double svThreshold = 1e-26;
3018  vpMatrix U, V;
3019 
3020  if (nrows < ncols) {
3021  U.resize(ncols, ncols, true);
3022  U.insert(*this, 0, 0);
3023  }
3024  else {
3025  U = *this;
3026  }
3027 
3028  U.svdLapack(sv, V);
3029 
3030  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
3031 
3032  return rank_out;
3033 }
3034 
3035 #endif
3036 #if defined(VISP_HAVE_EIGEN3)
3073 vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const
3074 {
3075  unsigned int nrows = getRows();
3076  unsigned int ncols = getCols();
3077  int rank_out;
3078  vpMatrix U, V, Ap;
3079  vpColVector sv;
3080 
3081  U = *this;
3082  U.svdEigen3(sv, V);
3083 
3084  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
3085 
3086  return Ap;
3087 }
3088 
3129 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) const
3130 {
3131  unsigned int nrows = getRows();
3132  unsigned int ncols = getCols();
3133  int rank_out;
3134  vpMatrix U, V;
3135  vpColVector sv;
3136 
3137  U = *this;
3138  U.svdEigen3(sv, V);
3139 
3140  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
3141 
3142  return static_cast<unsigned int>(rank_out);
3143 }
3144 
3192 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3193 {
3194  unsigned int nrows = getRows();
3195  unsigned int ncols = getCols();
3196  int rank_out;
3197  vpMatrix U, V;
3198 
3199  Ap.resize(ncols, nrows, false, false);
3200 
3201  U = *this;
3202  U.svdEigen3(sv, V);
3203 
3204  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
3205 
3206  return static_cast<unsigned int>(rank_out);
3207 }
3208 
3315 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3316  vpMatrix &imAt, vpMatrix &kerAt) const
3317 {
3318  unsigned int nrows = getRows();
3319  unsigned int ncols = getCols();
3320  int rank_out;
3321  vpMatrix U, V;
3322 
3323  if (nrows < ncols) {
3324  U.resize(ncols, ncols, true);
3325  U.insert(*this, 0, 0);
3326  }
3327  else {
3328  U = *this;
3329  }
3330 
3331  U.svdEigen3(sv, V);
3332 
3333  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt);
3334 
3335  return static_cast<unsigned int>(rank_out);
3336 }
3337 
3374 vpMatrix vpMatrix::pseudoInverseEigen3(int rank_in) const
3375 {
3376  unsigned int nrows = getRows();
3377  unsigned int ncols = getCols();
3378  int rank_out;
3379  double svThreshold = 1e-26;
3380  vpMatrix U, V, Ap;
3381  vpColVector sv;
3382 
3383  U = *this;
3384  U.svdEigen3(sv, V);
3385 
3386  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
3387 
3388  return Ap;
3389 }
3390 
3437 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, int rank_in) const
3438 {
3439  unsigned int nrows = getRows();
3440  unsigned int ncols = getCols();
3441  int rank_out;
3442  double svThreshold = 1e-26;
3443  vpMatrix U, V;
3444  vpColVector sv;
3445 
3446  Ap.resize(ncols, nrows, false, false);
3447 
3448  U = *this;
3449  U.svdEigen3(sv, V);
3450 
3451  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
3452 
3453  return rank_out;
3454 }
3455 
3509 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in) const
3510 {
3511  unsigned int nrows = getRows();
3512  unsigned int ncols = getCols();
3513  int rank_out;
3514  double svThreshold = 1e-26;
3515  vpMatrix U, V;
3516 
3517  Ap.resize(ncols, nrows, false, false);
3518 
3519  U = *this;
3520  U.svdEigen3(sv, V);
3521 
3522  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
3523 
3524  return rank_out;
3525 }
3526 
3638 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
3639  vpMatrix &kerAt) const
3640 {
3641  unsigned int nrows = getRows();
3642  unsigned int ncols = getCols();
3643  int rank_out;
3644  double svThreshold = 1e-26;
3645  vpMatrix U, V;
3646 
3647  if (nrows < ncols) {
3648  U.resize(ncols, ncols, true);
3649  U.insert(*this, 0, 0);
3650  }
3651  else {
3652  U = *this;
3653  }
3654  U.svdEigen3(sv, V);
3655 
3656  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
3657 
3658  return rank_out;
3659 }
3660 
3661 #endif
3662 #if defined(VISP_HAVE_OPENCV)
3699 vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const
3700 {
3701  unsigned int nrows = getRows();
3702  unsigned int ncols = getCols();
3703  int rank_out;
3704  vpMatrix U, V, Ap;
3705  vpColVector sv;
3706 
3707  U = *this;
3708  U.svdOpenCV(sv, V);
3709 
3710  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
3711 
3712  return Ap;
3713 }
3714 
3755 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) const
3756 {
3757  unsigned int nrows = getRows();
3758  unsigned int ncols = getCols();
3759  int rank_out;
3760  vpMatrix U, V;
3761  vpColVector sv;
3762 
3763  Ap.resize(ncols, nrows, false, false);
3764 
3765  U = *this;
3766  U.svdOpenCV(sv, V);
3767 
3768  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
3769 
3770  return static_cast<unsigned int>(rank_out);
3771 }
3819 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3820 {
3821  unsigned int nrows = getRows();
3822  unsigned int ncols = getCols();
3823  int rank_out;
3824  vpMatrix U, V;
3825 
3826  Ap.resize(ncols, nrows, false, false);
3827 
3828  U = *this;
3829  U.svdOpenCV(sv, V);
3830 
3831  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
3832 
3833  return static_cast<unsigned int>(rank_out);
3834 }
3835 
3942 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3943  vpMatrix &imAt, vpMatrix &kerAt) const
3944 {
3945  unsigned int nrows = getRows();
3946  unsigned int ncols = getCols();
3947  int rank_out;
3948  vpMatrix U, V;
3949 
3950  if (nrows < ncols) {
3951  U.resize(ncols, ncols, true);
3952  U.insert(*this, 0, 0);
3953  }
3954  else {
3955  U = *this;
3956  }
3957 
3958  U.svdOpenCV(sv, V);
3959 
3960  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt);
3961 
3962  return static_cast<unsigned int>(rank_out);
3963 }
3964 
4001 vpMatrix vpMatrix::pseudoInverseOpenCV(int rank_in) const
4002 {
4003  unsigned int nrows = getRows();
4004  unsigned int ncols = getCols();
4005  int rank_out;
4006  double svThreshold = 1e-26;
4007  vpMatrix U, V, Ap;
4008  vpColVector sv;
4009 
4010  U = *this;
4011  U.svdOpenCV(sv, V);
4012 
4013  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
4014 
4015  return Ap;
4016 }
4017 
4064 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, int rank_in) const
4065 {
4066  unsigned int nrows = getRows();
4067  unsigned int ncols = getCols();
4068  int rank_out;
4069  double svThreshold = 1e-26;
4070  vpMatrix U, V;
4071  vpColVector sv;
4072 
4073  Ap.resize(ncols, nrows, false, false);
4074 
4075  U = *this;
4076  U.svdOpenCV(sv, V);
4077 
4078  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
4079 
4080  return rank_out;
4081 }
4082 
4136 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4137 {
4138  unsigned int nrows = getRows();
4139  unsigned int ncols = getCols();
4140  int rank_out;
4141  double svThreshold = 1e-26;
4142  vpMatrix U, V;
4143 
4144  Ap.resize(ncols, nrows, false, false);
4145 
4146  U = *this;
4147  U.svdOpenCV(sv, V);
4148 
4149  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
4150 
4151  return rank_out;
4152 }
4153 
4265 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
4266  vpMatrix &kerAt) const
4267 {
4268  unsigned int nrows = getRows();
4269  unsigned int ncols = getCols();
4270  int rank_out;
4271  double svThreshold = 1e-26;
4272  vpMatrix U, V;
4273 
4274  if (nrows < ncols) {
4275  U.resize(ncols, ncols, true);
4276  U.insert(*this, 0, 0);
4277  }
4278  else {
4279  U = *this;
4280  }
4281 
4282  U.svdOpenCV(sv, V);
4283 
4284  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
4285 
4286  return rank_out;
4287 }
4288 
4289 #endif
4290 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
4291 
4353 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
4354 {
4355 #if defined(VISP_HAVE_LAPACK)
4356  return pseudoInverseLapack(Ap, sv, svThreshold);
4357 #elif defined(VISP_HAVE_EIGEN3)
4358  return pseudoInverseEigen3(Ap, sv, svThreshold);
4359 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
4360  return pseudoInverseOpenCV(Ap, sv, svThreshold);
4361 #else
4362  (void)Ap;
4363  (void)sv;
4364  (void)svThreshold;
4365  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4366  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4367 #endif
4368 }
4369 
4436 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4437 {
4438 #if defined(VISP_HAVE_LAPACK)
4439  return pseudoInverseLapack(Ap, sv, rank_in);
4440 #elif defined(VISP_HAVE_EIGEN3)
4441  return pseudoInverseEigen3(Ap, sv, rank_in);
4442 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
4443  return pseudoInverseOpenCV(Ap, sv, rank_in);
4444 #else
4445  (void)Ap;
4446  (void)sv;
4447  (void)rank_in;
4448  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4449  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4450 #endif
4451 }
4526 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt) const
4527 {
4528  vpMatrix kerAt;
4529  return pseudoInverse(Ap, sv, svThreshold, imA, imAt, kerAt);
4530 }
4531 
4610 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt) const
4611 {
4612  vpMatrix kerAt;
4613  return pseudoInverse(Ap, sv, rank_in, imA, imAt, kerAt);
4614 }
4615 
4717 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt,
4718  vpMatrix &kerAt) const
4719 {
4720 #if defined(VISP_HAVE_LAPACK)
4721  return pseudoInverseLapack(Ap, sv, svThreshold, imA, imAt, kerAt);
4722 #elif defined(VISP_HAVE_EIGEN3)
4723  return pseudoInverseEigen3(Ap, sv, svThreshold, imA, imAt, kerAt);
4724 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
4725  return pseudoInverseOpenCV(Ap, sv, svThreshold, imA, imAt, kerAt);
4726 #else
4727  (void)Ap;
4728  (void)sv;
4729  (void)svThreshold;
4730  (void)imA;
4731  (void)imAt;
4732  (void)kerAt;
4733  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4734  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4735 #endif
4736 }
4737 
4844 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
4845  vpMatrix &kerAt) const
4846 {
4847 #if defined(VISP_HAVE_LAPACK)
4848  return pseudoInverseLapack(Ap, sv, rank_in, imA, imAt, kerAt);
4849 #elif defined(VISP_HAVE_EIGEN3)
4850  return pseudoInverseEigen3(Ap, sv, rank_in, imA, imAt, kerAt);
4851 #elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
4852  return pseudoInverseOpenCV(Ap, sv, rank_in, imA, imAt, kerAt);
4853 #else
4854  (void)Ap;
4855  (void)sv;
4856  (void)rank_in;
4857  (void)imA;
4858  (void)imAt;
4859  (void)kerAt;
4860  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4861  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4862 #endif
4863 }
4864 
4906 vpColVector vpMatrix::getCol(unsigned int j, unsigned int i_begin, unsigned int column_size) const
4907 {
4908  if (((i_begin + column_size) > getRows()) || (j >= getCols())) {
4909  throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(),
4910  getCols()));
4911  }
4912  vpColVector c(column_size);
4913  for (unsigned int i = 0; i < column_size; ++i) {
4914  c[i] = (*this)[i_begin + i][j];
4915  }
4916  return c;
4917 }
4918 
4958 vpColVector vpMatrix::getCol(unsigned int j) const { return getCol(j, 0, rowNum); }
4959 
4996 vpRowVector vpMatrix::getRow(unsigned int i) const { return getRow(i, 0, colNum); }
4997 
5037 vpRowVector vpMatrix::getRow(unsigned int i, unsigned int j_begin, unsigned int row_size) const
5038 {
5039  if (((j_begin + row_size) > colNum) || (i >= rowNum)) {
5040  throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
5041  }
5042  vpRowVector r(row_size);
5043  if ((r.data != nullptr) && (data != nullptr)) {
5044  memcpy(r.data, (*this)[i] + j_begin, row_size * sizeof(double));
5045  }
5046 
5047  return r;
5048 }
5049 
5086 {
5087  unsigned int min_size = std::min<unsigned int>(rowNum, colNum);
5088  vpColVector diag;
5089 
5090  if (min_size > 0) {
5091  diag.resize(min_size, false);
5092 
5093  for (unsigned int i = 0; i < min_size; ++i) {
5094  diag[i] = (*this)[i][i];
5095  }
5096  }
5097 
5098  return diag;
5099 }
5100 
5112 {
5113  vpMatrix C;
5114 
5115  vpMatrix::stack(A, B, C);
5116 
5117  return C;
5118 }
5119 
5131 void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
5132 {
5133  unsigned int nra = A.getRows();
5134  unsigned int nrb = B.getRows();
5135 
5136  if (nra != 0) {
5137  if (A.getCols() != B.getCols()) {
5138  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5139  A.getCols(), B.getRows(), B.getCols()));
5140  }
5141  }
5142 
5143  if ((A.data != nullptr) && (A.data == C.data)) {
5144  std::cerr << "A and C must be two different objects!" << std::endl;
5145  return;
5146  }
5147 
5148  if ((B.data != nullptr) && (B.data == C.data)) {
5149  std::cerr << "B and C must be two different objects!" << std::endl;
5150  return;
5151  }
5152 
5153  C.resize(nra + nrb, B.getCols(), false, false);
5154 
5155  if ((C.data != nullptr) && (A.data != nullptr) && (A.size() > 0)) {
5156  // Copy A in C
5157  memcpy(C.data, A.data, sizeof(double) * A.size());
5158  }
5159 
5160  if ((C.data != nullptr) && (B.data != nullptr) && (B.size() > 0)) {
5161  // Copy B in C
5162  memcpy(C.data + A.size(), B.data, sizeof(double) * B.size());
5163  }
5164 }
5165 
5176 {
5177  vpMatrix C;
5178  vpMatrix::stack(A, r, C);
5179 
5180  return C;
5181 }
5182 
5194 void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
5195 {
5196  if ((A.data != nullptr) && (A.data == C.data)) {
5197  std::cerr << "A and C must be two different objects!" << std::endl;
5198  return;
5199  }
5200 
5201  C = A;
5202  C.stack(r);
5203 }
5204 
5215 {
5216  vpMatrix C;
5217  vpMatrix::stack(A, c, C);
5218 
5219  return C;
5220 }
5221 
5233 void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C)
5234 {
5235  if ((A.data != nullptr) && (A.data == C.data)) {
5236  std::cerr << "A and C must be two different objects!" << std::endl;
5237  return;
5238  }
5239 
5240  C = A;
5241  C.stack(c);
5242 }
5243 
5256 vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, unsigned int r, unsigned int c)
5257 {
5259 
5260  vpArray2D<double>::insert(A, B, C, r, c);
5261 
5262  return vpMatrix(C);
5263 }
5264 
5278 void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, unsigned int r, unsigned int c)
5279 {
5280  vpArray2D<double> C_array;
5281 
5282  vpArray2D<double>::insert(A, B, C_array, r, c);
5283 
5284  C = C_array;
5285 }
5286 
5299 {
5300  vpMatrix C;
5301 
5302  juxtaposeMatrices(A, B, C);
5303 
5304  return C;
5305 }
5306 
5320 {
5321  unsigned int nca = A.getCols();
5322  unsigned int ncb = B.getCols();
5323 
5324  if (nca != 0) {
5325  if (A.getRows() != B.getRows()) {
5326  throw(vpException(vpException::dimensionError, "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5327  A.getCols(), B.getRows(), B.getCols()));
5328  }
5329  }
5330 
5331  if ((B.getRows() == 0) || ((nca + ncb) == 0)) {
5332  std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl;
5333  return;
5334  }
5335 
5336  C.resize(B.getRows(), nca + ncb, false, false);
5337 
5338  C.insert(A, 0, 0);
5339  C.insert(B, 0, nca);
5340 }
5341 
5342 //--------------------------------------------------------------------
5343 // Output
5344 //--------------------------------------------------------------------
5345 
5365 int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &intro) const
5366 {
5367  typedef std::string::size_type size_type;
5368 
5369  unsigned int m = getRows();
5370  unsigned int n = getCols();
5371 
5372  std::vector<std::string> values(m * n);
5373  std::ostringstream oss;
5374  std::ostringstream ossFixed;
5375  std::ios_base::fmtflags original_flags = oss.flags();
5376 
5377  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
5378 
5379  size_type maxBefore = 0; // the length of the integral part
5380  size_type maxAfter = 0; // number of decimals plus
5381  // one place for the decimal point
5382  for (unsigned int i = 0; i < m; ++i) {
5383  for (unsigned int j = 0; j < n; ++j) {
5384  oss.str("");
5385  oss << (*this)[i][j];
5386  if (oss.str().find("e") != std::string::npos) {
5387  ossFixed.str("");
5388  ossFixed << (*this)[i][j];
5389  oss.str(ossFixed.str());
5390  }
5391 
5392  values[(i * n) + j] = oss.str();
5393  size_type thislen = values[(i * n) + j].size();
5394  size_type p = values[(i * n) + j].find('.');
5395 
5396  if (p == std::string::npos) {
5397  maxBefore = vpMath::maximum(maxBefore, thislen);
5398  // maxAfter remains the same
5399  }
5400  else {
5401  maxBefore = vpMath::maximum(maxBefore, p);
5402  maxAfter = vpMath::maximum(maxAfter, thislen - p);
5403  }
5404  }
5405  }
5406 
5407  size_type totalLength = length;
5408  // increase totalLength according to maxBefore
5409  totalLength = vpMath::maximum(totalLength, maxBefore);
5410  // decrease maxAfter according to totalLength
5411  maxAfter = std::min<size_type>(maxAfter, totalLength - maxBefore);
5412 
5413  if (!intro.empty()) {
5414  s << intro;
5415  }
5416  s << "[" << m << "," << n << "]=\n";
5417 
5418  for (unsigned int i = 0; i < m; ++i) {
5419  s << " ";
5420  for (unsigned int j = 0; j < n; ++j) {
5421  size_type p = values[(i * n) + j].find('.');
5422  s.setf(std::ios::right, std::ios::adjustfield);
5423  s.width(static_cast<std::streamsize>(maxBefore));
5424  s << values[(i * n) + j].substr(0, p).c_str();
5425 
5426  if (maxAfter > 0) {
5427  s.setf(std::ios::left, std::ios::adjustfield);
5428  if (p != std::string::npos) {
5429  s.width(static_cast<std::streamsize>(maxAfter));
5430  s << values[(i * n) + j].substr(p, maxAfter).c_str();
5431  }
5432  else {
5433  s.width(static_cast<std::streamsize>(maxAfter));
5434  s << ".0";
5435  }
5436  }
5437 
5438  s << ' ';
5439  }
5440  s << std::endl;
5441  }
5442 
5443  s.flags(original_flags); // restore s to standard state
5444 
5445  return static_cast<int>(maxBefore + maxAfter);
5446 }
5447 
5484 std::ostream &vpMatrix::matlabPrint(std::ostream &os) const
5485 {
5486  unsigned int this_rows = this->getRows();
5487  unsigned int this_col = this->getCols();
5488  os << "[ ";
5489  for (unsigned int i = 0; i < this_rows; ++i) {
5490  for (unsigned int j = 0; j < this_col; ++j) {
5491  os << (*this)[i][j] << ", ";
5492  }
5493  if (this->getRows() != (i + 1)) {
5494  os << ";" << std::endl;
5495  }
5496  else {
5497  os << "]" << std::endl;
5498  }
5499  }
5500  return os;
5501 }
5502 
5531 std::ostream &vpMatrix::maplePrint(std::ostream &os) const
5532 {
5533  unsigned int this_rows = this->getRows();
5534  unsigned int this_col = this->getCols();
5535  os << "([ " << std::endl;
5536  for (unsigned int i = 0; i < this_rows; ++i) {
5537  os << "[";
5538  for (unsigned int j = 0; j < this_col; ++j) {
5539  os << (*this)[i][j] << ", ";
5540  }
5541  os << "]," << std::endl;
5542  }
5543  os << "])" << std::endl;
5544  return os;
5545 }
5546 
5574 std::ostream &vpMatrix::csvPrint(std::ostream &os) const
5575 {
5576  unsigned int this_rows = this->getRows();
5577  unsigned int this_col = this->getCols();
5578  for (unsigned int i = 0; i < this_rows; ++i) {
5579  for (unsigned int j = 0; j < this_col; ++j) {
5580  os << (*this)[i][j];
5581  if (!(j == (this->getCols() - 1))) {
5582  os << ", ";
5583  }
5584  }
5585  os << std::endl;
5586  }
5587  return os;
5588 }
5589 
5625 std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
5626 {
5627  os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " << this->getCols() << "); " << std::endl;
5628 
5629  unsigned int this_rows = this->getRows();
5630  unsigned int this_col = this->getCols();
5631  for (unsigned int i = 0; i < this_rows; ++i) {
5632  for (unsigned int j = 0; j < this_col; ++j) {
5633  if (!octet) {
5634  os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl;
5635  }
5636  else {
5637  for (unsigned int k = 0; k < sizeof(double); ++k) {
5638  os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex
5639  << static_cast<unsigned int>(((unsigned char *)&((*this)[i][j]))[k]) << "; " << std::endl;
5640  }
5641  }
5642  }
5643  os << std::endl;
5644  }
5645  return os;
5646 }
5647 
5653 {
5654  if (rowNum == 0) {
5655  *this = A;
5656  }
5657  else if (A.getRows() > 0) {
5658  if (colNum != A.getCols()) {
5659  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum,
5660  A.getRows(), A.getCols()));
5661  }
5662 
5663  unsigned int rowNumOld = rowNum;
5664  resize(rowNum + A.getRows(), colNum, false, false);
5665  insert(A, rowNumOld, 0);
5666  }
5667 }
5668 
5685 {
5686  if (rowNum == 0) {
5687  *this = r;
5688  }
5689  else {
5690  if (colNum != r.getCols()) {
5691  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum,
5692  colNum, r.getCols()));
5693  }
5694 
5695  if (r.size() == 0) {
5696  return;
5697  }
5698 
5699  unsigned int oldSize = size();
5700  resize(rowNum + 1, colNum, false, false);
5701 
5702  if ((data != nullptr) && (r.data != nullptr) && (data != r.data)) {
5703  // Copy r in data
5704  memcpy(data + oldSize, r.data, sizeof(double) * r.size());
5705  }
5706  }
5707 }
5708 
5726 {
5727  if (colNum == 0) {
5728  *this = c;
5729  }
5730  else {
5731  if (rowNum != c.getRows()) {
5732  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum,
5733  colNum, c.getRows()));
5734  }
5735 
5736  if (c.size() == 0) {
5737  return;
5738  }
5739 
5740  vpMatrix tmp = *this;
5741  unsigned int oldColNum = colNum;
5742  resize(rowNum, colNum + 1, false, false);
5743 
5744  if ((data != nullptr) && (tmp.data != nullptr) && (data != tmp.data)) {
5745  // Copy c in data
5746  for (unsigned int i = 0; i < rowNum; ++i) {
5747  memcpy(data + (i * colNum), tmp.data + (i * oldColNum), sizeof(double) * oldColNum);
5748  rowPtrs[i][oldColNum] = c[i];
5749  }
5750  }
5751  }
5752 }
5753 
5764 void vpMatrix::insert(const vpMatrix &A, unsigned int r, unsigned int c)
5765 {
5766  if (((r + A.getRows()) <= rowNum) && ((c + A.getCols()) <= colNum)) {
5767  if ((A.colNum == colNum) && (data != nullptr) && (A.data != nullptr) && (A.data != data)) {
5768  memcpy(data + (r * colNum), A.data, sizeof(double) * A.size());
5769  }
5770  else if ((data != nullptr) && (A.data != nullptr) && (A.data != data)) {
5771  unsigned int a_rows = A.getRows();
5772  for (unsigned int i = r; i < (r + a_rows); ++i) {
5773  memcpy(data + (i * colNum) + c, A.data + ((i - r) * A.colNum), sizeof(double) * A.colNum);
5774  }
5775  }
5776  }
5777  else {
5778  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
5779  A.getRows(), A.getCols(), rowNum, colNum, r, c);
5780  }
5781 }
5782 
5820 {
5821  vpColVector evalue(rowNum); // Eigen values
5822 
5823  if (rowNum != colNum) {
5824  throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
5825  colNum));
5826  }
5827 
5828  // Check if the matrix is symmetric: At - A = 0
5829  vpMatrix At_A = (*this).t() - (*this);
5830  for (unsigned int i = 0; i < rowNum; ++i) {
5831  for (unsigned int j = 0; j < rowNum; ++j) {
5832  // in comment: if (At_A[i][j] != 0) {
5833  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
5834  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix"));
5835  }
5836  }
5837  }
5838 
5839 #if defined(VISP_HAVE_LAPACK)
5840 #if defined(VISP_HAVE_GSL) /* be careful of the copy below */
5841  {
5842  gsl_vector *eval = gsl_vector_alloc(rowNum);
5843  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
5844 
5845  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
5846  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
5847 
5848  unsigned int Atda = (unsigned int)m->tda;
5849  for (unsigned int i = 0; i < rowNum; i++) {
5850  unsigned int k = i * Atda;
5851  for (unsigned int j = 0; j < colNum; j++)
5852  m->data[k + j] = (*this)[i][j];
5853  }
5854  gsl_eigen_symmv(m, eval, evec, w);
5855 
5856  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
5857 
5858  for (unsigned int i = 0; i < rowNum; i++) {
5859  evalue[i] = gsl_vector_get(eval, i);
5860  }
5861 
5862  gsl_eigen_symmv_free(w);
5863  gsl_vector_free(eval);
5864  gsl_matrix_free(m);
5865  gsl_matrix_free(evec);
5866  }
5867 #else
5868  {
5869  const char jobz = 'N';
5870  const char uplo = 'U';
5871  vpMatrix A = (*this);
5872  vpColVector WORK;
5873  int lwork = -1;
5874  int info = 0;
5875  double wkopt;
5876  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
5877  lwork = static_cast<int>(wkopt);
5878  WORK.resize(lwork);
5879  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
5880  }
5881 #endif
5882 #else
5883  {
5884  throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. "
5885  "You should install Lapack 3rd party"));
5886  }
5887 #endif
5888  return evalue;
5889 }
5890 
5940 void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const
5941 {
5942  if (rowNum != colNum) {
5943  throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
5944  colNum));
5945  }
5946 
5947  // Check if the matrix is symmetric: At - A = 0
5948  vpMatrix At_A = (*this).t() - (*this);
5949  for (unsigned int i = 0; i < rowNum; ++i) {
5950  for (unsigned int j = 0; j < rowNum; ++j) {
5951  // -- in comment: if (At_A[i][j] != 0) {
5952  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
5953  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix"));
5954  }
5955  }
5956  }
5957 
5958  // Resize the output matrices
5959  evalue.resize(rowNum);
5960  evector.resize(rowNum, colNum);
5961 
5962 #if defined(VISP_HAVE_LAPACK)
5963 #if defined(VISP_HAVE_GSL) /* be careful of the copy below */
5964  {
5965  gsl_vector *eval = gsl_vector_alloc(rowNum);
5966  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
5967 
5968  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
5969  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
5970 
5971  unsigned int Atda = (unsigned int)m->tda;
5972  for (unsigned int i = 0; i < rowNum; i++) {
5973  unsigned int k = i * Atda;
5974  for (unsigned int j = 0; j < colNum; j++)
5975  m->data[k + j] = (*this)[i][j];
5976  }
5977  gsl_eigen_symmv(m, eval, evec, w);
5978 
5979  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
5980 
5981  for (unsigned int i = 0; i < rowNum; i++) {
5982  evalue[i] = gsl_vector_get(eval, i);
5983  }
5984  Atda = (unsigned int)evec->tda;
5985  for (unsigned int i = 0; i < rowNum; i++) {
5986  unsigned int k = i * Atda;
5987  for (unsigned int j = 0; j < rowNum; j++) {
5988  evector[i][j] = evec->data[k + j];
5989  }
5990  }
5991 
5992  gsl_eigen_symmv_free(w);
5993  gsl_vector_free(eval);
5994  gsl_matrix_free(m);
5995  gsl_matrix_free(evec);
5996  }
5997 #else // defined(VISP_HAVE_GSL)
5998  {
5999  const char jobz = 'V';
6000  const char uplo = 'U';
6001  vpMatrix A = (*this);
6002  vpColVector WORK;
6003  int lwork = -1;
6004  int info = 0;
6005  double wkopt;
6006  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
6007  lwork = static_cast<int>(wkopt);
6008  WORK.resize(lwork);
6009  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
6010  evector = A.t();
6011  }
6012 #endif // defined(VISP_HAVE_GSL)
6013 #else
6014  {
6015  throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. "
6016  "You should install Lapack 3rd party"));
6017  }
6018 #endif
6019 }
6020 
6039 unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const
6040 {
6041  unsigned int nbline = getRows();
6042  unsigned int nbcol = getCols();
6043 
6044  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6045  vpColVector sv;
6046  sv.resize(nbcol, false); // singular values
6047  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6048 
6049  // Copy and resize matrix to have at least as many rows as columns
6050  // kernel is computed in svd method only if the matrix has more rows than
6051  // columns
6052 
6053  if (nbline < nbcol) {
6054  U.resize(nbcol, nbcol, true);
6055  }
6056  else {
6057  U.resize(nbline, nbcol, false);
6058  }
6059 
6060  U.insert(*this, 0, 0);
6061 
6062  U.svd(sv, V);
6063 
6064  // Compute the highest singular value and rank of the matrix
6065  double maxsv = 0;
6066  for (unsigned int i = 0; i < nbcol; ++i) {
6067  if (sv[i] > maxsv) {
6068  maxsv = sv[i];
6069  }
6070  }
6071 
6072  unsigned int rank = 0;
6073  for (unsigned int i = 0; i < nbcol; ++i) {
6074  if (sv[i] >(maxsv * svThreshold)) {
6075  ++rank;
6076  }
6077  }
6078 
6079  kerAt.resize(nbcol - rank, nbcol);
6080  if (rank != nbcol) {
6081  for (unsigned int j = 0, k = 0; j < nbcol; ++j) {
6082  // if( v.col(j) in kernel and non zero )
6083  if ((sv[j] <= (maxsv * svThreshold)) &&
6084  (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
6085  unsigned int v_rows = V.getRows();
6086  for (unsigned int i = 0; i < v_rows; ++i) {
6087  kerAt[k][i] = V[i][j];
6088  }
6089  ++k;
6090  }
6091  }
6092  }
6093 
6094  return rank;
6095 }
6096 
6113 unsigned int vpMatrix::nullSpace(vpMatrix &kerA, double svThreshold) const
6114 {
6115  unsigned int nbrow = getRows();
6116  unsigned int nbcol = getCols();
6117 
6118  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6119  vpColVector sv;
6120  sv.resize(nbcol, false); // singular values
6121  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6122 
6123  // Copy and resize matrix to have at least as many rows as columns
6124  // kernel is computed in svd method only if the matrix has more rows than
6125  // columns
6126 
6127  if (nbrow < nbcol) {
6128  U.resize(nbcol, nbcol, true);
6129  }
6130  else {
6131  U.resize(nbrow, nbcol, false);
6132  }
6133 
6134  U.insert(*this, 0, 0);
6135 
6136  U.svd(sv, V);
6137 
6138  // Compute the highest singular value and rank of the matrix
6139  double maxsv = sv[0];
6140 
6141  unsigned int rank = 0;
6142  for (unsigned int i = 0; i < nbcol; ++i) {
6143  if (sv[i] >(maxsv * svThreshold)) {
6144  ++rank;
6145  }
6146  }
6147 
6148  kerA.resize(nbcol, nbcol - rank);
6149  if (rank != nbcol) {
6150  for (unsigned int j = 0, k = 0; j < nbcol; ++j) {
6151  // if( v.col(j) in kernel and non zero )
6152  if (sv[j] <= (maxsv * svThreshold)) {
6153  for (unsigned int i = 0; i < nbcol; ++i) {
6154  kerA[i][k] = V[i][j];
6155  }
6156  ++k;
6157  }
6158  }
6159  }
6160 
6161  return (nbcol - rank);
6162 }
6163 
6180 unsigned int vpMatrix::nullSpace(vpMatrix &kerA, int dim) const
6181 {
6182  unsigned int nbrow = getRows();
6183  unsigned int nbcol = getCols();
6184  unsigned int dim_ = static_cast<unsigned int>(dim);
6185 
6186  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6187  vpColVector sv;
6188  sv.resize(nbcol, false); // singular values
6189  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6190 
6191  // Copy and resize matrix to have at least as many rows as columns
6192  // kernel is computed in svd method only if the matrix has more rows than
6193  // columns
6194 
6195  if (nbrow < nbcol) {
6196  U.resize(nbcol, nbcol, true);
6197  }
6198  else {
6199  U.resize(nbrow, nbcol, false);
6200  }
6201 
6202  U.insert(*this, 0, 0);
6203 
6204  U.svd(sv, V);
6205 
6206  kerA.resize(nbcol, dim_);
6207  if (dim_ != 0) {
6208  unsigned int rank = nbcol - dim_;
6209  for (unsigned int k = 0; k < dim_; ++k) {
6210  unsigned int j = k + rank;
6211  for (unsigned int i = 0; i < nbcol; ++i) {
6212  kerA[i][k] = V[i][j];
6213  }
6214  }
6215  }
6216 
6217  double maxsv = sv[0];
6218  unsigned int rank = 0;
6219  for (unsigned int i = 0; i < nbcol; ++i) {
6220  if (sv[i] >(maxsv * 1e-6)) {
6221  ++rank;
6222  }
6223  }
6224  return (nbcol - rank);
6225 }
6226 
6258 double vpMatrix::det(vpDetMethod method) const
6259 {
6260  double det = 0.;
6261 
6262  if (method == LU_DECOMPOSITION) {
6263  det = this->detByLU();
6264  }
6265 
6266  return det;
6267 }
6268 
6270 {
6271 #if defined(VISP_HAVE_EIGEN3)
6272  return choleskyByEigen3();
6273 #elif defined(VISP_HAVE_LAPACK)
6274  return choleskyByLapack();
6275 #elif defined(VISP_HAVE_OPENCV)
6276  return choleskyByOpenCV();
6277 #else
6278  throw(vpException(vpException::fatalError, "Cannot compute matrix Chloesky decomposition. "
6279  "Install Lapack, Eigen3 or OpenCV 3rd party"));
6280 #endif
6281 }
6282 
6291 {
6292  if (colNum != rowNum) {
6293  throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix",
6294  rowNum, colNum));
6295  }
6296  else {
6297 #ifdef VISP_HAVE_GSL
6298  size_t size_ = rowNum * colNum;
6299  double *b = new double[size_];
6300  for (size_t i = 0; i < size_; i++)
6301  b[i] = 0.;
6302  gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum);
6303  gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum);
6304  gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
6305  // gsl_matrix_fprintf(stdout, &em.matrix, "%g");
6306  vpMatrix expA;
6307  expA.resize(rowNum, colNum, false);
6308  memcpy(expA.data, b, size_ * sizeof(double));
6309 
6310  delete[] b;
6311  return expA;
6312 #else
6313  vpMatrix v_expE(rowNum, colNum, false);
6314  vpMatrix v_expD(rowNum, colNum, false);
6315  vpMatrix v_expX(rowNum, colNum, false);
6316  vpMatrix v_expcX(rowNum, colNum, false);
6317  vpMatrix v_eye(rowNum, colNum, false);
6318 
6319  v_eye.eye();
6320  vpMatrix exp(*this);
6321 
6322  // -- in comment: double f;
6323  int e;
6324  double c = 0.5;
6325  int q = 6;
6326  int p = 1;
6327 
6328  double nA = 0;
6329  for (unsigned int i = 0; i < rowNum; ++i) {
6330  double sum = 0;
6331  for (unsigned int j = 0; j < colNum; ++j) {
6332  sum += fabs((*this)[i][j]);
6333  }
6334  if ((sum > nA) || (i == 0)) {
6335  nA = sum;
6336  }
6337  }
6338 
6339  /* f = */ frexp(nA, &e);
6340  // -- in comment: double s = (0 > e+1)?0:e+1;
6341  double s = e + 1;
6342 
6343  double sca = 1.0 / pow(2.0, s);
6344  exp = sca * exp;
6345  v_expX = *this;
6346  v_expE = (c * exp) + v_eye;
6347  v_expD = (-c * exp) + v_eye;
6348  for (int k = 2; k <= q; ++k) {
6349  c = (c * (static_cast<double>((q - k) + 1))) / (static_cast<double>(k * (((2 * q) - k) + 1)));
6350  v_expcX = exp * v_expX;
6351  v_expX = v_expcX;
6352  v_expcX = c * v_expX;
6353  v_expE = v_expE + v_expcX;
6354  if (p) {
6355  v_expD = v_expD + v_expcX;
6356  }
6357  else {
6358  v_expD = v_expD - v_expcX;
6359  }
6360  p = !p;
6361  }
6362  v_expX = v_expD.inverseByLU();
6363  exp = v_expX * v_expE;
6364  for (int k = 1; k <= s; ++k) {
6365  v_expE = exp * exp;
6366  exp = v_expE;
6367  }
6368  return exp;
6369 #endif
6370  }
6371 }
6372 
6373 /**************************************************************************************************************/
6374 /**************************************************************************************************************/
6375 
6376 // Specific functions
6377 
6378 /*
6379  input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.
6380 
6381  output:: the complement matrix of the element (rowNo,colNo).
6382  This is the matrix obtained from M after elimenating the row rowNo and column
6383  colNo
6384 
6385  example:
6386  1 2 3
6387  M = 4 5 6
6388  7 8 9
6389  1 3
6390  subblock(M, 1, 1) give the matrix 7 9
6391 */
6392 vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row)
6393 {
6394  vpMatrix M_comp;
6395  M_comp.resize(M.getRows() - 1, M.getCols() - 1, false);
6396 
6397  unsigned int m_rows = M.getRows();
6398  unsigned int m_col = M.getCols();
6399  for (unsigned int i = 0; i < col; ++i) {
6400  for (unsigned int j = 0; j < row; ++j) {
6401  M_comp[i][j] = M[i][j];
6402  }
6403  for (unsigned int j = row + 1; j < m_rows; ++j) {
6404  M_comp[i][j - 1] = M[i][j];
6405  }
6406  }
6407  for (unsigned int i = col + 1; i < m_col; ++i) {
6408  for (unsigned int j = 0; j < row; ++j) {
6409  M_comp[i - 1][j] = M[i][j];
6410  }
6411  for (unsigned int j = row + 1; j < m_rows; ++j) {
6412  M_comp[i - 1][j - 1] = M[i][j];
6413  }
6414  }
6415  return M_comp;
6416 }
6417 
6427 double vpMatrix::cond(double svThreshold) const
6428 {
6429  unsigned int nbline = getRows();
6430  unsigned int nbcol = getCols();
6431 
6432  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6433  vpColVector sv;
6434  sv.resize(nbcol); // singular values
6435  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6436 
6437  // Copy and resize matrix to have at least as many rows as columns
6438  // kernel is computed in svd method only if the matrix has more rows than
6439  // columns
6440 
6441  if (nbline < nbcol) {
6442  U.resize(nbcol, nbcol, true);
6443  }
6444  else {
6445  U.resize(nbline, nbcol, false);
6446  }
6447 
6448  U.insert(*this, 0, 0);
6449 
6450  U.svd(sv, V);
6451 
6452  // Compute the highest singular value
6453  double maxsv = 0;
6454  for (unsigned int i = 0; i < nbcol; ++i) {
6455  if (sv[i] > maxsv) {
6456  maxsv = sv[i];
6457  }
6458  }
6459 
6460  // Compute the rank of the matrix
6461  unsigned int rank = 0;
6462  for (unsigned int i = 0; i < nbcol; ++i) {
6463  if (sv[i] >(maxsv * svThreshold)) {
6464  ++rank;
6465  }
6466  }
6467 
6468  // Compute the lowest singular value
6469  double minsv = maxsv;
6470  for (unsigned int i = 0; i < rank; ++i) {
6471  if (sv[i] < minsv) {
6472  minsv = sv[i];
6473  }
6474  }
6475 
6476  if (std::fabs(minsv) > std::numeric_limits<double>::epsilon()) {
6477  return maxsv / minsv;
6478  }
6479  else {
6480  return std::numeric_limits<double>::infinity();
6481  }
6482 }
6483 
6490 void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
6491 {
6492  if (H.getCols() != H.getRows()) {
6493  throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(),
6494  H.getCols()));
6495  }
6496 
6497  HLM = H;
6498  unsigned int h_col = H.getCols();
6499  for (unsigned int i = 0; i < h_col; ++i) {
6500  HLM[i][i] += alpha * H[i][i];
6501  }
6502 }
6503 
6512 {
6513  double norm = 0.0;
6514  for (unsigned int i = 0; i < dsize; ++i) {
6515  double x = *(data + i);
6516  norm += x * x;
6517  }
6518 
6519  return sqrt(norm);
6520 }
6521 
6531 {
6532  if (this->dsize != 0) {
6533  vpMatrix v;
6534  vpColVector w;
6535 
6536  vpMatrix M = *this;
6537 
6538  M.svd(w, v);
6539 
6540  double max = w[0];
6541  unsigned int maxRank = std::min<unsigned int>(this->getCols(), this->getRows());
6542  // The maximum reachable rank is either the number of columns or the number of rows
6543  // of the matrix.
6544  unsigned int boundary = std::min<unsigned int>(maxRank, w.size());
6545  // boundary is here to ensure that the number of singular values used for the com-
6546  // putation of the euclidean norm of the matrix is not greater than the maximum
6547  // reachable rank. Indeed, some svd library pad the singular values vector with 0s
6548  // if the input matrix is non-square.
6549  for (unsigned int i = 0; i < boundary; ++i) {
6550  if (max < w[i]) {
6551  max = w[i];
6552  }
6553  }
6554  return max;
6555  }
6556  else {
6557  return 0.;
6558  }
6559 }
6560 
6572 {
6573  double norm = 0.0;
6574  for (unsigned int i = 0; i < rowNum; ++i) {
6575  double x = 0;
6576  for (unsigned int j = 0; j < colNum; ++j) {
6577  x += fabs(*(*(rowPtrs + i) + j));
6578  }
6579  if (x > norm) {
6580  norm = x;
6581  }
6582  }
6583  return norm;
6584 }
6585 
6592 double vpMatrix::sumSquare() const
6593 {
6594  double sum_square = 0.0;
6595  double x;
6596 
6597  for (unsigned int i = 0; i < rowNum; ++i) {
6598  for (unsigned int j = 0; j < colNum; ++j) {
6599  x = rowPtrs[i][j];
6600  sum_square += x * x;
6601  }
6602  }
6603 
6604  return sum_square;
6605 }
6606 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
6616 double vpMatrix::euclideanNorm() const { return frobeniusNorm(); }
6617 
6619 {
6620  return (vpMatrix)(vpColVector::stack(A, B));
6621 }
6622 
6624 {
6625  vpColVector::stack(A, B, C);
6626 }
6627 
6629 
6630 void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); }
6631 
6651 {
6652  vpRowVector c(getCols());
6653 
6654  for (unsigned int j = 0; j < getCols(); ++j) {
6655  c[j] = (*this)[i - 1][j];
6656  }
6657  return c;
6658 }
6659 
6678 {
6679  vpColVector c(getRows());
6680 
6681  for (unsigned int i = 0; i < getRows(); ++i) {
6682  c[i] = (*this)[i][j - 1];
6683  }
6684  return c;
6685 }
6686 
6693 void vpMatrix::setIdentity(const double &val)
6694 {
6695  for (unsigned int i = 0; i < rowNum; ++i) {
6696  for (unsigned int j = 0; j < colNum; ++j) {
6697  if (i == j) {
6698  (*this)[i][j] = val;
6699  }
6700  else {
6701  (*this)[i][j] = 0;
6702  }
6703  }
6704  }
6705 }
6706 
6707 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
6708 
6713 vpMatrix operator*(const double &x, const vpMatrix &B)
6714 {
6715  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
6716  return B;
6717  }
6718 
6719  unsigned int Brow = B.getRows();
6720  unsigned int Bcol = B.getCols();
6721 
6722  VISP_NAMESPACE_ADDRESSING vpMatrix C;
6723  C.resize(Brow, Bcol, false, false);
6724 
6725  for (unsigned int i = 0; i < Brow; ++i) {
6726  for (unsigned int j = 0; j < Bcol; ++j) {
6727  C[i][j] = B[i][j] * x;
6728  }
6729  }
6730 
6731  return C;
6732 }
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:127
unsigned int getCols() const
Definition: vpArray2D.h:330
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:140
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:134
void insert(const vpArray2D< Type > &A, unsigned int r, unsigned int c)
Definition: vpArray2D.h:487
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:355
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:130
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:136
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:342
unsigned int getRows() const
Definition: vpArray2D.h:340
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:132
Implementation of column vector and the associated operations.
Definition: vpColVector.h:171
double sumSquare() const
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1066
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:66
@ dimensionError
Bad dimension.
Definition: vpException.h:71
@ fatalError
Fatal error.
Definition: vpException.h:72
@ divideByZeroError
Division by zero.
Definition: vpException.h:70
Implementation of an homogeneous matrix and operations on such kind of matrices.
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:254
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:151
void svdLapack(vpColVector &w, vpMatrix &V)
void svdOpenCV(vpColVector &w, vpMatrix &V)
vpColVector eigenValues() const
Definition: vpMatrix.cpp:5819
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:5625
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6427
vpMatrix expm() const
Definition: vpMatrix.cpp:6290
vpMatrix pseudoInverseEigen3(double svThreshold=1e-6) const
vpMatrix hadamard(const vpMatrix &m) const
Definition: vpMatrix.cpp:1846
vpMatrix operator-() const
Definition: vpMatrix.cpp:1645
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:6693
vpMatrix & operator/=(double x)
Divide all the element of the matrix by x : Aij = Aij / x.
Definition: vpMatrix.cpp:1763
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5365
std::ostream & maplePrint(std::ostream &os) const
Definition: vpMatrix.cpp:5531
vpMatrix cholesky() const
Definition: vpMatrix.cpp:6269
void eye()
Definition: vpMatrix.cpp:456
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6039
vpMatrix operator*(const vpMatrix &B) const
Definition: vpMatrix.cpp:1206
vpMatrix operator/(double x) const
Cij = Aij / x (A is unchanged)
Definition: vpMatrix.cpp:1695
vpMatrix inverseByLU() const
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5652
vp_deprecated void stackMatrices(const vpMatrix &A)
Definition: vpMatrix.h:1011
vpMatrix & operator-=(const vpMatrix &B)
Operation A = A - B.
Definition: vpMatrix.cpp:1599
vpMatrix & operator*=(double x)
Multiply all the element of the matrix by x : Aij = Aij * x.
Definition: vpMatrix.cpp:1747
vpMatrix operator*(const double &x, const vpMatrix &B)
Definition: vpMatrix.cpp:6713
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:5298
vp_deprecated double euclideanNorm() const
Definition: vpMatrix.cpp:6616
vpMatrix & operator<<(double *p)
Definition: vpMatrix.cpp:818
vpColVector stackColumns()
Definition: vpMatrix.cpp:1810
vp_deprecated vpColVector column(unsigned int j)
Definition: vpMatrix.cpp:6677
static void sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1545
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2115
vpRowVector stackRows()
Definition: vpMatrix.cpp:1833
vpMatrix operator+(const vpMatrix &B) const
Definition: vpMatrix.cpp:1489
double sum() const
Definition: vpMatrix.cpp:1652
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:918
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:4996
vpMatrix t() const
Definition: vpMatrix.cpp:473
vpMatrix()
Definition: vpMatrix.h:167
vpMatrix & operator+=(const vpMatrix &B)
Operation A = A + B.
Definition: vpMatrix.cpp:1580
vpMatrix choleskyByOpenCV() const
Compute the Cholesky decomposition of a Hermitian positive-definite matrix using OpenCV library.
static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C)
Definition: vpMatrix.cpp:1397
vpMatrix choleskyByEigen3() const
Compute the Cholesky decomposition of a Hermitian positive-definite matrix using Eigen3 library.
double inducedL2Norm() const
Definition: vpMatrix.cpp:6530
double infinityNorm() const
Definition: vpMatrix.cpp:6571
vpMatrix & operator=(const vpArray2D< double > &A)
Definition: vpMatrix.cpp:678
double frobeniusNorm() const
Definition: vpMatrix.cpp:6511
vpColVector getDiag() const
Definition: vpMatrix.cpp:5085
vpMatrix AtA() const
Definition: vpMatrix.cpp:653
static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
Definition: vpMatrix.cpp:937
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:1993
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6490
vpMatrix pseudoInverseLapack(double svThreshold=1e-6) const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2326
vpMatrix & operator,(double val)
Definition: vpMatrix.cpp:835
static void multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
Definition: vpMatrix.cpp:992
vp_deprecated vpRowVector row(unsigned int i)
Definition: vpMatrix.cpp:6650
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:4958
double detByLU() const
vpMatrix choleskyByLapack() const
Compute the Cholesky decomposition of a Hermitian positive-definite matrix using Lapack library.
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6258
double sumSquare() const
Definition: vpMatrix.cpp:6592
vp_deprecated void init()
Definition: vpMatrix.h:1006
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1429
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5764
void svdEigen3(vpColVector &w, vpMatrix &V)
void kron(const vpMatrix &m1, vpMatrix &out) const
Definition: vpMatrix.cpp:1902
vpMatrix AAt() const
Definition: vpMatrix.cpp:525
vpMatrix transpose() const
Definition: vpMatrix.cpp:480
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1043
static void negateMatrix(const vpMatrix &A, vpMatrix &C)
Definition: vpMatrix.cpp:1625
@ LU_DECOMPOSITION
Definition: vpMatrix.h:159
std::ostream & csvPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5574
unsigned int nullSpace(vpMatrix &kerA, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6113
std::ostream & matlabPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5484
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:412
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:110
void resize(unsigned int i, bool flagNullify=true)
Definition: vpRowVector.h:264
Class that consider the case of a translation vector.