Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testMath.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 core functionalities.
33 *
34*****************************************************************************/
35
42#include <cfloat>
43#include <iostream>
44#include <limits>
45
46#include <visp3/core/vpMath.h>
47#include <visp3/core/vpColVector.h>
48#include <visp3/core/vpRxyzVector.h>
49
50#include <visp3/core/vpHomogeneousMatrix.h>
51
52#if defined _MSC_VER && _MSC_VER >= 1200
53#pragma warning(disable : 4723)
54
55 // 4723 : potential divide by 0
56#endif
57
58#ifdef WIN32
59#ifndef NAN
60// https://msdn.microsoft.com/en-us/library/w22adx1s%28v=vs.120%29.aspx
61// http://tdistler.com/2011/03/24/how-to-define-nan-not-a-number-on-windows
62static const unsigned long __nan[2] = { 0xffffffff, 0x7fffffff };
63#define NAN (*(const float *)__nan)
64#endif
65#endif
66
67int main()
68{
69 // Test isNaN
70 if (vpMath::isNaN(0.0)) {
71 std::cerr << "Fail: IsNaN(0.0)=" << vpMath::isNaN(0.0) << " / should be false" << std::endl;
72 return EXIT_FAILURE;
73 }
74
75 double num = 1.0, den = 0.0;
76 if (vpMath::isNaN(num / den)) {
77 std::cerr << "Fail: IsNaN(1.0/0.0)=" << vpMath::isNaN(num / den) << " / should be false" << std::endl;
78 return EXIT_FAILURE;
79 }
80
81 if (!vpMath::isNaN(NAN)) {
82 std::cerr << "Fail: IsNaN(NAN)=" << vpMath::isNaN(NAN) << " / should be true" << std::endl;
83 return EXIT_FAILURE;
84 }
85
86 num = 0.0;
87 if (!vpMath::isNaN(num / den)) {
88 std::cerr << "Fail: IsNaN(0.0/0.0)=" << vpMath::isNaN(num / den) << " / should be true" << std::endl;
89 return EXIT_FAILURE;
90 }
91
92 if (!vpMath::isNaN(std::numeric_limits<double>::quiet_NaN())) {
93 std::cerr << "Fail: IsNaN(quiet_NaN)=" << vpMath::isNaN(std::numeric_limits<double>::quiet_NaN())
94 << " / should be true" << std::endl;
95 return EXIT_FAILURE;
96 }
97
98 if (!vpMath::isNaN(std::numeric_limits<float>::quiet_NaN())) {
99 std::cerr << "Fail: IsNaN(quiet_NaN)=" << vpMath::isNaN(std::numeric_limits<float>::quiet_NaN())
100 << " / should be true" << std::endl;
101 return EXIT_FAILURE;
102 }
103
104 if (!vpMath::isNaN(std::numeric_limits<double>::signaling_NaN())) {
105 std::cerr << "Fail: IsNaN(signaling_NaN)=" << vpMath::isNaN(std::numeric_limits<double>::signaling_NaN())
106 << " / should be true" << std::endl;
107 return EXIT_FAILURE;
108 }
109
110 if (!vpMath::isNaN(std::numeric_limits<float>::signaling_NaN())) {
111 std::cerr << "Fail: IsNaN(signaling_NaN)=" << vpMath::isNaN(std::numeric_limits<float>::signaling_NaN())
112 << " / should be true" << std::endl;
113 return EXIT_FAILURE;
114 }
115
116 if (vpMath::isNaN(std::numeric_limits<double>::infinity())) {
117 std::cerr << "Fail: IsNaN(infinity)=" << vpMath::isNaN(std::numeric_limits<double>::infinity())
118 << " / should be false" << std::endl;
119 return EXIT_FAILURE;
120 }
121
122 if (vpMath::isNaN(std::numeric_limits<float>::infinity())) {
123 std::cerr << "Fail: IsNaN(infinity)=" << vpMath::isNaN(std::numeric_limits<float>::infinity())
124 << " / should be false" << std::endl;
125 return EXIT_FAILURE;
126 }
127
128 if (vpMath::isNaN(1.0 / std::numeric_limits<double>::epsilon())) {
129 std::cerr << "Fail: IsNaN(1.0/epsilon)=" << vpMath::isNaN(1.0 / std::numeric_limits<double>::epsilon())
130 << " / should be false" << std::endl;
131 return EXIT_FAILURE;
132 }
133
134 if (!vpMath::isNaN(std::numeric_limits<double>::infinity() - std::numeric_limits<double>::infinity())) {
135 std::cerr << "Fail: IsNaN(infinity - infinity)="
136 << vpMath::isNaN(std::numeric_limits<double>::infinity() - std::numeric_limits<double>::infinity())
137 << " / should be true" << std::endl;
138 return EXIT_FAILURE;
139 }
140
141 float a = 0.0f, b = 0.0f;
142 if (!vpMath::isNaN(a / b)) {
143 std::cerr << "Fail: IsNaN(0.0f/0.0f)=" << vpMath::isNaN(a / b) << " / should be true" << std::endl;
144 return EXIT_FAILURE;
145 }
146 std::cout << "vpMath::isNaN is Ok !" << std::endl;
147
148 // Test isInf
149 if (vpMath::isInf(NAN)) {
150 std::cerr << "Fail: vpMath::isInf(NAN)=" << vpMath::isInf(NAN) << " / should be false" << std::endl;
151 return EXIT_FAILURE;
152 }
153
154 if (!vpMath::isInf(1.0 / a)) {
155 std::cerr << "Fail: vpMath::isInf(1.0/0.0)=" << vpMath::isInf(1.0 / a) << " / should be true" << std::endl;
156 return EXIT_FAILURE;
157 }
158
159 if (vpMath::isInf(0.0)) {
160 std::cerr << "Fail: vpMath::isInf(0.0)=" << vpMath::isInf(0.0) << " / should be false" << std::endl;
161 return EXIT_FAILURE;
162 }
163
164 if (!vpMath::isInf(exp(800.))) {
165 std::cerr << "Fail: vpMath::isInf(exp(800.))=" << vpMath::isInf(exp(800.)) << " / should be true" << std::endl;
166 return EXIT_FAILURE;
167 }
168
169 if (vpMath::isInf(DBL_MIN / 2.0)) {
170 std::cerr << "Fail: vpMath::isInf(DBL_MIN/2.0)=" << vpMath::isInf(DBL_MIN / 2.0) << " / should be false"
171 << std::endl;
172 return EXIT_FAILURE;
173 }
174 std::cout << "vpMath::isInf is Ok !" << std::endl;
175
176 // Test isfinite
177 if (vpMath::isFinite(NAN)) {
178 std::cerr << "Fail: vpMath::isFinite(NAN)=" << vpMath::isFinite(NAN) << " / should be false" << std::endl;
179 return EXIT_FAILURE;
180 }
181
182 if (vpMath::isFinite(1.0 / a)) {
183 std::cerr << "Fail: vpMath::isFinite(1.0/0.0)=" << vpMath::isFinite(1.0 / a) << " / should be false" << std::endl;
184 return EXIT_FAILURE;
185 }
186
187 if (!vpMath::isFinite(0.0)) {
188 std::cerr << "Fail: vpMath::isFinite(0.0)=" << vpMath::isFinite(0.0) << " / should be true" << std::endl;
189 return EXIT_FAILURE;
190 }
191
192 if (vpMath::isFinite(exp(800.))) {
193 std::cerr << "Fail: vpMath::isFinite(exp(800.))=" << vpMath::isFinite(exp(800.)) << " / should be false" << std::endl;
194 return EXIT_FAILURE;
195 }
196
197 if (!vpMath::isFinite(DBL_MIN / 2.0)) {
198 std::cerr << "Fail: vpMath::isFinite(DBL_MIN/2.0)=" << vpMath::isFinite(DBL_MIN / 2.0) << " / should be true"
199 << std::endl;
200 return EXIT_FAILURE;
201 }
202
203 if (!vpMath::isFinite(std::numeric_limits<float>::max())) {
204 std::cerr << "Fail: vpMath::isFinite(DBL_MAX)=" << vpMath::isFinite(std::numeric_limits<float>::max()) << " / should be true"
205 << std::endl;
206 return EXIT_FAILURE;
207 }
208 std::cout << "vpMath::isFinite is Ok !" << std::endl;
209
210 // Test isNumber
211 if (!vpMath::isNumber("123")) {
212 std::cerr << "Fail: vpMath::isNumber(\"123\")=" << vpMath::isNumber("123") << " / should be false" << std::endl;
213 return EXIT_FAILURE;
214 }
215 if (vpMath::isNumber("string")) {
216 std::cerr << "Fail: vpMath::isNumber(\"string\")=" << vpMath::isNumber("string") << " / should be true" << std::endl;
217 return EXIT_FAILURE;
218 }
219 if (vpMath::isNumber("123string")) {
220 std::cerr << "Fail: vpMath::isNumber(\"123string\")=" << vpMath::isNumber("123string") << " / should be true" << std::endl;
221 return EXIT_FAILURE;
222 }
223 std::cout << "vpMath::isNumber is Ok !" << std::endl;
224
225 // Test round
226 if (vpMath::round(2.3) != 2) {
227 std::cerr << "Fail: vpMath::round(2.3)=" << vpMath::round(2.3) << " / should be 2" << std::endl;
228 return EXIT_FAILURE;
229 }
230
231 if (vpMath::round(3.8) != 4) {
232 std::cerr << "Fail: vpMath::round(3.8)=" << vpMath::round(3.8) << " / should be 4" << std::endl;
233 return EXIT_FAILURE;
234 }
235
236 if (vpMath::round(5.5) != 6) {
237 std::cerr << "Fail: vpMath::round(5.5)=" << vpMath::round(5.5) << " / should be 6" << std::endl;
238 return EXIT_FAILURE;
239 }
240
241 if (vpMath::round(-2.3) != -2) {
242 std::cerr << "Fail: vpMath::round(-2.3)=" << vpMath::round(-2.3) << " / should be -2" << std::endl;
243 return EXIT_FAILURE;
244 }
245
246 if (vpMath::round(-3.8) != -4) {
247 std::cerr << "Fail: vpMath::round(-3.8)=" << vpMath::round(-3.8) << " / should be -4" << std::endl;
248 return EXIT_FAILURE;
249 }
250
251 if (vpMath::round(-5.5) != -6) {
252 std::cerr << "Fail: vpMath::round(-5.5)=" << vpMath::round(-5.5) << " / should be -6" << std::endl;
253 return EXIT_FAILURE;
254 }
255
256 if (vpMath::round(0.0) != 0) {
257 std::cerr << "Fail: vpMath::round(0.0)=" << vpMath::round(0.0) << " / should be 0" << std::endl;
258 return EXIT_FAILURE;
259 }
260 std::cout << "vpMath::round is Ok !" << std::endl;
261
262 // Test saturate functions
263 // unsigned char
264 char char_value = -127;
265 unsigned char uchar_value = vpMath::saturate<unsigned char>(char_value);
266 if (uchar_value != 0) {
267 std::cerr << "Fail: vpMath::saturate<unsigned char>(-127)=" << uchar_value << " / should be 0" << std::endl;
268 return EXIT_FAILURE;
269 }
270
271 unsigned short ushort_value = 60000;
272 uchar_value = vpMath::saturate<unsigned char>(ushort_value);
273 if (uchar_value != UCHAR_MAX) {
274 std::cerr << "Fail: vpMath::saturate<unsigned char>(60000)=" << uchar_value << " / should be " << UCHAR_MAX
275 << std::endl;
276 return EXIT_FAILURE;
277 }
278
279 int int_value = 70000;
280 uchar_value = vpMath::saturate<unsigned char>(int_value);
281 if (uchar_value != UCHAR_MAX) {
282 std::cerr << "Fail: vpMath::saturate<unsigned char>(70000)=" << uchar_value << " / should be " << UCHAR_MAX
283 << std::endl;
284 return EXIT_FAILURE;
285 }
286
287 int_value = -70000;
288 uchar_value = vpMath::saturate<unsigned char>(int_value);
289 if (uchar_value != 0) {
290 std::cerr << "Fail: vpMath::saturate<unsigned char>(-70000)=" << uchar_value << " / should be 0" << std::endl;
291 return EXIT_FAILURE;
292 }
293
294 short short_value = 30000;
295 uchar_value = vpMath::saturate<unsigned char>(short_value);
296 if (uchar_value != UCHAR_MAX) {
297 std::cerr << "Fail: vpMath::saturate<unsigned char>(30000)=" << uchar_value << " / should be " << UCHAR_MAX
298 << std::endl;
299 return EXIT_FAILURE;
300 }
301
302 short_value = -30000;
303 uchar_value = vpMath::saturate<unsigned char>(short_value);
304 if (uchar_value != 0) {
305 std::cerr << "Fail: vpMath::saturate<unsigned char>(-30000)=" << uchar_value << " / should be 0" << std::endl;
306 return EXIT_FAILURE;
307 }
308
309 unsigned int uint_value = 10000;
310 uchar_value = vpMath::saturate<unsigned char>(uint_value);
311 if (uchar_value != UCHAR_MAX) {
312 std::cerr << "Fail: vpMath::saturate<unsigned char>(10000)=" << uchar_value << " / should be " << UCHAR_MAX
313 << std::endl;
314 return EXIT_FAILURE;
315 }
316
317 float float_value = 10000.1f;
318 uchar_value = vpMath::saturate<unsigned char>(float_value);
319 if (uchar_value != UCHAR_MAX) {
320 std::cerr << "Fail: vpMath::saturate<unsigned char>(10000.1f)=" << uchar_value << " / should be " << UCHAR_MAX
321 << std::endl;
322 return EXIT_FAILURE;
323 }
324
325 float_value = -10000.1f;
326 uchar_value = vpMath::saturate<unsigned char>(float_value);
327 if (uchar_value != 0) {
328 std::cerr << "Fail: vpMath::saturate<unsigned char>(-10000.1f)=" << uchar_value << " / should be 0" << std::endl;
329 return EXIT_FAILURE;
330 }
331
332 double double_value = 10000.1;
333 uchar_value = vpMath::saturate<unsigned char>(double_value);
334 if (uchar_value != UCHAR_MAX) {
335 std::cerr << "Fail: vpMath::saturate<unsigned char>(10000.0)=" << uchar_value << " / should be " << UCHAR_MAX
336 << std::endl;
337 return EXIT_FAILURE;
338 }
339
340 double_value = -10000.1;
341 uchar_value = vpMath::saturate<unsigned char>(double_value);
342 if (uchar_value != 0) {
343 std::cerr << "Fail: vpMath::saturate<unsigned char>(-10000.0)=" << uchar_value << " / should be 0" << std::endl;
344 return EXIT_FAILURE;
345 }
346 std::cout << "vpMath::saturate<unsigned char>() is Ok !" << std::endl;
347
348 // char
349 uchar_value = 255;
350 char_value = vpMath::saturate<char>(uchar_value);
351 if (char_value != SCHAR_MAX) {
352 std::cerr << "Fail: vpMath::saturate<char>(255)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
353 return EXIT_FAILURE;
354 }
355
356 ushort_value = 60000;
357 char_value = vpMath::saturate<char>(ushort_value);
358 if (char_value != SCHAR_MAX) {
359 std::cerr << "Fail: vpMath::saturate<char>(60000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
360 return EXIT_FAILURE;
361 }
362
363 int_value = 70000;
364 char_value = vpMath::saturate<char>(int_value);
365 if (char_value != SCHAR_MAX) {
366 std::cerr << "Fail: vpMath::saturate<char>(70000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
367 return EXIT_FAILURE;
368 }
369
370 int_value = -70000;
371 char_value = vpMath::saturate<char>(int_value);
372 if (char_value != (char)SCHAR_MIN) {
373 std::cerr << "Fail: vpMath::saturate<char>(-70000)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
374 return EXIT_FAILURE;
375 }
376
377 short_value = 30000;
378 char_value = vpMath::saturate<char>(short_value);
379 if (char_value != SCHAR_MAX) {
380 std::cerr << "Fail: vpMath::saturate<char>(30000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
381 return EXIT_FAILURE;
382 }
383
384 short_value = -30000;
385 char_value = vpMath::saturate<char>(short_value);
386 if (char_value != (char)SCHAR_MIN) {
387 std::cerr << "Fail: vpMath::saturate<char>(-30000)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
388 return EXIT_FAILURE;
389 }
390
391 uint_value = 10000;
392 char_value = vpMath::saturate<char>(uint_value);
393 if (char_value != SCHAR_MAX) {
394 std::cerr << "Fail: vpMath::saturate<char>(10000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
395 return EXIT_FAILURE;
396 }
397
398 float_value = 10000.1f;
399 char_value = vpMath::saturate<char>(float_value);
400 if (char_value != SCHAR_MAX) {
401 std::cerr << "Fail: vpMath::saturate<char>(10000.1f)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
402 return EXIT_FAILURE;
403 }
404
405 float_value = -10000.1f;
406 char_value = vpMath::saturate<char>(float_value);
407 if (char_value != (char)SCHAR_MIN) {
408 std::cerr << "Fail: vpMath::saturate<char>(-10000.1f)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
409 return EXIT_FAILURE;
410 }
411
412 double_value = 10000.1;
413 char_value = vpMath::saturate<char>(double_value);
414 if (char_value != SCHAR_MAX) {
415 std::cerr << "Fail: vpMath::saturate<char>(10000.1)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
416 return EXIT_FAILURE;
417 }
418
419 double_value = -10000.1;
420 char_value = vpMath::saturate<char>(double_value);
421 if (char_value != (char)SCHAR_MIN) {
422 std::cerr << "Fail: vpMath::saturate<char>(-10000.1)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
423 return EXIT_FAILURE;
424 }
425 std::cout << "vpMath::saturate<char>() is Ok !" << std::endl;
426
427 // unsigned short
428 char_value = -127;
429 ushort_value = vpMath::saturate<unsigned short>(char_value);
430 if (ushort_value != 0) {
431 std::cerr << "Fail: vpMath::saturate<unsigned short>(-127)=" << ushort_value << " / should be 0" << std::endl;
432 return EXIT_FAILURE;
433 }
434
435 short_value = -30000;
436 ushort_value = vpMath::saturate<unsigned short>(short_value);
437 if (ushort_value != 0) {
438 std::cerr << "Fail: vpMath::saturate<unsigned short>(-30000)=" << ushort_value << " / should be 0" << std::endl;
439 return EXIT_FAILURE;
440 }
441
442 int_value = 70000;
443 ushort_value = vpMath::saturate<unsigned short>(int_value);
444 if (ushort_value != USHRT_MAX) {
445 std::cerr << "Fail: vpMath::saturate<unsigned short>(70000)=" << ushort_value << " / should be " << USHRT_MAX
446 << std::endl;
447 return EXIT_FAILURE;
448 }
449
450 int_value = -70000;
451 ushort_value = vpMath::saturate<unsigned short>(int_value);
452 if (ushort_value != 0) {
453 std::cerr << "Fail: vpMath::saturate<unsigned short>(-70000)=" << ushort_value << " / should be 0" << std::endl;
454 return EXIT_FAILURE;
455 }
456
457 uint_value = 70000;
458 ushort_value = vpMath::saturate<unsigned short>(uint_value);
459 if (ushort_value != USHRT_MAX) {
460 std::cerr << "Fail: vpMath::saturate<unsigned short>(70000)=" << ushort_value << " / should be " << USHRT_MAX
461 << std::endl;
462 return EXIT_FAILURE;
463 }
464
465 float_value = 70000.1f;
466 ushort_value = vpMath::saturate<unsigned short>(float_value);
467 if (ushort_value != USHRT_MAX) {
468 std::cerr << "Fail: vpMath::saturate<unsigned short>(70000.1f)=" << ushort_value << " / should be " << USHRT_MAX
469 << std::endl;
470 return EXIT_FAILURE;
471 }
472
473 float_value = -10000.1f;
474 ushort_value = vpMath::saturate<unsigned short>(float_value);
475 if (ushort_value != 0) {
476 std::cerr << "Fail: vpMath::saturate<unsigned short>(-10000.1f)=" << ushort_value << " / should be 0" << std::endl;
477 return EXIT_FAILURE;
478 }
479
480 double_value = 70000.1;
481 ushort_value = vpMath::saturate<unsigned short>(double_value);
482 if (ushort_value != USHRT_MAX) {
483 std::cerr << "Fail: vpMath::saturate<unsigned short>(70000.1)=" << ushort_value << " / should be " << USHRT_MAX
484 << std::endl;
485 return EXIT_FAILURE;
486 }
487
488 double_value = -10000.1;
489 ushort_value = vpMath::saturate<unsigned short>(double_value);
490 if (ushort_value != 0) {
491 std::cerr << "Fail: vpMath::saturate<unsigned short>(-10000.1)=" << ushort_value << " / should be 0" << std::endl;
492 return EXIT_FAILURE;
493 }
494 std::cout << "vpMath::saturate<unsigned short>() is Ok !" << std::endl;
495
496 // short
497 ushort_value = 60000;
498 short_value = vpMath::saturate<short>(ushort_value);
499 if (short_value != SHRT_MAX) {
500 std::cerr << "Fail: vpMath::saturate<short>(60000)=" << short_value << " / should be " << SHRT_MAX << std::endl;
501 return EXIT_FAILURE;
502 }
503
504 int_value = 70000;
505 short_value = vpMath::saturate<short>(int_value);
506 if (short_value != SHRT_MAX) {
507 std::cerr << "Fail: vpMath::saturate<short>(70000)=" << short_value << " / should be " << SHRT_MAX << std::endl;
508 return EXIT_FAILURE;
509 }
510
511 int_value = -70000;
512 short_value = vpMath::saturate<short>(int_value);
513 if (short_value != SHRT_MIN) {
514 std::cerr << "Fail: vpMath::saturate<short>(-70000)=" << short_value << " / should be " << SHRT_MIN << std::endl;
515 return EXIT_FAILURE;
516 }
517
518 uint_value = 70000;
519 short_value = vpMath::saturate<short>(uint_value);
520 if (short_value != SHRT_MAX) {
521 std::cerr << "Fail: vpMath::saturate<short>(70000)=" << short_value << " / should be " << SHRT_MAX << std::endl;
522 return EXIT_FAILURE;
523 }
524
525 float_value = 70000.1f;
526 short_value = vpMath::saturate<short>(float_value);
527 if (short_value != SHRT_MAX) {
528 std::cerr << "Fail: vpMath::saturate<short>(70000.1f)=" << short_value << " / should be " << SHRT_MAX << std::endl;
529 return EXIT_FAILURE;
530 }
531
532 float_value = -70000.1f;
533 short_value = vpMath::saturate<short>(float_value);
534 if (short_value != SHRT_MIN) {
535 std::cerr << "Fail: vpMath::saturate<short>(-70000.1f)=" << short_value << " / should be " << SHRT_MIN << std::endl;
536 return EXIT_FAILURE;
537 }
538
539 double_value = 70000.1;
540 short_value = vpMath::saturate<short>(double_value);
541 if (short_value != SHRT_MAX) {
542 std::cerr << "Fail: vpMath::saturate<short>(70000.1)=" << short_value << " / should be " << SHRT_MAX << std::endl;
543 return EXIT_FAILURE;
544 }
545
546 double_value = -70000.1;
547 short_value = vpMath::saturate<short>(double_value);
548 if (short_value != SHRT_MIN) {
549 std::cerr << "Fail: vpMath::saturate<short>(70000.1)=" << short_value << " / should be " << SHRT_MIN << std::endl;
550 return EXIT_FAILURE;
551 }
552 std::cout << "vpMath::saturate<short>() is Ok !" << std::endl;
553
554 // Test mean, median and standard deviation against Matlab with rng(0) and
555 // rand(10,1)*10
556 std::vector<double> vectorOfDoubles(10);
557 vectorOfDoubles[0] = 8.1472;
558 vectorOfDoubles[1] = 9.0579;
559 vectorOfDoubles[2] = 1.2699;
560 vectorOfDoubles[3] = 9.1338;
561 vectorOfDoubles[4] = 6.3236;
562 vectorOfDoubles[5] = 0.9754;
563 vectorOfDoubles[6] = 2.7850;
564 vectorOfDoubles[7] = 5.4688;
565 vectorOfDoubles[8] = 9.5751;
566 vectorOfDoubles[9] = 9.6489;
567
568 double res = vpMath::getMean(vectorOfDoubles);
569 if (!vpMath::equal(res, 6.2386, 0.001)) {
570 std::cerr << "Problem with vpMath::getMean()=" << res << std::endl;
571 return EXIT_FAILURE;
572 }
573 std::cout << "vpMath::getMean() is Ok !" << std::endl;
574
575 res = vpMath::getStdev(vectorOfDoubles);
576 if (!vpMath::equal(res, 3.2810, 0.001)) {
577 std::cerr << "Problem with vpMath::getStdev()=" << res << std::endl;
578 return EXIT_FAILURE;
579 }
580
581 res = vpMath::getStdev(vectorOfDoubles, true);
582 if (!vpMath::equal(res, 3.4585, 0.001)) {
583 std::cerr << "Problem with vpMath::getStdev() with Bessel correction=" << res << std::endl;
584 return EXIT_FAILURE;
585 }
586 std::cout << "vpMath::getStdev() is Ok !" << std::endl;
587
588 res = vpMath::getMedian(vectorOfDoubles);
589 if (!vpMath::equal(res, 7.2354, 0.001)) {
590 std::cerr << "Problem with vpMath::getMedian()=" << res << std::endl;
591 return EXIT_FAILURE;
592 }
593
594 // Test median with odd number of elements
595 vectorOfDoubles.push_back(1.5761);
596 res = vpMath::getMedian(vectorOfDoubles);
597 if (!vpMath::equal(res, 6.3236, 0.001)) {
598 std::cerr << "Problem with vpMath::getMedian()=" << res << std::endl;
599 return EXIT_FAILURE;
600 }
601 std::cout << "vpMath::getMedian() is Ok !" << std::endl;
602
603 // Test clamp
604 {
605#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
606 const double lower { -10. }, upper { +10. };
607 std::vector<double> testing_values{5., -15., 15.};
608 std::vector<double> expected_values{5., -10., 10.};
609#else
610 const double lower = -10., upper = +10.;
611 std::vector<double> testing_values;
612 testing_values.push_back(5.);
613 testing_values.push_back(-15.);
614 testing_values.push_back(15.);
615 std::vector<double> expected_values;
616 expected_values.push_back(5.);
617 expected_values.push_back(-10.);
618 expected_values.push_back(10.);
619#endif
620
621 for (size_t i = 0u; i < testing_values.size(); i++) {
622 const double clamp_val = vpMath::clamp(testing_values.at(i), lower, upper);
623 if (!vpMath::equal(clamp_val, expected_values.at(i), 0.001)) {
624 std::cerr << "Problem with vpMath::clamp()=" << clamp_val << std::endl;
625 return EXIT_FAILURE;
626 }
627 }
628 std::cout << "vpMath::clamp() is Ok !" << std::endl;
629 }
630
631 // Test vpMath::deg() and vpMath::rad()
632 {
633 vpColVector rxyz_deg_truth(3);
634 rxyz_deg_truth[0] = 10;
635 rxyz_deg_truth[1] = 20;
636 rxyz_deg_truth[2] = -30;
637 vpColVector rxyz_rad_truth(3);
638 rxyz_rad_truth[0] = vpMath::rad(rxyz_deg_truth[0]);
639 rxyz_rad_truth[1] = vpMath::rad(rxyz_deg_truth[1]);
640 rxyz_rad_truth[2] = vpMath::rad(rxyz_deg_truth[2]);
641
642 {
643 vpRxyzVector rxyz(rxyz_rad_truth);
644 vpColVector rxyz_deg = vpMath::deg(rxyz);
645 for (unsigned int i = 0u; i < rxyz_deg_truth.size(); i++) {
646 if (!vpMath::equal(rxyz_deg[i], rxyz_deg_truth[i], 0.001)) {
647 std::cerr << "Problem with vpMath::deg(vpRotationVector) " << i << ": " << rxyz_deg[i] << std::endl;
648 return EXIT_FAILURE;
649 }
650 }
651 std::cout << "vpMath::deg(vpRxyzVector) is Ok !" << std::endl;
652 }
653 {
654 vpColVector rxyz_deg = vpMath::deg(rxyz_rad_truth);
655 for (unsigned int i = 0u; i < rxyz_deg_truth.size(); i++) {
656 if (!vpMath::equal(rxyz_deg[i], rxyz_deg_truth[i], 0.001)) {
657 std::cerr << "Problem with vpMath::deg(vpColVector) " << i << ": " << rxyz_deg[i] << std::endl;
658 return EXIT_FAILURE;
659 }
660 }
661 std::cout << "vpMath::deg(vpColVector) is Ok !" << std::endl;
662 }
663 {
664 vpColVector rxyz_rad = vpMath::rad(rxyz_deg_truth);
665 for (unsigned int i = 0u; i < rxyz_deg_truth.size(); i++) {
666 if (!vpMath::equal(rxyz_rad[i], rxyz_rad_truth[i], 0.001)) {
667 std::cerr << "Problem with vpMath::rad(vpColVector) " << i << ": " << rxyz_rad[i] << std::endl;
668 return EXIT_FAILURE;
669 }
670 }
671 std::cout << "vpMath::rad(vpColVector) is Ok !" << std::endl;
672 }
673 }
674
675 std::cout << "Test succeed" << std::endl;
676 return EXIT_SUCCESS;
677}
Implementation of column vector and the associated operations.
static bool isNaN(double value)
Definition vpMath.cpp:93
static double rad(double deg)
Definition vpMath.h:116
static double getMedian(const std::vector< double > &v)
Definition vpMath.cpp:314
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition vpMath.cpp:345
static bool equal(double x, double y, double threshold=0.001)
Definition vpMath.h:369
static T clamp(const T &v, const T &lower, const T &upper)
Definition vpMath.h:139
static int round(double x)
Definition vpMath.h:323
static bool isFinite(double value)
Definition vpMath.cpp:178
static double getMean(const std::vector< double > &v)
Definition vpMath.cpp:294
static bool isInf(double value)
Definition vpMath.cpp:137
static double deg(double rad)
Definition vpMath.h:106
static bool isNumber(const std::string &str)
Definition vpMath.cpp:215
static _Tp saturate(unsigned char v)
Definition vpMath.h:221
Implementation of a rotation vector as Euler angle minimal representation.