diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 9bc24c152..18713505e 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -144,7 +144,7 @@ namespace imuBias { /// return dimensionality of tangent space inline size_t dim() const { return dimension; } - /** Update the LieVector with a tangent space update */ + /** Update the bias with a tangent space update */ inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); } /** @return the local coordinates of another object */ diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index fc9d4f62b..96a0971c5 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -19,7 +19,6 @@ #include #include #include -#include namespace gtsam { @@ -165,7 +164,7 @@ public: * This version uses model measured bM = scale * bRn * direction + bias * and optimizes for both scale, direction, and the bias. */ -class MagFactor3: public NoiseModelFactor3 { +class MagFactor3: public NoiseModelFactor3 { const Point3 measured_; ///< The measured magnetometer values const Rot3 bRn_; ///< The assumed known rotation from nav to body @@ -175,7 +174,7 @@ public: /** Constructor */ MagFactor3(Key key1, Key key2, Key key3, const Point3& measured, const Rot3& nRb, const SharedNoiseModel& model) : - NoiseModelFactor3(model, key1, key2, key3), // + NoiseModelFactor3(model, key1, key2, key3), // measured_(measured), bRn_(nRb.inverse()) { } @@ -190,7 +189,7 @@ public: * @param nM (unknown) local earth magnetic field vector, in nav frame * @param bias (unknown) 3D bias */ - Vector evaluateError(const LieScalar& scale, const Unit3& direction, + Vector evaluateError(double scale, const Unit3& direction, const Point3& bias, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index ed3b95150..5fca3343e 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -142,9 +142,9 @@ TEST( CombinedImuFactor, ErrorWithBiases ) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v1(0.5, 0.0, 0.0); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v2(0.5, 0.0, 0.0); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index b01db3ffe..46e283d34 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -211,7 +211,7 @@ TEST (EssentialMatrixFactor2, minimization) { EssentialMatrix actual = result.at(100); EXPECT(assert_equal(trueE, actual, 1e-1)); for (size_t i = 0; i < 5; i++) - EXPECT(assert_equal(truth.at(i), result.at(i), 1e-1)); + EXPECT_DOUBLES_EQUAL(truth.at(i), result.at(i), 1e-1); // Check error at result EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); @@ -284,7 +284,7 @@ TEST (EssentialMatrixFactor3, minimization) { EssentialMatrix actual = result.at(100); EXPECT(assert_equal(bodyE, actual, 1e-1)); for (size_t i = 0; i < 5; i++) - EXPECT(assert_equal(truth.at(i), result.at(i), 1e-1)); + EXPECT_DOUBLES_EQUAL(truth.at(i), result.at(i), 1e-1); // Check error at result EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); @@ -427,7 +427,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) { EssentialMatrix actual = result.at(100); EXPECT(assert_equal(trueE, actual, 1e-1)); for (size_t i = 0; i < data.number_tracks(); i++) - EXPECT(assert_equal(truth.at(i), result.at(i), 1e-1)); + EXPECT_DOUBLES_EQUAL(truth.at(i), result.at(i), 1e-1); // Check error at result EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index 165f4fe19..5c821b2bf 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -78,9 +78,9 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { const Vector meas = z(); - if (H1) *H1 = numericalDerivative21( + if (H1) *H1 = numericalDerivative21( boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); - if (H2) *H2 = numericalDerivative22( + if (H2) *H2 = numericalDerivative22( boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); return predict_proxy(x1, x2, dt_, meas); } @@ -95,10 +95,10 @@ public: private: /** copy of the measurement function formulated for numerical derivatives */ - static LieVector predict_proxy(const PoseRTV& x1, const PoseRTV& x2, + static Vector predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt, const Vector& meas) { Vector hx = x1.imuPrediction(x2, dt); - return LieVector(meas - hx); + return Vector(meas - hx); } }; diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index 8cfc3cbcd..970af848c 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -9,7 +9,6 @@ #pragma once -#include #include namespace gtsam { @@ -21,11 +20,11 @@ namespace gtsam { * - For implicit Euler method: q_{k+1} = q_k + h*v_{k+1} * - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1} */ -class PendulumFactor1: public NoiseModelFactor3 { +class PendulumFactor1: public NoiseModelFactor3 { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 Base; /** default constructor to allow for serialization */ PendulumFactor1() {} @@ -38,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(LieScalar::Dim(), fabs(mu)), k1, k, velKey), h_(h) {} + : Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), k1, k, velKey), h_(h) {} /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { @@ -46,15 +45,15 @@ public: gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); } /** q_k + h*v - q_k1 = 0, with optional derivatives */ - Vector evaluateError(const LieScalar& qk1, const LieScalar& qk, const LieScalar& v, + Vector evaluateError(double qk1, double qk, double v, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - const size_t p = LieScalar::Dim(); + const size_t p = double::Dim(); if (H1) *H1 = -eye(p); if (H2) *H2 = eye(p); if (H3) *H3 = eye(p)*h_; - return qk1.localCoordinates(qk.compose(LieScalar(v*h_))); + return qk1.localCoordinates(qk.compose(double(v*h_))); } }; // \PendulumFactor1 @@ -67,11 +66,11 @@ public: * - For implicit Euler method: v_{k+1} = v_k - h*g/L*sin(q_{k+1}) * - For sympletic Euler method: v_{k+1} = v_k - h*g/L*sin(q_k) */ -class PendulumFactor2: public NoiseModelFactor3 { +class PendulumFactor2: public NoiseModelFactor3 { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 Base; /** default constructor to allow for serialization */ PendulumFactor2() {} @@ -86,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(LieScalar::Dim(), fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {} + : Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {} /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { @@ -94,15 +93,15 @@ public: gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); } /** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */ - Vector evaluateError(const LieScalar& vk1, const LieScalar& vk, const LieScalar& q, + Vector evaluateError(double vk1, double vk, double q, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - const size_t p = LieScalar::Dim(); + const size_t p = double::Dim(); if (H1) *H1 = -eye(p); if (H2) *H2 = eye(p); if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q.value()); - return vk1.localCoordinates(LieScalar(vk - h_*g_/r_*sin(q))); + return vk1.localCoordinates(double(vk - h_*g_/r_*sin(q))); } }; // \PendulumFactor2 @@ -114,11 +113,11 @@ public: * p_k = -D_1 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)+mgrh(1-\alpha)\,\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) * = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1}) */ -class PendulumFactorPk: public NoiseModelFactor3 { +class PendulumFactorPk: public NoiseModelFactor3 { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 Base; /** default constructor to allow for serialization */ PendulumFactorPk() {} @@ -136,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(LieScalar::Dim(), fabs(mu)), pKey, qKey, qKey1), + : Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), pKey, qKey, qKey1), h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} /// @return a deep copy of this factor @@ -145,11 +144,11 @@ public: gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); } /** 1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */ - Vector evaluateError(const LieScalar& pk, const LieScalar& qk, const LieScalar& qk1, + Vector evaluateError(double pk, double qk, double qk1, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - const size_t p = LieScalar::Dim(); + const size_t p = double::Dim(); double qmid = (1-alpha_)*qk + alpha_*qk1; double mr2_h = 1/h_*m_*r_*r_; @@ -159,7 +158,7 @@ public: if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid)); - return pk.localCoordinates(LieScalar(mr2_h*(qk1-qk) + mgrh*(1-alpha_)*sin(qmid))); + return pk.localCoordinates(double(mr2_h*(qk1-qk) + mgrh*(1-alpha_)*sin(qmid))); } }; // \PendulumFactorPk @@ -170,11 +169,11 @@ public: * p_k1 = D_2 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)-mgrh\alpha\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) * = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1}) */ -class PendulumFactorPk1: public NoiseModelFactor3 { +class PendulumFactorPk1: public NoiseModelFactor3 { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 Base; /** default constructor to allow for serialization */ PendulumFactorPk1() {} @@ -192,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(LieScalar::Dim(), fabs(mu)), pKey1, qKey, qKey1), + : Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), pKey1, qKey, qKey1), h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} /// @return a deep copy of this factor @@ -201,11 +200,11 @@ public: gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); } /** 1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */ - Vector evaluateError(const LieScalar& pk1, const LieScalar& qk, const LieScalar& qk1, + Vector evaluateError(double pk1, double qk, double qk1, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - const size_t p = LieScalar::Dim(); + const size_t p = double::Dim(); double qmid = (1-alpha_)*qk + alpha_*qk1; double mr2_h = 1/h_*m_*r_*r_; @@ -215,7 +214,7 @@ public: if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid)); - return pk1.localCoordinates(LieScalar(mr2_h*(qk1-qk) - mgrh*alpha_*sin(qmid))); + return pk1.localCoordinates(double(mr2_h*(qk1-qk) - mgrh*alpha_*sin(qmid))); } }; // \PendulumFactorPk1 diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index 83506e2d5..74cddff10 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -1,21 +1,20 @@ /** * @file VelocityConstraint3.h - * @brief A simple 3-way factor constraining LieScalar poses and velocity + * @brief A simple 3-way factor constraining double poses and velocity * @author Duy-Nguyen Ta */ #pragma once -#include #include namespace gtsam { -class VelocityConstraint3 : public NoiseModelFactor3 { +class VelocityConstraint3 : public NoiseModelFactor3 { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 Base; /** default constructor to allow for serialization */ VelocityConstraint3() {} @@ -28,7 +27,7 @@ public: ///TODO: comment VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu = 1000.0) - : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), key1, key2, velKey), dt_(dt) {} + : Base(noiseModel::Constrained::All(double::Dim(), fabs(mu)), key1, key2, velKey), dt_(dt) {} virtual ~VelocityConstraint3() {} /// @return a deep copy of this factor @@ -37,15 +36,15 @@ public: gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); } /** x1 + v*dt - x2 = 0, with optional derivatives */ - Vector evaluateError(const LieScalar& x1, const LieScalar& x2, const LieScalar& v, + Vector evaluateError(double x1, double x2, double v, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - const size_t p = LieScalar::Dim(); + const size_t p = double::Dim(); if (H1) *H1 = eye(p); if (H2) *H2 = -eye(p); if (H3) *H3 = eye(p)*dt_; - return x2.localCoordinates(x1.compose(LieScalar(v*dt_))); + return x2.localCoordinates(x1.compose(double(v*dt_))); } private: