diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index d2c1fb738..4095cc693 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -204,11 +204,6 @@ Vector ediv_(const Vector &a, const Vector &b) { return c; } -/* ************************************************************************* */ -double norm_2(const Vector& v) { - return v.norm(); -} - /* ************************************************************************* */ // imperative version, pass in x double houseInPlace(Vector &v) { diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index eeb2aa600..0b825bcfb 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -228,14 +228,6 @@ GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b); */ GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b); -/** - * Calculates L2 norm for a vector - * modeled after boost.ublas for compatibility - * @param v vector - * @return the L2 norm - */ -GTSAM_EXPORT double norm_2(const Vector& v); - /** * Dot product */ @@ -317,6 +309,7 @@ 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 double norm_2(const Vector& v) {return v.norm();} GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();} GTSAM_EXPORT inline double sum(const Vector &a){return a.sum();} #endif diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 1aba34bd6..0e7da537a 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -148,7 +148,7 @@ TEST(Pose3, Adjoint_full) Pose3 Agrawal06iros(const Vector& xi) { Vector w = xi.head(3); Vector v = xi.tail(3); - double t = norm_2(w); + double t = w.norm(); if (t < 1e-5) return Pose3(Rot3(), Point3(v)); else { diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 5364d72f6..9621898bf 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -96,7 +96,7 @@ TEST( Rot3, equals) /* ************************************************************************* */ // Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + .... Rot3 slow_but_correct_Rodrigues(const Vector& w) { - double t = norm_2(w); + double t = w.norm(); Matrix3 J = skewSymmetric(w / t); if (t < 1e-5) return Rot3(); Matrix3 R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J); @@ -130,7 +130,7 @@ TEST( Rot3, Rodrigues2) TEST( Rot3, Rodrigues3) { Vector w = Vector3(0.1, 0.2, 0.3); - Rot3 R1 = Rot3::AxisAngle(w / norm_2(w), norm_2(w)); + Rot3 R1 = Rot3::AxisAngle(w / w.norm(), w.norm()); Rot3 R2 = slow_but_correct_Rodrigues(w); CHECK(assert_equal(R2,R1)); } diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 02911acb1..208d0e709 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -268,7 +268,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) { const Vector3 x = thetahat; // parametrization of so(3) const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - double normx = norm_2(x); + double normx = x.norm(); const Matrix3 actualDelFdeltheta = I_3x3 + 0.5 * X + (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X * X; diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index e441c37ff..daecb56bf 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -652,7 +652,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) { Vector e1 = sfm1.evaluateError(values.at(c1), values.at(l1)); Vector e2 = sfm2.evaluateError(values.at(c2), values.at(l1)); double actualError = 0.5 - * (norm_2(e1) * norm_2(e1) + norm_2(e2) * norm_2(e2)); + * (e1.norm() * e1.norm() + e2.norm() * e2.norm()); double actualErrorGraph = generalGraph.error(values); DOUBLES_EQUAL(expectedErrorGraph, actualErrorGraph, 1e-7); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index 20ffbdd4f..2205f53bd 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -33,7 +33,7 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, if(g_e == 0) { if (flat) // acceleration measured is along the z-axis. - b_g = (Vector3(3) << 0.0, 0.0, norm_2(meanF)).finished(); + b_g = (Vector3(3) << 0.0, 0.0, meanF.norm()).finished(); else // acceleration measured is the opposite of gravity (10.13) b_g = -meanF;