diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index f36d2dbe6..8e1a2cf55 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -27,7 +27,7 @@ class Reconstruction : public NoiseModelFactor3 { typedef NoiseModelFactor3 Base; public: Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) : - Base(noiseModel::Constrained::All(Pose3::Dim()*3, fabs(mu)), gKey1, gKey, + Base(noiseModel::Constrained::All(Pose3::Dim(), fabs(mu)), gKey1, gKey, xiKey), h_(h) { } virtual ~Reconstruction() {} @@ -92,7 +92,7 @@ public: DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey, double h, const Matrix& Inertia, const Vector& Fu, double m, double mu = 1000.0) : - Base(noiseModel::Constrained::All(Pose3::Dim()*3, fabs(mu)), xiKey1, xiKey_1, gKey), + Base(noiseModel::Constrained::All(Pose3::Dim(), fabs(mu)), xiKey1, xiKey_1, gKey), h_(h), Inertia_(Inertia), Fu_(Fu), m_(m) { } virtual ~DiscreteEulerPoincareHelicopter() {} diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index cffa7ccde..ca27cd738 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -5,6 +5,7 @@ // specify the classes from gtsam we are using virtual class gtsam::Value; virtual class gtsam::LieScalar; +virtual class gtsam::LieVector; virtual class gtsam::Point2; virtual class gtsam::Rot2; virtual class gtsam::Pose2; @@ -412,6 +413,21 @@ virtual class PendulumFactorPk1 : gtsam::NonlinearFactor { Vector evaluateError(const gtsam::LieScalar& pk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const; }; +#include +#include + +virtual class Reconstruction : gtsam::NonlinearFactor { + Reconstruction(size_t gKey1, size_t gKey, size_t xiKey, double h); + + Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, const gtsam::LieVector& xiK) const; +}; + +virtual class DiscreteEulerPoincareHelicopter : gtsam::NonlinearFactor { + DiscreteEulerPoincareHelicopter(size_t xiKey, size_t xiKey_1, size_t gKey, + double h, Matrix Inertia, Vector Fu, double m); + + Vector evaluateError(const gtsam::LieVector& xiK, const gtsam::LieVector& xiK_1, const gtsam::Pose3& gK) const; +}; //************************************************************************* // nonlinear