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

Nonlinear bounding constraints are now working for simple examples. Also, removed extraneous constraint test files.

release/4.3a0
Alex Cunningham 2010-08-09 21:59:29 +00:00
parent efaca162cf
commit 4f9a60d41c
9 changed files with 492 additions and 369 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
nonlinear/NonlinearConstraint.h
View File

@ -325,35 +325,4 @@ 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_;
}
};
}

44
slam/BetweenConstraint.h Normal file
View File

@ -0,0 +1,44 @@
/**
* @file BetweenConstraint.h
* @brief Implements a constraint to force a between
* @author Alex Cunningham
*/
#pragma once
#include <NonlinearConstraint.h>
namespace gtsam {
/**
* 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_;
}
};
} // \namespace gtsam

105
slam/BoundingConstraint.h Normal file
View File

@ -0,0 +1,105 @@
/**
* @file BoundingConstraint.h
* @brief Provides partially implemented constraints to implement bounds
* @author Alex Cunningham
*/
#pragma once
#include <Lie.h>
#include <NonlinearConstraint.h>
namespace gtsam {
/**
* Unary inequality constraint forcing a scalar to be
* greater/less than a fixed threshold. The function
* will need to have its value function implemented to return
* a scalar for comparison.
*/
template<class Cfg, class Key, class X>
struct BoundingConstraint1: public NonlinearConstraint1<Cfg, Key, X> {
typedef NonlinearConstraint1<Cfg, Key, X> Base;
typedef boost::shared_ptr<BoundingConstraint1<Cfg, Key, X> > shared_ptr;
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
BoundingConstraint1(const Key& key, double threshold,
bool isGreaterThan, double mu = 1000.0) :
Base(key, 1, mu), threshold_(threshold), isGreaterThan_(isGreaterThan) {
}
inline double threshold() const { return threshold_; }
inline bool isGreaterThan() const { return isGreaterThan_; }
/**
* function producing a scalar value to compare to the threshold
* Must have optional argument for derivative with 1xN matrix, where
* N = X::dim()
*/
virtual double value(const X& x, boost::optional<Matrix&> H =
boost::none) const = 0;
/** active when constraint *NOT* met */
bool active(const Cfg& c) const {
// note: still active at equality to avoid zigzagging
double x = value(c[this->key_]);
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
}
Vector evaluateError(const X& x, boost::optional<Matrix&> H =
boost::none) const {
Matrix D;
double error = value(x, D) - threshold_;
if (H) *H = (isGreaterThan_) ? D : -1.0 * D;
return (isGreaterThan_) ? Vector_(1, error) : -1.0 * Vector_(1, error);
}
};
/**
* Binary scalar inequality constraint, with a similar value() function
* to implement for specific systems
*/
template<class Cfg, class Key1, class X1, class Key2, class X2>
struct BoundingConstraint2: public NonlinearConstraint2<Cfg, Key1, X1, Key2, X2> {
typedef NonlinearConstraint2<Cfg, Key1, X1, Key2, X2> Base;
typedef boost::shared_ptr<BoundingConstraint2<Cfg, Key1, X1, Key2, X2> > shared_ptr;
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
BoundingConstraint2(const Key1& key1, const Key2& key2, double threshold,
bool isGreaterThan, double mu = 1000.0)
: Base(key1, key2, 1, mu), threshold_(threshold), isGreaterThan_(isGreaterThan) {}
inline double threshold() const { return threshold_; }
inline bool isGreaterThan() const { return isGreaterThan_; }
/**
* function producing a scalar value to compare to the threshold
* Must have optional argument for derivatives)
*/
virtual double value(const X1& x1, const X2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const = 0;
/** active when constraint *NOT* met */
bool active(const Cfg& c) const {
// note: still active at equality to avoid zigzagging
double x = value(c[this->key1_], c[this->key2_]);
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
}
Vector evaluateError(const X1& x1, const X2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
Matrix D1, D2;
double error = value(x1, x2, D1, D2) - threshold_;
if (H1) *H1 = (isGreaterThan_) ? D1 : -1.0 * D1;
if (H2) *H2 = (isGreaterThan_) ? D2 : -1.0 * D2;
return (isGreaterThan_) ? Vector_(1, error) : -1.0 * Vector_(1, error);
}
};
} // \namespace gtsam

3
slam/Makefile.am
View File

@ -27,6 +27,9 @@ check_PROGRAMS += tests/testSimulated3D
# Pose SLAM headers
headers += BetweenFactor.h PriorFactor.h
# General constraints
headers += BetweenConstraint.h BoundingConstraint.h
# 2D Pose SLAM
sources += pose2SLAM.cpp Pose2SLAMOptimizer.cpp dataset.cpp
check_PROGRAMS += tests/testPose2Factor tests/testPose2Config tests/testPose2SLAM tests/testPose2Prior

85
slam/simulated2DConstraints.h
View File

@ -8,7 +8,11 @@
#pragma once
#include <numericalDerivative.h> // TODO: remove
#include <NonlinearConstraint.h>
#include <BetweenConstraint.h>
#include <BoundingConstraint.h>
#include <simulated2D.h>
// \namespace
@ -21,6 +25,7 @@ namespace gtsam {
/** Typedefs for regular use */
typedef NonlinearEquality1<Config, PoseKey, Point2> UnaryEqualityConstraint;
typedef NonlinearEquality1<Config, PointKey, Point2> UnaryEqualityPointConstraint;
typedef BetweenConstraint<Config, PoseKey, Point2> OdoEqualityConstraint;
/** Equality between variables */
@ -32,39 +37,85 @@ namespace gtsam {
namespace inequality_constraints {
/**
* Unary inequality constraint forcing a coordinate to be greater 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
*/
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;
struct ScalarCoordConstraint1: public BoundingConstraint1<Cfg, Key, Point2> {
typedef BoundingConstraint1<Cfg, Key, Point2> Base;
typedef boost::shared_ptr<ScalarCoordConstraint1<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) {
ScalarCoordConstraint1(const Key& key, double c,
bool isGreaterThan, double mu = 1000.0) :
Base(key, c, isGreaterThan, mu) {
}
/** 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
}
inline unsigned int index() const { return Idx; }
Vector evaluateError(const Point2& x, boost::optional<Matrix&> H =
/** 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 Vector_(1, x.vector()(Idx) - c_);
return x.vector()(Idx);
}
};
/** typedefs for use with simulated2D systems */
typedef ScalarInequalityConstraint1<Config, PoseKey, 0> PoseXInequality;
typedef ScalarInequalityConstraint1<Config, PoseKey, 1> PoseYInequality;
typedef ScalarCoordConstraint1<Config, PoseKey, 0> PoseXInequality;
typedef ScalarCoordConstraint1<Config, 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, Point2, Key, Point2> {
typedef BoundingConstraint2<Cfg, Key, Point2, Key, Point2> 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<Config, 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, Point2, PKey, Point2> {
typedef BoundingConstraint2<Cfg, XKey, Point2, PKey, Point2> 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<Config, PoseKey, PointKey> LandmarkAvoid;
} // \namespace inequality_constraints

3
tests/Makefile.am
View File

@ -10,8 +10,7 @@ check_PROGRAMS += testInference testIterative testGaussianJunctionTree
check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph
check_PROGRAMS += testNonlinearOptimizer testSubgraphPreconditioner
check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph testTupleConfig
# check_PROGRAMS += testNonlinearConstraint # disabled until examples rearranged
check_PROGRAMS += testNonlinearEqualityConstraint testNonlinearInequalityConstraint
check_PROGRAMS += testNonlinearEqualityConstraint testBoundingConstraint
if USE_LDL
check_PROGRAMS += testConstraintOptimizer

271
tests/testBoundingConstraint.cpp Normal file
View File

@ -0,0 +1,271 @@
/**
* @file testBoundingConstraint.cpp
* @brief test of nonlinear inequality constraints on scalar bounds
* @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 soft_model2_alt = noiseModel::Isotropic::Sigma(2, 0.1);
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;
// greater than
iq2D::PoseXInequality constraint1(key, 1.0, true, mu);
iq2D::PoseYInequality constraint2(key, 2.0, true, mu);
// less than
iq2D::PoseXInequality constraint3(key, 1.0, false, mu);
iq2D::PoseYInequality constraint4(key, 2.0, false, mu);
/* ************************************************************************* */
TEST( testBoundingConstraint, unary_basics_inactive1 ) {
Point2 pt1(2.0, 3.0);
simulated2D::Config config;
config.insert(key, pt1);
EXPECT(!constraint1.active(config));
EXPECT(!constraint2.active(config));
EXPECT_DOUBLES_EQUAL(1.0, constraint1.threshold(), tol);
EXPECT_DOUBLES_EQUAL(2.0, constraint2.threshold(), tol);
EXPECT(constraint1.isGreaterThan());
EXPECT(constraint2.isGreaterThan());
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(config), tol));
EXPECT(assert_equal(zero(1), constraint2.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(0.0, constraint1.error(config), tol);
EXPECT_DOUBLES_EQUAL(0.0, constraint2.error(config), tol);
}
/* ************************************************************************* */
TEST( testBoundingConstraint, unary_basics_inactive2 ) {
Point2 pt2(-2.0, -3.0);
simulated2D::Config config;
config.insert(key, pt2);
EXPECT(!constraint3.active(config));
EXPECT(!constraint4.active(config));
EXPECT_DOUBLES_EQUAL(1.0, constraint3.threshold(), tol);
EXPECT_DOUBLES_EQUAL(2.0, constraint4.threshold(), tol);
EXPECT(!constraint3.isGreaterThan());
EXPECT(!constraint4.isGreaterThan());
EXPECT(assert_equal(repeat(1, 3.0), constraint3.evaluateError(pt2), tol));
EXPECT(assert_equal(repeat(1, 5.0), constraint4.evaluateError(pt2), tol));
EXPECT(assert_equal(zero(1), constraint3.unwhitenedError(config), tol));
EXPECT(assert_equal(zero(1), constraint4.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(0.0, constraint3.error(config), tol);
EXPECT_DOUBLES_EQUAL(0.0, constraint4.error(config), tol);
}
/* ************************************************************************* */
TEST( testBoundingConstraint, unary_basics_active1 ) {
Point2 pt2(-2.0, -3.0);
simulated2D::Config config;
config.insert(key, pt2);
EXPECT(constraint1.active(config));
EXPECT(constraint2.active(config));
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(config), tol));
EXPECT(assert_equal(repeat(1,-5.0), constraint2.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(90.0, constraint1.error(config), tol);
EXPECT_DOUBLES_EQUAL(250.0, constraint2.error(config), tol);
}
/* ************************************************************************* */
TEST( testBoundingConstraint, unary_basics_active2 ) {
Point2 pt1(2.0, 3.0);
simulated2D::Config config;
config.insert(key, pt1);
EXPECT(constraint3.active(config));
EXPECT(constraint4.active(config));
EXPECT(assert_equal(-1.0 * ones(1), constraint3.evaluateError(pt1), tol));
EXPECT(assert_equal(-1.0 * ones(1), constraint4.evaluateError(pt1), tol));
EXPECT(assert_equal(-1.0 * ones(1), constraint3.unwhitenedError(config), tol));
EXPECT(assert_equal(-1.0 * ones(1), constraint4.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(10.0, constraint3.error(config), tol);
EXPECT_DOUBLES_EQUAL(10.0, constraint4.error(config), tol);
}
/* ************************************************************************* */
TEST( testBoundingConstraint, 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( testBoundingConstraint, 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( testBoundingConstraint, unary_simple_optimization1) {
// 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, true));
graph->add(iq2D::PoseYInequality(x1, 2.0, true));
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));
}
/* ************************************************************************* */
TEST( testBoundingConstraint, unary_simple_optimization2) {
// 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(2.0, 3.0);
shared_graph graph(new Graph());
simulated2D::PoseKey x1(1);
graph->add(iq2D::PoseXInequality(x1, 1.0, false));
graph->add(iq2D::PoseYInequality(x1, 2.0, false));
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));
}
/* ************************************************************************* */
TEST( testBoundingConstraint, MaxDistance_basics) {
simulated2D::PoseKey key1(1), key2(2);
Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0);
iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu);
EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol);
EXPECT(!rangeBound.isGreaterThan());
EXPECT(rangeBound.dim() == 1);
EXPECT(assert_equal(Vector_(1, 2.0), rangeBound.evaluateError(pt1, pt1)));
EXPECT(assert_equal(ones(1), rangeBound.evaluateError(pt1, pt2)));
EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3)));
EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4)));
simulated2D::Config config1;
config1.insert(key1, pt1);
config1.insert(key2, pt1);
EXPECT(!rangeBound.active(config1));
EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1)));
EXPECT(!rangeBound.linearize(config1));
EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
config1.update(key2, pt2);
EXPECT(!rangeBound.active(config1));
EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1)));
EXPECT(!rangeBound.linearize(config1));
EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
config1.update(key2, pt3);
EXPECT(rangeBound.active(config1));
EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1)));
EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
config1.update(key2, pt4);
EXPECT(rangeBound.active(config1));
EXPECT(assert_equal(-1.0*ones(1), rangeBound.unwhitenedError(config1)));
EXPECT_DOUBLES_EQUAL(1.0*mu, rangeBound.error(config1), tol);
}
/* ************************************************************************* */
TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
simulated2D::PoseKey x1(1), x2(2);
Graph graph;
graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1));
graph.add(simulated2D::Prior(pt2_init, soft_model2_alt, x2));
graph.add(iq2D::PoseMaxDistConstraint(x1, x2, 2.0));
simulated2D::Config initial_state;
initial_state.insert(x1, pt1);
initial_state.insert(x2, pt2_init);
Optimizer::shared_config actual = Optimizer::optimizeLM(graph, initial_state);
simulated2D::Config expected;
expected.insert(x1, pt1);
expected.insert(x2, pt2_goal);
EXPECT(assert_equal(expected, *actual, tol));
}
/* ************************************************************************* */
TEST( testBoundingConstraint, avoid_demo) {
simulated2D::PoseKey x1(1), x2(2), x3(3);
simulated2D::PointKey l1(1);
double radius = 1.0;
Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
Point2 odo(2.0, 0.0);
Graph graph;
graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(x1_pt, x1));
graph.add(simulated2D::Odometry(odo, soft_model2_alt, x1, x2));
graph.add(iq2D::LandmarkAvoid(x2, l1, radius));
graph.add(simulated2D::equality_constraints::UnaryEqualityPointConstraint(l1_pt, l1));
graph.add(simulated2D::Odometry(odo, soft_model2_alt, x2, x3));
graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(x3_pt, x3));
simulated2D::Config init, expected;
init.insert(x1, x1_pt);
init.insert(x3, x3_pt);
init.insert(l1, l1_pt);
expected = init;
init.insert(x2, x2_init);
expected.insert(x2, x2_goal);
Optimizer::shared_config actual = Optimizer::optimizeLM(graph, init);
EXPECT(assert_equal(expected, *actual, tol));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

207
tests/testNonlinearConstraint.cpp
View File

@ -1,207 +0,0 @@
/*
* @file testNonlinearConstraint.cpp
* @brief demos of constrained optimization using existing gtsam components
* @author Alex Cunningham
*/
#include <iostream>
#include <cmath>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/map.hpp> // for insert
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include <Point2.h>
#include <Pose3.h>
#include <GaussianFactorGraph.h>
#include <NonlinearOptimizer.h>
#include <simulated2D.h>
#include <Ordering.h>
#include <visualSLAM.h>
// templated implementations
#include <NonlinearFactorGraph-inl.h>
#include <NonlinearOptimizer-inl.h>
#include <TupleConfig-inl.h>
using namespace std;
using namespace gtsam;
using namespace boost;
using namespace boost::assign;
//// Models to use
//SharedDiagonal probModel1 = sharedSigma(1,1.0);
//SharedDiagonal probModel2 = sharedSigma(2,1.0);
//SharedDiagonal constraintModel1 = noiseModel::Constrained::All(1);
//
//// trick from some reading group
//#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
//
///* ********************************************************************* */
//// full components
//typedef simulated2D::Config Config2D;
//typedef NonlinearFactorGraph<Config2D> Graph2D;
//typedef NonlinearEquality<Config2D, simulated2D::PoseKey, Point2> NLE;
//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.
/* ********************************************************************* */
//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())));
//}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

112
tests/testNonlinearInequalityConstraint.cpp
View File

@ -1,112 +0,0 @@
/**
* @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); }
/* ************************************************************************* */