Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
testMath.cpp

Test some vpMath functionalities.

/****************************************************************************
*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2019 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
* See http://visp.inria.fr for more information.
*
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
*
* If you have questions regarding the use of this file, please contact
* Inria at visp@inria.fr
*
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
* Test some vpColVector functionalities.
*
* Authors:
* Souriya Trinh
*
*****************************************************************************/
#include <cfloat>
#include <iostream>
#include <limits>
#include <visp3/core/vpMath.h>
#if defined _MSC_VER && _MSC_VER >= 1200
#pragma warning(disable : 4723)
// 4723 : potential divide by 0
#endif
#ifdef WIN32
#ifndef NAN
// https://msdn.microsoft.com/en-us/library/w22adx1s%28v=vs.120%29.aspx
// http://tdistler.com/2011/03/24/how-to-define-nan-not-a-number-on-windows
static const unsigned long __nan[2] = {0xffffffff, 0x7fffffff};
#define NAN (*(const float *)__nan)
#endif
#endif
int main()
{
// Test isNaN
if (vpMath::isNaN(0.0)) {
std::cerr << "Fail: IsNaN(0.0)=" << vpMath::isNaN(0.0) << " / should be false" << std::endl;
return -1;
}
double num = 1.0, den = 0.0;
if (vpMath::isNaN(num / den)) {
std::cerr << "Fail: IsNaN(1.0/0.0)=" << vpMath::isNaN(num / den) << " / should be false" << std::endl;
return -1;
}
if (!vpMath::isNaN(NAN)) {
std::cerr << "Fail: IsNaN(NAN)=" << vpMath::isNaN(NAN) << " / should be true" << std::endl;
return -1;
}
num = 0.0;
if (!vpMath::isNaN(num / den)) {
std::cerr << "Fail: IsNaN(0.0/0.0)=" << vpMath::isNaN(num / den) << " / should be true" << std::endl;
return -1;
}
if (!vpMath::isNaN(std::numeric_limits<double>::quiet_NaN())) {
std::cerr << "Fail: IsNaN(quiet_NaN)=" << vpMath::isNaN(std::numeric_limits<double>::quiet_NaN())
<< " / should be true" << std::endl;
return -1;
}
if (!vpMath::isNaN(std::numeric_limits<float>::quiet_NaN())) {
std::cerr << "Fail: IsNaN(quiet_NaN)=" << vpMath::isNaN(std::numeric_limits<float>::quiet_NaN())
<< " / should be true" << std::endl;
return -1;
}
if (!vpMath::isNaN(std::numeric_limits<double>::signaling_NaN())) {
std::cerr << "Fail: IsNaN(signaling_NaN)=" << vpMath::isNaN(std::numeric_limits<double>::signaling_NaN())
<< " / should be true" << std::endl;
return -1;
}
if (!vpMath::isNaN(std::numeric_limits<float>::signaling_NaN())) {
std::cerr << "Fail: IsNaN(signaling_NaN)=" << vpMath::isNaN(std::numeric_limits<float>::signaling_NaN())
<< " / should be true" << std::endl;
return -1;
}
if (vpMath::isNaN(std::numeric_limits<double>::infinity())) {
std::cerr << "Fail: IsNaN(infinity)=" << vpMath::isNaN(std::numeric_limits<double>::infinity())
<< " / should be false" << std::endl;
return -1;
}
if (vpMath::isNaN(std::numeric_limits<float>::infinity())) {
std::cerr << "Fail: IsNaN(infinity)=" << vpMath::isNaN(std::numeric_limits<float>::infinity())
<< " / should be false" << std::endl;
return -1;
}
if (vpMath::isNaN(1.0 / std::numeric_limits<double>::epsilon())) {
std::cerr << "Fail: IsNaN(1.0/epsilon)=" << vpMath::isNaN(1.0 / std::numeric_limits<double>::epsilon())
<< " / should be false" << std::endl;
return -1;
}
if (!vpMath::isNaN(std::numeric_limits<double>::infinity() - std::numeric_limits<double>::infinity())) {
std::cerr << "Fail: IsNaN(infinity - infinity)="
<< vpMath::isNaN(std::numeric_limits<double>::infinity() - std::numeric_limits<double>::infinity())
<< " / should be true" << std::endl;
return -1;
}
float a = 0.0f, b = 0.0f;
if (!vpMath::isNaN(a / b)) {
std::cerr << "Fail: IsNaN(0.0f/0.0f)=" << vpMath::isNaN(a / b) << " / should be true" << std::endl;
return -1;
}
std::cout << "vpMath::isNaN is Ok !" << std::endl;
// Test isInf
#if !defined(VISP_HAVE_FUNC__FINITE)
// Disable this test if using _finite as (!_finite(NAN)) returns true
// whereas isinf(NAN) returns false
if (vpMath::isInf(NAN)) {
std::cerr << "Fail: vpMath::isInf(NAN)=" << vpMath::isInf(NAN) << " / should be false" << std::endl;
return -1;
}
#endif
if (!vpMath::isInf(1.0 / a)) {
std::cerr << "Fail: vpMath::isInf(1.0/0.0)=" << vpMath::isInf(1.0 / a) << " / should be true" << std::endl;
return -1;
}
if (vpMath::isInf(0.0)) {
std::cerr << "Fail: vpMath::isInf(0.0)=" << vpMath::isInf(0.0) << " / should be false" << std::endl;
return -1;
}
if (!vpMath::isInf(exp(800.))) {
std::cerr << "Fail: vpMath::isInf(exp(800.))=" << vpMath::isInf(exp(800.)) << " / should be true" << std::endl;
return -1;
}
if (vpMath::isInf(DBL_MIN / 2.0)) {
std::cerr << "Fail: vpMath::isInf(DBL_MIN/2.0)=" << vpMath::isInf(DBL_MIN / 2.0) << " / should be false"
<< std::endl;
return -1;
}
std::cout << "vpMath::isInf is Ok !" << std::endl;
// Test round
if (vpMath::round(2.3) != 2) {
std::cerr << "Fail: vpMath::round(2.3)=" << vpMath::round(2.3) << " / should be 2" << std::endl;
return -1;
}
if (vpMath::round(3.8) != 4) {
std::cerr << "Fail: vpMath::round(3.8)=" << vpMath::round(3.8) << " / should be 4" << std::endl;
return -1;
}
if (vpMath::round(5.5) != 6) {
std::cerr << "Fail: vpMath::round(5.5)=" << vpMath::round(5.5) << " / should be 6" << std::endl;
return -1;
}
if (vpMath::round(-2.3) != -2) {
std::cerr << "Fail: vpMath::round(-2.3)=" << vpMath::round(-2.3) << " / should be -2" << std::endl;
return -1;
}
if (vpMath::round(-3.8) != -4) {
std::cerr << "Fail: vpMath::round(-3.8)=" << vpMath::round(-3.8) << " / should be -4" << std::endl;
return -1;
}
if (vpMath::round(-5.5) != -6) {
std::cerr << "Fail: vpMath::round(-5.5)=" << vpMath::round(-5.5) << " / should be -6" << std::endl;
return -1;
}
if (vpMath::round(0.0) != 0) {
std::cerr << "Fail: vpMath::round(0.0)=" << vpMath::round(0.0) << " / should be 0" << std::endl;
return -1;
}
std::cout << "vpMath::round is Ok !" << std::endl;
// Test saturate functions
// unsigned char
char char_value = -127;
unsigned char uchar_value = vpMath::saturate<unsigned char>(char_value);
if (uchar_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned char>(-127)=" << uchar_value << " / should be 0" << std::endl;
return -1;
}
unsigned short ushort_value = 60000;
uchar_value = vpMath::saturate<unsigned char>(ushort_value);
if (uchar_value != UCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<unsigned char>(60000)=" << uchar_value << " / should be " << UCHAR_MAX
<< std::endl;
return -1;
}
int int_value = 70000;
uchar_value = vpMath::saturate<unsigned char>(int_value);
if (uchar_value != UCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<unsigned char>(70000)=" << uchar_value << " / should be " << UCHAR_MAX
<< std::endl;
return -1;
}
int_value = -70000;
uchar_value = vpMath::saturate<unsigned char>(int_value);
if (uchar_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned char>(-70000)=" << uchar_value << " / should be 0" << std::endl;
return -1;
}
short short_value = 30000;
uchar_value = vpMath::saturate<unsigned char>(short_value);
if (uchar_value != UCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<unsigned char>(30000)=" << uchar_value << " / should be " << UCHAR_MAX
<< std::endl;
return -1;
}
short_value = -30000;
uchar_value = vpMath::saturate<unsigned char>(short_value);
if (uchar_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned char>(-30000)=" << uchar_value << " / should be 0" << std::endl;
return -1;
}
unsigned int uint_value = 10000;
uchar_value = vpMath::saturate<unsigned char>(uint_value);
if (uchar_value != UCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<unsigned char>(10000)=" << uchar_value << " / should be " << UCHAR_MAX
<< std::endl;
return -1;
}
float float_value = 10000.1f;
uchar_value = vpMath::saturate<unsigned char>(float_value);
if (uchar_value != UCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<unsigned char>(10000.1f)=" << uchar_value << " / should be " << UCHAR_MAX
<< std::endl;
return -1;
}
float_value = -10000.1f;
uchar_value = vpMath::saturate<unsigned char>(float_value);
if (uchar_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned char>(-10000.1f)=" << uchar_value << " / should be 0" << std::endl;
return -1;
}
double double_value = 10000.1;
uchar_value = vpMath::saturate<unsigned char>(double_value);
if (uchar_value != UCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<unsigned char>(10000.0)=" << uchar_value << " / should be " << UCHAR_MAX
<< std::endl;
return -1;
}
double_value = -10000.1;
uchar_value = vpMath::saturate<unsigned char>(double_value);
if (uchar_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned char>(-10000.0)=" << uchar_value << " / should be 0" << std::endl;
return -1;
}
std::cout << "vpMath::saturate<unsigned char>() is Ok !" << std::endl;
// char
uchar_value = 255;
char_value = vpMath::saturate<char>(uchar_value);
if (char_value != SCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<char>(255)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
return -1;
}
ushort_value = 60000;
char_value = vpMath::saturate<char>(ushort_value);
if (char_value != SCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<char>(60000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
return -1;
}
int_value = 70000;
char_value = vpMath::saturate<char>(int_value);
if (char_value != SCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<char>(70000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
return -1;
}
int_value = -70000;
char_value = vpMath::saturate<char>(int_value);
if (char_value != (char)SCHAR_MIN) {
std::cerr << "Fail: vpMath::saturate<char>(-70000)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
return -1;
}
short_value = 30000;
char_value = vpMath::saturate<char>(short_value);
if (char_value != SCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<char>(30000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
return -1;
}
short_value = -30000;
char_value = vpMath::saturate<char>(short_value);
if (char_value != (char)SCHAR_MIN) {
std::cerr << "Fail: vpMath::saturate<char>(-30000)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
return -1;
}
uint_value = 10000;
char_value = vpMath::saturate<char>(uint_value);
if (char_value != SCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<char>(10000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
return -1;
}
float_value = 10000.1f;
char_value = vpMath::saturate<char>(float_value);
if (char_value != SCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<char>(10000.1f)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
return -1;
}
float_value = -10000.1f;
char_value = vpMath::saturate<char>(float_value);
if (char_value != (char)SCHAR_MIN) {
std::cerr << "Fail: vpMath::saturate<char>(-10000.1f)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
return -1;
}
double_value = 10000.1;
char_value = vpMath::saturate<char>(double_value);
if (char_value != SCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<char>(10000.1)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
return -1;
}
double_value = -10000.1;
char_value = vpMath::saturate<char>(double_value);
if (char_value != (char)SCHAR_MIN) {
std::cerr << "Fail: vpMath::saturate<char>(-10000.1)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
return -1;
}
std::cout << "vpMath::saturate<char>() is Ok !" << std::endl;
// unsigned short
char_value = -127;
ushort_value = vpMath::saturate<unsigned short>(char_value);
if (ushort_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned short>(-127)=" << ushort_value << " / should be 0" << std::endl;
return -1;
}
short_value = -30000;
ushort_value = vpMath::saturate<unsigned short>(short_value);
if (ushort_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned short>(-30000)=" << ushort_value << " / should be 0" << std::endl;
return -1;
}
int_value = 70000;
ushort_value = vpMath::saturate<unsigned short>(int_value);
if (ushort_value != USHRT_MAX) {
std::cerr << "Fail: vpMath::saturate<unsigned short>(70000)=" << ushort_value << " / should be " << USHRT_MAX
<< std::endl;
return -1;
}
int_value = -70000;
ushort_value = vpMath::saturate<unsigned short>(int_value);
if (ushort_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned short>(-70000)=" << ushort_value << " / should be 0" << std::endl;
return -1;
}
uint_value = 70000;
ushort_value = vpMath::saturate<unsigned short>(uint_value);
if (ushort_value != USHRT_MAX) {
std::cerr << "Fail: vpMath::saturate<unsigned short>(70000)=" << ushort_value << " / should be " << USHRT_MAX
<< std::endl;
return -1;
}
float_value = 70000.1f;
ushort_value = vpMath::saturate<unsigned short>(float_value);
if (ushort_value != USHRT_MAX) {
std::cerr << "Fail: vpMath::saturate<unsigned short>(70000.1f)=" << ushort_value << " / should be " << USHRT_MAX
<< std::endl;
return -1;
}
float_value = -10000.1f;
ushort_value = vpMath::saturate<unsigned short>(float_value);
if (ushort_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned short>(-10000.1f)=" << ushort_value << " / should be 0" << std::endl;
return -1;
}
double_value = 70000.1;
ushort_value = vpMath::saturate<unsigned short>(double_value);
if (ushort_value != USHRT_MAX) {
std::cerr << "Fail: vpMath::saturate<unsigned short>(70000.1)=" << ushort_value << " / should be " << USHRT_MAX
<< std::endl;
return -1;
}
double_value = -10000.1;
ushort_value = vpMath::saturate<unsigned short>(double_value);
if (ushort_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned short>(-10000.1)=" << ushort_value << " / should be 0" << std::endl;
return -1;
}
std::cout << "vpMath::saturate<unsigned short>() is Ok !" << std::endl;
// short
ushort_value = 60000;
short_value = vpMath::saturate<short>(ushort_value);
if (short_value != SHRT_MAX) {
std::cerr << "Fail: vpMath::saturate<short>(60000)=" << short_value << " / should be " << SHRT_MAX << std::endl;
return -1;
}
int_value = 70000;
short_value = vpMath::saturate<short>(int_value);
if (short_value != SHRT_MAX) {
std::cerr << "Fail: vpMath::saturate<short>(70000)=" << short_value << " / should be " << SHRT_MAX << std::endl;
return -1;
}
int_value = -70000;
short_value = vpMath::saturate<short>(int_value);
if (short_value != SHRT_MIN) {
std::cerr << "Fail: vpMath::saturate<short>(-70000)=" << short_value << " / should be " << SHRT_MIN << std::endl;
return -1;
}
uint_value = 70000;
short_value = vpMath::saturate<short>(uint_value);
if (short_value != SHRT_MAX) {
std::cerr << "Fail: vpMath::saturate<short>(70000)=" << short_value << " / should be " << SHRT_MAX << std::endl;
return -1;
}
float_value = 70000.1f;
short_value = vpMath::saturate<short>(float_value);
if (short_value != SHRT_MAX) {
std::cerr << "Fail: vpMath::saturate<short>(70000.1f)=" << short_value << " / should be " << SHRT_MAX << std::endl;
return -1;
}
float_value = -70000.1f;
short_value = vpMath::saturate<short>(float_value);
if (short_value != SHRT_MIN) {
std::cerr << "Fail: vpMath::saturate<short>(-70000.1f)=" << short_value << " / should be " << SHRT_MIN << std::endl;
return -1;
}
double_value = 70000.1;
short_value = vpMath::saturate<short>(double_value);
if (short_value != SHRT_MAX) {
std::cerr << "Fail: vpMath::saturate<short>(70000.1)=" << short_value << " / should be " << SHRT_MAX << std::endl;
return -1;
}
double_value = -70000.1;
short_value = vpMath::saturate<short>(double_value);
if (short_value != SHRT_MIN) {
std::cerr << "Fail: vpMath::saturate<short>(70000.1)=" << short_value << " / should be " << SHRT_MIN << std::endl;
return -1;
}
std::cout << "vpMath::saturate<short>() is Ok !" << std::endl;
// Test mean, median and standard deviation against Matlab with rng(0) and
// rand(10,1)*10
std::vector<double> vectorOfDoubles(10);
vectorOfDoubles[0] = 8.1472;
vectorOfDoubles[1] = 9.0579;
vectorOfDoubles[2] = 1.2699;
vectorOfDoubles[3] = 9.1338;
vectorOfDoubles[4] = 6.3236;
vectorOfDoubles[5] = 0.9754;
vectorOfDoubles[6] = 2.7850;
vectorOfDoubles[7] = 5.4688;
vectorOfDoubles[8] = 9.5751;
vectorOfDoubles[9] = 9.6489;
double res = vpMath::getMean(vectorOfDoubles);
if (!vpMath::equal(res, 6.2386, 0.001)) {
std::cerr << "Problem with vpMath::getMean()=" << res << std::endl;
return -1;
}
std::cout << "vpMath::getMean() is Ok !" << std::endl;
res = vpMath::getStdev(vectorOfDoubles);
if (!vpMath::equal(res, 3.2810, 0.001)) {
std::cerr << "Problem with vpMath::getStdev()=" << res << std::endl;
return -1;
}
res = vpMath::getStdev(vectorOfDoubles, true);
if (!vpMath::equal(res, 3.4585, 0.001)) {
std::cerr << "Problem with vpMath::getStdev() with Bessel correction=" << res << std::endl;
return -1;
}
std::cout << "vpMath::getStdev() is Ok !" << std::endl;
res = vpMath::getMedian(vectorOfDoubles);
if (!vpMath::equal(res, 7.2354, 0.001)) {
std::cerr << "Problem with vpMath::getMedian()=" << res << std::endl;
return -1;
}
// Test median with odd number of elements
vectorOfDoubles.push_back(1.5761);
res = vpMath::getMedian(vectorOfDoubles);
if (!vpMath::equal(res, 6.3236, 0.001)) {
std::cerr << "Problem with vpMath::getMedian()=" << res << std::endl;
return -1;
}
std::cout << "vpMath::getMedian() is Ok !" << std::endl;
std::cout << "OK !" << std::endl;
return 0;
}