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

Made simulated2D factors more generic - can now specify key types

release/4.3a0
Alex Cunningham 2010-02-09 19:47:39 +00:00
parent 8ff64793f1
commit d2a4bdae3c
1 changed files with 16 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
cpp/simulated2D.h
View File

@ -52,13 +52,13 @@ namespace gtsam {
/** /**
* Unary factor encoding a soft prior on a vector * Unary factor encoding a soft prior on a vector
*/ */
template<class Cfg=Config> template<class Cfg=Config, class Key=PoseKey>
struct GenericPrior: public NonlinearFactor1<Cfg, PoseKey, Point2> { struct GenericPrior: public NonlinearFactor1<Cfg, Key, Point2> {
Point2 z_; Point2 z_;
GenericPrior(const Point2& z, const SharedGaussian& model, const PoseKey& key) : GenericPrior(const Point2& z, const SharedGaussian& model, const Key& key) :
NonlinearFactor1<Cfg, PoseKey, Point2> (model, key), z_(z) { NonlinearFactor1<Cfg, Key, Point2> (model, key), z_(z) {
} }
Vector evaluateError(const Point2& x, boost::optional<Matrix&> H = Vector evaluateError(const Point2& x, boost::optional<Matrix&> H =
@ -71,14 +71,14 @@ namespace gtsam {
/** /**
* Binary factor simulating "odometry" between two Vectors * Binary factor simulating "odometry" between two Vectors
*/ */
template<class Cfg=Config> template<class Cfg=Config, class Key=PoseKey>
struct GenericOdometry: public NonlinearFactor2<Cfg, PoseKey, Point2, PoseKey, struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Point2, Key,
Point2> { Point2> {
Point2 z_; Point2 z_;
GenericOdometry(const Point2& z, const SharedGaussian& model, const PoseKey& j1, GenericOdometry(const Point2& z, const SharedGaussian& model, const Key& j1,
const PoseKey& j2) : const Key& j2) :
z_(z), NonlinearFactor2<Cfg, PoseKey, Point2, PoseKey, Point2> ( z_(z), NonlinearFactor2<Cfg, Key, Point2, Key, Point2> (
model, j1, j2) { model, j1, j2) {
} }
@ -92,16 +92,15 @@ namespace gtsam {
/** /**
* Binary factor simulating "measurement" between two Vectors * Binary factor simulating "measurement" between two Vectors
*/ */
template<class Cfg=Config> template<class Cfg=Config, class XKey=PoseKey, class LKey=PointKey>
class GenericMeasurement: public NonlinearFactor2<Cfg, PoseKey, Point2, class GenericMeasurement: public NonlinearFactor2<Cfg, XKey, Point2, LKey, Point2> {
PointKey, Point2> {
public: public:
Point2 z_; Point2 z_;
GenericMeasurement(const Point2& z, const SharedGaussian& model, GenericMeasurement(const Point2& z, const SharedGaussian& model,
const PoseKey& j1, const PointKey& j2) : const XKey& j1, const LKey& j2) :
z_(z), NonlinearFactor2<Cfg, PoseKey, Point2, PointKey, Point2> ( z_(z), NonlinearFactor2<Cfg, XKey, Point2, LKey, Point2> (
model, j1, j2) { model, j1, j2) {
} }
@ -113,9 +112,9 @@ namespace gtsam {
}; };
/** Typedefs for regular use */ /** Typedefs for regular use */
typedef GenericPrior<Config> Prior; typedef GenericPrior<Config, PoseKey> Prior;
typedef GenericOdometry<Config> Odometry; typedef GenericOdometry<Config, PoseKey> Odometry;
typedef GenericMeasurement<Config> Measurement; typedef GenericMeasurement<Config, PoseKey, PointKey> Measurement;
} // namespace simulated2D } // namespace simulated2D
} // namespace gtsam } // namespace gtsam