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

Used template parameters in simulated2D to make it possible to template factors on a config type with typedefs for the previous usage so no other code needs to change

release/4.3a0
Alex Cunningham 2010-02-05 14:49:40 +00:00
parent 040d45cb45
commit af9f45ff24
1 changed files with 26 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
cpp/simulated2D.h
View File

@ -52,12 +52,13 @@ namespace gtsam {
/**
* Unary factor encoding a soft prior on a vector
*/
struct Prior: public NonlinearFactor1<Config, PoseKey, Point2> {
template<class Cfg=Config>
struct GenericPrior: public NonlinearFactor1<Cfg, PoseKey, Point2> {
Point2 z_;
Prior(const Point2& z, const SharedGaussian& model, const PoseKey& key) :
NonlinearFactor1<Config, PoseKey, Point2> (model, key), z_(z) {
GenericPrior(const Point2& z, const SharedGaussian& model, const PoseKey& key) :
NonlinearFactor1<Cfg, PoseKey, Point2> (model, key), z_(z) {
}
Vector evaluateError(const Point2& x, boost::optional<Matrix&> H =
@ -70,13 +71,14 @@ namespace gtsam {
/**
* Binary factor simulating "odometry" between two Vectors
*/
struct Odometry: public NonlinearFactor2<Config, PoseKey, Point2, PoseKey,
template<class Cfg=Config>
struct GenericOdometry: public NonlinearFactor2<Cfg, PoseKey, Point2, PoseKey,
Point2> {
Point2 z_;
Odometry(const Point2& z, const SharedGaussian& model, const PoseKey& j1,
GenericOdometry(const Point2& z, const SharedGaussian& model, const PoseKey& j1,
const PoseKey& j2) :
z_(z), NonlinearFactor2<Config, PoseKey, Point2, PoseKey, Point2> (
z_(z), NonlinearFactor2<Cfg, PoseKey, Point2, PoseKey, Point2> (
model, j1, j2) {
}
@ -90,23 +92,30 @@ namespace gtsam {
/**
* Binary factor simulating "measurement" between two Vectors
*/
struct Measurement: public NonlinearFactor2<Config, PoseKey, Point2,
template<class Cfg=Config>
class GenericMeasurement: public NonlinearFactor2<Cfg, PoseKey, Point2,
PointKey, Point2> {
public:
Point2 z_;
Point2 z_;
Measurement(const Point2& z, const SharedGaussian& model,
const PoseKey& j1, const PointKey& j2) :
z_(z), NonlinearFactor2<Config, PoseKey, Point2, PointKey, Point2> (
model, j1, j2) {
}
GenericMeasurement(const Point2& z, const SharedGaussian& model,
const PoseKey& j1, const PointKey& j2) :
z_(z), NonlinearFactor2<Cfg, PoseKey, Point2, PointKey, Point2> (
model, j1, j2) {
}
Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional<
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
return (mea(x1, x2, H1, H2) - z_).vector();
}
Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional<
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
return (mea(x1, x2, H1, H2) - z_).vector();
}
};
/** Typedefs for regular use */
typedef GenericPrior<Config> Prior;
typedef GenericOdometry<Config> Odometry;
typedef GenericMeasurement<Config> Measurement;
} // namespace simulated2D
} // namespace gtsam