Visual Servoing Platform  version 3.6.1 under development (2024-11-07)
vpMatrix_operations.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Stack matrix.
32  */
33 
34 #include <visp3/core/vpConfig.h>
35 #include <visp3/core/vpMatrix.h>
36 
37 #if defined(VISP_HAVE_SIMDLIB)
38 #include <Simd/SimdLib.h>
39 #endif
40 
41 BEGIN_VISP_NAMESPACE
42 
46 vpMatrix vpMatrix::t() const { return transpose(); }
47 
54 {
55  vpMatrix At;
56  transpose(At);
57  return At;
58 }
59 
66 {
67  At.resize(colNum, rowNum, false, false);
68 
69  if ((rowNum <= 16) || (colNum <= 16)) {
70  for (unsigned int i = 0; i < rowNum; ++i) {
71  for (unsigned int j = 0; j < colNum; ++j) {
72  At[j][i] = (*this)[i][j];
73  }
74  }
75  }
76  else {
77 #if defined(VISP_HAVE_SIMDLIB)
78  SimdMatTranspose(data, rowNum, colNum, At.data);
79 #else
80  // https://stackoverflow.com/a/21548079
81  const int tileSize = 32;
82  for (unsigned int i = 0; i < rowNum; i += tileSize) {
83  for (unsigned int j = 0; j < colNum; ++j) {
84  for (unsigned int b = 0; ((b < static_cast<unsigned int>(tileSize)) && ((i + b) < rowNum)); ++b) {
85  At[j][i + b] = (*this)[i + b][j];
86  }
87  }
88  }
89 #endif
90  }
91 }
92 
102 {
103  if (A.colNum != v.getRows()) {
104  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
105  A.getRows(), A.getCols(), v.getRows()));
106  }
107 
108  if (A.rowNum != w.rowNum) {
109  w.resize(A.rowNum, false);
110  }
111 
112  // If available use Lapack only for large matrices
113  bool useLapack = ((A.rowNum > vpMatrix::m_lapack_min_size) || (A.colNum > vpMatrix::m_lapack_min_size));
114 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
115  useLapack = false;
116 #endif
117 
118  if (useLapack) {
119 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
120  double alpha = 1.0;
121  double beta = 0.0;
122  char trans = 't';
123  int incr = 1;
124 
125  vpMatrix::blas_dgemv(trans, A.colNum, A.rowNum, alpha, A.data, A.colNum, v.data, incr, beta, w.data, incr);
126 #endif
127  }
128  else {
129  w = 0.0;
130  for (unsigned int j = 0; j < A.colNum; ++j) {
131  double vj = v[j]; // optimization em 5/12/2006
132  for (unsigned int i = 0; i < A.rowNum; ++i) {
133  w[i] += A.rowPtrs[i][j] * vj;
134  }
135  }
136  }
137 }
138 
139 //---------------------------------
140 // Matrix operations.
141 //---------------------------------
142 
152 void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
153 {
154  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) {
155  C.resize(A.rowNum, B.colNum, false, false);
156  }
157 
158  if (A.colNum != B.rowNum) {
159  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (%dx%d) matrix", A.getRows(),
160  A.getCols(), B.getRows(), B.getCols()));
161  }
162 
163  // If available use Lapack only for large matrices
164  bool useLapack = ((A.rowNum > vpMatrix::m_lapack_min_size) || (A.colNum > vpMatrix::m_lapack_min_size) ||
165  (B.colNum > vpMatrix::m_lapack_min_size));
166 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
167  useLapack = false;
168 #endif
169 
170  if (useLapack) {
171 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
172  const double alpha = 1.0;
173  const double beta = 0.0;
174  const char trans = 'n';
175  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
176  C.data, B.colNum);
177 #endif
178  }
179  else {
180  // 5/12/06 some "very" simple optimization to avoid indexation
181  const unsigned int BcolNum = B.colNum;
182  const unsigned int BrowNum = B.rowNum;
183  double **BrowPtrs = B.rowPtrs;
184  for (unsigned int i = 0; i < A.rowNum; ++i) {
185  const double *rowptri = A.rowPtrs[i];
186  double *ci = C[i];
187  for (unsigned int j = 0; j < BcolNum; ++j) {
188  double s = 0;
189  for (unsigned int k = 0; k < BrowNum; ++k) {
190  s += rowptri[k] * BrowPtrs[k][j];
191  }
192  ci[j] = s;
193  }
194  }
195  }
196 }
197 
211 {
212  if ((A.colNum != 3) || (A.rowNum != 3) || (B.colNum != 3) || (B.rowNum != 3)) {
214  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
215  "rotation matrix",
216  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
217  }
218  // 5/12/06 some "very" simple optimization to avoid indexation
219  const unsigned int BcolNum = B.colNum;
220  const unsigned int BrowNum = B.rowNum;
221  double **BrowPtrs = B.rowPtrs;
222  for (unsigned int i = 0; i < A.rowNum; ++i) {
223  const double *rowptri = A.rowPtrs[i];
224  double *ci = C[i];
225  for (unsigned int j = 0; j < BcolNum; ++j) {
226  double s = 0;
227  for (unsigned int k = 0; k < BrowNum; ++k) {
228  s += rowptri[k] * BrowPtrs[k][j];
229  }
230  ci[j] = s;
231  }
232  }
233 }
234 
248 {
249  if ((A.colNum != 4) || (A.rowNum != 4) || (B.colNum != 4) || (B.rowNum != 4)) {
251  "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
252  "rotation matrix",
253  A.getRows(), A.getCols(), B.getRows(), B.getCols()));
254  }
255  // Considering perfMatrixMultiplication.cpp benchmark,
256  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
257  // Lapack usage needs to be validated again.
258  // If available use Lapack only for large matrices.
259  // Using SSE2 doesn't speed up.
260  bool useLapack = ((A.rowNum > vpMatrix::m_lapack_min_size) || (A.colNum > vpMatrix::m_lapack_min_size) ||
261  (B.colNum > vpMatrix::m_lapack_min_size));
262 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
263  useLapack = false;
264 #endif
265 
266  if (useLapack) {
267 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
268  const double alpha = 1.0;
269  const double beta = 0.0;
270  const char trans = 'n';
271  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
272  C.data, B.colNum);
273 #endif
274  }
275  else {
276  // 5/12/06 some "very" simple optimization to avoid indexation
277  const unsigned int BcolNum = B.colNum;
278  const unsigned int BrowNum = B.rowNum;
279  double **BrowPtrs = B.rowPtrs;
280  for (unsigned int i = 0; i < A.rowNum; ++i) {
281  const double *rowptri = A.rowPtrs[i];
282  double *ci = C[i];
283  for (unsigned int j = 0; j < BcolNum; ++j) {
284  double s = 0;
285  for (unsigned int k = 0; k < BrowNum; ++k) {
286  s += rowptri[k] * BrowPtrs[k][j];
287  }
288  ci[j] = s;
289  }
290  }
291  }
292 }
293 
307 {
309 }
310 
321 void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB,
322  vpMatrix &C)
323 {
324  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) {
325  C.resize(A.rowNum, B.colNum, false, false);
326  }
327 
328  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
329  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
330  A.getCols(), B.getRows(), B.getCols()));
331  }
332 
333  double **ArowPtrs = A.rowPtrs;
334  double **BrowPtrs = B.rowPtrs;
335  double **CrowPtrs = C.rowPtrs;
336 
337  for (unsigned int i = 0; i < A.rowNum; ++i) {
338  for (unsigned int j = 0; j < A.colNum; ++j) {
339  CrowPtrs[i][j] = (wB * BrowPtrs[i][j]) + (wA * ArowPtrs[i][j]);
340  }
341  }
342 }
343 
353 void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
354 {
355  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) {
356  C.resize(A.rowNum, B.colNum, false, false);
357  }
358 
359  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
360  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
361  A.getCols(), B.getRows(), B.getCols()));
362  }
363 
364  double **ArowPtrs = A.rowPtrs;
365  double **BrowPtrs = B.rowPtrs;
366  double **CrowPtrs = C.rowPtrs;
367 
368  for (unsigned int i = 0; i < A.rowNum; ++i) {
369  for (unsigned int j = 0; j < A.colNum; ++j) {
370  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
371  }
372  }
373 }
374 
388 {
389  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) {
390  C.resize(A.rowNum);
391  }
392 
393  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
394  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
395  A.getCols(), B.getRows(), B.getCols()));
396  }
397 
398  double **ArowPtrs = A.rowPtrs;
399  double **BrowPtrs = B.rowPtrs;
400  double **CrowPtrs = C.rowPtrs;
401 
402  for (unsigned int i = 0; i < A.rowNum; ++i) {
403  for (unsigned int j = 0; j < A.colNum; ++j) {
404  CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
405  }
406  }
407 }
408 
425 {
426  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) {
427  C.resize(A.rowNum);
428  }
429 
430  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
431  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
432  A.getCols(), B.getRows(), B.getCols()));
433  }
434 
435  double **ArowPtrs = A.rowPtrs;
436  double **BrowPtrs = B.rowPtrs;
437  double **CrowPtrs = C.rowPtrs;
438 
439  for (unsigned int i = 0; i < A.rowNum; ++i) {
440  for (unsigned int j = 0; j < A.colNum; ++j) {
441  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
442  }
443  }
444 }
445 
458 void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
459 {
460  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) {
461  C.resize(A.rowNum, A.colNum, false, false);
462  }
463 
464  if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
465  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
466  A.getCols(), B.getRows(), B.getCols()));
467  }
468 
469  double **ArowPtrs = A.rowPtrs;
470  double **BrowPtrs = B.rowPtrs;
471  double **CrowPtrs = C.rowPtrs;
472 
473  for (unsigned int i = 0; i < A.rowNum; ++i) {
474  for (unsigned int j = 0; j < A.colNum; ++j) {
475  CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
476  }
477  }
478 }
479 
490 {
491  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) {
492  C.resize(A.rowNum, A.colNum, false, false);
493  }
494 
495  double **ArowPtrs = A.rowPtrs;
496  double **CrowPtrs = C.rowPtrs;
497 
498  for (unsigned int i = 0; i < A.rowNum; ++i) {
499  for (unsigned int j = 0; j < A.colNum; ++j) {
500  CrowPtrs[i][j] = -ArowPtrs[i][j];
501  }
502  }
503 }
504 
511 {
512  vpMatrix B;
513 
514  AAt(B);
515 
516  return B;
517 }
518 
530 void vpMatrix::AAt(vpMatrix &B) const
531 {
532  if ((B.rowNum != rowNum) || (B.colNum != rowNum)) {
533  B.resize(rowNum, rowNum, false, false);
534  }
535 
536  // If available use Lapack only for large matrices
537  bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size));
538 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
539  useLapack = false;
540 #endif
541 
542  if (useLapack) {
543 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
544  const double alpha = 1.0;
545  const double beta = 0.0;
546  const char transa = 't';
547  const char transb = 'n';
548 
549  vpMatrix::blas_dgemm(transa, transb, rowNum, rowNum, colNum, alpha, data, colNum, data, colNum, beta, B.data,
550  rowNum);
551 #endif
552  }
553  else {
554  // compute A*A^T
555  for (unsigned int i = 0; i < rowNum; ++i) {
556  for (unsigned int j = i; j < rowNum; ++j) {
557  double *pi = rowPtrs[i]; // row i
558  double *pj = rowPtrs[j]; // row j
559 
560  // sum (row i .* row j)
561  double ssum = 0;
562  for (unsigned int k = 0; k < colNum; ++k) {
563  ssum += *(pi++) * *(pj++);
564  }
565 
566  B[i][j] = ssum; // upper triangle
567  if (i != j) {
568  B[j][i] = ssum; // lower triangle
569  }
570  }
571  }
572  }
573 }
574 
586 void vpMatrix::AtA(vpMatrix &B) const
587 {
588  if ((B.rowNum != colNum) || (B.colNum != colNum)) {
589  B.resize(colNum, colNum, false, false);
590  }
591 
592  // If available use Lapack only for large matrices
593  bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size));
594 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
595  useLapack = false;
596 #endif
597 
598  if (useLapack) {
599 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
600  const double alpha = 1.0;
601  const double beta = 0.0;
602  const char transa = 'n';
603  const char transb = 't';
604 
605  vpMatrix::blas_dgemm(transa, transb, colNum, colNum, rowNum, alpha, data, colNum, data, colNum, beta, B.data,
606  colNum);
607 #endif
608  }
609  else {
610  for (unsigned int i = 0; i < colNum; ++i) {
611  double *Bi = B[i];
612  for (unsigned int j = 0; j < i; ++j) {
613  double *ptr = data;
614  double s = 0;
615  for (unsigned int k = 0; k < rowNum; ++k) {
616  s += (*(ptr + i)) * (*(ptr + j));
617  ptr += colNum;
618  }
619  *Bi++ = s;
620  B[j][i] = s;
621  }
622  double *ptr = data;
623  double s = 0;
624  for (unsigned int k = 0; k < rowNum; ++k) {
625  s += (*(ptr + i)) * (*(ptr + i));
626  ptr += colNum;
627  }
628  *Bi = s;
629  }
630  }
631 }
632 
639 {
640  vpMatrix B;
641 
642  AtA(B);
643 
644  return B;
645 }
646 
687 {
688  unsigned int rows = A.getRows();
689  this->resize(rows, rows);
690 
691  (*this) = 0;
692  for (unsigned int i = 0; i < rows; ++i) {
693  (*this)[i][i] = A[i];
694  }
695 }
696 
731 void vpMatrix::diag(const double &val)
732 {
733  (*this) = 0;
734  unsigned int min_ = (rowNum < colNum) ? rowNum : colNum;
735  for (unsigned int i = 0; i < min_; ++i) {
736  (*this)[i][i] = val;
737  }
738 }
739 
751 {
752  unsigned int rows = A.getRows();
753  DA.resize(rows, rows, true);
754 
755  for (unsigned int i = 0; i < rows; ++i) {
756  DA[i][i] = A[i];
757  }
758 }
759 
764 void vpMatrix::eye(unsigned int n) { eye(n, n); }
765 
770 void vpMatrix::eye(unsigned int m, unsigned int n)
771 {
772  resize(m, n);
773 
774  eye();
775 }
776 
782 {
783  for (unsigned int i = 0; i < rowNum; ++i) {
784  for (unsigned int j = 0; j < colNum; ++j) {
785  if (i == j) {
786  (*this)[i][j] = 1.0;
787  }
788  else {
789  (*this)[i][j] = 0;
790  }
791  }
792  }
793 }
794 
802 {
803  if ((m.getRows() != rowNum) || (m.getCols() != colNum)) {
804  throw(vpException(vpException::dimensionError, "In Hadamard product: bad dimension of input matrix"));
805  }
806 
807  vpMatrix out;
808  out.resize(rowNum, colNum, false, false);
809 
810 #if defined(VISP_HAVE_SIMDLIB)
811  SimdVectorHadamard(data, m.data, dsize, out.data);
812 #else
813  for (unsigned int i = 0; i < dsize; ++i) {
814  out.data[i] = data[i] * m.data[i];
815  }
816 #endif
817 
818  return out;
819 }
820 
827 void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out)
828 {
829  unsigned int r1 = m1.getRows();
830  unsigned int c1 = m1.getCols();
831  unsigned int r2 = m2.getRows();
832  unsigned int c2 = m2.getCols();
833 
834  out.resize(r1 * r2, c1 * c2, false, false);
835 
836  for (unsigned int r = 0; r < r1; ++r) {
837  for (unsigned int c = 0; c < c1; ++c) {
838  double alpha = m1[r][c];
839  double *m2ptr = m2[0];
840  unsigned int roffset = r * r2;
841  unsigned int coffset = c * c2;
842  for (unsigned int rr = 0; rr < r2; ++rr) {
843  for (unsigned int cc = 0; cc < c2; ++cc) {
844  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
845  }
846  }
847  }
848  }
849 }
850 
857 void vpMatrix::kron(const vpMatrix &m, vpMatrix &out) const { kron(*this, m, out); }
858 
866 {
867  unsigned int r1 = m1.getRows();
868  unsigned int c1 = m1.getCols();
869  unsigned int r2 = m2.getRows();
870  unsigned int c2 = m2.getCols();
871 
872  vpMatrix out;
873  out.resize(r1 * r2, c1 * c2, false, false);
874 
875  for (unsigned int r = 0; r < r1; ++r) {
876  for (unsigned int c = 0; c < c1; ++c) {
877  double alpha = m1[r][c];
878  double *m2ptr = m2[0];
879  unsigned int roffset = r * r2;
880  unsigned int coffset = c * c2;
881  for (unsigned int rr = 0; rr < r2; ++rr) {
882  for (unsigned int cc = 0; cc < c2; ++cc) {
883  out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
884  }
885  }
886  }
887  }
888  return out;
889 }
890 
896 vpMatrix vpMatrix::kron(const vpMatrix &m) const { return kron(*this, m); }
897 
898 vpMatrix vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode)
899 {
900  vpMatrix res;
901  conv2(M, kernel, res, mode);
902  return res;
903 }
904 
905 void vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, vpMatrix &res, const std::string &mode)
906 {
907  if (((M.getRows() * M.getCols()) == 0) || ((kernel.getRows() * kernel.getCols()) == 0)) {
908  return;
909  }
910 
911  if (mode == "valid") {
912  if ((kernel.getRows() > M.getRows()) || (kernel.getCols() > M.getCols())) {
913  return;
914  }
915  }
916 
917  vpMatrix M_padded, res_same;
918 
919  if ((mode == "full") || (mode == "same")) {
920  const unsigned int pad_x = kernel.getCols() - 1;
921  const unsigned int pad_y = kernel.getRows() - 1;
922  const unsigned int pad = 2;
923  M_padded.resize(M.getRows() + (pad * pad_y), M.getCols() + (pad * pad_x), true, false);
924  M_padded.insert(M, pad_y, pad_x);
925 
926  if (mode == "same") {
927  res.resize(M.getRows(), M.getCols(), false, false);
928  res_same.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
929  }
930  else {
931  res.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
932  }
933  }
934  else if (mode == "valid") {
935  M_padded = M;
936  res.resize((M.getRows() - kernel.getRows()) + 1, (M.getCols() - kernel.getCols()) + 1);
937  }
938  else {
939  return;
940  }
941 
942  if (mode == "same") {
943  unsigned int res_same_rows = res_same.getRows();
944  unsigned int res_same_cols = res_same.getCols();
945  unsigned int kernel_rows = kernel.getRows();
946  unsigned int kernel_cols = kernel.getCols();
947  for (unsigned int i = 0; i < res_same_rows; ++i) {
948  for (unsigned int j = 0; j < res_same_cols; ++j) {
949  for (unsigned int k = 0; k < kernel_rows; ++k) {
950  for (unsigned int l = 0; l < kernel_cols; ++l) {
951  res_same[i][j] += M_padded[i + k][j + l] * kernel[kernel.getRows() - k - 1][kernel.getCols() - l - 1];
952  }
953  }
954  }
955  }
956 
957  const unsigned int start_i = kernel.getRows() / 2;
958  const unsigned int start_j = kernel.getCols() / 2;
959  unsigned int m_rows = M.getRows();
960  for (unsigned int i = 0; i < m_rows; ++i) {
961  memcpy(res.data + (i * M.getCols()), res_same.data + ((i + start_i) * res_same.getCols()) + start_j,
962  sizeof(double) * M.getCols());
963  }
964  }
965  else {
966  unsigned int res_rows = res.getRows();
967  unsigned int res_cols = res.getCols();
968  unsigned int kernel_rows = kernel.getRows();
969  unsigned int kernel_cols = kernel.getCols();
970  for (unsigned int i = 0; i < res_rows; ++i) {
971  for (unsigned int j = 0; j < res_cols; ++j) {
972  for (unsigned int k = 0; k < kernel_rows; ++k) {
973  for (unsigned int l = 0; l < kernel_cols; ++l) {
974  res[i][j] += M_padded[i + k][j + l] * kernel[kernel.getRows() - k - 1][kernel.getCols() - l - 1];
975  }
976  }
977  }
978  }
979  }
980 }
981 END_VISP_NAMESPACE
unsigned int getCols() const
Definition: vpArray2D.h:337
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:148
Type ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:1105
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:1101
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:1107
unsigned int getRows() const
Definition: vpArray2D.h:347
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:1103
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
vpMatrix hadamard(const vpMatrix &m) const
unsigned int kernel(vpMatrix &kerAt, double svThreshold=1e-6) const
Definition: vpMatrix.cpp:1416
static void sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
void diag(const double &val=1.0)
static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C)
vpMatrix AtA() const
static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
static void multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:1133
void kron(const vpMatrix &m1, vpMatrix &out) const
vpMatrix AAt() const
static vpMatrix conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode)
vpMatrix transpose() const
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
static void negateMatrix(const vpMatrix &A, vpMatrix &C)
vpMatrix t() const
Implementation of a rotation matrix and operations on such kind of matrices.