Visual Servoing Platform  version 3.2.1 under development (2019-07-18)
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  double *mdata = data;
1580  double *optr = out.data;
1581  for (unsigned int i = 0; i < dsize; i++) {
1582  *(optr++) = *(mdata++);
1583  }
1584 }
1590 {
1591  vpRowVector out(colNum * rowNum);
1592  stackRows(out);
1593  return out;
1594 }
1595 
1603 {
1604  if (m.getRows() != rowNum || m.getCols() != colNum) {
1605  throw(vpException(vpException::dimensionError, "In Hadamard product: bad dimension of input matrix"));
1606  }
1607 
1608  vpMatrix out;
1609  out.resize(rowNum, colNum, false);
1610 
1611  unsigned int i = 0;
1612 
1613 #if VISP_HAVE_SSE2
1614  if (vpCPUFeatures::checkSSE2() && dsize >= 2) {
1615  for (; i <= dsize - 2; i += 2) {
1616  __m128d vout = _mm_mul_pd(_mm_loadu_pd(data + i), _mm_loadu_pd(m.data + i));
1617  _mm_storeu_pd(out.data + i, vout);
1618  }
1619  }
1620 #endif
1621 
1622  for (; i < dsize; i++) {
1623  out.data[i] = data[i] * m.data[i];
1624  }
1625 
1626  return out;
1627 }
1628 
1635 void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out)
1636 {
1637  unsigned int r1 = m1.getRows();
1638  unsigned int c1 = m1.getCols();
1639  unsigned int r2 = m2.getRows();
1640  unsigned int c2 = m2.getCols();
1641 
1642  if (r1 * r2 != out.rowNum || c1 * c2 != out.colNum) {
1643  vpERROR_TRACE("Kronecker prodect bad dimension of output vpMatrix");
1644  throw(vpException(vpException::dimensionError, "In Kronecker product bad dimension of output matrix"));
1645  }
1646 
1647  for (unsigned int r = 0; r < r1; r++) {
1648  for (unsigned int c = 0; c < c1; c++) {
1649  double alpha = m1[r][c];
1650  double *m2ptr = m2[0];
1651  unsigned int roffset = r * r2;
1652  unsigned int coffset = c * c2;
1653  for (unsigned int rr = 0; rr < r2; rr++) {
1654  for (unsigned int cc = 0; cc < c2; cc++) {
1655  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1656  }
1657  }
1658  }
1659  }
1660 }
1661 
1668 void vpMatrix::kron(const vpMatrix &m, vpMatrix &out) const { kron(*this, m, out); }
1669 
1677 {
1678  unsigned int r1 = m1.getRows();
1679  unsigned int c1 = m1.getCols();
1680  unsigned int r2 = m2.getRows();
1681  unsigned int c2 = m2.getCols();
1682 
1683  vpMatrix out(r1 * r2, c1 * c2);
1684 
1685  for (unsigned int r = 0; r < r1; r++) {
1686  for (unsigned int c = 0; c < c1; c++) {
1687  double alpha = m1[r][c];
1688  double *m2ptr = m2[0];
1689  unsigned int roffset = r * r2;
1690  unsigned int coffset = c * c2;
1691  for (unsigned int rr = 0; rr < r2; rr++) {
1692  for (unsigned int cc = 0; cc < c2; cc++) {
1693  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
1694  }
1695  }
1696  }
1697  }
1698  return out;
1699 }
1700 
1706 vpMatrix vpMatrix::kron(const vpMatrix &m) const { return kron(*this, m); }
1707 
1758 void vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const { x = pseudoInverse(1e-6) * b; }
1759 
1810 {
1811  vpColVector X(colNum);
1812 
1813  solveBySVD(B, X);
1814  return X;
1815 }
1816 
1882 {
1883 #if defined(VISP_HAVE_LAPACK)
1884  svdLapack(w, V);
1885 #elif defined(VISP_HAVE_EIGEN3)
1886  svdEigen3(w, V);
1887 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
1888  svdOpenCV(w, V);
1889 #elif defined(VISP_HAVE_GSL)
1890  svdGsl(w, V);
1891 #else
1892  (void)w;
1893  (void)V;
1894  throw(vpException(vpException::fatalError, "Cannot compute SVD. Install Lapack, Eigen3, OpenCV or GSL 3rd party"));
1895 #endif
1896 }
1897 
1952 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
1953 {
1954 #if defined(VISP_HAVE_LAPACK)
1955  return pseudoInverseLapack(Ap, svThreshold);
1956 #elif defined(VISP_HAVE_EIGEN3)
1957  return pseudoInverseEigen3(Ap, svThreshold);
1958 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
1959  return pseudoInverseOpenCV(Ap, svThreshold);
1960 #elif defined(VISP_HAVE_GSL)
1961  return pseudoInverseGsl(Ap, svThreshold);
1962 #else
1963  (void)Ap;
1964  (void)svThreshold;
1965  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
1966  "Install Lapack, Eigen3, OpenCV "
1967  "or GSL 3rd party"));
1968 #endif
1969 }
1970 
2021 vpMatrix vpMatrix::pseudoInverse(double svThreshold) const
2022 {
2023 #if defined(VISP_HAVE_LAPACK)
2024  return pseudoInverseLapack(svThreshold);
2025 #elif defined(VISP_HAVE_EIGEN3)
2026  return pseudoInverseEigen3(svThreshold);
2027 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2028  return pseudoInverseOpenCV(svThreshold);
2029 #elif defined(VISP_HAVE_GSL)
2030  return pseudoInverseGsl(svThreshold);
2031 #else
2032  (void)svThreshold;
2033  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2034  "Install Lapack, Eigen3, OpenCV "
2035  "or GSL 3rd party"));
2036 #endif
2037 }
2038 
2039 #ifndef DOXYGEN_SHOULD_SKIP_THIS
2040 #if defined(VISP_HAVE_LAPACK)
2041 
2077 vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const
2078 {
2079  unsigned int nrows, ncols;
2080  unsigned int nrows_orig = getRows();
2081  unsigned int ncols_orig = getCols();
2082 
2083  vpMatrix Ap(ncols_orig, nrows_orig);
2084 
2085  if (nrows_orig >= ncols_orig) {
2086  nrows = nrows_orig;
2087  ncols = ncols_orig;
2088  } else {
2089  nrows = ncols_orig;
2090  ncols = nrows_orig;
2091  }
2092 
2093  vpMatrix U(nrows, ncols);
2094  vpMatrix V(ncols, ncols);
2095  vpColVector sv(ncols);
2096 
2097  if (nrows_orig >= ncols_orig)
2098  U = *this;
2099  else
2100  U = (*this).t();
2101 
2102  U.svdLapack(sv, V);
2103 
2104  unsigned int rank;
2105  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2106 
2107  return Ap;
2108 }
2109 
2150 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) const
2151 {
2152  unsigned int nrows, ncols;
2153  unsigned int nrows_orig = getRows();
2154  unsigned int ncols_orig = getCols();
2155  unsigned int rank;
2156 
2157  Ap.resize(ncols_orig, nrows_orig);
2158 
2159  if (nrows_orig >= ncols_orig) {
2160  nrows = nrows_orig;
2161  ncols = ncols_orig;
2162  } else {
2163  nrows = ncols_orig;
2164  ncols = nrows_orig;
2165  }
2166 
2167  vpMatrix U(nrows, ncols);
2168  vpMatrix V(ncols, ncols);
2169  vpColVector sv(ncols);
2170 
2171  if (nrows_orig >= ncols_orig)
2172  U = *this;
2173  else
2174  U = (*this).t();
2175 
2176  U.svdLapack(sv, V);
2177 
2178  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2179 
2180  return rank;
2181 }
2229 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2230 {
2231  unsigned int nrows, ncols;
2232  unsigned int nrows_orig = getRows();
2233  unsigned int ncols_orig = getCols();
2234  unsigned int rank;
2235 
2236  Ap.resize(ncols_orig, nrows_orig);
2237 
2238  if (nrows_orig >= ncols_orig) {
2239  nrows = nrows_orig;
2240  ncols = ncols_orig;
2241  } else {
2242  nrows = ncols_orig;
2243  ncols = nrows_orig;
2244  }
2245 
2246  vpMatrix U(nrows, ncols);
2247  vpMatrix V(ncols, ncols);
2248  sv.resize(ncols);
2249 
2250  if (nrows_orig >= ncols_orig)
2251  U = *this;
2252  else
2253  U = (*this).t();
2254 
2255  U.svdLapack(sv, V);
2256 
2257  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2258 
2259  return rank;
2260 }
2261 
2369 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
2370  vpMatrix &imAt, vpMatrix &kerA) const
2371 {
2372  unsigned int nrows = getRows();
2373  unsigned int ncols = getCols();
2374  unsigned int rank;
2375  vpMatrix U, V;
2376  vpColVector sv_;
2377 
2378  if (nrows < ncols) {
2379  U.resize(ncols, ncols);
2380  sv.resize(nrows);
2381  } else {
2382  U.resize(nrows, ncols);
2383  sv.resize(ncols);
2384  }
2385 
2386  U.insert(*this, 0, 0);
2387  U.svdLapack(sv_, V);
2388 
2389  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);
2390 
2391  // Remove singular values equal to to that correspond to the lines of 0
2392  // introduced when m < n
2393  for (unsigned int i = 0; i < sv.size(); i++)
2394  sv[i] = sv_[i];
2395 
2396  return rank;
2397 }
2398 #endif
2399 #if defined(VISP_HAVE_EIGEN3)
2400 
2436 vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const
2437 {
2438  unsigned int nrows, ncols;
2439  unsigned int nrows_orig = getRows();
2440  unsigned int ncols_orig = getCols();
2441 
2442  vpMatrix Ap(ncols_orig, nrows_orig);
2443 
2444  if (nrows_orig >= ncols_orig) {
2445  nrows = nrows_orig;
2446  ncols = ncols_orig;
2447  } else {
2448  nrows = ncols_orig;
2449  ncols = nrows_orig;
2450  }
2451 
2452  vpMatrix U(nrows, ncols);
2453  vpMatrix V(ncols, ncols);
2454  vpColVector sv(ncols);
2455 
2456  if (nrows_orig >= ncols_orig)
2457  U = *this;
2458  else
2459  U = (*this).t();
2460 
2461  U.svdEigen3(sv, V);
2462 
2463  unsigned int rank;
2464  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2465 
2466  return Ap;
2467 }
2468 
2509 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) const
2510 {
2511  unsigned int nrows, ncols;
2512  unsigned int nrows_orig = getRows();
2513  unsigned int ncols_orig = getCols();
2514  unsigned int rank;
2515 
2516  Ap.resize(ncols_orig, nrows_orig);
2517 
2518  if (nrows_orig >= ncols_orig) {
2519  nrows = nrows_orig;
2520  ncols = ncols_orig;
2521  } else {
2522  nrows = ncols_orig;
2523  ncols = nrows_orig;
2524  }
2525 
2526  vpMatrix U(nrows, ncols);
2527  vpMatrix V(ncols, ncols);
2528  vpColVector sv(ncols);
2529 
2530  if (nrows_orig >= ncols_orig)
2531  U = *this;
2532  else
2533  U = (*this).t();
2534 
2535  U.svdEigen3(sv, V);
2536 
2537  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2538 
2539  return rank;
2540 }
2588 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2589 {
2590  unsigned int nrows, ncols;
2591  unsigned int nrows_orig = getRows();
2592  unsigned int ncols_orig = getCols();
2593  unsigned int rank;
2594 
2595  Ap.resize(ncols_orig, nrows_orig);
2596 
2597  if (nrows_orig >= ncols_orig) {
2598  nrows = nrows_orig;
2599  ncols = ncols_orig;
2600  } else {
2601  nrows = ncols_orig;
2602  ncols = nrows_orig;
2603  }
2604 
2605  vpMatrix U(nrows, ncols);
2606  vpMatrix V(ncols, ncols);
2607  sv.resize(ncols);
2608 
2609  if (nrows_orig >= ncols_orig)
2610  U = *this;
2611  else
2612  U = (*this).t();
2613 
2614  U.svdEigen3(sv, V);
2615 
2616  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2617 
2618  return rank;
2619 }
2620 
2728 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
2729  vpMatrix &imAt, vpMatrix &kerA) const
2730 {
2731  unsigned int nrows = getRows();
2732  unsigned int ncols = getCols();
2733  unsigned int rank;
2734  vpMatrix U, V;
2735  vpColVector sv_;
2736 
2737  if (nrows < ncols) {
2738  U.resize(ncols, ncols);
2739  sv.resize(nrows);
2740  } else {
2741  U.resize(nrows, ncols);
2742  sv.resize(ncols);
2743  }
2744 
2745  U.insert(*this, 0, 0);
2746  U.svdEigen3(sv_, V);
2747 
2748  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);
2749 
2750  // Remove singular values equal to to that correspond to the lines of 0
2751  // introduced when m < n
2752  for (unsigned int i = 0; i < sv.size(); i++)
2753  sv[i] = sv_[i];
2754 
2755  return rank;
2756 }
2757 #endif
2758 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
2759 
2795 vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const
2796 {
2797  unsigned int nrows, ncols;
2798  unsigned int nrows_orig = getRows();
2799  unsigned int ncols_orig = getCols();
2800 
2801  vpMatrix Ap(ncols_orig, nrows_orig);
2802 
2803  if (nrows_orig >= ncols_orig) {
2804  nrows = nrows_orig;
2805  ncols = ncols_orig;
2806  } else {
2807  nrows = ncols_orig;
2808  ncols = nrows_orig;
2809  }
2810 
2811  vpMatrix U(nrows, ncols);
2812  vpMatrix V(ncols, ncols);
2813  vpColVector sv(ncols);
2814 
2815  if (nrows_orig >= ncols_orig)
2816  U = *this;
2817  else
2818  U = (*this).t();
2819 
2820  U.svdOpenCV(sv, V);
2821 
2822  unsigned int rank;
2823  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2824 
2825  return Ap;
2826 }
2827 
2868 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) const
2869 {
2870  unsigned int nrows, ncols;
2871  unsigned int nrows_orig = getRows();
2872  unsigned int ncols_orig = getCols();
2873  unsigned int rank;
2874 
2875  Ap.resize(ncols_orig, nrows_orig);
2876 
2877  if (nrows_orig >= ncols_orig) {
2878  nrows = nrows_orig;
2879  ncols = ncols_orig;
2880  } else {
2881  nrows = ncols_orig;
2882  ncols = nrows_orig;
2883  }
2884 
2885  vpMatrix U(nrows, ncols);
2886  vpMatrix V(ncols, ncols);
2887  vpColVector sv(ncols);
2888 
2889  if (nrows_orig >= ncols_orig)
2890  U = *this;
2891  else
2892  U = (*this).t();
2893 
2894  U.svdOpenCV(sv, V);
2895 
2896  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2897 
2898  return rank;
2899 }
2947 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2948 {
2949  unsigned int nrows, ncols;
2950  unsigned int nrows_orig = getRows();
2951  unsigned int ncols_orig = getCols();
2952  unsigned int rank;
2953 
2954  Ap.resize(ncols_orig, nrows_orig);
2955 
2956  if (nrows_orig >= ncols_orig) {
2957  nrows = nrows_orig;
2958  ncols = ncols_orig;
2959  } else {
2960  nrows = ncols_orig;
2961  ncols = nrows_orig;
2962  }
2963 
2964  vpMatrix U(nrows, ncols);
2965  vpMatrix V(ncols, ncols);
2966  sv.resize(ncols);
2967 
2968  if (nrows_orig >= ncols_orig)
2969  U = *this;
2970  else
2971  U = (*this).t();
2972 
2973  U.svdOpenCV(sv, V);
2974 
2975  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
2976 
2977  return rank;
2978 }
2979 
3087 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3088  vpMatrix &imAt, vpMatrix &kerA) const
3089 {
3090  unsigned int nrows = getRows();
3091  unsigned int ncols = getCols();
3092  unsigned int rank;
3093  vpMatrix U, V;
3094  vpColVector sv_;
3095 
3096  if (nrows < ncols) {
3097  U.resize(ncols, ncols);
3098  sv.resize(nrows);
3099  } else {
3100  U.resize(nrows, ncols);
3101  sv.resize(ncols);
3102  }
3103 
3104  U.insert(*this, 0, 0);
3105  U.svdOpenCV(sv_, V);
3106 
3107  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);
3108 
3109  // Remove singular values equal to to that correspond to the lines of 0
3110  // introduced when m < n
3111  for (unsigned int i = 0; i < sv.size(); i++)
3112  sv[i] = sv_[i];
3113 
3114  return rank;
3115 }
3116 #endif
3117 #if defined(VISP_HAVE_GSL)
3118 
3154 vpMatrix vpMatrix::pseudoInverseGsl(double svThreshold) const
3155 {
3156  unsigned int nrows, ncols;
3157  unsigned int nrows_orig = getRows();
3158  unsigned int ncols_orig = getCols();
3159 
3160  vpMatrix Ap(ncols_orig, nrows_orig);
3161 
3162  if (nrows_orig >= ncols_orig) {
3163  nrows = nrows_orig;
3164  ncols = ncols_orig;
3165  } else {
3166  nrows = ncols_orig;
3167  ncols = nrows_orig;
3168  }
3169 
3170  vpMatrix U(nrows, ncols);
3171  vpMatrix V(ncols, ncols);
3172  vpColVector sv(ncols);
3173 
3174  if (nrows_orig >= ncols_orig)
3175  U = *this;
3176  else
3177  U = (*this).t();
3178 
3179  U.svdGsl(sv, V);
3180 
3181  unsigned int rank;
3182  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
3183 
3184  return Ap;
3185 }
3186 
3227 unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, double svThreshold) const
3228 {
3229  unsigned int nrows, ncols;
3230  unsigned int nrows_orig = getRows();
3231  unsigned int ncols_orig = getCols();
3232  unsigned int rank;
3233 
3234  Ap.resize(ncols_orig, nrows_orig);
3235 
3236  if (nrows_orig >= ncols_orig) {
3237  nrows = nrows_orig;
3238  ncols = ncols_orig;
3239  } else {
3240  nrows = ncols_orig;
3241  ncols = nrows_orig;
3242  }
3243 
3244  vpMatrix U(nrows, ncols);
3245  vpMatrix V(ncols, ncols);
3246  vpColVector sv(ncols);
3247 
3248  if (nrows_orig >= ncols_orig)
3249  U = *this;
3250  else
3251  U = (*this).t();
3252 
3253  U.svdGsl(sv, V);
3254 
3255  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
3256 
3257  return rank;
3258 }
3306 unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3307 {
3308  unsigned int nrows, ncols;
3309  unsigned int nrows_orig = getRows();
3310  unsigned int ncols_orig = getCols();
3311  unsigned int rank;
3312 
3313  Ap.resize(ncols_orig, nrows_orig);
3314 
3315  if (nrows_orig >= ncols_orig) {
3316  nrows = nrows_orig;
3317  ncols = ncols_orig;
3318  } else {
3319  nrows = ncols_orig;
3320  ncols = nrows_orig;
3321  }
3322 
3323  vpMatrix U(nrows, ncols);
3324  vpMatrix V(ncols, ncols);
3325  sv.resize(ncols);
3326 
3327  if (nrows_orig >= ncols_orig)
3328  U = *this;
3329  else
3330  U = (*this).t();
3331 
3332  U.svdGsl(sv, V);
3333 
3334  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
3335 
3336  return rank;
3337 }
3338 
3445 unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3446  vpMatrix &imAt, vpMatrix &kerA) const
3447 {
3448  unsigned int nrows = getRows();
3449  unsigned int ncols = getCols();
3450  unsigned int rank;
3451  vpMatrix U, V;
3452  vpColVector sv_;
3453 
3454  if (nrows < ncols) {
3455  U.resize(ncols, ncols);
3456  sv.resize(nrows);
3457  } else {
3458  U.resize(nrows, ncols);
3459  sv.resize(ncols);
3460  }
3461 
3462  U.insert(*this, 0, 0);
3463  U.svdGsl(sv_, V);
3464 
3465  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);
3466 
3467  // Remove singular values equal to to that correspond to the lines of 0
3468  // introduced when m < n
3469  for (unsigned int i = 0; i < sv.size(); i++)
3470  sv[i] = sv_[i];
3471 
3472  return rank;
3473 }
3474 #endif
3475 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
3476 
3538 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3539 {
3540 #if defined(VISP_HAVE_LAPACK)
3541  return pseudoInverseLapack(Ap, sv, svThreshold);
3542 #elif defined(VISP_HAVE_EIGEN3)
3543  return pseudoInverseEigen3(Ap, sv, svThreshold);
3544 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
3545  return pseudoInverseOpenCV(Ap, sv, svThreshold);
3546 #elif defined(VISP_HAVE_GSL)
3547  return pseudoInverseGsl(Ap, sv, svThreshold);
3548 #else
3549  (void)Ap;
3550  (void)sv;
3551  (void)svThreshold;
3552  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
3553  "Install Lapack, Eigen3, OpenCV "
3554  "or GSL 3rd party"));
3555 #endif
3556 }
3557 
3632 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3633  vpMatrix &imAt) const
3634 {
3635  vpMatrix kerAt;
3636  return pseudoInverse(Ap, sv, svThreshold, imA, imAt, kerAt);
3637 }
3638 
3773 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt,
3774  vpMatrix &kerAt) const
3775 {
3776 #if defined(VISP_HAVE_LAPACK)
3777  return pseudoInverseLapack(Ap, sv, svThreshold, imA, imAt, kerAt);
3778 #elif defined(VISP_HAVE_EIGEN3)
3779  return pseudoInverseEigen3(Ap, sv, svThreshold, imA, imAt, kerAt);
3780 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
3781  return pseudoInverseOpenCV(Ap, sv, svThreshold, imA, imAt, kerAt);
3782 #elif defined(VISP_HAVE_GSL)
3783  return pseudoInverseGsl(Ap, sv, svThreshold, imA, imAt, kerAt);
3784 #else
3785  (void)Ap;
3786  (void)sv;
3787  (void)svThreshold;
3788  (void)imA;
3789  (void)imAt;
3790  (void)kerAt;
3791  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
3792  "Install Lapack, Eigen3, OpenCV "
3793  "or GSL 3rd party"));
3794 #endif
3795 }
3796 
3838 vpColVector vpMatrix::getCol(const unsigned int j, const unsigned int i_begin, const unsigned int column_size) const
3839 {
3840  if (i_begin + column_size > getRows() || j >= getCols())
3841  throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(), getCols()));
3842  vpColVector c(column_size);
3843  for (unsigned int i = 0; i < column_size; i++)
3844  c[i] = (*this)[i_begin + i][j];
3845  return c;
3846 }
3847 
3887 vpColVector vpMatrix::getCol(const unsigned int j) const
3888 {
3889  if (j >= getCols())
3890  throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(), getCols()));
3891  unsigned int nb_rows = getRows();
3892  vpColVector c(nb_rows);
3893  for (unsigned int i = 0; i < nb_rows; i++)
3894  c[i] = (*this)[i][j];
3895  return c;
3896 }
3897 
3933 vpRowVector vpMatrix::getRow(const unsigned int i) const
3934 {
3935  if (i >= getRows())
3936  throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
3937 
3938  vpRowVector r;
3939  r.resize(colNum, false);
3940 
3941  if (r.data != NULL && data != NULL && r.data != data) {
3942  memcpy(r.data, data + i * colNum, sizeof(double) * colNum);
3943  }
3944 
3945  return r;
3946 }
3947 
3985 vpRowVector vpMatrix::getRow(const unsigned int i, const unsigned int j_begin, const unsigned int row_size) const
3986 {
3987  if (j_begin + row_size > getCols() || i >= getRows())
3988  throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
3989  vpRowVector r(row_size);
3990  for (unsigned int j = 0; j < row_size; j++)
3991  r[j] = (*this)[i][j_begin + i];
3992  return r;
3993 }
3994 
4006 {
4007  vpMatrix C;
4008 
4009  vpMatrix::stack(A, B, C);
4010 
4011  return C;
4012 }
4013 
4025 void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
4026 {
4027  unsigned int nra = A.getRows();
4028  unsigned int nrb = B.getRows();
4029 
4030  if (nra != 0) {
4031  if (A.getCols() != B.getCols()) {
4032  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
4033  A.getCols(), B.getRows(), B.getCols()));
4034  }
4035  }
4036 
4037  if (A.data != NULL && A.data == C.data) {
4038  std::cerr << "A and C must be two different objects!" << std::endl;
4039  return;
4040  }
4041 
4042  if (B.data != NULL && B.data == C.data) {
4043  std::cerr << "B and C must be two different objects!" << std::endl;
4044  return;
4045  }
4046 
4047  C.resize(nra + nrb, B.getCols(), false, false);
4048 
4049  if (C.data != NULL && A.data != NULL && A.size() > 0) {
4050  // Copy A in C
4051  memcpy(C.data, A.data, sizeof(double) * A.size());
4052  }
4053 
4054  if (C.data != NULL && B.data != NULL && B.size() > 0) {
4055  // Copy B in C
4056  memcpy(C.data + A.size(), B.data, sizeof(double) * B.size());
4057  }
4058 }
4059 
4070 {
4071  vpMatrix C;
4072  vpMatrix::stack(A, r, C);
4073 
4074  return C;
4075 }
4076 
4088 void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
4089 {
4090  if (A.data != NULL && A.data == C.data) {
4091  std::cerr << "A and C must be two different objects!" << std::endl;
4092  return;
4093  }
4094 
4095  C = A;
4096  C.stack(r);
4097 }
4098 
4109 {
4110  vpMatrix C;
4111  vpMatrix::stack(A, c, C);
4112 
4113  return C;
4114 }
4115 
4127 void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C)
4128 {
4129  if (A.data != NULL && A.data == C.data) {
4130  std::cerr << "A and C must be two different objects!" << std::endl;
4131  return;
4132  }
4133 
4134  C = A;
4135  C.stack(c);
4136 }
4137 
4150 vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, const unsigned int r, const unsigned int c)
4151 {
4152  vpMatrix C;
4153 
4154  insert(A, B, C, r, c);
4155 
4156  return C;
4157 }
4158 
4172 void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, const unsigned int r, const unsigned int c)
4173 {
4174  if (((r + B.getRows()) <= A.getRows()) && ((c + B.getCols()) <= A.getCols())) {
4175  C.resize(A.getRows(), A.getCols(), false, false);
4176 
4177  for (unsigned int i = 0; i < A.getRows(); i++) {
4178  for (unsigned int j = 0; j < A.getCols(); j++) {
4179  if (i >= r && i < (r + B.getRows()) && j >= c && j < (c + B.getCols())) {
4180  C[i][j] = B[i - r][j - c];
4181  } else {
4182  C[i][j] = A[i][j];
4183  }
4184  }
4185  }
4186  } else {
4187  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
4188  B.getRows(), B.getCols(), A.getCols(), A.getRows(), r, c);
4189  }
4190 }
4191 
4204 {
4205  vpMatrix C;
4206 
4207  juxtaposeMatrices(A, B, C);
4208 
4209  return C;
4210 }
4211 
4225 {
4226  unsigned int nca = A.getCols();
4227  unsigned int ncb = B.getCols();
4228 
4229  if (nca != 0) {
4230  if (A.getRows() != B.getRows()) {
4231  throw(vpException(vpException::dimensionError, "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
4232  A.getCols(), B.getRows(), B.getCols()));
4233  }
4234  }
4235 
4236  if (B.getRows() == 0 || nca + ncb == 0) {
4237  std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl;
4238  return;
4239  }
4240 
4241  C.resize(B.getRows(), nca + ncb, false, false);
4242 
4243  C.insert(A, 0, 0);
4244  C.insert(B, 0, nca);
4245 }
4246 
4247 //--------------------------------------------------------------------
4248 // Output
4249 //--------------------------------------------------------------------
4250 
4270 int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &intro) const
4271 {
4272  typedef std::string::size_type size_type;
4273 
4274  unsigned int m = getRows();
4275  unsigned int n = getCols();
4276 
4277  std::vector<std::string> values(m * n);
4278  std::ostringstream oss;
4279  std::ostringstream ossFixed;
4280  std::ios_base::fmtflags original_flags = oss.flags();
4281 
4282  // ossFixed <<std::fixed;
4283  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
4284 
4285  size_type maxBefore = 0; // the length of the integral part
4286  size_type maxAfter = 0; // number of decimals plus
4287  // one place for the decimal point
4288  for (unsigned int i = 0; i < m; ++i) {
4289  for (unsigned int j = 0; j < n; ++j) {
4290  oss.str("");
4291  oss << (*this)[i][j];
4292  if (oss.str().find("e") != std::string::npos) {
4293  ossFixed.str("");
4294  ossFixed << (*this)[i][j];
4295  oss.str(ossFixed.str());
4296  }
4297 
4298  values[i * n + j] = oss.str();
4299  size_type thislen = values[i * n + j].size();
4300  size_type p = values[i * n + j].find('.');
4301 
4302  if (p == std::string::npos) {
4303  maxBefore = vpMath::maximum(maxBefore, thislen);
4304  // maxAfter remains the same
4305  } else {
4306  maxBefore = vpMath::maximum(maxBefore, p);
4307  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
4308  }
4309  }
4310  }
4311 
4312  size_type totalLength = length;
4313  // increase totalLength according to maxBefore
4314  totalLength = vpMath::maximum(totalLength, maxBefore);
4315  // decrease maxAfter according to totalLength
4316  maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
4317  if (maxAfter == 1)
4318  maxAfter = 0;
4319 
4320  // the following line is useful for debugging
4321  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
4322 
4323  if (! intro.empty())
4324  s << intro;
4325  s << "[" << m << "," << n << "]=\n";
4326 
4327  for (unsigned int i = 0; i < m; i++) {
4328  s << " ";
4329  for (unsigned int j = 0; j < n; j++) {
4330  size_type p = values[i * n + j].find('.');
4331  s.setf(std::ios::right, std::ios::adjustfield);
4332  s.width((std::streamsize)maxBefore);
4333  s << values[i * n + j].substr(0, p).c_str();
4334 
4335  if (maxAfter > 0) {
4336  s.setf(std::ios::left, std::ios::adjustfield);
4337  if (p != std::string::npos) {
4338  s.width((std::streamsize)maxAfter);
4339  s << values[i * n + j].substr(p, maxAfter).c_str();
4340  } else {
4341  assert(maxAfter > 1);
4342  s.width((std::streamsize)maxAfter);
4343  s << ".0";
4344  }
4345  }
4346 
4347  s << ' ';
4348  }
4349  s << std::endl;
4350  }
4351 
4352  s.flags(original_flags); // restore s to standard state
4353 
4354  return (int)(maxBefore + maxAfter);
4355 }
4356 
4393 std::ostream &vpMatrix::matlabPrint(std::ostream &os) const
4394 {
4395  os << "[ ";
4396  for (unsigned int i = 0; i < this->getRows(); ++i) {
4397  for (unsigned int j = 0; j < this->getCols(); ++j) {
4398  os << (*this)[i][j] << ", ";
4399  }
4400  if (this->getRows() != i + 1) {
4401  os << ";" << std::endl;
4402  } else {
4403  os << "]" << std::endl;
4404  }
4405  }
4406  return os;
4407 }
4408 
4437 std::ostream &vpMatrix::maplePrint(std::ostream &os) const
4438 {
4439  os << "([ " << std::endl;
4440  for (unsigned int i = 0; i < this->getRows(); ++i) {
4441  os << "[";
4442  for (unsigned int j = 0; j < this->getCols(); ++j) {
4443  os << (*this)[i][j] << ", ";
4444  }
4445  os << "]," << std::endl;
4446  }
4447  os << "])" << std::endl;
4448  return os;
4449 }
4450 
4478 std::ostream &vpMatrix::csvPrint(std::ostream &os) const
4479 {
4480  for (unsigned int i = 0; i < this->getRows(); ++i) {
4481  for (unsigned int j = 0; j < this->getCols(); ++j) {
4482  os << (*this)[i][j];
4483  if (!(j == (this->getCols() - 1)))
4484  os << ", ";
4485  }
4486  os << std::endl;
4487  }
4488  return os;
4489 }
4490 
4527 std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
4528 {
4529  os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " << this->getCols() << "); " << std::endl;
4530 
4531  for (unsigned int i = 0; i < this->getRows(); ++i) {
4532  for (unsigned int j = 0; j < this->getCols(); ++j) {
4533  if (!octet) {
4534  os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl;
4535  } else {
4536  for (unsigned int k = 0; k < sizeof(double); ++k) {
4537  os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex
4538  << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl;
4539  }
4540  }
4541  }
4542  os << std::endl;
4543  }
4544  return os;
4545 }
4546 
4552 {
4553  if (rowNum == 0) {
4554  *this = A;
4555  } else if (A.getRows() > 0) {
4556  if (colNum != A.getCols()) {
4557  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum,
4558  A.getRows(), A.getCols()));
4559  }
4560 
4561  unsigned int rowNumOld = rowNum;
4562  resize(rowNum + A.getRows(), colNum, false, false);
4563  insert(A, rowNumOld, 0);
4564  }
4565 }
4566 
4583 {
4584  if (rowNum == 0) {
4585  *this = r;
4586  } else {
4587  if (colNum != r.getCols()) {
4588  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum,
4589  colNum, r.getCols()));
4590  }
4591 
4592  if (r.size() == 0) {
4593  return;
4594  }
4595 
4596  unsigned int oldSize = size();
4597  resize(rowNum + 1, colNum, false, false);
4598 
4599  if (data != NULL && r.data != NULL && data != r.data) {
4600  // Copy r in data
4601  memcpy(data + oldSize, r.data, sizeof(double) * r.size());
4602  }
4603  }
4604 }
4605 
4623 {
4624  if (colNum == 0) {
4625  *this = c;
4626  } else {
4627  if (rowNum != c.getRows()) {
4628  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum,
4629  colNum, c.getRows()));
4630  }
4631 
4632  if (c.size() == 0) {
4633  return;
4634  }
4635 
4636  vpMatrix tmp = *this;
4637  unsigned int oldColNum = colNum;
4638  resize(rowNum, colNum + 1, false, false);
4639 
4640  if (data != NULL && tmp.data != NULL && data != tmp.data) {
4641  // Copy c in data
4642  for (unsigned int i = 0; i < rowNum; i++) {
4643  memcpy(data + i*colNum, tmp.data + i*oldColNum, sizeof(double) * oldColNum);
4644  rowPtrs[i][oldColNum] = c[i];
4645  }
4646  }
4647  }
4648 }
4649 
4660 void vpMatrix::insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
4661 {
4662  if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) {
4663  if (A.colNum == colNum && data != NULL && A.data != NULL && A.data != data) {
4664  memcpy(data + r * colNum, A.data, sizeof(double) * A.size());
4665  } else if (data != NULL && A.data != NULL && A.data != data) {
4666  for (unsigned int i = r; i < (r + A.getRows()); i++) {
4667  memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum);
4668  }
4669  }
4670  } else {
4671  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
4672  A.getRows(), A.getCols(), rowNum, colNum, r, c);
4673  }
4674 }
4675 
4715 {
4716  if (rowNum != colNum) {
4717  throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
4718  colNum));
4719  }
4720 
4721 #ifdef VISP_HAVE_GSL /* be careful of the copy below */
4722  {
4723  // Check if the matrix is symetric: At - A = 0
4724  vpMatrix At_A = (*this).t() - (*this);
4725  for (unsigned int i = 0; i < rowNum; i++) {
4726  for (unsigned int j = 0; j < rowNum; j++) {
4727  // if (At_A[i][j] != 0) {
4728  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
4729  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symetric matrix"));
4730  }
4731  }
4732  }
4733 
4734  vpColVector evalue(rowNum); // Eigen values
4735 
4736  gsl_vector *eval = gsl_vector_alloc(rowNum);
4737  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
4738 
4739  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
4740  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
4741 
4742  unsigned int Atda = (unsigned int)m->tda;
4743  for (unsigned int i = 0; i < rowNum; i++) {
4744  unsigned int k = i * Atda;
4745  for (unsigned int j = 0; j < colNum; j++)
4746  m->data[k + j] = (*this)[i][j];
4747  }
4748  gsl_eigen_symmv(m, eval, evec, w);
4749 
4750  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
4751 
4752  for (unsigned int i = 0; i < rowNum; i++) {
4753  evalue[i] = gsl_vector_get(eval, i);
4754  }
4755 
4756  gsl_eigen_symmv_free(w);
4757  gsl_vector_free(eval);
4758  gsl_matrix_free(m);
4759  gsl_matrix_free(evec);
4760 
4761  return evalue;
4762  }
4763 #else
4764  {
4765  throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. You "
4766  "should install GSL rd party"));
4767  }
4768 #endif
4769 }
4770 
4824 #ifdef VISP_HAVE_GSL /* be careful of the copy below */
4825 void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const
4826 #else
4827 void vpMatrix::eigenValues(vpColVector & /* evalue */, vpMatrix & /* evector */) const
4828 #endif
4829 {
4830  if (rowNum != colNum) {
4831  throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
4832  colNum));
4833  }
4834 
4835 #ifdef VISP_HAVE_GSL /* be careful of the copy below */
4836  {
4837  // Check if the matrix is symetric: At - A = 0
4838  vpMatrix At_A = (*this).t() - (*this);
4839  for (unsigned int i = 0; i < rowNum; i++) {
4840  for (unsigned int j = 0; j < rowNum; j++) {
4841  // if (At_A[i][j] != 0) {
4842  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
4843  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symetric matrix"));
4844  }
4845  }
4846  }
4847 
4848  // Resize the output matrices
4849  evalue.resize(rowNum);
4850  evector.resize(rowNum, colNum);
4851 
4852  gsl_vector *eval = gsl_vector_alloc(rowNum);
4853  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
4854 
4855  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
4856  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
4857 
4858  unsigned int Atda = (unsigned int)m->tda;
4859  for (unsigned int i = 0; i < rowNum; i++) {
4860  unsigned int k = i * Atda;
4861  for (unsigned int j = 0; j < colNum; j++)
4862  m->data[k + j] = (*this)[i][j];
4863  }
4864  gsl_eigen_symmv(m, eval, evec, w);
4865 
4866  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
4867 
4868  for (unsigned int i = 0; i < rowNum; i++) {
4869  evalue[i] = gsl_vector_get(eval, i);
4870  }
4871  Atda = (unsigned int)evec->tda;
4872  for (unsigned int i = 0; i < rowNum; i++) {
4873  unsigned int k = i * Atda;
4874  for (unsigned int j = 0; j < rowNum; j++) {
4875  evector[i][j] = evec->data[k + j];
4876  }
4877  }
4878 
4879  gsl_eigen_symmv_free(w);
4880  gsl_vector_free(eval);
4881  gsl_matrix_free(m);
4882  gsl_matrix_free(evec);
4883  }
4884 #else
4885  {
4886  throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. You "
4887  "should install GSL rd party"));
4888  }
4889 #endif
4890 }
4891 
4911 unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const
4912 {
4913  unsigned int nbline = getRows();
4914  unsigned int nbcol = getCols();
4915 
4916  vpMatrix U; // Copy of the matrix, SVD function is destructive
4917  vpColVector sv(nbcol); // singular values
4918  vpMatrix V(nbcol, nbcol); // V matrix of singular value decomposition
4919 
4920  // Copy and resize matrix to have at least as many rows as columns
4921  // kernel is computed in svd method only if the matrix has more rows than
4922  // columns
4923 
4924  if (nbline < nbcol)
4925  U.resize(nbcol, nbcol);
4926  else
4927  U.resize(nbline, nbcol);
4928 
4929  U.insert(*this, 0, 0);
4930 
4931  U.svd(sv, V);
4932 
4933  // Compute the highest singular value and rank of the matrix
4934  double maxsv = 0;
4935  for (unsigned int i = 0; i < nbcol; i++) {
4936  if (fabs(sv[i]) > maxsv) {
4937  maxsv = fabs(sv[i]);
4938  }
4939  }
4940 
4941  unsigned int rank = 0;
4942  for (unsigned int i = 0; i < nbcol; i++) {
4943  if (fabs(sv[i]) > maxsv * svThreshold) {
4944  rank++;
4945  }
4946  }
4947 
4948  kerAt.resize(nbcol - rank, nbcol);
4949  if (rank != nbcol) {
4950  for (unsigned int j = 0, k = 0; j < nbcol; j++) {
4951  // if( v.col(j) in kernel and non zero )
4952  if ((fabs(sv[j]) <= maxsv * svThreshold) &&
4953  (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
4954  for (unsigned int i = 0; i < V.getRows(); i++) {
4955  kerAt[k][i] = V[i][j];
4956  }
4957  k++;
4958  }
4959  }
4960  }
4961 
4962  return rank;
4963 }
4964 
4996 double vpMatrix::det(vpDetMethod method) const
4997 {
4998  double det = 0.;
4999 
5000  if (method == LU_DECOMPOSITION) {
5001  det = this->detByLU();
5002  }
5003 
5004  return (det);
5005 }
5006 
5015 {
5016  if (colNum != rowNum) {
5017  throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix",
5018  rowNum, colNum));
5019  } else {
5020 #ifdef VISP_HAVE_GSL
5021  size_t size_ = rowNum * colNum;
5022  double *b = new double[size_];
5023  for (size_t i = 0; i < size_; i++)
5024  b[i] = 0.;
5025  gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum);
5026  gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum);
5027  gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
5028  // gsl_matrix_fprintf(stdout, &em.matrix, "%g");
5029  vpMatrix expA(rowNum, colNum);
5030  memcpy(expA.data, b, size_ * sizeof(double));
5031 
5032  delete[] b;
5033  return expA;
5034 #else
5035  vpMatrix _expE(rowNum, colNum);
5036  vpMatrix _expD(rowNum, colNum);
5037  vpMatrix _expX(rowNum, colNum);
5038  vpMatrix _expcX(rowNum, colNum);
5039  vpMatrix _eye(rowNum, colNum);
5040 
5041  _eye.eye();
5042  vpMatrix exp(*this);
5043 
5044  // double f;
5045  int e;
5046  double c = 0.5;
5047  int q = 6;
5048  int p = 1;
5049 
5050  double nA = 0;
5051  for (unsigned int i = 0; i < rowNum; i++) {
5052  double sum = 0;
5053  for (unsigned int j = 0; j < colNum; j++) {
5054  sum += fabs((*this)[i][j]);
5055  }
5056  if (sum > nA || i == 0) {
5057  nA = sum;
5058  }
5059  }
5060 
5061  /* f = */ frexp(nA, &e);
5062  // double s = (0 > e+1)?0:e+1;
5063  double s = e + 1;
5064 
5065  double sca = 1.0 / pow(2.0, s);
5066  exp = sca * exp;
5067  _expX = *this;
5068  _expE = c * exp + _eye;
5069  _expD = -c * exp + _eye;
5070  for (int k = 2; k <= q; k++) {
5071  c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1)));
5072  _expcX = exp * _expX;
5073  _expX = _expcX;
5074  _expcX = c * _expX;
5075  _expE = _expE + _expcX;
5076  if (p)
5077  _expD = _expD + _expcX;
5078  else
5079  _expD = _expD - _expcX;
5080  p = !p;
5081  }
5082  _expX = _expD.inverseByLU();
5083  exp = _expX * _expE;
5084  for (int k = 1; k <= s; k++) {
5085  _expE = exp * exp;
5086  exp = _expE;
5087  }
5088  return exp;
5089 #endif
5090  }
5091 }
5092 
5093 /**************************************************************************************************************/
5094 /**************************************************************************************************************/
5095 
5096 // Specific functions
5097 
5098 /*
5099 input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.
5100 
5101 output:: the complement matrix of the element (rowNo,colNo).
5102 This is the matrix obtained from M after elimenating the row rowNo and column
5103 colNo
5104 
5105 example:
5106 1 2 3
5107 M = 4 5 6
5108 7 8 9
5109 1 3
5110 subblock(M, 1, 1) give the matrix 7 9
5111 */
5112 vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row)
5113 {
5114  vpMatrix M_comp(M.getRows() - 1, M.getCols() - 1);
5115 
5116  for (unsigned int i = 0; i < col; i++) {
5117  for (unsigned int j = 0; j < row; j++)
5118  M_comp[i][j] = M[i][j];
5119  for (unsigned int j = row + 1; j < M.getRows(); j++)
5120  M_comp[i][j - 1] = M[i][j];
5121  }
5122  for (unsigned int i = col + 1; i < M.getCols(); i++) {
5123  for (unsigned int j = 0; j < row; j++)
5124  M_comp[i - 1][j] = M[i][j];
5125  for (unsigned int j = row + 1; j < M.getRows(); j++)
5126  M_comp[i - 1][j - 1] = M[i][j];
5127  }
5128  return M_comp;
5129 }
5130 
5140 double vpMatrix::cond(double svThreshold) const
5141 {
5142  unsigned int nbline = getRows();
5143  unsigned int nbcol = getCols();
5144 
5145  vpMatrix U; // Copy of the matrix, SVD function is destructive
5146  vpColVector sv(nbcol); // singular values
5147  vpMatrix V(nbcol, nbcol); // V matrix of singular value decomposition
5148 
5149  // Copy and resize matrix to have at least as many rows as columns
5150  // kernel is computed in svd method only if the matrix has more rows than
5151  // columns
5152 
5153  if (nbline < nbcol)
5154  U.resize(nbcol, nbcol);
5155  else
5156  U.resize(nbline, nbcol);
5157 
5158  U.insert(*this, 0, 0);
5159 
5160  U.svd(sv, V);
5161 
5162  // Compute the highest singular value
5163  double maxsv = 0;
5164  for (unsigned int i = 0; i < nbcol; i++) {
5165  if (fabs(sv[i]) > maxsv) {
5166  maxsv = fabs(sv[i]);
5167  }
5168  }
5169 
5170  // Compute the rank of the matrix
5171  unsigned int rank = 0;
5172  for (unsigned int i = 0; i < nbcol; i++) {
5173  if (fabs(sv[i]) > maxsv * svThreshold) {
5174  rank++;
5175  }
5176  }
5177 
5178  // Compute the lowest singular value
5179  double minsv = maxsv;
5180  for (unsigned int i = 0; i < rank; i++) {
5181  if (fabs(sv[i]) < minsv) {
5182  minsv = fabs(sv[i]);
5183  }
5184  }
5185 
5186  if (std::fabs(minsv) > std::numeric_limits<double>::epsilon()) {
5187  return maxsv / minsv;
5188  }
5189  else {
5190  return std::numeric_limits<double>::infinity();
5191  }
5192 }
5193 
5200 void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
5201 {
5202  if (H.getCols() != H.getRows()) {
5203  throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(),
5204  H.getCols()));
5205  }
5206  HLM.resize(H.getRows(), H.getCols(), false, false);
5207 
5208  for (unsigned int i = 0; i < H.getCols(); i++) {
5209  for (unsigned int j = 0; j < H.getCols(); j++) {
5210  HLM[i][j] = H[i][j];
5211  if (i == j)
5212  HLM[i][j] += alpha * H[i][j];
5213  }
5214  }
5215 }
5216 
5226 vp_deprecated double vpMatrix::euclideanNorm() const
5227 {
5228  return frobeniusNorm();
5229 }
5230 
5239 {
5240  double norm = 0.0;
5241  for (unsigned int i = 0; i < dsize; i++) {
5242  double x = *(data + i);
5243  norm += x * x;
5244  }
5245 
5246  return sqrt(norm);
5247 }
5248 
5258 {
5259  if(this->dsize != 0){
5260  vpMatrix v;
5261  vpColVector w;
5262 
5263  vpMatrix M = *this;
5264 
5265  M.svd(w, v);
5266 
5267  double max = w[0];
5268  unsigned int maxRank = std::min(this->getCols(), this->getRows());
5269  // The maximum reachable rank is either the number of columns or the number of rows
5270  // of the matrix.
5271  unsigned int boundary = std::min(maxRank, w.size());
5272  // boundary is here to ensure that the number of singular values used for the com-
5273  // putation of the euclidean norm of the matrix is not greater than the maximum
5274  // reachable rank. Indeed, some svd library pad the singular values vector with 0s
5275  // if the input matrix is non-square.
5276  for (unsigned int i = 0; i < boundary; i++) {
5277  if (max < w[i]) {
5278  max = w[i];
5279  }
5280  }
5281  return max;
5282  }
5283  else {
5284  return 0.;
5285  }
5286 }
5287 
5299 {
5300  double norm = 0.0;
5301  for (unsigned int i = 0; i < rowNum; i++) {
5302  double x = 0;
5303  for (unsigned int j = 0; j < colNum; j++) {
5304  x += fabs(*(*(rowPtrs + i) + j));
5305  }
5306  if (x > norm) {
5307  norm = x;
5308  }
5309  }
5310  return norm;
5311 }
5312 
5319 double vpMatrix::sumSquare() const
5320 {
5321  double sum_square = 0.0;
5322  double x;
5323 
5324  for (unsigned int i = 0; i < rowNum; i++) {
5325  for (unsigned int j = 0; j < colNum; j++) {
5326  x = rowPtrs[i][j];
5327  sum_square += x * x;
5328  }
5329  }
5330 
5331  return sum_square;
5332 }
5333 
5345 vpMatrix vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode)
5346 {
5347  vpMatrix res;
5348  conv2(M, kernel, res, mode);
5349  return res;
5350 }
5351 
5364 void vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, vpMatrix &res, const std::string &mode)
5365 {
5366  if (M.getRows()*M.getCols() == 0 || kernel.getRows()*kernel.getCols() == 0)
5367  return;
5368 
5369  if (mode == "valid") {
5370  if (kernel.getRows() > M.getRows() || kernel.getCols() > M.getCols())
5371  return;
5372  }
5373 
5374  vpMatrix M_padded, res_same;
5375 
5376  if (mode == "full" || mode == "same") {
5377  const unsigned int pad_x = kernel.getCols()-1;
5378  const unsigned int pad_y = kernel.getRows()-1;
5379  M_padded.resize(M.getRows() + 2*pad_y, M.getCols() + 2*pad_x, true, false);
5380  M_padded.insert(M, pad_y, pad_x);
5381 
5382  if (mode == "same") {
5383  res.resize(M.getRows(), M.getCols(), false, false);
5384  res_same.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
5385  } else {
5386  res.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
5387  }
5388  } else if (mode == "valid") {
5389  M_padded = M;
5390  res.resize(M.getRows()-kernel.getRows()+1, M.getCols()-kernel.getCols()+1);
5391  } else {
5392  return;
5393  }
5394 
5395  if (mode == "same") {
5396  for (unsigned int i = 0; i < res_same.getRows(); i++) {
5397  for (unsigned int j = 0; j < res_same.getCols(); j++) {
5398  for (unsigned int k = 0; k < kernel.getRows(); k++) {
5399  for (unsigned int l = 0; l < kernel.getCols(); l++) {
5400  res_same[i][j] += M_padded[i+k][j+l] * kernel[kernel.getRows()-k-1][kernel.getCols()-l-1];
5401  }
5402  }
5403  }
5404  }
5405 
5406  const unsigned int start_i = kernel.getRows()/2;
5407  const unsigned int start_j = kernel.getCols()/2;
5408  for (unsigned int i = 0; i < M.getRows(); i++) {
5409  memcpy(res.data + i*M.getCols(), res_same.data + (i+start_i)*res_same.getCols() + start_j, sizeof(double)*M.getCols());
5410  }
5411  } else {
5412  for (unsigned int i = 0; i < res.getRows(); i++) {
5413  for (unsigned int j = 0; j < res.getCols(); j++) {
5414  for (unsigned int k = 0; k < kernel.getRows(); k++) {
5415  for (unsigned int l = 0; l < kernel.getCols(); l++) {
5416  res[i][j] += M_padded[i+k][j+l] * kernel[kernel.getRows()-k-1][kernel.getCols()-l-1];
5417  }
5418  }
5419  }
5420  }
5421  }
5422 }
5423 
5424 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
5426 {
5427  return (vpMatrix)(vpColVector::stack(A, B));
5428 }
5429 
5431 {
5432  vpColVector::stack(A, B, C);
5433 }
5434 
5436 
5437 void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); }
5438 
5457 vpRowVector vpMatrix::row(const unsigned int i)
5458 {
5459  vpRowVector c(getCols());
5460 
5461  for (unsigned int j = 0; j < getCols(); j++)
5462  c[j] = (*this)[i - 1][j];
5463  return c;
5464 }
5465 
5483 vpColVector vpMatrix::column(const unsigned int j)
5484 {
5485  vpColVector c(getRows());
5486 
5487  for (unsigned int i = 0; i < getRows(); i++)
5488  c[i] = (*this)[i][j - 1];
5489  return c;
5490 }
5491 
5498 void vpMatrix::setIdentity(const double &val)
5499 {
5500  for (unsigned int i = 0; i < rowNum; i++)
5501  for (unsigned int j = 0; j < colNum; j++)
5502  if (i == j)
5503  (*this)[i][j] = val;
5504  else
5505  (*this)[i][j] = 0;
5506 }
5507 
5508 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:1881
vpMatrix operator*(const double &x, const vpMatrix &B)
Definition: vpMatrix.cpp:1439
vpMatrix pseudoInverseEigen3(double svThreshold=1e-6) const
std::ostream & csvPrint(std::ostream &os) const
Definition: vpMatrix.cpp:4478
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpColVector eigenValues() const
Definition: vpMatrix.cpp:4714
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:4203
vpMatrix pseudoInverseOpenCV(double svThreshold=1e-6) const
vp_deprecated vpRowVector row(const unsigned int i)
Definition: vpMatrix.cpp:5457
double infinityNorm() const
Definition: vpMatrix.cpp:5298
static vpMatrix conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode="full")
Definition: vpMatrix.cpp:5345
vpMatrix pseudoInverseLapack(double svThreshold=1e-6) const
double euclideanNorm() const
Definition: vpMatrix.cpp:5226
vpMatrix expm() const
Definition: vpMatrix.cpp:5014
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:115
std::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
Definition: vpMatrix.cpp:4527
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1202
#define vpERROR_TRACE
Definition: vpDebug.h:393
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:4551
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
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:836
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
unsigned int getCols() const
Definition: vpArray2D.h:279
vpMatrix & operator+=(const vpMatrix &B)
Operation A = A + B.
Definition: vpMatrix.cpp:1349
double sumSquare() const
Definition: vpMatrix.cpp:5319
vpMatrix()
Definition: vpMatrix.h:180
double inducedL2Norm() const
Definition: vpMatrix.cpp:5257
vp_deprecated vpColVector column(const unsigned int j)
Definition: vpMatrix.cpp:5483
std::ostream & maplePrint(std::ostream &os) const
Definition: vpMatrix.cpp:4437
void svdLapack(vpColVector &w, vpMatrix &V)
vpMatrix & operator-=(const vpMatrix &B)
Operation A = A - B.
Definition: vpMatrix.cpp:1366
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.
vpRowVector getRow(const unsigned int i) const
Definition: vpMatrix.cpp:3933
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:4270
double frobeniusNorm() const
Definition: vpMatrix.cpp:5238
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
Definition: vpMatrix.cpp:4660
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:1758
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:5498
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:319
vpMatrix AtA() const
Definition: vpMatrix.cpp:524
vpMatrix AAt() const
Definition: vpMatrix.cpp:426
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:782
vp_deprecated void init()
Definition: vpMatrix.h:831
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()
static void multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
Definition: vpMatrix.cpp:854
vpColVector getCol(const unsigned int j) const
Definition: vpMatrix.cpp:3887
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:895
vpMatrix operator+(const vpMatrix &B) const
Definition: vpMatrix.cpp:1260
vpMatrix transpose() const
Definition: vpMatrix.cpp:394
double detByLU() const
void svdOpenCV(vpColVector &w, vpMatrix &V)
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:5140
vpMatrix pseudoInverseGsl(double svThreshold=1e-6) const
unsigned int getRows() const
Definition: vpArray2D.h:289
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:4996
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
void svdGsl(vpColVector &w, vpMatrix &V)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpRowVector.h:268
double sumSquare() const
vpMatrix t() const
Definition: vpMatrix.cpp:375
double sum() const
Definition: vpMatrix.cpp:1415
void kron(const vpMatrix &m1, vpMatrix &out) const
Definition: vpMatrix.cpp:1668
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
std::ostream & matlabPrint(std::ostream &os) const
Definition: vpMatrix.cpp:4393
vpMatrix inverseByLU() const
vpRowVector stackRows()
Definition: vpMatrix.cpp:1589
vpMatrix operator/(const double x) const
Cij = Aij / x (A is unchanged)
Definition: vpMatrix.cpp:1469
static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
Definition: vpMatrix.cpp:801
vpMatrix operator*(const vpMatrix &B) const
Definition: vpMatrix.cpp:1030
vpColVector stackColumns()
Definition: vpMatrix.cpp:1563
vpMatrix operator-() const
Definition: vpMatrix.cpp:1408
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2021
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:4911
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:141
static void negateMatrix(const vpMatrix &A, vpMatrix &C)
Definition: vpMatrix.cpp:1390
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:5200
vpMatrix & operator,(double val)
Definition: vpMatrix.cpp:700
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 & 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
vpMatrix hadamard(const vpMatrix &m) const
Definition: vpMatrix.cpp:1602