12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #115 from borglab/fix/absolute-value 

C++11 Absolute Value
release/4.3a0
Frank Dellaert 2019-09-19 07:45:40 -04:00 committed by GitHub
parent 8212cb2f8b ff1f37c26f
commit a614e5efa1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
44 changed files with 84 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/base/Matrix.h
View File

@ -90,7 +90,7 @@ bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBas
for(size_t j=0; j<n1; j++) {
if(boost::math::isnan(A(i,j)) ^ boost::math::isnan(B(i,j)))
return false;
else if(fabs(A(i,j) - B(i,j)) > tol)
else if(std::abs(A(i,j) - B(i,j)) > tol)
return false;
}
return true;

10
gtsam/base/Vector.cpp
View File

@ -84,7 +84,7 @@ bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) {
for(size_t i=0; i<m; ++i) {
if(std::isnan(vec1[i]) ^ std::isnan(vec2[i]))
return false;
if(fabs(vec1[i] - vec2[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<m; ++i) {
if(std::isnan(vec1[i]) ^ std::isnan(vec2[i]))
return false;
if(fabs(vec1[i] - vec2[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; i<m; i++) {
if((fabs(vec1[i])>tol&&fabs(vec2[i])<tol) || (fabs(vec1[i])<tol&&fabs(vec2[i])>tol))
if((std::abs(vec1[i])>tol && std::abs(vec2[i])<tol) || (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<bool> 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) {

2
gtsam/base/deprecated/LieScalar.h
View File

@ -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;
}
/// @}

2
gtsam/base/tests/testVector.cpp
View File

@ -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);
}
/* ************************************************************************* */

2
gtsam/discrete/DecisionTree-inl.h
View File

@ -79,7 +79,7 @@ namespace gtsam {
bool equals(const Node& q, double tol) const {
const Leaf* other = dynamic_cast<const Leaf*> (&q);
if (!other) return false;
return fabs(double(this->constant_ - other->constant_)) < tol;
return std::abs(double(this->constant_ - other->constant_)) < tol;
}
/** print */

6
gtsam/geometry/Cal3Bundler.cpp
View File

@ -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;
}

6
gtsam/geometry/Cal3DS2.cpp
View File

@ -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;
}

6
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -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;
}

8
gtsam/geometry/Cal3Unified.cpp
View File

@ -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;
}

10
gtsam/geometry/Cal3_S2.cpp
View File

@ -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;
}

2
gtsam/geometry/Cal3_S2Stereo.cpp
View File

@ -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);
}

2
gtsam/geometry/OrientedPlane3.h
View File

@ -78,7 +78,7 @@ public:
/// 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));
}
/// @}

4
gtsam/geometry/Point2.cpp
View File

@ -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);
}
/* ************************************************************************* */

6
gtsam/geometry/Point3.cpp
View File

@ -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 ?

4
gtsam/geometry/Pose2.cpp
View File

@ -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];

2
gtsam/geometry/Pose3.cpp
View File

@ -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

8
gtsam/geometry/Rot2.cpp
View File

@ -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;
}

4
gtsam/geometry/StereoPoint2.h
View File

@ -62,8 +62,8 @@ public:
/** 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);
}
/// @}

2
gtsam/geometry/Unit3.cpp
View File

@ -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)) {

4
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -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

6
gtsam/linear/NoiseModel.cpp
View File

@ -865,7 +865,7 @@ void GemanMcClure::print(const std::string &s="") const {
bool GemanMcClure::equals(const Base &expected, double tol) const {
const GemanMcClure* p = dynamic_cast<const GemanMcClure*>(&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) {
@ -897,7 +897,7 @@ void DCS::print(const std::string &s="") const {
bool DCS::equals(const Base &expected, double tol) const {
const DCS* p = dynamic_cast<const DCS*>(&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) {
@ -921,7 +921,7 @@ void L2WithDeadZone::print(const std::string &s="") const {
bool L2WithDeadZone::equals(const Base &expected, double tol) const {
const L2WithDeadZone* p = dynamic_cast<const L2WithDeadZone*>(&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) {

8
gtsam/linear/NoiseModel.h
View File

@ -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);
}
@ -984,14 +984,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;
}

2
gtsam/navigation/ManifoldPreintegration.cpp
View File

@ -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)

2
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -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);
}

2
gtsam/navigation/TangentPreintegration.cpp
View File

@ -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_,

4
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -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

4
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -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;

2
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
View File

@ -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);
}

2
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -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;}
};

2
gtsam/slam/BetweenFactor.h
View File

@ -146,7 +146,7 @@ namespace gtsam {
/** Syntactic sugar for constrained version */
BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) :
BetweenFactor<VALUE>(key1, key2, measured,
noiseModel::Constrained::All(traits<VALUE>::GetDimension(measured), fabs(mu)))
noiseModel::Constrained::All(traits<VALUE>::GetDimension(measured), std::abs(mu)))
{}
private:

2
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -61,7 +61,7 @@ public:
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 {

2
gtsam_unstable/dynamics/IMUFactor.h
View File

@ -54,7 +54,7 @@ public:
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 {

8
gtsam_unstable/dynamics/Pendulum.h
View File

@ -37,7 +37,7 @@ public:
///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 @@ public:
///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 @@ public:
///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 @@ public:
///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

2
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -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 =

2
gtsam_unstable/dynamics/VelocityConstraint3.h
View File

@ -27,7 +27,7 @@ public:
///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

2
gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
View File

@ -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;

2
gtsam_unstable/examples/SmartRangeExample_plaza2.cpp
View File

@ -167,7 +167,7 @@ int main(int argc, char** argv) {
RangeFactor<Pose2, Point2> 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;

2
gtsam_unstable/geometry/Pose3Upright.cpp
View File

@ -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;
}
/* ************************************************************************* */

8
gtsam_unstable/geometry/SimPolygon2D.cpp
View File

@ -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<boost::minstd_rand&, boost::normal_distribution<double> > gen_d(rng, norm_dist);
double d = -10.0;
for (size_t i=0; i<max_it; ++i) {
d = fabs(gen_d());
d = std::abs(gen_d());
if (d > 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<SimPolygon
/* ***************************************************************** */
bool SimPolygon2D::insideBox(double s, const Point2& p) {
return fabs(p.x()) < s/2.0 && fabs(p.y()) < s/2.0;
return std::abs(p.x()) < s/2.0 && std::abs(p.y()) < s/2.0;
}
/* ***************************************************************** */

4
gtsam_unstable/geometry/SimWall2D.cpp
View File

@ -61,7 +61,7 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional<Point2&> 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<Point2&> 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));

2
gtsam_unstable/linear/ActiveSetSolver-inl.h
View File

@ -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();

2
gtsam_unstable/nonlinear/FixedLagSmoother.cpp
View File

@ -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());
}

2
gtsam_unstable/slam/AHRS.cpp
View File

@ -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);
}
/* ************************************************************************* */

2
gtsam_unstable/slam/RelativeElevationFactor.cpp
View File

@ -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<const This*> (&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;
}
/* ************************************************************************* */