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