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

Added doxygen documentation

release/4.3a0
Alex Cunningham 2011-09-07 13:31:25 +00:00
parent 44e43f2393
commit b0c2295fdd
2 changed files with 103 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

134
gtsam/slam/simulated2DConstraints.h
View File

@ -49,83 +49,147 @@ namespace gtsam {
/** /**
* Unary inequality constraint forcing a coordinate to be greater/less than a fixed value (c) * Unary inequality constraint forcing a coordinate to be greater/less than a fixed value (c)
* Demo implementation: should be made more general using BoundingConstraint * @tparam VALUES is the values structure for the graph
* @tparam KEY is the key type for the variable constrained
* @tparam IDX is an index in tangent space to constrain, must be less than KEY::VALUE::Dim()
*/ */
template<class VALUES, class Key, unsigned int Idx> template<class VALUES, class KEY, unsigned int IDX>
struct ScalarCoordConstraint1: public BoundingConstraint1<VALUES, Key> { struct ScalarCoordConstraint1: public BoundingConstraint1<VALUES, KEY> {
typedef BoundingConstraint1<VALUES, Key> Base; typedef BoundingConstraint1<VALUES, KEY> Base; ///< Base class convenience typedef
typedef boost::shared_ptr<ScalarCoordConstraint1<VALUES, Key, Idx> > shared_ptr; typedef boost::shared_ptr<ScalarCoordConstraint1<VALUES, KEY, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef
typedef typename::KEY::Value Point; ///< Constrained variable type
ScalarCoordConstraint1(const Key& key, double c, /**
* Constructor for constraint
* @param key is the label for the constrained variable
* @param c is the measured value for the fixed coordinate
* @param isGreaterThan is a flag to set inequality as greater than or less than
* @param mu is the penalty function gain
*/
ScalarCoordConstraint1(const KEY& key, double c,
bool isGreaterThan, double mu = 1000.0) : bool isGreaterThan, double mu = 1000.0) :
Base(key, c, isGreaterThan, mu) { Base(key, c, isGreaterThan, mu) {
} }
inline unsigned int index() const { return Idx; } /**
* Access function for the constrained index
* @return the index for the constrained coordinate
*/
inline unsigned int index() const { return IDX; }
/** extracts a single value from the point */ /**
virtual double value(const Point2& x, boost::optional<Matrix&> H = * extracts a single value from the point to compute error
* @param x is the estimate of the constrained variable being evaluated
* @param H is an optional Jacobian, linearized at x
*/
virtual double value(const Point& x, boost::optional<Matrix&> H =
boost::none) const { boost::none) const {
if (H) { if (H) {
Matrix D = zeros(1, 2); Matrix D = zeros(1, x.dim());
D(0, Idx) = 1.0; D(0, IDX) = 1.0;
*H = D; *H = D;
} }
return x.vector()(Idx); return Point::Logmap(x)(IDX);
} }
}; };
/** typedefs for use with simulated2D systems */ /** typedefs for use with simulated2D systems */
typedef ScalarCoordConstraint1<Values, PoseKey, 0> PoseXInequality; typedef ScalarCoordConstraint1<Values, PoseKey, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X
typedef ScalarCoordConstraint1<Values, PoseKey, 1> PoseYInequality; typedef ScalarCoordConstraint1<Values, PoseKey, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
double range(const Point2& a, const Point2& b) { return a.dist(b); } /**
* Trait for distance constraints to provide distance
* @tparam T1 is a Lie value for which distance functions exist
* @tparam T2 is a Lie value for which distance functions exist
* @param a is the first Lie element
* @param b is the second Lie element
* @return a scalar distance between values
*/
template<class T1, class T2 = T1>
double range_trait(const T1& a, const T2& b) { return a.dist(b); }
/** /**
* Binary inequality constraint forcing the range between points * Binary inequality constraint forcing the range between points
* to be less than or equal to a bound * to be less than or equal to a bound
* @tparam VALUES is the variable set for the graph
* @tparam KEY is the type of the keys for the variables constrained
*/ */
template<class VALUES, class Key> template<class VALUES, class KEY>
struct MaxDistanceConstraint : public BoundingConstraint2<VALUES, Key, Key> { struct MaxDistanceConstraint : public BoundingConstraint2<VALUES, KEY, KEY> {
typedef BoundingConstraint2<VALUES, Key, Key> Base; typedef BoundingConstraint2<VALUES, KEY, KEY> Base; ///< Base class for factor
typedef typename KEY::Value Point; ///< Type of variable constrained
MaxDistanceConstraint(const Key& key1, const Key& key2, double range_bound, double mu = 1000.0) /**
* Primary constructor for factor
* @param key1 is the first variable key
* @param key2 is the second variable key
* @param range_bound is the maximum range allowed between the variables
* @param mu is the gain for the penalty function
*/
MaxDistanceConstraint(const KEY& key1, const KEY& key2, double range_bound, double mu = 1000.0)
: Base(key1, key2, range_bound, false, mu) {} : Base(key1, key2, range_bound, false, mu) {}
/** extracts a single scalar value with derivatives */ /**
virtual double value(const Point2& x1, const Point2& x2, * computes the range with derivatives
* @param x1 is the first variable value
* @param x2 is the second variable value
* @param H1 is an optional Jacobian in x1
* @param H2 is an optional Jacobian in x2
* @return the distance between the variables
*/
virtual double value(const Point& x1, const Point& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = numericalDerivative21(range, x1, x2, 1e-5); if (H1) *H1 = numericalDerivative21(range_trait<Point,Point>, x1, x2, 1e-5);
if (H1) *H2 = numericalDerivative22(range, x1, x2, 1e-5); if (H1) *H2 = numericalDerivative22(range_trait<Point,Point>, x1, x2, 1e-5);
return x1.dist(x2); return range_trait(x1, x2);
} }
}; };
typedef MaxDistanceConstraint<Values, PoseKey> PoseMaxDistConstraint; typedef MaxDistanceConstraint<Values, PoseKey> PoseMaxDistConstraint; ///< Simulated2D domain example factor
/** /**
* Binary inequality constraint forcing a minimum range * Binary inequality constraint forcing a minimum range
* NOTE: this is not a convex function! Be careful with initialization. * NOTE: this is not a convex function! Be careful with initialization.
* @tparam VALUES is the variable set for the graph
* @tparam XKEY is the type of the pose key constrained
* @tparam PKEY is the type of the point key constrained
*/ */
template<class VALUES, class XKey, class PKey> template<class VALUES, class XKEY, class PKEY>
struct MinDistanceConstraint : public BoundingConstraint2<VALUES, XKey, PKey> { struct MinDistanceConstraint : public BoundingConstraint2<VALUES, XKEY, PKEY> {
typedef BoundingConstraint2<VALUES, XKey, PKey> Base; typedef BoundingConstraint2<VALUES, XKEY, PKEY> Base; ///< Base class for factor
typedef typename XKEY::Value Pose; ///< Type of pose variable constrained
typedef typename PKEY::Value Point; ///< Type of point variable constrained
MinDistanceConstraint(const XKey& key1, const PKey& key2, double range_bound, double mu = 1000.0) /**
* Primary constructor for factor
* @param key1 is the first variable key
* @param key2 is the second variable key
* @param range_bound is the minimum range allowed between the variables
* @param mu is the gain for the penalty function
*/
MinDistanceConstraint(const XKEY& key1, const PKEY& key2,
double range_bound, double mu = 1000.0)
: Base(key1, key2, range_bound, true, mu) {} : Base(key1, key2, range_bound, true, mu) {}
/** extracts a single scalar value with derivatives */ /**
virtual double value(const Point2& x1, const Point2& x2, * computes the range with derivatives
* @param x1 is the first variable value
* @param x2 is the second variable value
* @param H1 is an optional Jacobian in x1
* @param H2 is an optional Jacobian in x2
* @return the distance between the variables
*/
virtual double value(const Pose& x1, const Point& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = numericalDerivative21(range, x1, x2, 1e-5); if (H1) *H1 = numericalDerivative21(range_trait<Pose,Point>, x1, x2, 1e-5);
if (H1) *H2 = numericalDerivative22(range, x1, x2, 1e-5); if (H1) *H2 = numericalDerivative22(range_trait<Pose,Point>, x1, x2, 1e-5);
return x1.dist(x2); return range_trait(x1, x2);
} }
}; };
typedef MinDistanceConstraint<Values, PoseKey, PointKey> LandmarkAvoid; typedef MinDistanceConstraint<Values, PoseKey, PointKey> LandmarkAvoid; ///< Simulated2D domain example factor
} // \namespace inequality_constraints } // \namespace inequality_constraints

6
gtsam/slam/simulated2DOriented.h
View File

@ -79,9 +79,11 @@ namespace gtsam {
*/ */
template<class VALUES = Values, class KEY = PoseKey> template<class VALUES = Values, class KEY = PoseKey>
struct GenericOdometry: public NonlinearFactor2<VALUES, KEY, KEY> { struct GenericOdometry: public NonlinearFactor2<VALUES, KEY, KEY> {
Pose2 z_; Pose2 z_; ///< Between measurement for odometry factor
/// Create generic odometry factor /**
* Creates an odometry factor between two poses
*/
GenericOdometry(const Pose2& z, const SharedNoiseModel& model, GenericOdometry(const Pose2& z, const SharedNoiseModel& model,
const KEY& i1, const KEY& i2) : const KEY& i1, const KEY& i2) :
NonlinearFactor2<VALUES, KEY, KEY>(model, i1, i2), z_(z) { NonlinearFactor2<VALUES, KEY, KEY>(model, i1, i2), z_(z) {