Visual Servoing Platform  version 3.6.1 under development (2024-04-25)
vpGEMM.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 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  * Matrix generalized multiplication.
32  */
33 
34 #ifndef _vpGEMM_h_
35 #define _vpGEMM_h_
36 
37 #include <visp3/core/vpArray2D.h>
38 #include <visp3/core/vpException.h>
39 
40 const vpArray2D<double> null(0, 0);
41 
52 typedef enum
53 {
54  VP_GEMM_A_T = 1,
55  VP_GEMM_B_T = 2,
56  VP_GEMM_C_T = 4,
57 } vpGEMMmethod;
58 
59 template <unsigned int>
60 inline void GEMMsize(const vpArray2D<double> & /*A*/, const vpArray2D<double> & /*B*/, unsigned int & /*Arows*/,
61  unsigned int & /*Acols*/, unsigned int & /*Brows*/, unsigned int & /*Bcols*/)
62 { }
63 
64 template <>
65 void inline GEMMsize<0>(const vpArray2D<double> &A, const vpArray2D<double> &B, unsigned int &Arows,
66  unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
67 {
68  Arows = A.getRows();
69  Acols = A.getCols();
70  Brows = B.getRows();
71  Bcols = B.getCols();
72 }
73 
74 template <>
75 inline void GEMMsize<1>(const vpArray2D<double> &A, const vpArray2D<double> &B, unsigned int &Arows,
76  unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
77 {
78  Arows = A.getCols();
79  Acols = A.getRows();
80  Brows = B.getRows();
81  Bcols = B.getCols();
82 }
83 template <>
84 inline void GEMMsize<2>(const vpArray2D<double> &A, const vpArray2D<double> &B, unsigned int &Arows,
85  unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
86 {
87  Arows = A.getRows();
88  Acols = A.getCols();
89  Brows = B.getCols();
90  Bcols = B.getRows();
91 }
92 template <>
93 inline void GEMMsize<3>(const vpArray2D<double> &A, const vpArray2D<double> &B, unsigned int &Arows,
94  unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
95 {
96  Arows = A.getCols();
97  Acols = A.getRows();
98  Brows = B.getCols();
99  Bcols = B.getRows();
100 }
101 
102 template <>
103 inline void GEMMsize<4>(const vpArray2D<double> &A, const vpArray2D<double> &B, unsigned int &Arows,
104  unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
105 {
106  Arows = A.getRows();
107  Acols = A.getCols();
108  Brows = B.getRows();
109  Bcols = B.getCols();
110 }
111 
112 template <>
113 inline void GEMMsize<5>(const vpArray2D<double> &A, const vpArray2D<double> &B, unsigned int &Arows,
114  unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
115 {
116  Arows = A.getCols();
117  Acols = A.getRows();
118  Brows = B.getRows();
119  Bcols = B.getCols();
120 }
121 
122 template <>
123 inline void GEMMsize<6>(const vpArray2D<double> &A, const vpArray2D<double> &B, unsigned int &Arows,
124  unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
125 {
126  Arows = A.getRows();
127  Acols = A.getCols();
128  Brows = B.getCols();
129  Bcols = B.getRows();
130 }
131 
132 template <>
133 inline void GEMMsize<7>(const vpArray2D<double> &A, const vpArray2D<double> &B, unsigned int &Arows,
134  unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
135 {
136  Arows = A.getCols();
137  Acols = A.getRows();
138  Brows = B.getCols();
139  Bcols = B.getRows();
140 }
141 
142 template <unsigned int>
143 inline void GEMM1(const unsigned int & /*Arows*/, const unsigned int & /*Brows*/, const unsigned int & /*Bcols*/,
144  const vpArray2D<double> & /*A*/, const vpArray2D<double> & /*B*/, const double & /*alpha*/,
145  vpArray2D<double> & /*D*/)
146 { }
147 
148 template <>
149 inline void GEMM1<0>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
150  const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha, vpArray2D<double> &D)
151 {
152  for (unsigned int r = 0; r < Arows; ++r)
153  for (unsigned int c = 0; c < Bcols; ++c) {
154  double sum = 0;
155  for (unsigned int n = 0; n < Brows; ++n)
156  sum += A[r][n] * B[n][c] * alpha;
157  D[r][c] = sum;
158  }
159 }
160 
161 template <>
162 inline void GEMM1<1>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
163  const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha, vpArray2D<double> &D)
164 {
165  for (unsigned int r = 0; r < Arows; ++r)
166  for (unsigned int c = 0; c < Bcols; ++c) {
167  double sum = 0;
168  for (unsigned int n = 0; n < Brows; ++n)
169  sum += A[n][r] * B[n][c] * alpha;
170  D[r][c] = sum;
171  }
172 }
173 
174 template <>
175 inline void GEMM1<2>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
176  const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha, vpArray2D<double> &D)
177 {
178  for (unsigned int r = 0; r < Arows; ++r)
179  for (unsigned int c = 0; c < Bcols; ++c) {
180  double sum = 0;
181  for (unsigned int n = 0; n < Brows; ++n)
182  sum += A[r][n] * B[c][n] * alpha;
183  D[r][c] = sum;
184  }
185 }
186 
187 template <>
188 inline void GEMM1<3>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
189  const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha, vpArray2D<double> &D)
190 {
191  for (unsigned int r = 0; r < Arows; ++r)
192  for (unsigned int c = 0; c < Bcols; ++c) {
193  double sum = 0;
194  for (unsigned int n = 0; n < Brows; ++n)
195  sum += A[n][r] * B[c][n] * alpha;
196  D[r][c] = sum;
197  }
198 }
199 
200 template <unsigned int>
201 inline void GEMM2(const unsigned int & /*Arows*/, const unsigned int & /*Brows*/, const unsigned int & /*Bcols*/,
202  const vpArray2D<double> & /*A*/, const vpArray2D<double> & /*B*/, const double & /*alpha*/,
203  const vpArray2D<double> & /*C*/, const double & /*beta*/, vpArray2D<double> & /*D*/)
204 { }
205 
206 template <>
207 inline void GEMM2<0>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
208  const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
209  const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
210 {
211  for (unsigned int r = 0; r < Arows; ++r)
212  for (unsigned int c = 0; c < Bcols; ++c) {
213  double sum = 0;
214  for (unsigned int n = 0; n < Brows; ++n)
215  sum += A[r][n] * B[n][c] * alpha;
216  D[r][c] = sum + C[r][c] * beta;
217  }
218 }
219 
220 template <>
221 inline void GEMM2<1>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
222  const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
223  const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
224 {
225  for (unsigned int r = 0; r < Arows; ++r)
226  for (unsigned int c = 0; c < Bcols; ++c) {
227  double sum = 0;
228  for (unsigned int n = 0; n < Brows; ++n)
229  sum += A[n][r] * B[n][c] * alpha;
230  D[r][c] = sum + C[r][c] * beta;
231  }
232 }
233 
234 template <>
235 inline void GEMM2<2>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
236  const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
237  const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
238 {
239  for (unsigned int r = 0; r < Arows; ++r)
240  for (unsigned int c = 0; c < Bcols; ++c) {
241  double sum = 0;
242  for (unsigned int n = 0; n < Brows; ++n)
243  sum += A[r][n] * B[c][n] * alpha;
244  D[r][c] = sum + C[r][c] * beta;
245  }
246 }
247 
248 template <>
249 inline void GEMM2<3>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
250  const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
251  const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
252 {
253  for (unsigned int r = 0; r < Arows; ++r)
254  for (unsigned int c = 0; c < Bcols; ++c) {
255  double sum = 0;
256  for (unsigned int n = 0; n < Brows; ++n)
257  sum += A[n][r] * B[c][n] * alpha;
258  D[r][c] = sum + C[r][c] * beta;
259  }
260 }
261 
262 template <>
263 inline void GEMM2<4>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
264  const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
265  const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
266 {
267  for (unsigned int r = 0; r < Arows; ++r)
268  for (unsigned int c = 0; c < Bcols; ++c) {
269  double sum = 0;
270  for (unsigned int n = 0; n < Brows; ++n)
271  sum += A[r][n] * B[n][c] * alpha;
272  D[r][c] = sum + C[c][r] * beta;
273  }
274 }
275 
276 template <>
277 inline void GEMM2<5>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
278  const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
279  const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
280 {
281  for (unsigned int r = 0; r < Arows; ++r)
282  for (unsigned int c = 0; c < Bcols; ++c) {
283  double sum = 0;
284  for (unsigned int n = 0; n < Brows; ++n)
285  sum += A[n][r] * B[n][c] * alpha;
286  D[r][c] = sum + C[c][r] * beta;
287  }
288 }
289 
290 template <>
291 inline void GEMM2<6>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
292  const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
293  const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
294 {
295  for (unsigned int r = 0; r < Arows; ++r)
296  for (unsigned int c = 0; c < Bcols; ++c) {
297  double sum = 0;
298  for (unsigned int n = 0; n < Brows; ++n)
299  sum += A[r][n] * B[c][n] * alpha;
300  D[r][c] = sum + C[c][r] * beta;
301  }
302 }
303 
304 template <>
305 inline void GEMM2<7>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
306  const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
307  const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
308 {
309  for (unsigned int r = 0; r < Arows; ++r)
310  for (unsigned int c = 0; c < Bcols; ++c) {
311  double sum = 0;
312  for (unsigned int n = 0; n < Brows; ++n)
313  sum += A[n][r] * B[c][n] * alpha;
314  D[r][c] = sum + C[c][r] * beta;
315  }
316 }
317 
318 template <unsigned int T>
319 inline void vpTGEMM(const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
320  const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
321 {
322  unsigned int Arows;
323  unsigned int Acols;
324  unsigned int Brows;
325  unsigned int Bcols;
326 
327  GEMMsize<T>(A, B, Arows, Acols, Brows, Bcols);
328 
329  try {
330  if ((Arows != D.getRows()) || (Bcols != D.getCols()))
331  D.resize(Arows, Bcols);
332  }
333  catch (...) {
334  throw;
335  }
336 
337  if (Acols != Brows) {
338  throw(vpException(vpException::dimensionError, "In vpGEMM, cannot multiply (%dx%d) matrix by (%dx%d) matrix", Arows,
339  Acols, Brows, Bcols));
340  }
341 
342  if (C.getRows() != 0 && C.getCols() != 0) {
343  if ((Arows != C.getRows()) || (Bcols != C.getCols())) {
344  throw(vpException(vpException::dimensionError, "In vpGEMM, cannot add resulting (%dx%d) matrix to (%dx%d) matrix",
345  Arows, Bcols, C.getRows(), C.getCols()));
346  }
347 
348  GEMM2<T>(Arows, Brows, Bcols, A, B, alpha, C, beta, D);
349  }
350  else {
351  GEMM1<T>(Arows, Brows, Bcols, A, B, alpha, D);
352  }
353 }
354 
388 inline void vpGEMM(const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
389  const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D, const unsigned int &ops = 0)
390 {
391  switch (ops) {
392  case 0:
393  vpTGEMM<0>(A, B, alpha, C, beta, D);
394  break;
395  case 1:
396  vpTGEMM<1>(A, B, alpha, C, beta, D);
397  break;
398  case 2:
399  vpTGEMM<2>(A, B, alpha, C, beta, D);
400  break;
401  case 3:
402  vpTGEMM<3>(A, B, alpha, C, beta, D);
403  break;
404  case 4:
405  vpTGEMM<4>(A, B, alpha, C, beta, D);
406  break;
407  case 5:
408  vpTGEMM<5>(A, B, alpha, C, beta, D);
409  break;
410  case 6:
411  vpTGEMM<6>(A, B, alpha, C, beta, D);
412  break;
413  case 7:
414  vpTGEMM<7>(A, B, alpha, C, beta, D);
415  break;
416  default:
417  throw(vpException(vpException::functionNotImplementedError, "Operation on vpGEMM not implemented"));
418  break;
419  }
420 }
421 
422 #endif
vpArray2D::getCols
unsigned int getCols() const
Definition: vpArray2D.h:327
vpArray2D::vpGEMM
void vpGEMM(const vpArray2D< double > &A, const vpArray2D< double > &B, const double &alpha, const vpArray2D< double > &C, const double &beta, vpArray2D< double > &D, const unsigned int &ops=0)
Definition: vpGEMM.h:388
vpArray2D::vpGEMMmethod
vpGEMMmethod
Definition: vpGEMM.h:53
vpArray2D::getRows
unsigned int getRows() const
Definition: vpArray2D.h:337
vpException
error that can be emitted by ViSP classes.
Definition: vpException.h:59
vpException::functionNotImplementedError
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:78
vpException::dimensionError
@ dimensionError
Bad dimension.
Definition: vpException.h:83