Visual Servoing Platform  version 3.3.1 under development (2020-08-10)
vpMatrix.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Matrix manipulation.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
43 #include <algorithm>
44 #include <assert.h>
45 #include <cmath> // std::fabs
46 #include <fstream>
47 #include <limits> // numeric_limits
48 #include <sstream>
49 #include <stdio.h>
50 #include <stdlib.h>
51 #include <string.h>
52 #include <string>
53 #include <vector>
54 
55 #include <visp3/core/vpConfig.h>
56 #include <visp3/core/vpCPUFeatures.h>
57 #include <visp3/core/vpColVector.h>
58 #include <visp3/core/vpDebug.h>
59 #include <visp3/core/vpException.h>
60 #include <visp3/core/vpMath.h>
61 #include <visp3/core/vpMatrix.h>
62 #include <visp3/core/vpTranslationVector.h>
63 
64 #ifdef VISP_HAVE_LAPACK
65 # ifdef VISP_HAVE_GSL
66 # include <gsl/gsl_eigen.h>
67 # include <gsl/gsl_math.h>
68 # include <gsl/gsl_linalg.h>
69 # elif defined(VISP_HAVE_MKL)
70 # include <mkl.h>
71 typedef MKL_INT integer;
72 
73 void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_,
74  double *w_data, double *work_data, int lwork_, int &info_)
75 {
76  MKL_INT n = static_cast<MKL_INT>(n_);
77  MKL_INT lda = static_cast<MKL_INT>(lda_);
78  MKL_INT lwork = static_cast<MKL_INT>(lwork_);
79  MKL_INT info = static_cast<MKL_INT>(info_);
80 
81  dsyev(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
82 }
83 
84 # else
85 # if defined(VISP_HAVE_LAPACK_BUILT_IN)
86 typedef long int integer;
87 # else
88 typedef int integer;
89 # endif
90 extern "C" integer dsyev_(char *jobz, char *uplo, integer *n, double *a, integer *lda,
91  double *w, double *WORK, integer *lwork, integer *info);
92 
93 void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_,
94  double *w_data, double *work_data, int lwork_, int &info_)
95 {
96  integer n = static_cast<integer>(n_);
97  integer lda = static_cast<integer>(lda_);
98  integer lwork = static_cast<integer>(lwork_);
99  integer info = static_cast<integer>(info_);
100 
101  dsyev_(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
102 
103  lwork_ = static_cast<int>(lwork);
104  info_ = static_cast<int>(info);
105 }
106 # endif
107 #endif
108 
109 #if !defined(VISP_USE_MSVC) || (defined(VISP_USE_MSVC) && !defined(VISP_BUILD_SHARED_LIBS))
110 const unsigned int vpMatrix::m_lapack_min_size_default = 0;
111 unsigned int vpMatrix::m_lapack_min_size = vpMatrix::m_lapack_min_size_default;
112 #endif
113 
114 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
115 #include <emmintrin.h>
116 #define VISP_HAVE_SSE2 1
117 #endif
118 
119 #if defined __AVX__
120 #include <immintrin.h>
121 #define VISP_HAVE_AVX 1
122 #endif
123 
124 #if VISP_HAVE_AVX
125 namespace {
126  void transpose4x4(const vpMatrix& a, vpMatrix& b, unsigned int i, unsigned int j)
127  {
128  __m256d a0 = _mm256_loadu_pd(&a[i][j]);
129  __m256d a1 = _mm256_loadu_pd(&a[i + 1][j]);
130  __m256d a2 = _mm256_loadu_pd(&a[i + 2][j]);
131  __m256d a3 = _mm256_loadu_pd(&a[i + 3][j]);
132 
133  __m256d T0 = _mm256_shuffle_pd(a0, a1, 15);
134  __m256d T1 = _mm256_shuffle_pd(a0, a1, 0);
135  __m256d T2 = _mm256_shuffle_pd(a2, a3, 15);
136  __m256d T3 = _mm256_shuffle_pd(a2, a3, 0);
137 
138  a1 = _mm256_permute2f128_pd(T0, T2, 32);
139  a3 = _mm256_permute2f128_pd(T0, T2, 49);
140  a0 = _mm256_permute2f128_pd(T1, T3, 32);
141  a2 = _mm256_permute2f128_pd(T1, T3, 49);
142 
143  _mm256_storeu_pd(&b[j][i], a0);
144  _mm256_storeu_pd(&b[j + 1][i], a1);
145  _mm256_storeu_pd(&b[j + 2][i], a2);
146  _mm256_storeu_pd(&b[j + 3][i], a3);
147  }
148 
149  void transpose16x16(const vpMatrix& a, vpMatrix& b, unsigned int i, unsigned int j)
150  {
151  transpose4x4(a, b, i, j);
152  transpose4x4(a, b, i, j + 4);
153  transpose4x4(a, b, i, j + 8);
154  transpose4x4(a, b, i, j + 12);
155 
156  transpose4x4(a, b, i + 4, j);
157  transpose4x4(a, b, i + 4, j + 4);
158  transpose4x4(a, b, i + 4, j + 8);
159  transpose4x4(a, b, i + 4, j + 12);
160 
161  transpose4x4(a, b, i + 8, j);
162  transpose4x4(a, b, i + 8, j + 4);
163  transpose4x4(a, b, i + 8, j + 8);
164  transpose4x4(a, b, i + 8, j + 12);
165 
166  transpose4x4(a, b, i + 12, j);
167  transpose4x4(a, b, i + 12, j + 4);
168  transpose4x4(a, b, i + 12, j + 8);
169  transpose4x4(a, b, i + 12, j + 12);
170  }
171 }
172 #endif
173 
174 // Prototypes of specific functions
175 vpMatrix subblock(const vpMatrix &, unsigned int, unsigned int);
176 
177 void compute_pseudo_inverse(const vpMatrix &a, const vpColVector &sv, const vpMatrix &v, unsigned int nrows,
178  unsigned int ncols, unsigned int nrows_orig, unsigned int ncols_orig, double svThreshold,
179  vpMatrix &Ap, unsigned int &rank)
180 {
181  vpMatrix a1;
182  a1.resize(ncols, nrows, false, false);
183 
184  // compute the highest singular value and the rank of h
185  double maxsv = 0;
186  for (unsigned int i = 0; i < ncols; i++) {
187  if (fabs(sv[i]) > maxsv)
188  maxsv = fabs(sv[i]);
189  }
190 
191  rank = 0;
192 
193  for (unsigned int i = 0; i < ncols; i++) {
194  if (fabs(sv[i]) > maxsv * svThreshold) {
195  rank++;
196  }
197 
198  for (unsigned int j = 0; j < nrows; j++) {
199  a1[i][j] = 0.0;
200 
201  for (unsigned int k = 0; k < ncols; k++) {
202  if (fabs(sv[k]) > maxsv * svThreshold) {
203  a1[i][j] += v[i][k] * a[j][k] / sv[k];
204  }
205  }
206  }
207  }
208  if (nrows_orig >= ncols_orig)
209  Ap = a1;
210  else
211  Ap = a1.t();
212 }
213 
214 void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows_orig,
215  unsigned int ncols_orig, double svThreshold, vpMatrix &Ap, unsigned int &rank,
216  vpMatrix &imA, vpMatrix &imAt, vpMatrix &kerAt)
217 {
218  Ap.resize(ncols_orig, nrows_orig, true);
219 
220  // compute the highest singular value and the rank of h
221  double maxsv = fabs(sv[0]);
222 
223  rank = 0;
224 
225  for (unsigned int i = 0; i < ncols_orig; i++) {
226  if (fabs(sv[i]) > maxsv * svThreshold) {
227  rank++;
228  }
229 
230  for (unsigned int j = 0; j < nrows_orig; j++) {
231  // Ap[i][j] = 0.0;
232 
233  for (unsigned int k = 0; k < ncols_orig; k++) {
234  if (fabs(sv[k]) > maxsv * svThreshold) {
235  Ap[i][j] += V[i][k] * U[j][k] / sv[k];
236  }
237  }
238  }
239  }
240 
241  // Compute im(A) and im(At)
242  imA.resize(nrows_orig, rank);
243  imAt.resize(ncols_orig, rank);
244 
245  for (unsigned int i = 0; i < nrows_orig; i++) {
246  for (unsigned int j = 0; j < rank; j++) {
247  imA[i][j] = U[i][j];
248  }
249  }
250 
251  for (unsigned int i = 0; i < ncols_orig; i++) {
252  for (unsigned int j = 0; j < rank; j++) {
253  imAt[i][j] = V[i][j];
254  }
255  }
256 
257  kerAt.resize(ncols_orig - rank, ncols_orig);
258  if (rank != ncols_orig) {
259  for (unsigned int j = 0, k = 0; j < ncols_orig; j++) {
260  // if( v.col(j) in kernel and non zero )
261  if ((fabs(sv[j]) <= maxsv * svThreshold) &&
262  (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
263  for (unsigned int i = 0; i < V.getRows(); i++) {
264  kerAt[k][i] = V[i][j];
265  }
266  k++;
267  }
268  }
269  }
270 }
271 
277 vpMatrix::vpMatrix(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
278  : vpArray2D<double>()
279 {
280  if (((r + nrows) > M.rowNum) || ((c + ncols) > M.colNum)) {
282  "Cannot construct a sub matrix (%dx%d) starting at "
283  "position (%d,%d) that is not contained in the "
284  "original matrix (%dx%d)",
285  nrows, ncols, r, c, M.rowNum, M.colNum));
286  }
287 
288  init(M, r, c, nrows, ncols);
289 }
290 
291 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
293 {
294  rowNum = A.rowNum;
295  colNum = A.colNum;
296  rowPtrs = A.rowPtrs;
297  dsize = A.dsize;
298  data = A.data;
299 
300  A.rowNum = 0;
301  A.colNum = 0;
302  A.rowPtrs = NULL;
303  A.dsize = 0;
304  A.data = NULL;
305 }
306 
332 vpMatrix::vpMatrix(const std::initializer_list<double> &list) : vpArray2D<double>(list) { }
333 
334 
359 vpMatrix::vpMatrix(unsigned int nrows, unsigned int ncols, const std::initializer_list<double> &list)
360  : vpArray2D<double>(nrows, ncols, list) {}
361 
384 vpMatrix::vpMatrix(const std::initializer_list<std::initializer_list<double> > &lists) : vpArray2D<double>(lists) { }
385 
386 #endif
387 
434 void vpMatrix::init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
435 {
436  unsigned int rnrows = r + nrows;
437  unsigned int cncols = c + ncols;
438 
439  if (rnrows > M.getRows())
440  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
441  M.getRows()));
442  if (cncols > M.getCols())
443  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
444  M.getCols()));
445  resize(nrows, ncols, false, false);
446 
447  if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
448  return; // Noting to do
449  for (unsigned int i = 0; i < nrows; i++) {
450  memcpy((*this)[i], &M[i + r][c], ncols * sizeof(double));
451  }
452 }
453 
495 vpMatrix vpMatrix::extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
496 {
497  unsigned int rnrows = r + nrows;
498  unsigned int cncols = c + ncols;
499 
500  if (rnrows > getRows())
501  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
502  getRows()));
503  if (cncols > getCols())
504  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
505  getCols()));
506 
507  vpMatrix M;
508  M.resize(nrows, ncols, false, false);
509  for (unsigned int i = 0; i < nrows; i++) {
510  memcpy(M[i], &(*this)[i + r][c], ncols * sizeof(double));
511  }
512 
513  return M;
514 }
515 
520 void vpMatrix::eye(unsigned int n) { eye(n, n); }
521 
526 void vpMatrix::eye(unsigned int m, unsigned int n)
527 {
528  resize(m, n);
529 
530  eye();
531 }
532 
538 {
539  for (unsigned int i = 0; i < rowNum; i++) {
540  for (unsigned int j = 0; j < colNum; j++) {
541  if (i == j)
542  (*this)[i][j] = 1.0;
543  else
544  (*this)[i][j] = 0;
545  }
546  }
547 }
548 
553 {
554  return transpose();
555 }
556 
563 {
564  vpMatrix At;
565  transpose(At);
566  return At;
567 }
568 
575 {
576  At.resize(colNum, rowNum, false, false);
577 
578  if (rowNum <= 16 || colNum <= 16) {
579  for (unsigned int i = 0; i < rowNum; i++) {
580  for (unsigned int j = 0; j < colNum; j++) {
581  At[j][i] = (*this)[i][j];
582  }
583  }
584  }
585  else {
586  bool haveAVX = vpCPUFeatures::checkAVX();
587 #if !VISP_HAVE_AVX
588  haveAVX = false;
589 #endif
590 
591  if (haveAVX) {
592 #if VISP_HAVE_AVX
593  // matrix transpose using tiling
594  const int nrows = static_cast<int>(rowNum);
595  const int ncols = static_cast<int>(colNum);
596  const int tileSize = 16;
597 
598  for (int i = 0; i < nrows;) {
599  for (; i <= nrows - tileSize; i += tileSize) {
600  int j = 0;
601  for (; j <= ncols - tileSize; j += tileSize) {
602  transpose16x16(*this, At, i, j);
603  }
604 
605  for (int k = i; k < i + tileSize; k++) {
606  for (int l = j; l < ncols; l++) {
607  At[l][k] = (*this)[k][l];
608  }
609  }
610  }
611 
612  for (; i < nrows; i++) {
613  for (int j = 0; j < ncols; j++) {
614  At[j][i] = (*this)[i][j];
615  }
616  }
617  }
618 
619  _mm256_zeroupper();
620 #endif
621  } else {
622  // https://stackoverflow.com/a/21548079
623  const int tileSize = 32;
624  for (unsigned int i = 0; i < rowNum; i += tileSize) {
625  for (unsigned int j = 0; j < colNum; j++) {
626  for (unsigned int b = 0; b < static_cast<unsigned int>(tileSize) && i + b < rowNum; b++) {
627  At[j][i + b] = (*this)[i + b][j];
628  }
629  }
630  }
631  }
632  }
633 }
634 
641 {
642  vpMatrix B;
643 
644  AAt(B);
645 
646  return B;
647 }
648 
660 void vpMatrix::AAt(vpMatrix &B) const
661 {
662  if ((B.rowNum != rowNum) || (B.colNum != rowNum))
663  B.resize(rowNum, rowNum, false, false);
664 
665  // If available use Lapack only for large matrices
666  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
667 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
668  useLapack = false;
669 #endif
670 
671  if (useLapack) {
672 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
673  const double alpha = 1.0;
674  const double beta = 0.0;
675  const char transa = 't';
676  const char transb = 'n';
677 
678  vpMatrix::blas_dgemm(transa, transb, rowNum, rowNum, colNum, alpha, data, colNum, data, colNum, beta, B.data, rowNum);
679 #endif
680  }
681  else {
682  // compute A*A^T
683  for (unsigned int i = 0; i < rowNum; i++) {
684  for (unsigned int j = i; j < rowNum; j++) {
685  double *pi = rowPtrs[i]; // row i
686  double *pj = rowPtrs[j]; // row j
687 
688  // sum (row i .* row j)
689  double ssum = 0;
690  for (unsigned int k = 0; k < colNum; k++)
691  ssum += *(pi++) * *(pj++);
692 
693  B[i][j] = ssum; // upper triangle
694  if (i != j)
695  B[j][i] = ssum; // lower triangle
696  }
697  }
698  }
699 }
700 
712 void vpMatrix::AtA(vpMatrix &B) const
713 {
714  if ((B.rowNum != colNum) || (B.colNum != colNum))
715  B.resize(colNum, colNum, false, false);
716 
717  // If available use Lapack only for large matrices
718  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
719 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
720  useLapack = false;
721 #endif
722 
723  if (useLapack) {
724 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
725  const double alpha = 1.0;
726  const double beta = 0.0;
727  const char transa = 'n';
728  const char transb = 't';
729 
730  vpMatrix::blas_dgemm(transa, transb, colNum, colNum, rowNum, alpha, data, colNum, data, colNum, beta, B.data, colNum);
731 #endif
732  }
733  else {
734  for (unsigned int i = 0; i < colNum; i++) {
735  double *Bi = B[i];
736  for (unsigned int j = 0; j < i; j++) {
737  double *ptr = data;
738  double s = 0;
739  for (unsigned int k = 0; k < rowNum; k++) {
740  s += (*(ptr + i)) * (*(ptr + j));
741  ptr += colNum;
742  }
743  *Bi++ = s;
744  B[j][i] = s;
745  }
746  double *ptr = data;
747  double s = 0;
748  for (unsigned int k = 0; k < rowNum; k++) {
749  s += (*(ptr + i)) * (*(ptr + i));
750  ptr += colNum;
751  }
752  *Bi = s;
753  }
754  }
755 }
756 
763 {
764  vpMatrix B;
765 
766  AtA(B);
767 
768  return B;
769 }
770 
788 {
789  resize(A.getRows(), A.getCols(), false, false);
790 
791  if (data != NULL && A.data != NULL && data != A.data) {
792  memcpy(data, A.data, dsize * sizeof(double));
793  }
794 
795  return *this;
796 }
797 
798 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
800 {
801  resize(A.getRows(), A.getCols(), false, false);
802 
803  if (data != NULL && A.data != NULL && data != A.data) {
804  memcpy(data, A.data, dsize * sizeof(double));
805  }
806 
807  return *this;
808 }
809 
811 {
812  if (this != &other) {
813  free(data);
814  free(rowPtrs);
815 
816  rowNum = other.rowNum;
817  colNum = other.colNum;
818  rowPtrs = other.rowPtrs;
819  dsize = other.dsize;
820  data = other.data;
821 
822  other.rowNum = 0;
823  other.colNum = 0;
824  other.rowPtrs = NULL;
825  other.dsize = 0;
826  other.data = NULL;
827  }
828 
829  return *this;
830 }
831 
856 vpMatrix& vpMatrix::operator=(const std::initializer_list<double> &list)
857 {
858  if (dsize != static_cast<unsigned int>(list.size())) {
859  resize(1, static_cast<unsigned int>(list.size()), false, false);
860  }
861 
862  std::copy(list.begin(), list.end(), data);
863 
864  return *this;
865 }
866 
890 vpMatrix& vpMatrix::operator=(const std::initializer_list<std::initializer_list<double> > &lists)
891 {
892  unsigned int nrows = static_cast<unsigned int>(lists.size()), ncols = 0;
893  for (auto& l : lists) {
894  if (static_cast<unsigned int>(l.size()) > ncols) {
895  ncols = static_cast<unsigned int>(l.size());
896  }
897  }
898 
899  resize(nrows, ncols, false, false);
900  auto it = lists.begin();
901  for (unsigned int i = 0; i < rowNum; i++, ++it) {
902  std::copy(it->begin(), it->end(), rowPtrs[i]);
903  }
904 
905  return *this;
906 }
907 #endif
908 
911 {
912  std::fill(data, data + rowNum*colNum, x);
913  return *this;
914 }
915 
922 {
923  for (unsigned int i = 0; i < rowNum; i++) {
924  for (unsigned int j = 0; j < colNum; j++) {
925  rowPtrs[i][j] = *x++;
926  }
927  }
928  return *this;
929 }
930 
932 {
933  resize(1, 1, false, false);
934  rowPtrs[0][0] = val;
935  return *this;
936 }
937 
939 {
940  resize(1, colNum + 1, false, false);
941  rowPtrs[0][colNum - 1] = val;
942  return *this;
943 }
944 
981 {
982  unsigned int rows = A.getRows();
983  this->resize(rows, rows);
984 
985  (*this) = 0;
986  for (unsigned int i = 0; i < rows; i++)
987  (*this)[i][i] = A[i];
988 }
989 
1020 void vpMatrix::diag(const double &val)
1021 {
1022  (*this) = 0;
1023  unsigned int min_ = (rowNum < colNum) ? rowNum : colNum;
1024  for (unsigned int i = 0; i < min_; i++)
1025  (*this)[i][i] = val;
1026 }
1027 
1040 {
1041  unsigned int rows = A.getRows();
1042  DA.resize(rows, rows, true);
1043 
1044  for (unsigned int i = 0; i < rows; i++)
1045  DA[i][i] = A[i];
1046 }
1047 
1053 {
1054  vpTranslationVector t_out;
1055 
1056  if (rowNum != 3 || colNum != 3) {
1057  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector",
1058  rowNum, colNum, tv.getRows(), tv.getCols()));
1059  }
1060 
1061  for (unsigned int j = 0; j < 3; j++)
1062  t_out[j] = 0;
1063 
1064  for (unsigned int j = 0; j < 3; j++) {
1065  double tj = tv[j]; // optimization em 5/12/2006
1066  for (unsigned int i = 0; i < 3; i++) {
1067  t_out[i] += rowPtrs[i][j] * tj;
1068  }
1069  }
1070  return t_out;
1071 }
1072 
1078 {
1079  vpColVector v_out;
1080  vpMatrix::multMatrixVector(*this, v, v_out);
1081  return v_out;
1082 }
1083 
1093 {
1094  if (A.colNum != v.getRows()) {
1095  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
1096  A.getRows(), A.getCols(), v.getRows()));
1097  }
1098 
1099  if (A.rowNum != w.rowNum)
1100  w.resize(A.rowNum, false);
1101 
1102  // If available use Lapack only for large matrices
1103  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size);
1104 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1105  useLapack = false;
1106 #endif
1107 
1108  if (useLapack) {
1109 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1110  double alpha = 1.0;
1111  double beta = 0.0;
1112  char trans = 't';
1113  int incr = 1;
1114 
1115  vpMatrix::blas_dgemv(trans, A.colNum, A.rowNum, alpha, A.data, A.colNum, v.data, incr, beta, w.data, incr);
1116 #endif
1117  }
1118  else {
1119  w = 0.0;
1120  for (unsigned int j = 0; j < A.colNum; j++) {
1121  double vj = v[j]; // optimization em 5/12/2006
1122  for (unsigned int i = 0; i < A.rowNum; i++) {
1123  w[i] += A.rowPtrs[i][j] * vj;
1124  }
1125  }
1126  }
1127 }
1128 
1129 //---------------------------------
1130 // Matrix operations.
1131 //---------------------------------
1132 
1143 {
1144  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1145  C.resize(A.rowNum, B.colNum, false, false);
1146 
1147  if (A.colNum != B.rowNum) {
1148  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (%dx%d) matrix", A.getRows(),
1149  A.getCols(), B.getRows(), B.getCols()));
1150  }
1151 
1152  // If available use Lapack only for large matrices
1153  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size || B.colNum > vpMatrix::m_lapack_min_size);
1154 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1155  useLapack = false;
1156 #endif
1157 
1158  if (useLapack) {
1159 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1160  const double alpha = 1.0;
1161  const double beta = 0.0;
1162  const char trans = 'n';
1163  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1164  C.data, B.colNum);
1165 #endif
1166  }
1167  else {
1168  // 5/12/06 some "very" simple optimization to avoid indexation
1169  const unsigned int BcolNum = B.colNum;
1170  const unsigned int BrowNum = B.rowNum;
1171  double **BrowPtrs = B.rowPtrs;
1172  for (unsigned int i = 0; i < A.rowNum; i++) {
1173  const double *rowptri = A.rowPtrs[i];
1174  double *ci = C[i];
1175  for (unsigned int j = 0; j < BcolNum; j++) {
1176  double s = 0;
1177  for (unsigned int k = 0; k < BrowNum; k++)
1178  s += rowptri[k] * BrowPtrs[k][j];
1179  ci[j] = s;
1180  }
1181  }
1182  }
1183 }
1184 
1199 {
1200  if (A.colNum != 3 || A.rowNum != 3 || B.colNum != 3 || B.rowNum != 3) {
1202  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1203  "rotation matrix",
1204  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1205  }
1206  // 5/12/06 some "very" simple optimization to avoid indexation
1207  const unsigned int BcolNum = B.colNum;
1208  const unsigned int BrowNum = B.rowNum;
1209  double **BrowPtrs = B.rowPtrs;
1210  for (unsigned int i = 0; i < A.rowNum; i++) {
1211  const double *rowptri = A.rowPtrs[i];
1212  double *ci = C[i];
1213  for (unsigned int j = 0; j < BcolNum; j++) {
1214  double s = 0;
1215  for (unsigned int k = 0; k < BrowNum; k++)
1216  s += rowptri[k] * BrowPtrs[k][j];
1217  ci[j] = s;
1218  }
1219  }
1220 }
1221 
1236 {
1237  if (A.colNum != 4 || A.rowNum != 4 || B.colNum != 4 || B.rowNum != 4) {
1239  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1240  "rotation matrix",
1241  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1242  }
1243  // Considering perfMatrixMultiplication.cpp benchmark,
1244  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1245  // Lapack usage needs to be validated again.
1246  // If available use Lapack only for large matrices.
1247  // Using SSE2 doesn't speed up.
1248  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size || B.colNum > vpMatrix::m_lapack_min_size);
1249 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1250  useLapack = false;
1251 #endif
1252 
1253  if (useLapack) {
1254 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1255  const double alpha = 1.0;
1256  const double beta = 0.0;
1257  const char trans = 'n';
1258  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1259  C.data, B.colNum);
1260 #endif
1261  }
1262  else {
1263  // 5/12/06 some "very" simple optimization to avoid indexation
1264  const unsigned int BcolNum = B.colNum;
1265  const unsigned int BrowNum = B.rowNum;
1266  double **BrowPtrs = B.rowPtrs;
1267  for (unsigned int i = 0; i < A.rowNum; i++) {
1268  const double *rowptri = A.rowPtrs[i];
1269  double *ci = C[i];
1270  for (unsigned int j = 0; j < BcolNum; j++) {
1271  double s = 0;
1272  for (unsigned int k = 0; k < BrowNum; k++)
1273  s += rowptri[k] * BrowPtrs[k][j];
1274  ci[j] = s;
1275  }
1276  }
1277  }
1278 }
1279 
1293 {
1294  vpMatrix::multMatrixVector(A, B, C);
1295 }
1296 
1302 {
1303  vpMatrix C;
1304 
1305  vpMatrix::mult2Matrices(*this, B, C);
1306 
1307  return C;
1308 }
1309 
1315 {
1316  if (colNum != R.getRows()) {
1317  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1318  colNum));
1319  }
1320  vpMatrix C;
1321  C.resize(rowNum, 3, false, false);
1322 
1323  unsigned int RcolNum = R.getCols();
1324  unsigned int RrowNum = R.getRows();
1325  for (unsigned int i = 0; i < rowNum; i++) {
1326  double *rowptri = rowPtrs[i];
1327  double *ci = C[i];
1328  for (unsigned int j = 0; j < RcolNum; j++) {
1329  double s = 0;
1330  for (unsigned int k = 0; k < RrowNum; k++)
1331  s += rowptri[k] * R[k][j];
1332  ci[j] = s;
1333  }
1334  }
1335 
1336  return C;
1337 }
1338 
1344 {
1345  if (colNum != M.getRows()) {
1346  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1347  colNum));
1348  }
1349  vpMatrix C;
1350  C.resize(rowNum, 4, false, false);
1351 
1352  const unsigned int McolNum = M.getCols();
1353  const unsigned int MrowNum = M.getRows();
1354  for (unsigned int i = 0; i < rowNum; i++) {
1355  const double *rowptri = rowPtrs[i];
1356  double *ci = C[i];
1357  for (unsigned int j = 0; j < McolNum; j++) {
1358  double s = 0;
1359  for (unsigned int k = 0; k < MrowNum; k++)
1360  s += rowptri[k] * M[k][j];
1361  ci[j] = s;
1362  }
1363  }
1364 
1365  return C;
1366 }
1367 
1373 {
1374  if (colNum != V.getRows()) {
1375  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix",
1376  rowNum, colNum));
1377  }
1378  vpMatrix M;
1379  M.resize(rowNum, 6, false, false);
1380 
1381  // Considering perfMatrixMultiplication.cpp benchmark,
1382  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1383  // Lapack usage needs to be validated again.
1384  // If available use Lapack only for large matrices.
1385  // Speed up obtained using SSE2.
1386  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size || V.colNum > vpMatrix::m_lapack_min_size);
1387 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1388  useLapack = false;
1389 #endif
1390 
1391  if (useLapack) {
1392 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1393  const double alpha = 1.0;
1394  const double beta = 0.0;
1395  const char trans = 'n';
1396  vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, V.colNum, data, colNum, beta,
1397  M.data, M.colNum);
1398 #endif
1399  }
1400  else {
1401  bool checkSSE2 = vpCPUFeatures::checkSSE2();
1402  #if !VISP_HAVE_SSE2
1403  checkSSE2 = false;
1404  #endif
1405 
1406  if (checkSSE2) {
1407  #if VISP_HAVE_SSE2
1408  vpMatrix V_trans;
1409  V_trans.resize(6, 6, false, false);
1410  for (unsigned int i = 0; i < 6; i++) {
1411  for (unsigned int j = 0; j < 6; j++) {
1412  V_trans[i][j] = V[j][i];
1413  }
1414  }
1415 
1416  for (unsigned int i = 0; i < rowNum; i++) {
1417  double *rowptri = rowPtrs[i];
1418  double *ci = M[i];
1419 
1420  for (int j = 0; j < 6; j++) {
1421  __m128d v_mul = _mm_setzero_pd();
1422  for (int k = 0; k < 6; k += 2) {
1423  v_mul = _mm_add_pd(v_mul, _mm_mul_pd(_mm_loadu_pd(&rowptri[k]), _mm_loadu_pd(&V_trans[j][k])));
1424  }
1425 
1426  double v_tmp[2];
1427  _mm_storeu_pd(v_tmp, v_mul);
1428  ci[j] = v_tmp[0] + v_tmp[1];
1429  }
1430  }
1431  #endif
1432  } else {
1433  const unsigned int VcolNum = V.getCols();
1434  const unsigned int VrowNum = V.getRows();
1435  for (unsigned int i = 0; i < rowNum; i++) {
1436  const double *rowptri = rowPtrs[i];
1437  double *ci = M[i];
1438  for (unsigned int j = 0; j < VcolNum; j++) {
1439  double s = 0;
1440  for (unsigned int k = 0; k < VrowNum; k++)
1441  s += rowptri[k] * V[k][j];
1442  ci[j] = s;
1443  }
1444  }
1445  }
1446  }
1447 
1448  return M;
1449 }
1450 
1456 {
1457  if (colNum != V.getRows()) {
1458  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) force/torque twist matrix",
1459  rowNum, colNum));
1460  }
1461  vpMatrix M;
1462  M.resize(rowNum, 6, false, false);
1463 
1464  // Considering perfMatrixMultiplication.cpp benchmark,
1465  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1466  // Lapack usage needs to be validated again.
1467  // If available use Lapack only for large matrices.
1468  // Speed up obtained using SSE2.
1469  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size || V.getCols() > vpMatrix::m_lapack_min_size);
1470 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1471  useLapack = false;
1472 #endif
1473 
1474  if (useLapack) {
1475 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1476  const double alpha = 1.0;
1477  const double beta = 0.0;
1478  const char trans = 'n';
1479  vpMatrix::blas_dgemm(trans, trans, V.getCols(), rowNum, colNum, alpha, V.data, V.getCols(), data, colNum, beta,
1480  M.data, M.colNum);
1481 #endif
1482  }
1483  else {
1484  bool checkSSE2 = vpCPUFeatures::checkSSE2();
1485  #if !VISP_HAVE_SSE2
1486  checkSSE2 = false;
1487  #endif
1488 
1489  if (checkSSE2) {
1490  #if VISP_HAVE_SSE2
1491  vpMatrix V_trans;
1492  V_trans.resize(6, 6, false, false);
1493  for (unsigned int i = 0; i < 6; i++) {
1494  for (unsigned int j = 0; j < 6; j++) {
1495  V_trans[i][j] = V[j][i];
1496  }
1497  }
1498 
1499  for (unsigned int i = 0; i < rowNum; i++) {
1500  double *rowptri = rowPtrs[i];
1501  double *ci = M[i];
1502 
1503  for (int j = 0; j < 6; j++) {
1504  __m128d v_mul = _mm_setzero_pd();
1505  for (int k = 0; k < 6; k += 2) {
1506  v_mul = _mm_add_pd(v_mul, _mm_mul_pd(_mm_loadu_pd(&rowptri[k]), _mm_loadu_pd(&V_trans[j][k])));
1507  }
1508 
1509  double v_tmp[2];
1510  _mm_storeu_pd(v_tmp, v_mul);
1511  ci[j] = v_tmp[0] + v_tmp[1];
1512  }
1513  }
1514  #endif
1515  } else {
1516  const unsigned int VcolNum = V.getCols();
1517  const unsigned int VrowNum = V.getRows();
1518  for (unsigned int i = 0; i < rowNum; i++) {
1519  const double *rowptri = rowPtrs[i];
1520  double *ci = M[i];
1521  for (unsigned int j = 0; j < VcolNum; j++) {
1522  double s = 0;
1523  for (unsigned int k = 0; k < VrowNum; k++)
1524  s += rowptri[k] * V[k][j];
1525  ci[j] = s;
1526  }
1527  }
1528  }
1529  }
1530 
1531  return M;
1532 }
1533 
1544 void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB,
1545  vpMatrix &C)
1546 {
1547  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1548  C.resize(A.rowNum, B.colNum, false, false);
1549 
1550  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1551  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1552  A.getCols(), B.getRows(), B.getCols()));
1553  }
1554 
1555  double **ArowPtrs = A.rowPtrs;
1556  double **BrowPtrs = B.rowPtrs;
1557  double **CrowPtrs = C.rowPtrs;
1558 
1559  for (unsigned int i = 0; i < A.rowNum; i++)
1560  for (unsigned int j = 0; j < A.colNum; j++)
1561  CrowPtrs[i][j] = wB * BrowPtrs[i][j] + wA * ArowPtrs[i][j];
1562 }
1563 
1573 void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1574 {
1575  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1576  C.resize(A.rowNum, B.colNum, false, false);
1577 
1578  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1579  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1580  A.getCols(), B.getRows(), B.getCols()));
1581  }
1582 
1583  double **ArowPtrs = A.rowPtrs;
1584  double **BrowPtrs = B.rowPtrs;
1585  double **CrowPtrs = C.rowPtrs;
1586 
1587  for (unsigned int i = 0; i < A.rowNum; i++) {
1588  for (unsigned int j = 0; j < A.colNum; j++) {
1589  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1590  }
1591  }
1592 }
1593 
1607 {
1608  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1609  C.resize(A.rowNum);
1610 
1611  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1612  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1613  A.getCols(), B.getRows(), B.getCols()));
1614  }
1615 
1616  double **ArowPtrs = A.rowPtrs;
1617  double **BrowPtrs = B.rowPtrs;
1618  double **CrowPtrs = C.rowPtrs;
1619 
1620  for (unsigned int i = 0; i < A.rowNum; i++) {
1621  for (unsigned int j = 0; j < A.colNum; j++) {
1622  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1623  }
1624  }
1625 }
1626 
1632 {
1633  vpMatrix C;
1634  vpMatrix::add2Matrices(*this, B, C);
1635  return C;
1636 }
1637 
1654 {
1655  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1656  C.resize(A.rowNum);
1657 
1658  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1659  throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1660  A.getCols(), B.getRows(), B.getCols()));
1661  }
1662 
1663  double **ArowPtrs = A.rowPtrs;
1664  double **BrowPtrs = B.rowPtrs;
1665  double **CrowPtrs = C.rowPtrs;
1666 
1667  for (unsigned int i = 0; i < A.rowNum; i++) {
1668  for (unsigned int j = 0; j < A.colNum; j++) {
1669  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1670  }
1671  }
1672 }
1673 
1686 void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1687 {
1688  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1689  C.resize(A.rowNum, A.colNum, false, false);
1690 
1691  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1692  throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1693  A.getCols(), B.getRows(), B.getCols()));
1694  }
1695 
1696  double **ArowPtrs = A.rowPtrs;
1697  double **BrowPtrs = B.rowPtrs;
1698  double **CrowPtrs = C.rowPtrs;
1699 
1700  for (unsigned int i = 0; i < A.rowNum; i++) {
1701  for (unsigned int j = 0; j < A.colNum; j++) {
1702  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1703  }
1704  }
1705 }
1706 
1712 {
1713  vpMatrix C;
1714  vpMatrix::sub2Matrices(*this, B, C);
1715  return C;
1716 }
1717 
1719 
1721 {
1722  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1723  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1724  B.getRows(), B.getCols()));
1725  }
1726 
1727  double **BrowPtrs = B.rowPtrs;
1728 
1729  for (unsigned int i = 0; i < rowNum; i++)
1730  for (unsigned int j = 0; j < colNum; j++)
1731  rowPtrs[i][j] += BrowPtrs[i][j];
1732 
1733  return *this;
1734 }
1735 
1738 {
1739  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1740  throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1741  B.getRows(), B.getCols()));
1742  }
1743 
1744  double **BrowPtrs = B.rowPtrs;
1745  for (unsigned int i = 0; i < rowNum; i++)
1746  for (unsigned int j = 0; j < colNum; j++)
1747  rowPtrs[i][j] -= BrowPtrs[i][j];
1748 
1749  return *this;
1750 }
1751 
1762 {
1763  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1764  C.resize(A.rowNum, A.colNum, false, false);
1765 
1766  double **ArowPtrs = A.rowPtrs;
1767  double **CrowPtrs = C.rowPtrs;
1768 
1769  // t0=vpTime::measureTimeMicros();
1770  for (unsigned int i = 0; i < A.rowNum; i++)
1771  for (unsigned int j = 0; j < A.colNum; j++)
1772  CrowPtrs[i][j] = -ArowPtrs[i][j];
1773 }
1774 
1780 {
1781  vpMatrix C;
1782  vpMatrix::negateMatrix(*this, C);
1783  return C;
1784 }
1785 
1786 double vpMatrix::sum() const
1787 {
1788  double s = 0.0;
1789  for (unsigned int i = 0; i < rowNum; i++) {
1790  for (unsigned int j = 0; j < colNum; j++) {
1791  s += rowPtrs[i][j];
1792  }
1793  }
1794 
1795  return s;
1796 }
1797 
1798 //---------------------------------
1799 // Matrix/vector operations.
1800 //---------------------------------
1801 
1802 //---------------------------------
1803 // Matrix/real operations.
1804 //---------------------------------
1805 
1810 vpMatrix operator*(const double &x, const vpMatrix &B)
1811 {
1812  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1813  return B;
1814  }
1815 
1816  unsigned int Brow = B.getRows();
1817  unsigned int Bcol = B.getCols();
1818 
1819  vpMatrix C;
1820  C.resize(Brow, Bcol, false, false);
1821 
1822  for (unsigned int i = 0; i < Brow; i++)
1823  for (unsigned int j = 0; j < Bcol; j++)
1824  C[i][j] = B[i][j] * x;
1825 
1826  return C;
1827 }
1828 
1834 {
1835  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1836  return (*this);
1837  }
1838 
1839  vpMatrix M;
1840  M.resize(rowNum, colNum, false, false);
1841 
1842  for (unsigned int i = 0; i < rowNum; i++)
1843  for (unsigned int j = 0; j < colNum; j++)
1844  M[i][j] = rowPtrs[i][j] * x;
1845 
1846  return M;
1847 }
1848 
1851 {
1852  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1853  return (*this);
1854  }
1855 
1856  if (std::fabs(x) < std::numeric_limits<double>::epsilon()) {
1857  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1858  }
1859 
1860  vpMatrix C;
1861  C.resize(rowNum, colNum, false, false);
1862 
1863  double xinv = 1 / x;
1864 
1865  for (unsigned int i = 0; i < rowNum; i++)
1866  for (unsigned int j = 0; j < colNum; j++)
1867  C[i][j] = rowPtrs[i][j] * xinv;
1868 
1869  return C;
1870 }
1871 
1874 {
1875  for (unsigned int i = 0; i < rowNum; i++)
1876  for (unsigned int j = 0; j < colNum; j++)
1877  rowPtrs[i][j] += x;
1878 
1879  return *this;
1880 }
1881 
1884 {
1885  for (unsigned int i = 0; i < rowNum; i++)
1886  for (unsigned int j = 0; j < colNum; j++)
1887  rowPtrs[i][j] -= x;
1888 
1889  return *this;
1890 }
1891 
1897 {
1898  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1899  return *this;
1900  }
1901 
1902  for (unsigned int i = 0; i < rowNum; i++)
1903  for (unsigned int j = 0; j < colNum; j++)
1904  rowPtrs[i][j] *= x;
1905 
1906  return *this;
1907 }
1908 
1911 {
1912  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1913  return *this;
1914  }
1915 
1916  if (std::fabs(x) < std::numeric_limits<double>::epsilon())
1917  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1918 
1919  double xinv = 1 / x;
1920 
1921  for (unsigned int i = 0; i < rowNum; i++)
1922  for (unsigned int j = 0; j < colNum; j++)
1923  rowPtrs[i][j] *= xinv;
1924 
1925  return *this;
1926 }
1927 
1928 //----------------------------------------------------------------
1929 // Matrix Operation
1930 //----------------------------------------------------------------
1931 
1937 {
1938  if ((out.rowNum != colNum * rowNum) || (out.colNum != 1))
1939  out.resize(colNum * rowNum, false, false);
1940 
1941  double *optr = out.data;
1942  for (unsigned int j = 0; j < colNum; j++) {
1943  for (unsigned int i = 0; i < rowNum; i++) {
1944  *(optr++) = rowPtrs[i][j];
1945  }
1946  }
1947 }
1948 
1954 {
1955  vpColVector out(colNum * rowNum);
1956  stackColumns(out);
1957  return out;
1958 }
1959 
1965 {
1966  if ((out.getRows() != 1) || (out.getCols() != colNum * rowNum))
1967  out.resize(colNum * rowNum, false, false);
1968 
1969  memcpy(out.data, data, sizeof(double)*out.getCols());
1970 }
1976 {
1977  vpRowVector out(colNum * rowNum);
1978  stackRows(out);
1979  return out;
1980 }
1981 
1989 {
1990  if (m.getRows() != rowNum || m.getCols() != colNum) {
1991  throw(vpException(vpException::dimensionError, "In Hadamard product: bad dimension of input matrix"));
1992  }
1993 
1994  vpMatrix out;
1995  out.resize(rowNum, colNum, false, false);
1996 
1997  unsigned int i = 0;
1998 
1999 #if VISP_HAVE_SSE2
2000  if (vpCPUFeatures::checkSSE2() && dsize >= 2) {
2001  for (; i <= dsize - 2; i += 2) {
2002  __m128d vout = _mm_mul_pd(_mm_loadu_pd(data + i), _mm_loadu_pd(m.data + i));
2003  _mm_storeu_pd(out.data + i, vout);
2004  }
2005  }
2006 #endif
2007 
2008  for (; i < dsize; i++) {
2009  out.data[i] = data[i] * m.data[i];
2010  }
2011 
2012  return out;
2013 }
2014 
2021 void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out)
2022 {
2023  unsigned int r1 = m1.getRows();
2024  unsigned int c1 = m1.getCols();
2025  unsigned int r2 = m2.getRows();
2026  unsigned int c2 = m2.getCols();
2027 
2028  out.resize(r1*r2, c1*c2, false, false);
2029 
2030  for (unsigned int r = 0; r < r1; r++) {
2031  for (unsigned int c = 0; c < c1; c++) {
2032  double alpha = m1[r][c];
2033  double *m2ptr = m2[0];
2034  unsigned int roffset = r * r2;
2035  unsigned int coffset = c * c2;
2036  for (unsigned int rr = 0; rr < r2; rr++) {
2037  for (unsigned int cc = 0; cc < c2; cc++) {
2038  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
2039  }
2040  }
2041  }
2042  }
2043 }
2044 
2051 void vpMatrix::kron(const vpMatrix &m, vpMatrix &out) const { kron(*this, m, out); }
2052 
2060 {
2061  unsigned int r1 = m1.getRows();
2062  unsigned int c1 = m1.getCols();
2063  unsigned int r2 = m2.getRows();
2064  unsigned int c2 = m2.getCols();
2065 
2066  vpMatrix out;
2067  out.resize(r1 * r2, c1 * c2, false, false);
2068 
2069  for (unsigned int r = 0; r < r1; r++) {
2070  for (unsigned int c = 0; c < c1; c++) {
2071  double alpha = m1[r][c];
2072  double *m2ptr = m2[0];
2073  unsigned int roffset = r * r2;
2074  unsigned int coffset = c * c2;
2075  for (unsigned int rr = 0; rr < r2; rr++) {
2076  for (unsigned int cc = 0; cc < c2; cc++) {
2077  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
2078  }
2079  }
2080  }
2081  }
2082  return out;
2083 }
2084 
2090 vpMatrix vpMatrix::kron(const vpMatrix &m) const { return kron(*this, m); }
2091 
2142 void vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const { x = pseudoInverse(1e-6) * b; }
2143 
2194 {
2195  vpColVector X(colNum);
2196 
2197  solveBySVD(B, X);
2198  return X;
2199 }
2200 
2265 {
2266 #if defined(VISP_HAVE_LAPACK)
2267  svdLapack(w, V);
2268 #elif defined(VISP_HAVE_EIGEN3)
2269  svdEigen3(w, V);
2270 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2271  svdOpenCV(w, V);
2272 #else
2273  (void)w;
2274  (void)V;
2275  throw(vpException(vpException::fatalError, "Cannot compute SVD. Install Lapack, Eigen3 or OpenCV 3rd party"));
2276 #endif
2277 }
2278 
2333 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
2334 {
2335 #if defined(VISP_HAVE_LAPACK)
2336  return pseudoInverseLapack(Ap, svThreshold);
2337 #elif defined(VISP_HAVE_EIGEN3)
2338  return pseudoInverseEigen3(Ap, svThreshold);
2339 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2340  return pseudoInverseOpenCV(Ap, svThreshold);
2341 #else
2342  (void)Ap;
2343  (void)svThreshold;
2344  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2345  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2346 #endif
2347 }
2348 
2399 vpMatrix vpMatrix::pseudoInverse(double svThreshold) const
2400 {
2401 #if defined(VISP_HAVE_LAPACK)
2402  return pseudoInverseLapack(svThreshold);
2403 #elif defined(VISP_HAVE_EIGEN3)
2404  return pseudoInverseEigen3(svThreshold);
2405 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2406  return pseudoInverseOpenCV(svThreshold);
2407 #else
2408  (void)svThreshold;
2409  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2410  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2411 #endif
2412 }
2413 
2414 #ifndef DOXYGEN_SHOULD_SKIP_THIS
2415 #if defined(VISP_HAVE_LAPACK)
2416 
2452 vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const
2453 {
2454  unsigned int nrows, ncols;
2455  unsigned int nrows_orig = getRows();
2456  unsigned int ncols_orig = getCols();
2457 
2458  vpMatrix Ap;
2459  Ap.resize(ncols_orig, nrows_orig, false, false);
2460 
2461  if (nrows_orig >= ncols_orig) {
2462  nrows = nrows_orig;
2463  ncols = ncols_orig;
2464  } else {
2465  nrows = ncols_orig;
2466  ncols = nrows_orig;
2467  }
2468 
2469  vpMatrix U, V;
2470  U.resize(nrows, ncols, false, false);
2471  V.resize(ncols, ncols, false, false);
2472  vpColVector sv;
2473  sv.resize(ncols, false);
2474 
2475  if (nrows_orig >= ncols_orig)
2476  U = *this;
2477  else
2478  U = (*this).t();
2479 
2480  U.svdLapack(sv, V);
2481 
2482  unsigned int rank;
2483  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2484 
2485  return Ap;
2486 }
2487 
2528 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) const
2529 {
2530  unsigned int nrows, ncols;
2531  unsigned int nrows_orig = getRows();
2532  unsigned int ncols_orig = getCols();
2533  unsigned int rank;
2534 
2535  Ap.resize(ncols_orig, nrows_orig, false, false);
2536 
2537  if (nrows_orig >= ncols_orig) {
2538  nrows = nrows_orig;
2539  ncols = ncols_orig;
2540  } else {
2541  nrows = ncols_orig;
2542  ncols = nrows_orig;
2543  }
2544 
2545  vpMatrix U, V;
2546  U.resize(nrows, ncols, false, false);
2547  V.resize(ncols, ncols, false, false);
2548  vpColVector sv;
2549  sv.resize(ncols, true);
2550 
2551  if (nrows_orig >= ncols_orig)
2552  U = *this;
2553  else
2554  U = (*this).t();
2555 
2556  U.svdLapack(sv, V);
2557 
2558  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2559 
2560  return rank;
2561 }
2609 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2610 {
2611  unsigned int nrows, ncols;
2612  unsigned int nrows_orig = getRows();
2613  unsigned int ncols_orig = getCols();
2614  unsigned int rank;
2615 
2616  Ap.resize(ncols_orig, nrows_orig, false, false);
2617 
2618  if (nrows_orig >= ncols_orig) {
2619  nrows = nrows_orig;
2620  ncols = ncols_orig;
2621  } else {
2622  nrows = ncols_orig;
2623  ncols = nrows_orig;
2624  }
2625 
2626  vpMatrix U, V;
2627  U.resize(nrows, ncols, false, false);
2628  V.resize(ncols, ncols, false, false);
2629  sv.resize(ncols, false);
2630 
2631  if (nrows_orig >= ncols_orig)
2632  U = *this;
2633  else
2634  U = (*this).t();
2635 
2636  U.svdLapack(sv, V);
2637 
2638  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2639 
2640  return rank;
2641 }
2642 
2750 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
2751  vpMatrix &imAt, vpMatrix &kerA) const
2752 {
2753  unsigned int nrows = getRows();
2754  unsigned int ncols = getCols();
2755  unsigned int rank;
2756  vpMatrix U, V;
2757  vpColVector sv_;
2758 
2759  if (nrows < ncols) {
2760  U.resize(ncols, ncols, true);
2761  sv.resize(nrows, false);
2762  } else {
2763  U.resize(nrows, ncols, false);
2764  sv.resize(ncols, false);
2765  }
2766 
2767  U.insert(*this, 0, 0);
2768  U.svdLapack(sv_, V);
2769 
2770  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);
2771 
2772  // Remove singular values equal to to that correspond to the lines of 0
2773  // introduced when m < n
2774  for (unsigned int i = 0; i < sv.size(); i++)
2775  sv[i] = sv_[i];
2776 
2777  return rank;
2778 }
2779 #endif
2780 #if defined(VISP_HAVE_EIGEN3)
2781 
2817 vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const
2818 {
2819  unsigned int nrows, ncols;
2820  unsigned int nrows_orig = getRows();
2821  unsigned int ncols_orig = getCols();
2822 
2823  vpMatrix Ap;
2824  Ap.resize(ncols_orig, nrows_orig, false);
2825 
2826  if (nrows_orig >= ncols_orig) {
2827  nrows = nrows_orig;
2828  ncols = ncols_orig;
2829  } else {
2830  nrows = ncols_orig;
2831  ncols = nrows_orig;
2832  }
2833 
2834  vpMatrix U, V;
2835  U.resize(nrows, ncols, false);
2836  V.resize(ncols, ncols, false);
2837  vpColVector sv;
2838  sv.resize(ncols, false);
2839 
2840  if (nrows_orig >= ncols_orig)
2841  U = *this;
2842  else
2843  U = (*this).t();
2844 
2845  U.svdEigen3(sv, V);
2846 
2847  unsigned int rank;
2848  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2849 
2850  return Ap;
2851 }
2852 
2893 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) const
2894 {
2895  unsigned int nrows, ncols;
2896  unsigned int nrows_orig = getRows();
2897  unsigned int ncols_orig = getCols();
2898  unsigned int rank;
2899 
2900  Ap.resize(ncols_orig, nrows_orig, false);
2901 
2902  if (nrows_orig >= ncols_orig) {
2903  nrows = nrows_orig;
2904  ncols = ncols_orig;
2905  } else {
2906  nrows = ncols_orig;
2907  ncols = nrows_orig;
2908  }
2909 
2910  vpMatrix U, V;
2911  U.resize(nrows, ncols, false);
2912  V.resize(ncols, ncols, false);
2913  vpColVector sv;
2914  sv.resize(ncols, false);
2915 
2916  if (nrows_orig >= ncols_orig)
2917  U = *this;
2918  else
2919  U = (*this).t();
2920 
2921  U.svdEigen3(sv, V);
2922 
2923  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2924 
2925  return rank;
2926 }
2974 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2975 {
2976  unsigned int nrows, ncols;
2977  unsigned int nrows_orig = getRows();
2978  unsigned int ncols_orig = getCols();
2979  unsigned int rank;
2980 
2981  Ap.resize(ncols_orig, nrows_orig, false);
2982 
2983  if (nrows_orig >= ncols_orig) {
2984  nrows = nrows_orig;
2985  ncols = ncols_orig;
2986  } else {
2987  nrows = ncols_orig;
2988  ncols = nrows_orig;
2989  }
2990 
2991  vpMatrix U, V;
2992  U.resize(nrows, ncols, false);
2993  V.resize(ncols, ncols, false);
2994  sv.resize(ncols, false);
2995 
2996  if (nrows_orig >= ncols_orig)
2997  U = *this;
2998  else
2999  U = (*this).t();
3000 
3001  U.svdEigen3(sv, V);
3002 
3003  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
3004 
3005  return rank;
3006 }
3007 
3115 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3116  vpMatrix &imAt, vpMatrix &kerA) const
3117 {
3118  unsigned int nrows = getRows();
3119  unsigned int ncols = getCols();
3120  unsigned int rank;
3121  vpMatrix U, V;
3122  vpColVector sv_;
3123 
3124  if (nrows < ncols) {
3125  U.resize(ncols, ncols, true);
3126  sv.resize(nrows, false);
3127  } else {
3128  U.resize(nrows, ncols, false);
3129  sv.resize(ncols, false);
3130  }
3131 
3132  U.insert(*this, 0, 0);
3133  U.svdEigen3(sv_, V);
3134 
3135  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);
3136 
3137  // Remove singular values equal to to that correspond to the lines of 0
3138  // introduced when m < n
3139  for (unsigned int i = 0; i < sv.size(); i++)
3140  sv[i] = sv_[i];
3141 
3142  return rank;
3143 }
3144 #endif
3145 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
3146 
3182 vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const
3183 {
3184  unsigned int nrows, ncols;
3185  unsigned int nrows_orig = getRows();
3186  unsigned int ncols_orig = getCols();
3187 
3188  vpMatrix Ap;
3189  Ap.resize(ncols_orig, nrows_orig, false);
3190 
3191  if (nrows_orig >= ncols_orig) {
3192  nrows = nrows_orig;
3193  ncols = ncols_orig;
3194  } else {
3195  nrows = ncols_orig;
3196  ncols = nrows_orig;
3197  }
3198 
3199  vpMatrix U, V;
3200  U.resize(nrows, ncols, false);
3201  V.resize(ncols, ncols, false);
3202  vpColVector sv;
3203  sv.resize(ncols, false);
3204 
3205  if (nrows_orig >= ncols_orig)
3206  U = *this;
3207  else
3208  U = (*this).t();
3209 
3210  U.svdOpenCV(sv, V);
3211 
3212  unsigned int rank;
3213  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
3214 
3215  return Ap;
3216 }
3217 
3258 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) const
3259 {
3260  unsigned int nrows, ncols;
3261  unsigned int nrows_orig = getRows();
3262  unsigned int ncols_orig = getCols();
3263  unsigned int rank;
3264 
3265  Ap.resize(ncols_orig, nrows_orig, false);
3266 
3267  if (nrows_orig >= ncols_orig) {
3268  nrows = nrows_orig;
3269  ncols = ncols_orig;
3270  } else {
3271  nrows = ncols_orig;
3272  ncols = nrows_orig;
3273  }
3274 
3275  vpMatrix U, V;
3276  U.resize(nrows, ncols, false);
3277  V.resize(ncols, ncols, false);
3278  vpColVector sv;
3279  sv.resize(ncols, false);
3280 
3281  if (nrows_orig >= ncols_orig)
3282  U = *this;
3283  else
3284  U = (*this).t();
3285 
3286  U.svdOpenCV(sv, V);
3287 
3288  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
3289 
3290  return rank;
3291 }
3339 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3340 {
3341  unsigned int nrows, ncols;
3342  unsigned int nrows_orig = getRows();
3343  unsigned int ncols_orig = getCols();
3344  unsigned int rank;
3345 
3346  Ap.resize(ncols_orig, nrows_orig);
3347 
3348  if (nrows_orig >= ncols_orig) {
3349  nrows = nrows_orig;
3350  ncols = ncols_orig;
3351  } else {
3352  nrows = ncols_orig;
3353  ncols = nrows_orig;
3354  }
3355 
3356  vpMatrix U, V;
3357  U.resize(nrows, ncols, false);
3358  V.resize(ncols, ncols, false);
3359  sv.resize(ncols, false);
3360 
3361  if (nrows_orig >= ncols_orig)
3362  U = *this;
3363  else
3364  U = (*this).t();
3365 
3366  U.svdOpenCV(sv, V);
3367 
3368  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
3369 
3370  return rank;
3371 }
3372 
3480 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3481  vpMatrix &imAt, vpMatrix &kerA) const
3482 {
3483  unsigned int nrows = getRows();
3484  unsigned int ncols = getCols();
3485  unsigned int rank;
3486  vpMatrix U, V;
3487  vpColVector sv_;
3488 
3489  if (nrows < ncols) {
3490  U.resize(ncols, ncols, true);
3491  sv.resize(nrows, false);
3492  } else {
3493  U.resize(nrows, ncols, false);
3494  sv.resize(ncols, false);
3495  }
3496 
3497  U.insert(*this, 0, 0);
3498  U.svdOpenCV(sv_, V);
3499 
3500  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);
3501 
3502  // Remove singular values equal to to that correspond to the lines of 0
3503  // introduced when m < n
3504  for (unsigned int i = 0; i < sv.size(); i++)
3505  sv[i] = sv_[i];
3506 
3507  return rank;
3508 }
3509 #endif
3510 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
3511 
3573 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3574 {
3575 #if defined(VISP_HAVE_LAPACK)
3576  return pseudoInverseLapack(Ap, sv, svThreshold);
3577 #elif defined(VISP_HAVE_EIGEN3)
3578  return pseudoInverseEigen3(Ap, sv, svThreshold);
3579 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
3580  return pseudoInverseOpenCV(Ap, sv, svThreshold);
3581 #else
3582  (void)Ap;
3583  (void)sv;
3584  (void)svThreshold;
3585  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
3586  "Install Lapack, Eigen3 or OpenCV 3rd party"));
3587 #endif
3588 }
3589 
3664 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3665  vpMatrix &imAt) const
3666 {
3667  vpMatrix kerAt;
3668  return pseudoInverse(Ap, sv, svThreshold, imA, imAt, kerAt);
3669 }
3670 
3805 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt,
3806  vpMatrix &kerAt) const
3807 {
3808 #if defined(VISP_HAVE_LAPACK)
3809  return pseudoInverseLapack(Ap, sv, svThreshold, imA, imAt, kerAt);
3810 #elif defined(VISP_HAVE_EIGEN3)
3811  return pseudoInverseEigen3(Ap, sv, svThreshold, imA, imAt, kerAt);
3812 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
3813  return pseudoInverseOpenCV(Ap, sv, svThreshold, imA, imAt, kerAt);
3814 #else
3815  (void)Ap;
3816  (void)sv;
3817  (void)svThreshold;
3818  (void)imA;
3819  (void)imAt;
3820  (void)kerAt;
3821  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
3822  "Install Lapack, Eigen3 or OpenCV 3rd party"));
3823 #endif
3824 }
3825 
3867 vpColVector vpMatrix::getCol(unsigned int j, unsigned int i_begin, unsigned int column_size) const
3868 {
3869  if (i_begin + column_size > getRows() || j >= getCols())
3870  throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(), getCols()));
3871  vpColVector c(column_size);
3872  for (unsigned int i = 0; i < column_size; i++)
3873  c[i] = (*this)[i_begin + i][j];
3874  return c;
3875 }
3876 
3916 vpColVector vpMatrix::getCol(unsigned int j) const
3917 {
3918  return getCol(j, 0, rowNum);
3919 }
3920 
3956 vpRowVector vpMatrix::getRow(unsigned int i) const
3957 {
3958  return getRow(i, 0, colNum);
3959 }
3960 
4000 vpRowVector vpMatrix::getRow(unsigned int i, unsigned int j_begin, unsigned int row_size) const
4001 {
4002  if (j_begin + row_size > colNum || i >= rowNum)
4003  throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
4004 
4005  vpRowVector r(row_size);
4006  if (r.data != NULL && data != NULL) {
4007  memcpy(r.data, (*this)[i] + j_begin, row_size*sizeof(double));
4008  }
4009 
4010  return r;
4011 }
4012 
4049 {
4050  unsigned int min_size = std::min(rowNum, colNum);
4051  vpColVector diag;
4052 
4053  if (min_size > 0) {
4054  diag.resize(min_size, false);
4055 
4056  for (unsigned int i = 0; i < min_size; i++) {
4057  diag[i] = (*this)[i][i];
4058  }
4059  }
4060 
4061  return diag;
4062 }
4063 
4075 {
4076  vpMatrix C;
4077 
4078  vpMatrix::stack(A, B, C);
4079 
4080  return C;
4081 }
4082 
4094 void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
4095 {
4096  unsigned int nra = A.getRows();
4097  unsigned int nrb = B.getRows();
4098 
4099  if (nra != 0) {
4100  if (A.getCols() != B.getCols()) {
4101  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
4102  A.getCols(), B.getRows(), B.getCols()));
4103  }
4104  }
4105 
4106  if (A.data != NULL && A.data == C.data) {
4107  std::cerr << "A and C must be two different objects!" << std::endl;
4108  return;
4109  }
4110 
4111  if (B.data != NULL && B.data == C.data) {
4112  std::cerr << "B and C must be two different objects!" << std::endl;
4113  return;
4114  }
4115 
4116  C.resize(nra + nrb, B.getCols(), false, false);
4117 
4118  if (C.data != NULL && A.data != NULL && A.size() > 0) {
4119  // Copy A in C
4120  memcpy(C.data, A.data, sizeof(double) * A.size());
4121  }
4122 
4123  if (C.data != NULL && B.data != NULL && B.size() > 0) {
4124  // Copy B in C
4125  memcpy(C.data + A.size(), B.data, sizeof(double) * B.size());
4126  }
4127 }
4128 
4139 {
4140  vpMatrix C;
4141  vpMatrix::stack(A, r, C);
4142 
4143  return C;
4144 }
4145 
4157 void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
4158 {
4159  if (A.data != NULL && A.data == C.data) {
4160  std::cerr << "A and C must be two different objects!" << std::endl;
4161  return;
4162  }
4163 
4164  C = A;
4165  C.stack(r);
4166 }
4167 
4178 {
4179  vpMatrix C;
4180  vpMatrix::stack(A, c, C);
4181 
4182  return C;
4183 }
4184 
4196 void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C)
4197 {
4198  if (A.data != NULL && A.data == C.data) {
4199  std::cerr << "A and C must be two different objects!" << std::endl;
4200  return;
4201  }
4202 
4203  C = A;
4204  C.stack(c);
4205 }
4206 
4219 vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, unsigned int r, unsigned int c)
4220 {
4221  vpMatrix C;
4222 
4223  insert(A, B, C, r, c);
4224 
4225  return C;
4226 }
4227 
4241 void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, unsigned int r, unsigned int c)
4242 {
4243  if (((r + B.getRows()) <= A.getRows()) && ((c + B.getCols()) <= A.getCols())) {
4244  C.resize(A.getRows(), A.getCols(), false, false);
4245 
4246  for (unsigned int i = 0; i < A.getRows(); i++) {
4247  for (unsigned int j = 0; j < A.getCols(); j++) {
4248  if (i >= r && i < (r + B.getRows()) && j >= c && j < (c + B.getCols())) {
4249  C[i][j] = B[i - r][j - c];
4250  } else {
4251  C[i][j] = A[i][j];
4252  }
4253  }
4254  }
4255  } else {
4256  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
4257  B.getRows(), B.getCols(), A.getCols(), A.getRows(), r, c);
4258  }
4259 }
4260 
4273 {
4274  vpMatrix C;
4275 
4276  juxtaposeMatrices(A, B, C);
4277 
4278  return C;
4279 }
4280 
4294 {
4295  unsigned int nca = A.getCols();
4296  unsigned int ncb = B.getCols();
4297 
4298  if (nca != 0) {
4299  if (A.getRows() != B.getRows()) {
4300  throw(vpException(vpException::dimensionError, "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
4301  A.getCols(), B.getRows(), B.getCols()));
4302  }
4303  }
4304 
4305  if (B.getRows() == 0 || nca + ncb == 0) {
4306  std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl;
4307  return;
4308  }
4309 
4310  C.resize(B.getRows(), nca + ncb, false, false);
4311 
4312  C.insert(A, 0, 0);
4313  C.insert(B, 0, nca);
4314 }
4315 
4316 //--------------------------------------------------------------------
4317 // Output
4318 //--------------------------------------------------------------------
4319 
4339 int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &intro) const
4340 {
4341  typedef std::string::size_type size_type;
4342 
4343  unsigned int m = getRows();
4344  unsigned int n = getCols();
4345 
4346  std::vector<std::string> values(m * n);
4347  std::ostringstream oss;
4348  std::ostringstream ossFixed;
4349  std::ios_base::fmtflags original_flags = oss.flags();
4350 
4351  // ossFixed <<std::fixed;
4352  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
4353 
4354  size_type maxBefore = 0; // the length of the integral part
4355  size_type maxAfter = 0; // number of decimals plus
4356  // one place for the decimal point
4357  for (unsigned int i = 0; i < m; ++i) {
4358  for (unsigned int j = 0; j < n; ++j) {
4359  oss.str("");
4360  oss << (*this)[i][j];
4361  if (oss.str().find("e") != std::string::npos) {
4362  ossFixed.str("");
4363  ossFixed << (*this)[i][j];
4364  oss.str(ossFixed.str());
4365  }
4366 
4367  values[i * n + j] = oss.str();
4368  size_type thislen = values[i * n + j].size();
4369  size_type p = values[i * n + j].find('.');
4370 
4371  if (p == std::string::npos) {
4372  maxBefore = vpMath::maximum(maxBefore, thislen);
4373  // maxAfter remains the same
4374  } else {
4375  maxBefore = vpMath::maximum(maxBefore, p);
4376  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
4377  }
4378  }
4379  }
4380 
4381  size_type totalLength = length;
4382  // increase totalLength according to maxBefore
4383  totalLength = vpMath::maximum(totalLength, maxBefore);
4384  // decrease maxAfter according to totalLength
4385  maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
4386  if (maxAfter == 1)
4387  maxAfter = 0;
4388 
4389  // the following line is useful for debugging
4390  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
4391 
4392  if (! intro.empty())
4393  s << intro;
4394  s << "[" << m << "," << n << "]=\n";
4395 
4396  for (unsigned int i = 0; i < m; i++) {
4397  s << " ";
4398  for (unsigned int j = 0; j < n; j++) {
4399  size_type p = values[i * n + j].find('.');
4400  s.setf(std::ios::right, std::ios::adjustfield);
4401  s.width((std::streamsize)maxBefore);
4402  s << values[i * n + j].substr(0, p).c_str();
4403 
4404  if (maxAfter > 0) {
4405  s.setf(std::ios::left, std::ios::adjustfield);
4406  if (p != std::string::npos) {
4407  s.width((std::streamsize)maxAfter);
4408  s << values[i * n + j].substr(p, maxAfter).c_str();
4409  } else {
4410  assert(maxAfter > 1);
4411  s.width((std::streamsize)maxAfter);
4412  s << ".0";
4413  }
4414  }
4415 
4416  s << ' ';
4417  }
4418  s << std::endl;
4419  }
4420 
4421  s.flags(original_flags); // restore s to standard state
4422 
4423  return (int)(maxBefore + maxAfter);
4424 }
4425 
4462 std::ostream &vpMatrix::matlabPrint(std::ostream &os) const
4463 {
4464  os << "[ ";
4465  for (unsigned int i = 0; i < this->getRows(); ++i) {
4466  for (unsigned int j = 0; j < this->getCols(); ++j) {
4467  os << (*this)[i][j] << ", ";
4468  }
4469  if (this->getRows() != i + 1) {
4470  os << ";" << std::endl;
4471  } else {
4472  os << "]" << std::endl;
4473  }
4474  }
4475  return os;
4476 }
4477 
4506 std::ostream &vpMatrix::maplePrint(std::ostream &os) const
4507 {
4508  os << "([ " << std::endl;
4509  for (unsigned int i = 0; i < this->getRows(); ++i) {
4510  os << "[";
4511  for (unsigned int j = 0; j < this->getCols(); ++j) {
4512  os << (*this)[i][j] << ", ";
4513  }
4514  os << "]," << std::endl;
4515  }
4516  os << "])" << std::endl;
4517  return os;
4518 }
4519 
4547 std::ostream &vpMatrix::csvPrint(std::ostream &os) const
4548 {
4549  for (unsigned int i = 0; i < this->getRows(); ++i) {
4550  for (unsigned int j = 0; j < this->getCols(); ++j) {
4551  os << (*this)[i][j];
4552  if (!(j == (this->getCols() - 1)))
4553  os << ", ";
4554  }
4555  os << std::endl;
4556  }
4557  return os;
4558 }
4559 
4596 std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
4597 {
4598  os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " << this->getCols() << "); " << std::endl;
4599 
4600  for (unsigned int i = 0; i < this->getRows(); ++i) {
4601  for (unsigned int j = 0; j < this->getCols(); ++j) {
4602  if (!octet) {
4603  os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl;
4604  } else {
4605  for (unsigned int k = 0; k < sizeof(double); ++k) {
4606  os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex
4607  << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl;
4608  }
4609  }
4610  }
4611  os << std::endl;
4612  }
4613  return os;
4614 }
4615 
4621 {
4622  if (rowNum == 0) {
4623  *this = A;
4624  } else if (A.getRows() > 0) {
4625  if (colNum != A.getCols()) {
4626  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum,
4627  A.getRows(), A.getCols()));
4628  }
4629 
4630  unsigned int rowNumOld = rowNum;
4631  resize(rowNum + A.getRows(), colNum, false, false);
4632  insert(A, rowNumOld, 0);
4633  }
4634 }
4635 
4652 {
4653  if (rowNum == 0) {
4654  *this = r;
4655  } else {
4656  if (colNum != r.getCols()) {
4657  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum,
4658  colNum, r.getCols()));
4659  }
4660 
4661  if (r.size() == 0) {
4662  return;
4663  }
4664 
4665  unsigned int oldSize = size();
4666  resize(rowNum + 1, colNum, false, false);
4667 
4668  if (data != NULL && r.data != NULL && data != r.data) {
4669  // Copy r in data
4670  memcpy(data + oldSize, r.data, sizeof(double) * r.size());
4671  }
4672  }
4673 }
4674 
4692 {
4693  if (colNum == 0) {
4694  *this = c;
4695  } else {
4696  if (rowNum != c.getRows()) {
4697  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum,
4698  colNum, c.getRows()));
4699  }
4700 
4701  if (c.size() == 0) {
4702  return;
4703  }
4704 
4705  vpMatrix tmp = *this;
4706  unsigned int oldColNum = colNum;
4707  resize(rowNum, colNum + 1, false, false);
4708 
4709  if (data != NULL && tmp.data != NULL && data != tmp.data) {
4710  // Copy c in data
4711  for (unsigned int i = 0; i < rowNum; i++) {
4712  memcpy(data + i*colNum, tmp.data + i*oldColNum, sizeof(double) * oldColNum);
4713  rowPtrs[i][oldColNum] = c[i];
4714  }
4715  }
4716  }
4717 }
4718 
4729 void vpMatrix::insert(const vpMatrix &A, unsigned int r, unsigned int c)
4730 {
4731  if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) {
4732  if (A.colNum == colNum && data != NULL && A.data != NULL && A.data != data) {
4733  memcpy(data + r * colNum, A.data, sizeof(double) * A.size());
4734  } else if (data != NULL && A.data != NULL && A.data != data) {
4735  for (unsigned int i = r; i < (r + A.getRows()); i++) {
4736  memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum);
4737  }
4738  }
4739  } else {
4740  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
4741  A.getRows(), A.getCols(), rowNum, colNum, r, c);
4742  }
4743 }
4744 
4782 {
4783  vpColVector evalue(rowNum); // Eigen values
4784 
4785  if (rowNum != colNum) {
4787  "Cannot compute eigen values on a non square matrix (%dx%d)",
4788  rowNum, colNum));
4789  }
4790 
4791  // Check if the matrix is symetric: At - A = 0
4792  vpMatrix At_A = (*this).t() - (*this);
4793  for (unsigned int i = 0; i < rowNum; i++) {
4794  for (unsigned int j = 0; j < rowNum; j++) {
4795  // if (At_A[i][j] != 0) {
4796  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
4797  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symetric matrix"));
4798  }
4799  }
4800  }
4801 
4802 #if defined(VISP_HAVE_LAPACK)
4803 #if defined(VISP_HAVE_GSL) /* be careful of the copy below */
4804  {
4805  gsl_vector *eval = gsl_vector_alloc(rowNum);
4806  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
4807 
4808  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
4809  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
4810 
4811  unsigned int Atda = (unsigned int)m->tda;
4812  for (unsigned int i = 0; i < rowNum; i++) {
4813  unsigned int k = i * Atda;
4814  for (unsigned int j = 0; j < colNum; j++)
4815  m->data[k + j] = (*this)[i][j];
4816  }
4817  gsl_eigen_symmv(m, eval, evec, w);
4818 
4819  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
4820 
4821  for (unsigned int i = 0; i < rowNum; i++) {
4822  evalue[i] = gsl_vector_get(eval, i);
4823  }
4824 
4825  gsl_eigen_symmv_free(w);
4826  gsl_vector_free(eval);
4827  gsl_matrix_free(m);
4828  gsl_matrix_free(evec);
4829  }
4830 #else
4831  {
4832  const char jobz = 'N';
4833  const char uplo = 'U';
4834  vpMatrix A = (*this);
4835  vpColVector WORK;
4836  int lwork = -1;
4837  int info;
4838  double wkopt;
4839  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
4840  lwork = static_cast<int>(wkopt);
4841  WORK.resize(lwork);
4842  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
4843  }
4844 #endif
4845 #else
4846  {
4848  "Eigen values computation is not implemented. "
4849  "You should install Lapack 3rd party"));
4850  }
4851 #endif
4852  return evalue;
4853 }
4854 
4905 void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const
4906 {
4907  if (rowNum != colNum) {
4909  "Cannot compute eigen values on a non square matrix (%dx%d)",
4910  rowNum, colNum));
4911  }
4912 
4913  // Check if the matrix is symetric: At - A = 0
4914  vpMatrix At_A = (*this).t() - (*this);
4915  for (unsigned int i = 0; i < rowNum; i++) {
4916  for (unsigned int j = 0; j < rowNum; j++) {
4917  // if (At_A[i][j] != 0) {
4918  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
4919  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symetric matrix"));
4920  }
4921  }
4922  }
4923 
4924  // Resize the output matrices
4925  evalue.resize(rowNum);
4926  evector.resize(rowNum, colNum);
4927 
4928 #if defined(VISP_HAVE_LAPACK)
4929 #if defined(VISP_HAVE_GSL) /* be careful of the copy below */
4930  {
4931  gsl_vector *eval = gsl_vector_alloc(rowNum);
4932  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
4933 
4934  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
4935  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
4936 
4937  unsigned int Atda = (unsigned int)m->tda;
4938  for (unsigned int i = 0; i < rowNum; i++) {
4939  unsigned int k = i * Atda;
4940  for (unsigned int j = 0; j < colNum; j++)
4941  m->data[k + j] = (*this)[i][j];
4942  }
4943  gsl_eigen_symmv(m, eval, evec, w);
4944 
4945  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
4946 
4947  for (unsigned int i = 0; i < rowNum; i++) {
4948  evalue[i] = gsl_vector_get(eval, i);
4949  }
4950  Atda = (unsigned int)evec->tda;
4951  for (unsigned int i = 0; i < rowNum; i++) {
4952  unsigned int k = i * Atda;
4953  for (unsigned int j = 0; j < rowNum; j++) {
4954  evector[i][j] = evec->data[k + j];
4955  }
4956  }
4957 
4958  gsl_eigen_symmv_free(w);
4959  gsl_vector_free(eval);
4960  gsl_matrix_free(m);
4961  gsl_matrix_free(evec);
4962  }
4963 #else // defined(VISP_HAVE_GSL)
4964  {
4965  const char jobz = 'V';
4966  const char uplo = 'U';
4967  vpMatrix A = (*this);
4968  vpColVector WORK;
4969  int lwork = -1;
4970  int info;
4971  double wkopt;
4972  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
4973  lwork = static_cast<int>(wkopt);
4974  WORK.resize(lwork);
4975  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
4976  evector = A.t();
4977  }
4978 #endif // defined(VISP_HAVE_GSL)
4979 #else
4980  {
4982  "Eigen values computation is not implemented. "
4983  "You should install Lapack 3rd party"));
4984  }
4985 #endif
4986 }
4987 
5007 unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const
5008 {
5009  unsigned int nbline = getRows();
5010  unsigned int nbcol = getCols();
5011 
5012  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
5013  vpColVector sv;
5014  sv.resize(nbcol, false); // singular values
5015  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
5016 
5017  // Copy and resize matrix to have at least as many rows as columns
5018  // kernel is computed in svd method only if the matrix has more rows than
5019  // columns
5020 
5021  if (nbline < nbcol)
5022  U.resize(nbcol, nbcol, true);
5023  else
5024  U.resize(nbline, nbcol, false);
5025 
5026  U.insert(*this, 0, 0);
5027 
5028  U.svd(sv, V);
5029 
5030  // Compute the highest singular value and rank of the matrix
5031  double maxsv = 0;
5032  for (unsigned int i = 0; i < nbcol; i++) {
5033  if (fabs(sv[i]) > maxsv) {
5034  maxsv = fabs(sv[i]);
5035  }
5036  }
5037 
5038  unsigned int rank = 0;
5039  for (unsigned int i = 0; i < nbcol; i++) {
5040  if (fabs(sv[i]) > maxsv * svThreshold) {
5041  rank++;
5042  }
5043  }
5044 
5045  kerAt.resize(nbcol - rank, nbcol);
5046  if (rank != nbcol) {
5047  for (unsigned int j = 0, k = 0; j < nbcol; j++) {
5048  // if( v.col(j) in kernel and non zero )
5049  if ((fabs(sv[j]) <= maxsv * svThreshold) &&
5050  (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
5051  for (unsigned int i = 0; i < V.getRows(); i++) {
5052  kerAt[k][i] = V[i][j];
5053  }
5054  k++;
5055  }
5056  }
5057  }
5058 
5059  return rank;
5060 }
5061 
5093 double vpMatrix::det(vpDetMethod method) const
5094 {
5095  double det = 0.;
5096 
5097  if (method == LU_DECOMPOSITION) {
5098  det = this->detByLU();
5099  }
5100 
5101  return (det);
5102 }
5103 
5112 {
5113  if (colNum != rowNum) {
5114  throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix",
5115  rowNum, colNum));
5116  } else {
5117 #ifdef VISP_HAVE_GSL
5118  size_t size_ = rowNum * colNum;
5119  double *b = new double[size_];
5120  for (size_t i = 0; i < size_; i++)
5121  b[i] = 0.;
5122  gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum);
5123  gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum);
5124  gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
5125  // gsl_matrix_fprintf(stdout, &em.matrix, "%g");
5126  vpMatrix expA;
5127  expA.resize(rowNum, colNum, false);
5128  memcpy(expA.data, b, size_ * sizeof(double));
5129 
5130  delete[] b;
5131  return expA;
5132 #else
5133  vpMatrix _expE(rowNum, colNum, false);
5134  vpMatrix _expD(rowNum, colNum, false);
5135  vpMatrix _expX(rowNum, colNum, false);
5136  vpMatrix _expcX(rowNum, colNum, false);
5137  vpMatrix _eye(rowNum, colNum, false);
5138 
5139  _eye.eye();
5140  vpMatrix exp(*this);
5141 
5142  // double f;
5143  int e;
5144  double c = 0.5;
5145  int q = 6;
5146  int p = 1;
5147 
5148  double nA = 0;
5149  for (unsigned int i = 0; i < rowNum; i++) {
5150  double sum = 0;
5151  for (unsigned int j = 0; j < colNum; j++) {
5152  sum += fabs((*this)[i][j]);
5153  }
5154  if (sum > nA || i == 0) {
5155  nA = sum;
5156  }
5157  }
5158 
5159  /* f = */ frexp(nA, &e);
5160  // double s = (0 > e+1)?0:e+1;
5161  double s = e + 1;
5162 
5163  double sca = 1.0 / pow(2.0, s);
5164  exp = sca * exp;
5165  _expX = *this;
5166  _expE = c * exp + _eye;
5167  _expD = -c * exp + _eye;
5168  for (int k = 2; k <= q; k++) {
5169  c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1)));
5170  _expcX = exp * _expX;
5171  _expX = _expcX;
5172  _expcX = c * _expX;
5173  _expE = _expE + _expcX;
5174  if (p)
5175  _expD = _expD + _expcX;
5176  else
5177  _expD = _expD - _expcX;
5178  p = !p;
5179  }
5180  _expX = _expD.inverseByLU();
5181  exp = _expX * _expE;
5182  for (int k = 1; k <= s; k++) {
5183  _expE = exp * exp;
5184  exp = _expE;
5185  }
5186  return exp;
5187 #endif
5188  }
5189 }
5190 
5191 /**************************************************************************************************************/
5192 /**************************************************************************************************************/
5193 
5194 // Specific functions
5195 
5196 /*
5197 input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.
5198 
5199 output:: the complement matrix of the element (rowNo,colNo).
5200 This is the matrix obtained from M after elimenating the row rowNo and column
5201 colNo
5202 
5203 example:
5204 1 2 3
5205 M = 4 5 6
5206 7 8 9
5207 1 3
5208 subblock(M, 1, 1) give the matrix 7 9
5209 */
5210 vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row)
5211 {
5212  vpMatrix M_comp;
5213  M_comp.resize(M.getRows() - 1, M.getCols() - 1, false);
5214 
5215  for (unsigned int i = 0; i < col; i++) {
5216  for (unsigned int j = 0; j < row; j++)
5217  M_comp[i][j] = M[i][j];
5218  for (unsigned int j = row + 1; j < M.getRows(); j++)
5219  M_comp[i][j - 1] = M[i][j];
5220  }
5221  for (unsigned int i = col + 1; i < M.getCols(); i++) {
5222  for (unsigned int j = 0; j < row; j++)
5223  M_comp[i - 1][j] = M[i][j];
5224  for (unsigned int j = row + 1; j < M.getRows(); j++)
5225  M_comp[i - 1][j - 1] = M[i][j];
5226  }
5227  return M_comp;
5228 }
5229 
5239 double vpMatrix::cond(double svThreshold) const
5240 {
5241  unsigned int nbline = getRows();
5242  unsigned int nbcol = getCols();
5243 
5244  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
5245  vpColVector sv;
5246  sv.resize(nbcol); // singular values
5247  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
5248 
5249  // Copy and resize matrix to have at least as many rows as columns
5250  // kernel is computed in svd method only if the matrix has more rows than
5251  // columns
5252 
5253  if (nbline < nbcol)
5254  U.resize(nbcol, nbcol, true);
5255  else
5256  U.resize(nbline, nbcol, false);
5257 
5258  U.insert(*this, 0, 0);
5259 
5260  U.svd(sv, V);
5261 
5262  // Compute the highest singular value
5263  double maxsv = 0;
5264  for (unsigned int i = 0; i < nbcol; i++) {
5265  if (fabs(sv[i]) > maxsv) {
5266  maxsv = fabs(sv[i]);
5267  }
5268  }
5269 
5270  // Compute the rank of the matrix
5271  unsigned int rank = 0;
5272  for (unsigned int i = 0; i < nbcol; i++) {
5273  if (fabs(sv[i]) > maxsv * svThreshold) {
5274  rank++;
5275  }
5276  }
5277 
5278  // Compute the lowest singular value
5279  double minsv = maxsv;
5280  for (unsigned int i = 0; i < rank; i++) {
5281  if (fabs(sv[i]) < minsv) {
5282  minsv = fabs(sv[i]);
5283  }
5284  }
5285 
5286  if (std::fabs(minsv) > std::numeric_limits<double>::epsilon()) {
5287  return maxsv / minsv;
5288  }
5289  else {
5290  return std::numeric_limits<double>::infinity();
5291  }
5292 }
5293 
5300 void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
5301 {
5302  if (H.getCols() != H.getRows()) {
5303  throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(),
5304  H.getCols()));
5305  }
5306 
5307  HLM = H;
5308  for (unsigned int i = 0; i < H.getCols(); i++) {
5309  HLM[i][i] += alpha * H[i][i];
5310  }
5311 }
5312 
5322 vp_deprecated double vpMatrix::euclideanNorm() const
5323 {
5324  return frobeniusNorm();
5325 }
5326 
5335 {
5336  double norm = 0.0;
5337  for (unsigned int i = 0; i < dsize; i++) {
5338  double x = *(data + i);
5339  norm += x * x;
5340  }
5341 
5342  return sqrt(norm);
5343 }
5344 
5354 {
5355  if(this->dsize != 0){
5356  vpMatrix v;
5357  vpColVector w;
5358 
5359  vpMatrix M = *this;
5360 
5361  M.svd(w, v);
5362 
5363  double max = w[0];
5364  unsigned int maxRank = std::min(this->getCols(), this->getRows());
5365  // The maximum reachable rank is either the number of columns or the number of rows
5366  // of the matrix.
5367  unsigned int boundary = std::min(maxRank, w.size());
5368  // boundary is here to ensure that the number of singular values used for the com-
5369  // putation of the euclidean norm of the matrix is not greater than the maximum
5370  // reachable rank. Indeed, some svd library pad the singular values vector with 0s
5371  // if the input matrix is non-square.
5372  for (unsigned int i = 0; i < boundary; i++) {
5373  if (max < w[i]) {
5374  max = w[i];
5375  }
5376  }
5377  return max;
5378  }
5379  else {
5380  return 0.;
5381  }
5382 }
5383 
5395 {
5396  double norm = 0.0;
5397  for (unsigned int i = 0; i < rowNum; i++) {
5398  double x = 0;
5399  for (unsigned int j = 0; j < colNum; j++) {
5400  x += fabs(*(*(rowPtrs + i) + j));
5401  }
5402  if (x > norm) {
5403  norm = x;
5404  }
5405  }
5406  return norm;
5407 }
5408 
5415 double vpMatrix::sumSquare() const
5416 {
5417  double sum_square = 0.0;
5418  double x;
5419 
5420  for (unsigned int i = 0; i < rowNum; i++) {
5421  for (unsigned int j = 0; j < colNum; j++) {
5422  x = rowPtrs[i][j];
5423  sum_square += x * x;
5424  }
5425  }
5426 
5427  return sum_square;
5428 }
5429 
5441 vpMatrix vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode)
5442 {
5443  vpMatrix res;
5444  conv2(M, kernel, res, mode);
5445  return res;
5446 }
5447 
5460 void vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, vpMatrix &res, const std::string &mode)
5461 {
5462  if (M.getRows()*M.getCols() == 0 || kernel.getRows()*kernel.getCols() == 0)
5463  return;
5464 
5465  if (mode == "valid") {
5466  if (kernel.getRows() > M.getRows() || kernel.getCols() > M.getCols())
5467  return;
5468  }
5469 
5470  vpMatrix M_padded, res_same;
5471 
5472  if (mode == "full" || mode == "same") {
5473  const unsigned int pad_x = kernel.getCols()-1;
5474  const unsigned int pad_y = kernel.getRows()-1;
5475  M_padded.resize(M.getRows() + 2*pad_y, M.getCols() + 2*pad_x, true, false);
5476  M_padded.insert(M, pad_y, pad_x);
5477 
5478  if (mode == "same") {
5479  res.resize(M.getRows(), M.getCols(), false, false);
5480  res_same.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
5481  } else {
5482  res.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
5483  }
5484  } else if (mode == "valid") {
5485  M_padded = M;
5486  res.resize(M.getRows()-kernel.getRows()+1, M.getCols()-kernel.getCols()+1);
5487  } else {
5488  return;
5489  }
5490 
5491  if (mode == "same") {
5492  for (unsigned int i = 0; i < res_same.getRows(); i++) {
5493  for (unsigned int j = 0; j < res_same.getCols(); j++) {
5494  for (unsigned int k = 0; k < kernel.getRows(); k++) {
5495  for (unsigned int l = 0; l < kernel.getCols(); l++) {
5496  res_same[i][j] += M_padded[i+k][j+l] * kernel[kernel.getRows()-k-1][kernel.getCols()-l-1];
5497  }
5498  }
5499  }
5500  }
5501 
5502  const unsigned int start_i = kernel.getRows()/2;
5503  const unsigned int start_j = kernel.getCols()/2;
5504  for (unsigned int i = 0; i < M.getRows(); i++) {
5505  memcpy(res.data + i*M.getCols(), res_same.data + (i+start_i)*res_same.getCols() + start_j, sizeof(double)*M.getCols());
5506  }
5507  } else {
5508  for (unsigned int i = 0; i < res.getRows(); i++) {
5509  for (unsigned int j = 0; j < res.getCols(); j++) {
5510  for (unsigned int k = 0; k < kernel.getRows(); k++) {
5511  for (unsigned int l = 0; l < kernel.getCols(); l++) {
5512  res[i][j] += M_padded[i+k][j+l] * kernel[kernel.getRows()-k-1][kernel.getCols()-l-1];
5513  }
5514  }
5515  }
5516  }
5517  }
5518 }
5519 
5520 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
5522 {
5523  return (vpMatrix)(vpColVector::stack(A, B));
5524 }
5525 
5527 {
5528  vpColVector::stack(A, B, C);
5529 }
5530 
5532 
5533 void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); }
5534 
5554 {
5555  vpRowVector c(getCols());
5556 
5557  for (unsigned int j = 0; j < getCols(); j++)
5558  c[j] = (*this)[i - 1][j];
5559  return c;
5560 }
5561 
5580 {
5581  vpColVector c(getRows());
5582 
5583  for (unsigned int i = 0; i < getRows(); i++)
5584  c[i] = (*this)[i][j - 1];
5585  return c;
5586 }
5587 
5594 void vpMatrix::setIdentity(const double &val)
5595 {
5596  for (unsigned int i = 0; i < rowNum; i++)
5597  for (unsigned int j = 0; j < colNum; j++)
5598  if (i == j)
5599  (*this)[i][j] = val;
5600  else
5601  (*this)[i][j] = 0;
5602 }
5603 
5604 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:2142
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2264
vpMatrix operator*(const double &x, const vpMatrix &B)
Definition: vpMatrix.cpp:1810
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:495
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2399
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:4272
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:305
vpColVector getDiag() const
Definition: vpMatrix.cpp:4048
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:5093
static vpMatrix conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode="full")
Definition: vpMatrix.cpp:5441
void kron(const vpMatrix &m1, vpMatrix &out) const
Definition: vpMatrix.cpp:2051
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpMatrix operator-() const
Definition: vpMatrix.cpp:1779
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:115
std::ostream & csvPrint(std::ostream &os) const
Definition: vpMatrix.cpp:4547
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1573
vpMatrix AtA() const
Definition: vpMatrix.cpp:762
double sum() const
Definition: vpMatrix.cpp:1786
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4620
vp_deprecated vpColVector column(unsigned int j)
Definition: vpMatrix.cpp:5579
vpMatrix inverseByLU() const
vpMatrix & operator/=(double x)
Divide all the element of the matrix by x : Aij = Aij / x.
Definition: vpMatrix.cpp:1910
error that can be emited by ViSP classes.
Definition: vpException.h:71
VISP_EXPORT bool checkAVX()
unsigned int getRows() const
Definition: vpArray2D.h:289
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
vp_deprecated void stackMatrices(const vpMatrix &A)
Definition: vpMatrix.h:774
Implementation of a generic 2D array used as base class for matrices and vectors. ...
Definition: vpArray2D.h:131
static void sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1686
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
vpMatrix pseudoInverseLapack(double svThreshold=1e-6) const
vpMatrix & operator+=(const vpMatrix &B)
Operation A = A + B.
Definition: vpMatrix.cpp:1720
vpMatrix()
Definition: vpMatrix.h:172
vpMatrix operator/(double x) const
Cij = Aij / x (A is unchanged)
Definition: vpMatrix.cpp:1850
double infinityNorm() const
Definition: vpMatrix.cpp:5394
void svdLapack(vpColVector &w, vpMatrix &V)
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:5239
vpMatrix & operator-=(const vpMatrix &B)
Operation A = A - B.
Definition: vpMatrix.cpp:1737
vpMatrix operator+(const vpMatrix &B) const
Definition: vpMatrix.cpp:1631
vpMatrix pseudoInverseEigen3(double svThreshold=1e-6) const
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
Implementation of a rotation matrix and operations on such kind of matrices.
vpMatrix & operator*=(double x)
Multiply all the element of the matrix by x : Aij = Aij * x.
Definition: vpMatrix.cpp:1896
unsigned int getCols() const
Definition: vpArray2D.h:279
std::ostream & maplePrint(std::ostream &os) const
Definition: vpMatrix.cpp:4506
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
double sumSquare() const
Definition: vpMatrix.cpp:5415
vpMatrix hadamard(const vpMatrix &m) const
Definition: vpMatrix.cpp:1988
vpMatrix t() const
Definition: vpMatrix.cpp:552
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:5594
std::ostream & matlabPrint(std::ostream &os) const
Definition: vpMatrix.cpp:4462
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:5007
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:1020
vp_deprecated void init()
Definition: vpMatrix.h:769
static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C)
Definition: vpMatrix.cpp:1544
VISP_EXPORT bool checkSSE2()
vpMatrix pseudoInverseOpenCV(double svThreshold=1e-6) const
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:3956
static void multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
Definition: vpMatrix.cpp:1092
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1142
std::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
Definition: vpMatrix.cpp:4596
vpColVector eigenValues() const
Definition: vpMatrix.cpp:4781
double euclideanNorm() const
Definition: vpMatrix.cpp:5322
void svdOpenCV(vpColVector &w, vpMatrix &V)
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:4339
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:3916
vpMatrix AAt() const
Definition: vpMatrix.cpp:640
vpMatrix transpose() const
Definition: vpMatrix.cpp:562
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
double inducedL2Norm() const
Definition: vpMatrix.cpp:5353
vpMatrix operator*(const vpMatrix &B) const
Definition: vpMatrix.cpp:1301
vp_deprecated vpRowVector row(unsigned int i)
Definition: vpMatrix.cpp:5553
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
vpRowVector stackRows()
Definition: vpMatrix.cpp:1975
double sumSquare() const
static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
Definition: vpMatrix.cpp:1039
vpColVector stackColumns()
Definition: vpMatrix.cpp:1953
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:141
double detByLU() const
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:4729
static void negateMatrix(const vpMatrix &A, vpMatrix &C)
Definition: vpMatrix.cpp:1761
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:5300
vpMatrix & operator,(double val)
Definition: vpMatrix.cpp:938
double frobeniusNorm() const
Definition: vpMatrix.cpp:5334
Function not implemented.
Definition: vpException.h:90
void resize(unsigned int i, bool flagNullify=true)
Definition: vpRowVector.h:271
Class that consider the case of a translation vector.
void eye()
Definition: vpMatrix.cpp:537
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:139
void svdEigen3(vpColVector &w, vpMatrix &V)
vpMatrix expm() const
Definition: vpMatrix.cpp:5111
vpMatrix & operator=(const vpArray2D< double > &A)
Definition: vpMatrix.cpp:787
vpMatrix & operator<<(double *)
Definition: vpMatrix.cpp:921