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

Added tests for nonlinear equality constraints, generalized equality constraints

release/4.3a0
Alex Cunningham 2010-08-09 17:21:11 +00:00
parent 38ea7d1ea5
commit efaca162cf
7 changed files with 439 additions and 218 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

126
nonlinear/NonlinearConstraint.h
View File

@ -33,7 +33,9 @@ class NonlinearConstraint : public NonlinearFactor<Config> {
protected:
typedef NonlinearConstraint<Config> This;
typedef NonlinearFactor<Config> Base;
double mu_; // gain for quadratic merit function
double mu_; /// gain for quadratic merit function
size_t dim_; /// dimension of the constraint
public:
@ -42,7 +44,7 @@ public:
* @param mu is the gain used at error evaluation (forced to be positive)
*/
NonlinearConstraint(size_t dim, double mu = 1000.0):
Base(noiseModel::Constrained::All(dim)), mu_(fabs(mu)) {}
Base(noiseModel::Constrained::All(dim)), mu_(fabs(mu)), dim_(dim) {}
/** returns the gain mu */
double mu() const { return mu_; }
@ -50,6 +52,9 @@ public:
/** Print */
virtual void print(const std::string& s = "") const=0;
/** dimension of the constraint (number of rows) */
size_t dim() const { return dim_; }
/** Check if two factors are equal */
virtual bool equals(const Factor<Config>& f, double tol=1e-9) const {
const This* p = dynamic_cast<const This*> (&f);
@ -70,10 +75,12 @@ public:
/**
* active set check, defines what type of constraint this is
* By default, the constraint is always active, so it is an equality constraint
*
* In an inequality/bounding constraint, this active() returns true
* when the constraint is *NOT* fulfilled.
* @return true if the constraint is active
*/
virtual bool active(const Config& c) const { return true; }
virtual bool active(const Config& c) const=0;
/**
* Linearizes around a given config
@ -124,8 +131,11 @@ public:
return Base::equals(*p, tol) && (key_ == p->key_);
}
/** error function g(x) */
/** error function g(x), switched depending on whether the constraint is active */
inline Vector unwhitenedError(const Config& x) const {
if (!active(x)) {
return zero(this->dim());
}
const Key& j = key_;
const X& xj = x[j];
return evaluateError(xj);
@ -145,11 +155,29 @@ public:
return GaussianFactor::shared_ptr(new GaussianFactor(this->key_, grad, g, model));
}
/** g(x) with optional derivative */
/** g(x) with optional derivative - does not depend on active */
virtual Vector evaluateError(const X& x, boost::optional<Matrix&> H =
boost::none) const = 0;
};
/**
* Unary Equality constraint - simply forces the value of active() to true
*/
template <class Config, class Key, class X>
class NonlinearEqualityConstraint1 : public NonlinearConstraint1<Config, Key, X> {
protected:
typedef NonlinearEqualityConstraint1<Config,Key,X> This;
typedef NonlinearConstraint1<Config,Key,X> Base;
public:
NonlinearEqualityConstraint1(const Key& key, size_t dim, double mu = 1000.0)
: Base(key, dim, mu) {}
/** Always active, so fixed value for active() */
virtual bool active(const Config& c) const { return true; }
};
/**
* A binary constraint with arbitrary cost and jacobian functions
*/
@ -194,8 +222,11 @@ public:
return Base::equals(*p, tol) && (key1_ == p->key1_) && (key2_ == p->key2_);
}
/** error function g(x) */
/** error function g(x), switched depending on whether the constraint is active */
inline Vector unwhitenedError(const Config& x) const {
if (!active(x)) {
return zero(this->dim());
}
const Key1& j1 = key1_;
const Key2& j2 = key2_;
const X1& xj1 = x[j1];
@ -217,20 +248,64 @@ public:
return GaussianFactor::shared_ptr(new GaussianFactor(j1, grad1, j2, grad2, g, model));
}
/** g(x) with optional derivative2 */
/** g(x) with optional derivative2 - does not depend on active */
virtual Vector evaluateError(const X1& x1, const X2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const = 0;
};
/**
* Binary Equality constraint - simply forces the value of active() to true
*/
template <class Config, class Key1, class X1, class Key2, class X2>
class NonlinearEqualityConstraint2 : public NonlinearConstraint2<Config, Key1, X1, Key2, X2> {
protected:
typedef NonlinearEqualityConstraint2<Config,Key1,X1,Key2,X2> This;
typedef NonlinearConstraint2<Config,Key1,X1,Key2,X2> Base;
public:
NonlinearEqualityConstraint2(const Key1& key1, const Key2& key2, size_t dim, double mu = 1000.0)
: Base(key1, key2, dim, mu) {}
/** Always active, so fixed value for active() */
virtual bool active(const Config& c) const { return true; }
};
/**
* Simple unary equality constraint - fixes a value for a variable
*/
template<class Config, class Key, class X>
class NonlinearEquality1 : public NonlinearEqualityConstraint1<Config, Key, X> {
protected:
typedef NonlinearEqualityConstraint1<Config, Key, X> Base;
X value_; /// fixed value for variable
public:
typedef boost::shared_ptr<NonlinearEquality1<Config, Key, X> > shared_ptr;
NonlinearEquality1(const X& value, const Key& key1, double mu = 1000.0)
: Base(key1, X::dim(), mu), value_(value) {}
/** g(x) with optional derivative */
Vector evaluateError(const X& x1, boost::optional<Matrix&> H1 = boost::none) const {
const size_t p = X::dim();
if (H1) *H1 = eye(p);
return logmap(value_, x1);
}
};
/**
* Simple binary equality constraint - this constraint forces two factors to
* be the same. This constraint requires the underlying type to a Lie type
*/
template<class Config, class Key, class X>
class NonlinearEquality2 : public NonlinearConstraint2<Config, Key, X, Key, X> {
class NonlinearEquality2 : public NonlinearEqualityConstraint2<Config, Key, X, Key, X> {
protected:
typedef NonlinearConstraint2<Config, Key, X, Key, X> Base;
typedef NonlinearEqualityConstraint2<Config, Key, X, Key, X> Base;
public:
@ -250,4 +325,35 @@ public:
}
};
/**
* Binary between constraint - forces between to a given value
* This constraint requires the underlying type to a Lie type
*/
template<class Config, class Key, class X>
class BetweenConstraint : public NonlinearEqualityConstraint2<Config, Key, X, Key, X> {
protected:
typedef NonlinearEqualityConstraint2<Config, Key, X, Key, X> Base;
X measured_; /// fixed between value
public:
typedef boost::shared_ptr<BetweenConstraint<Config, Key, X> > shared_ptr;
BetweenConstraint(const X& measured, const Key& key1, const Key& key2, double mu = 1000.0)
: Base(key1, key2, X::dim(), mu), measured_(measured) {}
/** g(x) with optional derivative2 */
Vector evaluateError(const X& x1, const X& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
X hx = between(x1, x2, H1, H2);
return logmap(measured_, hx);
}
inline const X measured() const {
return measured_;
}
};
}

2
nonlinear/NonlinearFactorGraph-inl.h
View File

@ -51,7 +51,7 @@ namespace gtsam {
// linearize all factors
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
boost::shared_ptr<GaussianFactor> lf = factor->linearize(config);
linearFG->push_back(lf);
if (lf) linearFG->push_back(lf);
}
return linearFG;

91
slam/simulated2DConstraints.h
View File

@ -17,55 +17,56 @@ namespace gtsam {
namespace simulated2D {
/**
* Unary constraint 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 constraint simulating "odometry" between two Poses
*/
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();
}
};
namespace equality_constraints {
/** Typedefs for regular use */
typedef GenericUnaryEqualityConstraint<Config, PoseKey> UnaryEqualityConstraint;
typedef GenericOdoHardEqualityConstraint<Config, PoseKey> OdoEqualityConstraint;
typedef NonlinearEquality1<Config, PoseKey, Point2> UnaryEqualityConstraint;
typedef BetweenConstraint<Config, PoseKey, Point2> OdoEqualityConstraint;
/** Equality between variables */
typedef NonlinearEquality2<Config, PoseKey, Point2> PoseEqualityConstraint;
typedef NonlinearEquality2<Config, PointKey, Point2> PointEqualityConstraint;
} // namespace simulated2D
} // namespace gtsam
} // \namespace equality_constraints
namespace inequality_constraints {
/**
* Unary inequality constraint forcing a coordinate to be greater than a fixed value (c)
*/
template<class Cfg, class Key, unsigned int Idx>
struct ScalarInequalityConstraint1: public NonlinearConstraint1<Cfg, Key, Point2> {
typedef NonlinearConstraint1<Cfg, Key, Point2> Base;
typedef boost::shared_ptr<ScalarInequalityConstraint1<Cfg, Key, Idx> > shared_ptr;
double c_; /// min value of the selected coordinate
ScalarInequalityConstraint1(const Key& key, double c, double mu = 1000.0) :
Base(key, 1, mu), c_(c) {
}
/** active when constraint not met */
virtual bool active(const Cfg& c) const {
return c[this->key_].vector()(Idx) <= c_; // greater than or equals to avoid zigzagging
}
Vector evaluateError(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 Vector_(1, x.vector()(Idx) - c_);
}
};
/** typedefs for use with simulated2D systems */
typedef ScalarInequalityConstraint1<Config, PoseKey, 0> PoseXInequality;
typedef ScalarInequalityConstraint1<Config, PoseKey, 1> PoseYInequality;
} // \namespace inequality_constraints
} // \namespace simulated2D
} // \namespace gtsam

2
tests/Makefile.am
View File

@ -11,7 +11,7 @@ check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorG
check_PROGRAMS += testNonlinearOptimizer testSubgraphPreconditioner
check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph testTupleConfig
# check_PROGRAMS += testNonlinearConstraint # disabled until examples rearranged
check_PROGRAMS += testNonlinearEqualityConstraint
check_PROGRAMS += testNonlinearEqualityConstraint testNonlinearInequalityConstraint
if USE_LDL
check_PROGRAMS += testConstraintOptimizer

300
tests/testNonlinearConstraint.cpp
View File

@ -49,158 +49,158 @@ using namespace boost::assign;
//typedef boost::shared_ptr<simulated2D::GenericMeasurement<Config2D> > shared;
//typedef NonlinearOptimizer<Graph2D, Config2D> Optimizer;
///* ********************************************************************* */
//// This is an obstacle avoidance demo, where there is a trajectory of
//// three points, where there is a circular obstacle in the middle. There
//// is a binary inequality constraint connecting the obstacle to the
//// states, which enforces a minimum distance.
///* ********************************************************************* */
/* ********************************************************************* */
// This is an obstacle avoidance demo, where there is a trajectory of
// three points, where there is a circular obstacle in the middle. There
// is a binary inequality constraint connecting the obstacle to the
// states, which enforces a minimum distance.
/* ********************************************************************* */
//typedef NonlinearConstraint2<Config2D, PoseKey, Point2, PointKey, Point2> AvoidConstraint;
//typedef shared_ptr<AvoidConstraint> shared_a;
//typedef NonlinearEquality<Config2D, simulated2D::PoseKey, Point2> PoseNLConstraint;
//typedef shared_ptr<PoseNLConstraint> shared_pc;
//typedef NonlinearEquality<Config2D, simulated2D::PointKey, Point2> ObstacleConstraint;
//typedef shared_ptr<ObstacleConstraint> shared_oc;
//
////typedef NonlinearConstraint2<Config2D, PoseKey, Point2, PointKey, Point2> AvoidConstraint;
////typedef shared_ptr<AvoidConstraint> shared_a;
////typedef NonlinearEquality<Config2D, simulated2D::PoseKey, Point2> PoseNLConstraint;
////typedef shared_ptr<PoseNLConstraint> shared_pc;
////typedef NonlinearEquality<Config2D, simulated2D::PointKey, Point2> ObstacleConstraint;
////typedef shared_ptr<ObstacleConstraint> shared_oc;
////
////
////namespace constrained_avoid1 {
////// avoidance radius
////double radius = 1.0;
////
////// binary avoidance constraint
/////** g(x) = ||x2-obs||^2 - radius^2 > 0 */
////Vector g_func(const Config2D& config, const PoseKey& x, const PointKey& obs) {
//// double dist2 = config[x].dist(config[obs]);
//// double thresh = radius*radius;
//// return Vector_(1, dist2-thresh);
////}
////
/////** jacobian at pose */
////Matrix jac_g1(const Config2D& config, const PoseKey& x, const PointKey& obs) {
//// Point2 p = config[x]-config[obs];
//// return Matrix_(1,2, 2.0*p.x(), 2.0*p.y());
////}
////
/////** jacobian at obstacle */
////Matrix jac_g2(const Config2D& config, const PoseKey& x, const PointKey& obs) {
//// Point2 p = config[x]-config[obs];
//// return Matrix_(1,2, -2.0*p.x(), -2.0*p.y());
////}
////}
////
////pair<NLGraph, Config2D> obstacleAvoidGraph() {
//// // Keys
//// PoseKey x1(1), x2(2), x3(3);
//// PointKey l1(1);
//// LagrangeKey L20(20);
////
//// // Constrained Points
//// Point2 pt_x1,
//// pt_x3(10.0, 0.0),
//// pt_l1(5.0, -0.5);
////
//// shared_pc e1(new PoseNLConstraint(x1, pt_x1));
//// shared_pc e2(new PoseNLConstraint(x3, pt_x3));
//// shared_oc e3(new ObstacleConstraint(l1, pt_l1));
////
//// // measurement from x1 to x2
//// Point2 x1x2(5.0, 0.0);
//// shared f1(new simulated2D::Odometry(x1x2, sigma, 1,2));
////
//// // measurement from x2 to x3
//// Point2 x2x3(5.0, 0.0);
//// shared f2(new simulated2D::Odometry(x2x3, sigma, 2,3));
////
//// // create a binary inequality constraint that forces the middle point away from
//// // the obstacle
//// shared_a c1(new AvoidConstraint(boost::bind(constrained_avoid1::g_func, _1, x2, l1),
//// x2, boost::bind(constrained_avoid1::jac_g1, _1, x2, l1),
//// l1,boost::bind(constrained_avoid1::jac_g2, _1, x2, l1),
//// 1, L20, false));
////
//// // construct the graph
//// NLGraph graph;
//// graph.push_back(e1);
//// graph.push_back(e2);
//// graph.push_back(e3);
//// graph.push_back(c1);
//// graph.push_back(f1);
//// graph.push_back(f2);
////
//// // make a config of the fixed values, for convenience
//// Config2D config;
//// config.insert(x1, pt_x1);
//// config.insert(x3, pt_x3);
//// config.insert(l1, pt_l1);
////
//// return make_pair(graph, config);
////}
////
/////* ********************************************************************* */
////TEST ( SQPOptimizer, inequality_avoid ) {
//// // create the graph
//// NLGraph graph; Config2D feasible;
//// boost::tie(graph, feasible) = obstacleAvoidGraph();
////
//// // create the rest of the config
//// shared_ptr<Config2D> init(new Config2D(feasible));
//// PoseKey x2(2);
//// init->insert(x2, Point2(5.0, 100.0));
////
//// // create an ordering
//// Ordering ord;
//// ord += "x1", "x2", "x3", "l1";
////
//// // create an optimizer
//// Optimizer optimizer(graph, ord, init);
////
//// // perform an iteration of optimization
//// // NOTE: the constraint will be inactive in the first iteration,
//// // so it will violate the constraint after one iteration
//// Optimizer afterOneIteration = optimizer.iterate(Optimizer::SILENT);
////
//// Config2D exp1(feasible);
//// exp1.insert(x2, Point2(5.0, 0.0));
//// CHECK(assert_equal(exp1, *(afterOneIteration.config())));
////
//// // the second iteration will activate the constraint and force the
//// // config to a viable configuration.
//// Optimizer after2ndIteration = afterOneIteration.iterate(Optimizer::SILENT);
////
//// Config2D exp2(feasible);
//// exp2.insert(x2, Point2(5.0, 0.5));
//// CHECK(assert_equal(exp2, *(after2ndIteration.config())));
////}
//
/////* ********************************************************************* */
////TEST ( SQPOptimizer, inequality_avoid_iterative ) {
//// // create the graph
//// NLGraph graph; Config2D feasible;
//// boost::tie(graph, feasible) = obstacleAvoidGraph();
////
//// // create the rest of the config
//// shared_ptr<Config2D> init(new Config2D(feasible));
//// PoseKey x2(2);
//// init->insert(x2, Point2(5.0, 100.0));
////
//// // create an ordering
//// Ordering ord;
//// ord += "x1", "x2", "x3", "l1";
////
//// // create an optimizer
//// Optimizer optimizer(graph, ord, init);
////
//// double relThresh = 1e-5; // minimum change in error between iterations
//// double absThresh = 1e-5; // minimum error necessary to converge
//// double constraintThresh = 1e-9; // minimum constraint error to be feasible
//// Optimizer final = optimizer.iterateSolve(relThresh, absThresh, constraintThresh);
////
//// // verify
//// Config2D exp2(feasible);
//// exp2.insert(x2, Point2(5.0, 0.5));
//// CHECK(assert_equal(exp2, *(final.config())));
////}
//namespace constrained_avoid1 {
//// avoidance radius
//double radius = 1.0;
//
//// binary avoidance constraint
///** g(x) = ||x2-obs||^2 - radius^2 > 0 */
//Vector g_func(const Config2D& config, const PoseKey& x, const PointKey& obs) {
// double dist2 = config[x].dist(config[obs]);
// double thresh = radius*radius;
// return Vector_(1, dist2-thresh);
//}
//
///** jacobian at pose */
//Matrix jac_g1(const Config2D& config, const PoseKey& x, const PointKey& obs) {
// Point2 p = config[x]-config[obs];
// return Matrix_(1,2, 2.0*p.x(), 2.0*p.y());
//}
//
///** jacobian at obstacle */
//Matrix jac_g2(const Config2D& config, const PoseKey& x, const PointKey& obs) {
// Point2 p = config[x]-config[obs];
// return Matrix_(1,2, -2.0*p.x(), -2.0*p.y());
//}
//}
//
//pair<NLGraph, Config2D> obstacleAvoidGraph() {
// // Keys
// PoseKey x1(1), x2(2), x3(3);
// PointKey l1(1);
// LagrangeKey L20(20);
//
// // Constrained Points
// Point2 pt_x1,
// pt_x3(10.0, 0.0),
// pt_l1(5.0, -0.5);
//
// shared_pc e1(new PoseNLConstraint(x1, pt_x1));
// shared_pc e2(new PoseNLConstraint(x3, pt_x3));
// shared_oc e3(new ObstacleConstraint(l1, pt_l1));
//
// // measurement from x1 to x2
// Point2 x1x2(5.0, 0.0);
// shared f1(new simulated2D::Odometry(x1x2, sigma, 1,2));
//
// // measurement from x2 to x3
// Point2 x2x3(5.0, 0.0);
// shared f2(new simulated2D::Odometry(x2x3, sigma, 2,3));
//
// // create a binary inequality constraint that forces the middle point away from
// // the obstacle
// shared_a c1(new AvoidConstraint(boost::bind(constrained_avoid1::g_func, _1, x2, l1),
// x2, boost::bind(constrained_avoid1::jac_g1, _1, x2, l1),
// l1,boost::bind(constrained_avoid1::jac_g2, _1, x2, l1),
// 1, L20, false));
//
// // construct the graph
// NLGraph graph;
// graph.push_back(e1);
// graph.push_back(e2);
// graph.push_back(e3);
// graph.push_back(c1);
// graph.push_back(f1);
// graph.push_back(f2);
//
// // make a config of the fixed values, for convenience
// Config2D config;
// config.insert(x1, pt_x1);
// config.insert(x3, pt_x3);
// config.insert(l1, pt_l1);
//
// return make_pair(graph, config);
//}
//
///* ********************************************************************* */
//TEST ( SQPOptimizer, inequality_avoid ) {
// // create the graph
// NLGraph graph; Config2D feasible;
// boost::tie(graph, feasible) = obstacleAvoidGraph();
//
// // create the rest of the config
// shared_ptr<Config2D> init(new Config2D(feasible));
// PoseKey x2(2);
// init->insert(x2, Point2(5.0, 100.0));
//
// // create an ordering
// Ordering ord;
// ord += "x1", "x2", "x3", "l1";
//
// // create an optimizer
// Optimizer optimizer(graph, ord, init);
//
// // perform an iteration of optimization
// // NOTE: the constraint will be inactive in the first iteration,
// // so it will violate the constraint after one iteration
// Optimizer afterOneIteration = optimizer.iterate(Optimizer::SILENT);
//
// Config2D exp1(feasible);
// exp1.insert(x2, Point2(5.0, 0.0));
// CHECK(assert_equal(exp1, *(afterOneIteration.config())));
//
// // the second iteration will activate the constraint and force the
// // config to a viable configuration.
// Optimizer after2ndIteration = afterOneIteration.iterate(Optimizer::SILENT);
//
// Config2D exp2(feasible);
// exp2.insert(x2, Point2(5.0, 0.5));
// CHECK(assert_equal(exp2, *(after2ndIteration.config())));
//}
//
///* ********************************************************************* */
//TEST ( SQPOptimizer, inequality_avoid_iterative ) {
// // create the graph
// NLGraph graph; Config2D feasible;
// boost::tie(graph, feasible) = obstacleAvoidGraph();
//
// // create the rest of the config
// shared_ptr<Config2D> init(new Config2D(feasible));
// PoseKey x2(2);
// init->insert(x2, Point2(5.0, 100.0));
//
// // create an ordering
// Ordering ord;
// ord += "x1", "x2", "x3", "l1";
//
// // create an optimizer
// Optimizer optimizer(graph, ord, init);
//
// double relThresh = 1e-5; // minimum change in error between iterations
// double absThresh = 1e-5; // minimum error necessary to converge
// double constraintThresh = 1e-9; // minimum constraint error to be feasible
// Optimizer final = optimizer.iterateSolve(relThresh, absThresh, constraintThresh);
//
// // verify
// Config2D exp2(feasible);
// exp2.insert(x2, Point2(5.0, 0.5));
// CHECK(assert_equal(exp2, *(final.config())));
//}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

32
tests/testNonlinearEqualityConstraint.cpp
View File

@ -10,6 +10,8 @@
#include <NonlinearFactorGraph-inl.h>
#include <NonlinearOptimizer-inl.h>
namespace eq2D = gtsam::simulated2D::equality_constraints;
using namespace std;
using namespace gtsam;
@ -28,7 +30,7 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) {
Point2 pt(1.0, 2.0);
simulated2D::PoseKey key(1);
double mu = 1000.0;
simulated2D::UnaryEqualityConstraint constraint(pt, key, mu);
eq2D::UnaryEqualityConstraint constraint(pt, key, mu);
simulated2D::Config config1;
config1.insert(key, pt);
@ -51,7 +53,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) {
Point2 pt(1.0, 2.0);
simulated2D::PoseKey key(1);
double mu = 1000.0;
simulated2D::UnaryEqualityConstraint constraint(pt, key, mu);
eq2D::UnaryEqualityConstraint constraint(pt, key, mu);
simulated2D::Config config1;
config1.insert(key, pt);
@ -74,8 +76,8 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
Point2 truth_pt(1.0, 2.0);
simulated2D::PoseKey key(1);
double mu = 1000.0;
simulated2D::UnaryEqualityConstraint::shared_ptr constraint(
new simulated2D::UnaryEqualityConstraint(truth_pt, key, mu));
eq2D::UnaryEqualityConstraint::shared_ptr constraint(
new eq2D::UnaryEqualityConstraint(truth_pt, key, mu));
Point2 badPt(100.0, -200.0);
simulated2D::Prior::shared_ptr factor(
@ -99,7 +101,7 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) {
Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0);
simulated2D::PoseKey key1(1), key2(2);
double mu = 1000.0;
simulated2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
simulated2D::Config config1;
config1.insert(key1, x1);
@ -125,7 +127,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) {
Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0);
simulated2D::PoseKey key1(1), key2(2);
double mu = 1000.0;
simulated2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
simulated2D::Config config1;
config1.insert(key1, x1);
@ -155,8 +157,8 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
simulated2D::PoseKey key1(1), key2(2);
// hard prior on x1
simulated2D::UnaryEqualityConstraint::shared_ptr constraint1(
new simulated2D::UnaryEqualityConstraint(truth_pt1, key1));
eq2D::UnaryEqualityConstraint::shared_ptr constraint1(
new eq2D::UnaryEqualityConstraint(truth_pt1, key1));
// soft prior on x2
Point2 badPt(100.0, -200.0);
@ -164,8 +166,8 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
new simulated2D::Prior(badPt, soft_model, key2));
// odometry constraint
simulated2D::OdoEqualityConstraint::shared_ptr constraint2(
new simulated2D::OdoEqualityConstraint(
eq2D::OdoEqualityConstraint::shared_ptr constraint2(
new eq2D::OdoEqualityConstraint(
gtsam::between(truth_pt1, truth_pt2), key1, key2));
shared_graph graph(new Graph());
@ -198,8 +200,8 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
simulated2D::PointKey l1(1), l2(2);
Point2 pt_x1(1.0, 1.0),
pt_x2(5.0, 6.0);
graph->add(simulated2D::UnaryEqualityConstraint(pt_x1, x1));
graph->add(simulated2D::UnaryEqualityConstraint(pt_x2, x2));
graph->add(eq2D::UnaryEqualityConstraint(pt_x1, x1));
graph->add(eq2D::UnaryEqualityConstraint(pt_x2, x2));
Point2 z1(0.0, 5.0);
SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1));
@ -208,7 +210,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
Point2 z2(-4.0, 0.0);
graph->add(simulated2D::Measurement(z2, sigma, x2,l2));
graph->add(simulated2D::PointEqualityConstraint(l1, l2));
graph->add(eq2D::PointEqualityConstraint(l1, l2));
shared_config initialEstimate(new simulated2D::Config());
initialEstimate->insert(x1, pt_x1);
@ -237,7 +239,7 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
// constant constraint on x1
Point2 pose1(1.0, 1.0);
graph->add(simulated2D::UnaryEqualityConstraint(pose1, x1));
graph->add(eq2D::UnaryEqualityConstraint(pose1, x1));
SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,0.1);
@ -250,7 +252,7 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
graph->add(simulated2D::Measurement(z2, sigma, x2, l2));
// equality constraint between l1 and l2
graph->add(simulated2D::PointEqualityConstraint(l1, l2));
graph->add(eq2D::PointEqualityConstraint(l1, l2));
// create an initial estimate
shared_config initialEstimate(new simulated2D::Config());

112
tests/testNonlinearInequalityConstraint.cpp Normal file
View File

@ -0,0 +1,112 @@
/**
* @file testNonlinearInequalityConstraint.cpp
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <simulated2DConstraints.h>
#include <NonlinearFactorGraph-inl.h>
#include <NonlinearOptimizer-inl.h>
namespace iq2D = gtsam::simulated2D::inequality_constraints;
using namespace std;
using namespace gtsam;
static const double tol = 1e-5;
SharedDiagonal soft_model2 = noiseModel::Unit::Create(2);
SharedDiagonal hard_model1 = noiseModel::Constrained::All(1);
typedef NonlinearFactorGraph<simulated2D::Config> Graph;
typedef boost::shared_ptr<Graph> shared_graph;
typedef boost::shared_ptr<simulated2D::Config> shared_config;
typedef NonlinearOptimizer<Graph, simulated2D::Config> Optimizer;
// some simple inequality constraints
simulated2D::PoseKey key(1);
double mu = 10.0;
iq2D::PoseXInequality constraint1(key, 1.0, mu);
iq2D::PoseYInequality constraint2(key, 2.0, mu);
/* ************************************************************************* */
TEST( testNonlinearInequalityConstraint, unary_basics_inactive ) {
Point2 pt1(2.0, 3.0);
simulated2D::Config config1;
config1.insert(key, pt1);
EXPECT(!constraint1.active(config1));
EXPECT(!constraint2.active(config1));
EXPECT(assert_equal(ones(1), constraint1.evaluateError(pt1), tol));
EXPECT(assert_equal(ones(1), constraint2.evaluateError(pt1), tol));
EXPECT(assert_equal(zero(1), constraint1.unwhitenedError(config1), tol));
EXPECT(assert_equal(zero(1), constraint2.unwhitenedError(config1), tol));
EXPECT_DOUBLES_EQUAL(0.0, constraint1.error(config1), tol);
EXPECT_DOUBLES_EQUAL(0.0, constraint2.error(config1), tol);
}
/* ************************************************************************* */
TEST( testNonlinearInequalityConstraint, unary_basics_active ) {
Point2 pt2(-2.0, -3.0);
simulated2D::Config config2;
config2.insert(key, pt2);
EXPECT(constraint1.active(config2));
EXPECT(constraint2.active(config2));
EXPECT(assert_equal(repeat(1, -3.0), constraint1.evaluateError(pt2), tol));
EXPECT(assert_equal(repeat(1, -5.0), constraint2.evaluateError(pt2), tol));
EXPECT(assert_equal(repeat(1, -3.0), constraint1.unwhitenedError(config2), tol));
EXPECT(assert_equal(repeat(1, -5.0), constraint2.unwhitenedError(config2), tol));
EXPECT_DOUBLES_EQUAL(90.0, constraint1.error(config2), tol);
EXPECT_DOUBLES_EQUAL(250.0, constraint2.error(config2), tol);
}
/* ************************************************************************* */
TEST( testNonlinearInequalityConstraint, unary_linearization_inactive) {
Point2 pt1(2.0, 3.0);
simulated2D::Config config1;
config1.insert(key, pt1);
GaussianFactor::shared_ptr actual1 = constraint1.linearize(config1);
GaussianFactor::shared_ptr actual2 = constraint2.linearize(config1);
EXPECT(!actual1);
EXPECT(!actual2);
}
/* ************************************************************************* */
TEST( testNonlinearInequalityConstraint, unary_linearization_active) {
Point2 pt2(-2.0, -3.0);
simulated2D::Config config2;
config2.insert(key, pt2);
GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2);
GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2);
GaussianFactor expected1(key, Matrix_(1, 2, 1.0, 0.0), repeat(1, 3.0), hard_model1);
GaussianFactor expected2(key, Matrix_(1, 2, 0.0, 1.0), repeat(1, 5.0), hard_model1);
EXPECT(assert_equal(expected1, *actual1, tol));
EXPECT(assert_equal(expected2, *actual2, tol));
}
/* ************************************************************************* */
TEST( testNonlinearInequalityConstraint, unary_simple_optimization) {
// create a single-node graph with a soft and hard constraint to
// ensure that the hard constraint overrides the soft constraint
Point2 goal_pt(1.0, 2.0);
Point2 start_pt(0.0, 1.0);
shared_graph graph(new Graph());
simulated2D::PoseKey x1(1);
graph->add(iq2D::PoseXInequality(x1, 1.0));
graph->add(iq2D::PoseYInequality(x1, 2.0));
graph->add(simulated2D::Prior(start_pt, soft_model2, x1));
shared_config initConfig(new simulated2D::Config());
initConfig->insert(x1, start_pt);
Optimizer::shared_config actual = Optimizer::optimizeLM(graph, initConfig, Optimizer::SILENT);
simulated2D::Config expected;
expected.insert(x1, goal_pt);
CHECK(assert_equal(expected, *actual, tol));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */