68 lines
1.9 KiB
C++
68 lines
1.9 KiB
C++
/**
|
|
* @file simulated2DConstraints.h
|
|
* @brief measurement functions and constraint definitions for simulated 2D robot
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
// \callgraph
|
|
|
|
#pragma once
|
|
|
|
#include <NonlinearConstraint.h>
|
|
#include <simulated2D.h>
|
|
|
|
// \namespace
|
|
|
|
namespace gtsam {
|
|
|
|
namespace simulated2D {
|
|
|
|
/**
|
|
* Unary factor encoding a hard equality on a Point
|
|
*/
|
|
template<class Cfg = Config, class Key = PoseKey>
|
|
struct GenericUnaryEqualityConstraint: public NonlinearConstraint1<Cfg, Key, Point2> {
|
|
typedef NonlinearConstraint1<Cfg, Key, Point2> Base;
|
|
typedef boost::shared_ptr<GenericUnaryEqualityConstraint<Cfg, Key> > shared_ptr;
|
|
|
|
Point2 z_;
|
|
|
|
GenericUnaryEqualityConstraint(const Point2& z, const Key& key, double mu = 1000.0) :
|
|
Base(key, 2, mu), z_(z) {
|
|
}
|
|
|
|
Vector evaluateError(const Point2& x, boost::optional<Matrix&> H =
|
|
boost::none) const {
|
|
return (prior(x, H) - z_).vector();
|
|
}
|
|
|
|
};
|
|
|
|
/**
|
|
* Binary factor simulating "odometry" between two Vectors
|
|
*/
|
|
template<class Cfg = Config, class Key = PoseKey>
|
|
struct GenericOdoHardEqualityConstraint: public NonlinearConstraint2<Cfg, Key, Point2, Key, Point2> {
|
|
typedef NonlinearConstraint2<Cfg, Key, Point2, Key, Point2> Base;
|
|
typedef boost::shared_ptr<GenericOdoHardEqualityConstraint<Cfg,Key> > shared_ptr;
|
|
Point2 z_;
|
|
|
|
GenericOdoHardEqualityConstraint(
|
|
const Point2& z, const Key& i1, const Key& i2, double mu = 1000.0) :
|
|
Base (i1, i2, 2, mu), z_(z) {
|
|
}
|
|
|
|
Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional<
|
|
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
|
|
return (odo(x1, x2, H1, H2) - z_).vector();
|
|
}
|
|
|
|
};
|
|
|
|
/** Typedefs for regular use */
|
|
typedef GenericUnaryEqualityConstraint<Config, PoseKey> UnaryEqualityConstraint;
|
|
typedef GenericOdoHardEqualityConstraint<Config, PoseKey> OdoEqualityConstraint;
|
|
|
|
} // namespace simulated2D
|
|
} // namespace gtsam
|