diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 8060ae7f4d..2910ce74c9 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -90,7 +90,7 @@ bool equal_with_abs_tol(const Eigen::DenseBase& A, const Eigen::DenseBas for(size_t j=0; j tol) + else if(std::abs(A(i,j) - B(i,j)) > tol) return false; } return true; diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 856b559c37..ac03c0f53f 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -84,7 +84,7 @@ bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) { for(size_t i=0; i tol) + if(std::abs(vec1[i] - vec2[i]) > tol) return false; } return true; @@ -97,7 +97,7 @@ bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol for(size_t i=0; i tol) + if(std::abs(vec1[i] - vec2[i]) > tol) return false; } return true; @@ -145,14 +145,14 @@ bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) { bool flag = false; double scale = 1.0; size_t m = vec1.size(); for(size_t i=0; itol&&fabs(vec2[i])tol)) + if((std::abs(vec1[i])>tol && std::abs(vec2[i])tol)) return false; if(vec1[i] == 0 && vec2[i] == 0) continue; if (!flag) { scale = vec1[i] / vec2[i]; flag = true ; } - else if (fabs(vec1[i] - vec2[i]*scale) > tol) return false; + else if (std::abs(vec1[i] - vec2[i]*scale) > tol) return false; } return flag; } @@ -213,7 +213,7 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights, // Check once for zero entries of a. TODO: really needed ? vector isZero; - for (size_t i = 0; i < m; ++i) isZero.push_back(fabs(a[i]) < 1e-9); + for (size_t i = 0; i < m; ++i) isZero.push_back(std::abs(a[i]) < 1e-9); // If there is a valid (a!=0) constraint (sigma==0) return the first one for (size_t i = 0; i < m; ++i) { diff --git a/gtsam/base/deprecated/LieScalar.h b/gtsam/base/deprecated/LieScalar.h index 4e9bfb7dbb..6c9a5f766d 100644 --- a/gtsam/base/deprecated/LieScalar.h +++ b/gtsam/base/deprecated/LieScalar.h @@ -53,7 +53,7 @@ namespace gtsam { std::cout << name << ": " << d_ << std::endl; } bool equals(const LieScalar& expected, double tol = 1e-5) const { - return fabs(expected.d_ - d_) <= tol; + return std::abs(expected.d_ - d_) <= tol; } /// @} diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index c315b9c273..bd715e3cb2 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -165,7 +165,7 @@ TEST(Vector, weightedPseudoinverse ) // verify EXPECT(assert_equal(expected,actual)); - EXPECT(fabs(expPrecision-precision) < 1e-5); + EXPECT(std::abs(expPrecision-precision) < 1e-5); } /* ************************************************************************* */ diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 2ad9fc1e32..2efd069cc0 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -79,7 +79,7 @@ namespace gtsam { bool equals(const Node& q, double tol) const { const Leaf* other = dynamic_cast (&q); if (!other) return false; - return fabs(double(this->constant_ - other->constant_)) < tol; + return std::abs(double(this->constant_ - other->constant_)) < tol; } /** print */ diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 4ad1dffa2b..223bcc2421 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -59,9 +59,9 @@ void Cal3Bundler::print(const std::string& s) const { /* ************************************************************************* */ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { - if (fabs(f_ - K.f_) > tol || fabs(k1_ - K.k1_) > tol - || fabs(k2_ - K.k2_) > tol || fabs(u0_ - K.u0_) > tol - || fabs(v0_ - K.v0_) > tol) + if (std::abs(f_ - K.f_) > tol || std::abs(k1_ - K.k1_) > tol + || std::abs(k2_ - K.k2_) > tol || std::abs(u0_ - K.u0_) > tol + || std::abs(v0_ - K.v0_) > tol) return false; return true; } diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index 044d47de1c..070d16c6cc 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -30,9 +30,9 @@ void Cal3DS2::print(const std::string& s_) const { /* ************************************************************************* */ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { - if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || - fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || - fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol) + if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol || + std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol || + std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol) return false; return true; } diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index 2071b87921..6c03883cef 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -49,9 +49,9 @@ void Cal3DS2_Base::print(const std::string& s_) const { /* ************************************************************************* */ bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const { - if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || - fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || - fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol) + if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol || + std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol || + std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol) return false; return true; } diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index 8b7c1abf46..b1b9c37221 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -43,10 +43,10 @@ void Cal3Unified::print(const std::string& s) const { /* ************************************************************************* */ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const { - if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || - fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || - fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol || - fabs(xi_ - K.xi_) > tol) + if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol || + std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol || + std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol || + std::abs(xi_ - K.xi_) > tol) return false; return true; } diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index ca4e88eb6a..b3d1be4b68 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -64,15 +64,15 @@ void Cal3_S2::print(const std::string& s) const { /* ************************************************************************* */ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { - if (fabs(fx_ - K.fx_) > tol) + if (std::abs(fx_ - K.fx_) > tol) return false; - if (fabs(fy_ - K.fy_) > tol) + if (std::abs(fy_ - K.fy_) > tol) return false; - if (fabs(s_ - K.s_) > tol) + if (std::abs(s_ - K.s_) > tol) return false; - if (fabs(u0_ - K.u0_) > tol) + if (std::abs(u0_ - K.u0_) > tol) return false; - if (fabs(v0_ - K.v0_) > tol) + if (std::abs(v0_ - K.v0_) > tol) return false; return true; } diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp index 414fe6711c..9b5aea4eda 100644 --- a/gtsam/geometry/Cal3_S2Stereo.cpp +++ b/gtsam/geometry/Cal3_S2Stereo.cpp @@ -30,7 +30,7 @@ void Cal3_S2Stereo::print(const std::string& s) const { /* ************************************************************************* */ bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const { - if (fabs(b_ - other.b_) > tol) return false; + if (std::abs(b_ - other.b_) > tol) return false; return K_.equals(other.K_,tol); } diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 596118385f..61d8a30d2e 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -78,7 +78,7 @@ class GTSAM_EXPORT OrientedPlane3 { /// The equals function with tolerance bool equals(const OrientedPlane3& s, double tol = 1e-9) const { - return (n_.equals(s.n_, tol) && (fabs(d_ - s.d_) < tol)); + return (n_.equals(s.n_, tol) && (std::abs(d_ - s.d_) < tol)); } /// @} diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 74b9a2bec8..3d4bb753ea 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -27,7 +27,7 @@ namespace gtsam { double norm2(const Point2& p, OptionalJacobian<1,2> H) { double r = std::sqrt(p.x() * p.x() + p.y() * p.y()); if (H) { - if (fabs(r) > 1e-10) + if (std::abs(r) > 1e-10) *H << p.x() / r, p.y() / r; else *H << 1, 1; // really infinity, why 1 ? @@ -59,7 +59,7 @@ void Point2::print(const string& s) const { /* ************************************************************************* */ bool Point2::equals(const Point2& q, double tol) const { - return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol); + return (std::abs(x() - q.x()) < tol && std::abs(y() - q.y()) < tol); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 8df5f56074..8aa339a898 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -24,8 +24,8 @@ namespace gtsam { #ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS bool Point3::equals(const Point3 &q, double tol) const { - return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol && - fabs(z() - q.z()) < tol); + return (std::abs(x() - q.x()) < tol && std::abs(y() - q.y()) < tol && + std::abs(z() - q.z()) < tol); } void Point3::print(const string& s) const { @@ -98,7 +98,7 @@ double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, double norm3(const Point3 &p, OptionalJacobian<1, 3> H) { double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z()); if (H) { - if (fabs(r) > 1e-10) + if (std::abs(r) > 1e-10) *H << p.x() / r, p.y() / r, p.z() / r; else *H << 1, 1, 1; // really infinity, why 1 ? diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 4e5085cfe7..9c41a76c89 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -142,7 +142,7 @@ Matrix3 Pose2::adjointMap(const Vector3& v) { Matrix3 Pose2::ExpmapDerivative(const Vector3& v) { double alpha = v[2]; Matrix3 J; - if (fabs(alpha) > 1e-5) { + if (std::abs(alpha) > 1e-5) { // Chirikjian11book2, pg. 36 /* !!!Warning!!! Compare Iserles05an, formula 2.42 and Chirikjian11book2 pg.26 * Iserles' right-trivialization dexpR is actually the left Jacobian J_l in Chirikjian's notation @@ -174,7 +174,7 @@ Matrix3 Pose2::LogmapDerivative(const Pose2& p) { Vector3 v = Logmap(p); double alpha = v[2]; Matrix3 J; - if (fabs(alpha) > 1e-5) { + if (std::abs(alpha) > 1e-5) { double alphaInv = 1/alpha; double halfCotHalfAlpha = 0.5*sin(alpha)/(1-cos(alpha)); double v1 = v[0], v2 = v[1]; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index e720fe0b91..01611d7397 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -221,7 +221,7 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { #else // The closed-form formula in Barfoot14tro eq. (102) double phi = w.norm(); - if (fabs(phi)>1e-5) { + if (std::abs(phi)>1e-5) { const double sinPhi = sin(phi), cosPhi = cos(phi); const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; // Invert the sign of odd-order terms to have the right Jacobian diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 9f3ed35b07..04ed16774f 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -25,7 +25,7 @@ namespace gtsam { /* ************************************************************************* */ Rot2 Rot2::fromCosSin(double c, double s) { - if (fabs(c * c + s * s - 1.0) > 1e-9) { + if (std::abs(c * c + s * s - 1.0) > 1e-9) { double norm_cs = sqrt(c*c + s*s); c = c/norm_cs; s = s/norm_cs; @@ -46,13 +46,13 @@ void Rot2::print(const string& s) const { /* ************************************************************************* */ bool Rot2::equals(const Rot2& R, double tol) const { - return fabs(c_ - R.c_) <= tol && fabs(s_ - R.s_) <= tol; + return std::abs(c_ - R.c_) <= tol && std::abs(s_ - R.s_) <= tol; } /* ************************************************************************* */ Rot2& Rot2::normalize() { double scale = c_*c_ + s_*s_; - if(fabs(scale-1.0)>1e-10) { + if(std::abs(scale-1.0)>1e-10) { scale = pow(scale, -0.5); c_ *= scale; s_ *= scale; @@ -115,7 +115,7 @@ Point2 Rot2::unrotate(const Point2& p, /* ************************************************************************* */ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) { double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); - if(fabs(n) > 1e-5) { + if(std::abs(n) > 1e-5) { if (H) { *H << -y / d2, x / d2; } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index e0f3c34242..9eef01577d 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -62,8 +62,8 @@ class GTSAM_EXPORT StereoPoint2 { /** equals */ bool equals(const StereoPoint2& q, double tol = 1e-9) const { - return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol - && fabs(v_ - q.v_) < tol); + return (std::abs(uL_ - q.uL_) < tol && std::abs(uR_ - q.uR_) < tol + && std::abs(v_ - q.v_) < tol); } /// @} diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index f661f819db..533ee500ef 100755 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -68,7 +68,7 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { /* ************************************************************************* */ // Get the axis of rotation with the minimum projected length of the point static Point3 CalculateBestAxis(const Point3& n) { - double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); + double mx = std::abs(n.x()), my = std::abs(n.y()), mz = std::abs(n.z()); if ((mx <= my) && (mx <= mz)) { return Point3(1.0, 0.0, 0.0); } else if ((my <= mx) && (my <= mz)) { diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 7ba8851155..c923e398ba 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -218,10 +218,10 @@ TEST (EssentialMatrix, epipoles) { } // check rank 2 constraint - CHECK(fabs(S(2))<1e-10); + CHECK(std::abs(S(2))<1e-10); // check epipoles not at infinity - CHECK(fabs(U(2,2))>1e-10 && fabs(V(2,2))>1e-10); + CHECK(std::abs(U(2,2))>1e-10 && std::abs(V(2,2))>1e-10); // Check epipoles diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 4d9c7883b9..b5b2594723 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -847,7 +847,7 @@ void GemanMcClure::print(const std::string &s="") const { bool GemanMcClure::equals(const Base &expected, double tol) const { const GemanMcClure* p = dynamic_cast(&expected); if (p == NULL) return false; - return fabs(c_ - p->c_) < tol; + return std::abs(c_ - p->c_) < tol; } GemanMcClure::shared_ptr GemanMcClure::Create(double c, const ReweightScheme reweight) { @@ -879,7 +879,7 @@ void DCS::print(const std::string &s="") const { bool DCS::equals(const Base &expected, double tol) const { const DCS* p = dynamic_cast(&expected); if (p == NULL) return false; - return fabs(c_ - p->c_) < tol; + return std::abs(c_ - p->c_) < tol; } DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) { @@ -903,7 +903,7 @@ void L2WithDeadZone::print(const std::string &s="") const { bool L2WithDeadZone::equals(const Base &expected, double tol) const { const L2WithDeadZone* p = dynamic_cast(&expected); if (p == NULL) return false; - return fabs(k_ - p->k_) < tol; + return std::abs(k_ - p->k_) < tol; } L2WithDeadZone::shared_ptr L2WithDeadZone::Create(double k, const ReweightScheme reweight) { diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index e495921c2d..d65aa5e67a 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -750,7 +750,7 @@ namespace gtsam { Fair(double c = 1.3998, const ReweightScheme reweight = Block); double weight(double error) const { - return 1.0 / (1.0 + fabs(error) / c_); + return 1.0 / (1.0 + std::abs(error) / c_); } void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; @@ -832,7 +832,7 @@ namespace gtsam { Tukey(double c = 4.6851, const ReweightScheme reweight = Block); double weight(double error) const { - if (std::fabs(error) <= c_) { + if (std::abs(error) <= c_) { double xc2 = error*error/csquared_; return (1.0-xc2)*(1.0-xc2); } @@ -952,14 +952,14 @@ namespace gtsam { L2WithDeadZone(double k, const ReweightScheme reweight = Block); double residual(double error) const { - const double abs_error = fabs(error); + const double abs_error = std::abs(error); return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); } double weight(double error) const { // note that this code is slightly uglier than above, because there are three distinct // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two // cases (deadzone, non-deadzone) above. - if (fabs(error) <= k_) return 0.0; + if (std::abs(error) <= k_) return 0.0; else if (error > k_) return (-k_+error)/error; else return (k_+error)/error; } diff --git a/gtsam/navigation/ManifoldPreintegration.cpp b/gtsam/navigation/ManifoldPreintegration.cpp index cc88d91014..165acdaf0e 100644 --- a/gtsam/navigation/ManifoldPreintegration.cpp +++ b/gtsam/navigation/ManifoldPreintegration.cpp @@ -46,7 +46,7 @@ void ManifoldPreintegration::resetIntegration() { //------------------------------------------------------------------------------ bool ManifoldPreintegration::equals(const ManifoldPreintegration& other, double tol) const { - return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol + return p_->equals(*other.p_, tol) && std::abs(deltaTij_ - other.deltaTij_) < tol && biasHat_.equals(other.biasHat_, tol) && deltaXij_.equals(other.deltaXij_, tol) && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index ec91d69b83..8c29d85ddb 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -65,7 +65,7 @@ bool PreintegratedRotation::equals(const PreintegratedRotation& other, double tol) const { return this->matchesParamsWith(other) && deltaRij_.equals(other.deltaRij_, tol) - && fabs(deltaTij_ - other.deltaTij_) < tol + && std::abs(deltaTij_ - other.deltaTij_) < tol && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); } diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp index 0ac57d05c7..4229d4f0ce 100644 --- a/gtsam/navigation/TangentPreintegration.cpp +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -41,7 +41,7 @@ void TangentPreintegration::resetIntegration() { //------------------------------------------------------------------------------ bool TangentPreintegration::equals(const TangentPreintegration& other, double tol) const { - return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol + return p_->equals(*other.p_, tol) && std::abs(deltaTij_ - other.deltaTij_) < tol && biasHat_.equals(other.biasHat_, tol) && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol) && equal_with_abs_tol(preintegrated_H_biasAcc_, diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index 9a70678780..8ce9b361ed 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -177,7 +177,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( gttic(adjust_delta); // Compute gain ratio. Here we take advantage of the invariant that the // Bayes' net error at zero is equal to the nonlinear error - const double rho = fabs(f_error - result.f_error) < 1e-15 || fabs(M_error - new_M_error) < 1e-15 ? + const double rho = std::abs(f_error - result.f_error) < 1e-15 || std::abs(M_error - new_M_error) < 1e-15 ? 0.5 : (f_error - result.f_error) / (M_error - new_M_error); @@ -191,7 +191,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY) stay = false; // If not searching, just return with the new delta else if(mode == SEARCH_EACH_ITERATION) { - if(fabs(newDelta - delta) < 1e-15 || lastAction == DECREASED_DELTA) + if(std::abs(newDelta - delta) < 1e-15 || lastAction == DECREASED_DELTA) stay = false; // Searching, but Newton's solution is within trust region so keep the same trust region else { stay = true; // Searching and increased delta, so try again to increase delta diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index b9579661d4..c85891af2d 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -201,9 +201,9 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, double minAbsoluteTolerance = params_.relativeErrorTol * currentState->error; // if the change is small we terminate - if (fabs(costChange) < minAbsoluteTolerance) { + if (std::abs(costChange) < minAbsoluteTolerance) { if (verbose) - cout << "fabs(costChange)=" << fabs(costChange) + cout << "abs(costChange)=" << std::abs(costChange) << " minAbsoluteTolerance=" << minAbsoluteTolerance << " (relativeErrorTol=" << params_.relativeErrorTol << ")" << endl; stopSearchingLambda = true; diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 0486c1f296..abf6b257aa 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -107,7 +107,7 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { newStep - resphi * (newStep - minStep); if ((maxStep - minStep) - < tau * (std::fabs(testStep) + std::fabs(newStep))) { + < tau * (std::abs(testStep) + std::abs(newStep))) { return 0.5 * (minStep + maxStep); } diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 67e0d1f246..8fcf84a113 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -121,7 +121,7 @@ class Class : public Point3 { return norm3(*this, H); } bool equals(const Class &q, double tol) const { - return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol && fabs(z() - q.z()) < tol); + return (std::abs(x() - q.x()) < tol && std::abs(y() - q.y()) < tol && std::abs(z() - q.z()) < tol); } void print(const string& s) const { cout << s << *this << endl;} }; diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 9c412eb191..f949514e39 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -146,7 +146,7 @@ namespace gtsam { /** Syntactic sugar for constrained version */ BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) : BetweenFactor(key1, key2, measured, - noiseModel::Constrained::All(traits::GetDimension(measured), fabs(mu))) + noiseModel::Constrained::All(traits::GetDimension(measured), std::abs(mu))) {} private: diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 1aa85b6fe8..1c5ade5b6c 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -61,7 +61,7 @@ class FullIMUFactor : public NoiseModelFactor2 { return f && Base::equals(e) && equal_with_abs_tol(accel_, f->accel_, tol) && equal_with_abs_tol(gyro_, f->gyro_, tol) && - fabs(dt_ - f->dt_) < tol; + std::abs(dt_ - f->dt_) < tol; } void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index 5ed079acb2..bb0a354eee 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -54,7 +54,7 @@ class IMUFactor : public NoiseModelFactor2 { return f && Base::equals(e) && equal_with_abs_tol(accel_, f->accel_, tol) && equal_with_abs_tol(gyro_, f->gyro_, tol) && - fabs(dt_ - f->dt_) < tol; + std::abs(dt_ - f->dt_) < tol; } void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index 9ec10e39ff..209456a621 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -37,7 +37,7 @@ class PendulumFactor1: public NoiseModelFactor3 { ///Constructor. k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0) - : Base(noiseModel::Constrained::All(1, fabs(mu)), k1, k, velKey), h_(h) {} + : Base(noiseModel::Constrained::All(1, std::abs(mu)), k1, k, velKey), h_(h) {} /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { @@ -85,7 +85,7 @@ class PendulumFactor2: public NoiseModelFactor3 { ///Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0) - : Base(noiseModel::Constrained::All(1, fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {} + : Base(noiseModel::Constrained::All(1, std::abs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {} /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { @@ -135,7 +135,7 @@ class PendulumFactorPk: public NoiseModelFactor3 { ///Constructor PendulumFactorPk(Key pKey, Key qKey, Key qKey1, double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0) - : Base(noiseModel::Constrained::All(1, fabs(mu)), pKey, qKey, qKey1), + : Base(noiseModel::Constrained::All(1, std::abs(mu)), pKey, qKey, qKey1), h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} /// @return a deep copy of this factor @@ -191,7 +191,7 @@ class PendulumFactorPk1: public NoiseModelFactor3 { ///Constructor PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1, double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0) - : Base(noiseModel::Constrained::All(1, fabs(mu)), pKey1, qKey, qKey1), + : Base(noiseModel::Constrained::All(1, std::abs(mu)), pKey1, qKey, qKey1), h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} /// @return a deep copy of this factor diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index c8c46ee7ba..cf21c315b9 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -99,7 +99,7 @@ PoseRTV PoseRTV::flyingDynamics( double yaw2 = r2.yaw(); double pitch2 = r2.pitch(); double forward_accel = -thrust * sin(pitch2); // r2, pitch (in global frame?) controls forward force - double loss_lift = lift*fabs(sin(pitch2)); + double loss_lift = lift*std::abs(sin(pitch2)); Rot3 yaw_correction_bn = Rot3::Yaw(yaw2); Point3 forward(forward_accel, 0.0, 0.0); Vector Acc_n = diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index f98879e410..721d0265b2 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -27,7 +27,7 @@ class VelocityConstraint3 : public NoiseModelFactor3 { ///TODO: comment VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu = 1000.0) - : Base(noiseModel::Constrained::All(1, fabs(mu)), key1, key2, velKey), dt_(dt) {} + : Base(noiseModel::Constrained::All(1, std::abs(mu)), key1, key2, velKey), dt_(dt) {} virtual ~VelocityConstraint3() {} /// @return a deep copy of this factor diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp index 24bab3feb1..9dbeeac89b 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -198,7 +198,7 @@ int main(int argc, char** argv) { rangeNoise); // Throw out obvious outliers based on current landmark estimates Vector error = factor.unwhitenedError(landmarkEstimates); - if (k <= 200 || fabs(error[0]) < 5) + if (k <= 200 || std::abs(error[0]) < 5) newFactors.push_back(factor); } totalCount += 1; diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp index a83eb06acc..a63a0ba20a 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp @@ -167,7 +167,7 @@ int main(int argc, char** argv) { RangeFactor factor(i, symbol('L', j), range, rangeNoise); // Throw out obvious outliers based on current landmark estimates Vector error = factor.unwhitenedError(landmarkEstimates); - if (k <= 200 || fabs(error[0]) < 5) + if (k <= 200 || std::abs(error[0]) < 5) newFactors.push_back(factor); k = k + 1; countK = countK + 1; diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp index 4237170f01..2a2afa4769 100644 --- a/gtsam_unstable/geometry/Pose3Upright.cpp +++ b/gtsam_unstable/geometry/Pose3Upright.cpp @@ -44,7 +44,7 @@ void Pose3Upright::print(const std::string& s) const { /* ************************************************************************* */ bool Pose3Upright::equals(const Pose3Upright& x, double tol) const { - return T_.equals(x.T_, tol) && fabs(z_ - x.z_) < tol; + return T_.equals(x.T_, tol) && std::abs(z_ - x.z_) < tol; } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index 00bd28ec7d..eb732f2c57 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -77,7 +77,7 @@ bool SimPolygon2D::contains(const Point2& c) const { Point2 dab = ab.b() - ab.a(); Point2 dac = c - ab.a(); double cross = dab.x() * dac.y() - dab.y() * dac.x(); - if (fabs(cross) < 1e-6) // check for on one of the edges + if (std::abs(cross) < 1e-6) // check for on one of the edges return true; bool side = cross > 0; // save the first side found @@ -241,14 +241,14 @@ double SimPolygon2D::randomDistance(double mu, double sigma, double min_dist) { boost::variate_generator > gen_d(rng, norm_dist); double d = -10.0; for (size_t i=0; i min_dist) return d; } cout << "Non viable distance: " << d << " with mu = " << mu << " sigma = " << sigma << " min_dist = " << min_dist << endl; throw runtime_error("Failed to find a viable distance"); - return fabs(norm_dist(rng)); + return std::abs(norm_dist(rng)); } /* ***************************************************************** */ @@ -313,7 +313,7 @@ Pose2 SimPolygon2D::randomFreePose(double boundary_size, const vector pt) cons } // handle vertical case to avoid calculating slope - if (fabs(Ba.x() - Bb.x()) < 1e-5) { + if (std::abs(Ba.x() - Bb.x()) < 1e-5) { if (debug) cout << "vertical line" << endl; if (Ba.x() < len && Ba.x() > 0.0) { if (pt) *pt = transform.transformFrom(Point2(Ba.x(), 0.0)); @@ -88,7 +88,7 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons // find x-intercept double slope = (high.y() - low.y())/(high.x() - low.x()); if (debug) cout << "slope " << slope << endl; - double xint = (low.x() < high.x()) ? low.x() + fabs(low.y())/slope : high.x() - fabs(high.y())/slope; + double xint = (low.x() < high.x()) ? low.x() + std::abs(low.y())/slope : high.x() - std::abs(high.y())/slope; if (debug) cout << "xintercept " << xint << endl; if (xint > 0.0 && xint < len) { if (pt) *pt = transform.transformFrom(Point2(xint, 0.0)); diff --git a/gtsam_unstable/linear/ActiveSetSolver-inl.h b/gtsam_unstable/linear/ActiveSetSolver-inl.h index 18dc07aec5..602012090a 100644 --- a/gtsam_unstable/linear/ActiveSetSolver-inl.h +++ b/gtsam_unstable/linear/ActiveSetSolver-inl.h @@ -252,7 +252,7 @@ Template InequalityFactorGraph This::identifyActiveConstraints( double error = workingFactor->error(initialValues); // Safety guard. This should not happen unless users provide a bad init if (error > 0) throw InfeasibleInitialValues(); - if (fabs(error) < 1e-7) + if (std::abs(error) < 1e-7) workingFactor->activate(); else workingFactor->inactivate(); diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp index 9c04f0eecc..34a23982f2 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp @@ -38,7 +38,7 @@ void FixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormat /* ************************************************************************* */ bool FixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { - return std::fabs(smootherLag_ - rhs.smootherLag_) < tol + return std::abs(smootherLag_ - rhs.smootherLag_) < tol && std::equal(timestampKeyMap_.begin(), timestampKeyMap_.end(), rhs.timestampKeyMap_.begin()); } diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 0a3fa02839..ef03eb2c2d 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -152,7 +152,7 @@ bool AHRS::isAidingAvailable(const Mechanization_bRn2& mech, double mu_f = f_.norm() - ge; // accelerometer same magnitude as local gravity ? double mu_u = u_.norm(); // gyro says we are not maneuvering ? - return (fabs(mu_f)<0.5 && mu_u < 5.0 / 180.0 * M_PI); + return (std::abs(mu_f)<0.5 && mu_u < 5.0 / 180.0 * M_PI); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/RelativeElevationFactor.cpp b/gtsam_unstable/slam/RelativeElevationFactor.cpp index db077994fe..32e8731cd8 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/RelativeElevationFactor.cpp @@ -38,7 +38,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p /* ************************************************************************* */ bool RelativeElevationFactor::equals(const NonlinearFactor& expected, double tol) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->measured_) < tol; + return e != NULL && Base::equals(*e, tol) && std::abs(this->measured_ - e->measured_) < tol; } /* ************************************************************************* */