48 #include <visp3/core/vpException.h>
49 #include <visp3/core/vpMath.h>
50 #include <visp3/core/vpMatrix.h>
52 #if defined(VISP_HAVE_FUNC__FINITE)
56 #if !(defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN)) || \
57 !(defined(VISP_HAVE_FUNC_ISINF) || defined(VISP_HAVE_FUNC_STD_ISINF)) || \
58 !(defined(VISP_HAVE_FUNC_ISFINITE) || defined(VISP_HAVE_FUNC_STD_ISFINITE) || defined(VISP_HAVE_FUNC__FINITE))
59 #if defined _MSC_VER || defined __BORLANDC__
60 typedef __int64 int64;
61 typedef unsigned __int64 uint64;
63 typedef int64_t int64;
64 typedef uint64_t uint64;
67 #ifndef DOXYGEN_SHOULD_SKIP_THIS
68 typedef union Vp64suf {
74 typedef union Vp32suf {
82 const double vpMath::ang_min_sinc = 1.0e-8;
83 const double vpMath::ang_min_mc = 2.5e-4;
92 #if defined(VISP_HAVE_FUNC_ISNAN)
94 #elif defined(VISP_HAVE_FUNC_STD_ISNAN)
95 return std::isnan(value);
96 #elif defined(VISP_HAVE_FUNC__ISNAN)
97 return (_isnan(value) != 0);
102 return (((
unsigned)(ieee754.u >> 32) & 0x7fffffff) + ((
unsigned)ieee754.u != 0) > 0x7ff00000) != 0;
113 #if defined(VISP_HAVE_FUNC_ISNAN)
115 #elif defined(VISP_HAVE_FUNC_STD_ISNAN)
116 return std::isnan(value);
117 #elif defined(VISP_HAVE_FUNC__ISNAN)
118 return (_isnan(value) != 0);
123 return ((
unsigned)ieee754.u & 0x7fffffff) > 0x7f800000;
136 #if defined(VISP_HAVE_FUNC_ISINF)
138 #elif defined(VISP_HAVE_FUNC_STD_ISINF)
139 return std::isinf(value);
144 return ((
unsigned)(ieee754.u >> 32) & 0x7fffffff) == 0x7ff00000 && (unsigned)ieee754.u == 0;
157 #if defined(VISP_HAVE_FUNC_ISINF)
159 #elif defined(VISP_HAVE_FUNC_STD_ISINF)
160 return std::isinf(value);
165 return ((
unsigned)ieee754.u & 0x7fffffff) == 0x7f800000;
177 #if defined(VISP_HAVE_FUNC_ISFINITE)
178 return isfinite(value);
179 #elif defined(VISP_HAVE_FUNC_STD_ISFINITE)
180 return std::isfinite(value);
181 #elif defined(VISP_HAVE_FUNC__FINITE)
182 return _finite(value);
196 #if defined(VISP_HAVE_FUNC_ISFINITE)
197 return isfinite(value);
198 #elif defined(VISP_HAVE_FUNC_STD_ISFINITE)
199 return std::isfinite(value);
200 #elif defined(VISP_HAVE_FUNC__FINITE)
201 return _finitef(value);
217 if (fabs(x) < ang_min_mc)
220 return ((1.0 - cosx) / x / x);
233 if (fabs(x) < ang_min_mc)
236 return ((1.0 - sinx / x) / x / x);
248 if (fabs(x) < ang_min_sinc)
263 if (fabs(x) < ang_min_sinc)
282 size_t size = v.size();
284 double sum = std::accumulate(v.begin(), v.end(), 0.0);
286 return sum / (double)size;
302 std::vector<double> v_copy = v;
303 size_t size = v_copy.size();
306 std::nth_element(v_copy.begin(), v_copy.begin() + n, v_copy.end());
307 double val_n = v_copy[n];
312 std::nth_element(v_copy.begin(), v_copy.begin() + n - 1, v_copy.end());
313 return 0.5 * (val_n + v_copy[n - 1]);
334 std::vector<double> diff(v.size());
335 #if VISP_CXX_STANDARD > VISP_CXX_STANDARD_98
336 std::transform(v.begin(), v.end(), diff.begin(), std::bind(std::minus<double>(), std::placeholders::_1, mean));
338 std::transform(v.begin(), v.end(), diff.begin(), std::bind2nd(std::minus<double>(), mean));
341 double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
342 double divisor = (double)v.size();
343 if (useBesselCorrection && v.size() > 1) {
344 divisor = divisor - 1;
347 return std::sqrt(sq_sum / divisor);
365 if (imPts.size() < 3) {
369 double x_mean = 0, y_mean = 0;
370 for (
size_t i = 0; i < imPts.size(); i++) {
372 x_mean += imPt.
get_u();
373 y_mean += imPt.
get_v();
375 x_mean /= imPts.size();
376 y_mean /= imPts.size();
379 for (
size_t i = 0; i < imPts.size(); i++) {
381 AtA[0][0] += (imPt.
get_u() - x_mean) * (imPt.
get_u() - x_mean);
382 AtA[0][1] += (imPt.
get_u() - x_mean) * (imPt.
get_v() - y_mean);
383 AtA[1][1] += (imPt.
get_v() - y_mean) * (imPt.
get_v() - y_mean);
385 AtA[1][0] = AtA[0][1];
391 a = eigenvectors[0][0];
392 b = eigenvectors[1][0];
393 c = a * x_mean + b * y_mean;
396 for (
size_t i = 0; i < imPts.size(); i++) {
397 double x0 = imPts[i].get_u();
398 double y0 = imPts[i].get_v();
400 error += std::fabs(a * x0 + b * y0 - c);
403 return error / imPts.size();
462 ecef_M_ned[0][0] = -sin(lat)*cos(lon); ecef_M_ned[0][1] = -sin(lon); ecef_M_ned[0][2] = -cos(lat)*cos(lon); ecef_M_ned[0][3] = radius*cos(lon)*cos(lat);
463 ecef_M_ned[1][0] = -sin(lat)*sin(lon); ecef_M_ned[1][1] = cos(lon); ecef_M_ned[1][2] = -cos(lat)*sin(lon); ecef_M_ned[1][3] = radius*sin(lon)*cos(lat);
464 ecef_M_ned[2][0] = cos(lat); ecef_M_ned[2][1] = 0; ecef_M_ned[2][2] = -sin(lat); ecef_M_ned[2][3] = radius*sin(lat);
514 ecef_M_enu[0][0] = -sin(lon); ecef_M_enu[0][1] = -sin(lat)*cos(lon); ecef_M_enu[0][2] = cos(lat)*cos(lon); ecef_M_enu[0][3] = radius*cos(lon)*cos(lat);
515 ecef_M_enu[1][0] = cos(lon); ecef_M_enu[1][1] = -sin(lat)*sin(lon); ecef_M_enu[1][2] = cos(lat)*sin(lon); ecef_M_enu[1][3] = radius*sin(lon)*cos(lat);
516 ecef_M_enu[2][0] = 0; ecef_M_enu[2][1] = cos(lat); ecef_M_enu[2][2] = sin(lat); ecef_M_enu[2][3] = radius*sin(lat);
537 assert(maxPoints > 0);
539 double a = 4.0 * M_PI / maxPoints;
541 int m_theta = int(
round(M_PI / d));
542 double d_theta = M_PI / m_theta;
543 double d_phi = a / d_theta;
545 std::vector<std::pair<double, double> > lonlat_vec;
546 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
547 lonlat_vec.reserve(
static_cast<unsigned int>(std::sqrt(maxPoints)));
549 lonlat_vec.reserve(
static_cast<unsigned int>(std::sqrt(
static_cast<double>(maxPoints))));
551 for (
int m = 0; m < m_theta; m++) {
552 double theta = M_PI * (m + 0.5) / m_theta;
553 int m_phi =
static_cast<int>(
round(2.0 * M_PI * sin(theta) / d_phi));
555 for (
int n = 0; n < m_phi; n++) {
556 double phi = 2.0 * M_PI * n / m_phi;
558 double lat = M_PI_2 - theta;
559 lonlat_vec.push_back(std::make_pair(
deg(lon),
deg(lat)));
588 std::vector<vpHomogeneousMatrix> ecef_M_local_vec;
589 ecef_M_local_vec.reserve(lonlatVec.size());
590 for (
size_t i = 0; i < lonlatVec.size(); i++) {
591 double lonDeg = lonlatVec[i].first;
592 double latDeg = lonlatVec[i].second;
595 ecef_M_local_vec.push_back(ecef_M_local);
597 return ecef_M_local_vec;
622 assert(from.
size() == 3);
623 assert(to.
size() == 3);
624 assert(tmp.
size() == 3);
630 wMc[0][0] = right[0]; wMc[0][1] = up[0]; wMc[0][2] = forward[0]; wMc[0][3] = from[0];
631 wMc[1][0] = right[1]; wMc[1][1] = up[1]; wMc[1][2] = forward[1]; wMc[1][3] = from[1];
632 wMc[2][0] = right[2]; wMc[2][1] = up[2]; wMc[2][2] = forward[2]; wMc[2][3] = from[2];
649 for (
unsigned int i = 0; i < r.
size(); i++) {
664 for (
unsigned int i = 0; i < r.
size(); i++) {
681 ned[2] = -enu[2] + M_PI_2;
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 emited 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 vpColVector enu2ned(const vpRxyzVector &rxyz)
static double mcosc(double cosx, double x)
static double deg(double rad)
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.
Implementation of a rotation vector as Euler angle minimal representation.
Class that consider the case of a translation vector.