Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
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
62 static const unsigned long __nan[2] = { 0xffffffff, 0x7fffffff };
63 #define NAN (*(const float *)__nan)
64 #endif
65 #endif
66 
67 int 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 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
605  {
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 
610  for (size_t i = 0u; i < testing_values.size(); i++) {
611  const double clamp_val = vpMath::clamp(testing_values.at(i), lower, upper);
612  if (!vpMath::equal(clamp_val, expected_values.at(i), 0.001)) {
613  std::cerr << "Problem with vpMath::clamp()=" << clamp_val << std::endl;
614  return EXIT_FAILURE;
615  }
616  }
617  std::cout << "vpMath::clamp() is Ok !" << std::endl;
618  }
619 #endif
620 
621  // Test vpMath::deg() and vpMath::rad()
622  {
623  vpColVector rxyz_deg_truth(3);
624  rxyz_deg_truth[0] = 10;
625  rxyz_deg_truth[1] = 20;
626  rxyz_deg_truth[2] = -30;
627  vpColVector rxyz_rad_truth(3);
628  rxyz_rad_truth[0] = vpMath::rad(rxyz_deg_truth[0]);
629  rxyz_rad_truth[1] = vpMath::rad(rxyz_deg_truth[1]);
630  rxyz_rad_truth[2] = vpMath::rad(rxyz_deg_truth[2]);
631 
632  {
633  vpRxyzVector rxyz(rxyz_rad_truth);
634  vpColVector rxyz_deg = vpMath::deg(rxyz);
635  for (unsigned int i = 0u; i < rxyz_deg_truth.size(); i++) {
636  if (!vpMath::equal(rxyz_deg[i], rxyz_deg_truth[i], 0.001)) {
637  std::cerr << "Problem with vpMath::deg(vpRotationVector) " << i << ": " << rxyz_deg[i] << std::endl;
638  return EXIT_FAILURE;
639  }
640  }
641  std::cout << "vpMath::deg(vpRxyzVector) is Ok !" << std::endl;
642  }
643  {
644  vpColVector rxyz_deg = vpMath::deg(rxyz_rad_truth);
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(vpColVector) " << i << ": " << rxyz_deg[i] << std::endl;
648  return EXIT_FAILURE;
649  }
650  }
651  std::cout << "vpMath::deg(vpColVector) is Ok !" << std::endl;
652  }
653  {
654  vpColVector rxyz_rad = vpMath::rad(rxyz_deg_truth);
655  for (unsigned int i = 0u; i < rxyz_deg_truth.size(); i++) {
656  if (!vpMath::equal(rxyz_rad[i], rxyz_rad_truth[i], 0.001)) {
657  std::cerr << "Problem with vpMath::rad(vpColVector) " << i << ": " << rxyz_rad[i] << std::endl;
658  return EXIT_FAILURE;
659  }
660  }
661  std::cout << "vpMath::rad(vpColVector) is Ok !" << std::endl;
662  }
663  }
664 
665  std::cout << "Test succeed" << std::endl;
666  return EXIT_SUCCESS;
667 }
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
static bool isNaN(double value)
Definition: vpMath.cpp:93
static double rad(double deg)
Definition: vpMath.h:127
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:449
static T clamp(const T &v, const T &lower, const T &upper)
Definition: vpMath.h:216
static int round(double x)
Definition: vpMath.h:403
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:117
static bool isNumber(const std::string &str)
Definition: vpMath.cpp:215
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:176