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