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(
double min,
double max)
78 return (max - min) * ((double)rand() / (double)RAND_MAX) + min;
81 bool equalMatrix(
const vpMatrix &A,
const vpMatrix &B,
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 vpMatrix generateRandomMatrix(
unsigned int rows,
unsigned int cols,
double min,
double max)
102 for (
unsigned int i = 0; i < M.
getRows(); i++) {
103 for (
unsigned int j = 0; j < M.
getCols(); j++) {
104 M[i][j] = getRandomValues(min, max);
112 int main(
int argc,
char *argv[])
116 for (
int i = 1; i < argc; i++) {
117 if (std::string(argv[i]) ==
"--benchmark") {
123 const double val = 10.0;
125 M.
resize(5, 5,
false,
false);
127 for (
unsigned int i = 0; i < M.
getRows(); i++) {
128 for (
unsigned int j = 0; j < M.
getCols(); j++) {
129 if (!
vpMath::equal(M[i][j], val, std::numeric_limits<double>::epsilon())) {
130 std::cerr <<
"Issue with matrix assignment with value." << std::endl;
134 if (!
vpMath::equal(M2[i][j], val, std::numeric_limits<double>::epsilon())) {
135 std::cerr <<
"Issue with matrix constructor initialized with value." << std::endl;
143 std::vector<double> bench(9, 0);
144 bench[2] = bench[4] = bench[6] = 1.;
146 M[2][0] = M[1][1] = M[0][2] = 1.;
148 if (test(
"R1", R1, bench) ==
false)
152 if (test(
"R2", R2, bench) ==
false)
158 std::vector<double> bench(6, 1);
160 if (test(
"M1", M1, bench) ==
false)
163 if (test(
"M2", M2, bench) ==
false)
169 for (
unsigned int i = 0; i < M.
getRows(); i++) {
170 for (
unsigned int j = 0; j < M.
getCols(); j++) {
175 M.
print(std::cout, 4);
178 N.
init(M, 0, 1, 2, 3);
180 N.
print(std::cout, 4);
181 std::string header(
"My 4-by-5 matrix\nwith a second line");
185 std::cout <<
"Matrix saved in matrix.mat file" << std::endl;
193 std::cout <<
"Matrix loaded from matrix.mat file with header \"" << header_ <<
"\": \n" << M1 << std::endl;
196 if (header != std::string(header_)) {
197 std::cout <<
"Bad header in matrix.mat" << std::endl;
203 std::cout <<
"Matrix saved in matrix.bin file" << std::endl;
209 std::cout <<
"Matrix loaded from matrix.bin file with header \"" << header_ <<
"\": \n" << M1 << std::endl;
212 if (header != std::string(header_)) {
213 std::cout <<
"Bad header in matrix.bin" << std::endl;
219 std::cout <<
"Matrix saved in matrix.yml file" << std::endl;
226 std::cout <<
"Matrix loaded from matrix.yml file with header \"" << header_ <<
"\": \n" << M2 << std::endl;
229 if (header != std::string(header_)) {
230 std::cout <<
"Bad header in matrix.mat" << std::endl;
237 std::cout <<
"R: \n" << R << std::endl;
239 std::cout <<
"M1: \n" << M1 << std::endl;
241 std::cout <<
"M2: \n" << M2 << std::endl;
243 std::cout <<
"M3: \n" << M3 << std::endl;
245 std::cout <<
"M4: \n" << M4 << std::endl;
249 std::cout <<
"------------------------" << std::endl;
250 std::cout <<
"--- TEST PRETTY PRINT---" << std::endl;
251 std::cout <<
"------------------------" << std::endl;
255 std::cout <<
"call std::cout << M;" << std::endl;
256 std::cout << M << std::endl;
258 std::cout <<
"call M.print (std::cout, 4);" << std::endl;
259 M.
print(std::cout, 4);
261 std::cout <<
"------------------------" << std::endl;
267 std::cout <<
"call std::cout << M;" << std::endl;
269 std::cout <<
"call M.print (std::cout, 6);" << std::endl;
270 M.
print(std::cout, 6);
271 std::cout << std::endl;
273 std::cout <<
"------------------------" << std::endl;
277 std::cout <<
"call std::cout << M;" << std::endl;
278 std::cout << M << std::endl;
280 std::cout <<
"call M.print (std::cout, 10);" << std::endl;
281 M.print(std::cout, 10);
282 std::cout << std::endl;
284 std::cout <<
"call M.print (std::cout, 2);" << std::endl;
285 M.print(std::cout, 2);
286 std::cout << std::endl;
288 std::cout <<
"------------------------" << std::endl;
291 M[0][2] = -0.0000000876;
292 std::cout <<
"call std::cout << M;" << std::endl;
293 std::cout << M << std::endl;
295 std::cout <<
"call M.print (std::cout, 4);" << std::endl;
296 M.print(std::cout, 4);
297 std::cout << std::endl;
298 std::cout <<
"call M.print (std::cout, 10, \"M\");" << std::endl;
299 M.print(std::cout, 10,
"M");
300 std::cout << std::endl;
301 std::cout <<
"call M.print (std::cout, 20, \"M\");" << std::endl;
302 M.print(std::cout, 20,
"M");
303 std::cout << std::endl;
305 std::cout <<
"------------------------" << std::endl;
306 std::cout <<
"--- TEST RESIZE --------" << std::endl;
307 std::cout <<
"------------------------" << std::endl;
308 std::cout <<
"5x5" << std::endl;
309 M.resize(5, 5,
false);
310 std::cout << M << std::endl;
311 std::cout <<
"3x2" << std::endl;
312 M.resize(3, 2,
false);
313 std::cout << M << std::endl;
314 std::cout <<
"2x2" << std::endl;
315 M.resize(2, 2,
false);
316 std::cout << M << std::endl;
317 std::cout <<
"------------------------" << std::endl;
326 std::cout <<
"------------------------" << std::endl;
327 std::cout <<
"--- TEST vpRowVector * vpColVector" << std::endl;
328 std::cout <<
"------------------------" << std::endl;
341 r.print(std::cout, 2,
"r");
342 c.print(std::cout, 2,
"c");
343 std::cout <<
"r * c = " << rc << std::endl;
345 std::cout <<
"------------------------" << std::endl;
346 std::cout <<
"--- TEST vpRowVector * vpMatrix" << std::endl;
347 std::cout <<
"------------------------" << std::endl;
356 r.
print(std::cout, 2,
"r");
357 M.print(std::cout, 10,
"M");
358 std::cout <<
"r * M = " << rM << std::endl;
360 std::cout <<
"------------------------" << std::endl;
361 std::cout <<
"--- TEST vpGEMM " << std::endl;
362 std::cout <<
"------------------------" << std::endl;
377 vpGEMM(M, N, 2, C, 3, D, VP_GEMM_A_T);
378 std::cout << D << std::endl;
382 std::cout <<
"------------------------" << std::endl;
383 std::cout <<
"--- TEST vpMatrix insert() with same colNum " << std::endl;
384 std::cout <<
"------------------------" << std::endl;
385 unsigned int nb = ctest ? 10 : 100;
386 const unsigned int size = ctest ? 10 : 100;
389 std::vector<vpMatrix> submatrices(nb);
390 for (
size_t cpt = 0; cpt < submatrices.size(); cpt++) {
393 for (
unsigned int i = 0; i < m.getRows(); i++) {
394 for (
unsigned int j = 0; j < m.getCols(); j++) {
395 m[i][j] = getRandomValues(-100.0, 100.0);
399 submatrices[cpt] = m;
403 for (
unsigned int i = 0; i < nb; i++) {
404 m_big.insert(submatrices[(
size_t)i], i * size, 0);
407 std::cout <<
"Matrix insert(): " << t <<
" ms" << std::endl;
409 for (
unsigned int cpt = 0; cpt < nb; cpt++) {
410 for (
unsigned int i = 0; i < size; i++) {
411 for (
unsigned int j = 0; j < 6; j++) {
412 if (!
vpMath::equal(m_big[cpt * size + i][j], submatrices[(
size_t)cpt][i][j],
413 std::numeric_limits<double>::epsilon())) {
414 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
426 std::cout <<
"Insert empty matrices:" << std::endl;
427 std::cout <<
"m1:\n" << m1 << std::endl;
428 std::cout <<
"m2:\n" << m2 << std::endl;
429 std::cout <<
"m3:\n" << m3 << std::endl;
431 std::cout <<
"\n------------------------" << std::endl;
432 std::cout <<
"--- TEST vpMatrix stack()" << std::endl;
433 std::cout <<
"------------------------" << std::endl;
439 std::cout <<
"L:\n" << L << std::endl;
443 std::cout <<
"L:\n" << L << std::endl;
449 for (
unsigned int i = 0; i < nb; i++) {
450 m_big_stack.
stack(submatrices[(
size_t)i]);
453 std::cout <<
"Matrix stack(): " << t <<
" ms" << std::endl;
455 if (!equalMatrix(m_big, m_big_stack)) {
456 std::cerr <<
"Problem with vpMatrix stack()!" << std::endl;
461 std::cout <<
"\n------------------------" << std::endl;
462 std::cout <<
"--- TEST vpMatrix stack(vpRowVector)" << std::endl;
463 std::cout <<
"------------------------" << std::endl;
465 vpMatrix m_big_stack = generateRandomMatrix(10000, ctest ? 10 : 100, -1000.0, 1000.0);
466 std::cout <<
"m_big_stack: " << m_big_stack.
getRows() <<
"x" << m_big_stack.
getCols() << std::endl;
470 for (
unsigned int i = 0; i < m_big_stack.
getRows(); i++) {
474 std::cout <<
"Matrix stack(vpRowVector): " << t <<
" ms" << std::endl;
476 if (!equalMatrix(m_big_stack, m_big_stack_row)) {
477 std::cerr <<
"Problem with vpMatrix stack(vpRowVector)!" << std::endl;
481 std::cout <<
"\n------------------------" << std::endl;
482 std::cout <<
"--- TEST vpMatrix stack(vpColVector)" << std::endl;
483 std::cout <<
"------------------------" << std::endl;
487 for (
unsigned int j = 0; j < m_big_stack.
getCols(); j++) {
491 std::cout <<
"Matrix stack(vpColVector): " << t <<
" ms" << std::endl;
493 if (!equalMatrix(m_big_stack, m_big_stack_col)) {
494 std::cerr <<
"Problem with vpMatrix stack(vpColVector)!" << std::endl;
498 std::cout <<
"\n------------------------" << std::endl;
499 std::cout <<
"--- TEST vpMatrix::stack()" << std::endl;
500 std::cout <<
"------------------------" << std::endl;
506 std::cout <<
"L:\n" << L << std::endl;
511 std::cout <<
"L:\n" << L << std::endl;
515 vpMatrix m_big_stack_static, m_big_stack_static_tmp;
517 for (
unsigned int i = 0; i < nb; i++) {
518 vpMatrix::stack(m_big_stack_static_tmp, submatrices[(
size_t)i], m_big_stack_static);
519 m_big_stack_static_tmp = m_big_stack_static;
522 std::cout <<
"Matrix::stack(): " << t <<
" ms" << std::endl;
524 if (!equalMatrix(m_big, m_big_stack_static)) {
525 std::cerr <<
"Problem with vpMatrix::stack()!" << std::endl;
530 std::cout <<
"\n------------------------" << std::endl;
531 std::cout <<
"--- TEST vpMatrix::stack(vpMatrix, vpRowVector, vpMatrix)" << std::endl;
532 std::cout <<
"------------------------" << std::endl;
534 vpMatrix m_big_stack_static = generateRandomMatrix(ctest ? 100 : 1000, ctest ? 10 : 100, -1000.0, 1000.0);
535 std::cout <<
"m_big_stack_static: " << m_big_stack_static.
getRows() <<
"x" << m_big_stack_static.
getCols() << std::endl;
537 vpMatrix m_big_stack_static_row, m_big_stack_static_row_tmp;
539 for (
unsigned int i = 0; i < m_big_stack_static.
getRows(); i++) {
541 m_big_stack_static_row_tmp = m_big_stack_static_row;
544 std::cout <<
"Matrix::stack(vpMatrix, vpRowVector, vpMatrix): " << t <<
" ms" << std::endl;
546 if (!equalMatrix(m_big_stack_static, m_big_stack_static_row)) {
547 std::cerr <<
"Problem with vpMatrix::stack(vpMatrix, vpRowVector, " 553 std::cout <<
"\n------------------------" << std::endl;
554 std::cout <<
"--- TEST vpMatrix::stack(vpMatrix, vpColVector, vpMatrix)" << std::endl;
555 std::cout <<
"------------------------" << std::endl;
557 vpMatrix m_big_stack_static_col, m_big_stack_static_col_tmp;
559 for (
unsigned int j = 0; j < m_big_stack_static.
getCols(); j++) {
561 m_big_stack_static_col_tmp = m_big_stack_static_col;
564 std::cout <<
"Matrix::stack(vpMatrix, vpColVector, vpMatrix): " << t <<
" ms" << std::endl;
566 if (!equalMatrix(m_big_stack_static, m_big_stack_static_col)) {
567 std::cerr <<
"Problem with vpMatrix::stack(vpMatrix, vpColVector, " 576 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
577 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
578 m2[i][j] = getRandomValues(-100.0, 100.0);
582 unsigned int offset_i = 4, offset_j = 3;
583 m1.insert(m2, offset_i, offset_j);
585 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
586 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
587 if (!
vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
588 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
596 m1.insert(m2, offset_i, offset_j);
598 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
599 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
600 if (!
vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
601 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
609 m1.insert(m2, offset_i, offset_j);
611 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
612 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
613 if (!
vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
614 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
622 std::cout <<
"\n------------------------" << std::endl;
623 std::cout <<
"--- TEST vpMatrix::juxtaposeMatrices()" << std::endl;
624 std::cout <<
"------------------------" << std::endl;
627 for (
unsigned int i = 0; i < A.
getRows(); i++) {
628 for (
unsigned int j = 0; j < A.
getCols(); j++) {
632 B[i][j] = (i * B.
getCols() + j) * 10;
639 std::cout <<
"juxtaposeM:\n" << juxtaposeM << std::endl;
642 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) 644 std::vector<vpMatrix> vec_mat;
645 vec_mat.emplace_back(5, 5);
651 std::cout <<
"\n1) A+B:\n" << res << std::endl;
655 std::cout <<
"\n2) A+B:\n" << res2 << std::endl;
660 std::cout <<
"\n------------------------" << std::endl;
661 std::cout <<
"--- TEST vpMatrix::hadamard()" << std::endl;
662 std::cout <<
"------------------------" << std::endl;
665 for (
unsigned int i = 0; i < M1.
size(); i++) {
670 std::cout <<
"M1:\n" << M1 << std::endl;
671 std::cout <<
"\nM2:\n" << M2 << std::endl;
673 std::cout <<
"\nRes:\n" << M2 << std::endl;
677 std::cout <<
"\n------------------------" << std::endl;
678 std::cout <<
"--- TEST vpMatrix::stackColums()" << std::endl;
679 std::cout <<
"------------------------" << std::endl;
681 for (
unsigned int j = 0; j < M.getCols(); j++) {
682 for (
unsigned int i = 0; i < M.getRows(); i++) {
683 M[i][j] = i + j*M.getRows();
686 std::cout <<
"M:\n" << M << std::endl;
688 std::cout <<
"Column stack: " << v.
t() << std::endl;
689 if (M.size() != v.
size()) {
690 std::cerr <<
"Problem in vpMatrix::stackColumns(): size differ" << std::endl;
693 for (
unsigned int i=0; i < v.
size(); i++) {
694 if (std::fabs(v[i]-static_cast<double>(i)) > std::numeric_limits<double>::epsilon()) {
695 std::cerr <<
"Problem in vpMatrix::stackColumns(): content differ" << std::endl;
702 std::cout <<
"\n------------------------" << std::endl;
703 std::cout <<
"--- TEST vpMatrix::stackRows()" << std::endl;
704 std::cout <<
"------------------------" << std::endl;
706 for (
unsigned int i = 0; i < M.getRows(); i++) {
707 for (
unsigned int j = 0; j < M.getCols(); j++) {
708 M[i][j] = i*M.getCols() + j;
711 std::cout <<
"M:\n" << M << std::endl;
713 std::cout <<
"Rows stack: " << v << std::endl;
714 if (M.size() != v.
size()) {
715 std::cerr <<
"Problem in vpMatrix::stackRows(): size differ" << std::endl;
718 for (
unsigned int i=0; i < v.
size(); i++) {
719 if (std::fabs(v[i]-static_cast<double>(i)) > std::numeric_limits<double>::epsilon()) {
720 std::cerr <<
"Problem in vpMatrix::stackRows(): content differ" << std::endl;
727 std::cout <<
"\n------------------------" << std::endl;
728 std::cout <<
"--- TEST vpMatrix::getCol()" << std::endl;
729 std::cout <<
"------------------------" << std::endl;
731 for(
unsigned int i=0; i < A.
getRows(); i++)
732 for(
unsigned int j=0; j < A.
getCols(); j++)
740 std::cerr <<
"Problem in vpMatrix::getCol(): values are different" << std::endl;
749 std::cerr <<
"Problem in vpMatrix::getCol(): values are different" << std::endl;
756 std::cout <<
"\n------------------------" << std::endl;
757 std::cout <<
"--- TEST vpMatrix::getRow()" << std::endl;
758 std::cout <<
"------------------------" << std::endl;
760 for(
unsigned int i=0; i < A.
getRows(); i++)
761 for(
unsigned int j=0; j < A.
getCols(); j++)
769 std::cerr <<
"Problem in vpMatrix::getRow(): values are different" << std::endl;
778 std::cerr <<
"Problem in vpMatrix::getRow(): values are different" << std::endl;
785 std::cout <<
"\n------------------------" << std::endl;
786 std::cout <<
"--- TEST vpMatrix::getDiag()" << std::endl;
787 std::cout <<
"------------------------" << std::endl;
789 for(
unsigned int i=0; i < A.
getRows(); i++)
790 for(
unsigned int j=0; j < A.
getCols(); j++)
795 ref << 0.0, 5.0, 10.0;
797 std::cerr <<
"Problem in vpMatrix::getDiag(): values are different" << std::endl;
802 std::cout <<
"\nAll tests succeeded" << std::endl;
805 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)
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
vpColVector getDiag() const
Implementation of row vector and the associated operations.
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.
static bool saveMatrix(const std::string &filename, const vpArray2D< double > &M, bool binary=false, const char *header="")
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)
vpRowVector getRow(unsigned int i) const
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
vpColVector getCol(unsigned int j) const
static double rad(double deg)
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)
Implementation of column vector and the associated operations.
static bool saveMatrixYAML(const std::string &filename, const vpArray2D< double > &M, const char *header="")
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
void resize(unsigned int i, bool flagNullify=true)
static bool loadMatrix(const std::string &filename, vpArray2D< double > &M, bool binary=false, char *header=NULL)
static bool loadMatrixYAML(const std::string &filename, vpArray2D< double > &M, char *header=NULL)