47 #include <visp3/core/vpException.h>
48 #include <visp3/core/vpMath.h>
49 #include <visp3/core/vpMatrix.h>
51 #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
84 const double vpMath::ang_min_sinc = 1.0e-8;
85 const double vpMath::ang_min_mc = 2.5e-4;
94 #if defined(VISP_HAVE_FUNC_STD_ISNAN)
95 return std::isnan(value);
96 #elif defined(VISP_HAVE_FUNC_ISNAN)
98 #elif defined(VISP_HAVE_FUNC__ISNAN)
99 return (_isnan(value) != 0);
104 return (((
unsigned)(ieee754.u >> 32) & 0x7fffffff) + ((
unsigned)ieee754.u != 0) > 0x7ff00000) != 0;
115 #if defined(VISP_HAVE_FUNC_STD_ISNAN)
116 return std::isnan(value);
117 #elif defined(VISP_HAVE_FUNC_ISNAN)
119 #elif defined(VISP_HAVE_FUNC__ISNAN)
120 return (_isnan(value) != 0);
125 return ((
unsigned)ieee754.u & 0x7fffffff) > 0x7f800000;
138 #if defined(VISP_HAVE_FUNC_STD_ISINF)
139 return std::isinf(value);
140 #elif defined(VISP_HAVE_FUNC_ISINF)
146 return ((
unsigned)(ieee754.u >> 32) & 0x7fffffff) == 0x7ff00000 && (unsigned)ieee754.u == 0;
159 #if defined(VISP_HAVE_FUNC_STD_ISINF)
160 return std::isinf(value);
161 #elif defined(VISP_HAVE_FUNC_ISINF)
167 return ((
unsigned)ieee754.u & 0x7fffffff) == 0x7f800000;
179 #if defined(VISP_HAVE_FUNC_STD_ISFINITE)
180 return std::isfinite(value);
181 #elif defined(VISP_HAVE_FUNC_ISFINITE)
182 return isfinite(value);
183 #elif defined(VISP_HAVE_FUNC__FINITE)
184 return _finite(value);
198 #if defined(VISP_HAVE_FUNC_STD_ISFINITE)
199 return std::isfinite(value);
200 #elif defined(VISP_HAVE_FUNC_ISFINITE)
201 return isfinite(value);
202 #elif defined(VISP_HAVE_FUNC__FINITE)
203 return _finitef(value);
216 size_t str_size = str.size();
217 for (
size_t i = 0; i < str_size; ++i) {
218 if (isdigit(str[i]) ==
false) {
235 if (fabs(x) < ang_min_mc) {
236 return 0.5 - x * x / 24.0;
239 return ((1.0 - cosx) / x / x);
253 if (fabs(x) < ang_min_mc) {
254 return (1. / 6.0 - x * x / 120.0);
257 return ((1.0 - (sinx / x)) / x / x);
270 if (fabs(x) < ang_min_sinc) {
271 return 1.0 - x * x / 6.0;
287 if (fabs(x) < ang_min_sinc) {
288 return 1.0 - x * x / 6.0;
308 size_t size = v.size();
310 double sum = std::accumulate(v.begin(), v.end(), 0.0);
312 return sum / (
static_cast<double>(size));
328 std::vector<double> v_copy = v;
329 size_t size = v_copy.size();
332 std::nth_element(v_copy.begin(), v_copy.begin() + n, v_copy.end());
333 double val_n = v_copy[n];
335 if ((size % 2) == 1) {
339 std::nth_element(v_copy.begin(), v_copy.begin() + (n - 1), v_copy.end());
340 return 0.5 * (val_n + v_copy[n - 1]);
361 std::vector<double> diff(v.size());
362 #if VISP_CXX_STANDARD > VISP_CXX_STANDARD_98
363 std::transform(v.begin(), v.end(), diff.begin(), std::bind(std::minus<double>(), std::placeholders::_1, mean));
365 std::transform(v.begin(), v.end(), diff.begin(), std::bind2nd(std::minus<double>(), mean));
368 double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
369 double divisor =
static_cast<double> (v.size());
370 if (useBesselCorrection && (v.size() > 1)) {
371 divisor = divisor - 1;
374 return std::sqrt(sq_sum / divisor);
392 if (imPts.size() < 3) {
396 double x_mean = 0, y_mean = 0;
397 size_t imPts_size = imPts.size();
398 for (
size_t i = 0; i < imPts_size; ++i) {
400 x_mean += imPt.
get_u();
401 y_mean += imPt.
get_v();
403 x_mean /= imPts.size();
404 y_mean /= imPts.size();
407 imPts_size = imPts.size();
408 for (
size_t i = 0; i < imPts_size; ++i) {
410 AtA[0][0] += (imPt.
get_u() - x_mean) * (imPt.
get_u() - x_mean);
411 AtA[0][1] += (imPt.
get_u() - x_mean) * (imPt.
get_v() - y_mean);
412 AtA[1][1] += (imPt.
get_v() - y_mean) * (imPt.
get_v() - y_mean);
414 AtA[1][0] = AtA[0][1];
420 a = eigenvectors[0][0];
421 b = eigenvectors[1][0];
422 c = (a * x_mean) + (b * y_mean);
425 imPts_size = imPts.
size();
426 for (
size_t i = 0; i < imPts_size; ++i) {
427 double x0 = imPts[i].get_u();
428 double y0 = imPts[i].get_v();
430 error += std::fabs((a * x0) + ((b * y0) - c));
433 return error / imPts.size();
490 const unsigned int index_0 = 0;
491 const unsigned int index_1 = 1;
492 const unsigned int index_2 = 2;
493 const unsigned int index_3 = 3;
495 ecef_M_ned[index_0][index_0] = -sin(lat) * cos(lon);
496 ecef_M_ned[index_0][index_1] = -sin(lon);
497 ecef_M_ned[index_0][index_2] = -cos(lat) * cos(lon);
498 ecef_M_ned[index_0][index_3] = radius * cos(lon) * cos(lat);
499 ecef_M_ned[index_1][index_0] = -sin(lat) * sin(lon);
500 ecef_M_ned[index_1][index_1] = cos(lon);
501 ecef_M_ned[index_1][index_2] = -cos(lat) * sin(lon);
502 ecef_M_ned[index_1][index_3] = radius * sin(lon) * cos(lat);
503 ecef_M_ned[index_2][index_0] = cos(lat);
504 ecef_M_ned[index_2][index_1] = 0;
505 ecef_M_ned[index_2][index_2] = -sin(lat);
506 ecef_M_ned[index_2][index_3] = radius * sin(lat);
554 const unsigned int index_0 = 0;
555 const unsigned int index_1 = 1;
556 const unsigned int index_2 = 2;
557 const unsigned int index_3 = 3;
560 ecef_M_enu[index_0][index_0] = -sin(lon);
561 ecef_M_enu[index_0][index_1] = -sin(lat) * cos(lon);
562 ecef_M_enu[index_0][index_2] = cos(lat) * cos(lon);
563 ecef_M_enu[index_0][index_3] = radius * cos(lon) * cos(lat);
564 ecef_M_enu[index_1][index_0] = cos(lon);
565 ecef_M_enu[index_1][index_1] = -sin(lat) * sin(lon);
566 ecef_M_enu[index_1][index_2] = cos(lat) * sin(lon);
567 ecef_M_enu[index_1][index_3] = radius * sin(lon) * cos(lat);
568 ecef_M_enu[index_2][index_0] = 0;
569 ecef_M_enu[index_2][index_1] = cos(lat);
570 ecef_M_enu[index_2][index_2] = sin(lat);
571 ecef_M_enu[index_2][index_3] = radius * sin(lat);
592 assert(maxPoints > 0);
594 double a = (4.0 * M_PI) / maxPoints;
596 int m_theta =
static_cast<int>(
round(M_PI / d));
597 double d_theta = M_PI / m_theta;
598 double d_phi = a / d_theta;
600 std::vector<std::pair<double, double> > lonlat_vec;
601 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
602 lonlat_vec.reserve(
static_cast<unsigned int>(std::sqrt(maxPoints)));
604 lonlat_vec.reserve(
static_cast<unsigned int>(std::sqrt(
static_cast<double>(maxPoints))));
607 for (
int m = 0; m < m_theta; ++m) {
608 double theta = (M_PI * (m + 0.5)) / m_theta;
609 int m_phi =
static_cast<int>(
round((2.0 * M_PI * sin(theta)) / d_phi));
611 for (
int n = 0; n < m_phi; ++n) {
612 double phi = (2.0 * M_PI * n) / m_phi;
614 double lat = M_PI_2 - theta;
615 lonlat_vec.push_back(std::make_pair(
deg(lon),
deg(lat)));
642 LongLattToHomogeneous toECEF)
644 std::vector<vpHomogeneousMatrix> ecef_M_local_vec;
645 ecef_M_local_vec.reserve(lonlatVec.size());
646 size_t lonlatVec_size = lonlatVec.size();
647 for (
size_t i = 0; i < lonlatVec_size; ++i) {
648 double lonDeg = lonlatVec[i].first;
649 double latDeg = lonlatVec[i].second;
652 ecef_M_local_vec.push_back(ecef_M_local);
654 return ecef_M_local_vec;
679 assert(from.
size() == 3);
680 assert(to.
size() == 3);
681 assert(tmp.
size() == 3);
685 const unsigned int index_0 = 0;
686 const unsigned int index_1 = 1;
687 const unsigned int index_2 = 2;
688 const unsigned int index_3 = 3;
691 wMc[index_0][index_0] = right[index_0];
692 wMc[index_0][index_1] = up[index_0];
693 wMc[index_0][index_2] = forward[index_0];
694 wMc[index_0][index_3] = from[index_0];
695 wMc[index_1][index_0] = right[index_1];
696 wMc[index_1][index_1] = up[index_1];
697 wMc[index_1][index_2] = forward[index_1];
698 wMc[index_1][index_3] = from[index_1];
699 wMc[index_2][index_0] = right[index_2];
700 wMc[index_2][index_1] = up[index_2];
701 wMc[index_2][index_2] = forward[index_2];
702 wMc[index_2][index_3] = from[index_2];
719 unsigned int r_size = r.
size();
720 for (
unsigned int i = 0; i < r_size; ++i) {
735 unsigned int r_size = r.
size();
736 for (
unsigned int i = 0; i < r_size; ++i) {
751 unsigned int r_size = r.
size();
752 for (
unsigned int i = 0; i < r_size; ++i) {
766 const unsigned int index_0 = 0;
767 const unsigned int index_1 = 1;
768 const unsigned int index_2 = 2;
769 ned_M_enu[index_0][index_0] = 0;
770 ned_M_enu[index_0][index_1] = 1;
771 ned_M_enu[index_1][index_0] = 1;
772 ned_M_enu[index_1][index_1] = 0;
773 ned_M_enu[index_2][index_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 double rad(double deg)
static double getMedian(const std::vector< double > &v)
static double sinc(double x)
static std::vector< vpHomogeneousMatrix > getLocalTangentPlaneTransformations(const std::vector< std::pair< double, double > > &lonlatVec, double radius, LongLattToHomogeneous func)
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
static vpHomogeneousMatrix lookAt(const vpColVector &from, const vpColVector &to, vpColVector tmp)
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 float modulo(const float &value, const float &modulo)
Gives the rest of value divided by modulo when the quotient can only be an integer.
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.