Visual Servoing Platform
version 3.5.1 under development (2023-09-22)
|
#include <visp3/core/vpMath.h>
Static Public Member Functions | |
static double | deg (double rad) |
static vpColVector | deg (const vpRotationVector &r) |
static vpColVector | deg (const vpColVector &r) |
static double | rad (double deg) |
static vpColVector | rad (const vpColVector &r) |
static double | sqr (double x) |
static double | fact (unsigned int x) |
static long double | comb (unsigned int n, unsigned int p) |
template<typename T > | |
static T | clamp (const T &v, const T &lower, const T &upper) |
static int | round (double x) |
static int | sign (double x) |
static bool | nul (double x, double threshold=0.001) |
static bool | equal (double x, double y, double threshold=0.001) |
static bool | greater (double x, double y, double threshold=0.001) |
template<class Type > | |
static Type | maximum (const Type &a, const Type &b) |
template<class Type > | |
static Type | minimum (const Type &a, const Type &b) |
template<class Type > | |
static Type | abs (const Type &x) |
static double | sinc (double x) |
static double | sinc (double sinx, double x) |
static double | mcosc (double cosx, double x) |
static double | msinc (double sinx, double x) |
static double | sigmoid (double x, double x0=0., double x1=1., double n=12.) |
template<class Type > | |
static void | swap (Type &a, Type &b) |
static bool | isNaN (double value) |
static bool | isNaN (float value) |
static bool | isInf (double value) |
static bool | isInf (float value) |
static bool | isFinite (double value) |
static bool | isFinite (float value) |
static bool | isNumber (const std::string &str) |
static double | lineFitting (const std::vector< vpImagePoint > &imPts, double &a, double &b, double &c) |
template<typename _Tp > | |
static _Tp | saturate (unsigned char v) |
template<typename _Tp > | |
static _Tp | saturate (char v) |
template<typename _Tp > | |
static _Tp | saturate (unsigned short v) |
template<typename _Tp > | |
static _Tp | saturate (short v) |
template<typename _Tp > | |
static _Tp | saturate (unsigned v) |
template<typename _Tp > | |
static _Tp | saturate (int v) |
template<typename _Tp > | |
static _Tp | saturate (float v) |
template<typename _Tp > | |
static _Tp | saturate (double v) |
static double | getMean (const std::vector< double > &v) |
static double | getMedian (const std::vector< double > &v) |
static double | getStdev (const std::vector< double > &v, bool useBesselCorrection=false) |
static int | modulo (int a, int n) |
static vpHomogeneousMatrix | ned2ecef (double lonDeg, double latDeg, double radius) |
static vpHomogeneousMatrix | enu2ecef (double lonDeg, double latDeg, double radius) |
static vpHomogeneousMatrix | enu2ned (const vpHomogeneousMatrix &enu_M) |
template<typename T > | |
static std::vector< double > | linspace (T start_in, T end_in, unsigned int num_in) |
static std::vector< std::pair< double, double > > | computeRegularPointsOnSphere (unsigned int maxPoints) |
static std::vector< vpHomogeneousMatrix > | getLocalTangentPlaneTransformations (const std::vector< std::pair< double, double > > &lonlatVec, double radius, vpHomogeneousMatrix(*toECEF)(double lonDeg, double latDeg, double radius)) |
static vpHomogeneousMatrix | lookAt (const vpColVector &from, const vpColVector &to, vpColVector tmp) |
template<> | |
unsigned char | saturate (char v) |
template<> | |
unsigned char | saturate (unsigned short v) |
template<> | |
unsigned char | saturate (int v) |
template<> | |
unsigned char | saturate (short v) |
template<> | |
unsigned char | saturate (unsigned int v) |
template<> | |
unsigned char | saturate (float v) |
template<> | |
unsigned char | saturate (double v) |
template<> | |
char | saturate (unsigned char v) |
template<> | |
char | saturate (unsigned short v) |
template<> | |
char | saturate (int v) |
template<> | |
char | saturate (short v) |
template<> | |
char | saturate (unsigned int v) |
template<> | |
char | saturate (float v) |
template<> | |
char | saturate (double v) |
template<> | |
unsigned short | saturate (char v) |
template<> | |
unsigned short | saturate (short v) |
template<> | |
unsigned short | saturate (int v) |
template<> | |
unsigned short | saturate (unsigned int v) |
template<> | |
unsigned short | saturate (float v) |
template<> | |
unsigned short | saturate (double v) |
template<> | |
short | saturate (unsigned short v) |
template<> | |
short | saturate (int v) |
template<> | |
short | saturate (unsigned int v) |
template<> | |
short | saturate (float v) |
template<> | |
short | saturate (double v) |
template<> | |
int | saturate (float v) |
template<> | |
int | saturate (double v) |
template<> | |
unsigned int | saturate (float v) |
template<> | |
unsigned int | saturate (double v) |
Provides simple mathematics computation tools that are not available in the C mathematics library (math.h)
|
inlinestatic |
Find the absolute value of a number (or other).
x | : The number. |
Definition at line 187 of file vpMath.h.
Referenced by vpImageTools::imageDifferenceAbsolute(), vpServo::secondaryTaskJointLimitAvoidance(), and vpMeNurbs::seekExtremities().
|
inlinestatic |
Clamp a value to boundaries.
v | : The value to clamp. |
lower,upper | : The boundaries to clamp v to. |
Throw a vpException if the value of lower
is greater than upper
.
Definition at line 139 of file vpMath.h.
References vpException::badValue.
Referenced by vpPlaneEstimation::estimatePlane().
|
inlinestatic |
Computes the number of combination of p elements inside n elements.
n | : total number of elements. |
p | : requested number of elements. |
Definition at line 309 of file vpMath.h.
References fact().
Referenced by vpMomentAlpha::compute(), vpMomentCentered::compute(), vpFeatureMomentCentered::compute_Lmu_pq(), vpNurbs::computeCurveDersPoint(), and vpPixelMeterConversion::convertMoment().
|
static |
Compute the vector of longitude / latitude (in degree) couples for maxPoints regularly spaced on a sphere, using the following paper:
Following image illustrates the camera poses regularly spaced on a sphere:
maxPoints | : The number of point coordinates to be sampled on a sphere. |
Definition at line 572 of file vpMath.cpp.
|
static |
Convert angles of a column vector from radians to degrees.
r | : Column vector with angles in radians. |
Definition at line 707 of file vpMath.cpp.
References deg(), and vpArray2D< Type >::size().
|
static |
Convert angles of a rotation vector into degrees.
r | : Rotation vector with angles in radians. |
Definition at line 689 of file vpMath.cpp.
References deg(), vpException::fatalError, and vpArray2D< Type >::size().
|
inlinestatic |
Convert an angle in radians into degrees.
rad | : Angle in radians. |
Definition at line 106 of file vpMath.h.
Referenced by vpPose::calculArbreDementhon(), vpHandEyeCalibration::calibrate(), vpMbGenericTracker::computeCurrentProjectionError(), vpMbTracker::computeCurrentProjectionError(), vpMbGenericTracker::computeProjectionError(), vpMbEdgeTracker::computeProjectionError(), computeRegularPointsOnSphere(), deg(), vpAfma6::getInverseKinematics(), vpMeEllipse::leastSquareRobust(), vpMbDepthDenseTracker::loadConfigFile(), vpMbDepthNormalTracker::loadConfigFile(), vpMbEdgeKltTracker::loadConfigFile(), vpMbEdgeTracker::loadConfigFile(), vpMbKltTracker::loadConfigFile(), vpMeEllipse::plugHoles(), vpPose::poseDementhonNonPlan(), vpPose::poseDementhonPlan(), vpPoseVector::print(), vpFeatureSegment::print(), vpMeEllipse::printParameters(), vpServoData::save(), vpRobotAfma4::savePosFile(), vpRobotAfma6::savePosFile(), vpRobotFranka::savePosFile(), vpRobotUniversalRobots::savePosFile(), vpRobotViper650::savePosFile(), vpRobotViper850::savePosFile(), vpSimulatorAfma6::savePosFile(), vpSimulatorViper850::savePosFile(), vpRobotFlirPtu::setJointVelocity(), vpRobotKinova::setJointVelocity(), vpRobotViper650::setPosition(), vpRobotViper850::setPosition(), vpRobotFlirPtu::setPosition(), vpRobotKinova::setPosition(), vpRobotPioneer::setVelocity(), vpMeEllipse::track(), vpSimulatorAfma6::updateArticularPosition(), vpSimulatorViper850::updateArticularPosition(), and vpRobotBiclops::vpRobotBiclopsSpeedControlLoop().
|
static |
Compute from a given longitude, latitude and a sphere radius the homogeneous transformation from the ENU frame to the ECEF frame:
lonDeg | : The longitude in degree or angle in previous equation. |
latDeg | : The latitude in degree or angle in previous equation. |
radius | : The sphere radius in meter. |
Definition at line 536 of file vpMath.cpp.
References rad().
|
static |
Convert from ENU (East-North-Up) to NED (North-East-Down) frame.
enu_M | : HomogeneousMatrix expressed in ENU frame. |
Definition at line 736 of file vpMath.cpp.
|
inlinestatic |
Compares to threshold
.
x | : x value. |
y | : y value. |
threshold | : Tolerance threshold. |
threshold
. Definition at line 369 of file vpMath.h.
References nul().
Referenced by vpMbDepthDenseTracker::display(), vpMbDepthNormalTracker::display(), vpMbEdgeKltTracker::display(), vpMbEdgeTracker::display(), vpMbKltTracker::display(), vpMbEdgeTracker::displayFeaturesOnImage(), vpHomogeneousMatrix::isAnHomogeneousMatrix(), vpCameraParameters::operator==(), vpColVector::operator==(), vpRowVector::operator==(), vpQbSoftHand::setPosition(), and vpMunkres::stepTwo().
|
inlinestatic |
|
static |
Compute transformations from the local tangent plane (e.g. NED, ECU, ...) to the ECEF frame.
Following image illustrates the camera poses sampled using longitude / latitude coordinates:
lonlatVec | : Vector of longitude/latitude coordinates in degree. |
radius | : Sphere radius in meter. |
toECEF | : Pointer to the function computing from a longitude / latitude coordinates in degree and a radius the corresponding transformation from the local frame (e.g. NED or ENU) to the ECEF frame. |
Definition at line 622 of file vpMath.cpp.
|
static |
Compute the mean value for a vector of double.
v | : Vector of double values. |
Definition at line 294 of file vpMath.cpp.
References vpException::notInitialized.
Referenced by getStdev().
|
static |
Compute the median value for a vector of double.
v | : Vector of double values. |
Definition at line 314 of file vpMath.cpp.
References vpException::notInitialized.
Referenced by vpColVector::median(), and vpRowVector::median().
|
static |
Compute the standard deviation value for a vector of double.
v | : Vector of double values. |
useBesselCorrection | : If true, the Bessel correction is used (normalize by N-1). |
Definition at line 345 of file vpMath.cpp.
References getMean(), and vpException::notInitialized.
|
inlinestatic |
|
static |
Returns whether a double is a finite value (neither infinite nor NaN).
value | : Double number to check. |
Definition at line 178 of file vpMath.cpp.
References isInf(), and isNaN().
Referenced by vpImage< Type >::getMinMaxValue().
|
static |
Returns whether a float is a finite value (neither infinite nor NaN).
value | : Float number to check. |
Definition at line 197 of file vpMath.cpp.
|
static |
Returns whether a double is an infinity value (either positive infinity or negative infinity).
value | : Double number to check. |
Definition at line 137 of file vpMath.cpp.
Referenced by isFinite().
|
static |
Returns whether a float is an infinity value (either positive infinity or negative infinity).
value | : Float number to check. |
Definition at line 158 of file vpMath.cpp.
|
static |
Check whether a double number is not a number (NaN) or not.
value | : Double number to check. |
Definition at line 93 of file vpMath.cpp.
Referenced by vpKeyPoint::compute3DForPointsOnCylinders(), vpMbtDistanceKltCylinder::computeInteractionMatrixAndResidu(), vpComedi::getPhyData(), vpMbtDistanceKltCylinder::init(), isFinite(), and vpHomogeneousMatrix::isValid().
|
static |
Check whether a float number is not a number (NaN) or not.
value | : Float number to check. |
Definition at line 114 of file vpMath.cpp.
|
static |
Returns whether a string is a number.
[in] | str | : String to check. |
Definition at line 215 of file vpMath.cpp.
|
static |
Compute the line equation using least-squares fitting that minimizes the cost function:
imPts | : Image points (size >= 3). |
a | : a coefficient. |
b | : b coefficient. |
c | : c coefficient. |
Definition at line 382 of file vpMath.cpp.
References vpException::dimensionError, vpMatrix::eigenValues(), vpImagePoint::get_u(), and vpImagePoint::get_v().
|
inlinestatic |
Similar to the NumPy linspace function: "Return evenly spaced numbers over a specified interval." Code from: https://stackoverflow.com/a/27030598
start_in | : The starting value of the sequence. |
end_in | : The end value of the sequence. |
num_in | : Number of samples to generate. |
|
static |
Compute the transformation such that the camera located at from position looks toward to position.
Right-handed coordinate system for OpenGL (figure from https://learnopengl.com/Getting-started/Coordinate-Systems):
See also:
from | : Current camera position as a 3-dim vector with 3D coordinates (X,Y,Z) in meter.. |
to | : Where the camera must point toward as a 3-dim vector with 3D coordinates (X,Y,Z) in meter. |
tmp | : Arbitrary up-vector as a 3-dim vector with coordinates along (X,Y,Z) in meter. |
Definition at line 657 of file vpMath.cpp.
References vpColVector::crossProd(), vpColVector::normalize(), and vpArray2D< Type >::size().
|
inlinestatic |
Find the maximum between two numbers (or other).
a | : First number. |
b | : Second number. |
Definition at line 172 of file vpMath.h.
Referenced by vpMbEdgeTracker::addCircle(), vpMbEdgeTracker::addCylinder(), vpMbTracker::addProjectionErrorCircle(), vpMbTracker::addProjectionErrorCylinder(), vpMbTracker::extractCylinders(), vpBSpline::findSpan(), vpRobotWireFrameSimulator::getExternalCameraParameters(), vpWireFrameSimulator::getExternalCameraParameters(), vpSimulatorAfma6::getExternalImage(), vpSimulatorViper850::getExternalImage(), vpWireFrameSimulator::getExternalImage(), vpWireFrameSimulator::getInternalCameraParameters(), vpWireFrameSimulator::getInternalImage(), vpRobotWireFrameSimulator::getInternalView(), vpMeNurbs::localReSample(), vpColVector::print(), vpForceTwistMatrix::print(), vpPoseVector::print(), vpRowVector::print(), vpVelocityTwistMatrix::print(), vpMatrix::print(), vpRansac< vpTransformation >::ransac(), vpDot2::searchDotsInArea(), vpMeNurbs::seekExtremitiesCanny(), and vpRobotBiclops::vpRobotBiclopsSpeedControlLoop().
|
static |
Compute
cosx | : Value of cos(x). |
x | : Value of x. |
Definition at line 233 of file vpMath.cpp.
Referenced by vpRotationMatrix::buildFrom(), vpExponentialMap::direct(), and vpExponentialMap::inverse().
|
inlinestatic |
Find the minimum between two numbers (or other).
a | : First number. |
b | : Second number. |
Definition at line 180 of file vpMath.h.
Referenced by vpRobotWireFrameSimulator::getExternalCameraParameters(), vpWireFrameSimulator::getExternalCameraParameters(), vpSimulatorAfma6::getExternalImage(), vpSimulatorViper850::getExternalImage(), vpWireFrameSimulator::getExternalImage(), vpWireFrameSimulator::getInternalCameraParameters(), vpWireFrameSimulator::getInternalImage(), vpRobotWireFrameSimulator::getInternalView(), vpIoTools::loadConfigFile(), vpWireFrameSimulator::navigation(), vpRansac< vpTransformation >::ransac(), and vpMbEdgeTracker::testTracking().
|
static |
Compute the modified modulo:
a | : The dividend. |
n | : The divisor. |
Definition at line 435 of file vpMath.cpp.
|
static |
Compute with .
sinx | : value of sin(x). |
x | : Value of x. |
Definition at line 249 of file vpMath.cpp.
Referenced by vpExponentialMap::direct(), and vpExponentialMap::inverse().
|
static |
Compute from a given longitude, latitude and a sphere radius the homogeneous transformation from the NED frame to the ECEF frame:
See also:
lonDeg | : The longitude in degree or angle in previous equation. |
latDeg | : The latitude in degree or angle in previous equation. |
radius | : The sphere radius in meter. |
Definition at line 475 of file vpMath.cpp.
References rad().
|
inlinestatic |
Compares to threshold
.
x | : Value to test. |
threshold | : Tolerance threshold |
threshold
. Definition at line 360 of file vpMath.h.
Referenced by vpPose::computeRansacIterations(), equal(), vpQuaternionVector::inverse(), vpHomogeneousMatrix::isAnHomogeneousMatrix(), vpQuaternionVector::normalize(), vpQuaternionVector::operator/(), and vpImageTools::warpImage().
|
static |
Convert angles of a column vector from degrees to radians.
r | : Column vector with angles in degrees. |
Definition at line 722 of file vpMath.cpp.
References rad(), and vpArray2D< Type >::size().
|
inlinestatic |
Convert an angle in degrees into radian.
deg | : Angle in degrees. |
Definition at line 116 of file vpMath.h.
Referenced by vpMbTracker::addPolygon(), vpMbTracker::addProjectionErrorPolygon(), vpMbGenericTracker::computeProjectionError(), enu2ecef(), vpRobotViper650::get_eJe(), vpRobotViper850::get_eJe(), vpAfma6::getInverseKinematics(), vpViper::getInverseKinematicsWrist(), vpRobotKinova::getJointPosition(), vpRobotPioneer::getVelocity(), vpRobotUniversalRobots::init(), vpSimulatorViper850::init(), vpAfma6::init(), vpSimulatorAfma6::init(), vpViper650::init(), vpViper850::init(), vpSimulatorAfma6::initDisplay(), vpSimulatorViper850::initDisplay(), vpMbtPolygon::isVisible(), vpMbTracker::loadCAOModel(), vpMbDepthDenseTracker::loadConfigFile(), vpMbDepthNormalTracker::loadConfigFile(), vpMbEdgeKltTracker::loadConfigFile(), vpMbEdgeTracker::loadConfigFile(), vpMbKltTracker::loadConfigFile(), vpWireFrameSimulator::navigation(), ned2ecef(), rad(), vpRobotAfma4::readPosFile(), vpRobotAfma6::readPosFile(), vpRobotFranka::readPosFile(), vpRobotUniversalRobots::readPosFile(), vpSimulatorAfma6::readPosFile(), vpMbDepthDenseTracker::resetTracker(), vpMbDepthNormalTracker::resetTracker(), vpMbEdgeTracker::resetTracker(), vpMbGenericTracker::resetTracker(), vpMbKltTracker::resetTracker(), vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(), vpRobotMavsdk::vpRobotMavsdk(), vpSickLDMRS::vpSickLDMRS(), vpViper::vpViper(), vpViper650::vpViper650(), vpViper850::vpViper850(), and vpWireFrameSimulator::vpWireFrameSimulator().
|
inlinestatic |
Round x to the nearest integer.
x | : Value to round. |
Definition at line 323 of file vpMath.h.
Referenced by computeRegularPointsOnSphere(), vpMeLine::computeRhoTheta(), vpMeSite::convolution(), vpKeyPoint::createImageMatching(), vpKltOpencv::display(), vpDisplayOpenCV::displayCharString(), vpDisplayOpenCV::displayCircle(), vpDisplayOpenCV::displayLine(), vpKeyPoint::displayMatching(), vpDisplayOpenCV::displayPoint(), vpMbtDistanceKltCylinder::displayPrimitive(), vpMbtDistanceKltPoints::displayPrimitive(), vpDisplayGTK::displayRectangle(), vpDisplayX::displayRectangle(), vpDisplayOpenCV::displayRectangle(), vp::equalizeHistogram(), vpImageTools::extract(), vpBSpline::findSpan(), vpMbtDistanceKltCylinder::getFeaturesForDisplay(), vpMbtDistanceKltPoints::getFeaturesForDisplay(), vpMeSite::getSign(), vpImage< Type >::getValue(), vpMeSite::init(), vpMeLine::initTracking(), vpKeyPoint::insertImageMatching(), vpImageTools::interpolate(), vpMeTracker::outOfImage(), vpMeEllipse::plugHoles(), vpMeEllipse::sample(), vpMeLine::sample(), saturate(), vpMeLine::updateDelta(), and vpMeSite::vpMeSite().
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
|
inlinestatic |
Return the sign of x.
x | : Value to test. |
Definition at line 342 of file vpMath.h.
Referenced by vpThetaUVector::buildFrom(), vpMbEdgeTracker::computeVVSFirstPhase(), and vpMeNurbs::seekExtremities().
|
static |
Compute sinus cardinal .
sinx | : Value of sin(x). |
x | : Value of x. |
Definition at line 279 of file vpMath.cpp.
|
static |
Compute sinus cardinal .
x | : Value of x. |
Definition at line 264 of file vpMath.cpp.
Referenced by vpThetaUVector::buildFrom(), vpRotationMatrix::buildFrom(), vpExponentialMap::direct(), vpFeatureThetaU::interaction(), and vpExponentialMap::inverse().
|
inlinestatic |
Compute x square value.
Definition at line 124 of file vpMath.h.
Referenced by vpMbTracker::addPolygon(), vpMbTracker::addProjectionErrorPolygon(), vpMbtDistanceCircle::computeInteractionMatrixError(), vpCircle::computeIntersectionPoint(), vpPose::computeResidual(), vpPose::computeResidualDementhon(), vpCalibration::computeStdDeviation(), vpCalibration::computeStdDeviation_dist(), vpMbDepthDenseTracker::computeVVS(), vpMbDepthNormalTracker::computeVVS(), vpMbEdgeTracker::computeVVS(), vpMbEdgeKltTracker::computeVVS(), vpMbGenericTracker::computeVVS(), vpMbEdgeTracker::computeVVSInit(), vpMeterPixelConversion::convertEllipse(), vpMeterPixelConversion::convertLine(), vpPixelMeterConversion::convertLine(), vpPose::coplanar(), vpFeatureBuilder::create(), vpDot2::defineDots(), vpDisplayGTK::displayArrow(), vpDisplayOpenCV::displayArrow(), vpDisplayX::displayArrow(), vpDisplayOpenCV::displayDotLine(), vpCalibration::displayGrid(), vpImagePoint::distance(), vpImageDraw::drawArrow(), vpImageDraw::drawDottedLine(), vpMbtFaceDepthNormal::estimateFeatures(), vpImageFilter::getGaussianDerivativeKernel(), vpImageFilter::getGaussianKernel(), vpViper::getInverseKinematicsWrist(), vpImageTools::initUndistortMap(), vpImageTools::integralImage(), vpFeatureEllipse::interaction(), vpFeatureThetaU::interaction(), vpRotationMatrix::isARotationMatrix(), vpScale::KernelDensityGradient(), vpScale::KernelDensityGradient_EPANECHNIKOV(), vpMeLine::leastSquare(), vpMeNurbs::localReSample(), vpScale::MeanShift(), vpPose::poseVirtualVSrobust(), vpPose::poseVirtualVSWithDepth(), vpColorDepthConversion::projectColorToDepth(), vpCircle::projection(), vpSphere::projection(), vpNurbs::removeCurveKnot(), vpMeLine::reSample(), vpMeNurbs::sample(), vpMeLine::sample(), vpMeLine::seekExtremities(), vpMeNurbs::seekExtremitiesCanny(), vpImagePoint::sqrDistance(), vpMeSite::sqrDistance(), vpMeNurbs::supressNearPoints(), vpMeSite::track(), and vpImageTools::undistort().