Visual Servoing Platform version 3.5.0
testMatrix.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Test some vpMatrix functionalities.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
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>
51
52#include <stdio.h>
53#include <stdlib.h>
54
55#include <iterator> // for std::back_inserter
56
57namespace
58{
59bool test(const std::string &s, const vpMatrix &M, const std::vector<double> &bench)
60{
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;
66 return false;
67 }
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;
71 return false;
72 }
73 }
74
75 return true;
76}
77
78double getRandomValues(double min, double max)
79{
80 return (max - min) * ((double)rand() / (double)RAND_MAX) + min;
81}
82
83bool equalMatrix(const vpMatrix &A, const vpMatrix &B, double tol = std::numeric_limits<double>::epsilon())
84{
85 if (A.getRows() != B.getRows() || A.getCols() != B.getCols()) {
86 return false;
87 }
88
89 for (unsigned int i = 0; i < A.getRows(); i++) {
90 for (unsigned int j = 0; j < A.getCols(); j++) {
91 if (!vpMath::equal(A[i][j], B[i][j], tol)) {
92 return false;
93 }
94 }
95 }
96
97 return true;
98}
99
100vpMatrix generateRandomMatrix(unsigned int rows, unsigned int cols, double min, double max)
101{
102 vpMatrix M(rows, cols);
103
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);
107 }
108 }
109
110 return M;
111}
112
113std::vector<double> computeHadamard(const std::vector<double>& v1, const std::vector<double>& v2)
114{
115 std::vector<double> result;
116 std::transform(v1.begin(), v1.end(), v2.begin(),
117 std::back_inserter(result), std::multiplies<double>());
118 return result;
119}
120} // namespace
121
122int main(int argc, char *argv[])
123{
124 try {
125 bool ctest = true;
126 for (int i = 1; i < argc; i++) {
127 if (std::string(argv[i]) == "--benchmark") {
128 ctest = false;
129 }
130 }
131
132 {
133 const double val = 10.0;
134 vpMatrix M, M2(5, 5, val);
135 M.resize(5, 5, false, false);
136 M = val;
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;
141 return EXIT_FAILURE;
142 }
143
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;
146 return EXIT_FAILURE;
147 }
148 }
149 }
150 }
151 {
152 // Test vpRotationMatrix construction
153 std::vector<double> bench(9, 0);
154 bench[2] = bench[4] = bench[6] = 1.;
155 vpMatrix M(3, 3);
156 M[2][0] = M[1][1] = M[0][2] = 1.;
157 vpRotationMatrix R1(M);
158 if (test("R1", R1, bench) == false)
159 return EXIT_FAILURE;
161 R2 = M;
162 if (test("R2", R2, bench) == false)
163 return EXIT_FAILURE;
164 }
165 {
166 vpColVector c(6, 1);
167 vpRowVector r(6, 1);
168 std::vector<double> bench(6, 1);
169 vpMatrix M1(c);
170 if (test("M1", M1, bench) == false)
171 return EXIT_FAILURE;
172 vpMatrix M2(r);
173 if (test("M2", M2, bench) == false)
174 return EXIT_FAILURE;
175 }
176 {
177 vpMatrix M(4, 5);
178 int val = 0;
179 for (unsigned int i = 0; i < M.getRows(); i++) {
180 for (unsigned int j = 0; j < M.getCols(); j++) {
181 M[i][j] = val++;
182 }
183 }
184 std::cout << "M ";
185 M.print(std::cout, 4);
186
187 vpMatrix N;
188 N.init(M, 0, 1, 2, 3);
189 std::cout << "N ";
190 N.print(std::cout, 4);
191 std::string header("My 4-by-5 matrix\nwith a second line");
192
193 // Save matrix in text format
194 if (vpMatrix::saveMatrix("matrix.mat", M, false, header.c_str()))
195 std::cout << "Matrix saved in matrix.mat file" << std::endl;
196 else
197 return EXIT_FAILURE;
198
199 // Load matrix in text format
200 vpMatrix M1;
201 char header_[100];
202 if (vpMatrix::loadMatrix("matrix.mat", M1, false, header_))
203 std::cout << "Matrix loaded from matrix.mat file with header \"" << header_ << "\": \n" << M1 << std::endl;
204 else
205 return EXIT_FAILURE;
206 if (header != std::string(header_)) {
207 std::cout << "Bad header in matrix.mat" << std::endl;
208 return EXIT_FAILURE;
209 }
210
211 // Save matrix in binary format
212 if (vpMatrix::saveMatrix("matrix.bin", M, true, header.c_str()))
213 std::cout << "Matrix saved in matrix.bin file" << std::endl;
214 else
215 return EXIT_FAILURE;
216
217 // Load matrix in binary format
218 if (vpMatrix::loadMatrix("matrix.bin", M1, true, header_))
219 std::cout << "Matrix loaded from matrix.bin file with header \"" << header_ << "\": \n" << M1 << std::endl;
220 else
221 return EXIT_FAILURE;
222 if (header != std::string(header_)) {
223 std::cout << "Bad header in matrix.bin" << std::endl;
224 return EXIT_FAILURE;
225 }
226
227 // Save matrix in YAML format
228 if (vpMatrix::saveMatrixYAML("matrix.yml", M, header.c_str()))
229 std::cout << "Matrix saved in matrix.yml file" << std::endl;
230 else
231 return EXIT_FAILURE;
232
233 // Read matrix in YAML format
234 vpMatrix M2;
235 if (vpMatrix::loadMatrixYAML("matrix.yml", M2, header_))
236 std::cout << "Matrix loaded from matrix.yml file with header \"" << header_ << "\": \n" << M2 << std::endl;
237 else
238 return EXIT_FAILURE;
239 if (header != std::string(header_)) {
240 std::cout << "Bad header in matrix.mat" << std::endl;
241 return EXIT_FAILURE;
242 }
243 }
244
245 {
247 std::cout << "R: \n" << R << std::endl;
248 vpMatrix M1(R);
249 std::cout << "M1: \n" << M1 << std::endl;
250 vpMatrix M2(M1);
251 std::cout << "M2: \n" << M2 << std::endl;
252 vpMatrix M3 = R;
253 std::cout << "M3: \n" << M3 << std::endl;
254 vpMatrix M4 = M1;
255 std::cout << "M4: \n" << M4 << std::endl;
256 }
257 {
258
259 std::cout << "------------------------" << std::endl;
260 std::cout << "--- TEST PRETTY PRINT---" << std::endl;
261 std::cout << "------------------------" << std::endl;
262 vpMatrix M;
263 M.eye(4);
264
265 std::cout << "call std::cout << M;" << std::endl;
266 std::cout << M << std::endl;
267
268 std::cout << "call M.print (std::cout, 4);" << std::endl;
269 M.print(std::cout, 4);
270
271 std::cout << "------------------------" << std::endl;
272 M.resize(3, 3);
273 M.eye(3);
274 M[1][0] = 1.235;
275 M[1][1] = 12.345;
276 M[1][2] = .12345;
277 std::cout << "call std::cout << M;" << std::endl;
278 std::cout << M;
279 std::cout << "call M.print (std::cout, 6);" << std::endl;
280 M.print(std::cout, 6);
281 std::cout << std::endl;
282
283 std::cout << "------------------------" << std::endl;
284 M[0][0] = -1.235;
285 M[1][0] = -12.235;
286
287 std::cout << "call std::cout << M;" << std::endl;
288 std::cout << M << std::endl;
289
290 std::cout << "call M.print (std::cout, 10);" << std::endl;
291 M.print(std::cout, 10);
292 std::cout << std::endl;
293
294 std::cout << "call M.print (std::cout, 2);" << std::endl;
295 M.print(std::cout, 2);
296 std::cout << std::endl;
297
298 std::cout << "------------------------" << std::endl;
299 M.resize(3, 3);
300 M.eye(3);
301 M[0][2] = -0.0000000876;
302 std::cout << "call std::cout << M;" << std::endl;
303 std::cout << M << std::endl;
304
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;
314
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;
328
330 vpMatrix A(1, 6), B;
331
332 A = 1.0;
333 // vMe=1.0;
334 B = A * vMe;
335
336 std::cout << "------------------------" << std::endl;
337 std::cout << "--- TEST vpRowVector * vpColVector" << std::endl;
338 std::cout << "------------------------" << std::endl;
339 vpRowVector r(3);
340 r[0] = 2;
341 r[1] = 3;
342 r[2] = 4;
343
344 vpColVector c(3);
345 c[0] = 1;
346 c[1] = 2;
347 c[2] = -1;
348
349 double rc = r * c;
350
351 r.print(std::cout, 2, "r");
352 c.print(std::cout, 2, "c");
353 std::cout << "r * c = " << rc << std::endl;
354
355 std::cout << "------------------------" << std::endl;
356 std::cout << "--- TEST vpRowVector * vpMatrix" << std::endl;
357 std::cout << "------------------------" << std::endl;
358 M.resize(3, 3);
359 M.eye(3);
360
361 M[1][0] = 1.5;
362 M[2][0] = 2.3;
363
364 vpRowVector rM = r * M;
365
366 r.print(std::cout, 2, "r");
367 M.print(std::cout, 10, "M");
368 std::cout << "r * M = " << rM << std::endl;
369
370 std::cout << "------------------------" << std::endl;
371 std::cout << "--- TEST vpGEMM " << std::endl;
372 std::cout << "------------------------" << std::endl;
373 M.resize(3, 3);
374 M.eye(3);
375 vpMatrix N(3, 3);
376 N[0][0] = 2;
377 N[1][0] = 1.2;
378 N[1][2] = 0.6;
379 N[2][2] = 0.25;
380
381 vpMatrix C(3, 3);
382 C.eye(3);
383
384 vpMatrix D;
385
386 // realise the operation D = 2 * M^T * N + 3 C
387 vpGEMM(M, N, 2, C, 3, D, VP_GEMM_A_T);
388 std::cout << D << std::endl;
389 }
390
391 {
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; // 10000;
396 const unsigned int size = ctest ? 10 : 100;
397
398 vpMatrix m_big(nb * size, 6);
399 std::vector<vpMatrix> submatrices(nb);
400 for (size_t cpt = 0; cpt < submatrices.size(); cpt++) {
401 vpMatrix m(size, 6);
402
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);
406 }
407 }
408
409 submatrices[cpt] = m;
410 }
411
412 double t = vpTime::measureTimeMs();
413 for (unsigned int i = 0; i < nb; i++) {
414 m_big.insert(submatrices[(size_t)i], i * size, 0);
415 }
416 t = vpTime::measureTimeMs() - t;
417 std::cout << "Matrix insert(): " << t << " ms" << std::endl;
418
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;
425 return EXIT_FAILURE;
426 }
427 }
428 }
429 }
430
431 // Try to insert empty matrices
432 vpMatrix m1(2, 3), m2, m3;
433 m1.insert(m2, 0, 0);
434 m3.insert(m2, 0, 0);
435
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;
440
441 std::cout << "\n------------------------" << std::endl;
442 std::cout << "--- TEST vpMatrix stack()" << std::endl;
443 std::cout << "------------------------" << std::endl;
444
445 {
446 vpMatrix L, L2(2, 6);
447 L2 = 2;
448 L.stack(L2);
449 std::cout << "L:\n" << L << std::endl;
450 L2.resize(3, 6);
451 L2 = 3;
452 L.stack(L2);
453 std::cout << "L:\n" << L << std::endl;
454 }
455
456 {
457 vpMatrix m_big_stack;
459 for (unsigned int i = 0; i < nb; i++) {
460 m_big_stack.stack(submatrices[(size_t)i]);
461 }
462 t = vpTime::measureTimeMs() - t;
463 std::cout << "Matrix stack(): " << t << " ms" << std::endl;
464
465 if (!equalMatrix(m_big, m_big_stack)) {
466 std::cerr << "Problem with vpMatrix stack()!" << std::endl;
467 return EXIT_FAILURE;
468 }
469 }
470
471 std::cout << "\n------------------------" << std::endl;
472 std::cout << "--- TEST vpMatrix stack(vpRowVector)" << std::endl;
473 std::cout << "------------------------" << std::endl;
474
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;
477
478 vpMatrix m_big_stack_row;
480 for (unsigned int i = 0; i < m_big_stack.getRows(); i++) {
481 m_big_stack_row.stack(m_big_stack.getRow(i));
482 }
483 t = vpTime::measureTimeMs() - t;
484 std::cout << "Matrix stack(vpRowVector): " << t << " ms" << std::endl;
485
486 if (!equalMatrix(m_big_stack, m_big_stack_row)) {
487 std::cerr << "Problem with vpMatrix stack(vpRowVector)!" << std::endl;
488 return EXIT_FAILURE;
489 }
490
491 std::cout << "\n------------------------" << std::endl;
492 std::cout << "--- TEST vpMatrix stack(vpColVector)" << std::endl;
493 std::cout << "------------------------" << std::endl;
494
495 vpMatrix m_big_stack_col;
497 for (unsigned int j = 0; j < m_big_stack.getCols(); j++) {
498 m_big_stack_col.stack(m_big_stack.getCol(j));
499 }
500 t = vpTime::measureTimeMs() - t;
501 std::cout << "Matrix stack(vpColVector): " << t << " ms" << std::endl;
502
503 if (!equalMatrix(m_big_stack, m_big_stack_col)) {
504 std::cerr << "Problem with vpMatrix stack(vpColVector)!" << std::endl;
505 return EXIT_FAILURE;
506 }
507
508 std::cout << "\n------------------------" << std::endl;
509 std::cout << "--- TEST vpMatrix::stack()" << std::endl;
510 std::cout << "------------------------" << std::endl;
511
512 {
513 vpMatrix L, L2(2, 6), L_tmp;
514 L2 = 2;
515 vpMatrix::stack(L_tmp, L2, L);
516 std::cout << "L:\n" << L << std::endl;
517 L2.resize(3, 6);
518 L2 = 3;
519 L_tmp = L;
520 vpMatrix::stack(L_tmp, L2, L);
521 std::cout << "L:\n" << L << std::endl;
522 }
523
524 {
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;
530 }
531 t = vpTime::measureTimeMs() - t;
532 std::cout << "Matrix::stack(): " << t << " ms" << std::endl;
533
534 if (!equalMatrix(m_big, m_big_stack_static)) {
535 std::cerr << "Problem with vpMatrix::stack()!" << std::endl;
536 return EXIT_FAILURE;
537 }
538 }
539
540 std::cout << "\n------------------------" << std::endl;
541 std::cout << "--- TEST vpMatrix::stack(vpMatrix, vpRowVector, vpMatrix)" << std::endl;
542 std::cout << "------------------------" << std::endl;
543
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;
546
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++) {
550 vpMatrix::stack(m_big_stack_static_row_tmp, m_big_stack_static.getRow(i), m_big_stack_static_row);
551 m_big_stack_static_row_tmp = m_big_stack_static_row;
552 }
553 t = vpTime::measureTimeMs() - t;
554 std::cout << "Matrix::stack(vpMatrix, vpRowVector, vpMatrix): " << t << " ms" << std::endl;
555
556 if (!equalMatrix(m_big_stack_static, m_big_stack_static_row)) {
557 std::cerr << "Problem with vpMatrix::stack(vpMatrix, vpRowVector, "
558 "vpMatrix)!"
559 << std::endl;
560 return EXIT_FAILURE;
561 }
562
563 std::cout << "\n------------------------" << std::endl;
564 std::cout << "--- TEST vpMatrix::stack(vpMatrix, vpColVector, vpMatrix)" << std::endl;
565 std::cout << "------------------------" << std::endl;
566
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++) {
570 vpMatrix::stack(m_big_stack_static_col_tmp, m_big_stack_static.getCol(j), m_big_stack_static_col);
571 m_big_stack_static_col_tmp = m_big_stack_static_col;
572 }
573 t = vpTime::measureTimeMs() - t;
574 std::cout << "Matrix::stack(vpMatrix, vpColVector, vpMatrix): " << t << " ms" << std::endl;
575
576 if (!equalMatrix(m_big_stack_static, m_big_stack_static_col)) {
577 std::cerr << "Problem with vpMatrix::stack(vpMatrix, vpColVector, "
578 "vpMatrix)!"
579 << std::endl;
580 return EXIT_FAILURE;
581 }
582 }
583
584 {
585 vpMatrix m1(11, 9), m2(3, 4);
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);
589 }
590 }
591
592 unsigned int offset_i = 4, offset_j = 3;
593 m1.insert(m2, offset_i, offset_j);
594
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;
599 return EXIT_FAILURE;
600 }
601 }
602 }
603
604 offset_i = 4;
605 offset_j = 5;
606 m1.insert(m2, offset_i, offset_j);
607
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;
612 return EXIT_FAILURE;
613 }
614 }
615 }
616
617 offset_i = 8;
618 offset_j = 5;
619 m1.insert(m2, offset_i, offset_j);
620
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;
625 return EXIT_FAILURE;
626 }
627 }
628 }
629 }
630
631 {
632 std::cout << "\n------------------------" << std::endl;
633 std::cout << "--- TEST vpMatrix::juxtaposeMatrices()" << std::endl;
634 std::cout << "------------------------" << std::endl;
635
636 vpMatrix A(5, 6), B(5, 4);
637 for (unsigned int i = 0; i < A.getRows(); i++) {
638 for (unsigned int j = 0; j < A.getCols(); j++) {
639 A[i][j] = i * A.getCols() + j;
640
641 if (j < B.getCols()) {
642 B[i][j] = (i * B.getCols() + j) * 10;
643 }
644 }
645 }
646
647 vpMatrix juxtaposeM;
648 vpMatrix::juxtaposeMatrices(A, B, juxtaposeM);
649 std::cout << "juxtaposeM:\n" << juxtaposeM << std::endl;
650 }
651
652#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
653 {
654 std::vector<vpMatrix> vec_mat;
655 vec_mat.emplace_back(5, 5);
656
657 vpMatrix A(4, 4), B(4, 4);
658 A = 1;
659 B = 2;
660 vpMatrix res = A + B;
661 std::cout << "\n1) A+B:\n" << res << std::endl;
662
663 vpMatrix res2;
664 res2 = A + B;
665 std::cout << "\n2) A+B:\n" << res2 << std::endl;
666 }
667#endif
668
669 {
670 std::cout << "\n------------------------" << std::endl;
671 std::cout << "--- TEST vpMatrix::hadamard()" << std::endl;
672 std::cout << "------------------------" << std::endl;
673
674 vpMatrix M1(3, 5), M2(3, 5);
675 for (unsigned int i = 0; i < M1.size(); i++) {
676 M1.data[i] = i;
677 M2.data[i] = i + 2;
678 }
679
680 // Reference
681 std::vector<double> references = computeHadamard(std::vector<double>(M1.data, M1.data + M1.size()),
682 std::vector<double>(M2.data, M2.data + M2.size()));
683
684 std::cout << "M1:\n" << M1 << std::endl;
685 std::cout << "\nM2:\n" << M2 << std::endl;
686 M2 = M1.hadamard(M2);
687 std::cout << "\nRes:\n" << M2 << std::endl;
688
689 if (!test("M2", M2, references)) {
690 std::cerr << "Error with Hadamard product" << std::endl;
691 return EXIT_FAILURE;
692 }
693 }
694
695 {
696 std::cout << "\n------------------------" << std::endl;
697 std::cout << "--- TEST vpMatrix::stackColums()" << std::endl;
698 std::cout << "------------------------" << std::endl;
699 vpMatrix M(3, 5);
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();
703 }
704 }
705 std::cout << "M:\n" << M << std::endl;
706 vpColVector v = M.stackColumns();
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;
710 return EXIT_FAILURE;
711 }
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;
715 return EXIT_FAILURE;
716 }
717 }
718 }
719
720 {
721 std::cout << "\n------------------------" << std::endl;
722 std::cout << "--- TEST vpMatrix::stackRows()" << std::endl;
723 std::cout << "------------------------" << std::endl;
724 vpMatrix M(3, 5);
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;
728 }
729 }
730 std::cout << "M:\n" << M << std::endl;
731 vpRowVector v = M.stackRows();
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;
735 return EXIT_FAILURE;
736 }
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;
740 return EXIT_FAILURE;
741 }
742 }
743 }
744
745 {
746 std::cout << "\n------------------------" << std::endl;
747 std::cout << "--- TEST vpMatrix::getCol()" << std::endl;
748 std::cout << "------------------------" << std::endl;
749 vpMatrix A(4,4);
750 for(unsigned int i=0; i < A.getRows(); i++)
751 for(unsigned int j=0; j < A.getCols(); j++)
752 A[i][j] = i*A.getCols()+j;
753
754 {
755 vpColVector cv = A.getCol(1, 1, 3);
756 vpColVector ref;
757 ref << 5, 9, 13;
758 if (cv != ref) {
759 std::cerr << "Problem in vpMatrix::getCol(): values are different" << std::endl;
760 return EXIT_FAILURE;
761 }
762 }
763 {
764 vpColVector cv = A.getCol(1);
765 vpColVector ref;
766 ref << 1, 5, 9, 13;
767 if (cv != ref) {
768 std::cerr << "Problem in vpMatrix::getCol(): values are different" << std::endl;
769 return EXIT_FAILURE;
770 }
771 }
772 }
773
774 {
775 std::cout << "\n------------------------" << std::endl;
776 std::cout << "--- TEST vpMatrix::getRow()" << std::endl;
777 std::cout << "------------------------" << std::endl;
778 vpMatrix A(4,4);
779 for(unsigned int i=0; i < A.getRows(); i++)
780 for(unsigned int j=0; j < A.getCols(); j++)
781 A[i][j] = i*A.getCols()+j;
782
783 {
784 vpRowVector rv = A.getRow(1, 1, 3);
785 vpRowVector ref;
786 ref << 5, 6, 7;
787 if (rv != ref) {
788 std::cerr << "Problem in vpMatrix::getRow(): values are different" << std::endl;
789 return EXIT_FAILURE;
790 }
791 }
792 {
793 vpRowVector rv = A.getRow(1);
794 vpRowVector ref;
795 ref << 4, 5, 6, 7;
796 if (rv != ref) {
797 std::cerr << "Problem in vpMatrix::getRow(): values are different" << std::endl;
798 return EXIT_FAILURE;
799 }
800 }
801 }
802
803 {
804 std::cout << "\n------------------------" << std::endl;
805 std::cout << "--- TEST vpMatrix::getDiag()" << std::endl;
806 std::cout << "------------------------" << std::endl;
807 vpMatrix A(3,4);
808 for(unsigned int i=0; i < A.getRows(); i++)
809 for(unsigned int j=0; j < A.getCols(); j++)
810 A[i][j] = i*A.getCols()+j;
811
812 vpColVector diag = A.getDiag();
813 vpColVector ref;
814 ref << 0.0, 5.0, 10.0;
815 if (diag != ref) {
816 std::cerr << "Problem in vpMatrix::getDiag(): values are different" << std::endl;
817 return EXIT_FAILURE;
818 }
819 }
820
821 std::cout << "\nAll tests succeeded" << std::endl;
822 return EXIT_SUCCESS;
823 } catch (const vpException &e) {
824 std::cout << "Catch an exception: " << e << std::endl;
825 return EXIT_FAILURE;
826 }
827}
unsigned int getCols() const
Definition: vpArray2D.h:279
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
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)
Definition: vpGEMM.h:393
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpRowVector t() const
error that can be emited by ViSP classes.
Definition: vpException.h:72
static double rad(double deg)
Definition: vpMath.h:110
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:295
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
static bool loadMatrixYAML(const std::string &filename, vpArray2D< double > &M, char *header=NULL)
Definition: vpMatrix.h:729
static bool loadMatrix(const std::string &filename, vpArray2D< double > &M, bool binary=false, char *header=NULL)
Definition: vpMatrix.h:713
vpMatrix hadamard(const vpMatrix &m) const
Definition: vpMatrix.cpp:1767
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5598
void init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
Definition: vpMatrix.cpp:346
void eye()
Definition: vpMatrix.cpp:449
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5879
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:5531
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:5215
vpColVector getDiag() const
Definition: vpMatrix.cpp:5307
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5175
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5988
static bool saveMatrixYAML(const std::string &filename, const vpArray2D< double > &M, const char *header="")
Definition: vpMatrix.h:766
static bool saveMatrix(const std::string &filename, const vpArray2D< double > &M, bool binary=false, const char *header="")
Definition: vpMatrix.h:748
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:116
void resize(unsigned int i, bool flagNullify=true)
Definition: vpRowVector.h:271
int print(std::ostream &s, unsigned int length, char const *intro=0) const
VISP_EXPORT double measureTimeMs()