49 #include <visp3/core/vpException.h>
50 #include <visp3/core/vpMath.h>
51 #include <visp3/core/vpMatrix.h>
53 #if defined(VISP_HAVE_FUNC__FINITE)
57 #if !(defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN)) || \
58 !(defined(VISP_HAVE_FUNC_ISINF) || defined(VISP_HAVE_FUNC_STD_ISINF)) || \
59 !(defined(VISP_HAVE_FUNC_ISFINITE) || defined(VISP_HAVE_FUNC_STD_ISFINITE) || defined(VISP_HAVE_FUNC__FINITE))
60 #if defined _MSC_VER || defined __BORLANDC__
61 typedef __int64 int64;
62 typedef unsigned __int64 uint64;
64 typedef int64_t int64;
65 typedef uint64_t uint64;
68 #ifndef DOXYGEN_SHOULD_SKIP_THIS
85 const double vpMath::ang_min_sinc = 1.0e-8;
86 const double vpMath::ang_min_mc = 2.5e-4;
95 #if defined(VISP_HAVE_FUNC_STD_ISNAN)
96 return std::isnan(value);
97 #elif defined(VISP_HAVE_FUNC_ISNAN)
99 #elif defined(VISP_HAVE_FUNC__ISNAN)
100 return (_isnan(value) != 0);
105 return (((
unsigned)(ieee754.u >> 32) & 0x7fffffff) + ((
unsigned)ieee754.u != 0) > 0x7ff00000) != 0;
116 #if defined(VISP_HAVE_FUNC_STD_ISNAN)
117 return std::isnan(value);
118 #elif defined(VISP_HAVE_FUNC_ISNAN)
120 #elif defined(VISP_HAVE_FUNC__ISNAN)
121 return (_isnan(value) != 0);
126 return ((
unsigned)ieee754.u & 0x7fffffff) > 0x7f800000;
139 #if defined(VISP_HAVE_FUNC_STD_ISINF)
140 return std::isinf(value);
141 #elif defined(VISP_HAVE_FUNC_ISINF)
147 return ((
unsigned)(ieee754.u >> 32) & 0x7fffffff) == 0x7ff00000 && (unsigned)ieee754.u == 0;
160 #if defined(VISP_HAVE_FUNC_STD_ISINF)
161 return std::isinf(value);
162 #elif defined(VISP_HAVE_FUNC_ISINF)
168 return ((
unsigned)ieee754.u & 0x7fffffff) == 0x7f800000;
180 #if defined(VISP_HAVE_FUNC_STD_ISFINITE)
181 return std::isfinite(value);
182 #elif defined(VISP_HAVE_FUNC_ISFINITE)
183 return isfinite(value);
184 #elif defined(VISP_HAVE_FUNC__FINITE)
185 return _finite(value);
199 #if defined(VISP_HAVE_FUNC_STD_ISFINITE)
200 return std::isfinite(value);
201 #elif defined(VISP_HAVE_FUNC_ISFINITE)
202 return isfinite(value);
203 #elif defined(VISP_HAVE_FUNC__FINITE)
204 return _finitef(value);
217 for (
size_t i = 0; i < str.size(); i++) {
218 if (isdigit(str[i]) ==
false) {
235 if (fabs(x) < ang_min_mc)
238 return ((1.0 - cosx) / x / x);
251 if (fabs(x) < ang_min_mc)
254 return ((1.0 - sinx / x) / x / x);
266 if (fabs(x) < ang_min_sinc)
281 if (fabs(x) < ang_min_sinc)
300 size_t size = v.size();
302 double sum = std::accumulate(v.begin(), v.end(), 0.0);
304 return sum / (double)size;
320 std::vector<double> v_copy = v;
321 size_t size = v_copy.size();
324 std::nth_element(v_copy.begin(), v_copy.begin() + n, v_copy.end());
325 double val_n = v_copy[n];
331 std::nth_element(v_copy.begin(), v_copy.begin() + n - 1, v_copy.end());
332 return 0.5 * (val_n + v_copy[n - 1]);
353 std::vector<double> diff(v.size());
354 #if VISP_CXX_STANDARD > VISP_CXX_STANDARD_98
355 std::transform(v.begin(), v.end(), diff.begin(), std::bind(std::minus<double>(), std::placeholders::_1, mean));
357 std::transform(v.begin(), v.end(), diff.begin(), std::bind2nd(std::minus<double>(), mean));
360 double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
361 double divisor = (double)v.size();
362 if (useBesselCorrection && v.size() > 1) {
363 divisor = divisor - 1;
366 return std::sqrt(sq_sum / divisor);
384 if (imPts.size() < 3) {
388 double x_mean = 0, y_mean = 0;
389 for (
size_t i = 0; i < imPts.size(); i++) {
391 x_mean += imPt.
get_u();
392 y_mean += imPt.
get_v();
394 x_mean /= imPts.size();
395 y_mean /= imPts.size();
398 for (
size_t i = 0; i < imPts.size(); i++) {
400 AtA[0][0] += (imPt.
get_u() - x_mean) * (imPt.
get_u() - x_mean);
401 AtA[0][1] += (imPt.
get_u() - x_mean) * (imPt.
get_v() - y_mean);
402 AtA[1][1] += (imPt.
get_v() - y_mean) * (imPt.
get_v() - y_mean);
404 AtA[1][0] = AtA[0][1];
410 a = eigenvectors[0][0];
411 b = eigenvectors[1][0];
412 c = a * x_mean + b * y_mean;
415 for (
size_t i = 0; i < imPts.size(); i++) {
416 double x0 = imPts[i].get_u();
417 double y0 = imPts[i].get_v();
419 error += std::fabs(a * x0 + b * y0 - c);
422 return error / imPts.size();
481 ecef_M_ned[0][0] = -sin(lat) * cos(lon);
482 ecef_M_ned[0][1] = -sin(lon);
483 ecef_M_ned[0][2] = -cos(lat) * cos(lon);
484 ecef_M_ned[0][3] = radius * cos(lon) * cos(lat);
485 ecef_M_ned[1][0] = -sin(lat) * sin(lon);
486 ecef_M_ned[1][1] = cos(lon);
487 ecef_M_ned[1][2] = -cos(lat) * sin(lon);
488 ecef_M_ned[1][3] = radius * sin(lon) * cos(lat);
489 ecef_M_ned[2][0] = cos(lat);
490 ecef_M_ned[2][1] = 0;
491 ecef_M_ned[2][2] = -sin(lat);
492 ecef_M_ned[2][3] = radius * sin(lat);
542 ecef_M_enu[0][0] = -sin(lon);
543 ecef_M_enu[0][1] = -sin(lat) * cos(lon);
544 ecef_M_enu[0][2] = cos(lat) * cos(lon);
545 ecef_M_enu[0][3] = radius * cos(lon) * cos(lat);
546 ecef_M_enu[1][0] = cos(lon);
547 ecef_M_enu[1][1] = -sin(lat) * sin(lon);
548 ecef_M_enu[1][2] = cos(lat) * sin(lon);
549 ecef_M_enu[1][3] = radius * sin(lon) * cos(lat);
550 ecef_M_enu[2][0] = 0;
551 ecef_M_enu[2][1] = cos(lat);
552 ecef_M_enu[2][2] = sin(lat);
553 ecef_M_enu[2][3] = radius * sin(lat);
574 assert(maxPoints > 0);
576 double a = 4.0 * M_PI / maxPoints;
578 int m_theta = int(
round(M_PI / d));
579 double d_theta = M_PI / m_theta;
580 double d_phi = a / d_theta;
582 std::vector<std::pair<double, double> > lonlat_vec;
583 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
584 lonlat_vec.reserve(
static_cast<unsigned int>(std::sqrt(maxPoints)));
586 lonlat_vec.reserve(
static_cast<unsigned int>(std::sqrt(
static_cast<double>(maxPoints))));
588 for (
int m = 0; m < m_theta; m++) {
589 double theta = M_PI * (m + 0.5) / m_theta;
590 int m_phi =
static_cast<int>(
round(2.0 * M_PI * sin(theta) / d_phi));
592 for (
int n = 0; n < m_phi; n++) {
593 double phi = 2.0 * M_PI * n / m_phi;
595 double lat = M_PI_2 - theta;
596 lonlat_vec.push_back(std::make_pair(
deg(lon),
deg(lat)));
625 std::vector<vpHomogeneousMatrix> ecef_M_local_vec;
626 ecef_M_local_vec.reserve(lonlatVec.size());
627 for (
size_t i = 0; i < lonlatVec.size(); i++) {
628 double lonDeg = lonlatVec[i].first;
629 double latDeg = lonlatVec[i].second;
632 ecef_M_local_vec.push_back(ecef_M_local);
634 return ecef_M_local_vec;
659 assert(from.
size() == 3);
660 assert(to.
size() == 3);
661 assert(tmp.
size() == 3);
667 wMc[0][0] = right[0];
669 wMc[0][2] = forward[0];
671 wMc[1][0] = right[1];
673 wMc[1][2] = forward[1];
675 wMc[2][0] = right[2];
677 wMc[2][2] = forward[2];
695 for (
unsigned int i = 0; i < r.
size(); i++) {
710 for (
unsigned int i = 0; i < r.
size(); i++) {
725 for (
unsigned int i = 0; i < r.
size(); i++) {
743 ned_M_enu[2][2] = -1;
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
vpColVector & normalize()
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
error that can be emitted by ViSP classes.
@ notInitialized
Used to indicate that a parameter is not initialized.
@ dimensionError
Bad dimension.
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static double msinc(double sinx, double x)
static bool isNaN(double value)
static std::vector< vpHomogeneousMatrix > getLocalTangentPlaneTransformations(const std::vector< std::pair< double, double > > &lonlatVec, double radius, vpHomogeneousMatrix(*toECEF)(double lonDeg, double latDeg, double radius))
static double rad(double deg)
static double getMedian(const std::vector< double > &v)
static double sinc(double x)
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
static vpHomogeneousMatrix lookAt(const vpColVector &from, const vpColVector &to, vpColVector tmp)
static int modulo(int a, int n)
static vpHomogeneousMatrix enu2ecef(double lonDeg, double latDeg, double radius)
static double lineFitting(const std::vector< vpImagePoint > &imPts, double &a, double &b, double &c)
static int round(double x)
static bool isFinite(double value)
static double getMean(const std::vector< double > &v)
static bool isInf(double value)
static vpHomogeneousMatrix enu2ned(const vpHomogeneousMatrix &enu_M)
static double mcosc(double cosx, double x)
static double deg(double rad)
static bool isNumber(const std::string &str)
static std::vector< std::pair< double, double > > computeRegularPointsOnSphere(unsigned int maxPoints)
static vpHomogeneousMatrix ned2ecef(double lonDeg, double latDeg, double radius)
Implementation of a matrix and operations on matrices.
vpColVector eigenValues() const
Implementation of a generic rotation vector.