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)
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)