Visual Servoing Platform version 3.5.0
testMath.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 vpColVector functionalities.
33 *
34 * Authors:
35 * Souriya Trinh
36 *
37 *****************************************************************************/
38
45#include <cfloat>
46#include <iostream>
47#include <limits>
48
49#include <visp3/core/vpMath.h>
50
51#if defined _MSC_VER && _MSC_VER >= 1200
52#pragma warning(disable : 4723)
53
54// 4723 : potential divide by 0
55#endif
56
57#ifdef WIN32
58#ifndef NAN
59// https://msdn.microsoft.com/en-us/library/w22adx1s%28v=vs.120%29.aspx
60// http://tdistler.com/2011/03/24/how-to-define-nan-not-a-number-on-windows
61static const unsigned long __nan[2] = {0xffffffff, 0x7fffffff};
62#define NAN (*(const float *)__nan)
63#endif
64#endif
65
66int main()
67{
68 // Test isNaN
69 if (vpMath::isNaN(0.0)) {
70 std::cerr << "Fail: IsNaN(0.0)=" << vpMath::isNaN(0.0) << " / should be false" << std::endl;
71 return -1;
72 }
73
74 double num = 1.0, den = 0.0;
75 if (vpMath::isNaN(num / den)) {
76 std::cerr << "Fail: IsNaN(1.0/0.0)=" << vpMath::isNaN(num / den) << " / should be false" << std::endl;
77 return -1;
78 }
79
80 if (!vpMath::isNaN(NAN)) {
81 std::cerr << "Fail: IsNaN(NAN)=" << vpMath::isNaN(NAN) << " / should be true" << std::endl;
82 return -1;
83 }
84
85 num = 0.0;
86 if (!vpMath::isNaN(num / den)) {
87 std::cerr << "Fail: IsNaN(0.0/0.0)=" << vpMath::isNaN(num / den) << " / should be true" << std::endl;
88 return -1;
89 }
90
91 if (!vpMath::isNaN(std::numeric_limits<double>::quiet_NaN())) {
92 std::cerr << "Fail: IsNaN(quiet_NaN)=" << vpMath::isNaN(std::numeric_limits<double>::quiet_NaN())
93 << " / should be true" << std::endl;
94 return -1;
95 }
96
97 if (!vpMath::isNaN(std::numeric_limits<float>::quiet_NaN())) {
98 std::cerr << "Fail: IsNaN(quiet_NaN)=" << vpMath::isNaN(std::numeric_limits<float>::quiet_NaN())
99 << " / should be true" << std::endl;
100 return -1;
101 }
102
103 if (!vpMath::isNaN(std::numeric_limits<double>::signaling_NaN())) {
104 std::cerr << "Fail: IsNaN(signaling_NaN)=" << vpMath::isNaN(std::numeric_limits<double>::signaling_NaN())
105 << " / should be true" << std::endl;
106 return -1;
107 }
108
109 if (!vpMath::isNaN(std::numeric_limits<float>::signaling_NaN())) {
110 std::cerr << "Fail: IsNaN(signaling_NaN)=" << vpMath::isNaN(std::numeric_limits<float>::signaling_NaN())
111 << " / should be true" << std::endl;
112 return -1;
113 }
114
115 if (vpMath::isNaN(std::numeric_limits<double>::infinity())) {
116 std::cerr << "Fail: IsNaN(infinity)=" << vpMath::isNaN(std::numeric_limits<double>::infinity())
117 << " / should be false" << std::endl;
118 return -1;
119 }
120
121 if (vpMath::isNaN(std::numeric_limits<float>::infinity())) {
122 std::cerr << "Fail: IsNaN(infinity)=" << vpMath::isNaN(std::numeric_limits<float>::infinity())
123 << " / should be false" << std::endl;
124 return -1;
125 }
126
127 if (vpMath::isNaN(1.0 / std::numeric_limits<double>::epsilon())) {
128 std::cerr << "Fail: IsNaN(1.0/epsilon)=" << vpMath::isNaN(1.0 / std::numeric_limits<double>::epsilon())
129 << " / should be false" << std::endl;
130 return -1;
131 }
132
133 if (!vpMath::isNaN(std::numeric_limits<double>::infinity() - std::numeric_limits<double>::infinity())) {
134 std::cerr << "Fail: IsNaN(infinity - infinity)="
135 << vpMath::isNaN(std::numeric_limits<double>::infinity() - std::numeric_limits<double>::infinity())
136 << " / should be true" << std::endl;
137 return -1;
138 }
139
140 float a = 0.0f, b = 0.0f;
141 if (!vpMath::isNaN(a / b)) {
142 std::cerr << "Fail: IsNaN(0.0f/0.0f)=" << vpMath::isNaN(a / b) << " / should be true" << std::endl;
143 return -1;
144 }
145 std::cout << "vpMath::isNaN is Ok !" << std::endl;
146
147// Test isInf
148#if !defined(VISP_HAVE_FUNC__FINITE)
149 // Disable this test if using _finite as (!_finite(NAN)) returns true
150 // whereas isinf(NAN) returns false
151 if (vpMath::isInf(NAN)) {
152 std::cerr << "Fail: vpMath::isInf(NAN)=" << vpMath::isInf(NAN) << " / should be false" << std::endl;
153 return -1;
154 }
155#endif
156
157 if (!vpMath::isInf(1.0 / a)) {
158 std::cerr << "Fail: vpMath::isInf(1.0/0.0)=" << vpMath::isInf(1.0 / a) << " / should be true" << std::endl;
159 return -1;
160 }
161
162 if (vpMath::isInf(0.0)) {
163 std::cerr << "Fail: vpMath::isInf(0.0)=" << vpMath::isInf(0.0) << " / should be false" << std::endl;
164 return -1;
165 }
166
167 if (!vpMath::isInf(exp(800.))) {
168 std::cerr << "Fail: vpMath::isInf(exp(800.))=" << vpMath::isInf(exp(800.)) << " / should be true" << std::endl;
169 return -1;
170 }
171
172 if (vpMath::isInf(DBL_MIN / 2.0)) {
173 std::cerr << "Fail: vpMath::isInf(DBL_MIN/2.0)=" << vpMath::isInf(DBL_MIN / 2.0) << " / should be false"
174 << std::endl;
175 return -1;
176 }
177 std::cout << "vpMath::isInf is Ok !" << std::endl;
178
179 // Test round
180 if (vpMath::round(2.3) != 2) {
181 std::cerr << "Fail: vpMath::round(2.3)=" << vpMath::round(2.3) << " / should be 2" << std::endl;
182 return -1;
183 }
184
185 if (vpMath::round(3.8) != 4) {
186 std::cerr << "Fail: vpMath::round(3.8)=" << vpMath::round(3.8) << " / should be 4" << std::endl;
187 return -1;
188 }
189
190 if (vpMath::round(5.5) != 6) {
191 std::cerr << "Fail: vpMath::round(5.5)=" << vpMath::round(5.5) << " / should be 6" << std::endl;
192 return -1;
193 }
194
195 if (vpMath::round(-2.3) != -2) {
196 std::cerr << "Fail: vpMath::round(-2.3)=" << vpMath::round(-2.3) << " / should be -2" << std::endl;
197 return -1;
198 }
199
200 if (vpMath::round(-3.8) != -4) {
201 std::cerr << "Fail: vpMath::round(-3.8)=" << vpMath::round(-3.8) << " / should be -4" << std::endl;
202 return -1;
203 }
204
205 if (vpMath::round(-5.5) != -6) {
206 std::cerr << "Fail: vpMath::round(-5.5)=" << vpMath::round(-5.5) << " / should be -6" << std::endl;
207 return -1;
208 }
209
210 if (vpMath::round(0.0) != 0) {
211 std::cerr << "Fail: vpMath::round(0.0)=" << vpMath::round(0.0) << " / should be 0" << std::endl;
212 return -1;
213 }
214 std::cout << "vpMath::round is Ok !" << std::endl;
215
216 // Test saturate functions
217 // unsigned char
218 char char_value = -127;
219 unsigned char uchar_value = vpMath::saturate<unsigned char>(char_value);
220 if (uchar_value != 0) {
221 std::cerr << "Fail: vpMath::saturate<unsigned char>(-127)=" << uchar_value << " / should be 0" << std::endl;
222 return -1;
223 }
224
225 unsigned short ushort_value = 60000;
226 uchar_value = vpMath::saturate<unsigned char>(ushort_value);
227 if (uchar_value != UCHAR_MAX) {
228 std::cerr << "Fail: vpMath::saturate<unsigned char>(60000)=" << uchar_value << " / should be " << UCHAR_MAX
229 << std::endl;
230 return -1;
231 }
232
233 int int_value = 70000;
234 uchar_value = vpMath::saturate<unsigned char>(int_value);
235 if (uchar_value != UCHAR_MAX) {
236 std::cerr << "Fail: vpMath::saturate<unsigned char>(70000)=" << uchar_value << " / should be " << UCHAR_MAX
237 << std::endl;
238 return -1;
239 }
240
241 int_value = -70000;
242 uchar_value = vpMath::saturate<unsigned char>(int_value);
243 if (uchar_value != 0) {
244 std::cerr << "Fail: vpMath::saturate<unsigned char>(-70000)=" << uchar_value << " / should be 0" << std::endl;
245 return -1;
246 }
247
248 short short_value = 30000;
249 uchar_value = vpMath::saturate<unsigned char>(short_value);
250 if (uchar_value != UCHAR_MAX) {
251 std::cerr << "Fail: vpMath::saturate<unsigned char>(30000)=" << uchar_value << " / should be " << UCHAR_MAX
252 << std::endl;
253 return -1;
254 }
255
256 short_value = -30000;
257 uchar_value = vpMath::saturate<unsigned char>(short_value);
258 if (uchar_value != 0) {
259 std::cerr << "Fail: vpMath::saturate<unsigned char>(-30000)=" << uchar_value << " / should be 0" << std::endl;
260 return -1;
261 }
262
263 unsigned int uint_value = 10000;
264 uchar_value = vpMath::saturate<unsigned char>(uint_value);
265 if (uchar_value != UCHAR_MAX) {
266 std::cerr << "Fail: vpMath::saturate<unsigned char>(10000)=" << uchar_value << " / should be " << UCHAR_MAX
267 << std::endl;
268 return -1;
269 }
270
271 float float_value = 10000.1f;
272 uchar_value = vpMath::saturate<unsigned char>(float_value);
273 if (uchar_value != UCHAR_MAX) {
274 std::cerr << "Fail: vpMath::saturate<unsigned char>(10000.1f)=" << uchar_value << " / should be " << UCHAR_MAX
275 << std::endl;
276 return -1;
277 }
278
279 float_value = -10000.1f;
280 uchar_value = vpMath::saturate<unsigned char>(float_value);
281 if (uchar_value != 0) {
282 std::cerr << "Fail: vpMath::saturate<unsigned char>(-10000.1f)=" << uchar_value << " / should be 0" << std::endl;
283 return -1;
284 }
285
286 double double_value = 10000.1;
287 uchar_value = vpMath::saturate<unsigned char>(double_value);
288 if (uchar_value != UCHAR_MAX) {
289 std::cerr << "Fail: vpMath::saturate<unsigned char>(10000.0)=" << uchar_value << " / should be " << UCHAR_MAX
290 << std::endl;
291 return -1;
292 }
293
294 double_value = -10000.1;
295 uchar_value = vpMath::saturate<unsigned char>(double_value);
296 if (uchar_value != 0) {
297 std::cerr << "Fail: vpMath::saturate<unsigned char>(-10000.0)=" << uchar_value << " / should be 0" << std::endl;
298 return -1;
299 }
300 std::cout << "vpMath::saturate<unsigned char>() is Ok !" << std::endl;
301
302 // char
303 uchar_value = 255;
304 char_value = vpMath::saturate<char>(uchar_value);
305 if (char_value != SCHAR_MAX) {
306 std::cerr << "Fail: vpMath::saturate<char>(255)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
307 return -1;
308 }
309
310 ushort_value = 60000;
311 char_value = vpMath::saturate<char>(ushort_value);
312 if (char_value != SCHAR_MAX) {
313 std::cerr << "Fail: vpMath::saturate<char>(60000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
314 return -1;
315 }
316
317 int_value = 70000;
318 char_value = vpMath::saturate<char>(int_value);
319 if (char_value != SCHAR_MAX) {
320 std::cerr << "Fail: vpMath::saturate<char>(70000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
321 return -1;
322 }
323
324 int_value = -70000;
325 char_value = vpMath::saturate<char>(int_value);
326 if (char_value != (char)SCHAR_MIN) {
327 std::cerr << "Fail: vpMath::saturate<char>(-70000)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
328 return -1;
329 }
330
331 short_value = 30000;
332 char_value = vpMath::saturate<char>(short_value);
333 if (char_value != SCHAR_MAX) {
334 std::cerr << "Fail: vpMath::saturate<char>(30000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
335 return -1;
336 }
337
338 short_value = -30000;
339 char_value = vpMath::saturate<char>(short_value);
340 if (char_value != (char)SCHAR_MIN) {
341 std::cerr << "Fail: vpMath::saturate<char>(-30000)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
342 return -1;
343 }
344
345 uint_value = 10000;
346 char_value = vpMath::saturate<char>(uint_value);
347 if (char_value != SCHAR_MAX) {
348 std::cerr << "Fail: vpMath::saturate<char>(10000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
349 return -1;
350 }
351
352 float_value = 10000.1f;
353 char_value = vpMath::saturate<char>(float_value);
354 if (char_value != SCHAR_MAX) {
355 std::cerr << "Fail: vpMath::saturate<char>(10000.1f)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
356 return -1;
357 }
358
359 float_value = -10000.1f;
360 char_value = vpMath::saturate<char>(float_value);
361 if (char_value != (char)SCHAR_MIN) {
362 std::cerr << "Fail: vpMath::saturate<char>(-10000.1f)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
363 return -1;
364 }
365
366 double_value = 10000.1;
367 char_value = vpMath::saturate<char>(double_value);
368 if (char_value != SCHAR_MAX) {
369 std::cerr << "Fail: vpMath::saturate<char>(10000.1)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
370 return -1;
371 }
372
373 double_value = -10000.1;
374 char_value = vpMath::saturate<char>(double_value);
375 if (char_value != (char)SCHAR_MIN) {
376 std::cerr << "Fail: vpMath::saturate<char>(-10000.1)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
377 return -1;
378 }
379 std::cout << "vpMath::saturate<char>() is Ok !" << std::endl;
380
381 // unsigned short
382 char_value = -127;
383 ushort_value = vpMath::saturate<unsigned short>(char_value);
384 if (ushort_value != 0) {
385 std::cerr << "Fail: vpMath::saturate<unsigned short>(-127)=" << ushort_value << " / should be 0" << std::endl;
386 return -1;
387 }
388
389 short_value = -30000;
390 ushort_value = vpMath::saturate<unsigned short>(short_value);
391 if (ushort_value != 0) {
392 std::cerr << "Fail: vpMath::saturate<unsigned short>(-30000)=" << ushort_value << " / should be 0" << std::endl;
393 return -1;
394 }
395
396 int_value = 70000;
397 ushort_value = vpMath::saturate<unsigned short>(int_value);
398 if (ushort_value != USHRT_MAX) {
399 std::cerr << "Fail: vpMath::saturate<unsigned short>(70000)=" << ushort_value << " / should be " << USHRT_MAX
400 << std::endl;
401 return -1;
402 }
403
404 int_value = -70000;
405 ushort_value = vpMath::saturate<unsigned short>(int_value);
406 if (ushort_value != 0) {
407 std::cerr << "Fail: vpMath::saturate<unsigned short>(-70000)=" << ushort_value << " / should be 0" << std::endl;
408 return -1;
409 }
410
411 uint_value = 70000;
412 ushort_value = vpMath::saturate<unsigned short>(uint_value);
413 if (ushort_value != USHRT_MAX) {
414 std::cerr << "Fail: vpMath::saturate<unsigned short>(70000)=" << ushort_value << " / should be " << USHRT_MAX
415 << std::endl;
416 return -1;
417 }
418
419 float_value = 70000.1f;
420 ushort_value = vpMath::saturate<unsigned short>(float_value);
421 if (ushort_value != USHRT_MAX) {
422 std::cerr << "Fail: vpMath::saturate<unsigned short>(70000.1f)=" << ushort_value << " / should be " << USHRT_MAX
423 << std::endl;
424 return -1;
425 }
426
427 float_value = -10000.1f;
428 ushort_value = vpMath::saturate<unsigned short>(float_value);
429 if (ushort_value != 0) {
430 std::cerr << "Fail: vpMath::saturate<unsigned short>(-10000.1f)=" << ushort_value << " / should be 0" << std::endl;
431 return -1;
432 }
433
434 double_value = 70000.1;
435 ushort_value = vpMath::saturate<unsigned short>(double_value);
436 if (ushort_value != USHRT_MAX) {
437 std::cerr << "Fail: vpMath::saturate<unsigned short>(70000.1)=" << ushort_value << " / should be " << USHRT_MAX
438 << std::endl;
439 return -1;
440 }
441
442 double_value = -10000.1;
443 ushort_value = vpMath::saturate<unsigned short>(double_value);
444 if (ushort_value != 0) {
445 std::cerr << "Fail: vpMath::saturate<unsigned short>(-10000.1)=" << ushort_value << " / should be 0" << std::endl;
446 return -1;
447 }
448 std::cout << "vpMath::saturate<unsigned short>() is Ok !" << std::endl;
449
450 // short
451 ushort_value = 60000;
452 short_value = vpMath::saturate<short>(ushort_value);
453 if (short_value != SHRT_MAX) {
454 std::cerr << "Fail: vpMath::saturate<short>(60000)=" << short_value << " / should be " << SHRT_MAX << std::endl;
455 return -1;
456 }
457
458 int_value = 70000;
459 short_value = vpMath::saturate<short>(int_value);
460 if (short_value != SHRT_MAX) {
461 std::cerr << "Fail: vpMath::saturate<short>(70000)=" << short_value << " / should be " << SHRT_MAX << std::endl;
462 return -1;
463 }
464
465 int_value = -70000;
466 short_value = vpMath::saturate<short>(int_value);
467 if (short_value != SHRT_MIN) {
468 std::cerr << "Fail: vpMath::saturate<short>(-70000)=" << short_value << " / should be " << SHRT_MIN << std::endl;
469 return -1;
470 }
471
472 uint_value = 70000;
473 short_value = vpMath::saturate<short>(uint_value);
474 if (short_value != SHRT_MAX) {
475 std::cerr << "Fail: vpMath::saturate<short>(70000)=" << short_value << " / should be " << SHRT_MAX << std::endl;
476 return -1;
477 }
478
479 float_value = 70000.1f;
480 short_value = vpMath::saturate<short>(float_value);
481 if (short_value != SHRT_MAX) {
482 std::cerr << "Fail: vpMath::saturate<short>(70000.1f)=" << short_value << " / should be " << SHRT_MAX << std::endl;
483 return -1;
484 }
485
486 float_value = -70000.1f;
487 short_value = vpMath::saturate<short>(float_value);
488 if (short_value != SHRT_MIN) {
489 std::cerr << "Fail: vpMath::saturate<short>(-70000.1f)=" << short_value << " / should be " << SHRT_MIN << std::endl;
490 return -1;
491 }
492
493 double_value = 70000.1;
494 short_value = vpMath::saturate<short>(double_value);
495 if (short_value != SHRT_MAX) {
496 std::cerr << "Fail: vpMath::saturate<short>(70000.1)=" << short_value << " / should be " << SHRT_MAX << std::endl;
497 return -1;
498 }
499
500 double_value = -70000.1;
501 short_value = vpMath::saturate<short>(double_value);
502 if (short_value != SHRT_MIN) {
503 std::cerr << "Fail: vpMath::saturate<short>(70000.1)=" << short_value << " / should be " << SHRT_MIN << std::endl;
504 return -1;
505 }
506 std::cout << "vpMath::saturate<short>() is Ok !" << std::endl;
507
508 // Test mean, median and standard deviation against Matlab with rng(0) and
509 // rand(10,1)*10
510 std::vector<double> vectorOfDoubles(10);
511 vectorOfDoubles[0] = 8.1472;
512 vectorOfDoubles[1] = 9.0579;
513 vectorOfDoubles[2] = 1.2699;
514 vectorOfDoubles[3] = 9.1338;
515 vectorOfDoubles[4] = 6.3236;
516 vectorOfDoubles[5] = 0.9754;
517 vectorOfDoubles[6] = 2.7850;
518 vectorOfDoubles[7] = 5.4688;
519 vectorOfDoubles[8] = 9.5751;
520 vectorOfDoubles[9] = 9.6489;
521
522 double res = vpMath::getMean(vectorOfDoubles);
523 if (!vpMath::equal(res, 6.2386, 0.001)) {
524 std::cerr << "Problem with vpMath::getMean()=" << res << std::endl;
525 return -1;
526 }
527 std::cout << "vpMath::getMean() is Ok !" << std::endl;
528
529 res = vpMath::getStdev(vectorOfDoubles);
530 if (!vpMath::equal(res, 3.2810, 0.001)) {
531 std::cerr << "Problem with vpMath::getStdev()=" << res << std::endl;
532 return -1;
533 }
534
535 res = vpMath::getStdev(vectorOfDoubles, true);
536 if (!vpMath::equal(res, 3.4585, 0.001)) {
537 std::cerr << "Problem with vpMath::getStdev() with Bessel correction=" << res << std::endl;
538 return -1;
539 }
540 std::cout << "vpMath::getStdev() is Ok !" << std::endl;
541
542 res = vpMath::getMedian(vectorOfDoubles);
543 if (!vpMath::equal(res, 7.2354, 0.001)) {
544 std::cerr << "Problem with vpMath::getMedian()=" << res << std::endl;
545 return -1;
546 }
547
548 // Test median with odd number of elements
549 vectorOfDoubles.push_back(1.5761);
550 res = vpMath::getMedian(vectorOfDoubles);
551 if (!vpMath::equal(res, 6.3236, 0.001)) {
552 std::cerr << "Problem with vpMath::getMedian()=" << res << std::endl;
553 return -1;
554 }
555 std::cout << "vpMath::getMedian() is Ok !" << std::endl;
556
557 std::cout << "OK !" << std::endl;
558 return 0;
559}
static bool isNaN(double value)
Definition: vpMath.cpp:85
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:261
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:291
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:295
static int round(double x)
Definition: vpMath.h:247
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:241
static bool isInf(double value)
Definition: vpMath.cpp:129