45 #include <visp3/core/vpConfig.h> 46 #include <visp3/core/vpDebug.h> 47 #include <visp3/core/vpGEMM.h> 48 #include <visp3/core/vpHomogeneousMatrix.h> 49 #include <visp3/core/vpMath.h> 50 #include <visp3/core/vpVelocityTwistMatrix.h> 57 bool test(
const std::string &s,
const vpMatrix &M,
const std::vector<double> &bench)
59 static unsigned int cpt = 0;
60 std::cout <<
"** Test " << ++cpt << std::endl;
61 std::cout << s <<
"(" << M.
getRows() <<
"," << M.
getCols() <<
") = \n" << M << std::endl;
62 if (bench.size() != M.
size()) {
63 std::cout <<
"Test fails: bad size wrt bench" << std::endl;
66 for (
unsigned int i = 0; i < M.
size(); i++) {
67 if (std::fabs(M.
data[i] - bench[i]) > std::fabs(M.
data[i]) * std::numeric_limits<double>::epsilon()) {
68 std::cout <<
"Test fails: bad content" << std::endl;
76 double getRandomValues(
const double min,
const double max)
78 return (max - min) * ((double)rand() / (double)RAND_MAX) + min;
81 bool equalMatrix(
const vpMatrix &A,
const vpMatrix &B,
const double tol = std::numeric_limits<double>::epsilon())
87 for (
unsigned int i = 0; i < A.
getRows(); i++) {
88 for (
unsigned int j = 0; j < A.
getCols(); j++) {
98 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) 99 vpMatrix generateRandomMatrix(
const unsigned int rows,
const unsigned int cols,
const double min,
const double max)
103 for (
unsigned int i = 0; i < M.
getRows(); i++) {
104 for (
unsigned int j = 0; j < M.
getCols(); j++) {
105 M[i][j] = getRandomValues(min, max);
112 vpColVector generateRandomVector(
const unsigned int rows,
const double min,
const double max)
116 for (
unsigned int i = 0; i < v.getRows(); i++) {
117 v[i] = getRandomValues(min, max);
137 unsigned int BcolNum = B.
getCols();
138 unsigned int BrowNum = B.
getRows();
139 unsigned int i, j, k;
140 for (i = 0; i < A.
getRows(); i++) {
141 double *rowptri = A[i];
143 for (j = 0; j < BcolNum; j++) {
145 for (k = 0; k < BrowNum; k++)
146 s += rowptri[k] * B[k][j];
160 unsigned int i, j, k;
163 for (i = 0; i < A.
getCols(); i++) {
165 for (j = 0; j < i; j++) {
168 for (k = 0; k < A.
getRows(); k++) {
169 s += (*(ptr + i)) * (*(ptr + j));
177 for (k = 0; k < A.
getRows(); k++) {
178 s += (*(ptr + i)) * (*(ptr + i));
199 for (
unsigned int j = 0; j < A.
getCols(); j++) {
201 for (
unsigned int i = 0; i < A.
getRows(); i++) {
202 w[i] += A[i][j] * vj;
221 unsigned int VcolNum = V.
getCols();
222 unsigned int VrowNum = V.
getRows();
224 for (
unsigned int i = 0; i < A.
getRows(); i++) {
225 double *rowptri = A[i];
227 for (
unsigned int j = 0; j < VcolNum; j++) {
229 for (
unsigned int k = 0; k < VrowNum; k++)
230 s += rowptri[k] * V[k][j];
247 std::vector<double> bench(6, 1);
249 if (test(
"M1", M1, bench) ==
false)
252 if (test(
"M2", M2, bench) ==
false)
258 for (
unsigned int i = 0; i < M.
getRows(); i++) {
259 for (
unsigned int j = 0; j < M.
getCols(); j++) {
264 M.
print(std::cout, 4);
267 N.
init(M, 0, 1, 2, 3);
269 N.
print(std::cout, 4);
270 std::string header(
"My 4-by-5 matrix\nwith a second line");
274 std::cout <<
"Matrix saved in matrix.mat file" << std::endl;
282 std::cout <<
"Matrix loaded from matrix.mat file with header \"" << header_ <<
"\": \n" << M1 << std::endl;
285 if (header != std::string(header_)) {
286 std::cout <<
"Bad header in matrix.mat" << std::endl;
292 std::cout <<
"Matrix saved in matrix.bin file" << std::endl;
298 std::cout <<
"Matrix loaded from matrix.bin file with header \"" << header_ <<
"\": \n" << M1 << std::endl;
301 if (header != std::string(header_)) {
302 std::cout <<
"Bad header in matrix.bin" << std::endl;
308 std::cout <<
"Matrix saved in matrix.yml file" << std::endl;
315 std::cout <<
"Matrix loaded from matrix.yml file with header \"" << header_ <<
"\": \n" << M2 << std::endl;
318 if (header != std::string(header_)) {
319 std::cout <<
"Bad header in matrix.mat" << std::endl;
326 std::cout <<
"R: \n" << R << std::endl;
328 std::cout <<
"M1: \n" << M1 << std::endl;
330 std::cout <<
"M2: \n" << M2 << std::endl;
332 std::cout <<
"M3: \n" << M3 << std::endl;
334 std::cout <<
"M4: \n" << M4 << std::endl;
338 std::cout <<
"------------------------" << std::endl;
339 std::cout <<
"--- TEST PRETTY PRINT---" << std::endl;
340 std::cout <<
"------------------------" << std::endl;
344 std::cout <<
"call std::cout << M;" << std::endl;
345 std::cout << M << std::endl;
347 std::cout <<
"call M.print (std::cout, 4);" << std::endl;
348 M.
print(std::cout, 4);
350 std::cout <<
"------------------------" << std::endl;
356 std::cout <<
"call std::cout << M;" << std::endl;
358 std::cout <<
"call M.print (std::cout, 6);" << std::endl;
359 M.
print(std::cout, 6);
360 std::cout << std::endl;
362 std::cout <<
"------------------------" << std::endl;
366 std::cout <<
"call std::cout << M;" << std::endl;
367 std::cout << M << std::endl;
369 std::cout <<
"call M.print (std::cout, 10);" << std::endl;
370 M.print(std::cout, 10);
371 std::cout << std::endl;
373 std::cout <<
"call M.print (std::cout, 2);" << std::endl;
374 M.print(std::cout, 2);
375 std::cout << std::endl;
377 std::cout <<
"------------------------" << std::endl;
380 M[0][2] = -0.0000000876;
381 std::cout <<
"call std::cout << M;" << std::endl;
382 std::cout << M << std::endl;
384 std::cout <<
"call M.print (std::cout, 4);" << std::endl;
385 M.print(std::cout, 4);
386 std::cout << std::endl;
387 std::cout <<
"call M.print (std::cout, 10, \"M\");" << std::endl;
388 M.print(std::cout, 10,
"M");
389 std::cout << std::endl;
390 std::cout <<
"call M.print (std::cout, 20, \"M\");" << std::endl;
391 M.print(std::cout, 20,
"M");
392 std::cout << std::endl;
394 std::cout <<
"------------------------" << std::endl;
395 std::cout <<
"--- TEST RESIZE --------" << std::endl;
396 std::cout <<
"------------------------" << std::endl;
397 std::cout <<
"5x5" << std::endl;
398 M.resize(5, 5,
false);
399 std::cout << M << std::endl;
400 std::cout <<
"3x2" << std::endl;
401 M.resize(3, 2,
false);
402 std::cout << M << std::endl;
403 std::cout <<
"2x2" << std::endl;
404 M.resize(2, 2,
false);
405 std::cout << M << std::endl;
406 std::cout <<
"------------------------" << std::endl;
415 std::cout <<
"------------------------" << std::endl;
416 std::cout <<
"--- TEST vpRowVector * vpColVector" << std::endl;
417 std::cout <<
"------------------------" << std::endl;
430 r.print(std::cout, 2,
"r");
431 c.print(std::cout, 2,
"c");
432 std::cout <<
"r * c = " << rc << std::endl;
434 std::cout <<
"------------------------" << std::endl;
435 std::cout <<
"--- TEST vpRowVector * vpMatrix" << std::endl;
436 std::cout <<
"------------------------" << std::endl;
445 r.
print(std::cout, 2,
"r");
446 M.print(std::cout, 10,
"M");
447 std::cout <<
"r * M = " << rM << std::endl;
449 std::cout <<
"------------------------" << std::endl;
450 std::cout <<
"--- TEST vpGEMM " << std::endl;
451 std::cout <<
"------------------------" << std::endl;
466 vpGEMM(M, N, 2, C, 3, D, VP_GEMM_A_T);
467 std::cout << D << std::endl;
471 std::cout <<
"------------------------" << std::endl;
472 std::cout <<
"--- TEST vpMatrix insert() with same colNum " << std::endl;
473 std::cout <<
"------------------------" << std::endl;
474 const unsigned int nb = 100;
476 const unsigned int size = 100;
479 std::vector<vpMatrix> submatrices(nb);
480 for (
size_t cpt = 0; cpt < submatrices.size(); cpt++) {
483 for (
unsigned int i = 0; i < m.getRows(); i++) {
484 for (
unsigned int j = 0; j < m.getCols(); j++) {
485 m[i][j] = getRandomValues(-100.0, 100.0);
489 submatrices[cpt] = m;
493 for (
unsigned int i = 0; i < nb; i++) {
494 m_big.insert(submatrices[(
size_t)i], i * size, 0);
497 std::cout <<
"Matrix insert(): " << t <<
" ms" << std::endl;
499 for (
unsigned int cpt = 0; cpt < nb; cpt++) {
500 for (
unsigned int i = 0; i < size; i++) {
501 for (
unsigned int j = 0; j < 6; j++) {
502 if (!
vpMath::equal(m_big[cpt * size + i][j], submatrices[(
size_t)cpt][i][j],
503 std::numeric_limits<double>::epsilon())) {
504 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
516 std::cout <<
"Insert empty matrices:" << std::endl;
517 std::cout <<
"m1:\n" << m1 << std::endl;
518 std::cout <<
"m2:\n" << m2 << std::endl;
519 std::cout <<
"m3:\n" << m3 << std::endl;
521 std::cout <<
"------------------------" << std::endl;
522 std::cout <<
"--- TEST vpMatrix stack()" << std::endl;
523 std::cout <<
"------------------------" << std::endl;
529 std::cout <<
"L:\n" << L << std::endl;
533 std::cout <<
"L:\n" << L << std::endl;
538 for (
unsigned int i = 0; i < nb; i++) {
539 m_big_stack.
stack(submatrices[(
size_t)i]);
542 std::cout <<
"\nMatrix stack(): " << t <<
" ms" << std::endl;
544 if (!equalMatrix(m_big, m_big_stack)) {
545 std::cerr <<
"Problem with vpMatrix stack()!" << std::endl;
549 std::cout <<
"------------------------" << std::endl;
550 std::cout <<
"--- TEST vpMatrix stack(vpRowVector)" << std::endl;
551 std::cout <<
"------------------------" << std::endl;
555 for (
unsigned int i = 0; i < m_big_stack.
getRows(); i++) {
559 std::cout <<
"\nMatrix stack(vpRowVector): " << t <<
" ms" << std::endl;
561 if (!equalMatrix(m_big_stack, m_big_stack_row)) {
562 std::cerr <<
"Problem with vpMatrix stack(vpRowVector)!" << std::endl;
566 std::cout <<
"------------------------" << std::endl;
567 std::cout <<
"--- TEST vpMatrix::stack()" << std::endl;
568 std::cout <<
"------------------------" << std::endl;
574 std::cout <<
"L:\n" << L << std::endl;
579 std::cout <<
"L:\n" << L << std::endl;
582 vpMatrix m_big_stack_static, m_big_stack_static_tmp;
584 for (
unsigned int i = 0; i < nb; i++) {
585 vpMatrix::stack(m_big_stack_static_tmp, submatrices[(
size_t)i], m_big_stack_static);
586 m_big_stack_static_tmp = m_big_stack_static;
589 std::cout <<
"\nMatrix::stack(): " << t <<
" ms" << std::endl;
591 if (!equalMatrix(m_big, m_big_stack_static)) {
592 std::cerr <<
"Problem with vpMatrix::stack()!" << std::endl;
596 std::cout <<
"------------------------" << std::endl;
597 std::cout <<
"--- TEST vpMatrix::stack(vpMatrix, vpRowVector, vpMatrix)" << std::endl;
598 std::cout <<
"------------------------" << std::endl;
600 vpMatrix m_big_stack_static_row, m_big_stack_static_row_tmp;
602 for (
unsigned int i = 0; i < m_big_stack_static.
getRows(); i++) {
604 m_big_stack_static_row_tmp = m_big_stack_static_row;
607 std::cout <<
"\nMatrix::stack(vpMatrix, vpRowVector, vpMatrix): " << t <<
" ms" << std::endl;
609 if (!equalMatrix(m_big_stack_static, m_big_stack_static_row)) {
610 std::cerr <<
"Problem with vpMatrix::stack(vpMatrix, vpRowVector, " 619 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
620 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
621 m2[i][j] = getRandomValues(-100.0, 100.0);
625 unsigned int offset_i = 4, offset_j = 3;
626 m1.insert(m2, offset_i, offset_j);
628 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
629 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
630 if (!
vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
631 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
637 offset_i = 4, offset_j = 5;
638 m1.insert(m2, offset_i, offset_j);
640 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
641 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
642 if (!
vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
643 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
649 offset_i = 8, offset_j = 5;
650 m1.insert(m2, offset_i, offset_j);
652 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
653 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
654 if (!
vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
655 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
663 std::cout <<
"------------------------" << std::endl;
664 std::cout <<
"--- TEST vpMatrix::juxtaposeMatrices()" << std::endl;
665 std::cout <<
"------------------------" << std::endl;
668 for (
unsigned int i = 0; i < A.
getRows(); i++) {
669 for (
unsigned int j = 0; j < A.
getCols(); j++) {
673 B[i][j] = (i * B.
getCols() + j) * 10;
680 std::cout <<
"juxtaposeM:\n" << juxtaposeM << std::endl;
683 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) 685 std::cout <<
"------------------------" << std::endl;
686 std::cout <<
"--- BENCHMARK dgemm/dgemv" << std::endl;
687 std::cout <<
"------------------------" << std::endl;
689 size_t nb_matrices = 10000;
690 unsigned int rows = 200, cols = 6;
691 double min = -1.0, max = 1.0;
692 std::vector<vpMatrix> vec_A, vec_B, vec_C, vec_C_regular;
693 vec_C.reserve(nb_matrices);
694 vec_C_regular.reserve(nb_matrices);
696 for (
size_t i = 0; i < nb_matrices; i++) {
697 vec_A.push_back(generateRandomMatrix(cols, rows, min, max));
698 vec_B.push_back(generateRandomMatrix(rows, cols, min, max));
702 for (
size_t i = 0; i < nb_matrices; i++) {
703 vec_C.push_back(vec_A[i] * vec_B[i]);
706 std::cout << nb_matrices <<
" matrix multiplication: (6x200) x (200x6)" << std::endl;
707 std::cout <<
"Lapack: " << t <<
" ms" << std::endl;
708 std::cout <<
"vec_C:\n" << vec_C.back() << std::endl;
711 for (
size_t i = 0; i < nb_matrices; i++) {
712 vec_C_regular.push_back(dgemm_regular(vec_A[i], vec_B[i]));
715 std::cout <<
"\nRegular: " << t <<
" ms" << std::endl;
716 std::cout <<
"vec_C_regular:\n" << vec_C_regular.back() << std::endl;
718 vpMatrix A = generateRandomMatrix(480, 640, min, max), B = generateRandomMatrix(640, 480, min, max);
724 std::cout <<
"\nMatrix multiplication: (480x640) x (640x480)" << std::endl;
725 std::cout <<
"Lapack: " << t <<
" ms" << std::endl;
729 AB_regular = dgemm_regular(A, B);
731 std::cout <<
"Regular: " << t <<
" ms" << std::endl;
733 bool res = equalMatrix(AB, AB_regular, 1e-9);
734 std::cout <<
"Check result: " << res << std::endl;
736 std::cerr <<
"Problem with matrix multiplication!" << std::endl;
740 int nb_iterations = 1000;
741 vpMatrix L = generateRandomMatrix(1000, 6, min, max);
745 for (
int i = 0; i < nb_iterations; i++)
748 std::cout <<
"\n" << nb_iterations <<
" iterations of AtA for size: (1000x6)" << std::endl;
749 std::cout <<
"Lapack: " << t <<
" ms" << std::endl;
750 std::cout <<
"LTL:\n" << LTL << std::endl;
753 for (
int i = 0; i < nb_iterations; i++)
754 LTL_regular = AtA_regular(L);
756 std::cout <<
"\nRegular: " << t <<
" ms" << std::endl;
757 std::cout <<
"LTL_regular:\n" << LTL_regular << std::endl;
758 res = equalMatrix(LTL, LTL_regular, 1e-9);
759 std::cout <<
"Check result: " << res << std::endl;
761 std::cerr <<
"Problem with vpMatrix::AtA()!" << std::endl;
765 vpMatrix LT = generateRandomMatrix(6, 1000, min, max);
766 vpColVector R = generateRandomVector(1000, min, max);
770 for (
int i = 0; i < nb_iterations; i++)
775 <<
" iterations of matrix vector multiplication: (6x1000) x " 778 std::cout <<
"Lapack: " << t <<
" ms" << std::endl;
779 std::cout <<
"LTR:\n" << LTR.
t() << std::endl;
782 for (
int i = 0; i < nb_iterations; i++)
783 LTR_regular = dgemv_regular(LT, R);
785 std::cout <<
"\nRegular: " << t <<
" ms" << std::endl;
786 std::cout <<
"LTR_regular:\n" << LTR_regular.
t() << std::endl;
787 res = equalMatrix(LTR, LTR_regular, 1e-9);
788 std::cout <<
"Check result: " << res << std::endl;
790 std::cerr <<
"Problem with dgemv!" << std::endl;
794 vpVelocityTwistMatrix V(getRandomValues(min, max), getRandomValues(min, max), getRandomValues(min, max),
795 getRandomValues(min, max), getRandomValues(min, max), getRandomValues(min, max));
799 for (
int i = 0; i < nb_iterations; i++)
804 <<
" iterations of matrix velocity twist matrix " 805 "multiplication: (1000x6) x (6x6)" 807 std::cout <<
"Lapack: " << t <<
" ms" << std::endl;
810 for (
int i = 0; i < nb_iterations; i++)
811 LV_regular = mat_mul_twist_matrix(L, V);
813 std::cout <<
"Regular: " << t <<
" ms" << std::endl;
814 res = equalMatrix(LV, LV_regular, 1e-9);
815 std::cout <<
"Check result: " << res << std::endl;
817 std::cerr <<
"Problem with matrix and velocity twist matrix multiplication!" << std::endl;
823 #ifdef VISP_HAVE_CPP11_COMPATIBILITY 825 std::vector<vpMatrix> vec_mat;
826 vec_mat.emplace_back(5, 5);
832 std::cout <<
"\n1) A+B:\n" << res << std::endl;
836 std::cout <<
"\n2) A+B:\n" << res2 << std::endl;
841 std::cout <<
"------------------------" << std::endl;
842 std::cout <<
"--- TEST vpMatrix::hadamard()" << std::endl;
843 std::cout <<
"------------------------" << std::endl;
846 for (
unsigned int i = 0; i < M1.
size(); i++) {
851 std::cout <<
"M1:\n" << M1 << std::endl;
852 std::cout <<
"\nM2:\n" << M2 << std::endl;
854 std::cout <<
"\nRes:\n" << M2 << std::endl;
857 std::cout <<
"\nAll tests succeed" << std::endl;
860 std::cout <<
"Catch an exception: " << e << std::endl;
Implementation of a matrix and operations on matrices.
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
vpRowVector getRow(const unsigned int i) const
static bool loadMatrix(const std::string &filename, vpArray2D< double > &M, const bool binary=false, char *header=NULL)
Implementation of row vector and the associated operations.
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
static bool equal(double x, double y, double s=0.001)
void stack(const vpMatrix &A)
error that can be emited by ViSP classes.
unsigned int getRows() const
Type * data
Address of the first element of the data array.
unsigned int size() const
Return the number of elements of the 2D array.
VISP_EXPORT double measureTimeMs()
int print(std::ostream &s, unsigned int length, char const *intro=0) const
Implementation of a rotation matrix and operations on such kind of matrices.
unsigned int getCols() const
vpMatrix hadamard(const vpMatrix &m) const
void init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
static double rad(double deg)
void resize(const unsigned int i, const bool flagNullify=true)
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)
int print(std::ostream &s, unsigned int length, char const *intro=0) const
Implementation of column vector and the associated operations.
static bool saveMatrixYAML(const std::string &filename, const vpArray2D< double > &M, const char *header="")
static bool saveMatrix(const std::string &filename, const vpArray2D< double > &M, const bool binary=false, const char *header="")
void resize(const unsigned int i, const bool flagNullify=true)
static bool loadMatrixYAML(const std::string &filename, vpArray2D< double > &M, char *header=NULL)