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

made simulated2D factors more generic

release/4.3a0
Alex Cunningham 2010-09-28 16:26:44 +00:00
parent 65d9cf881a
commit 4455bd0c1a
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
slam/simulated2D.h
View File

@ -57,14 +57,14 @@ namespace gtsam {
template<class Cfg = Config, class Key = PoseKey> template<class Cfg = Config, class Key = PoseKey>
struct GenericPrior: public NonlinearFactor1<Cfg, Key> { struct GenericPrior: public NonlinearFactor1<Cfg, Key> {
typedef boost::shared_ptr<GenericPrior<Cfg, Key> > shared_ptr; typedef boost::shared_ptr<GenericPrior<Cfg, Key> > shared_ptr;
typedef typename PoseKey::Value_t Point; typedef typename Key::Value_t Pose;
Point z_; Pose z_;
GenericPrior(const Point& z, const SharedGaussian& model, const Key& key) : GenericPrior(const Pose& z, const SharedGaussian& model, const Key& key) :
NonlinearFactor1<Cfg, Key> (model, key), z_(z) { NonlinearFactor1<Cfg, Key> (model, key), z_(z) {
} }
Vector evaluateError(const Point& x, boost::optional<Matrix&> H = Vector evaluateError(const Pose& x, boost::optional<Matrix&> H =
boost::none) const { boost::none) const {
return (prior(x, H) - z_).vector(); return (prior(x, H) - z_).vector();
} }
@ -77,7 +77,7 @@ namespace gtsam {
template<class Cfg = Config, class Key = PoseKey> template<class Cfg = Config, class Key = PoseKey>
struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Key> { struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Key> {
typedef boost::shared_ptr<GenericOdometry<Cfg, Key> > shared_ptr; typedef boost::shared_ptr<GenericOdometry<Cfg, Key> > shared_ptr;
typedef typename PoseKey::Value_t Pose; typedef typename Key::Value_t Pose;
Pose z_; Pose z_;
GenericOdometry(const Pose& z, const SharedGaussian& model, GenericOdometry(const Pose& z, const SharedGaussian& model,
@ -99,8 +99,8 @@ namespace gtsam {
class GenericMeasurement: public NonlinearFactor2<Cfg, XKey, LKey> { class GenericMeasurement: public NonlinearFactor2<Cfg, XKey, LKey> {
public: public:
typedef boost::shared_ptr<GenericMeasurement<Cfg, XKey, LKey> > shared_ptr; typedef boost::shared_ptr<GenericMeasurement<Cfg, XKey, LKey> > shared_ptr;
typedef typename PoseKey::Value_t Pose; typedef typename XKey::Value_t Pose;
typedef typename PointKey::Value_t Point; typedef typename LKey::Value_t Point;
Point z_; Point z_;