42#include <visp3/core/vpConfig.h>
43#include <visp3/core/vpDebug.h>
44#include <visp3/core/vpGEMM.h>
45#include <visp3/core/vpHomogeneousMatrix.h>
46#include <visp3/core/vpMath.h>
47#include <visp3/core/vpVelocityTwistMatrix.h>
56bool test(
const std::string &s,
const vpMatrix &M,
const std::vector<double> &bench)
58 static unsigned int cpt = 0;
59 std::cout <<
"** Test " << ++cpt << std::endl;
60 std::cout << s <<
"(" << M.
getRows() <<
"," << M.
getCols() <<
") = \n" << M << std::endl;
61 if (bench.size() != M.
size()) {
62 std::cout <<
"Test fails: bad size wrt bench" << std::endl;
65 for (
unsigned int i = 0; i < M.
size(); i++) {
66 if (std::fabs(M.
data[i] - bench[i]) > std::fabs(M.
data[i]) * std::numeric_limits<double>::epsilon()) {
67 std::cout <<
"Test fails: bad content" << std::endl;
75double getRandomValues(
double min,
double max) {
return (max - min) * ((double)rand() / (double)RAND_MAX) + min; }
77bool equalMatrix(
const vpMatrix &A,
const vpMatrix &B,
double tol = std::numeric_limits<double>::epsilon())
83 for (
unsigned int i = 0; i < A.
getRows(); i++) {
84 for (
unsigned int j = 0; j < A.
getCols(); j++) {
94vpMatrix generateRandomMatrix(
unsigned int rows,
unsigned int cols,
double min,
double max)
98 for (
unsigned int i = 0; i < M.
getRows(); i++) {
99 for (
unsigned int j = 0; j < M.
getCols(); j++) {
100 M[i][j] = getRandomValues(min, max);
107std::vector<double> computeHadamard(
const std::vector<double> &v1,
const std::vector<double> &v2)
109 std::vector<double> result;
110 std::transform(v1.begin(), v1.end(), v2.begin(), std::back_inserter(result), std::multiplies<double>());
115int main(
int argc,
char *argv[])
119 for (
int i = 1; i < argc; i++) {
120 if (std::string(argv[i]) ==
"--benchmark") {
126 const double val = 10.0;
128 M.
resize(5, 5,
false,
false);
130 for (
unsigned int i = 0; i < M.
getRows(); i++) {
131 for (
unsigned int j = 0; j < M.
getCols(); j++) {
132 if (!
vpMath::equal(M[i][j], val, std::numeric_limits<double>::epsilon())) {
133 std::cerr <<
"Issue with matrix assignment with value." << std::endl;
137 if (!
vpMath::equal(M2[i][j], val, std::numeric_limits<double>::epsilon())) {
138 std::cerr <<
"Issue with matrix constructor initialized with value." << std::endl;
146 std::vector<double> bench(9, 0);
147 bench[2] = bench[4] = bench[6] = 1.;
149 M[2][0] = M[1][1] = M[0][2] = 1.;
151 if (test(
"R1", R1, bench) ==
false)
155 if (test(
"R2", R2, bench) ==
false)
161 std::vector<double> bench(6, 1);
163 if (test(
"M1", M1, bench) ==
false)
166 if (test(
"M2", M2, bench) ==
false)
172 for (
unsigned int i = 0; i < M.
getRows(); i++) {
173 for (
unsigned int j = 0; j < M.
getCols(); j++) {
178 M.
print(std::cout, 4);
181 N.
init(M, 0, 1, 2, 3);
183 N.
print(std::cout, 4);
184 std::string header(
"My 4-by-5 matrix\nwith a second line");
188 std::cout <<
"Matrix saved in matrix.mat file" << std::endl;
196 std::cout <<
"Matrix loaded from matrix.mat file with header \"" << header_ <<
"\": \n" << M1 << std::endl;
199 if (header != std::string(header_)) {
200 std::cout <<
"Bad header in matrix.mat" << std::endl;
206 std::cout <<
"Matrix saved in matrix.bin file" << std::endl;
212 std::cout <<
"Matrix loaded from matrix.bin file with header \"" << header_ <<
"\": \n" << M1 << std::endl;
215 if (header != std::string(header_)) {
216 std::cout <<
"Bad header in matrix.bin" << std::endl;
222 std::cout <<
"Matrix saved in matrix.yml file" << std::endl;
229 std::cout <<
"Matrix loaded from matrix.yml file with header \"" << header_ <<
"\": \n" << M2 << std::endl;
232 if (header != std::string(header_)) {
233 std::cout <<
"Bad header in matrix.mat" << std::endl;
240 std::cout <<
"R: \n" << R << std::endl;
242 std::cout <<
"M1: \n" << M1 << std::endl;
244 std::cout <<
"M2: \n" << M2 << std::endl;
246 std::cout <<
"M3: \n" << M3 << std::endl;
248 std::cout <<
"M4: \n" << M4 << std::endl;
252 std::cout <<
"------------------------" << std::endl;
253 std::cout <<
"--- TEST PRETTY PRINT---" << std::endl;
254 std::cout <<
"------------------------" << std::endl;
258 std::cout <<
"call std::cout << M;" << std::endl;
259 std::cout << M << std::endl;
261 std::cout <<
"call M.print (std::cout, 4);" << std::endl;
262 M.
print(std::cout, 4);
264 std::cout <<
"------------------------" << std::endl;
270 std::cout <<
"call std::cout << M;" << std::endl;
271 std::cout << M << std::endl;
272 std::cout <<
"call M.print (std::cout, 6);" << std::endl;
273 M.
print(std::cout, 6);
274 std::cout << std::endl;
276 std::cout <<
"------------------------" << std::endl;
280 std::cout <<
"call std::cout << M;" << std::endl;
281 std::cout << M << std::endl;
283 std::cout <<
"call M.print (std::cout, 10);" << std::endl;
284 M.
print(std::cout, 10);
285 std::cout << std::endl;
287 std::cout <<
"call M.print (std::cout, 2);" << std::endl;
288 M.
print(std::cout, 2);
289 std::cout << std::endl;
291 std::cout <<
"------------------------" << std::endl;
294 M[0][2] = -0.0000876;
295 std::cout <<
"call std::cout << M;" << std::endl;
296 std::cout << M << std::endl;
298 std::cout <<
"call M.print (std::cout, 4);" << std::endl;
299 M.
print(std::cout, 4);
300 std::cout << std::endl;
301 std::cout <<
"call M.print (std::cout, 6, \"M\");" << std::endl;
302 M.
print(std::cout, 6,
"M");
303 std::cout << std::endl;
304 std::cout <<
"call M.print (std::cout, 10, \"M\");" << std::endl;
305 M.
print(std::cout, 10,
"M");
306 std::cout << std::endl;
315 std::cout <<
"call std::cout << M;" << std::endl;
316 std::cout << M << std::endl;
317 std::cout <<
"call M.print (std::cout, 5, \"M\");" << std::endl;
318 M.
print(std::cout, 5,
"M");
319 std::cout << std::endl;
328 std::cout <<
"call std::cout << M;" << std::endl;
329 std::cout << M << std::endl;
330 std::cout <<
"call M.print (std::cout, 5, \"M\");" << std::endl;
331 M.
print(std::cout, 5,
"M");
332 std::cout << std::endl;
334 std::cout <<
"------------------------" << std::endl;
335 std::cout <<
"--- TEST RESIZE --------" << std::endl;
336 std::cout <<
"------------------------" << std::endl;
337 std::cout <<
"5x5" << std::endl;
339 std::cout << M << std::endl;
340 std::cout <<
"3x2" << std::endl;
342 std::cout << M << std::endl;
343 std::cout <<
"2x2" << std::endl;
345 std::cout << M << std::endl;
346 std::cout <<
"------------------------" << std::endl;
355 std::cout <<
"------------------------" << std::endl;
356 std::cout <<
"--- TEST vpRowVector * vpColVector" << std::endl;
357 std::cout <<
"------------------------" << std::endl;
370 r.print(std::cout, 2,
"r");
371 c.print(std::cout, 2,
"c");
372 std::cout <<
"r * c = " << rc << std::endl;
374 std::cout <<
"------------------------" << std::endl;
375 std::cout <<
"--- TEST vpRowVector * vpMatrix" << std::endl;
376 std::cout <<
"------------------------" << std::endl;
385 r.
print(std::cout, 2,
"r");
386 M.print(std::cout, 10,
"M");
387 std::cout <<
"r * M = " << rM << std::endl;
389 std::cout <<
"------------------------" << std::endl;
390 std::cout <<
"--- TEST vpGEMM " << std::endl;
391 std::cout <<
"------------------------" << std::endl;
406 vpGEMM(M, N, 2, C, 3, D, VP_GEMM_A_T);
407 std::cout << D << std::endl;
411 std::cout <<
"------------------------" << std::endl;
412 std::cout <<
"--- TEST vpMatrix insert() with same colNum " << std::endl;
413 std::cout <<
"------------------------" << std::endl;
414 unsigned int nb = ctest ? 10 : 100;
415 const unsigned int size = ctest ? 10 : 100;
418 std::vector<vpMatrix> submatrices(nb);
419 for (
size_t cpt = 0; cpt < submatrices.size(); cpt++) {
422 for (
unsigned int i = 0; i < m.getRows(); i++) {
423 for (
unsigned int j = 0; j < m.getCols(); j++) {
424 m[i][j] = getRandomValues(-100.0, 100.0);
428 submatrices[cpt] = m;
432 for (
unsigned int i = 0; i < nb; i++) {
433 m_big.insert(submatrices[(
size_t)i], i * size, 0);
436 std::cout <<
"Matrix insert(): " << t <<
" ms" << std::endl;
438 for (
unsigned int cpt = 0; cpt < nb; cpt++) {
439 for (
unsigned int i = 0; i < size; i++) {
440 for (
unsigned int j = 0; j < 6; j++) {
441 if (!
vpMath::equal(m_big[cpt * size + i][j], submatrices[(
size_t)cpt][i][j],
442 std::numeric_limits<double>::epsilon())) {
443 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
455 std::cout <<
"Insert empty matrices:" << std::endl;
456 std::cout <<
"m1:\n" << m1 << std::endl;
457 std::cout <<
"m2:\n" << m2 << std::endl;
458 std::cout <<
"m3:\n" << m3 << std::endl;
460 std::cout <<
"\n------------------------" << std::endl;
461 std::cout <<
"--- TEST vpMatrix stack()" << std::endl;
462 std::cout <<
"------------------------" << std::endl;
468 std::cout <<
"L:\n" << L << std::endl;
472 std::cout <<
"L:\n" << L << std::endl;
478 for (
unsigned int i = 0; i < nb; i++) {
479 m_big_stack.
stack(submatrices[(
size_t)i]);
482 std::cout <<
"Matrix stack(): " << t <<
" ms" << std::endl;
484 if (!equalMatrix(m_big, m_big_stack)) {
485 std::cerr <<
"Problem with vpMatrix stack()!" << std::endl;
490 std::cout <<
"\n------------------------" << std::endl;
491 std::cout <<
"--- TEST vpMatrix stack(vpRowVector)" << std::endl;
492 std::cout <<
"------------------------" << std::endl;
494 vpMatrix m_big_stack = generateRandomMatrix(10000, ctest ? 10 : 100, -1000.0, 1000.0);
495 std::cout <<
"m_big_stack: " << m_big_stack.
getRows() <<
"x" << m_big_stack.
getCols() << std::endl;
499 for (
unsigned int i = 0; i < m_big_stack.
getRows(); i++) {
503 std::cout <<
"Matrix stack(vpRowVector): " << t <<
" ms" << std::endl;
505 if (!equalMatrix(m_big_stack, m_big_stack_row)) {
506 std::cerr <<
"Problem with vpMatrix stack(vpRowVector)!" << std::endl;
510 std::cout <<
"\n------------------------" << std::endl;
511 std::cout <<
"--- TEST vpMatrix stack(vpColVector)" << std::endl;
512 std::cout <<
"------------------------" << std::endl;
516 for (
unsigned int j = 0; j < m_big_stack.
getCols(); j++) {
520 std::cout <<
"Matrix stack(vpColVector): " << t <<
" ms" << std::endl;
522 if (!equalMatrix(m_big_stack, m_big_stack_col)) {
523 std::cerr <<
"Problem with vpMatrix stack(vpColVector)!" << std::endl;
527 std::cout <<
"\n------------------------" << std::endl;
528 std::cout <<
"--- TEST vpMatrix::stack()" << std::endl;
529 std::cout <<
"------------------------" << std::endl;
535 std::cout <<
"L:\n" << L << std::endl;
540 std::cout <<
"L:\n" << L << std::endl;
544 vpMatrix m_big_stack_static, m_big_stack_static_tmp;
546 for (
unsigned int i = 0; i < nb; i++) {
547 vpMatrix::stack(m_big_stack_static_tmp, submatrices[(
size_t)i], m_big_stack_static);
548 m_big_stack_static_tmp = m_big_stack_static;
551 std::cout <<
"Matrix::stack(): " << t <<
" ms" << std::endl;
553 if (!equalMatrix(m_big, m_big_stack_static)) {
554 std::cerr <<
"Problem with vpMatrix::stack()!" << std::endl;
559 std::cout <<
"\n------------------------" << std::endl;
560 std::cout <<
"--- TEST vpMatrix::stack(vpMatrix, vpRowVector, vpMatrix)" << std::endl;
561 std::cout <<
"------------------------" << std::endl;
563 vpMatrix m_big_stack_static = generateRandomMatrix(ctest ? 100 : 1000, ctest ? 10 : 100, -1000.0, 1000.0);
564 std::cout <<
"m_big_stack_static: " << m_big_stack_static.
getRows() <<
"x" << m_big_stack_static.
getCols()
567 vpMatrix m_big_stack_static_row, m_big_stack_static_row_tmp;
569 for (
unsigned int i = 0; i < m_big_stack_static.
getRows(); i++) {
571 m_big_stack_static_row_tmp = m_big_stack_static_row;
574 std::cout <<
"Matrix::stack(vpMatrix, vpRowVector, vpMatrix): " << t <<
" ms" << std::endl;
576 if (!equalMatrix(m_big_stack_static, m_big_stack_static_row)) {
577 std::cerr <<
"Problem with vpMatrix::stack(vpMatrix, vpRowVector, "
583 std::cout <<
"\n------------------------" << std::endl;
584 std::cout <<
"--- TEST vpMatrix::stack(vpMatrix, vpColVector, vpMatrix)" << std::endl;
585 std::cout <<
"------------------------" << std::endl;
587 vpMatrix m_big_stack_static_col, m_big_stack_static_col_tmp;
589 for (
unsigned int j = 0; j < m_big_stack_static.
getCols(); j++) {
591 m_big_stack_static_col_tmp = m_big_stack_static_col;
594 std::cout <<
"Matrix::stack(vpMatrix, vpColVector, vpMatrix): " << t <<
" ms" << std::endl;
596 if (!equalMatrix(m_big_stack_static, m_big_stack_static_col)) {
597 std::cerr <<
"Problem with vpMatrix::stack(vpMatrix, vpColVector, "
606 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
607 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
608 m2[i][j] = getRandomValues(-100.0, 100.0);
612 unsigned int offset_i = 4, offset_j = 3;
613 m1.insert(m2, offset_i, offset_j);
615 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
616 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
617 if (!
vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
618 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
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;
639 m1.insert(m2, offset_i, offset_j);
641 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
642 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
643 if (!
vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
644 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
652 std::cout <<
"\n------------------------" << std::endl;
653 std::cout <<
"--- TEST vpMatrix::juxtaposeMatrices()" << std::endl;
654 std::cout <<
"------------------------" << std::endl;
657 for (
unsigned int i = 0; i < A.
getRows(); i++) {
658 for (
unsigned int j = 0; j < A.
getCols(); j++) {
662 B[i][j] = (i * B.
getCols() + j) * 10;
669 std::cout <<
"juxtaposeM:\n" << juxtaposeM << std::endl;
672#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
674 std::vector<vpMatrix> vec_mat;
675 vec_mat.emplace_back(5, 5);
681 std::cout <<
"\n1) A+B:\n" << res << std::endl;
685 std::cout <<
"\n2) A+B:\n" << res2 << std::endl;
690 std::cout <<
"\n------------------------" << std::endl;
691 std::cout <<
"--- TEST vpMatrix::hadamard()" << std::endl;
692 std::cout <<
"------------------------" << std::endl;
695 for (
unsigned int i = 0; i < M1.
size(); i++) {
701 std::vector<double> references = computeHadamard(std::vector<double>(M1.
data, M1.
data + M1.
size()),
704 std::cout <<
"M1:\n" << M1 << std::endl;
705 std::cout <<
"\nM2:\n" << M2 << std::endl;
707 std::cout <<
"\nRes:\n" << M2 << std::endl;
709 if (!test(
"M2", M2, references)) {
710 std::cerr <<
"Error with Hadamard product" << std::endl;
716 std::cout <<
"\n------------------------" << std::endl;
717 std::cout <<
"--- TEST vpMatrix::stackColums()" << std::endl;
718 std::cout <<
"------------------------" << std::endl;
720 for (
unsigned int j = 0; j < M.getCols(); j++) {
721 for (
unsigned int i = 0; i < M.getRows(); i++) {
722 M[i][j] = i + j * M.getRows();
725 std::cout <<
"M:\n" << M << std::endl;
727 std::cout <<
"Column stack: " << v.
t() << std::endl;
728 if (M.size() != v.
size()) {
729 std::cerr <<
"Problem in vpMatrix::stackColumns(): size differ" << std::endl;
732 for (
unsigned int i = 0; i < v.
size(); i++) {
733 if (std::fabs(v[i] -
static_cast<double>(i)) > std::numeric_limits<double>::epsilon()) {
734 std::cerr <<
"Problem in vpMatrix::stackColumns(): content differ" << std::endl;
741 std::cout <<
"\n------------------------" << std::endl;
742 std::cout <<
"--- TEST vpMatrix::stackRows()" << std::endl;
743 std::cout <<
"------------------------" << std::endl;
745 for (
unsigned int i = 0; i < M.getRows(); i++) {
746 for (
unsigned int j = 0; j < M.getCols(); j++) {
747 M[i][j] = i * M.getCols() + j;
750 std::cout <<
"M:\n" << M << std::endl;
752 std::cout <<
"Rows stack: " << v << std::endl;
753 if (M.size() != v.
size()) {
754 std::cerr <<
"Problem in vpMatrix::stackRows(): size differ" << std::endl;
757 for (
unsigned int i = 0; i < v.
size(); i++) {
758 if (std::fabs(v[i] -
static_cast<double>(i)) > std::numeric_limits<double>::epsilon()) {
759 std::cerr <<
"Problem in vpMatrix::stackRows(): content differ" << std::endl;
766 std::cout <<
"\n------------------------" << std::endl;
767 std::cout <<
"--- TEST vpMatrix::getCol()" << std::endl;
768 std::cout <<
"------------------------" << std::endl;
770 for (
unsigned int i = 0; i < A.
getRows(); i++)
771 for (
unsigned int j = 0; j < A.
getCols(); j++)
779 std::cerr <<
"Problem in vpMatrix::getCol(): values are different" << std::endl;
788 std::cerr <<
"Problem in vpMatrix::getCol(): values are different" << std::endl;
795 std::cout <<
"\n------------------------" << std::endl;
796 std::cout <<
"--- TEST vpMatrix::getRow()" << std::endl;
797 std::cout <<
"------------------------" << std::endl;
799 for (
unsigned int i = 0; i < A.
getRows(); i++)
800 for (
unsigned int j = 0; j < A.
getCols(); j++)
808 std::cerr <<
"Problem in vpMatrix::getRow(): values are different" << std::endl;
817 std::cerr <<
"Problem in vpMatrix::getRow(): values are different" << std::endl;
824 std::cout <<
"\n------------------------" << std::endl;
825 std::cout <<
"--- TEST vpMatrix::getDiag()" << std::endl;
826 std::cout <<
"------------------------" << std::endl;
828 for (
unsigned int i = 0; i < A.
getRows(); i++)
829 for (
unsigned int j = 0; j < A.
getCols(); j++)
834 ref << 0.0, 5.0, 10.0;
836 std::cerr <<
"Problem in vpMatrix::getDiag(): values are different" << std::endl;
841 std::cout <<
"\nAll tests succeeded" << std::endl;
844 std::cout <<
"Catch an exception: " << e << std::endl;
unsigned int getCols() const
Type * data
Address of the first element of the data array.
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
unsigned int size() const
Return the number of elements of the 2D array.
unsigned int getRows() const
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
static double rad(double deg)
static bool equal(double x, double y, double threshold=0.001)
Implementation of a matrix and operations on matrices.
static bool loadMatrixYAML(const std::string &filename, vpArray2D< double > &M, char *header=NULL)
static bool loadMatrix(const std::string &filename, vpArray2D< double > &M, bool binary=false, char *header=NULL)
vpMatrix hadamard(const vpMatrix &m) const
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
void init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
void stack(const vpMatrix &A)
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
vpRowVector getRow(unsigned int i) const
vpColVector getDiag() const
vpColVector getCol(unsigned int j) const
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
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, bool binary=false, const char *header="")
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
VISP_EXPORT double measureTimeMs()