Visual Servoing Platform  version 3.2.1 under development (2019-09-17) under development (2019-09-17)
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 #define USE_SSE_CODE 1
70 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
71 #include <emmintrin.h>
72 #define VISP_HAVE_SSE2 1
73 #endif
74 
75 #if VISP_HAVE_SSE2 && USE_SSE_CODE
76 #define USE_SSE 1
77 #endif
78 
79 // Prototypes of specific functions
80 vpMatrix subblock(const vpMatrix &, unsigned int, unsigned int);
81 
82 void compute_pseudo_inverse(const vpMatrix &a, const vpColVector &sv, const vpMatrix &v, unsigned int nrows,
83  unsigned int ncols, unsigned int nrows_orig, unsigned int ncols_orig, double svThreshold,
84  vpMatrix &Ap, unsigned int &rank)
85 {
86  vpMatrix a1(ncols, nrows);
87 
88  // compute the highest singular value and the rank of h
89  double maxsv = 0;
90  for (unsigned int i = 0; i < ncols; i++) {
91  if (fabs(sv[i]) > maxsv)
92  maxsv = fabs(sv[i]);
93  }
94 
95  rank = 0;
96 
97  for (unsigned int i = 0; i < ncols; i++) {
98  if (fabs(sv[i]) > maxsv * svThreshold) {
99  rank++;
100  }
101 
102  for (unsigned int j = 0; j < nrows; j++) {
103  a1[i][j] = 0.0;
104 
105  for (unsigned int k = 0; k < ncols; k++) {
106  if (fabs(sv[k]) > maxsv * svThreshold) {
107  a1[i][j] += v[i][k] * a[j][k] / sv[k];
108  }
109  }
110  }
111  }
112  if (nrows_orig >= ncols_orig)
113  Ap = a1;
114  else
115  Ap = a1.t();
116 }
117 
118 void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows_orig,
119  unsigned int ncols_orig, double svThreshold, vpMatrix &Ap, unsigned int &rank,
120  vpMatrix &imA, vpMatrix &imAt, vpMatrix &kerAt)
121 {
122  Ap.resize(ncols_orig, nrows_orig);
123 
124  // compute the highest singular value and the rank of h
125  double maxsv = fabs(sv[0]);
126 
127  rank = 0;
128 
129  for (unsigned int i = 0; i < ncols_orig; i++) {
130  if (fabs(sv[i]) > maxsv * svThreshold) {
131  rank++;
132  }
133 
134  for (unsigned int j = 0; j < nrows_orig; j++) {
135  // Ap[i][j] = 0.0;
136 
137  for (unsigned int k = 0; k < ncols_orig; k++) {
138  if (fabs(sv[k]) > maxsv * svThreshold) {
139  Ap[i][j] += V[i][k] * U[j][k] / sv[k];
140  }
141  }
142  }
143  }
144 
145  // Compute im(A) and im(At)
146  imA.resize(nrows_orig, rank);
147  imAt.resize(ncols_orig, rank);
148 
149  for (unsigned int i = 0; i < nrows_orig; i++) {
150  for (unsigned int j = 0; j < rank; j++) {
151  imA[i][j] = U[i][j];
152  }
153  }
154 
155  for (unsigned int i = 0; i < ncols_orig; i++) {
156  for (unsigned int j = 0; j < rank; j++) {
157  imAt[i][j] = V[i][j];
158  }
159  }
160 
161  kerAt.resize(ncols_orig - rank, ncols_orig);
162  if (rank != ncols_orig) {
163  for (unsigned int j = 0, k = 0; j < ncols_orig; j++) {
164  // if( v.col(j) in kernel and non zero )
165  if ((fabs(sv[j]) <= maxsv * svThreshold) &&
166  (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
167  for (unsigned int i = 0; i < V.getRows(); i++) {
168  kerAt[k][i] = V[i][j];
169  }
170  k++;
171  }
172  }
173  }
174 }
175 
181 vpMatrix::vpMatrix(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
182  : vpArray2D<double>()
183 {
184  if (((r + nrows) > M.rowNum) || ((c + ncols) > M.colNum)) {
186  "Cannot construct a sub matrix (%dx%d) starting at "
187  "position (%d,%d) that is not contained in the "
188  "original matrix (%dx%d)",
189  nrows, ncols, r, c, M.rowNum, M.colNum));
190  }
191 
192  init(M, r, c, nrows, ncols);
193 }
194 
195 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
197 {
198  rowNum = A.rowNum;
199  colNum = A.colNum;
200  rowPtrs = A.rowPtrs;
201  dsize = A.dsize;
202  data = A.data;
203 
204  A.rowNum = 0;
205  A.colNum = 0;
206  A.rowPtrs = NULL;
207  A.dsize = 0;
208  A.data = NULL;
209 }
210 #endif
211 
258 void vpMatrix::init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
259 {
260  unsigned int rnrows = r + nrows;
261  unsigned int cncols = c + ncols;
262 
263  if (rnrows > M.getRows())
264  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
265  M.getRows()));
266  if (cncols > M.getCols())
267  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
268  M.getCols()));
269  resize(nrows, ncols, false, false);
270 
271  if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
272  return; // Noting to do
273  for (unsigned int i = 0; i < nrows; i++) {
274  memcpy((*this)[i], &M[i + r][c], ncols * sizeof(double));
275  }
276 }
277 
319 vpMatrix vpMatrix::extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
320 {
321  unsigned int rnrows = r + nrows;
322  unsigned int cncols = c + ncols;
323 
324  if (rnrows > getRows())
325  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
326  getRows()));
327  if (cncols > getCols())
328  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
329  getCols()));
330 
331  vpMatrix M(nrows, ncols);
332  for (unsigned int i = 0; i < nrows; i++) {
333  memcpy(M[i], &(*this)[i + r][c], ncols * sizeof(double));
334  }
335 
336  return M;
337 }
338 
343 void vpMatrix::eye(unsigned int n) { eye(n, n); }
344 
349 void vpMatrix::eye(unsigned int m, unsigned int n)
350 {
351  resize(m, n);
352 
353  eye();
354 }
355 
361 {
362  for (unsigned int i = 0; i < rowNum; i++) {
363  for (unsigned int j = 0; j < colNum; j++) {
364  if (i == j)
365  (*this)[i][j] = 1.0;
366  else
367  (*this)[i][j] = 0;
368  }
369  }
370 }
371 
376 {
377  vpMatrix At;
378 
379  At.resize(colNum, rowNum, false, false);
380 
381  for (unsigned int i = 0; i < rowNum; i++) {
382  double *coli = (*this)[i];
383  for (unsigned int j = 0; j < colNum; j++)
384  At[j][i] = coli[j];
385  }
386  return At;
387 }
388 
395 {
396  vpMatrix At;
397  transpose(At);
398  return At;
399 }
400 
407 {
408  At.resize(colNum, rowNum, false, false);
409 
410  size_t A_step = colNum;
411  double **AtRowPtrs = At.rowPtrs;
412 
413  for (unsigned int i = 0; i < colNum; i++) {
414  double *row_ = AtRowPtrs[i];
415  double *col = rowPtrs[0] + i;
416  for (unsigned int j = 0; j < rowNum; j++, col += A_step)
417  *(row_++) = *col;
418  }
419 }
420 
427 {
428  vpMatrix B;
429 
430  AAt(B);
431 
432  return B;
433 }
434 
446 void vpMatrix::AAt(vpMatrix &B) const
447 {
448  if ((B.rowNum != rowNum) || (B.colNum != rowNum))
449  B.resize(rowNum, rowNum, false, false);
450 
451  // compute A*A^T
452  for (unsigned int i = 0; i < rowNum; i++) {
453  for (unsigned int j = i; j < rowNum; j++) {
454  double *pi = rowPtrs[i]; // row i
455  double *pj = rowPtrs[j]; // row j
456 
457  // sum (row i .* row j)
458  double ssum = 0;
459  for (unsigned int k = 0; k < colNum; k++)
460  ssum += *(pi++) * *(pj++);
461 
462  B[i][j] = ssum; // upper triangle
463  if (i != j)
464  B[j][i] = ssum; // lower triangle
465  }
466  }
467 }
468 
480 void vpMatrix::AtA(vpMatrix &B) const
481 {
482  if ((B.rowNum != colNum) || (B.colNum != colNum))
483  B.resize(colNum, colNum, false, false);
484 
485 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
486  double alpha = 1.0;
487  double beta = 0.0;
488  char transa = 'n';
489  char transb = 't';
490 
491  vpMatrix::blas_dgemm(transa, transb, colNum, colNum, rowNum, alpha, data, colNum, data, colNum, beta, B.data, colNum);
492 #else
493  unsigned int i, j, k;
494  double s;
495  double *ptr;
496  for (i = 0; i < colNum; i++) {
497  double *Bi = B[i];
498  for (j = 0; j < i; j++) {
499  ptr = data;
500  s = 0;
501  for (k = 0; k < rowNum; k++) {
502  s += (*(ptr + i)) * (*(ptr + j));
503  ptr += colNum;
504  }
505  *Bi++ = s;
506  B[j][i] = s;
507  }
508  ptr = data;
509  s = 0;
510  for (k = 0; k < rowNum; k++) {
511  s += (*(ptr + i)) * (*(ptr + i));
512  ptr += colNum;
513  }
514  *Bi = s;
515  }
516 #endif
517 }
518 
525 {
526  vpMatrix B;
527 
528  AtA(B);
529 
530  return B;
531 }
532 
550 {
551  resize(A.getRows(), A.getCols(), false, false);
552 
553  if (data != NULL && A.data != NULL && data != A.data) {
554  memcpy(data, A.data, dsize * sizeof(double));
555  }
556 
557  return *this;
558 }
559 
560 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
562 {
563  resize(A.getRows(), A.getCols(), false, false);
564 
565  if (data != NULL && A.data != NULL && data != A.data) {
566  memcpy(data, A.data, dsize * sizeof(double));
567  }
568 
569  return *this;
570 }
571 
573 {
574  if (this != &other) {
575  free(data);
576  free(rowPtrs);
577 
578  rowNum = other.rowNum;
579  colNum = other.colNum;
580  rowPtrs = other.rowPtrs;
581  dsize = other.dsize;
582  data = other.data;
583 
584  other.rowNum = 0;
585  other.colNum = 0;
586  other.rowPtrs = NULL;
587  other.dsize = 0;
588  other.data = NULL;
589  }
590 
591  return *this;
592 }
593 
618 vpMatrix& vpMatrix::operator=(const std::initializer_list<double> &list)
619 {
620  if (dsize != static_cast<unsigned int>(list.size())) {
621  resize(1, static_cast<unsigned int>(list.size()), false, false);
622  }
623 
624  std::copy(list.begin(), list.end(), data);
625 
626  return *this;
627 }
628 
652 vpMatrix& vpMatrix::operator=(const std::initializer_list<std::initializer_list<double> > &lists)
653 {
654  unsigned int nrows = static_cast<unsigned int>(lists.size()), ncols = 0;
655  for (auto& l : lists) {
656  if (static_cast<unsigned int>(l.size()) > ncols) {
657  ncols = static_cast<unsigned int>(l.size());
658  }
659  }
660 
661  resize(nrows, ncols, false, false);
662  auto it = lists.begin();
663  for (unsigned int i = 0; i < rowNum; i++, ++it) {
664  std::copy(it->begin(), it->end(), rowPtrs[i]);
665  }
666 
667  return *this;
668 }
669 #endif
670 
673 {
674  std::fill(data, data + rowNum*colNum, x);
675  return *this;
676 }
677 
684 {
685  for (unsigned int i = 0; i < rowNum; i++) {
686  for (unsigned int j = 0; j < colNum; j++) {
687  rowPtrs[i][j] = *x++;
688  }
689  }
690  return *this;
691 }
692 
694 {
695  resize(1, 1, false, false);
696  rowPtrs[0][0] = val;
697  return *this;
698 }
699 
701 {
702  resize(1, colNum + 1, false, false);
703  rowPtrs[0][colNum - 1] = val;
704  return *this;
705 }
706 
743 {
744  unsigned int rows = A.getRows();
745  this->resize(rows, rows);
746 
747  (*this) = 0;
748  for (unsigned int i = 0; i < rows; i++)
749  (*this)[i][i] = A[i];
750 }
751 
782 void vpMatrix::diag(const double &val)
783 {
784  (*this) = 0;
785  unsigned int min_ = (rowNum < colNum) ? rowNum : colNum;
786  for (unsigned int i = 0; i < min_; i++)
787  (*this)[i][i] = val;
788 }
789 
802 {
803  unsigned int rows = A.getRows();
804  DA.resize(rows, rows);
805 
806  for (unsigned int i = 0; i < rows; i++)
807  DA[i][i] = A[i];
808 }
809 
815 {
816  vpTranslationVector t_out;
817 
818  if (rowNum != 3 || colNum != 3) {
819  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector",
820  rowNum, colNum, tv.getRows(), tv.getCols()));
821  }
822 
823  for (unsigned int j = 0; j < 3; j++)
824  t_out[j] = 0;
825 
826  for (unsigned int j = 0; j < 3; j++) {
827  double tj = tv[j]; // optimization em 5/12/2006
828  for (unsigned int i = 0; i < 3; i++) {
829  t_out[i] += rowPtrs[i][j] * tj;
830  }
831  }
832  return t_out;
833 }
834 
840 {
841  vpColVector v_out;
842  vpMatrix::multMatrixVector(*this, v, v_out);
843  return v_out;
844 }
845 
855 {
856  if (A.colNum != v.getRows()) {
857  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
858  A.getRows(), A.getCols(), v.getRows()));
859  }
860 
861  if (A.rowNum != w.rowNum)
862  w.resize(A.rowNum, false);
863 
864 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
865  double alpha = 1.0;
866  double beta = 0.0;
867  char trans = 't';
868  int incr = 1;
869 
870  vpMatrix::blas_dgemv(trans, A.colNum, A.rowNum, alpha, A.data, A.colNum, v.data, incr, beta, w.data, incr);
871 #else
872  w = 0.0;
873  for (unsigned int j = 0; j < A.colNum; j++) {
874  double vj = v[j]; // optimization em 5/12/2006
875  for (unsigned int i = 0; i < A.rowNum; i++) {
876  w[i] += A.rowPtrs[i][j] * vj;
877  }
878  }
879 #endif
880 }
881 
882 //---------------------------------
883 // Matrix operations.
884 //---------------------------------
885 
895 void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
896 {
897  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
898  C.resize(A.rowNum, B.colNum, false, false);
899 
900  if (A.colNum != B.rowNum) {
901  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (%dx%d) matrix", A.getRows(),
902  A.getCols(), B.getRows(), B.getCols()));
903  }
904 
905 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
906  double alpha = 1.0;
907  double beta = 0.0;
908  char trans = 'n';
909 
910  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
911  C.data, B.colNum);
912 #else
913  // 5/12/06 some "very" simple optimization to avoid indexation
914  unsigned int BcolNum = B.colNum;
915  unsigned int BrowNum = B.rowNum;
916  unsigned int i, j, k;
917  double **BrowPtrs = B.rowPtrs;
918  for (i = 0; i < A.rowNum; i++) {
919  double *rowptri = A.rowPtrs[i];
920  double *ci = C[i];
921  for (j = 0; j < BcolNum; j++) {
922  double s = 0;
923  for (k = 0; k < BrowNum; k++)
924  s += rowptri[k] * BrowPtrs[k][j];
925  ci[j] = s;
926  }
927  }
928 #endif
929 }
930 
945 {
946  if (A.colNum != 3 || A.rowNum != 3 || B.colNum != 3 || B.rowNum != 3) {
948  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
949  "rotation matrix",
950  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
951  }
952 
953  // 5/12/06 some "very" simple optimization to avoid indexation
954  unsigned int BcolNum = B.colNum;
955  unsigned int BrowNum = B.rowNum;
956  unsigned int i, j, k;
957  double **BrowPtrs = B.rowPtrs;
958  for (i = 0; i < A.rowNum; i++) {
959  double *rowptri = A.rowPtrs[i];
960  double *ci = C[i];
961  for (j = 0; j < BcolNum; j++) {
962  double s = 0;
963  for (k = 0; k < BrowNum; k++)
964  s += rowptri[k] * BrowPtrs[k][j];
965  ci[j] = s;
966  }
967  }
968 }
969 
984 {
985  if (A.colNum != 4 || A.rowNum != 4 || B.colNum != 4 || B.rowNum != 4) {
987  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
988  "rotation matrix",
989  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
990  }
991 
992  // 5/12/06 some "very" simple optimization to avoid indexation
993  unsigned int BcolNum = B.colNum;
994  unsigned int BrowNum = B.rowNum;
995  unsigned int i, j, k;
996  double **BrowPtrs = B.rowPtrs;
997  for (i = 0; i < A.rowNum; i++) {
998  double *rowptri = A.rowPtrs[i];
999  double *ci = C[i];
1000  for (j = 0; j < BcolNum; j++) {
1001  double s = 0;
1002  for (k = 0; k < BrowNum; k++)
1003  s += rowptri[k] * BrowPtrs[k][j];
1004  ci[j] = s;
1005  }
1006  }
1007 }
1008 
1022 {
1023  vpMatrix::multMatrixVector(A, B, C);
1024 }
1025 
1031 {
1032  vpMatrix C;
1033 
1034  vpMatrix::mult2Matrices(*this, B, C);
1035 
1036  return C;
1037 }
1038 
1044 {
1045  if (colNum != R.getRows()) {
1046  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1047  colNum));
1048  }
1049  vpMatrix C(rowNum, 3);
1050 
1051  unsigned int RcolNum = R.getCols();
1052  unsigned int RrowNum = R.getRows();
1053  for (unsigned int i = 0; i < rowNum; i++) {
1054  double *rowptri = rowPtrs[i];
1055  double *ci = C[i];
1056  for (unsigned int j = 0; j < RcolNum; j++) {
1057  double s = 0;
1058  for (unsigned int k = 0; k < RrowNum; k++)
1059  s += rowptri[k] * R[k][j];
1060  ci[j] = s;
1061  }
1062  }
1063 
1064  return C;
1065 }
1071 {
1072  if (colNum != V.getRows()) {
1073  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix",
1074  rowNum, colNum));
1075  }
1076  vpMatrix M;
1077  M.resize(rowNum, 6, false, false);
1078 
1079 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
1080  double alpha = 1.0;
1081  double beta = 0.0;
1082  char trans = 'n';
1083 
1084  vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, V.colNum, data, colNum, beta, M.data,
1085  V.colNum);
1086 #else
1087  bool checkSSE2 = vpCPUFeatures::checkSSE2();
1088 #if !USE_SSE
1089  checkSSE2 = false;
1090 #endif
1091 
1092  if (checkSSE2) {
1093 #if USE_SSE
1094  vpMatrix V_trans(6, 6);
1095  for (unsigned int i = 0; i < 6; i++) {
1096  for (unsigned int j = 0; j < 6; j++) {
1097  V_trans[i][j] = V[j][i];
1098  }
1099  }
1100 
1101  for (unsigned int i = 0; i < rowNum; i++) {
1102  double *rowptri = rowPtrs[i];
1103  double *ci = M[i];
1104 
1105  for (int j = 0; j < 6; j++) {
1106  __m128d v_mul = _mm_setzero_pd();
1107  for (int k = 0; k < 6; k += 2) {
1108  v_mul = _mm_add_pd(v_mul, _mm_mul_pd(_mm_loadu_pd(&rowptri[k]), _mm_loadu_pd(&V_trans[j][k])));
1109  }
1110 
1111  double v_tmp[2];
1112  _mm_storeu_pd(v_tmp, v_mul);
1113  ci[j] = v_tmp[0] + v_tmp[1];
1114  }
1115  }
1116 #endif
1117  } else {
1118  unsigned int VcolNum = V.getCols();
1119  unsigned int VrowNum = V.getRows();
1120  for (unsigned int i = 0; i < rowNum; i++) {
1121  double *rowptri = rowPtrs[i];
1122  double *ci = M[i];
1123  for (unsigned int j = 0; j < VcolNum; j++) {
1124  double s = 0;
1125  for (unsigned int k = 0; k < VrowNum; k++)
1126  s += rowptri[k] * V[k][j];
1127  ci[j] = s;
1128  }
1129  }
1130  }
1131 #endif
1132 
1133  return M;
1134 }
1140 {
1141  if (colNum != V.getRows()) {
1142  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) force/torque twist matrix",
1143  rowNum, colNum));
1144  }
1145  vpMatrix M(rowNum, 6);
1146 
1147  unsigned int VcolNum = V.getCols();
1148  unsigned int VrowNum = V.getRows();
1149  for (unsigned int i = 0; i < rowNum; i++) {
1150  double *rowptri = rowPtrs[i];
1151  double *ci = M[i];
1152  for (unsigned int j = 0; j < VcolNum; j++) {
1153  double s = 0;
1154  for (unsigned int k = 0; k < VrowNum; k++)
1155  s += rowptri[k] * V[k][j];
1156  ci[j] = s;
1157  }
1158  }
1159 
1160  return M;
1161 }
1162 
1173 void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB,
1174  vpMatrix &C)
1175 {
1176  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1177  C.resize(A.rowNum, B.colNum, false, false);
1178 
1179  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1180  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1181  A.getCols(), B.getRows(), B.getCols()));
1182  }
1183 
1184  double **ArowPtrs = A.rowPtrs;
1185  double **BrowPtrs = B.rowPtrs;
1186  double **CrowPtrs = C.rowPtrs;
1187 
1188  for (unsigned int i = 0; i < A.rowNum; i++)
1189  for (unsigned int j = 0; j < A.colNum; j++)
1190  CrowPtrs[i][j] = wB * BrowPtrs[i][j] + wA * ArowPtrs[i][j];
1191 }
1192 
1202 void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1203 {
1204  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1205  C.resize(A.rowNum, B.colNum, false, false);
1206 
1207  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1208  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1209  A.getCols(), B.getRows(), B.getCols()));
1210  }
1211 
1212  double **ArowPtrs = A.rowPtrs;
1213  double **BrowPtrs = B.rowPtrs;
1214  double **CrowPtrs = C.rowPtrs;
1215 
1216  for (unsigned int i = 0; i < A.rowNum; i++) {
1217  for (unsigned int j = 0; j < A.colNum; j++) {
1218  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1219  }
1220  }
1221 }
1222 
1236 {
1237  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1238  C.resize(A.rowNum);
1239 
1240  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1241  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1242  A.getCols(), B.getRows(), B.getCols()));
1243  }
1244 
1245  double **ArowPtrs = A.rowPtrs;
1246  double **BrowPtrs = B.rowPtrs;
1247  double **CrowPtrs = C.rowPtrs;
1248 
1249  for (unsigned int i = 0; i < A.rowNum; i++) {
1250  for (unsigned int j = 0; j < A.colNum; j++) {
1251  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1252  }
1253  }
1254 }
1255 
1261 {
1262  vpMatrix C;
1263  vpMatrix::add2Matrices(*this, B, C);
1264  return C;
1265 }
1266 
1283 {
1284  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1285  C.resize(A.rowNum);
1286 
1287  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1288  throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1289  A.getCols(), B.getRows(), B.getCols()));
1290  }
1291 
1292  double **ArowPtrs = A.rowPtrs;
1293  double **BrowPtrs = B.rowPtrs;
1294  double **CrowPtrs = C.rowPtrs;
1295 
1296  for (unsigned int i = 0; i < A.rowNum; i++) {
1297  for (unsigned int j = 0; j < A.colNum; j++) {
1298  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1299  }
1300  }
1301 }
1302 
1315 void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1316 {
1317  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1318  C.resize(A.rowNum, A.colNum, false, false);
1319 
1320  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1321  throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1322  A.getCols(), B.getRows(), B.getCols()));
1323  }
1324 
1325  double **ArowPtrs = A.rowPtrs;
1326  double **BrowPtrs = B.rowPtrs;
1327  double **CrowPtrs = C.rowPtrs;
1328 
1329  for (unsigned int i = 0; i < A.rowNum; i++) {
1330  for (unsigned int j = 0; j < A.colNum; j++) {
1331  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1332  }
1333  }
1334 }
1335 
1341 {
1342  vpMatrix C;
1343  vpMatrix::sub2Matrices(*this, B, C);
1344  return C;
1345 }
1346 
1348 
1350 {
1351  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1352  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1353  B.getRows(), B.getCols()));
1354  }
1355 
1356  double **BrowPtrs = B.rowPtrs;
1357 
1358  for (unsigned int i = 0; i < rowNum; i++)
1359  for (unsigned int j = 0; j < colNum; j++)
1360  rowPtrs[i][j] += BrowPtrs[i][j];
1361 
1362  return *this;
1363 }
1364 
1367 {
1368  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1369  throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1370  B.getRows(), B.getCols()));
1371  }
1372 
1373  double **BrowPtrs = B.rowPtrs;
1374  for (unsigned int i = 0; i < rowNum; i++)
1375  for (unsigned int j = 0; j < colNum; j++)
1376  rowPtrs[i][j] -= BrowPtrs[i][j];
1377 
1378  return *this;
1379 }
1380 
1391 {
1392  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1393  C.resize(A.rowNum, A.colNum, false, false);
1394 
1395  double **ArowPtrs = A.rowPtrs;
1396  double **CrowPtrs = C.rowPtrs;
1397 
1398  // t0=vpTime::measureTimeMicros();
1399  for (unsigned int i = 0; i < A.rowNum; i++)
1400  for (unsigned int j = 0; j < A.colNum; j++)
1401  CrowPtrs[i][j] = -ArowPtrs[i][j];
1402 }
1403 
1409 {
1410  vpMatrix C;
1411  vpMatrix::negateMatrix(*this, C);
1412  return C;
1413 }
1414 
1415 double vpMatrix::sum() const
1416 {
1417  double s = 0.0;
1418  for (unsigned int i = 0; i < rowNum; i++) {
1419  for (unsigned int j = 0; j < colNum; j++) {
1420  s += rowPtrs[i][j];
1421  }
1422  }
1423 
1424  return s;
1425 }
1426 
1427 //---------------------------------
1428 // Matrix/vector operations.
1429 //---------------------------------
1430 
1431 //---------------------------------
1432 // Matrix/real operations.
1433 //---------------------------------
1434 
1439 vpMatrix operator*(const double &x, const vpMatrix &B)
1440 {
1441  vpMatrix C(B.getRows(), B.getCols());
1442 
1443  unsigned int Brow = B.getRows();
1444  unsigned int Bcol = B.getCols();
1445 
1446  for (unsigned int i = 0; i < Brow; i++)
1447  for (unsigned int j = 0; j < Bcol; j++)
1448  C[i][j] = B[i][j] * x;
1449 
1450  return C;
1451 }
1452 
1458 {
1459  vpMatrix M(rowNum, colNum);
1460 
1461  for (unsigned int i = 0; i < rowNum; i++)
1462  for (unsigned int j = 0; j < colNum; j++)
1463  M[i][j] = rowPtrs[i][j] * x;
1464 
1465  return M;
1466 }
1467 
1470 {
1471  vpMatrix C;
1472 
1473  C.resize(rowNum, colNum, false, false);
1474 
1475  // if (x == 0) {
1476  if (std::fabs(x) <= std::numeric_limits<double>::epsilon()) {
1477  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1478  }
1479 
1480  double xinv = 1 / x;
1481 
1482  for (unsigned int i = 0; i < rowNum; i++)
1483  for (unsigned int j = 0; j < colNum; j++)
1484  C[i][j] = rowPtrs[i][j] * xinv;
1485 
1486  return C;
1487 }
1488 
1491 {
1492  for (unsigned int i = 0; i < rowNum; i++)
1493  for (unsigned int j = 0; j < colNum; j++)
1494  rowPtrs[i][j] += x;
1495 
1496  return *this;
1497 }
1498 
1501 {
1502  for (unsigned int i = 0; i < rowNum; i++)
1503  for (unsigned int j = 0; j < colNum; j++)
1504  rowPtrs[i][j] -= x;
1505 
1506  return *this;
1507 }
1508 
1514 {
1515  for (unsigned int i = 0; i < rowNum; i++)
1516  for (unsigned int j = 0; j < colNum; j++)
1517  rowPtrs[i][j] *= x;
1518 
1519  return *this;
1520 }
1521 
1524 {
1525  // if (x == 0)
1526  if (std::fabs(x) <= std::numeric_limits<double>::epsilon())
1527  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1528 
1529  double xinv = 1 / x;
1530 
1531  for (unsigned int i = 0; i < rowNum; i++)
1532  for (unsigned int j = 0; j < colNum; j++)
1533  rowPtrs[i][j] *= xinv;
1534 
1535  return *this;
1536 }
1537 
1538 //----------------------------------------------------------------
1539 // Matrix Operation
1540 //----------------------------------------------------------------
1541 
1547 {
1548  if ((out.rowNum != colNum * rowNum) || (out.colNum != 1))
1549  out.resize(colNum * rowNum, false, false);
1550 
1551  double *optr = out.data;
1552  for (unsigned int j = 0; j < colNum; j++) {
1553  for (unsigned int i = 0; i < rowNum; i++) {
1554  *(optr++) = rowPtrs[i][j];
1555  }
1556  }
1557 }
1558 
1564 {
1565  vpColVector out(colNum * rowNum);
1566  stackColumns(out);
1567  return out;
1568 }
1569 
1575 {
1576  if ((out.getRows() != 1) || (out.getCols() != colNum * rowNum))
1577  out.resize(colNum * rowNum, false, false);
1578 
1579  memcpy(out.data, data, sizeof(double)*out.getCols());
1580 }
1586 {
1587  vpRowVector out(colNum * rowNum);
1588  stackRows(out);
1589  return out;
1590 }
1591 
1599 {
1600  if (m.getRows() != rowNum || m.getCols() != colNum) {
1601  throw(vpException(vpException::dimensionError, "In Hadamard product: bad dimension of input matrix"));
1602  }
1603 
1604  vpMatrix out;
1605  out.resize(rowNum, colNum, false);
1606 
1607  unsigned int i = 0;
1608 
1609 #if VISP_HAVE_SSE2
1610  if (vpCPUFeatures::checkSSE2() && dsize >= 2) {
1611  for (; i <= dsize - 2; i += 2) {
1612  __m128d vout = _mm_mul_pd(_mm_loadu_pd(data + i), _mm_loadu_pd(m.data + i));
1613  _mm_storeu_pd(out.data + i, vout);
1614  }
1615  }
1616 #endif
1617 
1618  for (; i < dsize; i++) {
1619  out.data[i] = data[i] * m.data[i];
1620  }
1621 
1622  return out;
1623 }
1624 
1631 void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out)
1632 {
1633  unsigned int r1 = m1.getRows();
1634  unsigned int c1 = m1.getCols();
1635  unsigned int r2 = m2.getRows();
1636  unsigned int c2 = m2.getCols();
1637 
1638  out.resize(r1*r2, c1*c2, false, false);
1639 
1640  for (unsigned int r = 0; r < r1; r++) {
1641  for (unsigned int c = 0; c < c1; c++) {
1642  double alpha = m1[r][c];
1643  double *m2ptr = m2[0];
1644  unsigned int roffset = r * r2;
1645  unsigned int coffset = c * c2;
1646  for (unsigned int rr = 0; rr < r2; rr++) {
1647  for (unsigned int cc = 0; cc < c2; cc++) {
1648  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1649  }
1650  }
1651  }
1652  }
1653 }
1654 
1661 void vpMatrix::kron(const vpMatrix &m, vpMatrix &out) const { kron(*this, m, out); }
1662 
1670 {
1671  unsigned int r1 = m1.getRows();
1672  unsigned int c1 = m1.getCols();
1673  unsigned int r2 = m2.getRows();
1674  unsigned int c2 = m2.getCols();
1675 
1676  vpMatrix out(r1 * r2, c1 * c2);
1677 
1678  for (unsigned int r = 0; r < r1; r++) {
1679  for (unsigned int c = 0; c < c1; c++) {
1680  double alpha = m1[r][c];
1681  double *m2ptr = m2[0];
1682  unsigned int roffset = r * r2;
1683  unsigned int coffset = c * c2;
1684  for (unsigned int rr = 0; rr < r2; rr++) {
1685  for (unsigned int cc = 0; cc < c2; cc++) {
1686  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1687  }
1688  }
1689  }
1690  }
1691  return out;
1692 }
1693 
1699 vpMatrix vpMatrix::kron(const vpMatrix &m) const { return kron(*this, m); }
1700 
1751 void vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const { x = pseudoInverse(1e-6) * b; }
1752 
1803 {
1804  vpColVector X(colNum);
1805 
1806  solveBySVD(B, X);
1807  return X;
1808 }
1809 
1875 {
1876 #if defined(VISP_HAVE_LAPACK)
1877  svdLapack(w, V);
1878 #elif defined(VISP_HAVE_EIGEN3)
1879  svdEigen3(w, V);
1880 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
1881  svdOpenCV(w, V);
1882 #elif defined(VISP_HAVE_GSL)
1883  svdGsl(w, V);
1884 #else
1885  (void)w;
1886  (void)V;
1887  throw(vpException(vpException::fatalError, "Cannot compute SVD. Install Lapack, Eigen3, OpenCV or GSL 3rd party"));
1888 #endif
1889 }
1890 
1945 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
1946 {
1947 #if defined(VISP_HAVE_LAPACK)
1948  return pseudoInverseLapack(Ap, svThreshold);
1949 #elif defined(VISP_HAVE_EIGEN3)
1950  return pseudoInverseEigen3(Ap, svThreshold);
1951 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
1952  return pseudoInverseOpenCV(Ap, svThreshold);
1953 #elif defined(VISP_HAVE_GSL)
1954  return pseudoInverseGsl(Ap, svThreshold);
1955 #else
1956  (void)Ap;
1957  (void)svThreshold;
1958  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
1959  "Install Lapack, Eigen3, OpenCV "
1960  "or GSL 3rd party"));
1961 #endif
1962 }
1963 
2014 vpMatrix vpMatrix::pseudoInverse(double svThreshold) const
2015 {
2016 #if defined(VISP_HAVE_LAPACK)
2017  return pseudoInverseLapack(svThreshold);
2018 #elif defined(VISP_HAVE_EIGEN3)
2019  return pseudoInverseEigen3(svThreshold);
2020 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2021  return pseudoInverseOpenCV(svThreshold);
2022 #elif defined(VISP_HAVE_GSL)
2023  return pseudoInverseGsl(svThreshold);
2024 #else
2025  (void)svThreshold;
2026  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2027  "Install Lapack, Eigen3, OpenCV "
2028  "or GSL 3rd party"));
2029 #endif
2030 }
2031 
2032 #ifndef DOXYGEN_SHOULD_SKIP_THIS
2033 #if defined(VISP_HAVE_LAPACK)
2034 
2070 vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const
2071 {
2072  unsigned int nrows, ncols;
2073  unsigned int nrows_orig = getRows();
2074  unsigned int ncols_orig = getCols();
2075 
2076  vpMatrix Ap(ncols_orig, nrows_orig);
2077 
2078  if (nrows_orig >= ncols_orig) {
2079  nrows = nrows_orig;
2080  ncols = ncols_orig;
2081  } else {
2082  nrows = ncols_orig;
2083  ncols = nrows_orig;
2084  }
2085 
2086  vpMatrix U(nrows, ncols);
2087  vpMatrix V(ncols, ncols);
2088  vpColVector sv(ncols);
2089 
2090  if (nrows_orig >= ncols_orig)
2091  U = *this;
2092  else
2093  U = (*this).t();
2094 
2095  U.svdLapack(sv, V);
2096 
2097  unsigned int rank;
2098  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2099 
2100  return Ap;
2101 }
2102 
2143 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) const
2144 {
2145  unsigned int nrows, ncols;
2146  unsigned int nrows_orig = getRows();
2147  unsigned int ncols_orig = getCols();
2148  unsigned int rank;
2149 
2150  Ap.resize(ncols_orig, nrows_orig);
2151 
2152  if (nrows_orig >= ncols_orig) {
2153  nrows = nrows_orig;
2154  ncols = ncols_orig;
2155  } else {
2156  nrows = ncols_orig;
2157  ncols = nrows_orig;
2158  }
2159 
2160  vpMatrix U(nrows, ncols);
2161  vpMatrix V(ncols, ncols);
2162  vpColVector sv(ncols);
2163 
2164  if (nrows_orig >= ncols_orig)
2165  U = *this;
2166  else
2167  U = (*this).t();
2168 
2169  U.svdLapack(sv, V);
2170 
2171  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2172 
2173  return rank;
2174 }
2222 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2223 {
2224  unsigned int nrows, ncols;
2225  unsigned int nrows_orig = getRows();
2226  unsigned int ncols_orig = getCols();
2227  unsigned int rank;
2228 
2229  Ap.resize(ncols_orig, nrows_orig);
2230 
2231  if (nrows_orig >= ncols_orig) {
2232  nrows = nrows_orig;
2233  ncols = ncols_orig;
2234  } else {
2235  nrows = ncols_orig;
2236  ncols = nrows_orig;
2237  }
2238 
2239  vpMatrix U(nrows, ncols);
2240  vpMatrix V(ncols, ncols);
2241  sv.resize(ncols);
2242 
2243  if (nrows_orig >= ncols_orig)
2244  U = *this;
2245  else
2246  U = (*this).t();
2247 
2248  U.svdLapack(sv, V);
2249 
2250  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2251 
2252  return rank;
2253 }
2254 
2362 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
2363  vpMatrix &imAt, vpMatrix &kerA) const
2364 {
2365  unsigned int nrows = getRows();
2366  unsigned int ncols = getCols();
2367  unsigned int rank;
2368  vpMatrix U, V;
2369  vpColVector sv_;
2370 
2371  if (nrows < ncols) {
2372  U.resize(ncols, ncols);
2373  sv.resize(nrows);
2374  } else {
2375  U.resize(nrows, ncols);
2376  sv.resize(ncols);
2377  }
2378 
2379  U.insert(*this, 0, 0);
2380  U.svdLapack(sv_, V);
2381 
2382  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);
2383 
2384  // Remove singular values equal to to that correspond to the lines of 0
2385  // introduced when m < n
2386  for (unsigned int i = 0; i < sv.size(); i++)
2387  sv[i] = sv_[i];
2388 
2389  return rank;
2390 }
2391 #endif
2392 #if defined(VISP_HAVE_EIGEN3)
2393 
2429 vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const
2430 {
2431  unsigned int nrows, ncols;
2432  unsigned int nrows_orig = getRows();
2433  unsigned int ncols_orig = getCols();
2434 
2435  vpMatrix Ap(ncols_orig, nrows_orig);
2436 
2437  if (nrows_orig >= ncols_orig) {
2438  nrows = nrows_orig;
2439  ncols = ncols_orig;
2440  } else {
2441  nrows = ncols_orig;
2442  ncols = nrows_orig;
2443  }
2444 
2445  vpMatrix U(nrows, ncols);
2446  vpMatrix V(ncols, ncols);
2447  vpColVector sv(ncols);
2448 
2449  if (nrows_orig >= ncols_orig)
2450  U = *this;
2451  else
2452  U = (*this).t();
2453 
2454  U.svdEigen3(sv, V);
2455 
2456  unsigned int rank;
2457  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2458 
2459  return Ap;
2460 }
2461 
2502 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) const
2503 {
2504  unsigned int nrows, ncols;
2505  unsigned int nrows_orig = getRows();
2506  unsigned int ncols_orig = getCols();
2507  unsigned int rank;
2508 
2509  Ap.resize(ncols_orig, nrows_orig);
2510 
2511  if (nrows_orig >= ncols_orig) {
2512  nrows = nrows_orig;
2513  ncols = ncols_orig;
2514  } else {
2515  nrows = ncols_orig;
2516  ncols = nrows_orig;
2517  }
2518 
2519  vpMatrix U(nrows, ncols);
2520  vpMatrix V(ncols, ncols);
2521  vpColVector sv(ncols);
2522 
2523  if (nrows_orig >= ncols_orig)
2524  U = *this;
2525  else
2526  U = (*this).t();
2527 
2528  U.svdEigen3(sv, V);
2529 
2530  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2531 
2532  return rank;
2533 }
2581 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2582 {
2583  unsigned int nrows, ncols;
2584  unsigned int nrows_orig = getRows();
2585  unsigned int ncols_orig = getCols();
2586  unsigned int rank;
2587 
2588  Ap.resize(ncols_orig, nrows_orig);
2589 
2590  if (nrows_orig >= ncols_orig) {
2591  nrows = nrows_orig;
2592  ncols = ncols_orig;
2593  } else {
2594  nrows = ncols_orig;
2595  ncols = nrows_orig;
2596  }
2597 
2598  vpMatrix U(nrows, ncols);
2599  vpMatrix V(ncols, ncols);
2600  sv.resize(ncols);
2601 
2602  if (nrows_orig >= ncols_orig)
2603  U = *this;
2604  else
2605  U = (*this).t();
2606 
2607  U.svdEigen3(sv, V);
2608 
2609  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2610 
2611  return rank;
2612 }
2613 
2721 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
2722  vpMatrix &imAt, vpMatrix &kerA) const
2723 {
2724  unsigned int nrows = getRows();
2725  unsigned int ncols = getCols();
2726  unsigned int rank;
2727  vpMatrix U, V;
2728  vpColVector sv_;
2729 
2730  if (nrows < ncols) {
2731  U.resize(ncols, ncols);
2732  sv.resize(nrows);
2733  } else {
2734  U.resize(nrows, ncols);
2735  sv.resize(ncols);
2736  }
2737 
2738  U.insert(*this, 0, 0);
2739  U.svdEigen3(sv_, V);
2740 
2741  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);
2742 
2743  // Remove singular values equal to to that correspond to the lines of 0
2744  // introduced when m < n
2745  for (unsigned int i = 0; i < sv.size(); i++)
2746  sv[i] = sv_[i];
2747 
2748  return rank;
2749 }
2750 #endif
2751 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
2752 
2788 vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const
2789 {
2790  unsigned int nrows, ncols;
2791  unsigned int nrows_orig = getRows();
2792  unsigned int ncols_orig = getCols();
2793 
2794  vpMatrix Ap(ncols_orig, nrows_orig);
2795 
2796  if (nrows_orig >= ncols_orig) {
2797  nrows = nrows_orig;
2798  ncols = ncols_orig;
2799  } else {
2800  nrows = ncols_orig;
2801  ncols = nrows_orig;
2802  }
2803 
2804  vpMatrix U(nrows, ncols);
2805  vpMatrix V(ncols, ncols);
2806  vpColVector sv(ncols);
2807 
2808  if (nrows_orig >= ncols_orig)
2809  U = *this;
2810  else
2811  U = (*this).t();
2812 
2813  U.svdOpenCV(sv, V);
2814 
2815  unsigned int rank;
2816  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2817 
2818  return Ap;
2819 }
2820 
2861 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) const
2862 {
2863  unsigned int nrows, ncols;
2864  unsigned int nrows_orig = getRows();
2865  unsigned int ncols_orig = getCols();
2866  unsigned int rank;
2867 
2868  Ap.resize(ncols_orig, nrows_orig);
2869 
2870  if (nrows_orig >= ncols_orig) {
2871  nrows = nrows_orig;
2872  ncols = ncols_orig;
2873  } else {
2874  nrows = ncols_orig;
2875  ncols = nrows_orig;
2876  }
2877 
2878  vpMatrix U(nrows, ncols);
2879  vpMatrix V(ncols, ncols);
2880  vpColVector sv(ncols);
2881 
2882  if (nrows_orig >= ncols_orig)
2883  U = *this;
2884  else
2885  U = (*this).t();
2886 
2887  U.svdOpenCV(sv, V);
2888 
2889  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2890 
2891  return rank;
2892 }
2940 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2941 {
2942  unsigned int nrows, ncols;
2943  unsigned int nrows_orig = getRows();
2944  unsigned int ncols_orig = getCols();
2945  unsigned int rank;
2946 
2947  Ap.resize(ncols_orig, nrows_orig);
2948 
2949  if (nrows_orig >= ncols_orig) {
2950  nrows = nrows_orig;
2951  ncols = ncols_orig;
2952  } else {
2953  nrows = ncols_orig;
2954  ncols = nrows_orig;
2955  }
2956 
2957  vpMatrix U(nrows, ncols);
2958  vpMatrix V(ncols, ncols);
2959  sv.resize(ncols);
2960 
2961  if (nrows_orig >= ncols_orig)
2962  U = *this;
2963  else
2964  U = (*this).t();
2965 
2966  U.svdOpenCV(sv, V);
2967 
2968  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2969 
2970  return rank;
2971 }
2972 
3080 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3081  vpMatrix &imAt, vpMatrix &kerA) const
3082 {
3083  unsigned int nrows = getRows();
3084  unsigned int ncols = getCols();
3085  unsigned int rank;
3086  vpMatrix U, V;
3087  vpColVector sv_;
3088 
3089  if (nrows < ncols) {
3090  U.resize(ncols, ncols);
3091  sv.resize(nrows);
3092  } else {
3093  U.resize(nrows, ncols);
3094  sv.resize(ncols);
3095  }
3096 
3097  U.insert(*this, 0, 0);
3098  U.svdOpenCV(sv_, V);
3099 
3100  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);
3101 
3102  // Remove singular values equal to to that correspond to the lines of 0
3103  // introduced when m < n
3104  for (unsigned int i = 0; i < sv.size(); i++)
3105  sv[i] = sv_[i];
3106 
3107  return rank;
3108 }
3109 #endif
3110 #if defined(VISP_HAVE_GSL)
3111 
3147 vpMatrix vpMatrix::pseudoInverseGsl(double svThreshold) const
3148 {
3149  unsigned int nrows, ncols;
3150  unsigned int nrows_orig = getRows();
3151  unsigned int ncols_orig = getCols();
3152 
3153  vpMatrix Ap(ncols_orig, nrows_orig);
3154 
3155  if (nrows_orig >= ncols_orig) {
3156  nrows = nrows_orig;
3157  ncols = ncols_orig;
3158  } else {
3159  nrows = ncols_orig;
3160  ncols = nrows_orig;
3161  }
3162 
3163  vpMatrix U(nrows, ncols);
3164  vpMatrix V(ncols, ncols);
3165  vpColVector sv(ncols);
3166 
3167  if (nrows_orig >= ncols_orig)
3168  U = *this;
3169  else
3170  U = (*this).t();
3171 
3172  U.svdGsl(sv, V);
3173 
3174  unsigned int rank;
3175  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
3176 
3177  return Ap;
3178 }
3179 
3220 unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, double svThreshold) const
3221 {
3222  unsigned int nrows, ncols;
3223  unsigned int nrows_orig = getRows();
3224  unsigned int ncols_orig = getCols();
3225  unsigned int rank;
3226 
3227  Ap.resize(ncols_orig, nrows_orig);
3228 
3229  if (nrows_orig >= ncols_orig) {
3230  nrows = nrows_orig;
3231  ncols = ncols_orig;
3232  } else {
3233  nrows = ncols_orig;
3234  ncols = nrows_orig;
3235  }
3236 
3237  vpMatrix U(nrows, ncols);
3238  vpMatrix V(ncols, ncols);
3239  vpColVector sv(ncols);
3240 
3241  if (nrows_orig >= ncols_orig)
3242  U = *this;
3243  else
3244  U = (*this).t();
3245 
3246  U.svdGsl(sv, V);
3247 
3248  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
3249 
3250  return rank;
3251 }
3299 unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3300 {
3301  unsigned int nrows, ncols;
3302  unsigned int nrows_orig = getRows();
3303  unsigned int ncols_orig = getCols();
3304  unsigned int rank;
3305 
3306  Ap.resize(ncols_orig, nrows_orig);
3307 
3308  if (nrows_orig >= ncols_orig) {
3309  nrows = nrows_orig;
3310  ncols = ncols_orig;
3311  } else {
3312  nrows = ncols_orig;
3313  ncols = nrows_orig;
3314  }
3315 
3316  vpMatrix U(nrows, ncols);
3317  vpMatrix V(ncols, ncols);
3318  sv.resize(ncols);
3319 
3320  if (nrows_orig >= ncols_orig)
3321  U = *this;
3322  else
3323  U = (*this).t();
3324 
3325  U.svdGsl(sv, V);
3326 
3327  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
3328 
3329  return rank;
3330 }
3331 
3438 unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3439  vpMatrix &imAt, vpMatrix &kerA) const
3440 {
3441  unsigned int nrows = getRows();
3442  unsigned int ncols = getCols();
3443  unsigned int rank;
3444  vpMatrix U, V;
3445  vpColVector sv_;
3446 
3447  if (nrows < ncols) {
3448  U.resize(ncols, ncols);
3449  sv.resize(nrows);
3450  } else {
3451  U.resize(nrows, ncols);
3452  sv.resize(ncols);
3453  }
3454 
3455  U.insert(*this, 0, 0);
3456  U.svdGsl(sv_, V);
3457 
3458  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);
3459 
3460  // Remove singular values equal to to that correspond to the lines of 0
3461  // introduced when m < n
3462  for (unsigned int i = 0; i < sv.size(); i++)
3463  sv[i] = sv_[i];
3464 
3465  return rank;
3466 }
3467 #endif
3468 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
3469 
3531 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3532 {
3533 #if defined(VISP_HAVE_LAPACK)
3534  return pseudoInverseLapack(Ap, sv, svThreshold);
3535 #elif defined(VISP_HAVE_EIGEN3)
3536  return pseudoInverseEigen3(Ap, sv, svThreshold);
3537 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
3538  return pseudoInverseOpenCV(Ap, sv, svThreshold);
3539 #elif defined(VISP_HAVE_GSL)
3540  return pseudoInverseGsl(Ap, sv, svThreshold);
3541 #else
3542  (void)Ap;
3543  (void)sv;
3544  (void)svThreshold;
3545  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
3546  "Install Lapack, Eigen3, OpenCV "
3547  "or GSL 3rd party"));
3548 #endif
3549 }
3550 
3625 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3626  vpMatrix &imAt) const
3627 {
3628  vpMatrix kerAt;
3629  return pseudoInverse(Ap, sv, svThreshold, imA, imAt, kerAt);
3630 }
3631 
3766 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt,
3767  vpMatrix &kerAt) const
3768 {
3769 #if defined(VISP_HAVE_LAPACK)
3770  return pseudoInverseLapack(Ap, sv, svThreshold, imA, imAt, kerAt);
3771 #elif defined(VISP_HAVE_EIGEN3)
3772  return pseudoInverseEigen3(Ap, sv, svThreshold, imA, imAt, kerAt);
3773 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
3774  return pseudoInverseOpenCV(Ap, sv, svThreshold, imA, imAt, kerAt);
3775 #elif defined(VISP_HAVE_GSL)
3776  return pseudoInverseGsl(Ap, sv, svThreshold, imA, imAt, kerAt);
3777 #else
3778  (void)Ap;
3779  (void)sv;
3780  (void)svThreshold;
3781  (void)imA;
3782  (void)imAt;
3783  (void)kerAt;
3784  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
3785  "Install Lapack, Eigen3, OpenCV "
3786  "or GSL 3rd party"));
3787 #endif
3788 }
3789 
3831 vpColVector vpMatrix::getCol(const unsigned int j, const unsigned int i_begin, const unsigned int column_size) const
3832 {
3833  if (i_begin + column_size > getRows() || j >= getCols())
3834  throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(), getCols()));
3835  vpColVector c(column_size);
3836  for (unsigned int i = 0; i < column_size; i++)
3837  c[i] = (*this)[i_begin + i][j];
3838  return c;
3839 }
3840 
3880 vpColVector vpMatrix::getCol(const unsigned int j) const
3881 {
3882  return getCol(j, 0, rowNum);
3883 }
3884 
3920 vpRowVector vpMatrix::getRow(const unsigned int i) const
3921 {
3922  return getRow(i, 0, colNum);
3923 }
3924 
3964 vpRowVector vpMatrix::getRow(const unsigned int i, const unsigned int j_begin, const unsigned int row_size) const
3965 {
3966  if (j_begin + row_size > colNum || i >= rowNum)
3967  throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
3968 
3969  vpRowVector r(row_size);
3970  if (r.data != NULL && data != NULL) {
3971  memcpy(r.data, (*this)[i] + j_begin, row_size*sizeof(double));
3972  }
3973 
3974  return r;
3975 }
3976 
4013 {
4014  unsigned int min_size = std::min(rowNum, colNum);
4015  vpColVector diag;
4016 
4017  if (min_size > 0) {
4018  diag.resize(min_size, false);
4019 
4020  for (unsigned int i = 0; i < min_size; i++) {
4021  diag[i] = (*this)[i][i];
4022  }
4023  }
4024 
4025  return diag;
4026 }
4027 
4039 {
4040  vpMatrix C;
4041 
4042  vpMatrix::stack(A, B, C);
4043 
4044  return C;
4045 }
4046 
4058 void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
4059 {
4060  unsigned int nra = A.getRows();
4061  unsigned int nrb = B.getRows();
4062 
4063  if (nra != 0) {
4064  if (A.getCols() != B.getCols()) {
4065  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
4066  A.getCols(), B.getRows(), B.getCols()));
4067  }
4068  }
4069 
4070  if (A.data != NULL && A.data == C.data) {
4071  std::cerr << "A and C must be two different objects!" << std::endl;
4072  return;
4073  }
4074 
4075  if (B.data != NULL && B.data == C.data) {
4076  std::cerr << "B and C must be two different objects!" << std::endl;
4077  return;
4078  }
4079 
4080  C.resize(nra + nrb, B.getCols(), false, false);
4081 
4082  if (C.data != NULL && A.data != NULL && A.size() > 0) {
4083  // Copy A in C
4084  memcpy(C.data, A.data, sizeof(double) * A.size());
4085  }
4086 
4087  if (C.data != NULL && B.data != NULL && B.size() > 0) {
4088  // Copy B in C
4089  memcpy(C.data + A.size(), B.data, sizeof(double) * B.size());
4090  }
4091 }
4092 
4103 {
4104  vpMatrix C;
4105  vpMatrix::stack(A, r, C);
4106 
4107  return C;
4108 }
4109 
4121 void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
4122 {
4123  if (A.data != NULL && A.data == C.data) {
4124  std::cerr << "A and C must be two different objects!" << std::endl;
4125  return;
4126  }
4127 
4128  C = A;
4129  C.stack(r);
4130 }
4131 
4142 {
4143  vpMatrix C;
4144  vpMatrix::stack(A, c, C);
4145 
4146  return C;
4147 }
4148 
4160 void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C)
4161 {
4162  if (A.data != NULL && A.data == C.data) {
4163  std::cerr << "A and C must be two different objects!" << std::endl;
4164  return;
4165  }
4166 
4167  C = A;
4168  C.stack(c);
4169 }
4170 
4183 vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, const unsigned int r, const unsigned int c)
4184 {
4185  vpMatrix C;
4186 
4187  insert(A, B, C, r, c);
4188 
4189  return C;
4190 }
4191 
4205 void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, const unsigned int r, const unsigned int c)
4206 {
4207  if (((r + B.getRows()) <= A.getRows()) && ((c + B.getCols()) <= A.getCols())) {
4208  C.resize(A.getRows(), A.getCols(), false, false);
4209 
4210  for (unsigned int i = 0; i < A.getRows(); i++) {
4211  for (unsigned int j = 0; j < A.getCols(); j++) {
4212  if (i >= r && i < (r + B.getRows()) && j >= c && j < (c + B.getCols())) {
4213  C[i][j] = B[i - r][j - c];
4214  } else {
4215  C[i][j] = A[i][j];
4216  }
4217  }
4218  }
4219  } else {
4220  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
4221  B.getRows(), B.getCols(), A.getCols(), A.getRows(), r, c);
4222  }
4223 }
4224 
4237 {
4238  vpMatrix C;
4239 
4240  juxtaposeMatrices(A, B, C);
4241 
4242  return C;
4243 }
4244 
4258 {
4259  unsigned int nca = A.getCols();
4260  unsigned int ncb = B.getCols();
4261 
4262  if (nca != 0) {
4263  if (A.getRows() != B.getRows()) {
4264  throw(vpException(vpException::dimensionError, "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
4265  A.getCols(), B.getRows(), B.getCols()));
4266  }
4267  }
4268 
4269  if (B.getRows() == 0 || nca + ncb == 0) {
4270  std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl;
4271  return;
4272  }
4273 
4274  C.resize(B.getRows(), nca + ncb, false, false);
4275 
4276  C.insert(A, 0, 0);
4277  C.insert(B, 0, nca);
4278 }
4279 
4280 //--------------------------------------------------------------------
4281 // Output
4282 //--------------------------------------------------------------------
4283 
4303 int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &intro) const
4304 {
4305  typedef std::string::size_type size_type;
4306 
4307  unsigned int m = getRows();
4308  unsigned int n = getCols();
4309 
4310  std::vector<std::string> values(m * n);
4311  std::ostringstream oss;
4312  std::ostringstream ossFixed;
4313  std::ios_base::fmtflags original_flags = oss.flags();
4314 
4315  // ossFixed <<std::fixed;
4316  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
4317 
4318  size_type maxBefore = 0; // the length of the integral part
4319  size_type maxAfter = 0; // number of decimals plus
4320  // one place for the decimal point
4321  for (unsigned int i = 0; i < m; ++i) {
4322  for (unsigned int j = 0; j < n; ++j) {
4323  oss.str("");
4324  oss << (*this)[i][j];
4325  if (oss.str().find("e") != std::string::npos) {
4326  ossFixed.str("");
4327  ossFixed << (*this)[i][j];
4328  oss.str(ossFixed.str());
4329  }
4330 
4331  values[i * n + j] = oss.str();
4332  size_type thislen = values[i * n + j].size();
4333  size_type p = values[i * n + j].find('.');
4334 
4335  if (p == std::string::npos) {
4336  maxBefore = vpMath::maximum(maxBefore, thislen);
4337  // maxAfter remains the same
4338  } else {
4339  maxBefore = vpMath::maximum(maxBefore, p);
4340  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
4341  }
4342  }
4343  }
4344 
4345  size_type totalLength = length;
4346  // increase totalLength according to maxBefore
4347  totalLength = vpMath::maximum(totalLength, maxBefore);
4348  // decrease maxAfter according to totalLength
4349  maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
4350  if (maxAfter == 1)
4351  maxAfter = 0;
4352 
4353  // the following line is useful for debugging
4354  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
4355 
4356  if (! intro.empty())
4357  s << intro;
4358  s << "[" << m << "," << n << "]=\n";
4359 
4360  for (unsigned int i = 0; i < m; i++) {
4361  s << " ";
4362  for (unsigned int j = 0; j < n; j++) {
4363  size_type p = values[i * n + j].find('.');
4364  s.setf(std::ios::right, std::ios::adjustfield);
4365  s.width((std::streamsize)maxBefore);
4366  s << values[i * n + j].substr(0, p).c_str();
4367 
4368  if (maxAfter > 0) {
4369  s.setf(std::ios::left, std::ios::adjustfield);
4370  if (p != std::string::npos) {
4371  s.width((std::streamsize)maxAfter);
4372  s << values[i * n + j].substr(p, maxAfter).c_str();
4373  } else {
4374  assert(maxAfter > 1);
4375  s.width((std::streamsize)maxAfter);
4376  s << ".0";
4377  }
4378  }
4379 
4380  s << ' ';
4381  }
4382  s << std::endl;
4383  }
4384 
4385  s.flags(original_flags); // restore s to standard state
4386 
4387  return (int)(maxBefore + maxAfter);
4388 }
4389 
4426 std::ostream &vpMatrix::matlabPrint(std::ostream &os) const
4427 {
4428  os << "[ ";
4429  for (unsigned int i = 0; i < this->getRows(); ++i) {
4430  for (unsigned int j = 0; j < this->getCols(); ++j) {
4431  os << (*this)[i][j] << ", ";
4432  }
4433  if (this->getRows() != i + 1) {
4434  os << ";" << std::endl;
4435  } else {
4436  os << "]" << std::endl;
4437  }
4438  }
4439  return os;
4440 }
4441 
4470 std::ostream &vpMatrix::maplePrint(std::ostream &os) const
4471 {
4472  os << "([ " << std::endl;
4473  for (unsigned int i = 0; i < this->getRows(); ++i) {
4474  os << "[";
4475  for (unsigned int j = 0; j < this->getCols(); ++j) {
4476  os << (*this)[i][j] << ", ";
4477  }
4478  os << "]," << std::endl;
4479  }
4480  os << "])" << std::endl;
4481  return os;
4482 }
4483 
4511 std::ostream &vpMatrix::csvPrint(std::ostream &os) const
4512 {
4513  for (unsigned int i = 0; i < this->getRows(); ++i) {
4514  for (unsigned int j = 0; j < this->getCols(); ++j) {
4515  os << (*this)[i][j];
4516  if (!(j == (this->getCols() - 1)))
4517  os << ", ";
4518  }
4519  os << std::endl;
4520  }
4521  return os;
4522 }
4523 
4560 std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
4561 {
4562  os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " << this->getCols() << "); " << std::endl;
4563 
4564  for (unsigned int i = 0; i < this->getRows(); ++i) {
4565  for (unsigned int j = 0; j < this->getCols(); ++j) {
4566  if (!octet) {
4567  os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl;
4568  } else {
4569  for (unsigned int k = 0; k < sizeof(double); ++k) {
4570  os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex
4571  << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl;
4572  }
4573  }
4574  }
4575  os << std::endl;
4576  }
4577  return os;
4578 }
4579 
4585 {
4586  if (rowNum == 0) {
4587  *this = A;
4588  } else if (A.getRows() > 0) {
4589  if (colNum != A.getCols()) {
4590  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum,
4591  A.getRows(), A.getCols()));
4592  }
4593 
4594  unsigned int rowNumOld = rowNum;
4595  resize(rowNum + A.getRows(), colNum, false, false);
4596  insert(A, rowNumOld, 0);
4597  }
4598 }
4599 
4616 {
4617  if (rowNum == 0) {
4618  *this = r;
4619  } else {
4620  if (colNum != r.getCols()) {
4621  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum,
4622  colNum, r.getCols()));
4623  }
4624 
4625  if (r.size() == 0) {
4626  return;
4627  }
4628 
4629  unsigned int oldSize = size();
4630  resize(rowNum + 1, colNum, false, false);
4631 
4632  if (data != NULL && r.data != NULL && data != r.data) {
4633  // Copy r in data
4634  memcpy(data + oldSize, r.data, sizeof(double) * r.size());
4635  }
4636  }
4637 }
4638 
4656 {
4657  if (colNum == 0) {
4658  *this = c;
4659  } else {
4660  if (rowNum != c.getRows()) {
4661  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum,
4662  colNum, c.getRows()));
4663  }
4664 
4665  if (c.size() == 0) {
4666  return;
4667  }
4668 
4669  vpMatrix tmp = *this;
4670  unsigned int oldColNum = colNum;
4671  resize(rowNum, colNum + 1, false, false);
4672 
4673  if (data != NULL && tmp.data != NULL && data != tmp.data) {
4674  // Copy c in data
4675  for (unsigned int i = 0; i < rowNum; i++) {
4676  memcpy(data + i*colNum, tmp.data + i*oldColNum, sizeof(double) * oldColNum);
4677  rowPtrs[i][oldColNum] = c[i];
4678  }
4679  }
4680  }
4681 }
4682 
4693 void vpMatrix::insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
4694 {
4695  if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) {
4696  if (A.colNum == colNum && data != NULL && A.data != NULL && A.data != data) {
4697  memcpy(data + r * colNum, A.data, sizeof(double) * A.size());
4698  } else if (data != NULL && A.data != NULL && A.data != data) {
4699  for (unsigned int i = r; i < (r + A.getRows()); i++) {
4700  memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum);
4701  }
4702  }
4703  } else {
4704  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
4705  A.getRows(), A.getCols(), rowNum, colNum, r, c);
4706  }
4707 }
4708 
4748 {
4749  if (rowNum != colNum) {
4750  throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
4751  colNum));
4752  }
4753 
4754 #ifdef VISP_HAVE_GSL /* be careful of the copy below */
4755  {
4756  // Check if the matrix is symetric: At - A = 0
4757  vpMatrix At_A = (*this).t() - (*this);
4758  for (unsigned int i = 0; i < rowNum; i++) {
4759  for (unsigned int j = 0; j < rowNum; j++) {
4760  // if (At_A[i][j] != 0) {
4761  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
4762  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symetric matrix"));
4763  }
4764  }
4765  }
4766 
4767  vpColVector evalue(rowNum); // Eigen values
4768 
4769  gsl_vector *eval = gsl_vector_alloc(rowNum);
4770  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
4771 
4772  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
4773  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
4774 
4775  unsigned int Atda = (unsigned int)m->tda;
4776  for (unsigned int i = 0; i < rowNum; i++) {
4777  unsigned int k = i * Atda;
4778  for (unsigned int j = 0; j < colNum; j++)
4779  m->data[k + j] = (*this)[i][j];
4780  }
4781  gsl_eigen_symmv(m, eval, evec, w);
4782 
4783  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
4784 
4785  for (unsigned int i = 0; i < rowNum; i++) {
4786  evalue[i] = gsl_vector_get(eval, i);
4787  }
4788 
4789  gsl_eigen_symmv_free(w);
4790  gsl_vector_free(eval);
4791  gsl_matrix_free(m);
4792  gsl_matrix_free(evec);
4793 
4794  return evalue;
4795  }
4796 #else
4797  {
4798  throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. You "
4799  "should install GSL rd party"));
4800  }
4801 #endif
4802 }
4803 
4857 #ifdef VISP_HAVE_GSL /* be careful of the copy below */
4858 void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const
4859 #else
4860 void vpMatrix::eigenValues(vpColVector & /* evalue */, vpMatrix & /* evector */) const
4861 #endif
4862 {
4863  if (rowNum != colNum) {
4864  throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
4865  colNum));
4866  }
4867 
4868 #ifdef VISP_HAVE_GSL /* be careful of the copy below */
4869  {
4870  // Check if the matrix is symetric: At - A = 0
4871  vpMatrix At_A = (*this).t() - (*this);
4872  for (unsigned int i = 0; i < rowNum; i++) {
4873  for (unsigned int j = 0; j < rowNum; j++) {
4874  // if (At_A[i][j] != 0) {
4875  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
4876  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symetric matrix"));
4877  }
4878  }
4879  }
4880 
4881  // Resize the output matrices
4882  evalue.resize(rowNum);
4883  evector.resize(rowNum, colNum);
4884 
4885  gsl_vector *eval = gsl_vector_alloc(rowNum);
4886  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
4887 
4888  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
4889  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
4890 
4891  unsigned int Atda = (unsigned int)m->tda;
4892  for (unsigned int i = 0; i < rowNum; i++) {
4893  unsigned int k = i * Atda;
4894  for (unsigned int j = 0; j < colNum; j++)
4895  m->data[k + j] = (*this)[i][j];
4896  }
4897  gsl_eigen_symmv(m, eval, evec, w);
4898 
4899  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
4900 
4901  for (unsigned int i = 0; i < rowNum; i++) {
4902  evalue[i] = gsl_vector_get(eval, i);
4903  }
4904  Atda = (unsigned int)evec->tda;
4905  for (unsigned int i = 0; i < rowNum; i++) {
4906  unsigned int k = i * Atda;
4907  for (unsigned int j = 0; j < rowNum; j++) {
4908  evector[i][j] = evec->data[k + j];
4909  }
4910  }
4911 
4912  gsl_eigen_symmv_free(w);
4913  gsl_vector_free(eval);
4914  gsl_matrix_free(m);
4915  gsl_matrix_free(evec);
4916  }
4917 #else
4918  {
4919  throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. You "
4920  "should install GSL rd party"));
4921  }
4922 #endif
4923 }
4924 
4944 unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const
4945 {
4946  unsigned int nbline = getRows();
4947  unsigned int nbcol = getCols();
4948 
4949  vpMatrix U; // Copy of the matrix, SVD function is destructive
4950  vpColVector sv(nbcol); // singular values
4951  vpMatrix V(nbcol, nbcol); // V matrix of singular value decomposition
4952 
4953  // Copy and resize matrix to have at least as many rows as columns
4954  // kernel is computed in svd method only if the matrix has more rows than
4955  // columns
4956 
4957  if (nbline < nbcol)
4958  U.resize(nbcol, nbcol);
4959  else
4960  U.resize(nbline, nbcol);
4961 
4962  U.insert(*this, 0, 0);
4963 
4964  U.svd(sv, V);
4965 
4966  // Compute the highest singular value and rank of the matrix
4967  double maxsv = 0;
4968  for (unsigned int i = 0; i < nbcol; i++) {
4969  if (fabs(sv[i]) > maxsv) {
4970  maxsv = fabs(sv[i]);
4971  }
4972  }
4973 
4974  unsigned int rank = 0;
4975  for (unsigned int i = 0; i < nbcol; i++) {
4976  if (fabs(sv[i]) > maxsv * svThreshold) {
4977  rank++;
4978  }
4979  }
4980 
4981  kerAt.resize(nbcol - rank, nbcol);
4982  if (rank != nbcol) {
4983  for (unsigned int j = 0, k = 0; j < nbcol; j++) {
4984  // if( v.col(j) in kernel and non zero )
4985  if ((fabs(sv[j]) <= maxsv * svThreshold) &&
4986  (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
4987  for (unsigned int i = 0; i < V.getRows(); i++) {
4988  kerAt[k][i] = V[i][j];
4989  }
4990  k++;
4991  }
4992  }
4993  }
4994 
4995  return rank;
4996 }
4997 
5029 double vpMatrix::det(vpDetMethod method) const
5030 {
5031  double det = 0.;
5032 
5033  if (method == LU_DECOMPOSITION) {
5034  det = this->detByLU();
5035  }
5036 
5037  return (det);
5038 }
5039 
5048 {
5049  if (colNum != rowNum) {
5050  throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix",
5051  rowNum, colNum));
5052  } else {
5053 #ifdef VISP_HAVE_GSL
5054  size_t size_ = rowNum * colNum;
5055  double *b = new double[size_];
5056  for (size_t i = 0; i < size_; i++)
5057  b[i] = 0.;
5058  gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum);
5059  gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum);
5060  gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
5061  // gsl_matrix_fprintf(stdout, &em.matrix, "%g");
5062  vpMatrix expA(rowNum, colNum);
5063  memcpy(expA.data, b, size_ * sizeof(double));
5064 
5065  delete[] b;
5066  return expA;
5067 #else
5068  vpMatrix _expE(rowNum, colNum);
5069  vpMatrix _expD(rowNum, colNum);
5070  vpMatrix _expX(rowNum, colNum);
5071  vpMatrix _expcX(rowNum, colNum);
5072  vpMatrix _eye(rowNum, colNum);
5073 
5074  _eye.eye();
5075  vpMatrix exp(*this);
5076 
5077  // double f;
5078  int e;
5079  double c = 0.5;
5080  int q = 6;
5081  int p = 1;
5082 
5083  double nA = 0;
5084  for (unsigned int i = 0; i < rowNum; i++) {
5085  double sum = 0;
5086  for (unsigned int j = 0; j < colNum; j++) {
5087  sum += fabs((*this)[i][j]);
5088  }
5089  if (sum > nA || i == 0) {
5090  nA = sum;
5091  }
5092  }
5093 
5094  /* f = */ frexp(nA, &e);
5095  // double s = (0 > e+1)?0:e+1;
5096  double s = e + 1;
5097 
5098  double sca = 1.0 / pow(2.0, s);
5099  exp = sca * exp;
5100  _expX = *this;
5101  _expE = c * exp + _eye;
5102  _expD = -c * exp + _eye;
5103  for (int k = 2; k <= q; k++) {
5104  c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1)));
5105  _expcX = exp * _expX;
5106  _expX = _expcX;
5107  _expcX = c * _expX;
5108  _expE = _expE + _expcX;
5109  if (p)
5110  _expD = _expD + _expcX;
5111  else
5112  _expD = _expD - _expcX;
5113  p = !p;
5114  }
5115  _expX = _expD.inverseByLU();
5116  exp = _expX * _expE;
5117  for (int k = 1; k <= s; k++) {
5118  _expE = exp * exp;
5119  exp = _expE;
5120  }
5121  return exp;
5122 #endif
5123  }
5124 }
5125 
5126 /**************************************************************************************************************/
5127 /**************************************************************************************************************/
5128 
5129 // Specific functions
5130 
5131 /*
5132 input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.
5133 
5134 output:: the complement matrix of the element (rowNo,colNo).
5135 This is the matrix obtained from M after elimenating the row rowNo and column
5136 colNo
5137 
5138 example:
5139 1 2 3
5140 M = 4 5 6
5141 7 8 9
5142 1 3
5143 subblock(M, 1, 1) give the matrix 7 9
5144 */
5145 vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row)
5146 {
5147  vpMatrix M_comp(M.getRows() - 1, M.getCols() - 1);
5148 
5149  for (unsigned int i = 0; i < col; i++) {
5150  for (unsigned int j = 0; j < row; j++)
5151  M_comp[i][j] = M[i][j];
5152  for (unsigned int j = row + 1; j < M.getRows(); j++)
5153  M_comp[i][j - 1] = M[i][j];
5154  }
5155  for (unsigned int i = col + 1; i < M.getCols(); i++) {
5156  for (unsigned int j = 0; j < row; j++)
5157  M_comp[i - 1][j] = M[i][j];
5158  for (unsigned int j = row + 1; j < M.getRows(); j++)
5159  M_comp[i - 1][j - 1] = M[i][j];
5160  }
5161  return M_comp;
5162 }
5163 
5173 double vpMatrix::cond(double svThreshold) const
5174 {
5175  unsigned int nbline = getRows();
5176  unsigned int nbcol = getCols();
5177 
5178  vpMatrix U; // Copy of the matrix, SVD function is destructive
5179  vpColVector sv(nbcol); // singular values
5180  vpMatrix V(nbcol, nbcol); // V matrix of singular value decomposition
5181 
5182  // Copy and resize matrix to have at least as many rows as columns
5183  // kernel is computed in svd method only if the matrix has more rows than
5184  // columns
5185 
5186  if (nbline < nbcol)
5187  U.resize(nbcol, nbcol);
5188  else
5189  U.resize(nbline, nbcol);
5190 
5191  U.insert(*this, 0, 0);
5192 
5193  U.svd(sv, V);
5194 
5195  // Compute the highest singular value
5196  double maxsv = 0;
5197  for (unsigned int i = 0; i < nbcol; i++) {
5198  if (fabs(sv[i]) > maxsv) {
5199  maxsv = fabs(sv[i]);
5200  }
5201  }
5202 
5203  // Compute the rank of the matrix
5204  unsigned int rank = 0;
5205  for (unsigned int i = 0; i < nbcol; i++) {
5206  if (fabs(sv[i]) > maxsv * svThreshold) {
5207  rank++;
5208  }
5209  }
5210 
5211  // Compute the lowest singular value
5212  double minsv = maxsv;
5213  for (unsigned int i = 0; i < rank; i++) {
5214  if (fabs(sv[i]) < minsv) {
5215  minsv = fabs(sv[i]);
5216  }
5217  }
5218 
5219  if (std::fabs(minsv) > std::numeric_limits<double>::epsilon()) {
5220  return maxsv / minsv;
5221  }
5222  else {
5223  return std::numeric_limits<double>::infinity();
5224  }
5225 }
5226 
5233 void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
5234 {
5235  if (H.getCols() != H.getRows()) {
5236  throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(),
5237  H.getCols()));
5238  }
5239  HLM.resize(H.getRows(), H.getCols(), false, false);
5240 
5241  for (unsigned int i = 0; i < H.getCols(); i++) {
5242  for (unsigned int j = 0; j < H.getCols(); j++) {
5243  HLM[i][j] = H[i][j];
5244  if (i == j)
5245  HLM[i][j] += alpha * H[i][j];
5246  }
5247  }
5248 }
5249 
5259 vp_deprecated double vpMatrix::euclideanNorm() const
5260 {
5261  return frobeniusNorm();
5262 }
5263 
5272 {
5273  double norm = 0.0;
5274  for (unsigned int i = 0; i < dsize; i++) {
5275  double x = *(data + i);
5276  norm += x * x;
5277  }
5278 
5279  return sqrt(norm);
5280 }
5281 
5291 {
5292  if(this->dsize != 0){
5293  vpMatrix v;
5294  vpColVector w;
5295 
5296  vpMatrix M = *this;
5297 
5298  M.svd(w, v);
5299 
5300  double max = w[0];
5301  unsigned int maxRank = std::min(this->getCols(), this->getRows());
5302  // The maximum reachable rank is either the number of columns or the number of rows
5303  // of the matrix.
5304  unsigned int boundary = std::min(maxRank, w.size());
5305  // boundary is here to ensure that the number of singular values used for the com-
5306  // putation of the euclidean norm of the matrix is not greater than the maximum
5307  // reachable rank. Indeed, some svd library pad the singular values vector with 0s
5308  // if the input matrix is non-square.
5309  for (unsigned int i = 0; i < boundary; i++) {
5310  if (max < w[i]) {
5311  max = w[i];
5312  }
5313  }
5314  return max;
5315  }
5316  else {
5317  return 0.;
5318  }
5319 }
5320 
5332 {
5333  double norm = 0.0;
5334  for (unsigned int i = 0; i < rowNum; i++) {
5335  double x = 0;
5336  for (unsigned int j = 0; j < colNum; j++) {
5337  x += fabs(*(*(rowPtrs + i) + j));
5338  }
5339  if (x > norm) {
5340  norm = x;
5341  }
5342  }
5343  return norm;
5344 }
5345 
5352 double vpMatrix::sumSquare() const
5353 {
5354  double sum_square = 0.0;
5355  double x;
5356 
5357  for (unsigned int i = 0; i < rowNum; i++) {
5358  for (unsigned int j = 0; j < colNum; j++) {
5359  x = rowPtrs[i][j];
5360  sum_square += x * x;
5361  }
5362  }
5363 
5364  return sum_square;
5365 }
5366 
5378 vpMatrix vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode)
5379 {
5380  vpMatrix res;
5381  conv2(M, kernel, res, mode);
5382  return res;
5383 }
5384 
5397 void vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, vpMatrix &res, const std::string &mode)
5398 {
5399  if (M.getRows()*M.getCols() == 0 || kernel.getRows()*kernel.getCols() == 0)
5400  return;
5401 
5402  if (mode == "valid") {
5403  if (kernel.getRows() > M.getRows() || kernel.getCols() > M.getCols())
5404  return;
5405  }
5406 
5407  vpMatrix M_padded, res_same;
5408 
5409  if (mode == "full" || mode == "same") {
5410  const unsigned int pad_x = kernel.getCols()-1;
5411  const unsigned int pad_y = kernel.getRows()-1;
5412  M_padded.resize(M.getRows() + 2*pad_y, M.getCols() + 2*pad_x, true, false);
5413  M_padded.insert(M, pad_y, pad_x);
5414 
5415  if (mode == "same") {
5416  res.resize(M.getRows(), M.getCols(), false, false);
5417  res_same.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
5418  } else {
5419  res.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
5420  }
5421  } else if (mode == "valid") {
5422  M_padded = M;
5423  res.resize(M.getRows()-kernel.getRows()+1, M.getCols()-kernel.getCols()+1);
5424  } else {
5425  return;
5426  }
5427 
5428  if (mode == "same") {
5429  for (unsigned int i = 0; i < res_same.getRows(); i++) {
5430  for (unsigned int j = 0; j < res_same.getCols(); j++) {
5431  for (unsigned int k = 0; k < kernel.getRows(); k++) {
5432  for (unsigned int l = 0; l < kernel.getCols(); l++) {
5433  res_same[i][j] += M_padded[i+k][j+l] * kernel[kernel.getRows()-k-1][kernel.getCols()-l-1];
5434  }
5435  }
5436  }
5437  }
5438 
5439  const unsigned int start_i = kernel.getRows()/2;
5440  const unsigned int start_j = kernel.getCols()/2;
5441  for (unsigned int i = 0; i < M.getRows(); i++) {
5442  memcpy(res.data + i*M.getCols(), res_same.data + (i+start_i)*res_same.getCols() + start_j, sizeof(double)*M.getCols());
5443  }
5444  } else {
5445  for (unsigned int i = 0; i < res.getRows(); i++) {
5446  for (unsigned int j = 0; j < res.getCols(); j++) {
5447  for (unsigned int k = 0; k < kernel.getRows(); k++) {
5448  for (unsigned int l = 0; l < kernel.getCols(); l++) {
5449  res[i][j] += M_padded[i+k][j+l] * kernel[kernel.getRows()-k-1][kernel.getCols()-l-1];
5450  }
5451  }
5452  }
5453  }
5454  }
5455 }
5456 
5457 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
5459 {
5460  return (vpMatrix)(vpColVector::stack(A, B));
5461 }
5462 
5464 {
5465  vpColVector::stack(A, B, C);
5466 }
5467 
5469 
5470 void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); }
5471 
5490 vpRowVector vpMatrix::row(const unsigned int i)
5491 {
5492  vpRowVector c(getCols());
5493 
5494  for (unsigned int j = 0; j < getCols(); j++)
5495  c[j] = (*this)[i - 1][j];
5496  return c;
5497 }
5498 
5516 vpColVector vpMatrix::column(const unsigned int j)
5517 {
5518  vpColVector c(getRows());
5519 
5520  for (unsigned int i = 0; i < getRows(); i++)
5521  c[i] = (*this)[i][j - 1];
5522  return c;
5523 }
5524 
5531 void vpMatrix::setIdentity(const double &val)
5532 {
5533  for (unsigned int i = 0; i < rowNum; i++)
5534  for (unsigned int j = 0; j < colNum; j++)
5535  if (i == j)
5536  (*this)[i][j] = val;
5537  else
5538  (*this)[i][j] = 0;
5539 }
5540 
5541 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:1751
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:1874
vpMatrix operator*(const double &x, const vpMatrix &B)
Definition: vpMatrix.cpp:1439
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:319
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2014
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:4236
vpRowVector getRow(const unsigned int i) const
Definition: vpMatrix.cpp:3920
vpColVector getDiag() const
Definition: vpMatrix.cpp:4012
vp_deprecated vpRowVector row(const unsigned int i)
Definition: vpMatrix.cpp:5490
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:5029
static vpMatrix conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode="full")
Definition: vpMatrix.cpp:5378
void kron(const vpMatrix &m1, vpMatrix &out) const
Definition: vpMatrix.cpp:1661
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpMatrix operator-() const
Definition: vpMatrix.cpp:1408
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:115
std::ostream & csvPrint(std::ostream &os) const
Definition: vpMatrix.cpp:4511
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1202
vpMatrix AtA() const
Definition: vpMatrix.cpp:524
double sum() const
Definition: vpMatrix.cpp:1415
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:1513
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4584
vpMatrix inverseByLU() const
vpMatrix & operator/=(double x)
Divide all the element of the matrix by x : Aij = Aij / x.
Definition: vpMatrix.cpp:1523
error that can be emited by ViSP classes.
Definition: vpException.h:71
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:837
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:1315
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:1349
vpMatrix()
Definition: vpMatrix.h:180
vp_deprecated vpColVector column(const unsigned int j)
Definition: vpMatrix.cpp:5516
double infinityNorm() const
Definition: vpMatrix.cpp:5331
void svdLapack(vpColVector &w, vpMatrix &V)
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:5173
vpMatrix & operator-=(const vpMatrix &B)
Operation A = A - B.
Definition: vpMatrix.cpp:1366
vpMatrix operator+(const vpMatrix &B) const
Definition: vpMatrix.cpp:1260
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:4470
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
double sumSquare() const
Definition: vpMatrix.cpp:5352
vpMatrix hadamard(const vpMatrix &m) const
Definition: vpMatrix.cpp:1598
vpMatrix t() const
Definition: vpMatrix.cpp:375
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
Definition: vpMatrix.cpp:4693
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:5531
std::ostream & matlabPrint(std::ostream &os) const
Definition: vpMatrix.cpp:4426
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:4944
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:782
vp_deprecated void init()
Definition: vpMatrix.h:832
static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C)
Definition: vpMatrix.cpp:1173
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:854
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:895
std::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
Definition: vpMatrix.cpp:4560
vpColVector eigenValues() const
Definition: vpMatrix.cpp:4747
double euclideanNorm() const
Definition: vpMatrix.cpp:5259
void svdOpenCV(vpColVector &w, vpMatrix &V)
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:4303
vpMatrix AAt() const
Definition: vpMatrix.cpp:426
vpMatrix transpose() const
Definition: vpMatrix.cpp:394
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:1469
double inducedL2Norm() const
Definition: vpMatrix.cpp:5290
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:1030
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
vpRowVector stackRows()
Definition: vpMatrix.cpp:1585
double sumSquare() const
static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
Definition: vpMatrix.cpp:801
vpColVector stackColumns()
Definition: vpMatrix.cpp:1563
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:1390
vpColVector getCol(const unsigned int j) const
Definition: vpMatrix.cpp:3880
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:5233
vpMatrix & operator,(double val)
Definition: vpMatrix.cpp:700
double frobeniusNorm() const
Definition: vpMatrix.cpp:5271
Function not implemented.
Definition: vpException.h:90
Class that consider the case of a translation vector.
void eye()
Definition: vpMatrix.cpp:360
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:5047
vpMatrix & operator=(const vpArray2D< double > &A)
Definition: vpMatrix.cpp:549
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310
vpMatrix & operator<<(double *)
Definition: vpMatrix.cpp:683