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> 59 bool test(
const std::string &s,
const vpMatrix &M,
const std::vector<double> &bench)
61 static unsigned int cpt = 0;
62 std::cout <<
"** Test " << ++cpt << std::endl;
63 std::cout << s <<
"(" << M.
getRows() <<
"," << M.
getCols() <<
") = \n" << M << std::endl;
64 if (bench.size() != M.
size()) {
65 std::cout <<
"Test fails: bad size wrt bench" << std::endl;
68 for (
unsigned int i = 0; i < M.
size(); i++) {
69 if (std::fabs(M.
data[i] - bench[i]) > std::fabs(M.
data[i]) * std::numeric_limits<double>::epsilon()) {
70 std::cout <<
"Test fails: bad content" << std::endl;
78 double getRandomValues(
double min,
double max)
80 return (max - min) * ((double)rand() / (double)RAND_MAX) + min;
83 bool equalMatrix(
const vpMatrix &A,
const vpMatrix &B,
double tol = std::numeric_limits<double>::epsilon())
89 for (
unsigned int i = 0; i < A.
getRows(); i++) {
90 for (
unsigned int j = 0; j < A.
getCols(); j++) {
100 vpMatrix generateRandomMatrix(
unsigned int rows,
unsigned int cols,
double min,
double max)
104 for (
unsigned int i = 0; i < M.
getRows(); i++) {
105 for (
unsigned int j = 0; j < M.
getCols(); j++) {
106 M[i][j] = getRandomValues(min, max);
113 std::vector<double> computeHadamard(
const std::vector<double>& v1,
const std::vector<double>& v2)
115 std::vector<double> result;
116 std::transform(v1.begin(), v1.end(), v2.begin(),
117 std::back_inserter(result), std::multiplies<double>());
122 int main(
int argc,
char *argv[])
126 for (
int i = 1; i < argc; i++) {
127 if (std::string(argv[i]) ==
"--benchmark") {
133 const double val = 10.0;
135 M.
resize(5, 5,
false,
false);
137 for (
unsigned int i = 0; i < M.
getRows(); i++) {
138 for (
unsigned int j = 0; j < M.
getCols(); j++) {
139 if (!
vpMath::equal(M[i][j], val, std::numeric_limits<double>::epsilon())) {
140 std::cerr <<
"Issue with matrix assignment with value." << std::endl;
144 if (!
vpMath::equal(M2[i][j], val, std::numeric_limits<double>::epsilon())) {
145 std::cerr <<
"Issue with matrix constructor initialized with value." << std::endl;
153 std::vector<double> bench(9, 0);
154 bench[2] = bench[4] = bench[6] = 1.;
156 M[2][0] = M[1][1] = M[0][2] = 1.;
158 if (test(
"R1", R1, bench) ==
false)
162 if (test(
"R2", R2, bench) ==
false)
168 std::vector<double> bench(6, 1);
170 if (test(
"M1", M1, bench) ==
false)
173 if (test(
"M2", M2, bench) ==
false)
179 for (
unsigned int i = 0; i < M.
getRows(); i++) {
180 for (
unsigned int j = 0; j < M.
getCols(); j++) {
185 M.
print(std::cout, 4);
188 N.
init(M, 0, 1, 2, 3);
190 N.
print(std::cout, 4);
191 std::string header(
"My 4-by-5 matrix\nwith a second line");
195 std::cout <<
"Matrix saved in matrix.mat file" << std::endl;
203 std::cout <<
"Matrix loaded from matrix.mat file with header \"" << header_ <<
"\": \n" << M1 << std::endl;
206 if (header != std::string(header_)) {
207 std::cout <<
"Bad header in matrix.mat" << std::endl;
213 std::cout <<
"Matrix saved in matrix.bin file" << std::endl;
219 std::cout <<
"Matrix loaded from matrix.bin file with header \"" << header_ <<
"\": \n" << M1 << std::endl;
222 if (header != std::string(header_)) {
223 std::cout <<
"Bad header in matrix.bin" << std::endl;
229 std::cout <<
"Matrix saved in matrix.yml file" << std::endl;
236 std::cout <<
"Matrix loaded from matrix.yml file with header \"" << header_ <<
"\": \n" << M2 << std::endl;
239 if (header != std::string(header_)) {
240 std::cout <<
"Bad header in matrix.mat" << std::endl;
247 std::cout <<
"R: \n" << R << std::endl;
249 std::cout <<
"M1: \n" << M1 << std::endl;
251 std::cout <<
"M2: \n" << M2 << std::endl;
253 std::cout <<
"M3: \n" << M3 << std::endl;
255 std::cout <<
"M4: \n" << M4 << std::endl;
259 std::cout <<
"------------------------" << std::endl;
260 std::cout <<
"--- TEST PRETTY PRINT---" << std::endl;
261 std::cout <<
"------------------------" << std::endl;
265 std::cout <<
"call std::cout << M;" << std::endl;
266 std::cout << M << std::endl;
268 std::cout <<
"call M.print (std::cout, 4);" << std::endl;
269 M.
print(std::cout, 4);
271 std::cout <<
"------------------------" << std::endl;
277 std::cout <<
"call std::cout << M;" << std::endl;
279 std::cout <<
"call M.print (std::cout, 6);" << std::endl;
280 M.
print(std::cout, 6);
281 std::cout << std::endl;
283 std::cout <<
"------------------------" << std::endl;
287 std::cout <<
"call std::cout << M;" << std::endl;
288 std::cout << M << std::endl;
290 std::cout <<
"call M.print (std::cout, 10);" << std::endl;
291 M.print(std::cout, 10);
292 std::cout << std::endl;
294 std::cout <<
"call M.print (std::cout, 2);" << std::endl;
295 M.print(std::cout, 2);
296 std::cout << std::endl;
298 std::cout <<
"------------------------" << std::endl;
301 M[0][2] = -0.0000000876;
302 std::cout <<
"call std::cout << M;" << std::endl;
303 std::cout << M << std::endl;
305 std::cout <<
"call M.print (std::cout, 4);" << std::endl;
306 M.print(std::cout, 4);
307 std::cout << std::endl;
308 std::cout <<
"call M.print (std::cout, 10, \"M\");" << std::endl;
309 M.print(std::cout, 10,
"M");
310 std::cout << std::endl;
311 std::cout <<
"call M.print (std::cout, 20, \"M\");" << std::endl;
312 M.print(std::cout, 20,
"M");
313 std::cout << std::endl;
315 std::cout <<
"------------------------" << std::endl;
316 std::cout <<
"--- TEST RESIZE --------" << std::endl;
317 std::cout <<
"------------------------" << std::endl;
318 std::cout <<
"5x5" << std::endl;
319 M.resize(5, 5,
false);
320 std::cout << M << std::endl;
321 std::cout <<
"3x2" << std::endl;
322 M.resize(3, 2,
false);
323 std::cout << M << std::endl;
324 std::cout <<
"2x2" << std::endl;
325 M.resize(2, 2,
false);
326 std::cout << M << std::endl;
327 std::cout <<
"------------------------" << std::endl;
336 std::cout <<
"------------------------" << std::endl;
337 std::cout <<
"--- TEST vpRowVector * vpColVector" << std::endl;
338 std::cout <<
"------------------------" << std::endl;
351 r.print(std::cout, 2,
"r");
352 c.print(std::cout, 2,
"c");
353 std::cout <<
"r * c = " << rc << std::endl;
355 std::cout <<
"------------------------" << std::endl;
356 std::cout <<
"--- TEST vpRowVector * vpMatrix" << std::endl;
357 std::cout <<
"------------------------" << std::endl;
366 r.
print(std::cout, 2,
"r");
367 M.print(std::cout, 10,
"M");
368 std::cout <<
"r * M = " << rM << std::endl;
370 std::cout <<
"------------------------" << std::endl;
371 std::cout <<
"--- TEST vpGEMM " << std::endl;
372 std::cout <<
"------------------------" << std::endl;
387 vpGEMM(M, N, 2, C, 3, D, VP_GEMM_A_T);
388 std::cout << D << std::endl;
392 std::cout <<
"------------------------" << std::endl;
393 std::cout <<
"--- TEST vpMatrix insert() with same colNum " << std::endl;
394 std::cout <<
"------------------------" << std::endl;
395 unsigned int nb = ctest ? 10 : 100;
396 const unsigned int size = ctest ? 10 : 100;
399 std::vector<vpMatrix> submatrices(nb);
400 for (
size_t cpt = 0; cpt < submatrices.size(); cpt++) {
403 for (
unsigned int i = 0; i < m.getRows(); i++) {
404 for (
unsigned int j = 0; j < m.getCols(); j++) {
405 m[i][j] = getRandomValues(-100.0, 100.0);
409 submatrices[cpt] = m;
413 for (
unsigned int i = 0; i < nb; i++) {
414 m_big.insert(submatrices[(
size_t)i], i * size, 0);
417 std::cout <<
"Matrix insert(): " << t <<
" ms" << std::endl;
419 for (
unsigned int cpt = 0; cpt < nb; cpt++) {
420 for (
unsigned int i = 0; i < size; i++) {
421 for (
unsigned int j = 0; j < 6; j++) {
422 if (!
vpMath::equal(m_big[cpt * size + i][j], submatrices[(
size_t)cpt][i][j],
423 std::numeric_limits<double>::epsilon())) {
424 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
436 std::cout <<
"Insert empty matrices:" << std::endl;
437 std::cout <<
"m1:\n" << m1 << std::endl;
438 std::cout <<
"m2:\n" << m2 << std::endl;
439 std::cout <<
"m3:\n" << m3 << std::endl;
441 std::cout <<
"\n------------------------" << std::endl;
442 std::cout <<
"--- TEST vpMatrix stack()" << std::endl;
443 std::cout <<
"------------------------" << std::endl;
449 std::cout <<
"L:\n" << L << std::endl;
453 std::cout <<
"L:\n" << L << std::endl;
459 for (
unsigned int i = 0; i < nb; i++) {
460 m_big_stack.
stack(submatrices[(
size_t)i]);
463 std::cout <<
"Matrix stack(): " << t <<
" ms" << std::endl;
465 if (!equalMatrix(m_big, m_big_stack)) {
466 std::cerr <<
"Problem with vpMatrix stack()!" << std::endl;
471 std::cout <<
"\n------------------------" << std::endl;
472 std::cout <<
"--- TEST vpMatrix stack(vpRowVector)" << std::endl;
473 std::cout <<
"------------------------" << std::endl;
475 vpMatrix m_big_stack = generateRandomMatrix(10000, ctest ? 10 : 100, -1000.0, 1000.0);
476 std::cout <<
"m_big_stack: " << m_big_stack.
getRows() <<
"x" << m_big_stack.
getCols() << std::endl;
480 for (
unsigned int i = 0; i < m_big_stack.
getRows(); i++) {
484 std::cout <<
"Matrix stack(vpRowVector): " << t <<
" ms" << std::endl;
486 if (!equalMatrix(m_big_stack, m_big_stack_row)) {
487 std::cerr <<
"Problem with vpMatrix stack(vpRowVector)!" << std::endl;
491 std::cout <<
"\n------------------------" << std::endl;
492 std::cout <<
"--- TEST vpMatrix stack(vpColVector)" << std::endl;
493 std::cout <<
"------------------------" << std::endl;
497 for (
unsigned int j = 0; j < m_big_stack.
getCols(); j++) {
501 std::cout <<
"Matrix stack(vpColVector): " << t <<
" ms" << std::endl;
503 if (!equalMatrix(m_big_stack, m_big_stack_col)) {
504 std::cerr <<
"Problem with vpMatrix stack(vpColVector)!" << std::endl;
508 std::cout <<
"\n------------------------" << std::endl;
509 std::cout <<
"--- TEST vpMatrix::stack()" << std::endl;
510 std::cout <<
"------------------------" << std::endl;
516 std::cout <<
"L:\n" << L << std::endl;
521 std::cout <<
"L:\n" << L << std::endl;
525 vpMatrix m_big_stack_static, m_big_stack_static_tmp;
527 for (
unsigned int i = 0; i < nb; i++) {
528 vpMatrix::stack(m_big_stack_static_tmp, submatrices[(
size_t)i], m_big_stack_static);
529 m_big_stack_static_tmp = m_big_stack_static;
532 std::cout <<
"Matrix::stack(): " << t <<
" ms" << std::endl;
534 if (!equalMatrix(m_big, m_big_stack_static)) {
535 std::cerr <<
"Problem with vpMatrix::stack()!" << std::endl;
540 std::cout <<
"\n------------------------" << std::endl;
541 std::cout <<
"--- TEST vpMatrix::stack(vpMatrix, vpRowVector, vpMatrix)" << std::endl;
542 std::cout <<
"------------------------" << std::endl;
544 vpMatrix m_big_stack_static = generateRandomMatrix(ctest ? 100 : 1000, ctest ? 10 : 100, -1000.0, 1000.0);
545 std::cout <<
"m_big_stack_static: " << m_big_stack_static.
getRows() <<
"x" << m_big_stack_static.
getCols() << std::endl;
547 vpMatrix m_big_stack_static_row, m_big_stack_static_row_tmp;
549 for (
unsigned int i = 0; i < m_big_stack_static.
getRows(); i++) {
551 m_big_stack_static_row_tmp = m_big_stack_static_row;
554 std::cout <<
"Matrix::stack(vpMatrix, vpRowVector, vpMatrix): " << t <<
" ms" << std::endl;
556 if (!equalMatrix(m_big_stack_static, m_big_stack_static_row)) {
557 std::cerr <<
"Problem with vpMatrix::stack(vpMatrix, vpRowVector, " 563 std::cout <<
"\n------------------------" << std::endl;
564 std::cout <<
"--- TEST vpMatrix::stack(vpMatrix, vpColVector, vpMatrix)" << std::endl;
565 std::cout <<
"------------------------" << std::endl;
567 vpMatrix m_big_stack_static_col, m_big_stack_static_col_tmp;
569 for (
unsigned int j = 0; j < m_big_stack_static.
getCols(); j++) {
571 m_big_stack_static_col_tmp = m_big_stack_static_col;
574 std::cout <<
"Matrix::stack(vpMatrix, vpColVector, vpMatrix): " << t <<
" ms" << std::endl;
576 if (!equalMatrix(m_big_stack_static, m_big_stack_static_col)) {
577 std::cerr <<
"Problem with vpMatrix::stack(vpMatrix, vpColVector, " 586 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
587 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
588 m2[i][j] = getRandomValues(-100.0, 100.0);
592 unsigned int offset_i = 4, offset_j = 3;
593 m1.insert(m2, offset_i, offset_j);
595 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
596 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
597 if (!
vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
598 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
606 m1.insert(m2, offset_i, offset_j);
608 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
609 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
610 if (!
vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
611 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
619 m1.insert(m2, offset_i, offset_j);
621 for (
unsigned int i = 0; i < m2.
getRows(); i++) {
622 for (
unsigned int j = 0; j < m2.
getCols(); j++) {
623 if (!
vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
624 std::cerr <<
"Problem with vpMatrix insert()!" << std::endl;
632 std::cout <<
"\n------------------------" << std::endl;
633 std::cout <<
"--- TEST vpMatrix::juxtaposeMatrices()" << std::endl;
634 std::cout <<
"------------------------" << std::endl;
637 for (
unsigned int i = 0; i < A.
getRows(); i++) {
638 for (
unsigned int j = 0; j < A.
getCols(); j++) {
642 B[i][j] = (i * B.
getCols() + j) * 10;
649 std::cout <<
"juxtaposeM:\n" << juxtaposeM << std::endl;
652 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) 654 std::vector<vpMatrix> vec_mat;
655 vec_mat.emplace_back(5, 5);
661 std::cout <<
"\n1) A+B:\n" << res << std::endl;
665 std::cout <<
"\n2) A+B:\n" << res2 << std::endl;
670 std::cout <<
"\n------------------------" << std::endl;
671 std::cout <<
"--- TEST vpMatrix::hadamard()" << std::endl;
672 std::cout <<
"------------------------" << std::endl;
675 for (
unsigned int i = 0; i < M1.
size(); i++) {
681 std::vector<double> references = computeHadamard(std::vector<double>(M1.
data, M1.
data + M1.
size()),
684 std::cout <<
"M1:\n" << M1 << std::endl;
685 std::cout <<
"\nM2:\n" << M2 << std::endl;
687 std::cout <<
"\nRes:\n" << M2 << std::endl;
689 if (!test(
"M2", M2, references)) {
690 std::cerr <<
"Error with Hadamard product" << std::endl;
696 std::cout <<
"\n------------------------" << std::endl;
697 std::cout <<
"--- TEST vpMatrix::stackColums()" << std::endl;
698 std::cout <<
"------------------------" << std::endl;
700 for (
unsigned int j = 0; j < M.getCols(); j++) {
701 for (
unsigned int i = 0; i < M.getRows(); i++) {
702 M[i][j] = i + j*M.getRows();
705 std::cout <<
"M:\n" << M << std::endl;
707 std::cout <<
"Column stack: " << v.
t() << std::endl;
708 if (M.size() != v.
size()) {
709 std::cerr <<
"Problem in vpMatrix::stackColumns(): size differ" << std::endl;
712 for (
unsigned int i=0; i < v.
size(); i++) {
713 if (std::fabs(v[i]-static_cast<double>(i)) > std::numeric_limits<double>::epsilon()) {
714 std::cerr <<
"Problem in vpMatrix::stackColumns(): content differ" << std::endl;
721 std::cout <<
"\n------------------------" << std::endl;
722 std::cout <<
"--- TEST vpMatrix::stackRows()" << std::endl;
723 std::cout <<
"------------------------" << std::endl;
725 for (
unsigned int i = 0; i < M.getRows(); i++) {
726 for (
unsigned int j = 0; j < M.getCols(); j++) {
727 M[i][j] = i*M.getCols() + j;
730 std::cout <<
"M:\n" << M << std::endl;
732 std::cout <<
"Rows stack: " << v << std::endl;
733 if (M.size() != v.
size()) {
734 std::cerr <<
"Problem in vpMatrix::stackRows(): size differ" << std::endl;
737 for (
unsigned int i=0; i < v.
size(); i++) {
738 if (std::fabs(v[i]-static_cast<double>(i)) > std::numeric_limits<double>::epsilon()) {
739 std::cerr <<
"Problem in vpMatrix::stackRows(): content differ" << std::endl;
746 std::cout <<
"\n------------------------" << std::endl;
747 std::cout <<
"--- TEST vpMatrix::getCol()" << std::endl;
748 std::cout <<
"------------------------" << std::endl;
750 for(
unsigned int i=0; i < A.
getRows(); i++)
751 for(
unsigned int j=0; j < A.
getCols(); j++)
759 std::cerr <<
"Problem in vpMatrix::getCol(): values are different" << std::endl;
768 std::cerr <<
"Problem in vpMatrix::getCol(): values are different" << std::endl;
775 std::cout <<
"\n------------------------" << std::endl;
776 std::cout <<
"--- TEST vpMatrix::getRow()" << std::endl;
777 std::cout <<
"------------------------" << std::endl;
779 for(
unsigned int i=0; i < A.
getRows(); i++)
780 for(
unsigned int j=0; j < A.
getCols(); j++)
788 std::cerr <<
"Problem in vpMatrix::getRow(): values are different" << std::endl;
797 std::cerr <<
"Problem in vpMatrix::getRow(): values are different" << std::endl;
804 std::cout <<
"\n------------------------" << std::endl;
805 std::cout <<
"--- TEST vpMatrix::getDiag()" << std::endl;
806 std::cout <<
"------------------------" << std::endl;
808 for(
unsigned int i=0; i < A.
getRows(); i++)
809 for(
unsigned int j=0; j < A.
getCols(); j++)
814 ref << 0.0, 5.0, 10.0;
816 std::cerr <<
"Problem in vpMatrix::getDiag(): values are different" << std::endl;
821 std::cout <<
"\nAll tests succeeded" << std::endl;
824 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)
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.
Type * data
Address of the first element of the data array.
unsigned int size() const
Return the number of elements of the 2D array.
unsigned int getCols() const
vpColVector getDiag() const
static bool saveMatrix(const std::string &filename, const vpArray2D< double > &M, bool binary=false, const char *header="")
vpRowVector getRow(unsigned int i) const
VISP_EXPORT double measureTimeMs()
Implementation of a rotation matrix and operations on such kind of matrices.
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)
unsigned int getRows() 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.
int print(std::ostream &s, unsigned int length, char const *intro=0) const
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)
vpColVector getCol(unsigned int j) const
static bool loadMatrixYAML(const std::string &filename, vpArray2D< double > &M, char *header=NULL)
vpMatrix hadamard(const vpMatrix &m) const