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>
56 bool test_memory(
unsigned int nrows,
unsigned int ncols,
const vpMatrix &M,
const std::string &matrix_name,
bool pointer_is_null)
58 if (pointer_is_null) {
60 std::cerr <<
"Wrong data pointer (" << M.
data <<
") in matrix " << matrix_name <<
": should be null" << std::endl;
65 std::cerr <<
"Wrong data pointer (" << M.
data <<
") in matrix " << matrix_name <<
": should be non null" << std::endl;
71 std::cerr <<
"Wrong matrix " << matrix_name <<
"(" << nrows <<
", " << ncols <<
" size: "
75 std::cout <<
"Test matrix " << matrix_name <<
" succeed" << std::endl;
79 bool test(
const std::string &s,
const vpMatrix &M,
const std::vector<double> &bench)
81 static unsigned int cpt = 0;
82 std::cout <<
"** Test " << ++cpt << std::endl;
83 std::cout << s <<
"(" << M.
getRows() <<
"," << M.
getCols() <<
") = \n" << M << std::endl;
84 if (bench.size() != M.
size()) {
85 std::cout <<
"Test fails: bad size wrt bench" << std::endl;
88 for (
unsigned int i = 0; i < M.
size(); i++) {
89 if (std::fabs(M.
data[i] - bench[i]) > std::fabs(M.
data[i]) * std::numeric_limits<double>::epsilon()) {
90 std::cout <<
"Test fails: bad content" << std::endl;
98 double getRandomValues(
double min,
double max) {
return (max - min) * ((double)rand() / (double)RAND_MAX) + min; }
100 bool equalMatrix(
const vpMatrix &A,
const vpMatrix &B,
double tol = std::numeric_limits<double>::epsilon())
106 for (
unsigned int i = 0; i < A.
getRows(); i++) {
107 for (
unsigned int j = 0; j < A.
getCols(); j++) {
117 vpMatrix generateRandomMatrix(
unsigned int rows,
unsigned int cols,
double min,
double max)
121 for (
unsigned int i = 0; i < M.
getRows(); i++) {
122 for (
unsigned int j = 0; j < M.
getCols(); j++) {
123 M[i][j] = getRandomValues(min, max);
130 std::vector<double> computeHadamard(
const std::vector<double> &v1,
const std::vector<double> &v2)
132 std::vector<double> result;
133 std::transform(v1.begin(), v1.end(), v2.begin(), std::back_inserter(result), std::multiplies<double>());
138 int main(
int argc,
char *argv[])
142 for (
int i = 1; i < argc; i++) {
143 if (std::string(argv[i]) ==
"--benchmark") {
149 unsigned int nrows = 2, ncols = 3;
151 if (test_memory(nrows, ncols, A,
"A",
false) ==
false) {
155 if (test_memory(0, 0, B,
"B",
true) ==
false) {
158 if (test_memory(0, 0, C,
"C",
true)==
false) {
162 if (test_memory(nrows, ncols, B,
"B",
false)==
false) {
166 if (test_memory(0, 0, C,
"C",
true)==
false) {
171 const double val = 10.0;
173 M.
resize(5, 5,
false,
false);
175 for (
unsigned int i = 0; i < M.
getRows(); i++) {
176 for (
unsigned int j = 0; j < M.
getCols(); j++) {
177 if (!
vpMath::equal(M[i][j], val, std::numeric_limits<double>::epsilon())) {
178 std::cerr <<
"Issue with matrix assignment with value." << std::endl;
182 if (!
vpMath::equal(M2[i][j], val, std::numeric_limits<double>::epsilon())) {
183 std::cerr <<
"Issue with matrix constructor initialized with value." << std::endl;
191 std::vector<double> bench(9, 0);
192 bench[2] = bench[4] = bench[6] = 1.;
194 M[2][0] = M[1][1] = M[0][2] = 1.;
196 if (test(
"R1", R1, bench) ==
false)
200 if (test(
"R2", R2, bench) ==
false)
206 std::vector<double> bench(6, 1);
208 if (test(
"M1", M1, bench) ==
false)
211 if (test(
"M2", M2, bench) ==
false)
217 for (
unsigned int i = 0; i < M.
getRows(); i++) {
218 for (
unsigned int j = 0; j < M.
getCols(); j++) {
223 M.
print(std::cout, 4);
226 N.
init(M, 0, 1, 2, 3);
228 N.
print(std::cout, 4);
229 std::string header(
"My 4-by-5 matrix\nwith a second line");
233 std::cout <<
"Matrix saved in matrix.mat file" << std::endl;
241 std::cout <<
"Matrix loaded from matrix.mat file with header \"" << header_ <<
"\": \n" << M1 << std::endl;
244 if (header != std::string(header_)) {
245 std::cout <<
"Bad header in matrix.mat" << std::endl;
251 std::cout <<
"Matrix saved in matrix.bin file" << std::endl;
257 std::cout <<
"Matrix loaded from matrix.bin file with header \"" << header_ <<
"\": \n" << M1 << std::endl;
260 if (header != std::string(header_)) {
261 std::cout <<
"Bad header in matrix.bin" << std::endl;
267 std::cout <<
"Matrix saved in matrix.yml file" << std::endl;
274 std::cout <<
"Matrix loaded from matrix.yml file with header \"" << header_ <<
"\": \n" << M2 << std::endl;
277 if (header != std::string(header_)) {
278 std::cout <<
"Bad header in matrix.mat" << std::endl;
285 std::cout <<
"R: \n" << R << std::endl;
287 std::cout <<
"M1: \n" << M1 << std::endl;
289 std::cout <<
"M2: \n" << M2 << std::endl;
291 std::cout <<
"M3: \n" << M3 << std::endl;
293 std::cout <<
"M4: \n" << M4 << std::endl;
297 std::cout <<
"------------------------" << std::endl;
298 std::cout <<
"--- TEST PRETTY PRINT---" << std::endl;
299 std::cout <<
"------------------------" << std::endl;
303 std::cout <<
"call std::cout << M;" << std::endl;
304 std::cout << M << std::endl;
306 std::cout <<
"call M.print (std::cout, 4);" << std::endl;
307 M.
print(std::cout, 4);
309 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, 6);" << std::endl;
318 M.
print(std::cout, 6);
319 std::cout << std::endl;
321 std::cout <<
"------------------------" << std::endl;
325 std::cout <<
"call std::cout << M;" << std::endl;
326 std::cout << M << std::endl;
328 std::cout <<
"call M.print (std::cout, 10);" << std::endl;
329 M.
print(std::cout, 10);
330 std::cout << std::endl;
332 std::cout <<
"call M.print (std::cout, 2);" << std::endl;
333 M.
print(std::cout, 2);
334 std::cout << std::endl;
336 std::cout <<
"------------------------" << std::endl;
339 M[0][2] = -0.0000876;
340 std::cout <<
"call std::cout << M;" << std::endl;
341 std::cout << M << std::endl;
343 std::cout <<
"call M.print (std::cout, 4);" << std::endl;
344 M.
print(std::cout, 4);
345 std::cout << std::endl;
346 std::cout <<
"call M.print (std::cout, 6, \"M\");" << std::endl;
347 M.
print(std::cout, 6,
"M");
348 std::cout << std::endl;
349 std::cout <<
"call M.print (std::cout, 10, \"M\");" << std::endl;
350 M.
print(std::cout, 10,
"M");
351 std::cout << std::endl;
360 std::cout <<
"call std::cout << M;" << std::endl;
361 std::cout << M << std::endl;
362 std::cout <<
"call M.print (std::cout, 5, \"M\");" << std::endl;
363 M.
print(std::cout, 5,
"M");
364 std::cout << std::endl;
373 std::cout <<
"call std::cout << M;" << std::endl;
374 std::cout << M << std::endl;
375 std::cout <<
"call M.print (std::cout, 5, \"M\");" << std::endl;
376 M.
print(std::cout, 5,
"M");
377 std::cout << std::endl;
379 std::cout <<
"------------------------" << std::endl;
380 std::cout <<
"--- TEST RESIZE --------" << std::endl;
381 std::cout <<
"------------------------" << std::endl;
382 std::cout <<
"5x5" << std::endl;
384 std::cout << M << std::endl;
385 std::cout <<
"3x2" << std::endl;
387 std::cout << M << std::endl;
388 std::cout <<
"2x2" << std::endl;
390 std::cout << M << std::endl;
391 std::cout <<
"------------------------" << std::endl;
400 std::cout <<
"------------------------" << std::endl;
401 std::cout <<
"--- TEST vpRowVector * vpColVector" << std::endl;
402 std::cout <<
"------------------------" << std::endl;
415 r.print(std::cout, 2,
"r");
416 c.print(std::cout, 2,
"c");
417 std::cout <<
"r * c = " << rc << std::endl;
419 std::cout <<
"------------------------" << std::endl;
420 std::cout <<
"--- TEST vpRowVector * vpMatrix" << std::endl;
421 std::cout <<
"------------------------" << std::endl;
430 r.
print(std::cout, 2,
"r");
431 M.print(std::cout, 10,
"M");
432 std::cout <<
"r * M = " << rM << std::endl;
434 std::cout <<
"------------------------" << std::endl;
435 std::cout <<
"--- TEST vpGEMM " << std::endl;
436 std::cout <<
"------------------------" << std::endl;
451 vpGEMM(M, N, 2, C, 3, D, VP_GEMM_A_T);
452 std::cout << D << std::endl;
456 std::cout <<
"------------------------" << std::endl;
457 std::cout <<
"--- TEST vpMatrix insert() with same colNum " << std::endl;
458 std::cout <<
"------------------------" << std::endl;
459 unsigned int nb = ctest ? 10 : 100;
460 const unsigned int size = ctest ? 10 : 100;
463 std::vector<vpMatrix> submatrices(nb);
464 for (
size_t cpt = 0; cpt < submatrices.size(); cpt++) {
467 for (
unsigned int i = 0; i < m.getRows(); i++) {
468 for (
unsigned int j = 0; j < m.getCols(); j++) {
469 m[i][j] = getRandomValues(-100.0, 100.0);
473 submatrices[cpt] = m;
477 for (
unsigned int i = 0; i < nb; i++) {
478 m_big.insert(submatrices[(
size_t)i], i * size, 0);
481 std::cout <<
"Matrix insert(): " << t <<
" ms" << std::endl;
483 for (
unsigned int cpt = 0; cpt < nb; cpt++) {
484 for (
unsigned int i = 0; i < size; i++) {
485 for (
unsigned int j = 0; j < 6; j++) {
486 if (!
vpMath::equal(m_big[cpt * size + i][j], submatrices[(
size_t)cpt][i][j],
487 std::numeric_limits<double>::epsilon())) {
488 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
500 std::cout <<
"Insert empty matrices:" << std::endl;
501 std::cout <<
"m1:\n" << m1 << std::endl;
502 std::cout <<
"m2:\n" << m2 << std::endl;
503 std::cout <<
"m3:\n" << m3 << std::endl;
505 std::cout <<
"\n------------------------" << std::endl;
506 std::cout <<
"--- TEST vpMatrix stack()" << std::endl;
507 std::cout <<
"------------------------" << std::endl;
513 std::cout <<
"L:\n" << L << std::endl;
517 std::cout <<
"L:\n" << L << std::endl;
523 for (
unsigned int i = 0; i < nb; i++) {
524 m_big_stack.
stack(submatrices[(
size_t)i]);
527 std::cout <<
"Matrix stack(): " << t <<
" ms" << std::endl;
529 if (!equalMatrix(m_big, m_big_stack)) {
530 std::cerr <<
"Problem with vpMatrix stack()!" << std::endl;
535 std::cout <<
"\n------------------------" << std::endl;
536 std::cout <<
"--- TEST vpMatrix stack(vpRowVector)" << std::endl;
537 std::cout <<
"------------------------" << std::endl;
539 vpMatrix m_big_stack = generateRandomMatrix(10000, ctest ? 10 : 100, -1000.0, 1000.0);
540 std::cout <<
"m_big_stack: " << m_big_stack.
getRows() <<
"x" << m_big_stack.
getCols() << std::endl;
544 for (
unsigned int i = 0; i < m_big_stack.
getRows(); i++) {
548 std::cout <<
"Matrix stack(vpRowVector): " << t <<
" ms" << std::endl;
550 if (!equalMatrix(m_big_stack, m_big_stack_row)) {
551 std::cerr <<
"Problem with vpMatrix stack(vpRowVector)!" << std::endl;
555 std::cout <<
"\n------------------------" << std::endl;
556 std::cout <<
"--- TEST vpMatrix stack(vpColVector)" << std::endl;
557 std::cout <<
"------------------------" << std::endl;
561 for (
unsigned int j = 0; j < m_big_stack.
getCols(); j++) {
565 std::cout <<
"Matrix stack(vpColVector): " << t <<
" ms" << std::endl;
567 if (!equalMatrix(m_big_stack, m_big_stack_col)) {
568 std::cerr <<
"Problem with vpMatrix stack(vpColVector)!" << std::endl;
572 std::cout <<
"\n------------------------" << std::endl;
573 std::cout <<
"--- TEST vpMatrix::stack()" << std::endl;
574 std::cout <<
"------------------------" << std::endl;
580 std::cout <<
"L:\n" << L << std::endl;
585 std::cout <<
"L:\n" << L << std::endl;
589 vpMatrix m_big_stack_static, m_big_stack_static_tmp;
591 for (
unsigned int i = 0; i < nb; i++) {
592 vpMatrix::stack(m_big_stack_static_tmp, submatrices[(
size_t)i], m_big_stack_static);
593 m_big_stack_static_tmp = m_big_stack_static;
596 std::cout <<
"Matrix::stack(): " << t <<
" ms" << std::endl;
598 if (!equalMatrix(m_big, m_big_stack_static)) {
599 std::cerr <<
"Problem with vpMatrix::stack()!" << std::endl;
604 std::cout <<
"\n------------------------" << std::endl;
605 std::cout <<
"--- TEST vpMatrix::stack(vpMatrix, vpRowVector, vpMatrix)" << std::endl;
606 std::cout <<
"------------------------" << std::endl;
608 vpMatrix m_big_stack_static = generateRandomMatrix(ctest ? 100 : 1000, ctest ? 10 : 100, -1000.0, 1000.0);
609 std::cout <<
"m_big_stack_static: " << m_big_stack_static.
getRows() <<
"x" << m_big_stack_static.
getCols()
612 vpMatrix m_big_stack_static_row, m_big_stack_static_row_tmp;
614 for (
unsigned int i = 0; i < m_big_stack_static.
getRows(); i++) {
616 m_big_stack_static_row_tmp = m_big_stack_static_row;
619 std::cout <<
"Matrix::stack(vpMatrix, vpRowVector, vpMatrix): " << t <<
" ms" << std::endl;
621 if (!equalMatrix(m_big_stack_static, m_big_stack_static_row)) {
622 std::cerr <<
"Problem with vpMatrix::stack(vpMatrix, vpRowVector, "
628 std::cout <<
"\n------------------------" << std::endl;
629 std::cout <<
"--- TEST vpMatrix::stack(vpMatrix, vpColVector, vpMatrix)" << std::endl;
630 std::cout <<
"------------------------" << std::endl;
632 vpMatrix m_big_stack_static_col, m_big_stack_static_col_tmp;
634 for (
unsigned int j = 0; j < m_big_stack_static.
getCols(); j++) {
636 m_big_stack_static_col_tmp = m_big_stack_static_col;
639 std::cout <<
"Matrix::stack(vpMatrix, vpColVector, vpMatrix): " << t <<
" ms" << std::endl;
641 if (!equalMatrix(m_big_stack_static, m_big_stack_static_col)) {
642 std::cerr <<
"Problem with vpMatrix::stack(vpMatrix, vpColVector, "
651 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
652 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
653 m2[i][j] = getRandomValues(-100.0, 100.0);
657 unsigned int offset_i = 4, offset_j = 3;
658 m1.insert(m2, offset_i, offset_j);
660 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
661 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
662 if (!
vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
663 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
671 m1.insert(m2, offset_i, offset_j);
673 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
674 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
675 if (!
vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
676 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
684 m1.insert(m2, offset_i, offset_j);
686 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
687 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
688 if (!
vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
689 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
697 std::cout <<
"\n------------------------" << std::endl;
698 std::cout <<
"--- TEST vpMatrix::juxtaposeMatrices()" << std::endl;
699 std::cout <<
"------------------------" << std::endl;
702 for (
unsigned int i = 0; i < A.
getRows(); i++) {
703 for (
unsigned int j = 0; j < A.
getCols(); j++) {
707 B[i][j] = (i * B.
getCols() + j) * 10;
714 std::cout <<
"juxtaposeM:\n" << juxtaposeM << std::endl;
717 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
719 std::vector<vpMatrix> vec_mat;
720 vec_mat.emplace_back(5, 5);
726 std::cout <<
"\n1) A+B:\n" << res << std::endl;
730 std::cout <<
"\n2) A+B:\n" << res2 << std::endl;
735 std::cout <<
"\n------------------------" << std::endl;
736 std::cout <<
"--- TEST vpMatrix::hadamard()" << std::endl;
737 std::cout <<
"------------------------" << std::endl;
740 for (
unsigned int i = 0; i < M1.
size(); i++) {
746 std::vector<double> references = computeHadamard(std::vector<double>(M1.
data, M1.
data + M1.
size()),
749 std::cout <<
"M1:\n" << M1 << std::endl;
750 std::cout <<
"\nM2:\n" << M2 << std::endl;
752 std::cout <<
"\nRes:\n" << M2 << std::endl;
754 if (!test(
"M2", M2, references)) {
755 std::cerr <<
"Error with Hadamard product" << std::endl;
761 std::cout <<
"\n------------------------" << std::endl;
762 std::cout <<
"--- TEST vpMatrix::stackColums()" << std::endl;
763 std::cout <<
"------------------------" << std::endl;
765 for (
unsigned int j = 0; j < M.getCols(); j++) {
766 for (
unsigned int i = 0; i < M.getRows(); i++) {
767 M[i][j] = i + j * M.getRows();
770 std::cout <<
"M:\n" << M << std::endl;
772 std::cout <<
"Column stack: " << v.
t() << std::endl;
773 if (M.size() != v.
size()) {
774 std::cerr <<
"Problem in vpMatrix::stackColumns(): size differ" << std::endl;
777 for (
unsigned int i = 0; i < v.
size(); i++) {
778 if (std::fabs(v[i] -
static_cast<double>(i)) > std::numeric_limits<double>::epsilon()) {
779 std::cerr <<
"Problem in vpMatrix::stackColumns(): content differ" << std::endl;
786 std::cout <<
"\n------------------------" << std::endl;
787 std::cout <<
"--- TEST vpMatrix::stackRows()" << std::endl;
788 std::cout <<
"------------------------" << std::endl;
790 for (
unsigned int i = 0; i < M.getRows(); i++) {
791 for (
unsigned int j = 0; j < M.getCols(); j++) {
792 M[i][j] = i * M.getCols() + j;
795 std::cout <<
"M:\n" << M << std::endl;
797 std::cout <<
"Rows stack: " << v << std::endl;
798 if (M.size() != v.
size()) {
799 std::cerr <<
"Problem in vpMatrix::stackRows(): size differ" << std::endl;
802 for (
unsigned int i = 0; i < v.
size(); i++) {
803 if (std::fabs(v[i] -
static_cast<double>(i)) > std::numeric_limits<double>::epsilon()) {
804 std::cerr <<
"Problem in vpMatrix::stackRows(): content differ" << std::endl;
811 std::cout <<
"\n------------------------" << std::endl;
812 std::cout <<
"--- TEST vpMatrix::getCol()" << std::endl;
813 std::cout <<
"------------------------" << std::endl;
815 for (
unsigned int i = 0; i < A.
getRows(); i++)
816 for (
unsigned int j = 0; j < A.
getCols(); j++)
824 std::cerr <<
"Problem in vpMatrix::getCol(): values are different" << std::endl;
833 std::cerr <<
"Problem in vpMatrix::getCol(): values are different" << std::endl;
840 std::cout <<
"\n------------------------" << std::endl;
841 std::cout <<
"--- TEST vpMatrix::getRow()" << std::endl;
842 std::cout <<
"------------------------" << std::endl;
844 for (
unsigned int i = 0; i < A.
getRows(); i++)
845 for (
unsigned int j = 0; j < A.
getCols(); j++)
853 std::cerr <<
"Problem in vpMatrix::getRow(): values are different" << std::endl;
862 std::cerr <<
"Problem in vpMatrix::getRow(): values are different" << std::endl;
869 std::cout <<
"\n------------------------" << std::endl;
870 std::cout <<
"--- TEST vpMatrix::getDiag()" << std::endl;
871 std::cout <<
"------------------------" << std::endl;
873 for (
unsigned int i = 0; i < A.
getRows(); i++)
874 for (
unsigned int j = 0; j < A.
getCols(); j++)
879 ref << 0.0, 5.0, 10.0;
881 std::cerr <<
"Problem in vpMatrix::getDiag(): values are different" << std::endl;
886 std::cout <<
"\nAll tests succeeded" << std::endl;
890 std::cout <<
"Catch an exception: " << e << std::endl;
unsigned int getCols() const
Type * data
Address of the first element of the data array.
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)
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.
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)
static bool loadMatrix(const std::string &filename, vpArray2D< double > &M, bool binary=false, char *header=nullptr)
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 loadMatrixYAML(const std::string &filename, vpArray2D< double > &M, char *header=nullptr)
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()