12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wrap SimpleHelicopter factors and fix a bug in noisemodel dimension

release/4.3a0
Duy-Nguyen Ta 2013-04-30 17:21:42 +00:00
parent 7256c88bbd
commit 84ababc653
2 changed files with 18 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -27,7 +27,7 @@ class Reconstruction : public NoiseModelFactor3<Pose3, Pose3, LieVector> {
typedef NoiseModelFactor3<Pose3, Pose3, LieVector> Base; typedef NoiseModelFactor3<Pose3, Pose3, LieVector> Base;
public: public:
Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) : 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) { xiKey), h_(h) {
} }
virtual ~Reconstruction() {} virtual ~Reconstruction() {}
@ -92,7 +92,7 @@ public:
DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey, DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey,
double h, const Matrix& Inertia, const Vector& Fu, double m, double h, const Matrix& Inertia, const Vector& Fu, double m,
double mu = 1000.0) : 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) { h_(h), Inertia_(Inertia), Fu_(Fu), m_(m) {
} }
virtual ~DiscreteEulerPoincareHelicopter() {} virtual ~DiscreteEulerPoincareHelicopter() {}

16
gtsam_unstable/gtsam_unstable.h
View File

@ -5,6 +5,7 @@
// specify the classes from gtsam we are using // specify the classes from gtsam we are using
virtual class gtsam::Value; virtual class gtsam::Value;
virtual class gtsam::LieScalar; virtual class gtsam::LieScalar;
virtual class gtsam::LieVector;
virtual class gtsam::Point2; virtual class gtsam::Point2;
virtual class gtsam::Rot2; virtual class gtsam::Rot2;
virtual class gtsam::Pose2; 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; Vector evaluateError(const gtsam::LieScalar& pk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const;
}; };
#include <gtsam/base/LieVector.h>
#include <gtsam_unstable/dynamics/SimpleHelicopter.h>
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 // nonlinear