diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h index 1d147fec3..a7ad007d7 100644 --- a/gtsam/slam/simulated2DConstraints.h +++ b/gtsam/slam/simulated2DConstraints.h @@ -49,83 +49,147 @@ namespace gtsam { /** * 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 - struct ScalarCoordConstraint1: public BoundingConstraint1 { - typedef BoundingConstraint1 Base; - typedef boost::shared_ptr > shared_ptr; + template + struct ScalarCoordConstraint1: public BoundingConstraint1 { + typedef BoundingConstraint1 Base; ///< Base class convenience typedef + typedef boost::shared_ptr > 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) : 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 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 H = boost::none) const { if (H) { - Matrix D = zeros(1, 2); - D(0, Idx) = 1.0; + Matrix D = zeros(1, x.dim()); + D(0, IDX) = 1.0; *H = D; } - return x.vector()(Idx); + return Point::Logmap(x)(IDX); } }; /** typedefs for use with simulated2D systems */ - typedef ScalarCoordConstraint1 PoseXInequality; - typedef ScalarCoordConstraint1 PoseYInequality; + typedef ScalarCoordConstraint1 PoseXInequality; ///< Simulated2D domain example factor constraining X + typedef ScalarCoordConstraint1 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 + double range_trait(const T1& a, const T2& b) { return a.dist(b); } /** * Binary inequality constraint forcing the range between points * 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 - struct MaxDistanceConstraint : public BoundingConstraint2 { - typedef BoundingConstraint2 Base; + template + struct MaxDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 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) {} - /** 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 H1 = boost::none, boost::optional H2 = boost::none) const { - if (H1) *H1 = numericalDerivative21(range, x1, x2, 1e-5); - if (H1) *H2 = numericalDerivative22(range, x1, x2, 1e-5); - return x1.dist(x2); + if (H1) *H1 = numericalDerivative21(range_trait, x1, x2, 1e-5); + if (H1) *H2 = numericalDerivative22(range_trait, x1, x2, 1e-5); + return range_trait(x1, x2); } }; - typedef MaxDistanceConstraint PoseMaxDistConstraint; + typedef MaxDistanceConstraint PoseMaxDistConstraint; ///< Simulated2D domain example factor /** * Binary inequality constraint forcing a minimum range * 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 - struct MinDistanceConstraint : public BoundingConstraint2 { - typedef BoundingConstraint2 Base; + template + struct MinDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 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) {} - /** 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 H1 = boost::none, boost::optional H2 = boost::none) const { - if (H1) *H1 = numericalDerivative21(range, x1, x2, 1e-5); - if (H1) *H2 = numericalDerivative22(range, x1, x2, 1e-5); - return x1.dist(x2); + if (H1) *H1 = numericalDerivative21(range_trait, x1, x2, 1e-5); + if (H1) *H2 = numericalDerivative22(range_trait, x1, x2, 1e-5); + return range_trait(x1, x2); } }; - typedef MinDistanceConstraint LandmarkAvoid; + typedef MinDistanceConstraint LandmarkAvoid; ///< Simulated2D domain example factor } // \namespace inequality_constraints diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index 9e19a32bc..0934ed6b0 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -79,9 +79,11 @@ namespace gtsam { */ template struct GenericOdometry: public NonlinearFactor2 { - 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, const KEY& i1, const KEY& i2) : NonlinearFactor2(model, i1, i2), z_(z) {