Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testMatrix.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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*****************************************************************************/
35
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>
48
49#include <stdio.h>
50#include <stdlib.h>
51
52#include <iterator> // for std::back_inserter
53
54namespace
55{
56bool test(const std::string &s, const vpMatrix &M, const std::vector<double> &bench)
57{
58 static unsigned int cpt = 0;
59 std::cout << "** Test " << ++cpt << std::endl;
60 std::cout << s << "(" << M.getRows() << "," << M.getCols() << ") = \n" << M << std::endl;
61 if (bench.size() != M.size()) {
62 std::cout << "Test fails: bad size wrt bench" << std::endl;
63 return false;
64 }
65 for (unsigned int i = 0; i < M.size(); i++) {
66 if (std::fabs(M.data[i] - bench[i]) > std::fabs(M.data[i]) * std::numeric_limits<double>::epsilon()) {
67 std::cout << "Test fails: bad content" << std::endl;
68 return false;
69 }
70 }
71
72 return true;
73}
74
75double getRandomValues(double min, double max) { return (max - min) * ((double)rand() / (double)RAND_MAX) + min; }
76
77bool equalMatrix(const vpMatrix &A, const vpMatrix &B, double tol = std::numeric_limits<double>::epsilon())
78{
79 if (A.getRows() != B.getRows() || A.getCols() != B.getCols()) {
80 return false;
81 }
82
83 for (unsigned int i = 0; i < A.getRows(); i++) {
84 for (unsigned int j = 0; j < A.getCols(); j++) {
85 if (!vpMath::equal(A[i][j], B[i][j], tol)) {
86 return false;
87 }
88 }
89 }
90
91 return true;
92}
93
94vpMatrix generateRandomMatrix(unsigned int rows, unsigned int cols, double min, double max)
95{
96 vpMatrix M(rows, cols);
97
98 for (unsigned int i = 0; i < M.getRows(); i++) {
99 for (unsigned int j = 0; j < M.getCols(); j++) {
100 M[i][j] = getRandomValues(min, max);
101 }
102 }
103
104 return M;
105}
106
107std::vector<double> computeHadamard(const std::vector<double> &v1, const std::vector<double> &v2)
108{
109 std::vector<double> result;
110 std::transform(v1.begin(), v1.end(), v2.begin(), std::back_inserter(result), std::multiplies<double>());
111 return result;
112}
113} // namespace
114
115int main(int argc, char *argv[])
116{
117 try {
118 bool ctest = true;
119 for (int i = 1; i < argc; i++) {
120 if (std::string(argv[i]) == "--benchmark") {
121 ctest = false;
122 }
123 }
124
125 {
126 const double val = 10.0;
127 vpMatrix M, M2(5, 5, val);
128 M.resize(5, 5, false, false);
129 M = val;
130 for (unsigned int i = 0; i < M.getRows(); i++) {
131 for (unsigned int j = 0; j < M.getCols(); j++) {
132 if (!vpMath::equal(M[i][j], val, std::numeric_limits<double>::epsilon())) {
133 std::cerr << "Issue with matrix assignment with value." << std::endl;
134 return EXIT_FAILURE;
135 }
136
137 if (!vpMath::equal(M2[i][j], val, std::numeric_limits<double>::epsilon())) {
138 std::cerr << "Issue with matrix constructor initialized with value." << std::endl;
139 return EXIT_FAILURE;
140 }
141 }
142 }
143 }
144 {
145 // Test vpRotationMatrix construction
146 std::vector<double> bench(9, 0);
147 bench[2] = bench[4] = bench[6] = 1.;
148 vpMatrix M(3, 3);
149 M[2][0] = M[1][1] = M[0][2] = 1.;
150 vpRotationMatrix R1(M);
151 if (test("R1", R1, bench) == false)
152 return EXIT_FAILURE;
154 R2 = M;
155 if (test("R2", R2, bench) == false)
156 return EXIT_FAILURE;
157 }
158 {
159 vpColVector c(6, 1);
160 vpRowVector r(6, 1);
161 std::vector<double> bench(6, 1);
162 vpMatrix M1(c);
163 if (test("M1", M1, bench) == false)
164 return EXIT_FAILURE;
165 vpMatrix M2(r);
166 if (test("M2", M2, bench) == false)
167 return EXIT_FAILURE;
168 }
169 {
170 vpMatrix M(4, 5);
171 int val = 0;
172 for (unsigned int i = 0; i < M.getRows(); i++) {
173 for (unsigned int j = 0; j < M.getCols(); j++) {
174 M[i][j] = val++;
175 }
176 }
177 std::cout << "M ";
178 M.print(std::cout, 4);
179
180 vpMatrix N;
181 N.init(M, 0, 1, 2, 3);
182 std::cout << "N ";
183 N.print(std::cout, 4);
184 std::string header("My 4-by-5 matrix\nwith a second line");
185
186 // Save matrix in text format
187 if (vpMatrix::saveMatrix("matrix.mat", M, false, header.c_str()))
188 std::cout << "Matrix saved in matrix.mat file" << std::endl;
189 else
190 return EXIT_FAILURE;
191
192 // Load matrix in text format
193 vpMatrix M1;
194 char header_[100];
195 if (vpMatrix::loadMatrix("matrix.mat", M1, false, header_))
196 std::cout << "Matrix loaded from matrix.mat file with header \"" << header_ << "\": \n" << M1 << std::endl;
197 else
198 return EXIT_FAILURE;
199 if (header != std::string(header_)) {
200 std::cout << "Bad header in matrix.mat" << std::endl;
201 return EXIT_FAILURE;
202 }
203
204 // Save matrix in binary format
205 if (vpMatrix::saveMatrix("matrix.bin", M, true, header.c_str()))
206 std::cout << "Matrix saved in matrix.bin file" << std::endl;
207 else
208 return EXIT_FAILURE;
209
210 // Load matrix in binary format
211 if (vpMatrix::loadMatrix("matrix.bin", M1, true, header_))
212 std::cout << "Matrix loaded from matrix.bin file with header \"" << header_ << "\": \n" << M1 << std::endl;
213 else
214 return EXIT_FAILURE;
215 if (header != std::string(header_)) {
216 std::cout << "Bad header in matrix.bin" << std::endl;
217 return EXIT_FAILURE;
218 }
219
220 // Save matrix in YAML format
221 if (vpMatrix::saveMatrixYAML("matrix.yml", M, header.c_str()))
222 std::cout << "Matrix saved in matrix.yml file" << std::endl;
223 else
224 return EXIT_FAILURE;
225
226 // Read matrix in YAML format
227 vpMatrix M2;
228 if (vpMatrix::loadMatrixYAML("matrix.yml", M2, header_))
229 std::cout << "Matrix loaded from matrix.yml file with header \"" << header_ << "\": \n" << M2 << std::endl;
230 else
231 return EXIT_FAILURE;
232 if (header != std::string(header_)) {
233 std::cout << "Bad header in matrix.mat" << std::endl;
234 return EXIT_FAILURE;
235 }
236 }
237
238 {
240 std::cout << "R: \n" << R << std::endl;
241 vpMatrix M1(R);
242 std::cout << "M1: \n" << M1 << std::endl;
243 vpMatrix M2(M1);
244 std::cout << "M2: \n" << M2 << std::endl;
245 vpMatrix M3 = R;
246 std::cout << "M3: \n" << M3 << std::endl;
247 vpMatrix M4 = M1;
248 std::cout << "M4: \n" << M4 << std::endl;
249 }
250 {
251
252 std::cout << "------------------------" << std::endl;
253 std::cout << "--- TEST PRETTY PRINT---" << std::endl;
254 std::cout << "------------------------" << std::endl;
255 vpMatrix M;
256 M.eye(4);
257
258 std::cout << "call std::cout << M;" << std::endl;
259 std::cout << M << std::endl;
260
261 std::cout << "call M.print (std::cout, 4);" << std::endl;
262 M.print(std::cout, 4);
263
264 std::cout << "------------------------" << std::endl;
265 M.resize(3, 3);
266 M.eye(3);
267 M[1][0] = 1.235;
268 M[1][1] = 12.345;
269 M[1][2] = .12345;
270 std::cout << "call std::cout << M;" << std::endl;
271 std::cout << M << std::endl;
272 std::cout << "call M.print (std::cout, 6);" << std::endl;
273 M.print(std::cout, 6);
274 std::cout << std::endl;
275
276 std::cout << "------------------------" << std::endl;
277 M[0][0] = -1.235;
278 M[1][0] = -12.235;
279
280 std::cout << "call std::cout << M;" << std::endl;
281 std::cout << M << std::endl;
282
283 std::cout << "call M.print (std::cout, 10);" << std::endl;
284 M.print(std::cout, 10);
285 std::cout << std::endl;
286
287 std::cout << "call M.print (std::cout, 2);" << std::endl;
288 M.print(std::cout, 2);
289 std::cout << std::endl;
290
291 std::cout << "------------------------" << std::endl;
292 M.resize(3, 3);
293 M.eye(3);
294 M[0][2] = -0.0000876;
295 std::cout << "call std::cout << M;" << std::endl;
296 std::cout << M << std::endl;
297
298 std::cout << "call M.print (std::cout, 4);" << std::endl;
299 M.print(std::cout, 4);
300 std::cout << std::endl;
301 std::cout << "call M.print (std::cout, 6, \"M\");" << std::endl;
302 M.print(std::cout, 6, "M");
303 std::cout << std::endl;
304 std::cout << "call M.print (std::cout, 10, \"M\");" << std::endl;
305 M.print(std::cout, 10, "M");
306 std::cout << std::endl;
307
308 M.resize(2, 3);
309 M[0][0] = -1;
310 M[0][1] = -2;
311 M[0][2] = -3;
312 M[1][0] = 4;
313 M[1][1] = 5.5;
314 M[1][2] = 6.0f;
315 std::cout << "call std::cout << M;" << std::endl;
316 std::cout << M << std::endl;
317 std::cout << "call M.print (std::cout, 5, \"M\");" << std::endl;
318 M.print(std::cout, 5, "M");
319 std::cout << std::endl;
320
321 M.resize(2, 3);
322 M[0][0] = -1;
323 M[0][1] = -2;
324 M[0][2] = -3;
325 M[1][0] = 4;
326 M[1][1] = 5.;
327 M[1][2] = 6;
328 std::cout << "call std::cout << M;" << std::endl;
329 std::cout << M << std::endl;
330 std::cout << "call M.print (std::cout, 5, \"M\");" << std::endl;
331 M.print(std::cout, 5, "M");
332 std::cout << std::endl;
333
334 std::cout << "------------------------" << std::endl;
335 std::cout << "--- TEST RESIZE --------" << std::endl;
336 std::cout << "------------------------" << std::endl;
337 std::cout << "5x5" << std::endl;
338 M.resize(5, 5, false);
339 std::cout << M << std::endl;
340 std::cout << "3x2" << std::endl;
341 M.resize(3, 2, false);
342 std::cout << M << std::endl;
343 std::cout << "2x2" << std::endl;
344 M.resize(2, 2, false);
345 std::cout << M << std::endl;
346 std::cout << "------------------------" << std::endl;
347
349 vpMatrix A(1, 6), B;
350
351 A = 1.0;
352 // vMe=1.0;
353 B = A * vMe;
354
355 std::cout << "------------------------" << std::endl;
356 std::cout << "--- TEST vpRowVector * vpColVector" << std::endl;
357 std::cout << "------------------------" << std::endl;
358 vpRowVector r(3);
359 r[0] = 2;
360 r[1] = 3;
361 r[2] = 4;
362
363 vpColVector c(3);
364 c[0] = 1;
365 c[1] = 2;
366 c[2] = -1;
367
368 double rc = r * c;
369
370 r.print(std::cout, 2, "r");
371 c.print(std::cout, 2, "c");
372 std::cout << "r * c = " << rc << std::endl;
373
374 std::cout << "------------------------" << std::endl;
375 std::cout << "--- TEST vpRowVector * vpMatrix" << std::endl;
376 std::cout << "------------------------" << std::endl;
377 M.resize(3, 3);
378 M.eye(3);
379
380 M[1][0] = 1.5;
381 M[2][0] = 2.3;
382
383 vpRowVector rM = r * M;
384
385 r.print(std::cout, 2, "r");
386 M.print(std::cout, 10, "M");
387 std::cout << "r * M = " << rM << std::endl;
388
389 std::cout << "------------------------" << std::endl;
390 std::cout << "--- TEST vpGEMM " << std::endl;
391 std::cout << "------------------------" << std::endl;
392 M.resize(3, 3);
393 M.eye(3);
394 vpMatrix N(3, 3);
395 N[0][0] = 2;
396 N[1][0] = 1.2;
397 N[1][2] = 0.6;
398 N[2][2] = 0.25;
399
400 vpMatrix C(3, 3);
401 C.eye(3);
402
403 vpMatrix D;
404
405 // realise the operation D = 2 * M^T * N + 3 C
406 vpGEMM(M, N, 2, C, 3, D, VP_GEMM_A_T);
407 std::cout << D << std::endl;
408 }
409
410 {
411 std::cout << "------------------------" << std::endl;
412 std::cout << "--- TEST vpMatrix insert() with same colNum " << std::endl;
413 std::cout << "------------------------" << std::endl;
414 unsigned int nb = ctest ? 10 : 100; // 10000;
415 const unsigned int size = ctest ? 10 : 100;
416
417 vpMatrix m_big(nb * size, 6);
418 std::vector<vpMatrix> submatrices(nb);
419 for (size_t cpt = 0; cpt < submatrices.size(); cpt++) {
420 vpMatrix m(size, 6);
421
422 for (unsigned int i = 0; i < m.getRows(); i++) {
423 for (unsigned int j = 0; j < m.getCols(); j++) {
424 m[i][j] = getRandomValues(-100.0, 100.0);
425 }
426 }
427
428 submatrices[cpt] = m;
429 }
430
431 double t = vpTime::measureTimeMs();
432 for (unsigned int i = 0; i < nb; i++) {
433 m_big.insert(submatrices[(size_t)i], i * size, 0);
434 }
435 t = vpTime::measureTimeMs() - t;
436 std::cout << "Matrix insert(): " << t << " ms" << std::endl;
437
438 for (unsigned int cpt = 0; cpt < nb; cpt++) {
439 for (unsigned int i = 0; i < size; i++) {
440 for (unsigned int j = 0; j < 6; j++) {
441 if (!vpMath::equal(m_big[cpt * size + i][j], submatrices[(size_t)cpt][i][j],
442 std::numeric_limits<double>::epsilon())) {
443 std::cerr << "Problem with vpMatrix insert()!" << std::endl;
444 return EXIT_FAILURE;
445 }
446 }
447 }
448 }
449
450 // Try to insert empty matrices
451 vpMatrix m1(2, 3), m2, m3;
452 m1.insert(m2, 0, 0);
453 m3.insert(m2, 0, 0);
454
455 std::cout << "Insert empty matrices:" << std::endl;
456 std::cout << "m1:\n" << m1 << std::endl;
457 std::cout << "m2:\n" << m2 << std::endl;
458 std::cout << "m3:\n" << m3 << std::endl;
459
460 std::cout << "\n------------------------" << std::endl;
461 std::cout << "--- TEST vpMatrix stack()" << std::endl;
462 std::cout << "------------------------" << std::endl;
463
464 {
465 vpMatrix L, L2(2, 6);
466 L2 = 2;
467 L.stack(L2);
468 std::cout << "L:\n" << L << std::endl;
469 L2.resize(3, 6);
470 L2 = 3;
471 L.stack(L2);
472 std::cout << "L:\n" << L << std::endl;
473 }
474
475 {
476 vpMatrix m_big_stack;
478 for (unsigned int i = 0; i < nb; i++) {
479 m_big_stack.stack(submatrices[(size_t)i]);
480 }
481 t = vpTime::measureTimeMs() - t;
482 std::cout << "Matrix stack(): " << t << " ms" << std::endl;
483
484 if (!equalMatrix(m_big, m_big_stack)) {
485 std::cerr << "Problem with vpMatrix stack()!" << std::endl;
486 return EXIT_FAILURE;
487 }
488 }
489
490 std::cout << "\n------------------------" << std::endl;
491 std::cout << "--- TEST vpMatrix stack(vpRowVector)" << std::endl;
492 std::cout << "------------------------" << std::endl;
493
494 vpMatrix m_big_stack = generateRandomMatrix(10000, ctest ? 10 : 100, -1000.0, 1000.0);
495 std::cout << "m_big_stack: " << m_big_stack.getRows() << "x" << m_big_stack.getCols() << std::endl;
496
497 vpMatrix m_big_stack_row;
499 for (unsigned int i = 0; i < m_big_stack.getRows(); i++) {
500 m_big_stack_row.stack(m_big_stack.getRow(i));
501 }
502 t = vpTime::measureTimeMs() - t;
503 std::cout << "Matrix stack(vpRowVector): " << t << " ms" << std::endl;
504
505 if (!equalMatrix(m_big_stack, m_big_stack_row)) {
506 std::cerr << "Problem with vpMatrix stack(vpRowVector)!" << std::endl;
507 return EXIT_FAILURE;
508 }
509
510 std::cout << "\n------------------------" << std::endl;
511 std::cout << "--- TEST vpMatrix stack(vpColVector)" << std::endl;
512 std::cout << "------------------------" << std::endl;
513
514 vpMatrix m_big_stack_col;
516 for (unsigned int j = 0; j < m_big_stack.getCols(); j++) {
517 m_big_stack_col.stack(m_big_stack.getCol(j));
518 }
519 t = vpTime::measureTimeMs() - t;
520 std::cout << "Matrix stack(vpColVector): " << t << " ms" << std::endl;
521
522 if (!equalMatrix(m_big_stack, m_big_stack_col)) {
523 std::cerr << "Problem with vpMatrix stack(vpColVector)!" << std::endl;
524 return EXIT_FAILURE;
525 }
526
527 std::cout << "\n------------------------" << std::endl;
528 std::cout << "--- TEST vpMatrix::stack()" << std::endl;
529 std::cout << "------------------------" << std::endl;
530
531 {
532 vpMatrix L, L2(2, 6), L_tmp;
533 L2 = 2;
534 vpMatrix::stack(L_tmp, L2, L);
535 std::cout << "L:\n" << L << std::endl;
536 L2.resize(3, 6);
537 L2 = 3;
538 L_tmp = L;
539 vpMatrix::stack(L_tmp, L2, L);
540 std::cout << "L:\n" << L << std::endl;
541 }
542
543 {
544 vpMatrix m_big_stack_static, m_big_stack_static_tmp;
546 for (unsigned int i = 0; i < nb; i++) {
547 vpMatrix::stack(m_big_stack_static_tmp, submatrices[(size_t)i], m_big_stack_static);
548 m_big_stack_static_tmp = m_big_stack_static;
549 }
550 t = vpTime::measureTimeMs() - t;
551 std::cout << "Matrix::stack(): " << t << " ms" << std::endl;
552
553 if (!equalMatrix(m_big, m_big_stack_static)) {
554 std::cerr << "Problem with vpMatrix::stack()!" << std::endl;
555 return EXIT_FAILURE;
556 }
557 }
558
559 std::cout << "\n------------------------" << std::endl;
560 std::cout << "--- TEST vpMatrix::stack(vpMatrix, vpRowVector, vpMatrix)" << std::endl;
561 std::cout << "------------------------" << std::endl;
562
563 vpMatrix m_big_stack_static = generateRandomMatrix(ctest ? 100 : 1000, ctest ? 10 : 100, -1000.0, 1000.0);
564 std::cout << "m_big_stack_static: " << m_big_stack_static.getRows() << "x" << m_big_stack_static.getCols()
565 << std::endl;
566
567 vpMatrix m_big_stack_static_row, m_big_stack_static_row_tmp;
569 for (unsigned int i = 0; i < m_big_stack_static.getRows(); i++) {
570 vpMatrix::stack(m_big_stack_static_row_tmp, m_big_stack_static.getRow(i), m_big_stack_static_row);
571 m_big_stack_static_row_tmp = m_big_stack_static_row;
572 }
573 t = vpTime::measureTimeMs() - t;
574 std::cout << "Matrix::stack(vpMatrix, vpRowVector, vpMatrix): " << t << " ms" << std::endl;
575
576 if (!equalMatrix(m_big_stack_static, m_big_stack_static_row)) {
577 std::cerr << "Problem with vpMatrix::stack(vpMatrix, vpRowVector, "
578 "vpMatrix)!"
579 << std::endl;
580 return EXIT_FAILURE;
581 }
582
583 std::cout << "\n------------------------" << std::endl;
584 std::cout << "--- TEST vpMatrix::stack(vpMatrix, vpColVector, vpMatrix)" << std::endl;
585 std::cout << "------------------------" << std::endl;
586
587 vpMatrix m_big_stack_static_col, m_big_stack_static_col_tmp;
589 for (unsigned int j = 0; j < m_big_stack_static.getCols(); j++) {
590 vpMatrix::stack(m_big_stack_static_col_tmp, m_big_stack_static.getCol(j), m_big_stack_static_col);
591 m_big_stack_static_col_tmp = m_big_stack_static_col;
592 }
593 t = vpTime::measureTimeMs() - t;
594 std::cout << "Matrix::stack(vpMatrix, vpColVector, vpMatrix): " << t << " ms" << std::endl;
595
596 if (!equalMatrix(m_big_stack_static, m_big_stack_static_col)) {
597 std::cerr << "Problem with vpMatrix::stack(vpMatrix, vpColVector, "
598 "vpMatrix)!"
599 << std::endl;
600 return EXIT_FAILURE;
601 }
602 }
603
604 {
605 vpMatrix m1(11, 9), m2(3, 4);
606 for (unsigned int i = 0; i < m2.getRows(); i++) {
607 for (unsigned int j = 0; j < m2.getCols(); j++) {
608 m2[i][j] = getRandomValues(-100.0, 100.0);
609 }
610 }
611
612 unsigned int offset_i = 4, offset_j = 3;
613 m1.insert(m2, offset_i, offset_j);
614
615 for (unsigned int i = 0; i < m2.getRows(); i++) {
616 for (unsigned int j = 0; j < m2.getCols(); j++) {
617 if (!vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
618 std::cerr << "Problem with vpMatrix insert()!" << std::endl;
619 return EXIT_FAILURE;
620 }
621 }
622 }
623
624 offset_i = 4;
625 offset_j = 5;
626 m1.insert(m2, offset_i, offset_j);
627
628 for (unsigned int i = 0; i < m2.getRows(); i++) {
629 for (unsigned int j = 0; j < m2.getCols(); j++) {
630 if (!vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
631 std::cerr << "Problem with vpMatrix insert()!" << std::endl;
632 return EXIT_FAILURE;
633 }
634 }
635 }
636
637 offset_i = 8;
638 offset_j = 5;
639 m1.insert(m2, offset_i, offset_j);
640
641 for (unsigned int i = 0; i < m2.getRows(); i++) {
642 for (unsigned int j = 0; j < m2.getCols(); j++) {
643 if (!vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
644 std::cerr << "Problem with vpMatrix insert()!" << std::endl;
645 return EXIT_FAILURE;
646 }
647 }
648 }
649 }
650
651 {
652 std::cout << "\n------------------------" << std::endl;
653 std::cout << "--- TEST vpMatrix::juxtaposeMatrices()" << std::endl;
654 std::cout << "------------------------" << std::endl;
655
656 vpMatrix A(5, 6), B(5, 4);
657 for (unsigned int i = 0; i < A.getRows(); i++) {
658 for (unsigned int j = 0; j < A.getCols(); j++) {
659 A[i][j] = i * A.getCols() + j;
660
661 if (j < B.getCols()) {
662 B[i][j] = (i * B.getCols() + j) * 10;
663 }
664 }
665 }
666
667 vpMatrix juxtaposeM;
668 vpMatrix::juxtaposeMatrices(A, B, juxtaposeM);
669 std::cout << "juxtaposeM:\n" << juxtaposeM << std::endl;
670 }
671
672#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
673 {
674 std::vector<vpMatrix> vec_mat;
675 vec_mat.emplace_back(5, 5);
676
677 vpMatrix A(4, 4), B(4, 4);
678 A = 1;
679 B = 2;
680 vpMatrix res = A + B;
681 std::cout << "\n1) A+B:\n" << res << std::endl;
682
683 vpMatrix res2;
684 res2 = A + B;
685 std::cout << "\n2) A+B:\n" << res2 << std::endl;
686 }
687#endif
688
689 {
690 std::cout << "\n------------------------" << std::endl;
691 std::cout << "--- TEST vpMatrix::hadamard()" << std::endl;
692 std::cout << "------------------------" << std::endl;
693
694 vpMatrix M1(3, 5), M2(3, 5);
695 for (unsigned int i = 0; i < M1.size(); i++) {
696 M1.data[i] = i;
697 M2.data[i] = i + 2;
698 }
699
700 // Reference
701 std::vector<double> references = computeHadamard(std::vector<double>(M1.data, M1.data + M1.size()),
702 std::vector<double>(M2.data, M2.data + M2.size()));
703
704 std::cout << "M1:\n" << M1 << std::endl;
705 std::cout << "\nM2:\n" << M2 << std::endl;
706 M2 = M1.hadamard(M2);
707 std::cout << "\nRes:\n" << M2 << std::endl;
708
709 if (!test("M2", M2, references)) {
710 std::cerr << "Error with Hadamard product" << std::endl;
711 return EXIT_FAILURE;
712 }
713 }
714
715 {
716 std::cout << "\n------------------------" << std::endl;
717 std::cout << "--- TEST vpMatrix::stackColums()" << std::endl;
718 std::cout << "------------------------" << std::endl;
719 vpMatrix M(3, 5);
720 for (unsigned int j = 0; j < M.getCols(); j++) {
721 for (unsigned int i = 0; i < M.getRows(); i++) {
722 M[i][j] = i + j * M.getRows();
723 }
724 }
725 std::cout << "M:\n" << M << std::endl;
726 vpColVector v = M.stackColumns();
727 std::cout << "Column stack: " << v.t() << std::endl;
728 if (M.size() != v.size()) {
729 std::cerr << "Problem in vpMatrix::stackColumns(): size differ" << std::endl;
730 return EXIT_FAILURE;
731 }
732 for (unsigned int i = 0; i < v.size(); i++) {
733 if (std::fabs(v[i] - static_cast<double>(i)) > std::numeric_limits<double>::epsilon()) {
734 std::cerr << "Problem in vpMatrix::stackColumns(): content differ" << std::endl;
735 return EXIT_FAILURE;
736 }
737 }
738 }
739
740 {
741 std::cout << "\n------------------------" << std::endl;
742 std::cout << "--- TEST vpMatrix::stackRows()" << std::endl;
743 std::cout << "------------------------" << std::endl;
744 vpMatrix M(3, 5);
745 for (unsigned int i = 0; i < M.getRows(); i++) {
746 for (unsigned int j = 0; j < M.getCols(); j++) {
747 M[i][j] = i * M.getCols() + j;
748 }
749 }
750 std::cout << "M:\n" << M << std::endl;
751 vpRowVector v = M.stackRows();
752 std::cout << "Rows stack: " << v << std::endl;
753 if (M.size() != v.size()) {
754 std::cerr << "Problem in vpMatrix::stackRows(): size differ" << std::endl;
755 return EXIT_FAILURE;
756 }
757 for (unsigned int i = 0; i < v.size(); i++) {
758 if (std::fabs(v[i] - static_cast<double>(i)) > std::numeric_limits<double>::epsilon()) {
759 std::cerr << "Problem in vpMatrix::stackRows(): content differ" << std::endl;
760 return EXIT_FAILURE;
761 }
762 }
763 }
764
765 {
766 std::cout << "\n------------------------" << std::endl;
767 std::cout << "--- TEST vpMatrix::getCol()" << std::endl;
768 std::cout << "------------------------" << std::endl;
769 vpMatrix A(4, 4);
770 for (unsigned int i = 0; i < A.getRows(); i++)
771 for (unsigned int j = 0; j < A.getCols(); j++)
772 A[i][j] = i * A.getCols() + j;
773
774 {
775 vpColVector cv = A.getCol(1, 1, 3);
776 vpColVector ref;
777 ref << 5, 9, 13;
778 if (cv != ref) {
779 std::cerr << "Problem in vpMatrix::getCol(): values are different" << std::endl;
780 return EXIT_FAILURE;
781 }
782 }
783 {
784 vpColVector cv = A.getCol(1);
785 vpColVector ref;
786 ref << 1, 5, 9, 13;
787 if (cv != ref) {
788 std::cerr << "Problem in vpMatrix::getCol(): values are different" << std::endl;
789 return EXIT_FAILURE;
790 }
791 }
792 }
793
794 {
795 std::cout << "\n------------------------" << std::endl;
796 std::cout << "--- TEST vpMatrix::getRow()" << std::endl;
797 std::cout << "------------------------" << std::endl;
798 vpMatrix A(4, 4);
799 for (unsigned int i = 0; i < A.getRows(); i++)
800 for (unsigned int j = 0; j < A.getCols(); j++)
801 A[i][j] = i * A.getCols() + j;
802
803 {
804 vpRowVector rv = A.getRow(1, 1, 3);
805 vpRowVector ref;
806 ref << 5, 6, 7;
807 if (rv != ref) {
808 std::cerr << "Problem in vpMatrix::getRow(): values are different" << std::endl;
809 return EXIT_FAILURE;
810 }
811 }
812 {
813 vpRowVector rv = A.getRow(1);
814 vpRowVector ref;
815 ref << 4, 5, 6, 7;
816 if (rv != ref) {
817 std::cerr << "Problem in vpMatrix::getRow(): values are different" << std::endl;
818 return EXIT_FAILURE;
819 }
820 }
821 }
822
823 {
824 std::cout << "\n------------------------" << std::endl;
825 std::cout << "--- TEST vpMatrix::getDiag()" << std::endl;
826 std::cout << "------------------------" << std::endl;
827 vpMatrix A(3, 4);
828 for (unsigned int i = 0; i < A.getRows(); i++)
829 for (unsigned int j = 0; j < A.getCols(); j++)
830 A[i][j] = i * A.getCols() + j;
831
832 vpColVector diag = A.getDiag();
833 vpColVector ref;
834 ref << 0.0, 5.0, 10.0;
835 if (diag != ref) {
836 std::cerr << "Problem in vpMatrix::getDiag(): values are different" << std::endl;
837 return EXIT_FAILURE;
838 }
839 }
840
841 std::cout << "\nAll tests succeeded" << std::endl;
842 return EXIT_SUCCESS;
843 } catch (const vpException &e) {
844 std::cout << "Catch an exception: " << e << std::endl;
845 return EXIT_FAILURE;
846 }
847}
unsigned int getCols() const
Definition vpArray2D.h:280
Type * data
Address of the first element of the data array.
Definition vpArray2D.h:144
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
unsigned int getRows() const
Definition vpArray2D.h:290
Implementation of column vector and the associated operations.
vpRowVector t() const
error that can be emitted by ViSP classes.
Definition vpException.h:59
static double rad(double deg)
Definition vpMath.h:116
static bool equal(double x, double y, double threshold=0.001)
Definition vpMath.h:369
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
static bool loadMatrixYAML(const std::string &filename, vpArray2D< double > &M, char *header=NULL)
Definition vpMatrix.h:838
static bool loadMatrix(const std::string &filename, vpArray2D< double > &M, bool binary=false, char *header=NULL)
Definition vpMatrix.h:763
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)
Definition vpMatrix.cpp:343
void eye()
Definition vpMatrix.cpp:446
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="")
Definition vpMatrix.h:987
static bool saveMatrix(const std::string &filename, const vpArray2D< double > &M, bool binary=false, const char *header="")
Definition vpMatrix.h:911
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()