Moved unused simulated2DConstraints to experimental
Richard Roberts
parent
a735b7cdeb
commit
403dcd6a00
|
|
@ -1,134 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file simulated2DConstraints.h
|
||||
* @brief measurement functions and constraint definitions for simulated 2D robot
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearConstraint.h>
|
||||
#include <gtsam/slam/BetweenConstraint.h>
|
||||
#include <gtsam/slam/BoundingConstraint.h>
|
||||
#include <gtsam/slam/simulated2D.h>
|
||||
|
||||
// \namespace
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
namespace simulated2D {
|
||||
|
||||
namespace equality_constraints {
|
||||
|
||||
/** Typedefs for regular use */
|
||||
typedef NonlinearEquality1<Values, PoseKey> UnaryEqualityConstraint;
|
||||
typedef NonlinearEquality1<Values, PointKey> UnaryEqualityPointConstraint;
|
||||
typedef BetweenConstraint<Values, PoseKey> OdoEqualityConstraint;
|
||||
|
||||
/** Equality between variables */
|
||||
typedef NonlinearEquality2<Values, PoseKey> PoseEqualityConstraint;
|
||||
typedef NonlinearEquality2<Values, PointKey> PointEqualityConstraint;
|
||||
|
||||
} // \namespace equality_constraints
|
||||
|
||||
namespace inequality_constraints {
|
||||
|
||||
/**
|
||||
* Unary inequality constraint forcing a coordinate to be greater/less than a fixed value (c)
|
||||
* Demo implementation: should be made more general using BoundingConstraint
|
||||
*/
|
||||
template<class Cfg, class Key, unsigned int Idx>
|
||||
struct ScalarCoordConstraint1: public BoundingConstraint1<Cfg, Key> {
|
||||
typedef BoundingConstraint1<Cfg, Key> Base;
|
||||
typedef boost::shared_ptr<ScalarCoordConstraint1<Cfg, Key, Idx> > shared_ptr;
|
||||
|
||||
ScalarCoordConstraint1(const Key& key, double c,
|
||||
bool isGreaterThan, double mu = 1000.0) :
|
||||
Base(key, c, isGreaterThan, mu) {
|
||||
}
|
||||
|
||||
inline unsigned int index() const { return Idx; }
|
||||
|
||||
/** extracts a single value from the point */
|
||||
virtual double value(const Point2& x, boost::optional<Matrix&> H =
|
||||
boost::none) const {
|
||||
if (H) {
|
||||
Matrix D = zeros(1, 2);
|
||||
D(0, Idx) = 1.0;
|
||||
*H = D;
|
||||
}
|
||||
return x.vector()(Idx);
|
||||
}
|
||||
};
|
||||
|
||||
/** typedefs for use with simulated2D systems */
|
||||
typedef ScalarCoordConstraint1<Values, PoseKey, 0> PoseXInequality;
|
||||
typedef ScalarCoordConstraint1<Values, PoseKey, 1> PoseYInequality;
|
||||
|
||||
double range(const Point2& a, const Point2& b) { return a.dist(b); }
|
||||
|
||||
/**
|
||||
* Binary inequality constraint forcing the range between points
|
||||
* to be less than or equal to a bound
|
||||
*/
|
||||
template<class Cfg, class Key>
|
||||
struct MaxDistanceConstraint : public BoundingConstraint2<Cfg, Key, Key> {
|
||||
typedef BoundingConstraint2<Cfg, Key, Key> Base;
|
||||
|
||||
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,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> 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);
|
||||
}
|
||||
};
|
||||
|
||||
typedef MaxDistanceConstraint<Values, PoseKey> PoseMaxDistConstraint;
|
||||
|
||||
/**
|
||||
* Binary inequality constraint forcing a minimum range
|
||||
* NOTE: this is not a convex function! Be careful with initialization.
|
||||
*/
|
||||
template<class Cfg, class XKey, class PKey>
|
||||
struct MinDistanceConstraint : public BoundingConstraint2<Cfg, XKey, PKey> {
|
||||
typedef BoundingConstraint2<Cfg, XKey, PKey> Base;
|
||||
|
||||
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,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> 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);
|
||||
}
|
||||
};
|
||||
|
||||
typedef MinDistanceConstraint<Values, PoseKey, PointKey> LandmarkAvoid;
|
||||
|
||||
|
||||
} // \namespace inequality_constraints
|
||||
|
||||
} // \namespace simulated2D
|
||||
} // \namespace gtsam
|
||||
Loading…
Reference in New Issue