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

simulated2D now allows for custom variable types (as long as they are still Point2)

release/4.3a0
Alex Cunningham 2010-09-27 18:46:13 +00:00
parent 9ffd075225
commit dbe01bd0c8
1 changed files with 14 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
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;
Point z_;
Point2 z_; GenericPrior(const Point& z, const SharedGaussian& model, const Key& key) :
GenericPrior(const Point2& z, const SharedGaussian& model, const Key& key) :
NonlinearFactor1<Cfg, Key> (model, key), z_(z) { NonlinearFactor1<Cfg, Key> (model, key), z_(z) {
} }
Vector evaluateError(const Point2& x, boost::optional<Matrix&> H = Vector evaluateError(const Point& x, boost::optional<Matrix&> H =
boost::none) const { boost::none) const {
return (prior(x, H) - z_).vector(); return (prior(x, H) - z_).vector();
} }
@ -77,14 +77,15 @@ 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;
Point2 z_; typedef typename PoseKey::Value_t Pose;
Pose z_;
GenericOdometry(const Point2& z, const SharedGaussian& model, GenericOdometry(const Pose& z, const SharedGaussian& model,
const Key& i1, const Key& i2) : const Key& i1, const Key& i2) :
NonlinearFactor2<Cfg, Key, Key> (model, i1, i2), z_(z) { NonlinearFactor2<Cfg, Key, Key> (model, i1, i2), z_(z) {
} }
Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< Vector evaluateError(const Pose& x1, const Pose& x2, boost::optional<
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const { Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
return (odo(x1, x2, H1, H2) - z_).vector(); return (odo(x1, x2, H1, H2) - z_).vector();
} }
@ -98,14 +99,17 @@ 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;
Point2 z_; typedef typename PoseKey::Value_t Pose;
typedef typename PointKey::Value_t Point;
GenericMeasurement(const Point2& z, const SharedGaussian& model, Point z_;
GenericMeasurement(const Point& z, const SharedGaussian& model,
const XKey& i, const LKey& j) : const XKey& i, const LKey& j) :
NonlinearFactor2<Cfg, XKey, LKey> (model, i, j), z_(z) { NonlinearFactor2<Cfg, XKey, LKey> (model, i, j), z_(z) {
} }
Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< Vector evaluateError(const Pose& x1, const Point& x2, boost::optional<
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const { Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
return (mea(x1, x2, H1, H2) - z_).vector(); return (mea(x1, x2, H1, H2) - z_).vector();
} }