diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 390609917..d2c1fb738 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -209,11 +209,6 @@ double norm_2(const Vector& v) { return v.norm(); } -/* ************************************************************************* */ -Vector esqrt(const Vector& v) { - return v.cwiseSqrt(); -} - /* ************************************************************************* */ // imperative version, pass in x double houseInPlace(Vector &v) { @@ -337,6 +332,4 @@ Vector concatVectors(size_t nrVectors, ...) return concatVectors(vs); } -/* ************************************************************************* */ - } // namespace gtsam diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index ea7ab08cf..eeb2aa600 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -236,13 +236,6 @@ GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b); */ GTSAM_EXPORT double norm_2(const Vector& v); -/** - * Elementwise sqrt of vector elements - * @param v is a vector - * @return [sqrt(a(i))] - */ -GTSAM_EXPORT Vector esqrt(const Vector& v); - /** * Dot product */ @@ -321,6 +314,7 @@ GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 GTSAM_EXPORT inline Vector abs(const Vector& v){return v.cwiseAbs();} +GTSAM_EXPORT inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();} GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();} GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();} diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 15b2dcf81..5212374a7 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -258,7 +258,7 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { if (variances(j) != variances(0)) goto full; return Isotropic::Variance(n, variances(0), true); } - full: return shared_ptr(new Diagonal(esqrt(variances))); + full: return shared_ptr(new Diagonal(variances.cwiseSqrt())); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 2f229cb22..cef270100 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -432,10 +432,10 @@ namespace gtsam { * standard devations, some of which might be zero */ static shared_ptr MixedVariances(const Vector& mu, const Vector& variances) { - return shared_ptr(new Constrained(mu, esqrt(variances))); + return shared_ptr(new Constrained(mu, variances.cwiseSqrt())); } static shared_ptr MixedVariances(const Vector& variances) { - return shared_ptr(new Constrained(esqrt(variances))); + return shared_ptr(new Constrained(variances.cwiseSqrt())); } /** diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 258bc5d03..ce8fd29f3 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -32,7 +32,7 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, // estimate standard deviation on gyroscope readings Pg_ = Cov(stationaryU); - Vector3 sigmas_v_g = esqrt(Pg_.diagonal() * T); + Vector3 sigmas_v_g = (Pg_.diagonal() * T).cwiseSqrt(); // estimate standard deviation on accelerometer readings Pa_ = Cov(stationaryF); @@ -52,7 +52,7 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, var_w_ << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a; // Quantities needed for aiding - sigmas_v_a_ = esqrt(T * Pa_.diagonal()); + sigmas_v_a_ = (T * Pa_.diagonal()).cwiseSqrt(); // gravity in nav frame n_g_ = (Vector(3) << 0.0, 0.0, g_e).finished();