Visual Servoing Platform  version 3.3.1 under development (2020-10-25)
vpMatrix.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Matrix manipulation.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
43 #include <algorithm>
44 #include <assert.h>
45 #include <cmath> // std::fabs
46 #include <fstream>
47 #include <limits> // numeric_limits
48 #include <sstream>
49 #include <stdio.h>
50 #include <stdlib.h>
51 #include <string.h>
52 #include <string>
53 #include <vector>
54 
55 #include <visp3/core/vpConfig.h>
56 #include <visp3/core/vpCPUFeatures.h>
57 #include <visp3/core/vpColVector.h>
58 #include <visp3/core/vpDebug.h>
59 #include <visp3/core/vpException.h>
60 #include <visp3/core/vpMath.h>
61 #include <visp3/core/vpMatrix.h>
62 #include <visp3/core/vpTranslationVector.h>
63 
64 #ifdef VISP_HAVE_LAPACK
65 # ifdef VISP_HAVE_GSL
66 # include <gsl/gsl_eigen.h>
67 # include <gsl/gsl_math.h>
68 # include <gsl/gsl_linalg.h>
69 # elif defined(VISP_HAVE_MKL)
70 # include <mkl.h>
71 typedef MKL_INT integer;
72 
73 void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_,
74  double *w_data, double *work_data, int lwork_, int &info_)
75 {
76  MKL_INT n = static_cast<MKL_INT>(n_);
77  MKL_INT lda = static_cast<MKL_INT>(lda_);
78  MKL_INT lwork = static_cast<MKL_INT>(lwork_);
79  MKL_INT info = static_cast<MKL_INT>(info_);
80 
81  dsyev(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
82 }
83 
84 # else
85 # if defined(VISP_HAVE_LAPACK_BUILT_IN)
86 typedef long int integer;
87 # else
88 typedef int integer;
89 # endif
90 extern "C" integer dsyev_(char *jobz, char *uplo, integer *n, double *a, integer *lda,
91  double *w, double *WORK, integer *lwork, integer *info);
92 
93 void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_,
94  double *w_data, double *work_data, int lwork_, int &info_)
95 {
96  integer n = static_cast<integer>(n_);
97  integer lda = static_cast<integer>(lda_);
98  integer lwork = static_cast<integer>(lwork_);
99  integer info = static_cast<integer>(info_);
100 
101  dsyev_(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
102 
103  lwork_ = static_cast<int>(lwork);
104  info_ = static_cast<int>(info);
105 }
106 # endif
107 #endif
108 
109 #if !defined(VISP_USE_MSVC) || (defined(VISP_USE_MSVC) && !defined(VISP_BUILD_SHARED_LIBS))
110 const unsigned int vpMatrix::m_lapack_min_size_default = 0;
111 unsigned int vpMatrix::m_lapack_min_size = vpMatrix::m_lapack_min_size_default;
112 #endif
113 
114 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
115 #include <emmintrin.h>
116 #define VISP_HAVE_SSE2 1
117 #endif
118 
119 #if defined __AVX__
120 #include <immintrin.h>
121 #define VISP_HAVE_AVX 1
122 #endif
123 
124 #if VISP_HAVE_AVX
125 namespace {
126  void transpose4x4(const vpMatrix& a, vpMatrix& b, unsigned int i, unsigned int j)
127  {
128  __m256d a0 = _mm256_loadu_pd(&a[i][j]);
129  __m256d a1 = _mm256_loadu_pd(&a[i + 1][j]);
130  __m256d a2 = _mm256_loadu_pd(&a[i + 2][j]);
131  __m256d a3 = _mm256_loadu_pd(&a[i + 3][j]);
132 
133  __m256d T0 = _mm256_shuffle_pd(a0, a1, 15);
134  __m256d T1 = _mm256_shuffle_pd(a0, a1, 0);
135  __m256d T2 = _mm256_shuffle_pd(a2, a3, 15);
136  __m256d T3 = _mm256_shuffle_pd(a2, a3, 0);
137 
138  a1 = _mm256_permute2f128_pd(T0, T2, 32);
139  a3 = _mm256_permute2f128_pd(T0, T2, 49);
140  a0 = _mm256_permute2f128_pd(T1, T3, 32);
141  a2 = _mm256_permute2f128_pd(T1, T3, 49);
142 
143  _mm256_storeu_pd(&b[j][i], a0);
144  _mm256_storeu_pd(&b[j + 1][i], a1);
145  _mm256_storeu_pd(&b[j + 2][i], a2);
146  _mm256_storeu_pd(&b[j + 3][i], a3);
147  }
148 
149  void transpose16x16(const vpMatrix& a, vpMatrix& b, unsigned int i, unsigned int j)
150  {
151  transpose4x4(a, b, i, j);
152  transpose4x4(a, b, i, j + 4);
153  transpose4x4(a, b, i, j + 8);
154  transpose4x4(a, b, i, j + 12);
155 
156  transpose4x4(a, b, i + 4, j);
157  transpose4x4(a, b, i + 4, j + 4);
158  transpose4x4(a, b, i + 4, j + 8);
159  transpose4x4(a, b, i + 4, j + 12);
160 
161  transpose4x4(a, b, i + 8, j);
162  transpose4x4(a, b, i + 8, j + 4);
163  transpose4x4(a, b, i + 8, j + 8);
164  transpose4x4(a, b, i + 8, j + 12);
165 
166  transpose4x4(a, b, i + 12, j);
167  transpose4x4(a, b, i + 12, j + 4);
168  transpose4x4(a, b, i + 12, j + 8);
169  transpose4x4(a, b, i + 12, j + 12);
170  }
171 }
172 #endif
173 
174 // Prototypes of specific functions
175 vpMatrix subblock(const vpMatrix &, unsigned int, unsigned int);
176 
177 void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows,
178  unsigned int ncols, double svThreshold, vpMatrix &Ap, int &rank_out,
179  int *rank_in, vpMatrix *imA, vpMatrix *imAt, vpMatrix *kerAt)
180 {
181  Ap.resize(ncols, nrows, true, false);
182 
183  // compute the highest singular value and the rank of h
184  double maxsv = fabs(sv[0]);
185 
186  rank_out = 0;
187 
188  for (unsigned int i = 0; i < ncols; i++) {
189  if (fabs(sv[i]) > maxsv * svThreshold) {
190  rank_out++;
191  }
192  }
193 
194  unsigned int rank = static_cast<unsigned int>(rank_out);
195  if (rank_in) {
196  rank = static_cast<unsigned int>(*rank_in);
197  }
198 
199  for (unsigned int i = 0; i < ncols; i++) {
200  for (unsigned int j = 0; j < nrows; j++) {
201  for (unsigned int k = 0; k < rank; k++) {
202  Ap[i][j] += V[i][k] * U[j][k] / sv[k];
203  }
204  }
205  }
206 
207  // Compute im(A)
208  if (imA) {
209  imA->resize(nrows, rank);
210 
211  for (unsigned int i = 0; i < nrows; i++) {
212  for (unsigned int j = 0; j < rank; j++) {
213  (*imA)[i][j] = U[i][j];
214  }
215  }
216  }
217 
218  // Compute im(At)
219  if (imAt) {
220  imAt->resize(ncols, rank);
221  for (unsigned int i = 0; i < ncols; i++) {
222  for (unsigned int j = 0; j < rank; j++) {
223  (*imAt)[i][j] = V[i][j];
224  }
225  }
226  }
227 
228  // Compute ker(At)
229  if (kerAt) {
230  kerAt->resize(ncols - rank, ncols);
231  if (rank != ncols) {
232  for (unsigned int j = 0, k = 0; j < ncols; j++) {
233  // if( v.col(j) in kernel and non zero )
234  if ((fabs(sv[j]) <= maxsv * svThreshold) &&
235  (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
236  for (unsigned int i = 0; i < V.getRows(); i++) {
237  (*kerAt)[k][i] = V[i][j];
238  }
239  k++;
240  }
241  }
242  }
243  }
244 }
245 
251 vpMatrix::vpMatrix(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
252  : vpArray2D<double>()
253 {
254  if (((r + nrows) > M.rowNum) || ((c + ncols) > M.colNum)) {
256  "Cannot construct a sub matrix (%dx%d) starting at "
257  "position (%d,%d) that is not contained in the "
258  "original matrix (%dx%d)",
259  nrows, ncols, r, c, M.rowNum, M.colNum));
260  }
261 
262  init(M, r, c, nrows, ncols);
263 }
264 
265 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
267 {
268  rowNum = A.rowNum;
269  colNum = A.colNum;
270  rowPtrs = A.rowPtrs;
271  dsize = A.dsize;
272  data = A.data;
273 
274  A.rowNum = 0;
275  A.colNum = 0;
276  A.rowPtrs = NULL;
277  A.dsize = 0;
278  A.data = NULL;
279 }
280 
306 vpMatrix::vpMatrix(const std::initializer_list<double> &list) : vpArray2D<double>(list) { }
307 
308 
333 vpMatrix::vpMatrix(unsigned int nrows, unsigned int ncols, const std::initializer_list<double> &list)
334  : vpArray2D<double>(nrows, ncols, list) {}
335 
358 vpMatrix::vpMatrix(const std::initializer_list<std::initializer_list<double> > &lists) : vpArray2D<double>(lists) { }
359 
360 #endif
361 
408 void vpMatrix::init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
409 {
410  unsigned int rnrows = r + nrows;
411  unsigned int cncols = c + ncols;
412 
413  if (rnrows > M.getRows())
414  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
415  M.getRows()));
416  if (cncols > M.getCols())
417  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
418  M.getCols()));
419  resize(nrows, ncols, false, false);
420 
421  if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
422  return; // Noting to do
423  for (unsigned int i = 0; i < nrows; i++) {
424  memcpy((*this)[i], &M[i + r][c], ncols * sizeof(double));
425  }
426 }
427 
469 vpMatrix vpMatrix::extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
470 {
471  unsigned int rnrows = r + nrows;
472  unsigned int cncols = c + ncols;
473 
474  if (rnrows > getRows())
475  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows,
476  getRows()));
477  if (cncols > getCols())
478  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols,
479  getCols()));
480 
481  vpMatrix M;
482  M.resize(nrows, ncols, false, false);
483  for (unsigned int i = 0; i < nrows; i++) {
484  memcpy(M[i], &(*this)[i + r][c], ncols * sizeof(double));
485  }
486 
487  return M;
488 }
489 
494 void vpMatrix::eye(unsigned int n) { eye(n, n); }
495 
500 void vpMatrix::eye(unsigned int m, unsigned int n)
501 {
502  resize(m, n);
503 
504  eye();
505 }
506 
512 {
513  for (unsigned int i = 0; i < rowNum; i++) {
514  for (unsigned int j = 0; j < colNum; j++) {
515  if (i == j)
516  (*this)[i][j] = 1.0;
517  else
518  (*this)[i][j] = 0;
519  }
520  }
521 }
522 
527 {
528  return transpose();
529 }
530 
537 {
538  vpMatrix At;
539  transpose(At);
540  return At;
541 }
542 
549 {
550  At.resize(colNum, rowNum, false, false);
551 
552  if (rowNum <= 16 || colNum <= 16) {
553  for (unsigned int i = 0; i < rowNum; i++) {
554  for (unsigned int j = 0; j < colNum; j++) {
555  At[j][i] = (*this)[i][j];
556  }
557  }
558  }
559  else {
560  bool haveAVX = vpCPUFeatures::checkAVX();
561 #if !VISP_HAVE_AVX
562  haveAVX = false;
563 #endif
564 
565  if (haveAVX) {
566 #if VISP_HAVE_AVX
567  // matrix transpose using tiling
568  const int nrows = static_cast<int>(rowNum);
569  const int ncols = static_cast<int>(colNum);
570  const int tileSize = 16;
571 
572  for (int i = 0; i < nrows;) {
573  for (; i <= nrows - tileSize; i += tileSize) {
574  int j = 0;
575  for (; j <= ncols - tileSize; j += tileSize) {
576  transpose16x16(*this, At, i, j);
577  }
578 
579  for (int k = i; k < i + tileSize; k++) {
580  for (int l = j; l < ncols; l++) {
581  At[l][k] = (*this)[k][l];
582  }
583  }
584  }
585 
586  for (; i < nrows; i++) {
587  for (int j = 0; j < ncols; j++) {
588  At[j][i] = (*this)[i][j];
589  }
590  }
591  }
592 
593  _mm256_zeroupper();
594 #endif
595  } else {
596  // https://stackoverflow.com/a/21548079
597  const int tileSize = 32;
598  for (unsigned int i = 0; i < rowNum; i += tileSize) {
599  for (unsigned int j = 0; j < colNum; j++) {
600  for (unsigned int b = 0; b < static_cast<unsigned int>(tileSize) && i + b < rowNum; b++) {
601  At[j][i + b] = (*this)[i + b][j];
602  }
603  }
604  }
605  }
606  }
607 }
608 
615 {
616  vpMatrix B;
617 
618  AAt(B);
619 
620  return B;
621 }
622 
634 void vpMatrix::AAt(vpMatrix &B) const
635 {
636  if ((B.rowNum != rowNum) || (B.colNum != rowNum))
637  B.resize(rowNum, rowNum, false, false);
638 
639  // If available use Lapack only for large matrices
640  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
641 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
642  useLapack = false;
643 #endif
644 
645  if (useLapack) {
646 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
647  const double alpha = 1.0;
648  const double beta = 0.0;
649  const char transa = 't';
650  const char transb = 'n';
651 
652  vpMatrix::blas_dgemm(transa, transb, rowNum, rowNum, colNum, alpha, data, colNum, data, colNum, beta, B.data, rowNum);
653 #endif
654  }
655  else {
656  // compute A*A^T
657  for (unsigned int i = 0; i < rowNum; i++) {
658  for (unsigned int j = i; j < rowNum; j++) {
659  double *pi = rowPtrs[i]; // row i
660  double *pj = rowPtrs[j]; // row j
661 
662  // sum (row i .* row j)
663  double ssum = 0;
664  for (unsigned int k = 0; k < colNum; k++)
665  ssum += *(pi++) * *(pj++);
666 
667  B[i][j] = ssum; // upper triangle
668  if (i != j)
669  B[j][i] = ssum; // lower triangle
670  }
671  }
672  }
673 }
674 
686 void vpMatrix::AtA(vpMatrix &B) const
687 {
688  if ((B.rowNum != colNum) || (B.colNum != colNum))
689  B.resize(colNum, colNum, false, false);
690 
691  // If available use Lapack only for large matrices
692  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size);
693 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
694  useLapack = false;
695 #endif
696 
697  if (useLapack) {
698 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
699  const double alpha = 1.0;
700  const double beta = 0.0;
701  const char transa = 'n';
702  const char transb = 't';
703 
704  vpMatrix::blas_dgemm(transa, transb, colNum, colNum, rowNum, alpha, data, colNum, data, colNum, beta, B.data, colNum);
705 #endif
706  }
707  else {
708  for (unsigned int i = 0; i < colNum; i++) {
709  double *Bi = B[i];
710  for (unsigned int j = 0; j < i; j++) {
711  double *ptr = data;
712  double s = 0;
713  for (unsigned int k = 0; k < rowNum; k++) {
714  s += (*(ptr + i)) * (*(ptr + j));
715  ptr += colNum;
716  }
717  *Bi++ = s;
718  B[j][i] = s;
719  }
720  double *ptr = data;
721  double s = 0;
722  for (unsigned int k = 0; k < rowNum; k++) {
723  s += (*(ptr + i)) * (*(ptr + i));
724  ptr += colNum;
725  }
726  *Bi = s;
727  }
728  }
729 }
730 
737 {
738  vpMatrix B;
739 
740  AtA(B);
741 
742  return B;
743 }
744 
762 {
763  resize(A.getRows(), A.getCols(), false, false);
764 
765  if (data != NULL && A.data != NULL && data != A.data) {
766  memcpy(data, A.data, dsize * sizeof(double));
767  }
768 
769  return *this;
770 }
771 
772 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
774 {
775  resize(A.getRows(), A.getCols(), false, false);
776 
777  if (data != NULL && A.data != NULL && data != A.data) {
778  memcpy(data, A.data, dsize * sizeof(double));
779  }
780 
781  return *this;
782 }
783 
785 {
786  if (this != &other) {
787  free(data);
788  free(rowPtrs);
789 
790  rowNum = other.rowNum;
791  colNum = other.colNum;
792  rowPtrs = other.rowPtrs;
793  dsize = other.dsize;
794  data = other.data;
795 
796  other.rowNum = 0;
797  other.colNum = 0;
798  other.rowPtrs = NULL;
799  other.dsize = 0;
800  other.data = NULL;
801  }
802 
803  return *this;
804 }
805 
830 vpMatrix& vpMatrix::operator=(const std::initializer_list<double> &list)
831 {
832  if (dsize != static_cast<unsigned int>(list.size())) {
833  resize(1, static_cast<unsigned int>(list.size()), false, false);
834  }
835 
836  std::copy(list.begin(), list.end(), data);
837 
838  return *this;
839 }
840 
864 vpMatrix& vpMatrix::operator=(const std::initializer_list<std::initializer_list<double> > &lists)
865 {
866  unsigned int nrows = static_cast<unsigned int>(lists.size()), ncols = 0;
867  for (auto& l : lists) {
868  if (static_cast<unsigned int>(l.size()) > ncols) {
869  ncols = static_cast<unsigned int>(l.size());
870  }
871  }
872 
873  resize(nrows, ncols, false, false);
874  auto it = lists.begin();
875  for (unsigned int i = 0; i < rowNum; i++, ++it) {
876  std::copy(it->begin(), it->end(), rowPtrs[i]);
877  }
878 
879  return *this;
880 }
881 #endif
882 
885 {
886  std::fill(data, data + rowNum*colNum, x);
887  return *this;
888 }
889 
896 {
897  for (unsigned int i = 0; i < rowNum; i++) {
898  for (unsigned int j = 0; j < colNum; j++) {
899  rowPtrs[i][j] = *x++;
900  }
901  }
902  return *this;
903 }
904 
906 {
907  resize(1, 1, false, false);
908  rowPtrs[0][0] = val;
909  return *this;
910 }
911 
913 {
914  resize(1, colNum + 1, false, false);
915  rowPtrs[0][colNum - 1] = val;
916  return *this;
917 }
918 
955 {
956  unsigned int rows = A.getRows();
957  this->resize(rows, rows);
958 
959  (*this) = 0;
960  for (unsigned int i = 0; i < rows; i++)
961  (*this)[i][i] = A[i];
962 }
963 
994 void vpMatrix::diag(const double &val)
995 {
996  (*this) = 0;
997  unsigned int min_ = (rowNum < colNum) ? rowNum : colNum;
998  for (unsigned int i = 0; i < min_; i++)
999  (*this)[i][i] = val;
1000 }
1001 
1014 {
1015  unsigned int rows = A.getRows();
1016  DA.resize(rows, rows, true);
1017 
1018  for (unsigned int i = 0; i < rows; i++)
1019  DA[i][i] = A[i];
1020 }
1021 
1027 {
1028  vpTranslationVector t_out;
1029 
1030  if (rowNum != 3 || colNum != 3) {
1031  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector",
1032  rowNum, colNum, tv.getRows(), tv.getCols()));
1033  }
1034 
1035  for (unsigned int j = 0; j < 3; j++)
1036  t_out[j] = 0;
1037 
1038  for (unsigned int j = 0; j < 3; j++) {
1039  double tj = tv[j]; // optimization em 5/12/2006
1040  for (unsigned int i = 0; i < 3; i++) {
1041  t_out[i] += rowPtrs[i][j] * tj;
1042  }
1043  }
1044  return t_out;
1045 }
1046 
1052 {
1053  vpColVector v_out;
1054  vpMatrix::multMatrixVector(*this, v, v_out);
1055  return v_out;
1056 }
1057 
1067 {
1068  if (A.colNum != v.getRows()) {
1069  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
1070  A.getRows(), A.getCols(), v.getRows()));
1071  }
1072 
1073  if (A.rowNum != w.rowNum)
1074  w.resize(A.rowNum, false);
1075 
1076  // If available use Lapack only for large matrices
1077  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size);
1078 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1079  useLapack = false;
1080 #endif
1081 
1082  if (useLapack) {
1083 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1084  double alpha = 1.0;
1085  double beta = 0.0;
1086  char trans = 't';
1087  int incr = 1;
1088 
1089  vpMatrix::blas_dgemv(trans, A.colNum, A.rowNum, alpha, A.data, A.colNum, v.data, incr, beta, w.data, incr);
1090 #endif
1091  }
1092  else {
1093  w = 0.0;
1094  for (unsigned int j = 0; j < A.colNum; j++) {
1095  double vj = v[j]; // optimization em 5/12/2006
1096  for (unsigned int i = 0; i < A.rowNum; i++) {
1097  w[i] += A.rowPtrs[i][j] * vj;
1098  }
1099  }
1100  }
1101 }
1102 
1103 //---------------------------------
1104 // Matrix operations.
1105 //---------------------------------
1106 
1117 {
1118  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1119  C.resize(A.rowNum, B.colNum, false, false);
1120 
1121  if (A.colNum != B.rowNum) {
1122  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (%dx%d) matrix", A.getRows(),
1123  A.getCols(), B.getRows(), B.getCols()));
1124  }
1125 
1126  // If available use Lapack only for large matrices
1127  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size || B.colNum > vpMatrix::m_lapack_min_size);
1128 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1129  useLapack = false;
1130 #endif
1131 
1132  if (useLapack) {
1133 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1134  const double alpha = 1.0;
1135  const double beta = 0.0;
1136  const char trans = 'n';
1137  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1138  C.data, B.colNum);
1139 #endif
1140  }
1141  else {
1142  // 5/12/06 some "very" simple optimization to avoid indexation
1143  const unsigned int BcolNum = B.colNum;
1144  const unsigned int BrowNum = B.rowNum;
1145  double **BrowPtrs = B.rowPtrs;
1146  for (unsigned int i = 0; i < A.rowNum; i++) {
1147  const double *rowptri = A.rowPtrs[i];
1148  double *ci = C[i];
1149  for (unsigned int j = 0; j < BcolNum; j++) {
1150  double s = 0;
1151  for (unsigned int k = 0; k < BrowNum; k++)
1152  s += rowptri[k] * BrowPtrs[k][j];
1153  ci[j] = s;
1154  }
1155  }
1156  }
1157 }
1158 
1173 {
1174  if (A.colNum != 3 || A.rowNum != 3 || B.colNum != 3 || B.rowNum != 3) {
1176  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1177  "rotation matrix",
1178  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1179  }
1180  // 5/12/06 some "very" simple optimization to avoid indexation
1181  const unsigned int BcolNum = B.colNum;
1182  const unsigned int BrowNum = B.rowNum;
1183  double **BrowPtrs = B.rowPtrs;
1184  for (unsigned int i = 0; i < A.rowNum; i++) {
1185  const double *rowptri = A.rowPtrs[i];
1186  double *ci = C[i];
1187  for (unsigned int j = 0; j < BcolNum; j++) {
1188  double s = 0;
1189  for (unsigned int k = 0; k < BrowNum; k++)
1190  s += rowptri[k] * BrowPtrs[k][j];
1191  ci[j] = s;
1192  }
1193  }
1194 }
1195 
1210 {
1211  if (A.colNum != 4 || A.rowNum != 4 || B.colNum != 4 || B.rowNum != 4) {
1213  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
1214  "rotation matrix",
1215  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
1216  }
1217  // Considering perfMatrixMultiplication.cpp benchmark,
1218  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1219  // Lapack usage needs to be validated again.
1220  // If available use Lapack only for large matrices.
1221  // Using SSE2 doesn't speed up.
1222  bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size || B.colNum > vpMatrix::m_lapack_min_size);
1223 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1224  useLapack = false;
1225 #endif
1226 
1227  if (useLapack) {
1228 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1229  const double alpha = 1.0;
1230  const double beta = 0.0;
1231  const char trans = 'n';
1232  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
1233  C.data, B.colNum);
1234 #endif
1235  }
1236  else {
1237  // 5/12/06 some "very" simple optimization to avoid indexation
1238  const unsigned int BcolNum = B.colNum;
1239  const unsigned int BrowNum = B.rowNum;
1240  double **BrowPtrs = B.rowPtrs;
1241  for (unsigned int i = 0; i < A.rowNum; i++) {
1242  const double *rowptri = A.rowPtrs[i];
1243  double *ci = C[i];
1244  for (unsigned int j = 0; j < BcolNum; j++) {
1245  double s = 0;
1246  for (unsigned int k = 0; k < BrowNum; k++)
1247  s += rowptri[k] * BrowPtrs[k][j];
1248  ci[j] = s;
1249  }
1250  }
1251  }
1252 }
1253 
1267 {
1268  vpMatrix::multMatrixVector(A, B, C);
1269 }
1270 
1276 {
1277  vpMatrix C;
1278 
1279  vpMatrix::mult2Matrices(*this, B, C);
1280 
1281  return C;
1282 }
1283 
1289 {
1290  if (colNum != R.getRows()) {
1291  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1292  colNum));
1293  }
1294  vpMatrix C;
1295  C.resize(rowNum, 3, false, false);
1296 
1297  unsigned int RcolNum = R.getCols();
1298  unsigned int RrowNum = R.getRows();
1299  for (unsigned int i = 0; i < rowNum; i++) {
1300  double *rowptri = rowPtrs[i];
1301  double *ci = C[i];
1302  for (unsigned int j = 0; j < RcolNum; j++) {
1303  double s = 0;
1304  for (unsigned int k = 0; k < RrowNum; k++)
1305  s += rowptri[k] * R[k][j];
1306  ci[j] = s;
1307  }
1308  }
1309 
1310  return C;
1311 }
1312 
1318 {
1319  if (colNum != M.getRows()) {
1320  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
1321  colNum));
1322  }
1323  vpMatrix C;
1324  C.resize(rowNum, 4, false, false);
1325 
1326  const unsigned int McolNum = M.getCols();
1327  const unsigned int MrowNum = M.getRows();
1328  for (unsigned int i = 0; i < rowNum; i++) {
1329  const double *rowptri = rowPtrs[i];
1330  double *ci = C[i];
1331  for (unsigned int j = 0; j < McolNum; j++) {
1332  double s = 0;
1333  for (unsigned int k = 0; k < MrowNum; k++)
1334  s += rowptri[k] * M[k][j];
1335  ci[j] = s;
1336  }
1337  }
1338 
1339  return C;
1340 }
1341 
1347 {
1348  if (colNum != V.getRows()) {
1349  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix",
1350  rowNum, colNum));
1351  }
1352  vpMatrix M;
1353  M.resize(rowNum, 6, false, false);
1354 
1355  // Considering perfMatrixMultiplication.cpp benchmark,
1356  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1357  // Lapack usage needs to be validated again.
1358  // If available use Lapack only for large matrices.
1359  // Speed up obtained using SSE2.
1360  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size || V.colNum > vpMatrix::m_lapack_min_size);
1361 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1362  useLapack = false;
1363 #endif
1364 
1365  if (useLapack) {
1366 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1367  const double alpha = 1.0;
1368  const double beta = 0.0;
1369  const char trans = 'n';
1370  vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, V.colNum, data, colNum, beta,
1371  M.data, M.colNum);
1372 #endif
1373  }
1374  else {
1375  bool checkSSE2 = vpCPUFeatures::checkSSE2();
1376  #if !VISP_HAVE_SSE2
1377  checkSSE2 = false;
1378  #endif
1379 
1380  if (checkSSE2) {
1381  #if VISP_HAVE_SSE2
1382  vpMatrix V_trans;
1383  V_trans.resize(6, 6, false, false);
1384  for (unsigned int i = 0; i < 6; i++) {
1385  for (unsigned int j = 0; j < 6; j++) {
1386  V_trans[i][j] = V[j][i];
1387  }
1388  }
1389 
1390  for (unsigned int i = 0; i < rowNum; i++) {
1391  double *rowptri = rowPtrs[i];
1392  double *ci = M[i];
1393 
1394  for (int j = 0; j < 6; j++) {
1395  __m128d v_mul = _mm_setzero_pd();
1396  for (int k = 0; k < 6; k += 2) {
1397  v_mul = _mm_add_pd(v_mul, _mm_mul_pd(_mm_loadu_pd(&rowptri[k]), _mm_loadu_pd(&V_trans[j][k])));
1398  }
1399 
1400  double v_tmp[2];
1401  _mm_storeu_pd(v_tmp, v_mul);
1402  ci[j] = v_tmp[0] + v_tmp[1];
1403  }
1404  }
1405  #endif
1406  } else {
1407  const unsigned int VcolNum = V.getCols();
1408  const unsigned int VrowNum = V.getRows();
1409  for (unsigned int i = 0; i < rowNum; i++) {
1410  const double *rowptri = rowPtrs[i];
1411  double *ci = M[i];
1412  for (unsigned int j = 0; j < VcolNum; j++) {
1413  double s = 0;
1414  for (unsigned int k = 0; k < VrowNum; k++)
1415  s += rowptri[k] * V[k][j];
1416  ci[j] = s;
1417  }
1418  }
1419  }
1420  }
1421 
1422  return M;
1423 }
1424 
1430 {
1431  if (colNum != V.getRows()) {
1432  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) force/torque twist matrix",
1433  rowNum, colNum));
1434  }
1435  vpMatrix M;
1436  M.resize(rowNum, 6, false, false);
1437 
1438  // Considering perfMatrixMultiplication.cpp benchmark,
1439  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
1440  // Lapack usage needs to be validated again.
1441  // If available use Lapack only for large matrices.
1442  // Speed up obtained using SSE2.
1443  bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size || V.getCols() > vpMatrix::m_lapack_min_size);
1444 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
1445  useLapack = false;
1446 #endif
1447 
1448  if (useLapack) {
1449 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
1450  const double alpha = 1.0;
1451  const double beta = 0.0;
1452  const char trans = 'n';
1453  vpMatrix::blas_dgemm(trans, trans, V.getCols(), rowNum, colNum, alpha, V.data, V.getCols(), data, colNum, beta,
1454  M.data, M.colNum);
1455 #endif
1456  }
1457  else {
1458  bool checkSSE2 = vpCPUFeatures::checkSSE2();
1459  #if !VISP_HAVE_SSE2
1460  checkSSE2 = false;
1461  #endif
1462 
1463  if (checkSSE2) {
1464  #if VISP_HAVE_SSE2
1465  vpMatrix V_trans;
1466  V_trans.resize(6, 6, false, false);
1467  for (unsigned int i = 0; i < 6; i++) {
1468  for (unsigned int j = 0; j < 6; j++) {
1469  V_trans[i][j] = V[j][i];
1470  }
1471  }
1472 
1473  for (unsigned int i = 0; i < rowNum; i++) {
1474  double *rowptri = rowPtrs[i];
1475  double *ci = M[i];
1476 
1477  for (int j = 0; j < 6; j++) {
1478  __m128d v_mul = _mm_setzero_pd();
1479  for (int k = 0; k < 6; k += 2) {
1480  v_mul = _mm_add_pd(v_mul, _mm_mul_pd(_mm_loadu_pd(&rowptri[k]), _mm_loadu_pd(&V_trans[j][k])));
1481  }
1482 
1483  double v_tmp[2];
1484  _mm_storeu_pd(v_tmp, v_mul);
1485  ci[j] = v_tmp[0] + v_tmp[1];
1486  }
1487  }
1488  #endif
1489  } else {
1490  const unsigned int VcolNum = V.getCols();
1491  const unsigned int VrowNum = V.getRows();
1492  for (unsigned int i = 0; i < rowNum; i++) {
1493  const double *rowptri = rowPtrs[i];
1494  double *ci = M[i];
1495  for (unsigned int j = 0; j < VcolNum; j++) {
1496  double s = 0;
1497  for (unsigned int k = 0; k < VrowNum; k++)
1498  s += rowptri[k] * V[k][j];
1499  ci[j] = s;
1500  }
1501  }
1502  }
1503  }
1504 
1505  return M;
1506 }
1507 
1518 void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB,
1519  vpMatrix &C)
1520 {
1521  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1522  C.resize(A.rowNum, B.colNum, false, false);
1523 
1524  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1525  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1526  A.getCols(), B.getRows(), B.getCols()));
1527  }
1528 
1529  double **ArowPtrs = A.rowPtrs;
1530  double **BrowPtrs = B.rowPtrs;
1531  double **CrowPtrs = C.rowPtrs;
1532 
1533  for (unsigned int i = 0; i < A.rowNum; i++)
1534  for (unsigned int j = 0; j < A.colNum; j++)
1535  CrowPtrs[i][j] = wB * BrowPtrs[i][j] + wA * ArowPtrs[i][j];
1536 }
1537 
1547 void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1548 {
1549  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1550  C.resize(A.rowNum, B.colNum, false, false);
1551 
1552  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1553  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1554  A.getCols(), B.getRows(), B.getCols()));
1555  }
1556 
1557  double **ArowPtrs = A.rowPtrs;
1558  double **BrowPtrs = B.rowPtrs;
1559  double **CrowPtrs = C.rowPtrs;
1560 
1561  for (unsigned int i = 0; i < A.rowNum; i++) {
1562  for (unsigned int j = 0; j < A.colNum; j++) {
1563  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1564  }
1565  }
1566 }
1567 
1581 {
1582  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
1583  C.resize(A.rowNum);
1584 
1585  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1586  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
1587  A.getCols(), B.getRows(), B.getCols()));
1588  }
1589 
1590  double **ArowPtrs = A.rowPtrs;
1591  double **BrowPtrs = B.rowPtrs;
1592  double **CrowPtrs = C.rowPtrs;
1593 
1594  for (unsigned int i = 0; i < A.rowNum; i++) {
1595  for (unsigned int j = 0; j < A.colNum; j++) {
1596  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
1597  }
1598  }
1599 }
1600 
1606 {
1607  vpMatrix C;
1608  vpMatrix::add2Matrices(*this, B, C);
1609  return C;
1610 }
1611 
1628 {
1629  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1630  C.resize(A.rowNum);
1631 
1632  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1633  throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1634  A.getCols(), B.getRows(), B.getCols()));
1635  }
1636 
1637  double **ArowPtrs = A.rowPtrs;
1638  double **BrowPtrs = B.rowPtrs;
1639  double **CrowPtrs = C.rowPtrs;
1640 
1641  for (unsigned int i = 0; i < A.rowNum; i++) {
1642  for (unsigned int j = 0; j < A.colNum; j++) {
1643  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1644  }
1645  }
1646 }
1647 
1660 void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
1661 {
1662  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1663  C.resize(A.rowNum, A.colNum, false, false);
1664 
1665  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
1666  throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
1667  A.getCols(), B.getRows(), B.getCols()));
1668  }
1669 
1670  double **ArowPtrs = A.rowPtrs;
1671  double **BrowPtrs = B.rowPtrs;
1672  double **CrowPtrs = C.rowPtrs;
1673 
1674  for (unsigned int i = 0; i < A.rowNum; i++) {
1675  for (unsigned int j = 0; j < A.colNum; j++) {
1676  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
1677  }
1678  }
1679 }
1680 
1686 {
1687  vpMatrix C;
1688  vpMatrix::sub2Matrices(*this, B, C);
1689  return C;
1690 }
1691 
1693 
1695 {
1696  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1697  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1698  B.getRows(), B.getCols()));
1699  }
1700 
1701  double **BrowPtrs = B.rowPtrs;
1702 
1703  for (unsigned int i = 0; i < rowNum; i++)
1704  for (unsigned int j = 0; j < colNum; j++)
1705  rowPtrs[i][j] += BrowPtrs[i][j];
1706 
1707  return *this;
1708 }
1709 
1712 {
1713  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
1714  throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
1715  B.getRows(), B.getCols()));
1716  }
1717 
1718  double **BrowPtrs = B.rowPtrs;
1719  for (unsigned int i = 0; i < rowNum; i++)
1720  for (unsigned int j = 0; j < colNum; j++)
1721  rowPtrs[i][j] -= BrowPtrs[i][j];
1722 
1723  return *this;
1724 }
1725 
1736 {
1737  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
1738  C.resize(A.rowNum, A.colNum, false, false);
1739 
1740  double **ArowPtrs = A.rowPtrs;
1741  double **CrowPtrs = C.rowPtrs;
1742 
1743  // t0=vpTime::measureTimeMicros();
1744  for (unsigned int i = 0; i < A.rowNum; i++)
1745  for (unsigned int j = 0; j < A.colNum; j++)
1746  CrowPtrs[i][j] = -ArowPtrs[i][j];
1747 }
1748 
1754 {
1755  vpMatrix C;
1756  vpMatrix::negateMatrix(*this, C);
1757  return C;
1758 }
1759 
1760 double vpMatrix::sum() const
1761 {
1762  double s = 0.0;
1763  for (unsigned int i = 0; i < rowNum; i++) {
1764  for (unsigned int j = 0; j < colNum; j++) {
1765  s += rowPtrs[i][j];
1766  }
1767  }
1768 
1769  return s;
1770 }
1771 
1772 //---------------------------------
1773 // Matrix/vector operations.
1774 //---------------------------------
1775 
1776 //---------------------------------
1777 // Matrix/real operations.
1778 //---------------------------------
1779 
1784 vpMatrix operator*(const double &x, const vpMatrix &B)
1785 {
1786  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1787  return B;
1788  }
1789 
1790  unsigned int Brow = B.getRows();
1791  unsigned int Bcol = B.getCols();
1792 
1793  vpMatrix C;
1794  C.resize(Brow, Bcol, false, false);
1795 
1796  for (unsigned int i = 0; i < Brow; i++)
1797  for (unsigned int j = 0; j < Bcol; j++)
1798  C[i][j] = B[i][j] * x;
1799 
1800  return C;
1801 }
1802 
1808 {
1809  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1810  return (*this);
1811  }
1812 
1813  vpMatrix M;
1814  M.resize(rowNum, colNum, false, false);
1815 
1816  for (unsigned int i = 0; i < rowNum; i++)
1817  for (unsigned int j = 0; j < colNum; j++)
1818  M[i][j] = rowPtrs[i][j] * x;
1819 
1820  return M;
1821 }
1822 
1825 {
1826  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1827  return (*this);
1828  }
1829 
1830  if (std::fabs(x) < std::numeric_limits<double>::epsilon()) {
1831  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1832  }
1833 
1834  vpMatrix C;
1835  C.resize(rowNum, colNum, false, false);
1836 
1837  double xinv = 1 / x;
1838 
1839  for (unsigned int i = 0; i < rowNum; i++)
1840  for (unsigned int j = 0; j < colNum; j++)
1841  C[i][j] = rowPtrs[i][j] * xinv;
1842 
1843  return C;
1844 }
1845 
1848 {
1849  for (unsigned int i = 0; i < rowNum; i++)
1850  for (unsigned int j = 0; j < colNum; j++)
1851  rowPtrs[i][j] += x;
1852 
1853  return *this;
1854 }
1855 
1858 {
1859  for (unsigned int i = 0; i < rowNum; i++)
1860  for (unsigned int j = 0; j < colNum; j++)
1861  rowPtrs[i][j] -= x;
1862 
1863  return *this;
1864 }
1865 
1871 {
1872  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1873  return *this;
1874  }
1875 
1876  for (unsigned int i = 0; i < rowNum; i++)
1877  for (unsigned int j = 0; j < colNum; j++)
1878  rowPtrs[i][j] *= x;
1879 
1880  return *this;
1881 }
1882 
1885 {
1886  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
1887  return *this;
1888  }
1889 
1890  if (std::fabs(x) < std::numeric_limits<double>::epsilon())
1891  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
1892 
1893  double xinv = 1 / x;
1894 
1895  for (unsigned int i = 0; i < rowNum; i++)
1896  for (unsigned int j = 0; j < colNum; j++)
1897  rowPtrs[i][j] *= xinv;
1898 
1899  return *this;
1900 }
1901 
1902 //----------------------------------------------------------------
1903 // Matrix Operation
1904 //----------------------------------------------------------------
1905 
1911 {
1912  if ((out.rowNum != colNum * rowNum) || (out.colNum != 1))
1913  out.resize(colNum * rowNum, false, false);
1914 
1915  double *optr = out.data;
1916  for (unsigned int j = 0; j < colNum; j++) {
1917  for (unsigned int i = 0; i < rowNum; i++) {
1918  *(optr++) = rowPtrs[i][j];
1919  }
1920  }
1921 }
1922 
1928 {
1929  vpColVector out(colNum * rowNum);
1930  stackColumns(out);
1931  return out;
1932 }
1933 
1939 {
1940  if ((out.getRows() != 1) || (out.getCols() != colNum * rowNum))
1941  out.resize(colNum * rowNum, false, false);
1942 
1943  memcpy(out.data, data, sizeof(double)*out.getCols());
1944 }
1950 {
1951  vpRowVector out(colNum * rowNum);
1952  stackRows(out);
1953  return out;
1954 }
1955 
1963 {
1964  if (m.getRows() != rowNum || m.getCols() != colNum) {
1965  throw(vpException(vpException::dimensionError, "In Hadamard product: bad dimension of input matrix"));
1966  }
1967 
1968  vpMatrix out;
1969  out.resize(rowNum, colNum, false, false);
1970 
1971  unsigned int i = 0;
1972 
1973 #if VISP_HAVE_SSE2
1974  if (vpCPUFeatures::checkSSE2() && dsize >= 2) {
1975  for (; i <= dsize - 2; i += 2) {
1976  __m128d vout = _mm_mul_pd(_mm_loadu_pd(data + i), _mm_loadu_pd(m.data + i));
1977  _mm_storeu_pd(out.data + i, vout);
1978  }
1979  }
1980 #endif
1981 
1982  for (; i < dsize; i++) {
1983  out.data[i] = data[i] * m.data[i];
1984  }
1985 
1986  return out;
1987 }
1988 
1995 void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out)
1996 {
1997  unsigned int r1 = m1.getRows();
1998  unsigned int c1 = m1.getCols();
1999  unsigned int r2 = m2.getRows();
2000  unsigned int c2 = m2.getCols();
2001 
2002  out.resize(r1*r2, c1*c2, false, false);
2003 
2004  for (unsigned int r = 0; r < r1; r++) {
2005  for (unsigned int c = 0; c < c1; c++) {
2006  double alpha = m1[r][c];
2007  double *m2ptr = m2[0];
2008  unsigned int roffset = r * r2;
2009  unsigned int coffset = c * c2;
2010  for (unsigned int rr = 0; rr < r2; rr++) {
2011  for (unsigned int cc = 0; cc < c2; cc++) {
2012  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
2013  }
2014  }
2015  }
2016  }
2017 }
2018 
2025 void vpMatrix::kron(const vpMatrix &m, vpMatrix &out) const { kron(*this, m, out); }
2026 
2034 {
2035  unsigned int r1 = m1.getRows();
2036  unsigned int c1 = m1.getCols();
2037  unsigned int r2 = m2.getRows();
2038  unsigned int c2 = m2.getCols();
2039 
2040  vpMatrix out;
2041  out.resize(r1 * r2, c1 * c2, false, false);
2042 
2043  for (unsigned int r = 0; r < r1; r++) {
2044  for (unsigned int c = 0; c < c1; c++) {
2045  double alpha = m1[r][c];
2046  double *m2ptr = m2[0];
2047  unsigned int roffset = r * r2;
2048  unsigned int coffset = c * c2;
2049  for (unsigned int rr = 0; rr < r2; rr++) {
2050  for (unsigned int cc = 0; cc < c2; cc++) {
2051  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
2052  }
2053  }
2054  }
2055  }
2056  return out;
2057 }
2058 
2064 vpMatrix vpMatrix::kron(const vpMatrix &m) const { return kron(*this, m); }
2065 
2116 void vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const { x = pseudoInverse(1e-6) * b; }
2117 
2168 {
2169  vpColVector X(colNum);
2170 
2171  solveBySVD(B, X);
2172  return X;
2173 }
2174 
2239 {
2240 #if defined(VISP_HAVE_LAPACK)
2241  svdLapack(w, V);
2242 #elif defined(VISP_HAVE_EIGEN3)
2243  svdEigen3(w, V);
2244 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2245  svdOpenCV(w, V);
2246 #else
2247  (void)w;
2248  (void)V;
2249  throw(vpException(vpException::fatalError, "Cannot compute SVD. Install Lapack, Eigen3 or OpenCV 3rd party"));
2250 #endif
2251 }
2252 
2307 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
2308 {
2309 #if defined(VISP_HAVE_LAPACK)
2310  return pseudoInverseLapack(Ap, svThreshold);
2311 #elif defined(VISP_HAVE_EIGEN3)
2312  return pseudoInverseEigen3(Ap, svThreshold);
2313 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2314  return pseudoInverseOpenCV(Ap, svThreshold);
2315 #else
2316  (void)Ap;
2317  (void)svThreshold;
2318  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2319  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2320 #endif
2321 }
2322 
2383 int vpMatrix::pseudoInverse(vpMatrix &Ap, int rank_in) const
2384 {
2385 #if defined(VISP_HAVE_LAPACK)
2386  return pseudoInverseLapack(Ap, rank_in);
2387 #elif defined(VISP_HAVE_EIGEN3)
2388  return pseudoInverseEigen3(Ap, rank_in);
2389 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2390  return pseudoInverseOpenCV(Ap, rank_in);
2391 #else
2392  (void)Ap;
2393  (void)svThreshold;
2394  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2395  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2396 #endif
2397 }
2398 
2449 vpMatrix vpMatrix::pseudoInverse(double svThreshold) const
2450 {
2451 #if defined(VISP_HAVE_LAPACK)
2452  return pseudoInverseLapack(svThreshold);
2453 #elif defined(VISP_HAVE_EIGEN3)
2454  return pseudoInverseEigen3(svThreshold);
2455 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2456  return pseudoInverseOpenCV(svThreshold);
2457 #else
2458  (void)svThreshold;
2459  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2460  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2461 #endif
2462 }
2463 
2515 {
2516 #if defined(VISP_HAVE_LAPACK)
2517  return pseudoInverseLapack(rank_in);
2518 #elif defined(VISP_HAVE_EIGEN3)
2519  return pseudoInverseEigen3(rank_in);
2520 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
2521  return pseudoInverseOpenCV(rank_in);
2522 #else
2523  (void)svThreshold;
2524  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
2525  "Install Lapack, Eigen3 or OpenCV 3rd party"));
2526 #endif
2527 }
2528 
2529 #ifndef DOXYGEN_SHOULD_SKIP_THIS
2530 #if defined(VISP_HAVE_LAPACK)
2531 
2567 vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const
2568 {
2569  unsigned int nrows = getRows();
2570  unsigned int ncols = getCols();
2571  int rank_out;
2572 
2573  vpMatrix U, V, Ap;
2574  vpColVector sv;
2575 
2576  Ap.resize(ncols, nrows, false, false);
2577 
2578  if (nrows < ncols) {
2579  U.resize(ncols, ncols, true);
2580  sv.resize(nrows, false);
2581  } else {
2582  U.resize(nrows, ncols, false);
2583  sv.resize(ncols, false);
2584  }
2585 
2586  U.insert(*this, 0, 0);
2587  U.svdLapack(sv, V);
2588 
2589  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
2590 
2591  return Ap;
2592 }
2593 
2634 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) const
2635 {
2636  unsigned int nrows = getRows();
2637  unsigned int ncols = getCols();
2638  int rank_out;
2639 
2640  vpMatrix U, V;
2641  vpColVector sv;
2642 
2643  Ap.resize(ncols, nrows, false, false);
2644 
2645  if (nrows < ncols) {
2646  U.resize(ncols, ncols, true);
2647  sv.resize(nrows, false);
2648  } else {
2649  U.resize(nrows, ncols, false);
2650  sv.resize(ncols, false);
2651  }
2652 
2653  U.insert(*this, 0, 0);
2654  U.svdLapack(sv, V);
2655 
2656  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
2657 
2658  return static_cast<unsigned int>(rank_out);
2659 }
2707 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
2708 {
2709  unsigned int nrows = getRows();
2710  unsigned int ncols = getCols();
2711  int rank_out;
2712 
2713  vpMatrix U, V;
2714  vpColVector sv_;
2715 
2716  Ap.resize(ncols, nrows, false, false);
2717 
2718  if (nrows < ncols) {
2719  U.resize(ncols, ncols, true);
2720  sv.resize(nrows, false);
2721  } else {
2722  U.resize(nrows, ncols, false);
2723  sv.resize(ncols, false);
2724  }
2725 
2726  U.insert(*this, 0, 0);
2727  U.svdLapack(sv_, V);
2728 
2729  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
2730 
2731  // Remove singular values equal to the one that corresponds to the lines of 0
2732  // introduced when m < n
2733  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2734 
2735  return static_cast<unsigned int>(rank_out);
2736 }
2737 
2844 unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
2845  vpMatrix &imAt, vpMatrix &kerA) const
2846 {
2847  unsigned int nrows = getRows();
2848  unsigned int ncols = getCols();
2849  int rank_out;
2850  vpMatrix U, V;
2851  vpColVector sv_;
2852 
2853  if (nrows < ncols) {
2854  U.resize(ncols, ncols, true);
2855  sv.resize(nrows, false);
2856  } else {
2857  U.resize(nrows, ncols, false);
2858  sv.resize(ncols, false);
2859  }
2860 
2861  U.insert(*this, 0, 0);
2862  U.svdLapack(sv_, V);
2863 
2864  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerA);
2865 
2866  // Remove singular values equal to the one that corresponds to the lines of 0
2867  // introduced when m < n
2868  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
2869 
2870  return static_cast<unsigned int>(rank_out);
2871 }
2872 
2913 vpMatrix vpMatrix::pseudoInverseLapack(int rank_in) const
2914 {
2915  unsigned int nrows = getRows();
2916  unsigned int ncols = getCols();
2917  int rank_out;
2918  double svThreshold = 1e-26;
2919 
2920  vpMatrix U, V, Ap;
2921  vpColVector sv;
2922 
2923  Ap.resize(ncols, nrows, false, false);
2924 
2925  if (nrows < ncols) {
2926  U.resize(ncols, ncols, true);
2927  sv.resize(nrows, false);
2928  } else {
2929  U.resize(nrows, ncols, false);
2930  sv.resize(ncols, false);
2931  }
2932 
2933  U.insert(*this, 0, 0);
2934  U.svdLapack(sv, V);
2935 
2936  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
2937 
2938  return Ap;
2939 }
2940 
2987 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, int rank_in) const
2988 {
2989  unsigned int nrows = getRows();
2990  unsigned int ncols = getCols();
2991  int rank_out;
2992  double svThreshold = 1e-26;
2993 
2994  vpMatrix U, V;
2995  vpColVector sv;
2996 
2997  Ap.resize(ncols, nrows, false, false);
2998 
2999  if (nrows < ncols) {
3000  U.resize(ncols, ncols, true);
3001  sv.resize(nrows, false);
3002  } else {
3003  U.resize(nrows, ncols, false);
3004  sv.resize(ncols, false);
3005  }
3006 
3007  U.insert(*this, 0, 0);
3008  U.svdLapack(sv, V);
3009 
3010  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3011 
3012  return rank_out;
3013 }
3014 
3068 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in) const
3069 {
3070  unsigned int nrows = getRows();
3071  unsigned int ncols = getCols();
3072  int rank_out;
3073  double svThreshold = 1e-26;
3074 
3075  vpMatrix U, V;
3076  vpColVector sv_;
3077 
3078  Ap.resize(ncols, nrows, false, false);
3079 
3080  if (nrows < ncols) {
3081  U.resize(ncols, ncols, true);
3082  sv.resize(nrows, false);
3083  } else {
3084  U.resize(nrows, ncols, false);
3085  sv.resize(ncols, false);
3086  }
3087 
3088  U.insert(*this, 0, 0);
3089  U.svdLapack(sv_, V);
3090 
3091  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3092 
3093  // Remove singular values equal to the one that corresponds to the lines of 0
3094  // introduced when m < n
3095  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3096 
3097  return rank_out;
3098 }
3099 
3211 int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA,
3212  vpMatrix &imAt, vpMatrix &kerA) const
3213 {
3214  unsigned int nrows = getRows();
3215  unsigned int ncols = getCols();
3216  int rank_out;
3217  double svThreshold = 1e-26;
3218  vpMatrix U, V;
3219  vpColVector sv_;
3220 
3221  if (nrows < ncols) {
3222  U.resize(ncols, ncols, true);
3223  sv.resize(nrows, false);
3224  } else {
3225  U.resize(nrows, ncols, false);
3226  sv.resize(ncols, false);
3227  }
3228 
3229  U.insert(*this, 0, 0);
3230  U.svdLapack(sv_, V);
3231 
3232  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerA);
3233 
3234  // Remove singular values equal to the one that corresponds to the lines of 0
3235  // introduced when m < n
3236  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3237 
3238  return rank_out;
3239 }
3240 
3241 #endif
3242 #if defined(VISP_HAVE_EIGEN3)
3243 
3279 vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const
3280 {
3281  unsigned int nrows = getRows();
3282  unsigned int ncols = getCols();
3283  int rank_out;
3284 
3285  vpMatrix U, V, Ap;
3286  vpColVector sv;
3287 
3288  Ap.resize(ncols, nrows, false, false);
3289 
3290  if (nrows < ncols) {
3291  U.resize(ncols, ncols, true);
3292  sv.resize(nrows, false);
3293  } else {
3294  U.resize(nrows, ncols, false);
3295  sv.resize(ncols, false);
3296  }
3297 
3298  U.insert(*this, 0, 0);
3299  U.svdEigen3(sv, V);
3300 
3301  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3302 
3303  return Ap;
3304 }
3305 
3346 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) const
3347 {
3348  unsigned int nrows = getRows();
3349  unsigned int ncols = getCols();
3350  int rank_out;
3351 
3352  vpMatrix U, V;
3353  vpColVector sv;
3354 
3355  Ap.resize(ncols, nrows, false, false);
3356 
3357  if (nrows < ncols) {
3358  U.resize(ncols, ncols, true);
3359  sv.resize(nrows, false);
3360  } else {
3361  U.resize(nrows, ncols, false);
3362  sv.resize(ncols, false);
3363  }
3364 
3365  U.insert(*this, 0, 0);
3366  U.svdEigen3(sv, V);
3367 
3368  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3369 
3370  return static_cast<unsigned int>(rank_out);
3371 }
3419 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
3420 {
3421  unsigned int nrows = getRows();
3422  unsigned int ncols = getCols();
3423  int rank_out;
3424 
3425  vpMatrix U, V;
3426  vpColVector sv_;
3427 
3428  Ap.resize(ncols, nrows, false, false);
3429 
3430  if (nrows < ncols) {
3431  U.resize(ncols, ncols, true);
3432  sv.resize(nrows, false);
3433  } else {
3434  U.resize(nrows, ncols, false);
3435  sv.resize(ncols, false);
3436  }
3437 
3438  U.insert(*this, 0, 0);
3439  U.svdEigen3(sv_, V);
3440 
3441  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
3442 
3443  // Remove singular values equal to the one that corresponds to the lines of 0
3444  // introduced when m < n
3445  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3446 
3447  return static_cast<unsigned int>(rank_out);
3448 }
3449 
3556 unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
3557  vpMatrix &imAt, vpMatrix &kerA) const
3558 {
3559  unsigned int nrows = getRows();
3560  unsigned int ncols = getCols();
3561  int rank_out;
3562  vpMatrix U, V;
3563  vpColVector sv_;
3564 
3565  if (nrows < ncols) {
3566  U.resize(ncols, ncols, true);
3567  sv.resize(nrows, false);
3568  } else {
3569  U.resize(nrows, ncols, false);
3570  sv.resize(ncols, false);
3571  }
3572 
3573  U.insert(*this, 0, 0);
3574  U.svdEigen3(sv_, V);
3575 
3576  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerA);
3577 
3578  // Remove singular values equal to the one that corresponds to the lines of 0
3579  // introduced when m < n
3580  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3581 
3582  return static_cast<unsigned int>(rank_out);
3583 }
3584 
3625 vpMatrix vpMatrix::pseudoInverseEigen3(int rank_in) const
3626 {
3627  unsigned int nrows = getRows();
3628  unsigned int ncols = getCols();
3629  int rank_out;
3630  double svThreshold = 1e-26;
3631 
3632  vpMatrix U, V, Ap;
3633  vpColVector sv;
3634 
3635  Ap.resize(ncols, nrows, false, false);
3636 
3637  if (nrows < ncols) {
3638  U.resize(ncols, ncols, true);
3639  sv.resize(nrows, false);
3640  } else {
3641  U.resize(nrows, ncols, false);
3642  sv.resize(ncols, false);
3643  }
3644 
3645  U.insert(*this, 0, 0);
3646  U.svdEigen3(sv, V);
3647 
3648  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3649 
3650  return Ap;
3651 }
3652 
3699 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, int rank_in) const
3700 {
3701  unsigned int nrows = getRows();
3702  unsigned int ncols = getCols();
3703  int rank_out;
3704  double svThreshold = 1e-26;
3705 
3706  vpMatrix U, V;
3707  vpColVector sv;
3708 
3709  Ap.resize(ncols, nrows, false, false);
3710 
3711  if (nrows < ncols) {
3712  U.resize(ncols, ncols, true);
3713  sv.resize(nrows, false);
3714  } else {
3715  U.resize(nrows, ncols, false);
3716  sv.resize(ncols, false);
3717  }
3718 
3719  U.insert(*this, 0, 0);
3720  U.svdEigen3(sv, V);
3721 
3722  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3723 
3724  return rank_out;
3725 }
3726 
3780 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in) const
3781 {
3782  unsigned int nrows = getRows();
3783  unsigned int ncols = getCols();
3784  int rank_out;
3785  double svThreshold = 1e-26;
3786 
3787  vpMatrix U, V;
3788  vpColVector sv_;
3789 
3790  Ap.resize(ncols, nrows, false, false);
3791 
3792  if (nrows < ncols) {
3793  U.resize(ncols, ncols, true);
3794  sv.resize(nrows, false);
3795  } else {
3796  U.resize(nrows, ncols, false);
3797  sv.resize(ncols, false);
3798  }
3799 
3800  U.insert(*this, 0, 0);
3801  U.svdEigen3(sv_, V);
3802 
3803  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
3804 
3805  // Remove singular values equal to the one that corresponds to the lines of 0
3806  // introduced when m < n
3807  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3808 
3809  return rank_out;
3810 }
3811 
3923 int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA,
3924  vpMatrix &imAt, vpMatrix &kerA) const
3925 {
3926  unsigned int nrows = getRows();
3927  unsigned int ncols = getCols();
3928  int rank_out;
3929  double svThreshold = 1e-26;
3930  vpMatrix U, V;
3931  vpColVector sv_;
3932 
3933  if (nrows < ncols) {
3934  U.resize(ncols, ncols, true);
3935  sv.resize(nrows, false);
3936  } else {
3937  U.resize(nrows, ncols, false);
3938  sv.resize(ncols, false);
3939  }
3940 
3941  U.insert(*this, 0, 0);
3942  U.svdEigen3(sv_, V);
3943 
3944  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerA);
3945 
3946  // Remove singular values equal to the one that corresponds to the lines of 0
3947  // introduced when m < n
3948  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
3949 
3950  return rank_out;
3951 }
3952 
3953 #endif
3954 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
3955 
3991 vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const
3992 {
3993  unsigned int nrows = getRows();
3994  unsigned int ncols = getCols();
3995  int rank_out;
3996 
3997  vpMatrix U, V, Ap;
3998  vpColVector sv;
3999 
4000  Ap.resize(ncols, nrows, false, false);
4001 
4002  if (nrows < ncols) {
4003  U.resize(ncols, ncols, true);
4004  sv.resize(nrows, false);
4005  } else {
4006  U.resize(nrows, ncols, false);
4007  sv.resize(ncols, false);
4008  }
4009 
4010  U.insert(*this, 0, 0);
4011  U.svdOpenCV(sv, V);
4012 
4013  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
4014 
4015  return Ap;
4016 }
4017 
4058 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) const
4059 {
4060  unsigned int nrows = getRows();
4061  unsigned int ncols = getCols();
4062  int rank_out;
4063 
4064  vpMatrix U, V;
4065  vpColVector sv;
4066 
4067  Ap.resize(ncols, nrows, false, false);
4068 
4069  if (nrows < ncols) {
4070  U.resize(ncols, ncols, true);
4071  sv.resize(nrows, false);
4072  } else {
4073  U.resize(nrows, ncols, false);
4074  sv.resize(ncols, false);
4075  }
4076 
4077  U.insert(*this, 0, 0);
4078  U.svdOpenCV(sv, V);
4079 
4080  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
4081 
4082  return static_cast<unsigned int>(rank_out);
4083 }
4131 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
4132 {
4133  unsigned int nrows = getRows();
4134  unsigned int ncols = getCols();
4135  int rank_out;
4136 
4137  vpMatrix U, V;
4138  vpColVector sv_;
4139 
4140  Ap.resize(ncols, nrows, false, false);
4141 
4142  if (nrows < ncols) {
4143  U.resize(ncols, ncols, true);
4144  sv.resize(nrows, false);
4145  } else {
4146  U.resize(nrows, ncols, false);
4147  sv.resize(ncols, false);
4148  }
4149 
4150  U.insert(*this, 0, 0);
4151  U.svdOpenCV(sv_, V);
4152 
4153  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, NULL, NULL, NULL);
4154 
4155  // Remove singular values equal to the one that corresponds to the lines of 0
4156  // introduced when m < n
4157  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4158 
4159  return static_cast<unsigned int>(rank_out);
4160 }
4161 
4268 unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
4269  vpMatrix &imAt, vpMatrix &kerA) const
4270 {
4271  unsigned int nrows = getRows();
4272  unsigned int ncols = getCols();
4273  int rank_out;
4274  vpMatrix U, V;
4275  vpColVector sv_;
4276 
4277  if (nrows < ncols) {
4278  U.resize(ncols, ncols, true);
4279  sv.resize(nrows, false);
4280  } else {
4281  U.resize(nrows, ncols, false);
4282  sv.resize(ncols, false);
4283  }
4284 
4285  U.insert(*this, 0, 0);
4286  U.svdOpenCV(sv_, V);
4287 
4288  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, NULL, &imA, &imAt, &kerA);
4289 
4290  // Remove singular values equal to the one that corresponds to the lines of 0
4291  // introduced when m < n
4292  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4293 
4294  return static_cast<unsigned int>(rank_out);
4295 }
4296 
4337 vpMatrix vpMatrix::pseudoInverseOpenCV(int rank_in) const
4338 {
4339  unsigned int nrows = getRows();
4340  unsigned int ncols = getCols();
4341  int rank_out;
4342  double svThreshold = 1e-26;
4343 
4344  vpMatrix U, V, Ap;
4345  vpColVector sv;
4346 
4347  Ap.resize(ncols, nrows, false, false);
4348 
4349  if (nrows < ncols) {
4350  U.resize(ncols, ncols, true);
4351  sv.resize(nrows, false);
4352  } else {
4353  U.resize(nrows, ncols, false);
4354  sv.resize(ncols, false);
4355  }
4356 
4357  U.insert(*this, 0, 0);
4358  U.svdOpenCV(sv, V);
4359 
4360  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4361 
4362  return Ap;
4363 }
4364 
4411 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, int rank_in) const
4412 {
4413  unsigned int nrows = getRows();
4414  unsigned int ncols = getCols();
4415  int rank_out;
4416  double svThreshold = 1e-26;
4417 
4418  vpMatrix U, V;
4419  vpColVector sv;
4420 
4421  Ap.resize(ncols, nrows, false, false);
4422 
4423  if (nrows < ncols) {
4424  U.resize(ncols, ncols, true);
4425  sv.resize(nrows, false);
4426  } else {
4427  U.resize(nrows, ncols, false);
4428  sv.resize(ncols, false);
4429  }
4430 
4431  U.insert(*this, 0, 0);
4432  U.svdOpenCV(sv, V);
4433 
4434  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4435 
4436  return rank_out;
4437 }
4438 
4492 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4493 {
4494  unsigned int nrows = getRows();
4495  unsigned int ncols = getCols();
4496  int rank_out;
4497  double svThreshold = 1e-26;
4498 
4499  vpMatrix U, V;
4500  vpColVector sv_;
4501 
4502  Ap.resize(ncols, nrows, false, false);
4503 
4504  if (nrows < ncols) {
4505  U.resize(ncols, ncols, true);
4506  sv.resize(nrows, false);
4507  } else {
4508  U.resize(nrows, ncols, false);
4509  sv.resize(ncols, false);
4510  }
4511 
4512  U.insert(*this, 0, 0);
4513  U.svdOpenCV(sv_, V);
4514 
4515  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, NULL, NULL, NULL);
4516 
4517  // Remove singular values equal to the one that corresponds to the lines of 0
4518  // introduced when m < n
4519  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4520 
4521  return rank_out;
4522 }
4523 
4635 int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA,
4636  vpMatrix &imAt, vpMatrix &kerA) const
4637 {
4638  unsigned int nrows = getRows();
4639  unsigned int ncols = getCols();
4640  int rank_out;
4641  double svThreshold = 1e-26;
4642  vpMatrix U, V;
4643  vpColVector sv_;
4644 
4645  if (nrows < ncols) {
4646  U.resize(ncols, ncols, true);
4647  sv.resize(nrows, false);
4648  } else {
4649  U.resize(nrows, ncols, false);
4650  sv.resize(ncols, false);
4651  }
4652 
4653  U.insert(*this, 0, 0);
4654  U.svdOpenCV(sv_, V);
4655 
4656  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerA);
4657 
4658  // Remove singular values equal to the one that corresponds to the lines of 0
4659  // introduced when m < n
4660  memcpy(sv.data, sv_.data, sv.size() * sizeof(double));
4661 
4662  return rank_out;
4663 }
4664 
4665 #endif
4666 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
4667 
4729 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
4730 {
4731 #if defined(VISP_HAVE_LAPACK)
4732  return pseudoInverseLapack(Ap, sv, svThreshold);
4733 #elif defined(VISP_HAVE_EIGEN3)
4734  return pseudoInverseEigen3(Ap, sv, svThreshold);
4735 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
4736  return pseudoInverseOpenCV(Ap, sv, svThreshold);
4737 #else
4738  (void)Ap;
4739  (void)sv;
4740  (void)svThreshold;
4741  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4742  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4743 #endif
4744 }
4745 
4812 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in) const
4813 {
4814 #if defined(VISP_HAVE_LAPACK)
4815  return pseudoInverseLapack(Ap, sv, rank_in);
4816 #elif defined(VISP_HAVE_EIGEN3)
4817  return pseudoInverseEigen3(Ap, sv, rank_in);
4818 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
4819  return pseudoInverseOpenCV(Ap, sv, rank_in);
4820 #else
4821  (void)Ap;
4822  (void)sv;
4823  (void)svThreshold;
4824  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
4825  "Install Lapack, Eigen3 or OpenCV 3rd party"));
4826 #endif
4827 }
4902 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt) const
4903 {
4904  vpMatrix kerAt;
4905  return pseudoInverse(Ap, sv, svThreshold, imA, imAt, kerAt);
4906 }
4907 
4986 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt) const
4987 {
4988  vpMatrix kerAt;
4989  return pseudoInverse(Ap, sv, rank_in, imA, imAt, kerAt);
4990 }
4991 
5126 unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt, vpMatrix &kerAt) const
5127 {
5128 #if defined(VISP_HAVE_LAPACK)
5129  return pseudoInverseLapack(Ap, sv, svThreshold, imA, imAt, kerAt);
5130 #elif defined(VISP_HAVE_EIGEN3)
5131  return pseudoInverseEigen3(Ap, sv, svThreshold, imA, imAt, kerAt);
5132 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
5133  return pseudoInverseOpenCV(Ap, sv, svThreshold, imA, imAt, kerAt);
5134 #else
5135  (void)Ap;
5136  (void)sv;
5137  (void)svThreshold;
5138  (void)imA;
5139  (void)imAt;
5140  (void)kerAt;
5141  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
5142  "Install Lapack, Eigen3 or OpenCV 3rd party"));
5143 #endif
5144 }
5145 
5285 int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt, vpMatrix &kerAt) const
5286 {
5287 #if defined(VISP_HAVE_LAPACK)
5288  return pseudoInverseLapack(Ap, sv, rank_in, imA, imAt, kerAt);
5289 #elif defined(VISP_HAVE_EIGEN3)
5290  return pseudoInverseEigen3(Ap, sv, rank_in, imA, imAt, kerAt);
5291 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
5292  return pseudoInverseOpenCV(Ap, sv, rank_in, imA, imAt, kerAt);
5293 #else
5294  (void)Ap;
5295  (void)sv;
5296  (void)svThreshold;
5297  (void)imA;
5298  (void)imAt;
5299  (void)kerAt;
5300  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
5301  "Install Lapack, Eigen3 or OpenCV 3rd party"));
5302 #endif
5303 }
5304 
5346 vpColVector vpMatrix::getCol(unsigned int j, unsigned int i_begin, unsigned int column_size) const
5347 {
5348  if (i_begin + column_size > getRows() || j >= getCols())
5349  throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(), getCols()));
5350  vpColVector c(column_size);
5351  for (unsigned int i = 0; i < column_size; i++)
5352  c[i] = (*this)[i_begin + i][j];
5353  return c;
5354 }
5355 
5395 vpColVector vpMatrix::getCol(unsigned int j) const
5396 {
5397  return getCol(j, 0, rowNum);
5398 }
5399 
5435 vpRowVector vpMatrix::getRow(unsigned int i) const
5436 {
5437  return getRow(i, 0, colNum);
5438 }
5439 
5479 vpRowVector vpMatrix::getRow(unsigned int i, unsigned int j_begin, unsigned int row_size) const
5480 {
5481  if (j_begin + row_size > colNum || i >= rowNum)
5482  throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
5483 
5484  vpRowVector r(row_size);
5485  if (r.data != NULL && data != NULL) {
5486  memcpy(r.data, (*this)[i] + j_begin, row_size*sizeof(double));
5487  }
5488 
5489  return r;
5490 }
5491 
5528 {
5529  unsigned int min_size = std::min(rowNum, colNum);
5530  vpColVector diag;
5531 
5532  if (min_size > 0) {
5533  diag.resize(min_size, false);
5534 
5535  for (unsigned int i = 0; i < min_size; i++) {
5536  diag[i] = (*this)[i][i];
5537  }
5538  }
5539 
5540  return diag;
5541 }
5542 
5554 {
5555  vpMatrix C;
5556 
5557  vpMatrix::stack(A, B, C);
5558 
5559  return C;
5560 }
5561 
5573 void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
5574 {
5575  unsigned int nra = A.getRows();
5576  unsigned int nrb = B.getRows();
5577 
5578  if (nra != 0) {
5579  if (A.getCols() != B.getCols()) {
5580  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5581  A.getCols(), B.getRows(), B.getCols()));
5582  }
5583  }
5584 
5585  if (A.data != NULL && A.data == C.data) {
5586  std::cerr << "A and C must be two different objects!" << std::endl;
5587  return;
5588  }
5589 
5590  if (B.data != NULL && B.data == C.data) {
5591  std::cerr << "B and C must be two different objects!" << std::endl;
5592  return;
5593  }
5594 
5595  C.resize(nra + nrb, B.getCols(), false, false);
5596 
5597  if (C.data != NULL && A.data != NULL && A.size() > 0) {
5598  // Copy A in C
5599  memcpy(C.data, A.data, sizeof(double) * A.size());
5600  }
5601 
5602  if (C.data != NULL && B.data != NULL && B.size() > 0) {
5603  // Copy B in C
5604  memcpy(C.data + A.size(), B.data, sizeof(double) * B.size());
5605  }
5606 }
5607 
5618 {
5619  vpMatrix C;
5620  vpMatrix::stack(A, r, C);
5621 
5622  return C;
5623 }
5624 
5636 void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
5637 {
5638  if (A.data != NULL && A.data == C.data) {
5639  std::cerr << "A and C must be two different objects!" << std::endl;
5640  return;
5641  }
5642 
5643  C = A;
5644  C.stack(r);
5645 }
5646 
5657 {
5658  vpMatrix C;
5659  vpMatrix::stack(A, c, C);
5660 
5661  return C;
5662 }
5663 
5675 void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C)
5676 {
5677  if (A.data != NULL && A.data == C.data) {
5678  std::cerr << "A and C must be two different objects!" << std::endl;
5679  return;
5680  }
5681 
5682  C = A;
5683  C.stack(c);
5684 }
5685 
5698 vpMatrix vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, unsigned int r, unsigned int c)
5699 {
5700  vpMatrix C;
5701 
5702  insert(A, B, C, r, c);
5703 
5704  return C;
5705 }
5706 
5720 void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, unsigned int r, unsigned int c)
5721 {
5722  if (((r + B.getRows()) <= A.getRows()) && ((c + B.getCols()) <= A.getCols())) {
5723  C.resize(A.getRows(), A.getCols(), false, false);
5724 
5725  for (unsigned int i = 0; i < A.getRows(); i++) {
5726  for (unsigned int j = 0; j < A.getCols(); j++) {
5727  if (i >= r && i < (r + B.getRows()) && j >= c && j < (c + B.getCols())) {
5728  C[i][j] = B[i - r][j - c];
5729  } else {
5730  C[i][j] = A[i][j];
5731  }
5732  }
5733  }
5734  } else {
5735  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
5736  B.getRows(), B.getCols(), A.getCols(), A.getRows(), r, c);
5737  }
5738 }
5739 
5752 {
5753  vpMatrix C;
5754 
5755  juxtaposeMatrices(A, B, C);
5756 
5757  return C;
5758 }
5759 
5773 {
5774  unsigned int nca = A.getCols();
5775  unsigned int ncb = B.getCols();
5776 
5777  if (nca != 0) {
5778  if (A.getRows() != B.getRows()) {
5779  throw(vpException(vpException::dimensionError, "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
5780  A.getCols(), B.getRows(), B.getCols()));
5781  }
5782  }
5783 
5784  if (B.getRows() == 0 || nca + ncb == 0) {
5785  std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl;
5786  return;
5787  }
5788 
5789  C.resize(B.getRows(), nca + ncb, false, false);
5790 
5791  C.insert(A, 0, 0);
5792  C.insert(B, 0, nca);
5793 }
5794 
5795 //--------------------------------------------------------------------
5796 // Output
5797 //--------------------------------------------------------------------
5798 
5818 int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &intro) const
5819 {
5820  typedef std::string::size_type size_type;
5821 
5822  unsigned int m = getRows();
5823  unsigned int n = getCols();
5824 
5825  std::vector<std::string> values(m * n);
5826  std::ostringstream oss;
5827  std::ostringstream ossFixed;
5828  std::ios_base::fmtflags original_flags = oss.flags();
5829 
5830  // ossFixed <<std::fixed;
5831  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
5832 
5833  size_type maxBefore = 0; // the length of the integral part
5834  size_type maxAfter = 0; // number of decimals plus
5835  // one place for the decimal point
5836  for (unsigned int i = 0; i < m; ++i) {
5837  for (unsigned int j = 0; j < n; ++j) {
5838  oss.str("");
5839  oss << (*this)[i][j];
5840  if (oss.str().find("e") != std::string::npos) {
5841  ossFixed.str("");
5842  ossFixed << (*this)[i][j];
5843  oss.str(ossFixed.str());
5844  }
5845 
5846  values[i * n + j] = oss.str();
5847  size_type thislen = values[i * n + j].size();
5848  size_type p = values[i * n + j].find('.');
5849 
5850  if (p == std::string::npos) {
5851  maxBefore = vpMath::maximum(maxBefore, thislen);
5852  // maxAfter remains the same
5853  } else {
5854  maxBefore = vpMath::maximum(maxBefore, p);
5855  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
5856  }
5857  }
5858  }
5859 
5860  size_type totalLength = length;
5861  // increase totalLength according to maxBefore
5862  totalLength = vpMath::maximum(totalLength, maxBefore);
5863  // decrease maxAfter according to totalLength
5864  maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
5865  if (maxAfter == 1)
5866  maxAfter = 0;
5867 
5868  // the following line is useful for debugging
5869  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
5870 
5871  if (! intro.empty())
5872  s << intro;
5873  s << "[" << m << "," << n << "]=\n";
5874 
5875  for (unsigned int i = 0; i < m; i++) {
5876  s << " ";
5877  for (unsigned int j = 0; j < n; j++) {
5878  size_type p = values[i * n + j].find('.');
5879  s.setf(std::ios::right, std::ios::adjustfield);
5880  s.width((std::streamsize)maxBefore);
5881  s << values[i * n + j].substr(0, p).c_str();
5882 
5883  if (maxAfter > 0) {
5884  s.setf(std::ios::left, std::ios::adjustfield);
5885  if (p != std::string::npos) {
5886  s.width((std::streamsize)maxAfter);
5887  s << values[i * n + j].substr(p, maxAfter).c_str();
5888  } else {
5889  assert(maxAfter > 1);
5890  s.width((std::streamsize)maxAfter);
5891  s << ".0";
5892  }
5893  }
5894 
5895  s << ' ';
5896  }
5897  s << std::endl;
5898  }
5899 
5900  s.flags(original_flags); // restore s to standard state
5901 
5902  return (int)(maxBefore + maxAfter);
5903 }
5904 
5941 std::ostream &vpMatrix::matlabPrint(std::ostream &os) const
5942 {
5943  os << "[ ";
5944  for (unsigned int i = 0; i < this->getRows(); ++i) {
5945  for (unsigned int j = 0; j < this->getCols(); ++j) {
5946  os << (*this)[i][j] << ", ";
5947  }
5948  if (this->getRows() != i + 1) {
5949  os << ";" << std::endl;
5950  } else {
5951  os << "]" << std::endl;
5952  }
5953  }
5954  return os;
5955 }
5956 
5985 std::ostream &vpMatrix::maplePrint(std::ostream &os) const
5986 {
5987  os << "([ " << std::endl;
5988  for (unsigned int i = 0; i < this->getRows(); ++i) {
5989  os << "[";
5990  for (unsigned int j = 0; j < this->getCols(); ++j) {
5991  os << (*this)[i][j] << ", ";
5992  }
5993  os << "]," << std::endl;
5994  }
5995  os << "])" << std::endl;
5996  return os;
5997 }
5998 
6026 std::ostream &vpMatrix::csvPrint(std::ostream &os) const
6027 {
6028  for (unsigned int i = 0; i < this->getRows(); ++i) {
6029  for (unsigned int j = 0; j < this->getCols(); ++j) {
6030  os << (*this)[i][j];
6031  if (!(j == (this->getCols() - 1)))
6032  os << ", ";
6033  }
6034  os << std::endl;
6035  }
6036  return os;
6037 }
6038 
6075 std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
6076 {
6077  os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " << this->getCols() << "); " << std::endl;
6078 
6079  for (unsigned int i = 0; i < this->getRows(); ++i) {
6080  for (unsigned int j = 0; j < this->getCols(); ++j) {
6081  if (!octet) {
6082  os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl;
6083  } else {
6084  for (unsigned int k = 0; k < sizeof(double); ++k) {
6085  os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex
6086  << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl;
6087  }
6088  }
6089  }
6090  os << std::endl;
6091  }
6092  return os;
6093 }
6094 
6100 {
6101  if (rowNum == 0) {
6102  *this = A;
6103  } else if (A.getRows() > 0) {
6104  if (colNum != A.getCols()) {
6105  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum,
6106  A.getRows(), A.getCols()));
6107  }
6108 
6109  unsigned int rowNumOld = rowNum;
6110  resize(rowNum + A.getRows(), colNum, false, false);
6111  insert(A, rowNumOld, 0);
6112  }
6113 }
6114 
6131 {
6132  if (rowNum == 0) {
6133  *this = r;
6134  } else {
6135  if (colNum != r.getCols()) {
6136  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum,
6137  colNum, r.getCols()));
6138  }
6139 
6140  if (r.size() == 0) {
6141  return;
6142  }
6143 
6144  unsigned int oldSize = size();
6145  resize(rowNum + 1, colNum, false, false);
6146 
6147  if (data != NULL && r.data != NULL && data != r.data) {
6148  // Copy r in data
6149  memcpy(data + oldSize, r.data, sizeof(double) * r.size());
6150  }
6151  }
6152 }
6153 
6171 {
6172  if (colNum == 0) {
6173  *this = c;
6174  } else {
6175  if (rowNum != c.getRows()) {
6176  throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum,
6177  colNum, c.getRows()));
6178  }
6179 
6180  if (c.size() == 0) {
6181  return;
6182  }
6183 
6184  vpMatrix tmp = *this;
6185  unsigned int oldColNum = colNum;
6186  resize(rowNum, colNum + 1, false, false);
6187 
6188  if (data != NULL && tmp.data != NULL && data != tmp.data) {
6189  // Copy c in data
6190  for (unsigned int i = 0; i < rowNum; i++) {
6191  memcpy(data + i*colNum, tmp.data + i*oldColNum, sizeof(double) * oldColNum);
6192  rowPtrs[i][oldColNum] = c[i];
6193  }
6194  }
6195  }
6196 }
6197 
6208 void vpMatrix::insert(const vpMatrix &A, unsigned int r, unsigned int c)
6209 {
6210  if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) {
6211  if (A.colNum == colNum && data != NULL && A.data != NULL && A.data != data) {
6212  memcpy(data + r * colNum, A.data, sizeof(double) * A.size());
6213  } else if (data != NULL && A.data != NULL && A.data != data) {
6214  for (unsigned int i = r; i < (r + A.getRows()); i++) {
6215  memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum);
6216  }
6217  }
6218  } else {
6219  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
6220  A.getRows(), A.getCols(), rowNum, colNum, r, c);
6221  }
6222 }
6223 
6261 {
6262  vpColVector evalue(rowNum); // Eigen values
6263 
6264  if (rowNum != colNum) {
6266  "Cannot compute eigen values on a non square matrix (%dx%d)",
6267  rowNum, colNum));
6268  }
6269 
6270  // Check if the matrix is symetric: At - A = 0
6271  vpMatrix At_A = (*this).t() - (*this);
6272  for (unsigned int i = 0; i < rowNum; i++) {
6273  for (unsigned int j = 0; j < rowNum; j++) {
6274  // if (At_A[i][j] != 0) {
6275  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
6276  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symetric matrix"));
6277  }
6278  }
6279  }
6280 
6281 #if defined(VISP_HAVE_LAPACK)
6282 #if defined(VISP_HAVE_GSL) /* be careful of the copy below */
6283  {
6284  gsl_vector *eval = gsl_vector_alloc(rowNum);
6285  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
6286 
6287  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
6288  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
6289 
6290  unsigned int Atda = (unsigned int)m->tda;
6291  for (unsigned int i = 0; i < rowNum; i++) {
6292  unsigned int k = i * Atda;
6293  for (unsigned int j = 0; j < colNum; j++)
6294  m->data[k + j] = (*this)[i][j];
6295  }
6296  gsl_eigen_symmv(m, eval, evec, w);
6297 
6298  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
6299 
6300  for (unsigned int i = 0; i < rowNum; i++) {
6301  evalue[i] = gsl_vector_get(eval, i);
6302  }
6303 
6304  gsl_eigen_symmv_free(w);
6305  gsl_vector_free(eval);
6306  gsl_matrix_free(m);
6307  gsl_matrix_free(evec);
6308  }
6309 #else
6310  {
6311  const char jobz = 'N';
6312  const char uplo = 'U';
6313  vpMatrix A = (*this);
6314  vpColVector WORK;
6315  int lwork = -1;
6316  int info;
6317  double wkopt;
6318  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
6319  lwork = static_cast<int>(wkopt);
6320  WORK.resize(lwork);
6321  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
6322  }
6323 #endif
6324 #else
6325  {
6327  "Eigen values computation is not implemented. "
6328  "You should install Lapack 3rd party"));
6329  }
6330 #endif
6331  return evalue;
6332 }
6333 
6384 void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const
6385 {
6386  if (rowNum != colNum) {
6388  "Cannot compute eigen values on a non square matrix (%dx%d)",
6389  rowNum, colNum));
6390  }
6391 
6392  // Check if the matrix is symetric: At - A = 0
6393  vpMatrix At_A = (*this).t() - (*this);
6394  for (unsigned int i = 0; i < rowNum; i++) {
6395  for (unsigned int j = 0; j < rowNum; j++) {
6396  // if (At_A[i][j] != 0) {
6397  if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
6398  throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symetric matrix"));
6399  }
6400  }
6401  }
6402 
6403  // Resize the output matrices
6404  evalue.resize(rowNum);
6405  evector.resize(rowNum, colNum);
6406 
6407 #if defined(VISP_HAVE_LAPACK)
6408 #if defined(VISP_HAVE_GSL) /* be careful of the copy below */
6409  {
6410  gsl_vector *eval = gsl_vector_alloc(rowNum);
6411  gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
6412 
6413  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
6414  gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
6415 
6416  unsigned int Atda = (unsigned int)m->tda;
6417  for (unsigned int i = 0; i < rowNum; i++) {
6418  unsigned int k = i * Atda;
6419  for (unsigned int j = 0; j < colNum; j++)
6420  m->data[k + j] = (*this)[i][j];
6421  }
6422  gsl_eigen_symmv(m, eval, evec, w);
6423 
6424  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
6425 
6426  for (unsigned int i = 0; i < rowNum; i++) {
6427  evalue[i] = gsl_vector_get(eval, i);
6428  }
6429  Atda = (unsigned int)evec->tda;
6430  for (unsigned int i = 0; i < rowNum; i++) {
6431  unsigned int k = i * Atda;
6432  for (unsigned int j = 0; j < rowNum; j++) {
6433  evector[i][j] = evec->data[k + j];
6434  }
6435  }
6436 
6437  gsl_eigen_symmv_free(w);
6438  gsl_vector_free(eval);
6439  gsl_matrix_free(m);
6440  gsl_matrix_free(evec);
6441  }
6442 #else // defined(VISP_HAVE_GSL)
6443  {
6444  const char jobz = 'V';
6445  const char uplo = 'U';
6446  vpMatrix A = (*this);
6447  vpColVector WORK;
6448  int lwork = -1;
6449  int info;
6450  double wkopt;
6451  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
6452  lwork = static_cast<int>(wkopt);
6453  WORK.resize(lwork);
6454  vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
6455  evector = A.t();
6456  }
6457 #endif // defined(VISP_HAVE_GSL)
6458 #else
6459  {
6461  "Eigen values computation is not implemented. "
6462  "You should install Lapack 3rd party"));
6463  }
6464 #endif
6465 }
6466 
6486 unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const
6487 {
6488  unsigned int nbline = getRows();
6489  unsigned int nbcol = getCols();
6490 
6491  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6492  vpColVector sv;
6493  sv.resize(nbcol, false); // singular values
6494  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6495 
6496  // Copy and resize matrix to have at least as many rows as columns
6497  // kernel is computed in svd method only if the matrix has more rows than
6498  // columns
6499 
6500  if (nbline < nbcol)
6501  U.resize(nbcol, nbcol, true);
6502  else
6503  U.resize(nbline, nbcol, false);
6504 
6505  U.insert(*this, 0, 0);
6506 
6507  U.svd(sv, V);
6508 
6509  // Compute the highest singular value and rank of the matrix
6510  double maxsv = 0;
6511  for (unsigned int i = 0; i < nbcol; i++) {
6512  if (fabs(sv[i]) > maxsv) {
6513  maxsv = fabs(sv[i]);
6514  }
6515  }
6516 
6517  unsigned int rank = 0;
6518  for (unsigned int i = 0; i < nbcol; i++) {
6519  if (fabs(sv[i]) > maxsv * svThreshold) {
6520  rank++;
6521  }
6522  }
6523 
6524  kerAt.resize(nbcol - rank, nbcol);
6525  if (rank != nbcol) {
6526  for (unsigned int j = 0, k = 0; j < nbcol; j++) {
6527  // if( v.col(j) in kernel and non zero )
6528  if ((fabs(sv[j]) <= maxsv * svThreshold) &&
6529  (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
6530  for (unsigned int i = 0; i < V.getRows(); i++) {
6531  kerAt[k][i] = V[i][j];
6532  }
6533  k++;
6534  }
6535  }
6536  }
6537 
6538  return rank;
6539 }
6540 
6572 double vpMatrix::det(vpDetMethod method) const
6573 {
6574  double det = 0.;
6575 
6576  if (method == LU_DECOMPOSITION) {
6577  det = this->detByLU();
6578  }
6579 
6580  return (det);
6581 }
6582 
6591 {
6592  if (colNum != rowNum) {
6593  throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix",
6594  rowNum, colNum));
6595  } else {
6596 #ifdef VISP_HAVE_GSL
6597  size_t size_ = rowNum * colNum;
6598  double *b = new double[size_];
6599  for (size_t i = 0; i < size_; i++)
6600  b[i] = 0.;
6601  gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum);
6602  gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum);
6603  gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
6604  // gsl_matrix_fprintf(stdout, &em.matrix, "%g");
6605  vpMatrix expA;
6606  expA.resize(rowNum, colNum, false);
6607  memcpy(expA.data, b, size_ * sizeof(double));
6608 
6609  delete[] b;
6610  return expA;
6611 #else
6612  vpMatrix _expE(rowNum, colNum, false);
6613  vpMatrix _expD(rowNum, colNum, false);
6614  vpMatrix _expX(rowNum, colNum, false);
6615  vpMatrix _expcX(rowNum, colNum, false);
6616  vpMatrix _eye(rowNum, colNum, false);
6617 
6618  _eye.eye();
6619  vpMatrix exp(*this);
6620 
6621  // double f;
6622  int e;
6623  double c = 0.5;
6624  int q = 6;
6625  int p = 1;
6626 
6627  double nA = 0;
6628  for (unsigned int i = 0; i < rowNum; i++) {
6629  double sum = 0;
6630  for (unsigned int j = 0; j < colNum; j++) {
6631  sum += fabs((*this)[i][j]);
6632  }
6633  if (sum > nA || i == 0) {
6634  nA = sum;
6635  }
6636  }
6637 
6638  /* f = */ frexp(nA, &e);
6639  // double s = (0 > e+1)?0:e+1;
6640  double s = e + 1;
6641 
6642  double sca = 1.0 / pow(2.0, s);
6643  exp = sca * exp;
6644  _expX = *this;
6645  _expE = c * exp + _eye;
6646  _expD = -c * exp + _eye;
6647  for (int k = 2; k <= q; k++) {
6648  c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1)));
6649  _expcX = exp * _expX;
6650  _expX = _expcX;
6651  _expcX = c * _expX;
6652  _expE = _expE + _expcX;
6653  if (p)
6654  _expD = _expD + _expcX;
6655  else
6656  _expD = _expD - _expcX;
6657  p = !p;
6658  }
6659  _expX = _expD.inverseByLU();
6660  exp = _expX * _expE;
6661  for (int k = 1; k <= s; k++) {
6662  _expE = exp * exp;
6663  exp = _expE;
6664  }
6665  return exp;
6666 #endif
6667  }
6668 }
6669 
6670 /**************************************************************************************************************/
6671 /**************************************************************************************************************/
6672 
6673 // Specific functions
6674 
6675 /*
6676 input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.
6677 
6678 output:: the complement matrix of the element (rowNo,colNo).
6679 This is the matrix obtained from M after elimenating the row rowNo and column
6680 colNo
6681 
6682 example:
6683 1 2 3
6684 M = 4 5 6
6685 7 8 9
6686 1 3
6687 subblock(M, 1, 1) give the matrix 7 9
6688 */
6689 vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row)
6690 {
6691  vpMatrix M_comp;
6692  M_comp.resize(M.getRows() - 1, M.getCols() - 1, false);
6693 
6694  for (unsigned int i = 0; i < col; i++) {
6695  for (unsigned int j = 0; j < row; j++)
6696  M_comp[i][j] = M[i][j];
6697  for (unsigned int j = row + 1; j < M.getRows(); j++)
6698  M_comp[i][j - 1] = M[i][j];
6699  }
6700  for (unsigned int i = col + 1; i < M.getCols(); i++) {
6701  for (unsigned int j = 0; j < row; j++)
6702  M_comp[i - 1][j] = M[i][j];
6703  for (unsigned int j = row + 1; j < M.getRows(); j++)
6704  M_comp[i - 1][j - 1] = M[i][j];
6705  }
6706  return M_comp;
6707 }
6708 
6718 double vpMatrix::cond(double svThreshold) const
6719 {
6720  unsigned int nbline = getRows();
6721  unsigned int nbcol = getCols();
6722 
6723  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
6724  vpColVector sv;
6725  sv.resize(nbcol); // singular values
6726  V.resize(nbcol, nbcol, false); // V matrix of singular value decomposition
6727 
6728  // Copy and resize matrix to have at least as many rows as columns
6729  // kernel is computed in svd method only if the matrix has more rows than
6730  // columns
6731 
6732  if (nbline < nbcol)
6733  U.resize(nbcol, nbcol, true);
6734  else
6735  U.resize(nbline, nbcol, false);
6736 
6737  U.insert(*this, 0, 0);
6738 
6739  U.svd(sv, V);
6740 
6741  // Compute the highest singular value
6742  double maxsv = 0;
6743  for (unsigned int i = 0; i < nbcol; i++) {
6744  if (fabs(sv[i]) > maxsv) {
6745  maxsv = fabs(sv[i]);
6746  }
6747  }
6748 
6749  // Compute the rank of the matrix
6750  unsigned int rank = 0;
6751  for (unsigned int i = 0; i < nbcol; i++) {
6752  if (fabs(sv[i]) > maxsv * svThreshold) {
6753  rank++;
6754  }
6755  }
6756 
6757  // Compute the lowest singular value
6758  double minsv = maxsv;
6759  for (unsigned int i = 0; i < rank; i++) {
6760  if (fabs(sv[i]) < minsv) {
6761  minsv = fabs(sv[i]);
6762  }
6763  }
6764 
6765  if (std::fabs(minsv) > std::numeric_limits<double>::epsilon()) {
6766  return maxsv / minsv;
6767  }
6768  else {
6769  return std::numeric_limits<double>::infinity();
6770  }
6771 }
6772 
6779 void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
6780 {
6781  if (H.getCols() != H.getRows()) {
6782  throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(),
6783  H.getCols()));
6784  }
6785 
6786  HLM = H;
6787  for (unsigned int i = 0; i < H.getCols(); i++) {
6788  HLM[i][i] += alpha * H[i][i];
6789  }
6790 }
6791 
6801 vp_deprecated double vpMatrix::euclideanNorm() const
6802 {
6803  return frobeniusNorm();
6804 }
6805 
6814 {
6815  double norm = 0.0;
6816  for (unsigned int i = 0; i < dsize; i++) {
6817  double x = *(data + i);
6818  norm += x * x;
6819  }
6820 
6821  return sqrt(norm);
6822 }
6823 
6833 {
6834  if(this->dsize != 0){
6835  vpMatrix v;
6836  vpColVector w;
6837 
6838  vpMatrix M = *this;
6839 
6840  M.svd(w, v);
6841 
6842  double max = w[0];
6843  unsigned int maxRank = std::min(this->getCols(), this->getRows());
6844  // The maximum reachable rank is either the number of columns or the number of rows
6845  // of the matrix.
6846  unsigned int boundary = std::min(maxRank, w.size());
6847  // boundary is here to ensure that the number of singular values used for the com-
6848  // putation of the euclidean norm of the matrix is not greater than the maximum
6849  // reachable rank. Indeed, some svd library pad the singular values vector with 0s
6850  // if the input matrix is non-square.
6851  for (unsigned int i = 0; i < boundary; i++) {
6852  if (max < w[i]) {
6853  max = w[i];
6854  }
6855  }
6856  return max;
6857  }
6858  else {
6859  return 0.;
6860  }
6861 }
6862 
6874 {
6875  double norm = 0.0;
6876  for (unsigned int i = 0; i < rowNum; i++) {
6877  double x = 0;
6878  for (unsigned int j = 0; j < colNum; j++) {
6879  x += fabs(*(*(rowPtrs + i) + j));
6880  }
6881  if (x > norm) {
6882  norm = x;
6883  }
6884  }
6885  return norm;
6886 }
6887 
6894 double vpMatrix::sumSquare() const
6895 {
6896  double sum_square = 0.0;
6897  double x;
6898 
6899  for (unsigned int i = 0; i < rowNum; i++) {
6900  for (unsigned int j = 0; j < colNum; j++) {
6901  x = rowPtrs[i][j];
6902  sum_square += x * x;
6903  }
6904  }
6905 
6906  return sum_square;
6907 }
6908 
6920 vpMatrix vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode)
6921 {
6922  vpMatrix res;
6923  conv2(M, kernel, res, mode);
6924  return res;
6925 }
6926 
6939 void vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, vpMatrix &res, const std::string &mode)
6940 {
6941  if (M.getRows()*M.getCols() == 0 || kernel.getRows()*kernel.getCols() == 0)
6942  return;
6943 
6944  if (mode == "valid") {
6945  if (kernel.getRows() > M.getRows() || kernel.getCols() > M.getCols())
6946  return;
6947  }
6948 
6949  vpMatrix M_padded, res_same;
6950 
6951  if (mode == "full" || mode == "same") {
6952  const unsigned int pad_x = kernel.getCols()-1;
6953  const unsigned int pad_y = kernel.getRows()-1;
6954  M_padded.resize(M.getRows() + 2*pad_y, M.getCols() + 2*pad_x, true, false);
6955  M_padded.insert(M, pad_y, pad_x);
6956 
6957  if (mode == "same") {
6958  res.resize(M.getRows(), M.getCols(), false, false);
6959  res_same.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
6960  } else {
6961  res.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
6962  }
6963  } else if (mode == "valid") {
6964  M_padded = M;
6965  res.resize(M.getRows()-kernel.getRows()+1, M.getCols()-kernel.getCols()+1);
6966  } else {
6967  return;
6968  }
6969 
6970  if (mode == "same") {
6971  for (unsigned int i = 0; i < res_same.getRows(); i++) {
6972  for (unsigned int j = 0; j < res_same.getCols(); j++) {
6973  for (unsigned int k = 0; k < kernel.getRows(); k++) {
6974  for (unsigned int l = 0; l < kernel.getCols(); l++) {
6975  res_same[i][j] += M_padded[i+k][j+l] * kernel[kernel.getRows()-k-1][kernel.getCols()-l-1];
6976  }
6977  }
6978  }
6979  }
6980 
6981  const unsigned int start_i = kernel.getRows()/2;
6982  const unsigned int start_j = kernel.getCols()/2;
6983  for (unsigned int i = 0; i < M.getRows(); i++) {
6984  memcpy(res.data + i*M.getCols(), res_same.data + (i+start_i)*res_same.getCols() + start_j, sizeof(double)*M.getCols());
6985  }
6986  } else {
6987  for (unsigned int i = 0; i < res.getRows(); i++) {
6988  for (unsigned int j = 0; j < res.getCols(); j++) {
6989  for (unsigned int k = 0; k < kernel.getRows(); k++) {
6990  for (unsigned int l = 0; l < kernel.getCols(); l++) {
6991  res[i][j] += M_padded[i+k][j+l] * kernel[kernel.getRows()-k-1][kernel.getCols()-l-1];
6992  }
6993  }
6994  }
6995  }
6996  }
6997 }
6998 
6999 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
7001 {
7002  return (vpMatrix)(vpColVector::stack(A, B));
7003 }
7004 
7006 {
7007  vpColVector::stack(A, B, C);
7008 }
7009 
7011 
7012 void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C) { vpMatrix::stack(A, B, C); }
7013 
7033 {
7034  vpRowVector c(getCols());
7035 
7036  for (unsigned int j = 0; j < getCols(); j++)
7037  c[j] = (*this)[i - 1][j];
7038  return c;
7039 }
7040 
7059 {
7060  vpColVector c(getRows());
7061 
7062  for (unsigned int i = 0; i < getRows(); i++)
7063  c[i] = (*this)[i][j - 1];
7064  return c;
7065 }
7066 
7073 void vpMatrix::setIdentity(const double &val)
7074 {
7075  for (unsigned int i = 0; i < rowNum; i++)
7076  for (unsigned int j = 0; j < colNum; j++)
7077  if (i == j)
7078  (*this)[i][j] = val;
7079  else
7080  (*this)[i][j] = 0;
7081 }
7082 
7083 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:2116
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2238
vpMatrix operator*(const double &x, const vpMatrix &B)
Definition: vpMatrix.cpp:1784
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:469
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2449
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:5751
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
vpColVector getDiag() const
Definition: vpMatrix.cpp:5527
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6572
static vpMatrix conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode="full")
Definition: vpMatrix.cpp:6920
void kron(const vpMatrix &m1, vpMatrix &out) const
Definition: vpMatrix.cpp:2025
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpMatrix operator-() const
Definition: vpMatrix.cpp:1753
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:115
std::ostream & csvPrint(std::ostream &os) const
Definition: vpMatrix.cpp:6026
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1547
vpMatrix AtA() const
Definition: vpMatrix.cpp:736
double sum() const
Definition: vpMatrix.cpp:1760
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:6099
vp_deprecated vpColVector column(unsigned int j)
Definition: vpMatrix.cpp:7058
vpMatrix inverseByLU() const
vpMatrix & operator/=(double x)
Divide all the element of the matrix by x : Aij = Aij / x.
Definition: vpMatrix.cpp:1884
error that can be emited by ViSP classes.
Definition: vpException.h:71
VISP_EXPORT bool checkAVX()
unsigned int getRows() const
Definition: vpArray2D.h:289
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
vp_deprecated void stackMatrices(const vpMatrix &A)
Definition: vpMatrix.h:787
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:1660
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:1694
vpMatrix()
Definition: vpMatrix.h:172
vpMatrix operator/(double x) const
Cij = Aij / x (A is unchanged)
Definition: vpMatrix.cpp:1824
double infinityNorm() const
Definition: vpMatrix.cpp:6873
void svdLapack(vpColVector &w, vpMatrix &V)
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6718
vpMatrix & operator-=(const vpMatrix &B)
Operation A = A - B.
Definition: vpMatrix.cpp:1711
vpMatrix operator+(const vpMatrix &B) const
Definition: vpMatrix.cpp:1605
vpMatrix pseudoInverseEigen3(double svThreshold=1e-6) const
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
Implementation of a rotation matrix and operations on such kind of matrices.
vpMatrix & operator*=(double x)
Multiply all the element of the matrix by x : Aij = Aij * x.
Definition: vpMatrix.cpp:1870
unsigned int getCols() const
Definition: vpArray2D.h:279
std::ostream & maplePrint(std::ostream &os) const
Definition: vpMatrix.cpp:5985
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
double sumSquare() const
Definition: vpMatrix.cpp:6894
vpMatrix hadamard(const vpMatrix &m) const
Definition: vpMatrix.cpp:1962
vpMatrix t() const
Definition: vpMatrix.cpp:526
vp_deprecated void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:7073
std::ostream & matlabPrint(std::ostream &os) const
Definition: vpMatrix.cpp:5941
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6486
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:994
vp_deprecated void init()
Definition: vpMatrix.h:782
static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C)
Definition: vpMatrix.cpp:1518
VISP_EXPORT bool checkSSE2()
vpMatrix pseudoInverseOpenCV(double svThreshold=1e-6) const
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:5435
static void multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
Definition: vpMatrix.cpp:1066
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
Definition: vpMatrix.cpp:1116
std::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
Definition: vpMatrix.cpp:6075
vpColVector eigenValues() const
Definition: vpMatrix.cpp:6260
double euclideanNorm() const
Definition: vpMatrix.cpp:6801
void svdOpenCV(vpColVector &w, vpMatrix &V)
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5818
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5395
vpMatrix AAt() const
Definition: vpMatrix.cpp:614
vpMatrix transpose() const
Definition: vpMatrix.cpp:536
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
double inducedL2Norm() const
Definition: vpMatrix.cpp:6832
vpMatrix operator*(const vpMatrix &B) const
Definition: vpMatrix.cpp:1275
vp_deprecated vpRowVector row(unsigned int i)
Definition: vpMatrix.cpp:7032
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
vpRowVector stackRows()
Definition: vpMatrix.cpp:1949
double sumSquare() const
static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
Definition: vpMatrix.cpp:1013
vpColVector stackColumns()
Definition: vpMatrix.cpp:1927
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:141
double detByLU() const
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:6208
static void negateMatrix(const vpMatrix &A, vpMatrix &C)
Definition: vpMatrix.cpp:1735
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6779
vpMatrix & operator,(double val)
Definition: vpMatrix.cpp:912
double frobeniusNorm() const
Definition: vpMatrix.cpp:6813
Function not implemented.
Definition: vpException.h:90
void resize(unsigned int i, bool flagNullify=true)
Definition: vpRowVector.h:271
Class that consider the case of a translation vector.
void eye()
Definition: vpMatrix.cpp:511
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:6590
vpMatrix & operator=(const vpArray2D< double > &A)
Definition: vpMatrix.cpp:761
vpMatrix & operator<<(double *)
Definition: vpMatrix.cpp:895