No more LieVector/LieScalar
							parent
							
								
									f1dd554a9d
								
							
						
					
					
						commit
						da3677e704
					
				|  | @ -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 */ | ||||
|  |  | |||
|  | @ -19,7 +19,6 @@ | |||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| #include <gtsam/geometry/Rot2.h> | ||||
| #include <gtsam/geometry/Rot3.h> | ||||
| #include <gtsam/base/LieScalar.h> | ||||
| 
 | ||||
| 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<LieScalar, Unit3, Point3> { | ||||
| class MagFactor3: public NoiseModelFactor3<double, Unit3, Point3> { | ||||
| 
 | ||||
|   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<LieScalar, Unit3, Point3>(model, key1, key2, key3), //
 | ||||
|       NoiseModelFactor3<double, Unit3, Point3>(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<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 = | ||||
|           boost::none) const { | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -211,7 +211,7 @@ TEST (EssentialMatrixFactor2, minimization) { | |||
|   EssentialMatrix actual = result.at<EssentialMatrix>(100); | ||||
|   EXPECT(assert_equal(trueE, actual, 1e-1)); | ||||
|   for (size_t i = 0; i < 5; i++) | ||||
|     EXPECT(assert_equal(truth.at<double>(i), result.at<double>(i), 1e-1)); | ||||
|     EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(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<EssentialMatrix>(100); | ||||
|   EXPECT(assert_equal(bodyE, actual, 1e-1)); | ||||
|   for (size_t i = 0; i < 5; i++) | ||||
|     EXPECT(assert_equal(truth.at<double>(i), result.at<double>(i), 1e-1)); | ||||
|     EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(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<EssentialMatrix>(100); | ||||
|   EXPECT(assert_equal(trueE, actual, 1e-1)); | ||||
|   for (size_t i = 0; i < data.number_tracks(); i++) | ||||
|     EXPECT(assert_equal(truth.at<double>(i), result.at<double>(i), 1e-1)); | ||||
|     EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1); | ||||
| 
 | ||||
|   // Check error at result
 | ||||
|   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); | ||||
|  |  | |||
|  | @ -78,9 +78,9 @@ public: | |||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none) const { | ||||
|     const Vector meas = z(); | ||||
|     if (H1) *H1 = numericalDerivative21<LieVector, PoseRTV, PoseRTV>( | ||||
|     if (H1) *H1 = numericalDerivative21<Vector, PoseRTV, PoseRTV>( | ||||
|         boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); | ||||
|     if (H2) *H2 = numericalDerivative22<LieVector, PoseRTV, PoseRTV>( | ||||
|     if (H2) *H2 = numericalDerivative22<Vector, PoseRTV, PoseRTV>( | ||||
|         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); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
|  |  | |||
|  | @ -9,7 +9,6 @@ | |||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/base/LieScalar.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| 
 | ||||
| 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<LieScalar, LieScalar, LieScalar> { | ||||
| class PendulumFactor1: public NoiseModelFactor3<double, double, double> { | ||||
| public: | ||||
| 
 | ||||
| protected: | ||||
|   typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base; | ||||
|   typedef NoiseModelFactor3<double, double, double> 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<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, | ||||
|       boost::optional<Matrix&> 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<LieScalar, LieScalar, LieScalar> { | ||||
| class PendulumFactor2: public NoiseModelFactor3<double, double, double> { | ||||
| public: | ||||
| 
 | ||||
| protected: | ||||
|   typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base; | ||||
|   typedef NoiseModelFactor3<double, double, double> 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<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, | ||||
|       boost::optional<Matrix&> 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<LieScalar, LieScalar, LieScalar> { | ||||
| class PendulumFactorPk: public NoiseModelFactor3<double, double, double> { | ||||
| public: | ||||
| 
 | ||||
| protected: | ||||
|   typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base; | ||||
|   typedef NoiseModelFactor3<double, double, double> 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<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, | ||||
|       boost::optional<Matrix&> 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<LieScalar, LieScalar, LieScalar> { | ||||
| class PendulumFactorPk1: public NoiseModelFactor3<double, double, double> { | ||||
| public: | ||||
| 
 | ||||
| protected: | ||||
|   typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base; | ||||
|   typedef NoiseModelFactor3<double, double, double> 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<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, | ||||
|       boost::optional<Matrix&> 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
 | ||||
|  |  | |||
|  | @ -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 <gtsam/base/LieScalar.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| class VelocityConstraint3 : public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> { | ||||
| class VelocityConstraint3 : public NoiseModelFactor3<double, double, double> { | ||||
| public: | ||||
| 
 | ||||
| protected: | ||||
|   typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base; | ||||
|   typedef NoiseModelFactor3<double, double, double> 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<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, | ||||
|       boost::optional<Matrix&> 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: | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue