Reimplemented nonlinear constraints to remove the old dependency on using boost function pointers and allow for inequality and bounding constraints. Added simple equality constraints in simulated2D and new set of tests. Removed/disabled old tests until they can be worked into new structure.

release/4.3a0
Alex Cunningham 2010-08-06 18:30:07 +00:00
parent 7e3442286e
commit a55860eb1d
11 changed files with 1314 additions and 1711 deletions

View File

@ -22,9 +22,8 @@ headers += NonlinearFactor.h
sources += NonlinearOptimizer.cpp
# Nonlinear constraints
headers += NonlinearConstraint.h NonlinearConstraint-inl.h
headers += NonlinearConstraint.h
headers += NonlinearEquality.h
check_PROGRAMS += tests/testNonlinearConstraint
# SQP
if USE_LDL

View File

@ -1,166 +0,0 @@
/*
* @file NonlinearConstraint-inl.h
* @brief Implementation for NonlinearConstraints
* @author Alex Cunningham
*/
#pragma once
#include <iostream>
#include <boost/bind.hpp>
#include "NonlinearConstraint.h"
#define INSTANTIATE_NONLINEAR_CONSTRAINT(C) \
INSTANTIATE_FACTOR_GRAPH(NonlinearConstraint<C>); \
template class NonlinearConstraint<C>;
namespace gtsam {
/* ************************************************************************* */
// Implementations of base class
/* ************************************************************************* */
/* ************************************************************************* */
template <class Config>
NonlinearConstraint<Config>::NonlinearConstraint(size_t dim, double mu) :
NonlinearFactor<Config>(noiseModel::Constrained::All(dim)),
mu_(fabs(mu))
{
}
/* ************************************************************************* */
template <class Config>
double NonlinearConstraint<Config>::error(const Config& c) const {
const Vector error_vector = unwhitenedError(c);
return mu_ * inner_prod(error_vector, error_vector);
}
/* ************************************************************************* */
// Implementations of unary nonlinear constraints
/* ************************************************************************* */
template <class Config, class Key, class X>
NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
Vector (*g)(const Config& config),
const Key& key,
Matrix (*gradG)(const Config& config),
size_t dim, double mu) :
NonlinearConstraint<Config>(dim, mu),
G_(boost::bind(gradG, _1)), g_(boost::bind(g, _1)), key_(key)
{
this->keys_.push_back(key);
}
/* ************************************************************************* */
template <class Config, class Key, class X>
NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
boost::function<Vector(const Config& config)> g,
const Key& key,
boost::function<Matrix(const Config& config)> gradG,
size_t dim, double mu) :
NonlinearConstraint<Config>(dim, mu),
G_(gradG), g_(g), key_(key)
{
this->keys_.push_back(key);
}
/* ************************************************************************* */
template <class Config, class Key, class X>
void NonlinearConstraint1<Config, Key, X>::print(const std::string& s) const {
std::cout << "NonlinearConstraint1 [" << s << "]: Dim: " << this->dim()
<< " mu: " << this->mu_ << "\n"
<< " Key : " << (std::string) this->key_ << "\n";
}
/* ************************************************************************* */
template <class Config, class Key, class X>
bool NonlinearConstraint1<Config, Key, X>::equals(const Factor<Config>& f, double tol) const {
const NonlinearConstraint1<Config, Key, X>* p = dynamic_cast<const NonlinearConstraint1<Config, Key, X>*> (&f);
if (p == NULL) return false;
if (!(key_ == p->key_)) return false;
if (fabs(this->mu_ - p->mu_ ) > tol) return false;
return this->dim() == p->dim();
}
/* ************************************************************************* */
template <class Config, class Key, class X>
GaussianFactor::shared_ptr
NonlinearConstraint1<Config, Key, X>::linearize(const Config& config) const {
Vector g = -1.0 * g_(config);
Matrix grad = G_(config);
SharedDiagonal model = noiseModel::Constrained::All(this->dim());
GaussianFactor::shared_ptr factor(new GaussianFactor(this->key_, grad, g, model));
return factor;
}
/* ************************************************************************* */
// Implementations of binary nonlinear constraints
/* ************************************************************************* */
/* ************************************************************************* */
template <class Config, class Key1, class X1, class Key2, class X2>
NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
Vector (*g)(const Config& config),
const Key1& key1,
Matrix (*G1)(const Config& config),
const Key2& key2,
Matrix (*G2)(const Config& config),
size_t dim, double mu)
: NonlinearConstraint<Config>(dim, mu),
G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)), g_(boost::bind(g, _1)),
key1_(key1), key2_(key2)
{
this->keys_.push_back(key1);
this->keys_.push_back(key2);
}
/* ************************************************************************* */
template <class Config, class Key1, class X1, class Key2, class X2>
NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
boost::function<Vector(const Config& config)> g,
const Key1& key1,
boost::function<Matrix(const Config& config)> G1,
const Key2& key2,
boost::function<Matrix(const Config& config)> G2,
size_t dim, double mu)
: NonlinearConstraint<Config>(dim, mu),
G1_(G1), G2_(G2), g_(g),
key1_(key1), key2_(key2)
{
this->keys_.push_back(key1);
this->keys_.push_back(key2);
}
/* ************************************************************************* */
template <class Config, class Key1, class X1, class Key2, class X2>
void NonlinearConstraint2<Config, Key1, X1, Key2, X2>::print(const std::string& s) const {
std::cout << "NonlinearConstraint2 [" << s << "]: Dim: " << this->dim()
<< " mu: " << this->mu_ << "\n"
<< " Key1 : " << (std::string) this->key1_ << "\n"
<< " Key2 : " << (std::string) this->key2_ << "\n";
}
/* ************************************************************************* */
template <class Config, class Key1, class X1, class Key2, class X2>
bool NonlinearConstraint2<Config, Key1, X1, Key2, X2>::equals(const Factor<Config>& f, double tol) const {
const NonlinearConstraint2<Config, Key1, X1, Key2, X2>* p = dynamic_cast<const NonlinearConstraint2<Config, Key1, X1, Key2, X2>*> (&f);
if (p == NULL) return false;
if (!(key1_ == p->key1_)) return false;
if (!(key2_ == p->key2_)) return false;
if (fabs(this->mu_ - p->mu_ ) > tol) return false;
return this->dim() == p->dim();
}
/* ************************************************************************* */
template<class Config, class Key1, class X1, class Key2, class X2>
GaussianFactor::shared_ptr
NonlinearConstraint2<Config, Key1, X1, Key2, X2>::linearize(const Config& config) const {
Vector g = -1.0 * g_(config);
Matrix grad1 = G1_(config);
Matrix grad2 = G2_(config);
SharedDiagonal model = noiseModel::Constrained::All(this->dim());
GaussianFactor::shared_ptr factor(new GaussianFactor(this->key1_, grad1, this->key2_, grad2, g, model));
return factor;
}
}

View File

@ -31,7 +31,8 @@ template <class Config>
class NonlinearConstraint : public NonlinearFactor<Config> {
protected:
typedef NonlinearConstraint<Config> This;
typedef NonlinearFactor<Config> Base;
double mu_; // gain for quadratic merit function
public:
@ -40,7 +41,8 @@ public:
* @param dim is the dimension of the factor
* @param mu is the gain used at error evaluation (forced to be positive)
*/
NonlinearConstraint(size_t dim, double mu = 1000.0);
NonlinearConstraint(size_t dim, double mu = 1000.0):
Base(noiseModel::Constrained::All(dim)), mu_(fabs(mu)) {}
/** returns the gain mu */
double mu() const { return mu_; }
@ -49,10 +51,29 @@ public:
virtual void print(const std::string& s = "") const=0;
/** Check if two factors are equal */
virtual bool equals(const Factor<Config>& f, double tol=1e-9) const=0;
virtual bool equals(const Factor<Config>& f, double tol=1e-9) const {
const This* p = dynamic_cast<const This*> (&f);
if (p == NULL) return false;
return Base::equals(*p, tol) && (mu_ == p->mu_);
}
/** error function - returns the quadratic merit function */
virtual double error(const Config& c) const;
virtual double error(const Config& c) const {
const Vector error_vector = unwhitenedError(c);
if (active(c))
return mu_ * inner_prod(error_vector, error_vector);
else return 0.0;
}
/** Raw error vector function g(x) */
virtual Vector unwhitenedError(const Config& c) const = 0;
/**
* active set check, defines what type of constraint this is
* By default, the constraint is always active, so it is an equality constraint
* @return true if the constraint is active
*/
virtual bool active(const Config& c) const { return true; }
/**
* Linearizes around a given config
@ -64,31 +85,14 @@ public:
/**
* A unary constraint with arbitrary cost and jacobian functions
* This is an example class designed for easy testing, but real uses should probably
* subclass NonlinearConstraint and implement virtual functions directly
* A unary constraint that defaults to an equality constraint
*/
template <class Config, class Key, class X>
class NonlinearConstraint1 : public NonlinearConstraint<Config> {
private:
/**
* Calculates the jacobian of the constraint function
* returns a pxn matrix
* Use boost.bind to create the function object
* @param config to use for linearization
* @return the jacobian of the constraint in terms of key
*/
boost::function<Matrix(const Config& config)> G_;
/** calculates the constraint function of the current config
* If the value is zero, the constraint is not active
* Use boost.bind to create the function object
* @param config is a configuration of all the variables
* @return the cost for each of p constraints, arranged in a vector
*/
boost::function<Vector(const Config& config)> g_;
protected:
typedef NonlinearConstraint1<Config,Key,X> This;
typedef NonlinearConstraint<Config> Base;
/** key for the constrained variable */
Key key_;
@ -98,44 +102,52 @@ public:
/**
* Basic constructor
* @param key is the identifier for the variable constrained
* @param G gives the jacobian of the constraint function
* @param g is the constraint function
* @param dim is the size of the constraint (p)
* @param mu is the gain for the factor
*/
NonlinearConstraint1(
Vector (*g)(const Config& config),
const Key& key,
Matrix (*G)(const Config& config),
size_t dim,
double mu = 1000.0);
NonlinearConstraint1(const Key& key, size_t dim, double mu = 1000.0)
: Base(dim, mu), key_(key) {
this->keys_.push_back(key);
}
/**
* Basic constructor with boost function pointers
* @param key is the identifier for the variable constrained
* @param G gives the jacobian of the constraint function
* @param g is the constraint function as a boost function pointer
* @param dim is the size of the constraint (p)
* @param mu is the gain for the factor
*/
NonlinearConstraint1(
boost::function<Vector(const Config& config)> g,
const Key& key,
boost::function<Matrix(const Config& config)> G,
size_t dim,
double mu = 1000.0);
/* print */
void print(const std::string& s = "") const {
std::cout << "NonlinearConstraint1 " << s << std::endl;
std::cout << "key: " << (std::string) key_ << std::endl;
std::cout << "mu: " << this->mu_ << std::endl;
}
/** Print */
void print(const std::string& s = "") const;
/** Check if two factors are equal. Note type is Factor and needs cast. */
virtual bool equals(const Factor<Config>& f, double tol = 1e-9) const {
const This* p = dynamic_cast<const This*> (&f);
if (p == NULL) return false;
return Base::equals(*p, tol) && (key_ == p->key_);
}
/** Check if two factors are equal */
bool equals(const Factor<Config>& f, double tol=1e-9) const;
/** Error function */
virtual inline Vector unwhitenedError(const Config& c) const { return g_(c); }
/** error function g(x) */
inline Vector unwhitenedError(const Config& x) const {
const Key& j = key_;
const X& xj = x[j];
return evaluateError(xj);
}
/** Linearize from config */
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const;
boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
if (!active(c)) {
boost::shared_ptr<GaussianFactor> factor;
return factor;
}
const Key& j = key_;
const X& x = c[j];
Matrix grad;
Vector g = -1.0 * evaluateError(x, grad);
SharedDiagonal model = noiseModel::Constrained::All(this->dim());
return GaussianFactor::shared_ptr(new GaussianFactor(this->key_, grad, g, model));
}
/** g(x) with optional derivative */
virtual Vector evaluateError(const X& x, boost::optional<Matrix&> H =
boost::none) const = 0;
};
/**
@ -144,25 +156,9 @@ public:
template <class Config, class Key1, class X1, class Key2, class X2>
class NonlinearConstraint2 : public NonlinearConstraint<Config> {
private:
/**
* Calculates the jacobians of the constraint function in terms of
* the first and second variables
* returns a pxn matrix
* @param config to use for linearization
* @return the jacobian of the constraint in terms of key
*/
boost::function<Matrix(const Config& config)> G1_;
boost::function<Matrix(const Config& config)> G2_;
/** calculates the constraint function of the current config
* If the value is zero, the constraint is not active
* Use boost.bind to create the function object
* @param config is a configuration of all the variables
* @return the cost for each of p constraints, arranged in a vector
*/
boost::function<Vector(const Config& config)> g_;
protected:
typedef NonlinearConstraint2<Config,Key1,X1,Key2,X2> This;
typedef NonlinearConstraint<Config> Base;
/** keys for the constrained variables */
Key1 key1_;
@ -172,50 +168,59 @@ public:
/**
* Basic constructor
* @param key is the identifier for the variable constrained
* @param G gives the jacobian of the constraint function
* @param g is the constraint function
* @param key1 is the identifier for the first variable constrained
* @param key2 is the identifier for the second variable constrained
* @param dim is the size of the constraint (p)
* @param mu is the gain for the factor
*/
NonlinearConstraint2(
Vector (*g)(const Config& config),
const Key1& key1,
Matrix (*G1)(const Config& config),
const Key2& key2,
Matrix (*G2)(const Config& config),
size_t dim,
double mu = 1000.0);
NonlinearConstraint2(const Key1& key1, const Key2& key2, size_t dim, double mu = 1000.0) :
Base(dim, mu), key1_(key1), key2_(key2) {
this->keys_.push_back(key1);
this->keys_.push_back(key2);
}
/**
* Basic constructor with direct function objects
* Use boost.bind to construct the function objects
* @param key is the identifier for the variable constrained
* @param G gives the jacobian of the constraint function
* @param g is the constraint function
* @param dim is the size of the constraint (p)
* @param mu is the gain for the factor
*/
NonlinearConstraint2(
boost::function<Vector(const Config& config)> g,
const Key1& key1,
boost::function<Matrix(const Config& config)> G1,
const Key2& key2,
boost::function<Matrix(const Config& config)> G2,
size_t dim,
double mu = 1000.0);
/* print */
void print(const std::string& s = "") const {
std::cout << "NonlinearConstraint2 " << s << std::endl;
std::cout << "key1: " << (std::string) key1_ << std::endl;
std::cout << "key2: " << (std::string) key2_ << std::endl;
std::cout << "mu: " << this->mu_ << std::endl;
}
/** Print */
void print(const std::string& s = "") const;
/** Check if two factors are equal. Note type is Factor and needs cast. */
virtual bool equals(const Factor<Config>& f, double tol = 1e-9) const {
const This* p = dynamic_cast<const This*> (&f);
if (p == NULL) return false;
return Base::equals(*p, tol) && (key1_ == p->key1_) && (key2_ == p->key2_);
}
/** Check if two factors are equal */
bool equals(const Factor<Config>& f, double tol=1e-9) const;
/** Error function */
virtual inline Vector unwhitenedError(const Config& c) const { return g_(c); }
/** error function g(x) */
inline Vector unwhitenedError(const Config& x) const {
const Key1& j1 = key1_;
const Key2& j2 = key2_;
const X1& xj1 = x[j1];
const X2& xj2 = x[j2];
return evaluateError(xj1, xj2);
}
/** Linearize from config */
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const;
boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
if (!active(c)) {
boost::shared_ptr<GaussianFactor> factor;
return factor;
}
const Key1& j1 = key1_; const Key2& j2 = key2_;
const X1& x1 = c[j1]; const X2& x2 = c[j2];
Matrix grad1, grad2;
Vector g = -1.0 * evaluateError(x1, x2, grad1, grad2);
SharedDiagonal model = noiseModel::Constrained::All(this->dim());
return GaussianFactor::shared_ptr(new GaussianFactor(j1, grad1, j2, grad2, g, model));
}
/** g(x) with optional derivative2 */
virtual Vector evaluateError(const X1& x1, const X2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const = 0;
};
}

View File

@ -236,8 +236,6 @@ namespace gtsam {
/**
* A Gaussian nonlinear factor that takes 2 parameters
* Note: cannot be serialized as contains function pointers
* Specialized derived classes could do this
*/
template<class Config, class Key1, class X1, class Key2, class X2>
class NonlinearFactor2: public NonlinearFactor<Config> {
@ -353,8 +351,6 @@ namespace gtsam {
/**
* A Gaussian nonlinear factor that takes 3 parameters
* Note: cannot be serialized as contains function pointers
* Specialized derived classes could do this
*/
template<class Config, class Key1, class X1, class Key2, class X2, class Key3, class X3>
class NonlinearFactor3: public NonlinearFactor<Config> {

View File

@ -25,6 +25,106 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
/* *********************************************************************
* Example from SQP testing:
*
* This example uses a nonlinear objective function and
* nonlinear equality constraint. The formulation is actually
* the Cholesky form that creates the full Hessian explicitly,
* and isn't expecially compatible with our machinery.
*/
TEST (NonlinearConstraint, problem1_cholesky ) {
bool verbose = false;
// use a nonlinear function of f(x) = x^2+y^2
// nonlinear equality constraint: g(x) = x^2-5-y=0
// Lagrangian: f(x) + \lambda*g(x)
// Symbols
Symbol x1("x1"), y1("y1"), L1("L1");
// state structure: [x y \lambda]
VectorConfig init, state;
init.insert(x1, Vector_(1, 1.0));
init.insert(y1, Vector_(1, 1.0));
init.insert(L1, Vector_(1, 1.0));
state = init;
if (verbose) init.print("Initial State");
// loop until convergence
int maxIt = 10;
for (int i = 0; i<maxIt; ++i) {
if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl;
// extract the states
double x, y, lambda;
x = state[x1](0);
y = state[y1](0);
lambda = state[L1](0);
// calculate the components
Matrix H1, H2, gradG;
Vector gradL, gx;
// hessian of lagrangian function, in two columns:
H1 = Matrix_(2,1,
2.0+2.0*lambda,
0.0);
H2 = Matrix_(2,1,
0.0,
2.0);
// deriviative of lagrangian function
gradL = Vector_(2,
2.0*x*(1+lambda),
2.0*y-lambda);
// constraint derivatives
gradG = Matrix_(2,1,
2.0*x,
0.0);
// constraint value
gx = Vector_(1,
x*x-5-y);
// create a factor for the states
GaussianFactor::shared_ptr f1(new
GaussianFactor(x1, H1, y1, H2, L1, gradG, gradL, probModel2));
// create a factor for the lagrange multiplier
GaussianFactor::shared_ptr f2(new
GaussianFactor(x1, -sub(gradG, 0, 1, 0, 1),
y1, -sub(gradG, 1, 2, 0, 1), -gx, constraintModel1));
// construct graph
GaussianFactorGraph fg;
fg.push_back(f1);
fg.push_back(f2);
if (verbose) fg.print("Graph");
// solve
Ordering ord;
ord += x1, y1, L1;
VectorConfig delta = fg.optimize(ord).scale(-1.0);
if (verbose) delta.print("Delta");
// update initial estimate
VectorConfig newState = expmap(state, delta);
state = newState;
if (verbose) state.print("Updated State");
}
// verify that it converges to the nearest optimal point
VectorConfig expected;
expected.insert(L1, Vector_(1, -1.0));
expected.insert(x1, Vector_(1, 2.12));
expected.insert(y1, Vector_(1, -0.5));
CHECK(assert_equal(expected,state, 1e-2));
}
/* ************************************************************************* */
// Example of a single Constrained QP problem from the matlab testCQP.m file.
TEST( matrix, CQP_example ) {

View File

@ -1,373 +0,0 @@
/**
* @file testNonlinearConstraint.cpp
* @brief Tests for nonlinear constraints handled via SQP
* @author Alex Cunningham
*/
#include <list>
#include <boost/bind.hpp>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/list.hpp> // for operator +=
#define GTSAM_MAGIC_KEY
#include <VectorConfig.h>
#include <NonlinearConstraint.h>
#include <NonlinearConstraint-inl.h>
#include <TupleConfig-inl.h>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
typedef TypedSymbol<Vector, 'x'> Key;
typedef LieConfig<Key, Vector> VecConfig;
typedef NonlinearConstraint1<VecConfig, Key, Vector> NLC1;
typedef NonlinearConstraint2<VecConfig, Key, Vector, Key, Vector> NLC2;
/* ************************************************************************* */
// unary functions with scalar variables
/* ************************************************************************* */
namespace test1 {
/** p = 1, g(x) = x^2-5 = 0 */
Vector g(const VecConfig& config, const list<Key>& keys) {
double x = config[keys.front()](0);
return Vector_(1, x * x - 5);
}
/** p = 1, jacobianG(x) = 2x */
Matrix G(const VecConfig& config, const list<Key>& keys) {
double x = config[keys.front()](0);
return Matrix_(1, 1, 2 * x);
}
} // \namespace test1
/* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_construction ) {
// construct a constraint on x
size_t p = 1;
Key x1(1);
list<Key> keys; keys += x1;
NLC1 c1(boost::bind(test1::g, _1, keys),
x1, boost::bind(test1::G, _1, keys),
p, 10.0);
// get a configuration to use for finding the error
VecConfig config;
config.insert(x1, Vector_(1, 1.0));
// calculate the error
Vector actualVec = c1.unwhitenedError(config);
Vector expectedVec = Vector_(1, -4.0);
CHECK(assert_equal(actualVec, expectedVec, 1e-5));
double actError = c1.error(config);
double expError = 160.0;
DOUBLES_EQUAL(expError, actError, 1e-5);
}
/* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_linearize ) {
size_t p = 1;
Key x1(1);
list<Key> keys; keys += x1;
NLC1 c1(boost::bind(test1::g, _1, keys),
x1, boost::bind(test1::G, _1, keys),
p);
// get a configuration to use for linearization
VecConfig realconfig;
realconfig.insert(x1, Vector_(1, 1.0));
// linearize the system
GaussianFactor::shared_ptr linfactor = c1.linearize(realconfig);
// verify
SharedDiagonal model = noiseModel::Constrained::All(p);
Matrix Ax1 = Matrix_(p,1, 2.0);
Vector rhs = Vector_(p, 4.0);
GaussianFactor expectedFactor(x1, Ax1, rhs, model);
CHECK(assert_equal(*linfactor, expectedFactor));
}
/* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_equal ) {
Key x(0), y(1);
list<Key> keys1, keys2; keys1 += x; keys2 += y;
NLC1
c1(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1),
c2(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1),
c3(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 2),
c4(boost::bind(test1::g, _1, keys2), y, boost::bind(test1::G, _1, keys2), 1);
CHECK(assert_equal(c1, c2));
CHECK(assert_equal(c2, c1));
CHECK(!c1.equals(c3));
CHECK(!c1.equals(c4));
}
/* ************************************************************************* */
// binary functions with scalar variables
/* ************************************************************************* */
namespace test2 {
/** p = 1, g(x) = x^2-5 -y = 0 */
Vector g(const VecConfig& config, const list<Key>& keys) {
double x = config[keys.front()](0);
double y = config[keys.back()](0);
return Vector_(1, x * x - 5.0 - y);
}
/** jacobian for x, jacobianG(x,y) in x: 2x*/
Matrix G1(const VecConfig& config, const list<Key>& keys) {
double x = config[keys.front()](0);
return Matrix_(1, 1, 2.0 * x);
}
/** jacobian for y, jacobianG(x,y) in y: -1 */
Matrix G2(const VecConfig& config, const list<Key>& keys) {
return Matrix_(1, 1, -1.0);
}
} // \namespace test2
/* ************************************************************************* */
TEST( NonlinearConstraint2, binary_scalar_construction ) {
// construct a constraint on x and y
size_t p = 1;
Key x0(0), x1(1);
list<Key> keys; keys += x0, x1;
NLC2 c1(
boost::bind(test2::g, _1, keys),
x0, boost::bind(test2::G1, _1, keys),
x1, boost::bind(test2::G1, _1, keys),
p);
// get a configuration to use for finding the error
VecConfig config;
config.insert(x0, Vector_(1, 1.0));
config.insert(x1, Vector_(1, 2.0));
// calculate the error
Vector actual = c1.unwhitenedError(config);
Vector expected = Vector_(1.0, -6.0);
CHECK(assert_equal(actual, expected, 1e-5));
}
/* ************************************************************************* */
TEST( NonlinearConstraint2, binary_scalar_linearize ) {
// create a constraint
size_t p = 1;
Key x0(0), x1(1);
list<Key> keys; keys += x0, x1;
NLC2 c1(
boost::bind(test2::g, _1, keys),
x0, boost::bind(test2::G1, _1, keys),
x1, boost::bind(test2::G2, _1, keys),
p);
// get a configuration to use for finding the error
VecConfig realconfig;
realconfig.insert(x0, Vector_(1, 1.0));
realconfig.insert(x1, Vector_(1, 2.0));
// linearize the system
GaussianFactor::shared_ptr actualFactor = c1.linearize(realconfig);
// verify
Matrix Ax0 = Matrix_(1,1, 2.0),
Ax1 = Matrix_(1,1,-1.0);
Vector rhs = Vector_(1, 6.0);
SharedDiagonal expModel = noiseModel::Constrained::All(p);
GaussianFactor expFactor(x0,Ax0, x1, Ax1,rhs, expModel);
CHECK(assert_equal(expFactor, *actualFactor));
}
/* ************************************************************************* */
TEST( NonlinearConstraint2, binary_scalar_equal ) {
list<Key> keys1, keys2, keys3;
Key x0(0), x1(1), x2(2);
keys1 += x0, x1; keys2 += x1, x0; keys3 += x0;
NLC2
c1(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1),
c2(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1),
c3(boost::bind(test2::g, _1, keys2), x1, boost::bind(test2::G1, _1, keys2), x0, boost::bind(test2::G2, _1, keys2), 1),
c4(boost::bind(test2::g, _1, keys3), x0, boost::bind(test2::G1, _1, keys3), x2, boost::bind(test2::G2, _1, keys3), 3);
CHECK(assert_equal(c1, c2));
CHECK(assert_equal(c2, c1));
CHECK(!c1.equals(c3));
CHECK(!c1.equals(c4));
}
/* ************************************************************************* */
// Inequality tests - DISABLED
/* ************************************************************************* */
//namespace inequality1 {
//
// /** p = 1, g(x) x^2 - 5 > 0 */
// Vector g(const VecConfig& config, const Key& key) {
// double x = config[key](0);
// double g = x * x - 5;
// return Vector_(1, g); // return the actual cost
// }
//
// /** p = 1, jacobianG(x) = 2*x */
// Matrix G(const VecConfig& config, const Key& key) {
// double x = config[key](0);
// return Matrix_(1, 1, 2 * x);
// }
//
//} // \namespace inequality1
//
///* ************************************************************************* */
//TEST( NonlinearConstraint1, unary_inequality ) {
// size_t p = 1;
// Key x0(0);
// NLC1 c1(boost::bind(inequality1::g, _1, x0),
// x0, boost::bind(inequality1::G, _1, x0),
// p, false); // inequality constraint
//
// // get configurations to use for evaluation
// VecConfig config1, config2;
// config1.insert(x0, Vector_(1, 10.0)); // should be inactive
// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
//
// // check error
// CHECK(!c1.active(config1));
// Vector actualError2 = c1.unwhitenedError(config2);
// CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
// CHECK(c1.active(config2));
//}
//
///* ************************************************************************* */
//TEST( NonlinearConstraint1, unary_inequality_linearize ) {
// size_t p = 1;
// Key x0(0);
// NLC1 c1(boost::bind(inequality1::g, _1, x0),
// x0, boost::bind(inequality1::G, _1, x0),
// p, false); // inequality constraint
//
// // get configurations to use for linearization
// VecConfig config1, config2;
// config1.insert(x0, Vector_(1, 10.0)); // should have zero error
// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
//
// // linearize for inactive constraint
// GaussianFactor::shared_ptr actualFactor1 = c1.linearize(config1);
//
// // check if the factor is active
// CHECK(!c1.active(config1));
//
// // linearize for active constraint
// GaussianFactor::shared_ptr actualFactor2 = c1.linearize(config2);
// CHECK(c1.active(config2));
//
// // verify
// Vector sigmas = Vector_(2, 1.0, 0.0);
// SharedDiagonal model = noiseModel::Constrained::MixedSigmas(sigmas);
// GaussianFactor expectedFactor(x0, Matrix_(2,1, 6.0, 2.0),
// L1, Matrix_(2,1, 1.0, 0.0),
// Vector_(2, 0.0, 4.0), model);
//
// CHECK(assert_equal(*actualFactor2, expectedFactor));
//}
/* ************************************************************************* */
// Binding arbitrary functions
/* ************************************************************************* */
//namespace binding1 {
//
// /** p = 1, g(x) x^2 - r > 0 */
// Vector g(double r, const VecConfig& config, const Key& key) {
// double x = config[key](0);
// double g = x * x - r;
// return Vector_(1, g); // return the actual cost
// }
//
// /** p = 1, jacobianG(x) = 2*x */
// Matrix G(double coeff, const VecConfig& config,
// const Key& key) {
// double x = config[key](0);
// return Matrix_(1, 1, coeff * x);
// }
//
//} // \namespace binding1
//
///* ************************************************************************* */
//TEST( NonlinearConstraint1, unary_binding ) {
// size_t p = 1;
// double coeff = 2;
// double radius = 5;
// Key x0(0);
// NLC1 c1(boost::bind(binding1::g, radius, _1, x0),
// x0, boost::bind(binding1::G, coeff, _1, x0),
// p, false); // inequality constraint
//
// // get configurations to use for evaluation
// VecConfig config1, config2;
// config1.insert(x0, Vector_(1, 10.0)); // should have zero error
// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
//
// // check error
// CHECK(!c1.active(config1));
// Vector actualError2 = c1.unwhitenedError(config2);
// CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
// CHECK(c1.active(config2));
//}
/* ************************************************************************* */
namespace binding2 {
/** p = 1, g(x) = x^2-5 -y = 0 */
Vector g(double r, const VecConfig& config, const Key& k1, const Key& k2) {
double x = config[k1](0);
double y = config[k2](0);
return Vector_(1, x * x - r - y);
}
/** jacobian for x, jacobianG(x,y) in x: 2x*/
Matrix G1(double c, const VecConfig& config, const Key& key) {
double x = config[key](0);
return Matrix_(1, 1, c * x);
}
/** jacobian for y, jacobianG(x,y) in y: -1 */
Matrix G2(double c, const VecConfig& config) {
return Matrix_(1, 1, -1.0 * c);
}
} // \namespace binding2
/* ************************************************************************* */
TEST( NonlinearConstraint2, binary_binding ) {
// construct a constraint on x and y
size_t p = 1;
double a = 2.0;
double b = 1.0;
double r = 5.0;
Key x0(0), x1(1);
NLC2 c1(boost::bind(binding2::g, r, _1, x0, x1),
x0, boost::bind(binding2::G1, a, _1, x0),
x1, boost::bind(binding2::G2, b, _1),
p);
// get a configuration to use for finding the error
VecConfig config;
config.insert(x0, Vector_(1, 1.0));
config.insert(x1, Vector_(1, 2.0));
// calculate the error
Vector actual = c1.unwhitenedError(config);
Vector expected = Vector_(1.0, -6.0);
CHECK(assert_equal(actual, expected, 1e-5));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

View File

@ -56,6 +56,7 @@ namespace gtsam {
*/
template<class Cfg = Config, class Key = PoseKey>
struct GenericPrior: public NonlinearFactor1<Cfg, Key, Point2> {
typedef boost::shared_ptr<GenericPrior<Cfg, Key> > shared_ptr;
Point2 z_;
@ -76,6 +77,7 @@ namespace gtsam {
template<class Cfg = Config, class Key = PoseKey>
struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Point2, Key,
Point2> {
typedef boost::shared_ptr<GenericOdometry<Cfg, Key> > shared_ptr;
Point2 z_;
GenericOdometry(const Point2& z, const SharedGaussian& model,
@ -97,7 +99,7 @@ namespace gtsam {
class GenericMeasurement: public NonlinearFactor2<Cfg, XKey, Point2, LKey,
Point2> {
public:
typedef boost::shared_ptr<GenericMeasurement<Cfg, XKey, LKey> > shared_ptr;
Point2 z_;
GenericMeasurement(const Point2& z, const SharedGaussian& model,

View File

@ -0,0 +1,67 @@
/**
* @file simulated2DConstraints.h
* @brief measurement functions and constraint definitions for simulated 2D robot
* @author Alex Cunningham
*/
// \callgraph
#pragma once
#include <NonlinearConstraint.h>
#include <simulated2D.h>
// \namespace
namespace gtsam {
namespace simulated2D {
/**
* Unary factor 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 factor simulating "odometry" between two Vectors
*/
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();
}
};
/** Typedefs for regular use */
typedef GenericUnaryEqualityConstraint<Config, PoseKey> UnaryEqualityConstraint;
typedef GenericOdoHardEqualityConstraint<Config, PoseKey> OdoEqualityConstraint;
} // namespace simulated2D
} // namespace gtsam

View File

@ -8,8 +8,10 @@ check_PROGRAMS += testGaussianBayesNet testGaussianFactor testGaussianFactorGrap
check_PROGRAMS += testGaussianISAM testGraph
check_PROGRAMS += testInference testIterative testGaussianJunctionTree
check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph
check_PROGRAMS += testNonlinearOptimizer testNonlinearConstraint testSubgraphPreconditioner
check_PROGRAMS += testNonlinearOptimizer testSubgraphPreconditioner
check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph testTupleConfig
# check_PROGRAMS += testNonlinearConstraint # disabled until examples rearranged
check_PROGRAMS += testNonlinearEqualityConstraint
if USE_LDL
check_PROGRAMS += testConstraintOptimizer

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,191 @@
/**
* @file testNonlinearEqualityConstraint.cpp
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <simulated2DConstraints.h>
#include <NonlinearFactorGraph-inl.h>
#include <NonlinearOptimizer-inl.h>
using namespace std;
using namespace gtsam;
static const double tol = 1e-9;
SharedDiagonal hard_model = noiseModel::Constrained::All(2);
SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
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;
/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, unary_basics ) {
Point2 pt(1.0, 2.0);
simulated2D::PoseKey key(1);
double mu = 1000.0;
simulated2D::UnaryEqualityConstraint constraint(pt, key, mu);
simulated2D::Config config1;
config1.insert(key, pt);
EXPECT(constraint.active(config1));
EXPECT(assert_equal(zero(2), constraint.evaluateError(pt), tol));
EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol));
EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol);
simulated2D::Config config2;
Point2 ptBad1(2.0, 2.0);
config2.insert(key, ptBad1);
EXPECT(constraint.active(config2));
EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.evaluateError(ptBad1), tol));
EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.unwhitenedError(config2), tol));
EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol);
}
/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, unary_linearization ) {
Point2 pt(1.0, 2.0);
simulated2D::PoseKey key(1);
double mu = 1000.0;
simulated2D::UnaryEqualityConstraint constraint(pt, key, mu);
simulated2D::Config config1;
config1.insert(key, pt);
GaussianFactor::shared_ptr actual1 = constraint.linearize(config1);
GaussianFactor::shared_ptr expected1(new GaussianFactor(key, eye(2,2), zero(2), hard_model));
EXPECT(assert_equal(*expected1, *actual1, tol));
simulated2D::Config config2;
Point2 ptBad(2.0, 2.0);
config2.insert(key, ptBad);
GaussianFactor::shared_ptr actual2 = constraint.linearize(config2);
GaussianFactor::shared_ptr expected2(new GaussianFactor(key, eye(2,2), Vector_(2,-1.0,0.0), hard_model));
EXPECT(assert_equal(*expected2, *actual2, tol));
}
/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, 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 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));
Point2 badPt(100.0, -200.0);
simulated2D::Prior::shared_ptr factor(
new simulated2D::Prior(badPt, soft_model, key));
shared_graph graph(new Graph());
graph->push_back(constraint);
graph->push_back(factor);
shared_config initConfig(new simulated2D::Config());
initConfig->insert(key, badPt);
Optimizer::shared_config actual = Optimizer::optimizeLM(graph, initConfig);
simulated2D::Config expected;
expected.insert(key, truth_pt);
CHECK(assert_equal(expected, *actual, tol));
}
/* ************************************************************************* */
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);
simulated2D::Config config1;
config1.insert(key1, x1);
config1.insert(key2, x2);
EXPECT(constraint.active(config1));
EXPECT(assert_equal(zero(2), constraint.evaluateError(x1, x2), tol));
EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol));
EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol);
simulated2D::Config config2;
Point2 x1bad(2.0, 2.0);
Point2 x2bad(2.0, 2.0);
config2.insert(key1, x1bad);
config2.insert(key2, x2bad);
EXPECT(constraint.active(config2));
EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol));
EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.unwhitenedError(config2), tol));
EXPECT_DOUBLES_EQUAL(2000.0, constraint.error(config2), tol);
}
/* ************************************************************************* */
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);
simulated2D::Config config1;
config1.insert(key1, x1);
config1.insert(key2, x2);
GaussianFactor::shared_ptr actual1 = constraint.linearize(config1);
GaussianFactor::shared_ptr expected1(
new GaussianFactor(key1, -eye(2,2), key2, eye(2,2), zero(2), hard_model));
EXPECT(assert_equal(*expected1, *actual1, tol));
simulated2D::Config config2;
Point2 x1bad(2.0, 2.0);
Point2 x2bad(2.0, 2.0);
config2.insert(key1, x1bad);
config2.insert(key2, x2bad);
GaussianFactor::shared_ptr actual2 = constraint.linearize(config2);
GaussianFactor::shared_ptr expected2(
new GaussianFactor(key1, -eye(2,2), key2, eye(2,2), Vector_(2, 1.0, 1.0), hard_model));
EXPECT(assert_equal(*expected2, *actual2, tol));
}
/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
// create a two-node graph, connected by an odometry constraint, with
// a hard prior on one variable, and a conflicting soft prior
// on the other variable - the constraints should override the soft constraint
Point2 truth_pt1(1.0, 2.0), truth_pt2(3.0, 2.0);
simulated2D::PoseKey key1(1), key2(2);
// hard prior on x1
simulated2D::UnaryEqualityConstraint::shared_ptr constraint1(
new simulated2D::UnaryEqualityConstraint(truth_pt1, key1));
// soft prior on x2
Point2 badPt(100.0, -200.0);
simulated2D::Prior::shared_ptr factor(
new simulated2D::Prior(badPt, soft_model, key2));
// odometry constraint
simulated2D::OdoEqualityConstraint::shared_ptr constraint2(
new simulated2D::OdoEqualityConstraint(
gtsam::between(truth_pt1, truth_pt2), key1, key2));
shared_graph graph(new Graph());
graph->push_back(constraint1);
graph->push_back(constraint2);
graph->push_back(factor);
shared_config initConfig(new simulated2D::Config());
initConfig->insert(key1, Point2());
initConfig->insert(key2, badPt);
Optimizer::shared_config actual = Optimizer::optimizeLM(graph, initConfig);
simulated2D::Config expected;
expected.insert(key1, truth_pt1);
expected.insert(key2, truth_pt2);
CHECK(assert_equal(expected, *actual, tol));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */