Visual Servoing Platform  version 3.2.1 under development (2019-11-19)
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 
57 #ifdef VISP_HAVE_GSL
58 #include <gsl/gsl_linalg.h>
59 #endif
60 
61 #include <visp3/core/vpCPUFeatures.h>
62 #include <visp3/core/vpColVector.h>
63 #include <visp3/core/vpDebug.h>
64 #include <visp3/core/vpException.h>
65 #include <visp3/core/vpMath.h>
66 #include <visp3/core/vpMatrix.h>
67 #include <visp3/core/vpTranslationVector.h>
68 
69 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
70 #include <emmintrin.h>
71 #define VISP_HAVE_SSE2 1
72 #endif
73 
74 #if defined __AVX__
75 #include <immintrin.h>
76 #define VISP_HAVE_AVX 1
77 #endif
78 
79 #if VISP_HAVE_AVX
80 namespace {
81  void transpose4x4(const vpMatrix& a, vpMatrix& b, unsigned int i, unsigned int j)
82  {
83  __m256d a0 = _mm256_loadu_pd(&a[i][j]);
84  __m256d a1 = _mm256_loadu_pd(&a[i + 1][j]);
85  __m256d a2 = _mm256_loadu_pd(&a[i + 2][j]);
86  __m256d a3 = _mm256_loadu_pd(&a[i + 3][j]);
87 
88  __m256d T0 = _mm256_shuffle_pd(a0, a1, 15);
89  __m256d T1 = _mm256_shuffle_pd(a0, a1, 0);
90  __m256d T2 = _mm256_shuffle_pd(a2, a3, 15);
91  __m256d T3 = _mm256_shuffle_pd(a2, a3, 0);
92 
93  a1 = _mm256_permute2f128_pd(T0, T2, 32);
94  a3 = _mm256_permute2f128_pd(T0, T2, 49);
95  a0 = _mm256_permute2f128_pd(T1, T3, 32);
96  a2 = _mm256_permute2f128_pd(T1, T3, 49);
97 
98  _mm256_storeu_pd(&b[j][i], a0);
99  _mm256_storeu_pd(&b[j + 1][i], a1);
100  _mm256_storeu_pd(&b[j + 2][i], a2);
101  _mm256_storeu_pd(&b[j + 3][i], a3);
102  }
103 
104  void transpose16x16(const vpMatrix& a, vpMatrix& b, unsigned int i, unsigned int j)
105  {
106  transpose4x4(a, b, i, j);
107  transpose4x4(a, b, i, j + 4);
108  transpose4x4(a, b, i, j + 8);
109  transpose4x4(a, b, i, j + 12);
110 
111  transpose4x4(a, b, i + 4, j);
112  transpose4x4(a, b, i + 4, j + 4);
113  transpose4x4(a, b, i + 4, j + 8);
114  transpose4x4(a, b, i + 4, j + 12);
115 
116  transpose4x4(a, b, i + 8, j);
117  transpose4x4(a, b, i + 8, j + 4);
118  transpose4x4(a, b, i + 8, j + 8);
119  transpose4x4(a, b, i + 8, j + 12);
120 
121  transpose4x4(a, b, i + 12, j);
122  transpose4x4(a, b, i + 12, j + 4);
123  transpose4x4(a, b, i + 12, j + 8);
124  transpose4x4(a, b, i + 12, j + 12);
125  }
126 }
127 #endif
128 
129 // Prototypes of specific functions
130 vpMatrix subblock(const vpMatrix &, unsigned int, unsigned int);
131 
132 void compute_pseudo_inverse(const vpMatrix &a, const vpColVector &sv, const vpMatrix &v, unsigned int nrows,
133  unsigned int ncols, unsigned int nrows_orig, unsigned int ncols_orig, double svThreshold,
134  vpMatrix &Ap, unsigned int &rank)
135 {
136  vpMatrix a1;
137  a1.resize(ncols, nrows, false, false);
138 
139  // compute the highest singular value and the rank of h
140  double maxsv = 0;
141  for (unsigned int i = 0; i < ncols; i++) {
142  if (fabs(sv[i]) > maxsv)
143  maxsv = fabs(sv[i]);
144  }
145 
146  rank = 0;
147 
148  for (unsigned int i = 0; i < ncols; i++) {
149  if (fabs(sv[i]) > maxsv * svThreshold) {
150  rank++;
151  }
152 
153  for (unsigned int j = 0; j < nrows; j++) {
154  a1[i][j] = 0.0;
155 
156  for (unsigned int k = 0; k < ncols; k++) {
157  if (fabs(sv[k]) > maxsv * svThreshold) {
158  a1[i][j] += v[i][k] * a[j][k] / sv[k];
159  }
160  }
161  }
162  }
163  if (nrows_orig >= ncols_orig)
164  Ap = a1;
165  else
166  Ap = a1.t();
167 }
168 
169 void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows_orig,
170  unsigned int ncols_orig, double svThreshold, vpMatrix &Ap, unsigned int &rank,
171  vpMatrix &imA, vpMatrix &imAt, vpMatrix &kerAt)
172 {
173  Ap.resize(ncols_orig, nrows_orig, true);
174 
175  // compute the highest singular value and the rank of h
176  double maxsv = fabs(sv[0]);
177 
178  rank = 0;
179 
180  for (unsigned int i = 0; i < ncols_orig; i++) {
181  if (fabs(sv[i]) > maxsv * svThreshold) {
182  rank++;
183  }
184 
185  for (unsigned int j = 0; j < nrows_orig; j++) {
186  // Ap[i][j] = 0.0;
187 
188  for (unsigned int k = 0; k < ncols_orig; k++) {
189  if (fabs(sv[k]) > maxsv * svThreshold) {
190  Ap[i][j] += V[i][k] * U[j][k] / sv[k];
191  }
192  }
193  }
194  }
195 
196  // Compute im(A) and im(At)
197  imA.resize(nrows_orig, rank);
198  imAt.resize(ncols_orig, rank);
199 
200  for (unsigned int i = 0; i < nrows_orig; i++) {
201  for (unsigned int j = 0; j < rank; j++) {
202  imA[i][j] = U[i][j];
203  }
204  }
205 
206  for (unsigned int i = 0; i < ncols_orig; i++) {
207  for (unsigned int j = 0; j < rank; j++) {
208  imAt[i][j] = V[i][j];
209  }
210  }
211 
212  kerAt.resize(ncols_orig - rank, ncols_orig);
213  if (rank != ncols_orig) {
214  for (unsigned int j = 0, k = 0; j < ncols_orig; j++) {
215  // if( v.col(j) in kernel and non zero )
216  if ((fabs(sv[j]) <= maxsv * svThreshold) &&
217  (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
218  for (unsigned int i = 0; i < V.getRows(); i++) {
219  kerAt[k][i] = V[i][j];
220  }
221  k++;
222  }
223  }
224  }
225 }
226 
232 vpMatrix::vpMatrix(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
233  : vpArray2D<double>()
234 {
235  if (((r + nrows) > M.rowNum) || ((c + ncols) > M.colNum)) {
237  "Cannot construct a sub matrix (%dx%d) starting at "
238  "position (%d,%d) that is not contained in the "
239  "original matrix (%dx%d)",
240  nrows, ncols, r, c, M.rowNum, M.colNum));
241  }
242 
243  init(M, r, c, nrows, ncols);
244 }
245 
246 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
248 {
249  rowNum = A.rowNum;
250  colNum = A.colNum;
251  rowPtrs = A.rowPtrs;
252  dsize = A.dsize;
253  data = A.data;
254 
255  A.rowNum = 0;
256  A.colNum = 0;
257  A.rowPtrs = NULL;
258  A.dsize = 0;
259  A.data = NULL;
260 }
261 
287 vpMatrix::vpMatrix(const std::initializer_list<double> &list) : vpArray2D<double>(list) { }
288 
289 
314 vpMatrix::vpMatrix(unsigned int nrows, unsigned int ncols, const std::initializer_list<double> &list)
315  : vpArray2D<double>(nrows, ncols, list) {}
316 
339 vpMatrix::vpMatrix(const std::initializer_list<std::initializer_list<double> > &lists) : vpArray2D<double>(lists) { }
340 
341 #endif
342 
389 void vpMatrix::init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
390 {
391  unsigned int rnrows = r + nrows;
392  unsigned int cncols = c + ncols;
393 
394  if (rnrows > M.getRows())
395  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
396  M.getRows()));
397  if (cncols > M.getCols())
398  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
399  M.getCols()));
400  resize(nrows, ncols, false, false);
401 
402  if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
403  return; // Noting to do
404  for (unsigned int i = 0; i < nrows; i++) {
405  memcpy((*this)[i], &M[i + r][c], ncols * sizeof(double));
406  }
407 }
408 
450 vpMatrix vpMatrix::extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
451 {
452  unsigned int rnrows = r + nrows;
453  unsigned int cncols = c + ncols;
454 
455  if (rnrows > getRows())
456  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
457  getRows()));
458  if (cncols > getCols())
459  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
460  getCols()));
461 
462  vpMatrix M;
463  M.resize(nrows, ncols, false, false);
464  for (unsigned int i = 0; i < nrows; i++) {
465  memcpy(M[i], &(*this)[i + r][c], ncols * sizeof(double));
466  }
467 
468  return M;
469 }
470 
475 void vpMatrix::eye(unsigned int n) { eye(n, n); }
476 
481 void vpMatrix::eye(unsigned int m, unsigned int n)
482 {
483  resize(m, n);
484 
485  eye();
486 }
487 
493 {
494  for (unsigned int i = 0; i < rowNum; i++) {
495  for (unsigned int j = 0; j < colNum; j++) {
496  if (i == j)
497  (*this)[i][j] = 1.0;
498  else
499  (*this)[i][j] = 0;
500  }
501  }
502 }
503 
508 {
509  return transpose();
510 }
511 
518 {
519  vpMatrix At;
520  transpose(At);
521  return At;
522 }
523 
530 {
531  At.resize(colNum, rowNum, false, false);
532 
533  if (rowNum <= 16 || colNum <= 16) {
534  for (unsigned int i = 0; i < rowNum; i++) {
535  for (unsigned int j = 0; j < colNum; j++) {
536  At[j][i] = (*this)[i][j];
537  }
538  }
539  }
540  else {
541  bool haveAVX = vpCPUFeatures::checkAVX();
542 #if !VISP_HAVE_AVX
543  haveAVX = false;
544 #endif
545 
546  if (haveAVX) {
547 #if VISP_HAVE_AVX
548  // matrix transpose using tiling
549  const int nrows = static_cast<int>(rowNum);
550  const int ncols = static_cast<int>(colNum);
551  const int tileSize = 16;
552 
553  for (int i = 0; i < nrows;) {
554  for (; i <= nrows - tileSize; i += tileSize) {
555  int j = 0;
556  for (; j <= ncols - tileSize; j += tileSize) {
557  transpose16x16(*this, At, i, j);
558  }
559 
560  for (int k = i; k < i + tileSize; k++) {
561  for (int l = j; l < ncols; l++) {
562  At[l][k] = (*this)[k][l];
563  }
564  }
565  }
566 
567  for (; i < nrows; i++) {
568  for (int j = 0; j < ncols; j++) {
569  At[j][i] = (*this)[i][j];
570  }
571  }
572  }
573 
574  _mm256_zeroupper();
575 #endif
576  } else {
577  // https://stackoverflow.com/a/21548079
578  const int tileSize = 32;
579  for (unsigned int i = 0; i < rowNum; i += tileSize) {
580  for (unsigned int j = 0; j < colNum; j++) {
581  for (unsigned int b = 0; b < static_cast<unsigned int>(tileSize) && i + b < rowNum; b++) {
582  At[j][i + b] = (*this)[i + b][j];
583  }
584  }
585  }
586  }
587  }
588 }
589 
596 {
597  vpMatrix B;
598 
599  AAt(B);
600 
601  return B;
602 }
603 
615 void vpMatrix::AAt(vpMatrix &B) const
616 {
617  if ((B.rowNum != rowNum) || (B.colNum != rowNum))
618  B.resize(rowNum, rowNum, false, false);
619 
620  // compute A*A^T
621  for (unsigned int i = 0; i < rowNum; i++) {
622  for (unsigned int j = i; j < rowNum; j++) {
623  double *pi = rowPtrs[i]; // row i
624  double *pj = rowPtrs[j]; // row j
625 
626  // sum (row i .* row j)
627  double ssum = 0;
628  for (unsigned int k = 0; k < colNum; k++)
629  ssum += *(pi++) * *(pj++);
630 
631  B[i][j] = ssum; // upper triangle
632  if (i != j)
633  B[j][i] = ssum; // lower triangle
634  }
635  }
636 }
637 
649 void vpMatrix::AtA(vpMatrix &B) const
650 {
651  if ((B.rowNum != colNum) || (B.colNum != colNum))
652  B.resize(colNum, colNum, false, false);
653 
654 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
655  double alpha = 1.0;
656  double beta = 0.0;
657  char transa = 'n';
658  char transb = 't';
659 
660  vpMatrix::blas_dgemm(transa, transb, colNum, colNum, rowNum, alpha, data, colNum, data, colNum, beta, B.data, colNum);
661 #else
662  unsigned int i, j, k;
663  double s;
664  double *ptr;
665  for (i = 0; i < colNum; i++) {
666  double *Bi = B[i];
667  for (j = 0; j < i; j++) {
668  ptr = data;
669  s = 0;
670  for (k = 0; k < rowNum; k++) {
671  s += (*(ptr + i)) * (*(ptr + j));
672  ptr += colNum;
673  }
674  *Bi++ = s;
675  B[j][i] = s;
676  }
677  ptr = data;
678  s = 0;
679  for (k = 0; k < rowNum; k++) {
680  s += (*(ptr + i)) * (*(ptr + i));
681  ptr += colNum;
682  }
683  *Bi = s;
684  }
685 #endif
686 }
687 
694 {
695  vpMatrix B;
696 
697  AtA(B);
698 
699  return B;
700 }
701 
719 {
720  resize(A.getRows(), A.getCols(), false, false);
721 
722  if (data != NULL && A.data != NULL && data != A.data) {
723  memcpy(data, A.data, dsize * sizeof(double));
724  }
725 
726  return *this;
727 }
728 
729 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
731 {
732  resize(A.getRows(), A.getCols(), false, false);
733 
734  if (data != NULL && A.data != NULL && data != A.data) {
735  memcpy(data, A.data, dsize * sizeof(double));
736  }
737 
738  return *this;
739 }
740 
742 {
743  if (this != &other) {
744  free(data);
745  free(rowPtrs);
746 
747  rowNum = other.rowNum;
748  colNum = other.colNum;
749  rowPtrs = other.rowPtrs;
750  dsize = other.dsize;
751  data = other.data;
752 
753  other.rowNum = 0;
754  other.colNum = 0;
755  other.rowPtrs = NULL;
756  other.dsize = 0;
757  other.data = NULL;
758  }
759 
760  return *this;
761 }
762 
787 vpMatrix& vpMatrix::operator=(const std::initializer_list<double> &list)
788 {
789  if (dsize != static_cast<unsigned int>(list.size())) {
790  resize(1, static_cast<unsigned int>(list.size()), false, false);
791  }
792 
793  std::copy(list.begin(), list.end(), data);
794 
795  return *this;
796 }
797 
821 vpMatrix& vpMatrix::operator=(const std::initializer_list<std::initializer_list<double> > &lists)
822 {
823  unsigned int nrows = static_cast<unsigned int>(lists.size()), ncols = 0;
824  for (auto& l : lists) {
825  if (static_cast<unsigned int>(l.size()) > ncols) {
826  ncols = static_cast<unsigned int>(l.size());
827  }
828  }
829 
830  resize(nrows, ncols, false, false);
831  auto it = lists.begin();
832  for (unsigned int i = 0; i < rowNum; i++, ++it) {
833  std::copy(it->begin(), it->end(), rowPtrs[i]);
834  }
835 
836  return *this;
837 }
838 #endif
839 
842 {
843  std::fill(data, data + rowNum*colNum, x);
844  return *this;
845 }
846 
853 {
854  for (unsigned int i = 0; i < rowNum; i++) {
855  for (unsigned int j = 0; j < colNum; j++) {
856  rowPtrs[i][j] = *x++;
857  }
858  }
859  return *this;
860 }
861 
863 {
864  resize(1, 1, false, false);
865  rowPtrs[0][0] = val;
866  return *this;
867 }
868 
870 {
871  resize(1, colNum + 1, false, false);
872  rowPtrs[0][colNum - 1] = val;
873  return *this;
874 }
875 
912 {
913  unsigned int rows = A.getRows();
914  this->resize(rows, rows);
915 
916  (*this) = 0;
917  for (unsigned int i = 0; i < rows; i++)
918  (*this)[i][i] = A[i];
919 }
920 
951 void vpMatrix::diag(const double &val)
952 {
953  (*this) = 0;
954  unsigned int min_ = (rowNum < colNum) ? rowNum : colNum;
955  for (unsigned int i = 0; i < min_; i++)
956  (*this)[i][i] = val;
957 }
958 
971 {
972  unsigned int rows = A.getRows();
973  DA.resize(rows, rows, true);
974 
975  for (unsigned int i = 0; i < rows; i++)
976  DA[i][i] = A[i];
977 }
978 
984 {
985  vpTranslationVector t_out;
986 
987  if (rowNum != 3 || colNum != 3) {
988  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector",
989  rowNum, colNum, tv.getRows(), tv.getCols()));
990  }
991 
992  for (unsigned int j = 0; j < 3; j++)
993  t_out[j] = 0;
994 
995  for (unsigned int j = 0; j < 3; j++) {
996  double tj = tv[j]; // optimization em 5/12/2006
997  for (unsigned int i = 0; i < 3; i++) {
998  t_out[i] += rowPtrs[i][j] * tj;
999  }
1000  }
1001  return t_out;
1002 }
1003 
1009 {
1010  vpColVector v_out;
1011  vpMatrix::multMatrixVector(*this, v, v_out);
1012  return v_out;
1013 }
1014 
1024 {
1025  if (A.colNum != v.getRows()) {
1026  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
1027  A.getRows(), A.getCols(), v.getRows()));
1028  }
1029 
1030  if (A.rowNum != w.rowNum)
1031  w.resize(A.rowNum, false);
1032 
1033 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
1034  double alpha = 1.0;
1035  double beta = 0.0;
1036  char trans = 't';
1037  int incr = 1;
1038 
1039  vpMatrix::blas_dgemv(trans, A.colNum, A.rowNum, alpha, A.data, A.colNum, v.data, incr, beta, w.data, incr);
1040 #else
1041  w = 0.0;
1042  for (unsigned int j = 0; j < A.colNum; j++) {
1043  double vj = v[j]; // optimization em 5/12/2006
1044  for (unsigned int i = 0; i < A.rowNum; i++) {
1045  w[i] += A.rowPtrs[i][j] * vj;
1046  }
1047  }
1048 #endif
1049 }
1050 
1051 //---------------------------------
1052 // Matrix operations.
1053 //---------------------------------
1054 
1065 {
1066  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1067  C.resize(A.rowNum, B.colNum, false, false);
1068 
1069  if (A.colNum != B.rowNum) {
1070  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (%dx%d) matrix", A.getRows(),
1071  A.getCols(), B.getRows(), B.getCols()));
1072  }
1073 
1074 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
1075  double alpha = 1.0;
1076  double beta = 0.0;
1077  char trans = 'n';
1078 
1079  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1080  C.data, B.colNum);
1081 #else
1082  // 5/12/06 some "very" simple optimization to avoid indexation
1083  unsigned int BcolNum = B.colNum;
1084  unsigned int BrowNum = B.rowNum;
1085  unsigned int i, j, k;
1086  double **BrowPtrs = B.rowPtrs;
1087  for (i = 0; i < A.rowNum; i++) {
1088  double *rowptri = A.rowPtrs[i];
1089  double *ci = C[i];
1090  for (j = 0; j < BcolNum; j++) {
1091  double s = 0;
1092  for (k = 0; k < BrowNum; k++)
1093  s += rowptri[k] * BrowPtrs[k][j];
1094  ci[j] = s;
1095  }
1096  }
1097 #endif
1098 }
1099 
1114 {
1115  if (A.colNum != 3 || A.rowNum != 3 || B.colNum != 3 || B.rowNum != 3) {
1117  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1118  "rotation matrix",
1119  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1120  }
1121 
1122  // 5/12/06 some "very" simple optimization to avoid indexation
1123  unsigned int BcolNum = B.colNum;
1124  unsigned int BrowNum = B.rowNum;
1125  unsigned int i, j, k;
1126  double **BrowPtrs = B.rowPtrs;
1127  for (i = 0; i < A.rowNum; i++) {
1128  double *rowptri = A.rowPtrs[i];
1129  double *ci = C[i];
1130  for (j = 0; j < BcolNum; j++) {
1131  double s = 0;
1132  for (k = 0; k < BrowNum; k++)
1133  s += rowptri[k] * BrowPtrs[k][j];
1134  ci[j] = s;
1135  }
1136  }
1137 }
1138 
1153 {
1154  if (A.colNum != 4 || A.rowNum != 4 || B.colNum != 4 || B.rowNum != 4) {
1156  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1157  "rotation matrix",
1158  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1159  }
1160 
1161  // 5/12/06 some "very" simple optimization to avoid indexation
1162  unsigned int BcolNum = B.colNum;
1163  unsigned int BrowNum = B.rowNum;
1164  unsigned int i, j, k;
1165  double **BrowPtrs = B.rowPtrs;
1166  for (i = 0; i < A.rowNum; i++) {
1167  double *rowptri = A.rowPtrs[i];
1168  double *ci = C[i];
1169  for (j = 0; j < BcolNum; j++) {
1170  double s = 0;
1171  for (k = 0; k < BrowNum; k++)
1172  s += rowptri[k] * BrowPtrs[k][j];
1173  ci[j] = s;
1174  }
1175  }
1176 }
1177 
1191 {
1192  vpMatrix::multMatrixVector(A, B, C);
1193 }
1194 
1200 {
1201  vpMatrix C;
1202 
1203  vpMatrix::mult2Matrices(*this, B, C);
1204 
1205  return C;
1206 }
1207 
1213 {
1214  if (colNum != R.getRows()) {
1215  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1216  colNum));
1217  }
1218  vpMatrix C;
1219  C.resize(rowNum, 3, false, false);
1220 
1221  unsigned int RcolNum = R.getCols();
1222  unsigned int RrowNum = R.getRows();
1223  for (unsigned int i = 0; i < rowNum; i++) {
1224  double *rowptri = rowPtrs[i];
1225  double *ci = C[i];
1226  for (unsigned int j = 0; j < RcolNum; j++) {
1227  double s = 0;
1228  for (unsigned int k = 0; k < RrowNum; k++)
1229  s += rowptri[k] * R[k][j];
1230  ci[j] = s;
1231  }
1232  }
1233 
1234  return C;
1235 }
1241 {
1242  if (colNum != V.getRows()) {
1243  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix",
1244  rowNum, colNum));
1245  }
1246  vpMatrix M;
1247  M.resize(rowNum, 6, false, false);
1248 
1249 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
1250  double alpha = 1.0;
1251  double beta = 0.0;
1252  char trans = 'n';
1253 
1254  vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, V.colNum, data, colNum, beta, M.data,
1255  V.colNum);
1256 #else
1257  bool checkSSE2 = vpCPUFeatures::checkSSE2();
1258 #if !VISP_HAVE_SSE2
1259  checkSSE2 = false;
1260 #endif
1261 
1262  if (checkSSE2) {
1263 #if VISP_HAVE_SSE2
1264  vpMatrix V_trans;
1265  V_trans.resize(6, 6, false, false);
1266  for (unsigned int i = 0; i < 6; i++) {
1267  for (unsigned int j = 0; j < 6; j++) {
1268  V_trans[i][j] = V[j][i];
1269  }
1270  }
1271 
1272  for (unsigned int i = 0; i < rowNum; i++) {
1273  double *rowptri = rowPtrs[i];
1274  double *ci = M[i];
1275 
1276  for (int j = 0; j < 6; j++) {
1277  __m128d v_mul = _mm_setzero_pd();
1278  for (int k = 0; k < 6; k += 2) {
1279  v_mul = _mm_add_pd(v_mul, _mm_mul_pd(_mm_loadu_pd(&rowptri[k]), _mm_loadu_pd(&V_trans[j][k])));
1280  }
1281 
1282  double v_tmp[2];
1283  _mm_storeu_pd(v_tmp, v_mul);
1284  ci[j] = v_tmp[0] + v_tmp[1];
1285  }
1286  }
1287 #endif
1288  } else {
1289  unsigned int VcolNum = V.getCols();
1290  unsigned int VrowNum = V.getRows();
1291  for (unsigned int i = 0; i < rowNum; i++) {
1292  double *rowptri = rowPtrs[i];
1293  double *ci = M[i];
1294  for (unsigned int j = 0; j < VcolNum; j++) {
1295  double s = 0;
1296  for (unsigned int k = 0; k < VrowNum; k++)
1297  s += rowptri[k] * V[k][j];
1298  ci[j] = s;
1299  }
1300  }
1301  }
1302 #endif
1303 
1304  return M;
1305 }
1311 {
1312  if (colNum != V.getRows()) {
1313  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) force/torque twist matrix",
1314  rowNum, colNum));
1315  }
1316  vpMatrix M;
1317  M.resize(rowNum, 6, false, false);
1318 
1319  unsigned int VcolNum = V.getCols();
1320  unsigned int VrowNum = V.getRows();
1321  for (unsigned int i = 0; i < rowNum; i++) {
1322  double *rowptri = rowPtrs[i];
1323  double *ci = M[i];
1324  for (unsigned int j = 0; j < VcolNum; j++) {
1325  double s = 0;
1326  for (unsigned int k = 0; k < VrowNum; k++)
1327  s += rowptri[k] * V[k][j];
1328  ci[j] = s;
1329  }
1330  }
1331 
1332  return M;
1333 }
1334 
1345 void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB,
1346  vpMatrix &C)
1347 {
1348  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1349  C.resize(A.rowNum, B.colNum, false, false);
1350 
1351  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1352  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1353  A.getCols(), B.getRows(), B.getCols()));
1354  }
1355 
1356  double **ArowPtrs = A.rowPtrs;
1357  double **BrowPtrs = B.rowPtrs;
1358  double **CrowPtrs = C.rowPtrs;
1359 
1360  for (unsigned int i = 0; i < A.rowNum; i++)
1361  for (unsigned int j = 0; j < A.colNum; j++)
1362  CrowPtrs[i][j] = wB * BrowPtrs[i][j] + wA * ArowPtrs[i][j];
1363 }
1364 
1374 void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1375 {
1376  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1377  C.resize(A.rowNum, B.colNum, false, false);
1378 
1379  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1380  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1381  A.getCols(), B.getRows(), B.getCols()));
1382  }
1383 
1384  double **ArowPtrs = A.rowPtrs;
1385  double **BrowPtrs = B.rowPtrs;
1386  double **CrowPtrs = C.rowPtrs;
1387 
1388  for (unsigned int i = 0; i < A.rowNum; i++) {
1389  for (unsigned int j = 0; j < A.colNum; j++) {
1390  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1391  }
1392  }
1393 }
1394 
1408 {
1409  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1410  C.resize(A.rowNum);
1411 
1412  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1413  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1414  A.getCols(), B.getRows(), B.getCols()));
1415  }
1416 
1417  double **ArowPtrs = A.rowPtrs;
1418  double **BrowPtrs = B.rowPtrs;
1419  double **CrowPtrs = C.rowPtrs;
1420 
1421  for (unsigned int i = 0; i < A.rowNum; i++) {
1422  for (unsigned int j = 0; j < A.colNum; j++) {
1423  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1424  }
1425  }
1426 }
1427 
1433 {
1434  vpMatrix C;
1435  vpMatrix::add2Matrices(*this, B, C);
1436  return C;
1437 }
1438 
1455 {
1456  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1457  C.resize(A.rowNum);
1458 
1459  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1460  throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1461  A.getCols(), B.getRows(), B.getCols()));
1462  }
1463 
1464  double **ArowPtrs = A.rowPtrs;
1465  double **BrowPtrs = B.rowPtrs;
1466  double **CrowPtrs = C.rowPtrs;
1467 
1468  for (unsigned int i = 0; i < A.rowNum; i++) {
1469  for (unsigned int j = 0; j < A.colNum; j++) {
1470  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1471  }
1472  }
1473 }
1474 
1487 void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1488 {
1489  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1490  C.resize(A.rowNum, A.colNum, false, false);
1491 
1492  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1493  throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1494  A.getCols(), B.getRows(), B.getCols()));
1495  }
1496 
1497  double **ArowPtrs = A.rowPtrs;
1498  double **BrowPtrs = B.rowPtrs;
1499  double **CrowPtrs = C.rowPtrs;
1500 
1501  for (unsigned int i = 0; i < A.rowNum; i++) {
1502  for (unsigned int j = 0; j < A.colNum; j++) {
1503  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1504  }
1505  }
1506 }
1507 
1513 {
1514  vpMatrix C;
1515  vpMatrix::sub2Matrices(*this, B, C);
1516  return C;
1517 }
1518 
1520 
1522 {
1523  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1524  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1525  B.getRows(), B.getCols()));
1526  }
1527 
1528  double **BrowPtrs = B.rowPtrs;
1529 
1530  for (unsigned int i = 0; i < rowNum; i++)
1531  for (unsigned int j = 0; j < colNum; j++)
1532  rowPtrs[i][j] += BrowPtrs[i][j];
1533 
1534  return *this;
1535 }
1536 
1539 {
1540  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1541  throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1542  B.getRows(), B.getCols()));
1543  }
1544 
1545  double **BrowPtrs = B.rowPtrs;
1546  for (unsigned int i = 0; i < rowNum; i++)
1547  for (unsigned int j = 0; j < colNum; j++)
1548  rowPtrs[i][j] -= BrowPtrs[i][j];
1549 
1550  return *this;
1551 }
1552 
1563 {
1564  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1565  C.resize(A.rowNum, A.colNum, false, false);
1566 
1567  double **ArowPtrs = A.rowPtrs;
1568  double **CrowPtrs = C.rowPtrs;
1569 
1570  // t0=vpTime::measureTimeMicros();
1571  for (unsigned int i = 0; i < A.rowNum; i++)
1572  for (unsigned int j = 0; j < A.colNum; j++)
1573  CrowPtrs[i][j] = -ArowPtrs[i][j];
1574 }
1575 
1581 {
1582  vpMatrix C;
1583  vpMatrix::negateMatrix(*this, C);
1584  return C;
1585 }
1586 
1587 double vpMatrix::sum() const
1588 {
1589  double s = 0.0;
1590  for (unsigned int i = 0; i < rowNum; i++) {
1591  for (unsigned int j = 0; j < colNum; j++) {
1592  s += rowPtrs[i][j];
1593  }
1594  }
1595 
1596  return s;
1597 }
1598 
1599 //---------------------------------
1600 // Matrix/vector operations.
1601 //---------------------------------
1602 
1603 //---------------------------------
1604 // Matrix/real operations.
1605 //---------------------------------
1606 
1611 vpMatrix operator*(const double &x, const vpMatrix &B)
1612 {
1613  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1614  return B;
1615  }
1616 
1617  unsigned int Brow = B.getRows();
1618  unsigned int Bcol = B.getCols();
1619 
1620  vpMatrix C;
1621  C.resize(Brow, Bcol, false, false);
1622 
1623  for (unsigned int i = 0; i < Brow; i++)
1624  for (unsigned int j = 0; j < Bcol; j++)
1625  C[i][j] = B[i][j] * x;
1626 
1627  return C;
1628 }
1629 
1635 {
1636  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1637  return (*this);
1638  }
1639 
1640  vpMatrix M;
1641  M.resize(rowNum, colNum, false, false);
1642 
1643  for (unsigned int i = 0; i < rowNum; i++)
1644  for (unsigned int j = 0; j < colNum; j++)
1645  M[i][j] = rowPtrs[i][j] * x;
1646 
1647  return M;
1648 }
1649 
1652 {
1653  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1654  return (*this);
1655  }
1656 
1657  if (std::fabs(x) < std::numeric_limits<double>::epsilon()) {
1658  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1659  }
1660 
1661  vpMatrix C;
1662  C.resize(rowNum, colNum, false, false);
1663 
1664  double xinv = 1 / x;
1665 
1666  for (unsigned int i = 0; i < rowNum; i++)
1667  for (unsigned int j = 0; j < colNum; j++)
1668  C[i][j] = rowPtrs[i][j] * xinv;
1669 
1670  return C;
1671 }
1672 
1675 {
1676  for (unsigned int i = 0; i < rowNum; i++)
1677  for (unsigned int j = 0; j < colNum; j++)
1678  rowPtrs[i][j] += x;
1679 
1680  return *this;
1681 }
1682 
1685 {
1686  for (unsigned int i = 0; i < rowNum; i++)
1687  for (unsigned int j = 0; j < colNum; j++)
1688  rowPtrs[i][j] -= x;
1689 
1690  return *this;
1691 }
1692 
1698 {
1699  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1700  return *this;
1701  }
1702 
1703  for (unsigned int i = 0; i < rowNum; i++)
1704  for (unsigned int j = 0; j < colNum; j++)
1705  rowPtrs[i][j] *= x;
1706 
1707  return *this;
1708 }
1709 
1712 {
1713  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1714  return *this;
1715  }
1716 
1717  if (std::fabs(x) < std::numeric_limits<double>::epsilon())
1718  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1719 
1720  double xinv = 1 / x;
1721 
1722  for (unsigned int i = 0; i < rowNum; i++)
1723  for (unsigned int j = 0; j < colNum; j++)
1724  rowPtrs[i][j] *= xinv;
1725 
1726  return *this;
1727 }
1728 
1729 //----------------------------------------------------------------
1730 // Matrix Operation
1731 //----------------------------------------------------------------
1732 
1738 {
1739  if ((out.rowNum != colNum * rowNum) || (out.colNum != 1))
1740  out.resize(colNum * rowNum, false, false);
1741 
1742  double *optr = out.data;
1743  for (unsigned int j = 0; j < colNum; j++) {
1744  for (unsigned int i = 0; i < rowNum; i++) {
1745  *(optr++) = rowPtrs[i][j];
1746  }
1747  }
1748 }
1749 
1755 {
1756  vpColVector out(colNum * rowNum);
1757  stackColumns(out);
1758  return out;
1759 }
1760 
1766 {
1767  if ((out.getRows() != 1) || (out.getCols() != colNum * rowNum))
1768  out.resize(colNum * rowNum, false, false);
1769 
1770  memcpy(out.data, data, sizeof(double)*out.getCols());
1771 }
1777 {
1778  vpRowVector out(colNum * rowNum);
1779  stackRows(out);
1780  return out;
1781 }
1782 
1790 {
1791  if (m.getRows() != rowNum || m.getCols() != colNum) {
1792  throw(vpException(vpException::dimensionError, "In Hadamard product: bad dimension of input matrix"));
1793  }
1794 
1795  vpMatrix out;
1796  out.resize(rowNum, colNum, false, false);
1797 
1798  unsigned int i = 0;
1799 
1800 #if VISP_HAVE_SSE2
1801  if (vpCPUFeatures::checkSSE2() && dsize >= 2) {
1802  for (; i <= dsize - 2; i += 2) {
1803  __m128d vout = _mm_mul_pd(_mm_loadu_pd(data + i), _mm_loadu_pd(m.data + i));
1804  _mm_storeu_pd(out.data + i, vout);
1805  }
1806  }
1807 #endif
1808 
1809  for (; i < dsize; i++) {
1810  out.data[i] = data[i] * m.data[i];
1811  }
1812 
1813  return out;
1814 }
1815 
1822 void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out)
1823 {
1824  unsigned int r1 = m1.getRows();
1825  unsigned int c1 = m1.getCols();
1826  unsigned int r2 = m2.getRows();
1827  unsigned int c2 = m2.getCols();
1828 
1829  out.resize(r1*r2, c1*c2, false, false);
1830 
1831  for (unsigned int r = 0; r < r1; r++) {
1832  for (unsigned int c = 0; c < c1; c++) {
1833  double alpha = m1[r][c];
1834  double *m2ptr = m2[0];
1835  unsigned int roffset = r * r2;
1836  unsigned int coffset = c * c2;
1837  for (unsigned int rr = 0; rr < r2; rr++) {
1838  for (unsigned int cc = 0; cc < c2; cc++) {
1839  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1840  }
1841  }
1842  }
1843  }
1844 }
1845 
1852 void vpMatrix::kron(const vpMatrix &m, vpMatrix &out) const { kron(*this, m, out); }
1853 
1861 {
1862  unsigned int r1 = m1.getRows();
1863  unsigned int c1 = m1.getCols();
1864  unsigned int r2 = m2.getRows();
1865  unsigned int c2 = m2.getCols();
1866 
1867  vpMatrix out;
1868  out.resize(r1 * r2, c1 * c2, false, false);
1869 
1870  for (unsigned int r = 0; r < r1; r++) {
1871  for (unsigned int c = 0; c < c1; c++) {
1872  double alpha = m1[r][c];
1873  double *m2ptr = m2[0];
1874  unsigned int roffset = r * r2;
1875  unsigned int coffset = c * c2;
1876  for (unsigned int rr = 0; rr < r2; rr++) {
1877  for (unsigned int cc = 0; cc < c2; cc++) {
1878  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1879  }
1880  }
1881  }
1882  }
1883  return out;
1884 }
1885 
1891 vpMatrix vpMatrix::kron(const vpMatrix &m) const { return kron(*this, m); }
1892 
1943 void vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const { x = pseudoInverse(1e-6) * b; }
1944 
1995 {
1996  vpColVector X(colNum);
1997 
1998  solveBySVD(B, X);
1999  return X;
2000 }
2001 
2067 {
2068 #if defined(VISP_HAVE_LAPACK)
2069  svdLapack(w, V);
2070 #elif defined(VISP_HAVE_EIGEN3)
2071  svdEigen3(w, V);
2072 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2073  svdOpenCV(w, V);
2074 #elif defined(VISP_HAVE_GSL)
2075  svdGsl(w, V);
2076 #else
2077  (void)w;
2078  (void)V;
2079  throw(vpException(vpException::fatalError, "Cannot compute SVD. Install Lapack, Eigen3, OpenCV or GSL 3rd party"));
2080 #endif
2081 }
2082 
2137 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
2138 {
2139 #if defined(VISP_HAVE_LAPACK)
2140  return pseudoInverseLapack(Ap, svThreshold);
2141 #elif defined(VISP_HAVE_EIGEN3)
2142  return pseudoInverseEigen3(Ap, svThreshold);
2143 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2144  return pseudoInverseOpenCV(Ap, svThreshold);
2145 #elif defined(VISP_HAVE_GSL)
2146  return pseudoInverseGsl(Ap, svThreshold);
2147 #else
2148  (void)Ap;
2149  (void)svThreshold;
2150  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2151  "Install Lapack, Eigen3, OpenCV "
2152  "or GSL 3rd party"));
2153 #endif
2154 }
2155 
2206 vpMatrix vpMatrix::pseudoInverse(double svThreshold) const
2207 {
2208 #if defined(VISP_HAVE_LAPACK)
2209  return pseudoInverseLapack(svThreshold);
2210 #elif defined(VISP_HAVE_EIGEN3)
2211  return pseudoInverseEigen3(svThreshold);
2212 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2213  return pseudoInverseOpenCV(svThreshold);
2214 #elif defined(VISP_HAVE_GSL)
2215  return pseudoInverseGsl(svThreshold);
2216 #else
2217  (void)svThreshold;
2218  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2219  "Install Lapack, Eigen3, OpenCV "
2220  "or GSL 3rd party"));
2221 #endif
2222 }
2223 
2224 #ifndef DOXYGEN_SHOULD_SKIP_THIS
2225 #if defined(VISP_HAVE_LAPACK)
2226 
2262 vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const
2263 {
2264  unsigned int nrows, ncols;
2265  unsigned int nrows_orig = getRows();
2266  unsigned int ncols_orig = getCols();
2267 
2268  vpMatrix Ap;
2269  Ap.resize(ncols_orig, nrows_orig, false, false);
2270 
2271  if (nrows_orig >= ncols_orig) {
2272  nrows = nrows_orig;
2273  ncols = ncols_orig;
2274  } else {
2275  nrows = ncols_orig;
2276  ncols = nrows_orig;
2277  }
2278 
2279  vpMatrix U, V;
2280  U.resize(nrows, ncols, false, false);
2281  V.resize(ncols, ncols, false, false);
2282  vpColVector sv;
2283  sv.resize(ncols, false);
2284 
2285  if (nrows_orig >= ncols_orig)
2286  U = *this;
2287  else
2288  U = (*this).t();
2289 
2290  U.svdLapack(sv, V);
2291 
2292  unsigned int rank;
2293  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2294 
2295  return Ap;
2296 }
2297 
2338 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) const
2339 {
2340  unsigned int nrows, ncols;
2341  unsigned int nrows_orig = getRows();
2342  unsigned int ncols_orig = getCols();
2343  unsigned int rank;
2344 
2345  Ap.resize(ncols_orig, nrows_orig, false, false);
2346 
2347  if (nrows_orig >= ncols_orig) {
2348  nrows = nrows_orig;
2349  ncols = ncols_orig;
2350  } else {
2351  nrows = ncols_orig;
2352  ncols = nrows_orig;
2353  }
2354 
2355  vpMatrix U, V;
2356  U.resize(nrows, ncols, false, false);
2357  V.resize(ncols, ncols, false, false);
2358  vpColVector sv;
2359  sv.resize(ncols, true);
2360 
2361  if (nrows_orig >= ncols_orig)
2362  U = *this;
2363  else
2364  U = (*this).t();
2365 
2366  U.svdLapack(sv, V);
2367 
2368  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2369 
2370  return rank;
2371 }
2419 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2420 {
2421  unsigned int nrows, ncols;
2422  unsigned int nrows_orig = getRows();
2423  unsigned int ncols_orig = getCols();
2424  unsigned int rank;
2425 
2426  Ap.resize(ncols_orig, nrows_orig, false, false);
2427 
2428  if (nrows_orig >= ncols_orig) {
2429  nrows = nrows_orig;
2430  ncols = ncols_orig;
2431  } else {
2432  nrows = ncols_orig;
2433  ncols = nrows_orig;
2434  }
2435 
2436  vpMatrix U, V;
2437  U.resize(nrows, ncols, false, false);
2438  V.resize(ncols, ncols, false, false);
2439  sv.resize(ncols, false);
2440 
2441  if (nrows_orig >= ncols_orig)
2442  U = *this;
2443  else
2444  U = (*this).t();
2445 
2446  U.svdLapack(sv, V);
2447 
2448  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2449 
2450  return rank;
2451 }
2452 
2560 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
2561  vpMatrix &imAt, vpMatrix &kerA) const
2562 {
2563  unsigned int nrows = getRows();
2564  unsigned int ncols = getCols();
2565  unsigned int rank;
2566  vpMatrix U, V;
2567  vpColVector sv_;
2568 
2569  if (nrows < ncols) {
2570  U.resize(ncols, ncols, true);
2571  sv.resize(nrows, false);
2572  } else {
2573  U.resize(nrows, ncols, false);
2574  sv.resize(ncols, false);
2575  }
2576 
2577  U.insert(*this, 0, 0);
2578  U.svdLapack(sv_, V);
2579 
2580  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);
2581 
2582  // Remove singular values equal to to that correspond to the lines of 0
2583  // introduced when m < n
2584  for (unsigned int i = 0; i < sv.size(); i++)
2585  sv[i] = sv_[i];
2586 
2587  return rank;
2588 }
2589 #endif
2590 #if defined(VISP_HAVE_EIGEN3)
2591 
2627 vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const
2628 {
2629  unsigned int nrows, ncols;
2630  unsigned int nrows_orig = getRows();
2631  unsigned int ncols_orig = getCols();
2632 
2633  vpMatrix Ap;
2634  Ap.resize(ncols_orig, nrows_orig, false);
2635 
2636  if (nrows_orig >= ncols_orig) {
2637  nrows = nrows_orig;
2638  ncols = ncols_orig;
2639  } else {
2640  nrows = ncols_orig;
2641  ncols = nrows_orig;
2642  }
2643 
2644  vpMatrix U, V;
2645  U.resize(nrows, ncols, false);
2646  V.resize(ncols, ncols, false);
2647  vpColVector sv;
2648  sv.resize(ncols, false);
2649 
2650  if (nrows_orig >= ncols_orig)
2651  U = *this;
2652  else
2653  U = (*this).t();
2654 
2655  U.svdEigen3(sv, V);
2656 
2657  unsigned int rank;
2658  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2659 
2660  return Ap;
2661 }
2662 
2703 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) const
2704 {
2705  unsigned int nrows, ncols;
2706  unsigned int nrows_orig = getRows();
2707  unsigned int ncols_orig = getCols();
2708  unsigned int rank;
2709 
2710  Ap.resize(ncols_orig, nrows_orig, false);
2711 
2712  if (nrows_orig >= ncols_orig) {
2713  nrows = nrows_orig;
2714  ncols = ncols_orig;
2715  } else {
2716  nrows = ncols_orig;
2717  ncols = nrows_orig;
2718  }
2719 
2720  vpMatrix U, V;
2721  U.resize(nrows, ncols, false);
2722  V.resize(ncols, ncols, false);
2723  vpColVector sv;
2724  sv.resize(ncols, false);
2725 
2726  if (nrows_orig >= ncols_orig)
2727  U = *this;
2728  else
2729  U = (*this).t();
2730 
2731  U.svdEigen3(sv, V);
2732 
2733  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2734 
2735  return rank;
2736 }
2784 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2785 {
2786  unsigned int nrows, ncols;
2787  unsigned int nrows_orig = getRows();
2788  unsigned int ncols_orig = getCols();
2789  unsigned int rank;
2790 
2791  Ap.resize(ncols_orig, nrows_orig, false);
2792 
2793  if (nrows_orig >= ncols_orig) {
2794  nrows = nrows_orig;
2795  ncols = ncols_orig;
2796  } else {
2797  nrows = ncols_orig;
2798  ncols = nrows_orig;
2799  }
2800 
2801  vpMatrix U, V;
2802  U.resize(nrows, ncols, false);
2803  V.resize(ncols, ncols, false);
2804  sv.resize(ncols, false);
2805 
2806  if (nrows_orig >= ncols_orig)
2807  U = *this;
2808  else
2809  U = (*this).t();
2810 
2811  U.svdEigen3(sv, V);
2812 
2813  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2814 
2815  return rank;
2816 }
2817 
2925 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
2926  vpMatrix &imAt, vpMatrix &kerA) const
2927 {
2928  unsigned int nrows = getRows();
2929  unsigned int ncols = getCols();
2930  unsigned int rank;
2931  vpMatrix U, V;
2932  vpColVector sv_;
2933 
2934  if (nrows < ncols) {
2935  U.resize(ncols, ncols, true);
2936  sv.resize(nrows, false);
2937  } else {
2938  U.resize(nrows, ncols, false);
2939  sv.resize(ncols, false);
2940  }
2941 
2942  U.insert(*this, 0, 0);
2943  U.svdEigen3(sv_, V);
2944 
2945  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);
2946 
2947  // Remove singular values equal to to that correspond to the lines of 0
2948  // introduced when m < n
2949  for (unsigned int i = 0; i < sv.size(); i++)
2950  sv[i] = sv_[i];
2951 
2952  return rank;
2953 }
2954 #endif
2955 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
2956 
2992 vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const
2993 {
2994  unsigned int nrows, ncols;
2995  unsigned int nrows_orig = getRows();
2996  unsigned int ncols_orig = getCols();
2997 
2998  vpMatrix Ap;
2999  Ap.resize(ncols_orig, nrows_orig, false);
3000 
3001  if (nrows_orig >= ncols_orig) {
3002  nrows = nrows_orig;
3003  ncols = ncols_orig;
3004  } else {
3005  nrows = ncols_orig;
3006  ncols = nrows_orig;
3007  }
3008 
3009  vpMatrix U, V;
3010  U.resize(nrows, ncols, false);
3011  V.resize(ncols, ncols, false);
3012  vpColVector sv;
3013  sv.resize(ncols, false);
3014 
3015  if (nrows_orig >= ncols_orig)
3016  U = *this;
3017  else
3018  U = (*this).t();
3019 
3020  U.svdOpenCV(sv, V);
3021 
3022  unsigned int rank;
3023  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
3024 
3025  return Ap;
3026 }
3027 
3068 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) const
3069 {
3070  unsigned int nrows, ncols;
3071  unsigned int nrows_orig = getRows();
3072  unsigned int ncols_orig = getCols();
3073  unsigned int rank;
3074 
3075  Ap.resize(ncols_orig, nrows_orig, false);
3076 
3077  if (nrows_orig >= ncols_orig) {
3078  nrows = nrows_orig;
3079  ncols = ncols_orig;
3080  } else {
3081  nrows = ncols_orig;
3082  ncols = nrows_orig;
3083  }
3084 
3085  vpMatrix U, V;
3086  U.resize(nrows, ncols, false);
3087  V.resize(ncols, ncols, false);
3088  vpColVector sv;
3089  sv.resize(ncols, false);
3090 
3091  if (nrows_orig >= ncols_orig)
3092  U = *this;
3093  else
3094  U = (*this).t();
3095 
3096  U.svdOpenCV(sv, V);
3097 
3098  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
3099 
3100  return rank;
3101 }
3149 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3150 {
3151  unsigned int nrows, ncols;
3152  unsigned int nrows_orig = getRows();
3153  unsigned int ncols_orig = getCols();
3154  unsigned int rank;
3155 
3156  Ap.resize(ncols_orig, nrows_orig);
3157 
3158  if (nrows_orig >= ncols_orig) {
3159  nrows = nrows_orig;
3160  ncols = ncols_orig;
3161  } else {
3162  nrows = ncols_orig;
3163  ncols = nrows_orig;
3164  }
3165 
3166  vpMatrix U, V;
3167  U.resize(nrows, ncols, false);
3168  V.resize(ncols, ncols, false);
3169  sv.resize(ncols, false);
3170 
3171  if (nrows_orig >= ncols_orig)
3172  U = *this;
3173  else
3174  U = (*this).t();
3175 
3176  U.svdOpenCV(sv, V);
3177 
3178  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
3179 
3180  return rank;
3181 }
3182 
3290 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3291  vpMatrix &imAt, vpMatrix &kerA) const
3292 {
3293  unsigned int nrows = getRows();
3294  unsigned int ncols = getCols();
3295  unsigned int rank;
3296  vpMatrix U, V;
3297  vpColVector sv_;
3298 
3299  if (nrows < ncols) {
3300  U.resize(ncols, ncols, true);
3301  sv.resize(nrows, false);
3302  } else {
3303  U.resize(nrows, ncols, false);
3304  sv.resize(ncols, false);
3305  }
3306 
3307  U.insert(*this, 0, 0);
3308  U.svdOpenCV(sv_, V);
3309 
3310  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);
3311 
3312  // Remove singular values equal to to that correspond to the lines of 0
3313  // introduced when m < n
3314  for (unsigned int i = 0; i < sv.size(); i++)
3315  sv[i] = sv_[i];
3316 
3317  return rank;
3318 }
3319 #endif
3320 #if defined(VISP_HAVE_GSL)
3321 
3357 vpMatrix vpMatrix::pseudoInverseGsl(double svThreshold) const
3358 {
3359  unsigned int nrows, ncols;
3360  unsigned int nrows_orig = getRows();
3361  unsigned int ncols_orig = getCols();
3362 
3363  vpMatrix Ap;
3364  Ap.resize(ncols_orig, nrows_orig, false);
3365 
3366  if (nrows_orig >= ncols_orig) {
3367  nrows = nrows_orig;
3368  ncols = ncols_orig;
3369  } else {
3370  nrows = ncols_orig;
3371  ncols = nrows_orig;
3372  }
3373 
3374  vpMatrix U, V;
3375  U.resize(nrows, ncols, false);
3376  V.resize(ncols, ncols, false);
3377  vpColVector sv;
3378  sv.resize(ncols, false);
3379 
3380  if (nrows_orig >= ncols_orig)
3381  U = *this;
3382  else
3383  U = (*this).t();
3384 
3385  U.svdGsl(sv, V);
3386 
3387  unsigned int rank;
3388  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
3389 
3390  return Ap;
3391 }
3392 
3433 unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, double svThreshold) const
3434 {
3435  unsigned int nrows, ncols;
3436  unsigned int nrows_orig = getRows();
3437  unsigned int ncols_orig = getCols();
3438  unsigned int rank;
3439 
3440  Ap.resize(ncols_orig, nrows_orig, false);
3441 
3442  if (nrows_orig >= ncols_orig) {
3443  nrows = nrows_orig;
3444  ncols = ncols_orig;
3445  } else {
3446  nrows = ncols_orig;
3447  ncols = nrows_orig;
3448  }
3449 
3450  vpMatrix U, V;
3451  U.resize(nrows, ncols, false);
3452  V.resize(ncols, ncols, false);
3453  vpColVector sv;
3454  sv.resize(ncols, false);
3455 
3456  if (nrows_orig >= ncols_orig)
3457  U = *this;
3458  else
3459  U = (*this).t();
3460 
3461  U.svdGsl(sv, V);
3462 
3463  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
3464 
3465  return rank;
3466 }
3514 unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3515 {
3516  unsigned int nrows, ncols;
3517  unsigned int nrows_orig = getRows();
3518  unsigned int ncols_orig = getCols();
3519  unsigned int rank;
3520 
3521  Ap.resize(ncols_orig, nrows_orig, false);
3522 
3523  if (nrows_orig >= ncols_orig) {
3524  nrows = nrows_orig;
3525  ncols = ncols_orig;
3526  } else {
3527  nrows = ncols_orig;
3528  ncols = nrows_orig;
3529  }
3530 
3531  vpMatrix U, V;
3532  U.resize(nrows, ncols, false);
3533  V.resize(ncols, ncols, false);
3534  sv.resize(ncols, false);
3535 
3536  if (nrows_orig >= ncols_orig)
3537  U = *this;
3538  else
3539  U = (*this).t();
3540 
3541  U.svdGsl(sv, V);
3542 
3543  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
3544 
3545  return rank;
3546 }
3547 
3654 unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3655  vpMatrix &imAt, vpMatrix &kerA) const
3656 {
3657  unsigned int nrows = getRows();
3658  unsigned int ncols = getCols();
3659  unsigned int rank;
3660  vpMatrix U, V;
3661  vpColVector sv_;
3662 
3663  if (nrows < ncols) {
3664  U.resize(ncols, ncols, true);
3665  sv.resize(nrows, false);
3666  } else {
3667  U.resize(nrows, ncols, false);
3668  sv.resize(ncols, false);
3669  }
3670 
3671  U.insert(*this, 0, 0);
3672  U.svdGsl(sv_, V);
3673 
3674  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);
3675 
3676  // Remove singular values equal to to that correspond to the lines of 0
3677  // introduced when m < n
3678  for (unsigned int i = 0; i < sv.size(); i++)
3679  sv[i] = sv_[i];
3680 
3681  return rank;
3682 }
3683 #endif
3684 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
3685 
3747 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3748 {
3749 #if defined(VISP_HAVE_LAPACK)
3750  return pseudoInverseLapack(Ap, sv, svThreshold);
3751 #elif defined(VISP_HAVE_EIGEN3)
3752  return pseudoInverseEigen3(Ap, sv, svThreshold);
3753 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
3754  return pseudoInverseOpenCV(Ap, sv, svThreshold);
3755 #elif defined(VISP_HAVE_GSL)
3756  return pseudoInverseGsl(Ap, sv, svThreshold);
3757 #else
3758  (void)Ap;
3759  (void)sv;
3760  (void)svThreshold;
3761  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
3762  "Install Lapack, Eigen3, OpenCV "
3763  "or GSL 3rd party"));
3764 #endif
3765 }
3766 
3841 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3842  vpMatrix &imAt) const
3843 {
3844  vpMatrix kerAt;
3845  return pseudoInverse(Ap, sv, svThreshold, imA, imAt, kerAt);
3846 }
3847 
3982 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt,
3983  vpMatrix &kerAt) const
3984 {
3985 #if defined(VISP_HAVE_LAPACK)
3986  return pseudoInverseLapack(Ap, sv, svThreshold, imA, imAt, kerAt);
3987 #elif defined(VISP_HAVE_EIGEN3)
3988  return pseudoInverseEigen3(Ap, sv, svThreshold, imA, imAt, kerAt);
3989 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
3990  return pseudoInverseOpenCV(Ap, sv, svThreshold, imA, imAt, kerAt);
3991 #elif defined(VISP_HAVE_GSL)
3992  return pseudoInverseGsl(Ap, sv, svThreshold, imA, imAt, kerAt);
3993 #else
3994  (void)Ap;
3995  (void)sv;
3996  (void)svThreshold;
3997  (void)imA;
3998  (void)imAt;
3999  (void)kerAt;
4000  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4001  "Install Lapack, Eigen3, OpenCV "
4002  "or GSL 3rd party"));
4003 #endif
4004 }
4005 
4047 vpColVector vpMatrix::getCol(const unsigned int j, const unsigned int i_begin, const unsigned int column_size) const
4048 {
4049  if (i_begin + column_size > getRows() || j >= getCols())
4050  throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(), getCols()));
4051  vpColVector c(column_size);
4052  for (unsigned int i = 0; i < column_size; i++)
4053  c[i] = (*this)[i_begin + i][j];
4054  return c;
4055 }
4056 
4096 vpColVector vpMatrix::getCol(const unsigned int j) const
4097 {
4098  return getCol(j, 0, rowNum);
4099 }
4100 
4136 vpRowVector vpMatrix::getRow(const unsigned int i) const
4137 {
4138  return getRow(i, 0, colNum);
4139 }
4140 
4180 vpRowVector vpMatrix::getRow(const unsigned int i, const unsigned int j_begin, const unsigned int row_size) const
4181 {
4182  if (j_begin + row_size > colNum || i >= rowNum)
4183  throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
4184 
4185  vpRowVector r(row_size);
4186  if (r.data != NULL && data != NULL) {
4187  memcpy(r.data, (*this)[i] + j_begin, row_size*sizeof(double));
4188  }
4189 
4190  return r;
4191 }
4192 
4229 {
4230  unsigned int min_size = std::min(rowNum, colNum);
4231  vpColVector diag;
4232 
4233  if (min_size > 0) {
4234  diag.resize(min_size, false);
4235 
4236  for (unsigned int i = 0; i < min_size; i++) {
4237  diag[i] = (*this)[i][i];
4238  }
4239  }
4240 
4241  return diag;
4242 }
4243 
4255 {
4256  vpMatrix C;
4257 
4258  vpMatrix::stack(A, B, C);
4259 
4260  return C;
4261 }
4262 
4274 void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
4275 {
4276  unsigned int nra = A.getRows();
4277  unsigned int nrb = B.getRows();
4278 
4279  if (nra != 0) {
4280  if (A.getCols() != B.getCols()) {
4281  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
4282  A.getCols(), B.getRows(), B.getCols()));
4283  }
4284  }
4285 
4286  if (A.data != NULL && A.data == C.data) {
4287  std::cerr << "A and C must be two different objects!" << std::endl;
4288  return;
4289  }
4290 
4291  if (B.data != NULL && B.data == C.data) {
4292  std::cerr << "B and C must be two different objects!" << std::endl;
4293  return;
4294  }
4295 
4296  C.resize(nra + nrb, B.getCols(), false, false);
4297 
4298  if (C.data != NULL && A.data != NULL && A.size() > 0) {
4299  // Copy A in C
4300  memcpy(C.data, A.data, sizeof(double) * A.size());
4301  }
4302 
4303  if (C.data != NULL && B.data != NULL && B.size() > 0) {
4304  // Copy B in C
4305  memcpy(C.data + A.size(), B.data, sizeof(double) * B.size());
4306  }
4307 }
4308 
4319 {
4320  vpMatrix C;
4321  vpMatrix::stack(A, r, C);
4322 
4323  return C;
4324 }
4325 
4337 void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
4338 {
4339  if (A.data != NULL && A.data == C.data) {
4340  std::cerr << "A and C must be two different objects!" << std::endl;
4341  return;
4342  }
4343 
4344  C = A;
4345  C.stack(r);
4346 }
4347 
4358 {
4359  vpMatrix C;
4360  vpMatrix::stack(A, c, C);
4361 
4362  return C;
4363 }
4364 
4376 void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C)
4377 {
4378  if (A.data != NULL && A.data == C.data) {
4379  std::cerr << "A and C must be two different objects!" << std::endl;
4380  return;
4381  }
4382 
4383  C = A;
4384  C.stack(c);
4385 }
4386 
4399 vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, const unsigned int r, const unsigned int c)
4400 {
4401  vpMatrix C;
4402 
4403  insert(A, B, C, r, c);
4404 
4405  return C;
4406 }
4407 
4421 void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, const unsigned int r, const unsigned int c)
4422 {
4423  if (((r + B.getRows()) <= A.getRows()) && ((c + B.getCols()) <= A.getCols())) {
4424  C.resize(A.getRows(), A.getCols(), false, false);
4425 
4426  for (unsigned int i = 0; i < A.getRows(); i++) {
4427  for (unsigned int j = 0; j < A.getCols(); j++) {
4428  if (i >= r && i < (r + B.getRows()) && j >= c && j < (c + B.getCols())) {
4429  C[i][j] = B[i - r][j - c];
4430  } else {
4431  C[i][j] = A[i][j];
4432  }
4433  }
4434  }
4435  } else {
4436  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
4437  B.getRows(), B.getCols(), A.getCols(), A.getRows(), r, c);
4438  }
4439 }
4440 
4453 {
4454  vpMatrix C;
4455 
4456  juxtaposeMatrices(A, B, C);
4457 
4458  return C;
4459 }
4460 
4474 {
4475  unsigned int nca = A.getCols();
4476  unsigned int ncb = B.getCols();
4477 
4478  if (nca != 0) {
4479  if (A.getRows() != B.getRows()) {
4480  throw(vpException(vpException::dimensionError, "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
4481  A.getCols(), B.getRows(), B.getCols()));
4482  }
4483  }
4484 
4485  if (B.getRows() == 0 || nca + ncb == 0) {
4486  std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl;
4487  return;
4488  }
4489 
4490  C.resize(B.getRows(), nca + ncb, false, false);
4491 
4492  C.insert(A, 0, 0);
4493  C.insert(B, 0, nca);
4494 }
4495 
4496 //--------------------------------------------------------------------
4497 // Output
4498 //--------------------------------------------------------------------
4499 
4519 int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &intro) const
4520 {
4521  typedef std::string::size_type size_type;
4522 
4523  unsigned int m = getRows();
4524  unsigned int n = getCols();
4525 
4526  std::vector<std::string> values(m * n);
4527  std::ostringstream oss;
4528  std::ostringstream ossFixed;
4529  std::ios_base::fmtflags original_flags = oss.flags();
4530 
4531  // ossFixed <<std::fixed;
4532  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
4533 
4534  size_type maxBefore = 0; // the length of the integral part
4535  size_type maxAfter = 0; // number of decimals plus
4536  // one place for the decimal point
4537  for (unsigned int i = 0; i < m; ++i) {
4538  for (unsigned int j = 0; j < n; ++j) {
4539  oss.str("");
4540  oss << (*this)[i][j];
4541  if (oss.str().find("e") != std::string::npos) {
4542  ossFixed.str("");
4543  ossFixed << (*this)[i][j];
4544  oss.str(ossFixed.str());
4545  }
4546 
4547  values[i * n + j] = oss.str();
4548  size_type thislen = values[i * n + j].size();
4549  size_type p = values[i * n + j].find('.');
4550 
4551  if (p == std::string::npos) {
4552  maxBefore = vpMath::maximum(maxBefore, thislen);
4553  // maxAfter remains the same
4554  } else {
4555  maxBefore = vpMath::maximum(maxBefore, p);
4556  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
4557  }
4558  }
4559  }
4560 
4561  size_type totalLength = length;
4562  // increase totalLength according to maxBefore
4563  totalLength = vpMath::maximum(totalLength, maxBefore);
4564  // decrease maxAfter according to totalLength
4565  maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
4566  if (maxAfter == 1)
4567  maxAfter = 0;
4568 
4569  // the following line is useful for debugging
4570  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
4571 
4572  if (! intro.empty())
4573  s << intro;
4574  s << "[" << m << "," << n << "]=\n";
4575 
4576  for (unsigned int i = 0; i < m; i++) {
4577  s << " ";
4578  for (unsigned int j = 0; j < n; j++) {
4579  size_type p = values[i * n + j].find('.');
4580  s.setf(std::ios::right, std::ios::adjustfield);
4581  s.width((std::streamsize)maxBefore);
4582  s << values[i * n + j].substr(0, p).c_str();
4583 
4584  if (maxAfter > 0) {
4585  s.setf(std::ios::left, std::ios::adjustfield);
4586  if (p != std::string::npos) {
4587  s.width((std::streamsize)maxAfter);
4588  s << values[i * n + j].substr(p, maxAfter).c_str();
4589  } else {
4590  assert(maxAfter > 1);
4591  s.width((std::streamsize)maxAfter);
4592  s << ".0";
4593  }
4594  }
4595 
4596  s << ' ';
4597  }
4598  s << std::endl;
4599  }
4600 
4601  s.flags(original_flags); // restore s to standard state
4602 
4603  return (int)(maxBefore + maxAfter);
4604 }
4605 
4642 std::ostream &vpMatrix::matlabPrint(std::ostream &os) const
4643 {
4644  os << "[ ";
4645  for (unsigned int i = 0; i < this->getRows(); ++i) {
4646  for (unsigned int j = 0; j < this->getCols(); ++j) {
4647  os << (*this)[i][j] << ", ";
4648  }
4649  if (this->getRows() != i + 1) {
4650  os << ";" << std::endl;
4651  } else {
4652  os << "]" << std::endl;
4653  }
4654  }
4655  return os;
4656 }
4657 
4686 std::ostream &vpMatrix::maplePrint(std::ostream &os) const
4687 {
4688  os << "([ " << std::endl;
4689  for (unsigned int i = 0; i < this->getRows(); ++i) {
4690  os << "[";
4691  for (unsigned int j = 0; j < this->getCols(); ++j) {
4692  os << (*this)[i][j] << ", ";
4693  }
4694  os << "]," << std::endl;
4695  }
4696  os << "])" << std::endl;
4697  return os;
4698 }
4699 
4727 std::ostream &vpMatrix::csvPrint(std::ostream &os) const
4728 {
4729  for (unsigned int i = 0; i < this->getRows(); ++i) {
4730  for (unsigned int j = 0; j < this->getCols(); ++j) {
4731  os << (*this)[i][j];
4732  if (!(j == (this->getCols() - 1)))
4733  os << ", ";
4734  }
4735  os << std::endl;
4736  }
4737  return os;
4738 }
4739 
4776 std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
4777 {
4778  os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " << this->getCols() << "); " << std::endl;
4779 
4780  for (unsigned int i = 0; i < this->getRows(); ++i) {
4781  for (unsigned int j = 0; j < this->getCols(); ++j) {
4782  if (!octet) {
4783  os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl;
4784  } else {
4785  for (unsigned int k = 0; k < sizeof(double); ++k) {
4786  os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex
4787  << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl;
4788  }
4789  }
4790  }
4791  os << std::endl;
4792  }
4793  return os;
4794 }
4795 
4801 {
4802  if (rowNum == 0) {
4803  *this = A;
4804  } else if (A.getRows() > 0) {
4805  if (colNum != A.getCols()) {
4806  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum,
4807  A.getRows(), A.getCols()));
4808  }
4809 
4810  unsigned int rowNumOld = rowNum;
4811  resize(rowNum + A.getRows(), colNum, false, false);
4812  insert(A, rowNumOld, 0);
4813  }
4814 }
4815 
4832 {
4833  if (rowNum == 0) {
4834  *this = r;
4835  } else {
4836  if (colNum != r.getCols()) {
4837  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum,
4838  colNum, r.getCols()));
4839  }
4840 
4841  if (r.size() == 0) {
4842  return;
4843  }
4844 
4845  unsigned int oldSize = size();
4846  resize(rowNum + 1, colNum, false, false);
4847 
4848  if (data != NULL && r.data != NULL && data != r.data) {
4849  // Copy r in data
4850  memcpy(data + oldSize, r.data, sizeof(double) * r.size());
4851  }
4852  }
4853 }
4854 
4872 {
4873  if (colNum == 0) {
4874  *this = c;
4875  } else {
4876  if (rowNum != c.getRows()) {
4877  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum,
4878  colNum, c.getRows()));
4879  }
4880 
4881  if (c.size() == 0) {
4882  return;
4883  }
4884 
4885  vpMatrix tmp = *this;
4886  unsigned int oldColNum = colNum;
4887  resize(rowNum, colNum + 1, false, false);
4888 
4889  if (data != NULL && tmp.data != NULL && data != tmp.data) {
4890  // Copy c in data
4891  for (unsigned int i = 0; i < rowNum; i++) {
4892  memcpy(data + i*colNum, tmp.data + i*oldColNum, sizeof(double) * oldColNum);
4893  rowPtrs[i][oldColNum] = c[i];
4894  }
4895  }
4896  }
4897 }
4898 
4909 void vpMatrix::insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
4910 {
4911  if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) {
4912  if (A.colNum == colNum && data != NULL && A.data != NULL && A.data != data) {
4913  memcpy(data + r * colNum, A.data, sizeof(double) * A.size());
4914  } else if (data != NULL && A.data != NULL && A.data != data) {
4915  for (unsigned int i = r; i < (r + A.getRows()); i++) {
4916  memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum);
4917  }
4918  }
4919  } else {
4920  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
4921  A.getRows(), A.getCols(), rowNum, colNum, r, c);
4922  }
4923 }
4924 
4964 {
4965  if (rowNum != colNum) {
4966  throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
4967  colNum));
4968  }
4969 
4970 #ifdef VISP_HAVE_GSL /* be careful of the copy below */
4971  {
4972  // Check if the matrix is symetric: At - A = 0
4973  vpMatrix At_A = (*this).t() - (*this);
4974  for (unsigned int i = 0; i < rowNum; i++) {
4975  for (unsigned int j = 0; j < rowNum; j++) {
4976  // if (At_A[i][j] != 0) {
4977  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
4978  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symetric matrix"));
4979  }
4980  }
4981  }
4982 
4983  vpColVector evalue(rowNum); // Eigen values
4984 
4985  gsl_vector *eval = gsl_vector_alloc(rowNum);
4986  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
4987 
4988  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
4989  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
4990 
4991  unsigned int Atda = (unsigned int)m->tda;
4992  for (unsigned int i = 0; i < rowNum; i++) {
4993  unsigned int k = i * Atda;
4994  for (unsigned int j = 0; j < colNum; j++)
4995  m->data[k + j] = (*this)[i][j];
4996  }
4997  gsl_eigen_symmv(m, eval, evec, w);
4998 
4999  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
5000 
5001  for (unsigned int i = 0; i < rowNum; i++) {
5002  evalue[i] = gsl_vector_get(eval, i);
5003  }
5004 
5005  gsl_eigen_symmv_free(w);
5006  gsl_vector_free(eval);
5007  gsl_matrix_free(m);
5008  gsl_matrix_free(evec);
5009 
5010  return evalue;
5011  }
5012 #else
5013  {
5014  throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. You "
5015  "should install GSL rd party"));
5016  }
5017 #endif
5018 }
5019 
5073 #ifdef VISP_HAVE_GSL /* be careful of the copy below */
5074 void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const
5075 #else
5076 void vpMatrix::eigenValues(vpColVector & /* evalue */, vpMatrix & /* evector */) const
5077 #endif
5078 {
5079  if (rowNum != colNum) {
5080  throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
5081  colNum));
5082  }
5083 
5084 #ifdef VISP_HAVE_GSL /* be careful of the copy below */
5085  {
5086  // Check if the matrix is symetric: At - A = 0
5087  vpMatrix At_A = (*this).t() - (*this);
5088  for (unsigned int i = 0; i < rowNum; i++) {
5089  for (unsigned int j = 0; j < rowNum; j++) {
5090  // if (At_A[i][j] != 0) {
5091  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
5092  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symetric matrix"));
5093  }
5094  }
5095  }
5096 
5097  // Resize the output matrices
5098  evalue.resize(rowNum);
5099  evector.resize(rowNum, colNum);
5100 
5101  gsl_vector *eval = gsl_vector_alloc(rowNum);
5102  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
5103 
5104  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
5105  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
5106 
5107  unsigned int Atda = (unsigned int)m->tda;
5108  for (unsigned int i = 0; i < rowNum; i++) {
5109  unsigned int k = i * Atda;
5110  for (unsigned int j = 0; j < colNum; j++)
5111  m->data[k + j] = (*this)[i][j];
5112  }
5113  gsl_eigen_symmv(m, eval, evec, w);
5114 
5115  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
5116 
5117  for (unsigned int i = 0; i < rowNum; i++) {
5118  evalue[i] = gsl_vector_get(eval, i);
5119  }
5120  Atda = (unsigned int)evec->tda;
5121  for (unsigned int i = 0; i < rowNum; i++) {
5122  unsigned int k = i * Atda;
5123  for (unsigned int j = 0; j < rowNum; j++) {
5124  evector[i][j] = evec->data[k + j];
5125  }
5126  }
5127 
5128  gsl_eigen_symmv_free(w);
5129  gsl_vector_free(eval);
5130  gsl_matrix_free(m);
5131  gsl_matrix_free(evec);
5132  }
5133 #else
5134  {
5135  throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. You "
5136  "should install GSL rd party"));
5137  }
5138 #endif
5139 }
5140 
5160 unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const
5161 {
5162  unsigned int nbline = getRows();
5163  unsigned int nbcol = getCols();
5164 
5165  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
5166  vpColVector sv;
5167  sv.resize(nbcol, false); // singular values
5168  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
5169 
5170  // Copy and resize matrix to have at least as many rows as columns
5171  // kernel is computed in svd method only if the matrix has more rows than
5172  // columns
5173 
5174  if (nbline < nbcol)
5175  U.resize(nbcol, nbcol, true);
5176  else
5177  U.resize(nbline, nbcol, false);
5178 
5179  U.insert(*this, 0, 0);
5180 
5181  U.svd(sv, V);
5182 
5183  // Compute the highest singular value and rank of the matrix
5184  double maxsv = 0;
5185  for (unsigned int i = 0; i < nbcol; i++) {
5186  if (fabs(sv[i]) > maxsv) {
5187  maxsv = fabs(sv[i]);
5188  }
5189  }
5190 
5191  unsigned int rank = 0;
5192  for (unsigned int i = 0; i < nbcol; i++) {
5193  if (fabs(sv[i]) > maxsv * svThreshold) {
5194  rank++;
5195  }
5196  }
5197 
5198  kerAt.resize(nbcol - rank, nbcol);
5199  if (rank != nbcol) {
5200  for (unsigned int j = 0, k = 0; j < nbcol; j++) {
5201  // if( v.col(j) in kernel and non zero )
5202  if ((fabs(sv[j]) <= maxsv * svThreshold) &&
5203  (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
5204  for (unsigned int i = 0; i < V.getRows(); i++) {
5205  kerAt[k][i] = V[i][j];
5206  }
5207  k++;
5208  }
5209  }
5210  }
5211 
5212  return rank;
5213 }
5214 
5246 double vpMatrix::det(vpDetMethod method) const
5247 {
5248  double det = 0.;
5249 
5250  if (method == LU_DECOMPOSITION) {
5251  det = this->detByLU();
5252  }
5253 
5254  return (det);
5255 }
5256 
5265 {
5266  if (colNum != rowNum) {
5267  throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix",
5268  rowNum, colNum));
5269  } else {
5270 #ifdef VISP_HAVE_GSL
5271  size_t size_ = rowNum * colNum;
5272  double *b = new double[size_];
5273  for (size_t i = 0; i < size_; i++)
5274  b[i] = 0.;
5275  gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum);
5276  gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum);
5277  gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
5278  // gsl_matrix_fprintf(stdout, &em.matrix, "%g");
5279  vpMatrix expA;
5280  expA.resize(rowNum, colNum, false);
5281  memcpy(expA.data, b, size_ * sizeof(double));
5282 
5283  delete[] b;
5284  return expA;
5285 #else
5286  vpMatrix _expE(rowNum, colNum, false);
5287  vpMatrix _expD(rowNum, colNum, false);
5288  vpMatrix _expX(rowNum, colNum, false);
5289  vpMatrix _expcX(rowNum, colNum, false);
5290  vpMatrix _eye(rowNum, colNum, false);
5291 
5292  _eye.eye();
5293  vpMatrix exp(*this);
5294 
5295  // double f;
5296  int e;
5297  double c = 0.5;
5298  int q = 6;
5299  int p = 1;
5300 
5301  double nA = 0;
5302  for (unsigned int i = 0; i < rowNum; i++) {
5303  double sum = 0;
5304  for (unsigned int j = 0; j < colNum; j++) {
5305  sum += fabs((*this)[i][j]);
5306  }
5307  if (sum > nA || i == 0) {
5308  nA = sum;
5309  }
5310  }
5311 
5312  /* f = */ frexp(nA, &e);
5313  // double s = (0 > e+1)?0:e+1;
5314  double s = e + 1;
5315 
5316  double sca = 1.0 / pow(2.0, s);
5317  exp = sca * exp;
5318  _expX = *this;
5319  _expE = c * exp + _eye;
5320  _expD = -c * exp + _eye;
5321  for (int k = 2; k <= q; k++) {
5322  c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1)));
5323  _expcX = exp * _expX;
5324  _expX = _expcX;
5325  _expcX = c * _expX;
5326  _expE = _expE + _expcX;
5327  if (p)
5328  _expD = _expD + _expcX;
5329  else
5330  _expD = _expD - _expcX;
5331  p = !p;
5332  }
5333  _expX = _expD.inverseByLU();
5334  exp = _expX * _expE;
5335  for (int k = 1; k <= s; k++) {
5336  _expE = exp * exp;
5337  exp = _expE;
5338  }
5339  return exp;
5340 #endif
5341  }
5342 }
5343 
5344 /**************************************************************************************************************/
5345 /**************************************************************************************************************/
5346 
5347 // Specific functions
5348 
5349 /*
5350 input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.
5351 
5352 output:: the complement matrix of the element (rowNo,colNo).
5353 This is the matrix obtained from M after elimenating the row rowNo and column
5354 colNo
5355 
5356 example:
5357 1 2 3
5358 M = 4 5 6
5359 7 8 9
5360 1 3
5361 subblock(M, 1, 1) give the matrix 7 9
5362 */
5363 vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row)
5364 {
5365  vpMatrix M_comp;
5366  M_comp.resize(M.getRows() - 1, M.getCols() - 1, false);
5367 
5368  for (unsigned int i = 0; i < col; i++) {
5369  for (unsigned int j = 0; j < row; j++)
5370  M_comp[i][j] = M[i][j];
5371  for (unsigned int j = row + 1; j < M.getRows(); j++)
5372  M_comp[i][j - 1] = M[i][j];
5373  }
5374  for (unsigned int i = col + 1; i < M.getCols(); i++) {
5375  for (unsigned int j = 0; j < row; j++)
5376  M_comp[i - 1][j] = M[i][j];
5377  for (unsigned int j = row + 1; j < M.getRows(); j++)
5378  M_comp[i - 1][j - 1] = M[i][j];
5379  }
5380  return M_comp;
5381 }
5382 
5392 double vpMatrix::cond(double svThreshold) const
5393 {
5394  unsigned int nbline = getRows();
5395  unsigned int nbcol = getCols();
5396 
5397  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
5398  vpColVector sv;
5399  sv.resize(nbcol); // singular values
5400  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
5401 
5402  // Copy and resize matrix to have at least as many rows as columns
5403  // kernel is computed in svd method only if the matrix has more rows than
5404  // columns
5405 
5406  if (nbline < nbcol)
5407  U.resize(nbcol, nbcol, true);
5408  else
5409  U.resize(nbline, nbcol, false);
5410 
5411  U.insert(*this, 0, 0);
5412 
5413  U.svd(sv, V);
5414 
5415  // Compute the highest singular value
5416  double maxsv = 0;
5417  for (unsigned int i = 0; i < nbcol; i++) {
5418  if (fabs(sv[i]) > maxsv) {
5419  maxsv = fabs(sv[i]);
5420  }
5421  }
5422 
5423  // Compute the rank of the matrix
5424  unsigned int rank = 0;
5425  for (unsigned int i = 0; i < nbcol; i++) {
5426  if (fabs(sv[i]) > maxsv * svThreshold) {
5427  rank++;
5428  }
5429  }
5430 
5431  // Compute the lowest singular value
5432  double minsv = maxsv;
5433  for (unsigned int i = 0; i < rank; i++) {
5434  if (fabs(sv[i]) < minsv) {
5435  minsv = fabs(sv[i]);
5436  }
5437  }
5438 
5439  if (std::fabs(minsv) > std::numeric_limits<double>::epsilon()) {
5440  return maxsv / minsv;
5441  }
5442  else {
5443  return std::numeric_limits<double>::infinity();
5444  }
5445 }
5446 
5453 void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
5454 {
5455  if (H.getCols() != H.getRows()) {
5456  throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(),
5457  H.getCols()));
5458  }
5459 
5460  HLM = H;
5461  for (unsigned int i = 0; i < H.getCols(); i++) {
5462  HLM[i][i] += alpha * H[i][i];
5463  }
5464 }
5465 
5475 vp_deprecated double vpMatrix::euclideanNorm() const
5476 {
5477  return frobeniusNorm();
5478 }
5479 
5488 {
5489  double norm = 0.0;
5490  for (unsigned int i = 0; i < dsize; i++) {
5491  double x = *(data + i);
5492  norm += x * x;
5493  }
5494 
5495  return sqrt(norm);
5496 }
5497 
5507 {
5508  if(this->dsize != 0){
5509  vpMatrix v;
5510  vpColVector w;
5511 
5512  vpMatrix M = *this;
5513 
5514  M.svd(w, v);
5515 
5516  double max = w[0];
5517  unsigned int maxRank = std::min(this->getCols(), this->getRows());
5518  // The maximum reachable rank is either the number of columns or the number of rows
5519  // of the matrix.
5520  unsigned int boundary = std::min(maxRank, w.size());
5521  // boundary is here to ensure that the number of singular values used for the com-
5522  // putation of the euclidean norm of the matrix is not greater than the maximum
5523  // reachable rank. Indeed, some svd library pad the singular values vector with 0s
5524  // if the input matrix is non-square.
5525  for (unsigned int i = 0; i < boundary; i++) {
5526  if (max < w[i]) {
5527  max = w[i];
5528  }
5529  }
5530  return max;
5531  }
5532  else {
5533  return 0.;
5534  }
5535 }
5536 
5548 {
5549  double norm = 0.0;
5550  for (unsigned int i = 0; i < rowNum; i++) {
5551  double x = 0;
5552  for (unsigned int j = 0; j < colNum; j++) {
5553  x += fabs(*(*(rowPtrs + i) + j));
5554  }
5555  if (x > norm) {
5556  norm = x;
5557  }
5558  }
5559  return norm;
5560 }
5561 
5568 double vpMatrix::sumSquare() const
5569 {
5570  double sum_square = 0.0;
5571  double x;
5572 
5573  for (unsigned int i = 0; i < rowNum; i++) {
5574  for (unsigned int j = 0; j < colNum; j++) {
5575  x = rowPtrs[i][j];
5576  sum_square += x * x;
5577  }
5578  }
5579 
5580  return sum_square;
5581 }
5582 
5594 vpMatrix vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode)
5595 {
5596  vpMatrix res;
5597  conv2(M, kernel, res, mode);
5598  return res;
5599 }
5600 
5613 void vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, vpMatrix &res, const std::string &mode)
5614 {
5615  if (M.getRows()*M.getCols() == 0 || kernel.getRows()*kernel.getCols() == 0)
5616  return;
5617 
5618  if (mode == "valid") {
5619  if (kernel.getRows() > M.getRows() || kernel.getCols() > M.getCols())
5620  return;
5621  }
5622 
5623  vpMatrix M_padded, res_same;
5624 
5625  if (mode == "full" || mode == "same") {
5626  const unsigned int pad_x = kernel.getCols()-1;
5627  const unsigned int pad_y = kernel.getRows()-1;
5628  M_padded.resize(M.getRows() + 2*pad_y, M.getCols() + 2*pad_x, true, false);
5629  M_padded.insert(M, pad_y, pad_x);
5630 
5631  if (mode == "same") {
5632  res.resize(M.getRows(), M.getCols(), false, false);
5633  res_same.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
5634  } else {
5635  res.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
5636  }
5637  } else if (mode == "valid") {
5638  M_padded = M;
5639  res.resize(M.getRows()-kernel.getRows()+1, M.getCols()-kernel.getCols()+1);
5640  } else {
5641  return;
5642  }
5643 
5644  if (mode == "same") {
5645  for (unsigned int i = 0; i < res_same.getRows(); i++) {
5646  for (unsigned int j = 0; j < res_same.getCols(); j++) {
5647  for (unsigned int k = 0; k < kernel.getRows(); k++) {
5648  for (unsigned int l = 0; l < kernel.getCols(); l++) {
5649  res_same[i][j] += M_padded[i+k][j+l] * kernel[kernel.getRows()-k-1][kernel.getCols()-l-1];
5650  }
5651  }
5652  }
5653  }
5654 
5655  const unsigned int start_i = kernel.getRows()/2;
5656  const unsigned int start_j = kernel.getCols()/2;
5657  for (unsigned int i = 0; i < M.getRows(); i++) {
5658  memcpy(res.data + i*M.getCols(), res_same.data + (i+start_i)*res_same.getCols() + start_j, sizeof(double)*M.getCols());
5659  }
5660  } else {
5661  for (unsigned int i = 0; i < res.getRows(); i++) {
5662  for (unsigned int j = 0; j < res.getCols(); j++) {
5663  for (unsigned int k = 0; k < kernel.getRows(); k++) {
5664  for (unsigned int l = 0; l < kernel.getCols(); l++) {
5665  res[i][j] += M_padded[i+k][j+l] * kernel[kernel.getRows()-k-1][kernel.getCols()-l-1];
5666  }
5667  }
5668  }
5669  }
5670  }
5671 }
5672 
5673 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
5675 {
5676  return (vpMatrix)(vpColVector::stack(A, B));
5677 }
5678 
5680 {
5681  vpColVector::stack(A, B, C);
5682 }
5683 
5685 
5686 void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); }
5687 
5706 vpRowVector vpMatrix::row(const unsigned int i)
5707 {
5708  vpRowVector c(getCols());
5709 
5710  for (unsigned int j = 0; j < getCols(); j++)
5711  c[j] = (*this)[i - 1][j];
5712  return c;
5713 }
5714 
5732 vpColVector vpMatrix::column(const unsigned int j)
5733 {
5734  vpColVector c(getRows());
5735 
5736  for (unsigned int i = 0; i < getRows(); i++)
5737  c[i] = (*this)[i][j - 1];
5738  return c;
5739 }
5740 
5747 void vpMatrix::setIdentity(const double &val)
5748 {
5749  for (unsigned int i = 0; i < rowNum; i++)
5750  for (unsigned int j = 0; j < colNum; j++)
5751  if (i == j)
5752  (*this)[i][j] = val;
5753  else
5754  (*this)[i][j] = 0;
5755 }
5756 
5757 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:1943
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2066
vpMatrix operator*(const double &x, const vpMatrix &B)
Definition: vpMatrix.cpp:1611
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:450
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2206
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:4452
vpRowVector getRow(const unsigned int i) const
Definition: vpMatrix.cpp:4136
vpColVector getDiag() const
Definition: vpMatrix.cpp:4228
vp_deprecated vpRowVector row(const unsigned int i)
Definition: vpMatrix.cpp:5706
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:5246
static vpMatrix conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode="full")
Definition: vpMatrix.cpp:5594
void kron(const vpMatrix &m1, vpMatrix &out) const
Definition: vpMatrix.cpp:1852
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpMatrix operator-() const
Definition: vpMatrix.cpp:1580
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:115
std::ostream & csvPrint(std::ostream &os) const
Definition: vpMatrix.cpp:4727
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1374
vpMatrix AtA() const
Definition: vpMatrix.cpp:693
double sum() const
Definition: vpMatrix.cpp:1587
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
vpMatrix & operator*=(const double x)
Multiply all the element of the matrix by x : Aij = Aij * x.
Definition: vpMatrix.cpp:1697
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4800
vpMatrix inverseByLU() const
vpMatrix & operator/=(double x)
Divide all the element of the matrix by x : Aij = Aij / x.
Definition: vpMatrix.cpp:1711
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:765
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:1487
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:1521
vpMatrix()
Definition: vpMatrix.h:180
vp_deprecated vpColVector column(const unsigned int j)
Definition: vpMatrix.cpp:5732
double infinityNorm() const
Definition: vpMatrix.cpp:5547
void svdLapack(vpColVector &w, vpMatrix &V)
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:5392
vpMatrix & operator-=(const vpMatrix &B)
Operation A = A - B.
Definition: vpMatrix.cpp:1538
vpMatrix operator+(const vpMatrix &B) const
Definition: vpMatrix.cpp:1432
vpMatrix pseudoInverseEigen3(double svThreshold=1e-6) const
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:143
Implementation of a rotation matrix and operations on such kind of matrices.
unsigned int getCols() const
Definition: vpArray2D.h:279
std::ostream & maplePrint(std::ostream &os) const
Definition: vpMatrix.cpp:4686
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
double sumSquare() const
Definition: vpMatrix.cpp:5568
vpMatrix hadamard(const vpMatrix &m) const
Definition: vpMatrix.cpp:1789
vpMatrix t() const
Definition: vpMatrix.cpp:507
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
Definition: vpMatrix.cpp:4909
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:5747
std::ostream & matlabPrint(std::ostream &os) const
Definition: vpMatrix.cpp:4642
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:5160
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:951
vp_deprecated void init()
Definition: vpMatrix.h:760
static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C)
Definition: vpMatrix.cpp:1345
VISP_EXPORT bool checkSSE2()
vpMatrix pseudoInverseOpenCV(double svThreshold=1e-6) const
vpMatrix pseudoInverseGsl(double svThreshold=1e-6) const
static void multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
Definition: vpMatrix.cpp:1023
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1064
std::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
Definition: vpMatrix.cpp:4776
vpColVector eigenValues() const
Definition: vpMatrix.cpp:4963
double euclideanNorm() const
Definition: vpMatrix.cpp:5475
void svdOpenCV(vpColVector &w, vpMatrix &V)
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:4519
vpMatrix AAt() const
Definition: vpMatrix.cpp:595
vpMatrix transpose() const
Definition: vpMatrix.cpp:517
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
vpMatrix operator/(const double x) const
Cij = Aij / x (A is unchanged)
Definition: vpMatrix.cpp:1651
double inducedL2Norm() const
Definition: vpMatrix.cpp:5506
void svdGsl(vpColVector &w, vpMatrix &V)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpRowVector.h:271
vpMatrix operator*(const vpMatrix &B) const
Definition: vpMatrix.cpp:1199
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
vpRowVector stackRows()
Definition: vpMatrix.cpp:1776
double sumSquare() const
static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
Definition: vpMatrix.cpp:970
vpColVector stackColumns()
Definition: vpMatrix.cpp:1754
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:141
double detByLU() const
static void negateMatrix(const vpMatrix &A, vpMatrix &C)
Definition: vpMatrix.cpp:1562
vpColVector getCol(const unsigned int j) const
Definition: vpMatrix.cpp:4096
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:5453
vpMatrix & operator,(double val)
Definition: vpMatrix.cpp:869
double frobeniusNorm() const
Definition: vpMatrix.cpp:5487
Function not implemented.
Definition: vpException.h:90
Class that consider the case of a translation vector.
void eye()
Definition: vpMatrix.cpp:492
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:5264
vpMatrix & operator=(const vpArray2D< double > &A)
Definition: vpMatrix.cpp:718
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310
vpMatrix & operator<<(double *)
Definition: vpMatrix.cpp:852