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

Changed NonlinearConstraint to correctly use new keys

release/4.3a0
Alex Cunningham 2010-02-04 16:08:11 +00:00
parent 01bbd3cf8d
commit 67744a5f07
3 changed files with 275 additions and 299 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
cpp/NonlinearConstraint-inl.h
View File

@ -18,7 +18,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template <class Config> template <class Config>
NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key, NonlinearConstraint<Config>::NonlinearConstraint(const LagrangeKey& lagrange_key,
size_t dim_lagrange, size_t dim_lagrange,
Vector (*g)(const Config& config), Vector (*g)(const Config& config),
bool isEquality) bool isEquality)
@ -28,7 +28,7 @@ NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key
/* ************************************************************************* */ /* ************************************************************************* */
template <class Config> template <class Config>
NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key, NonlinearConstraint<Config>::NonlinearConstraint(const LagrangeKey& lagrange_key,
size_t dim_lagrange, size_t dim_lagrange,
boost::function<Vector(const Config& config)> g, boost::function<Vector(const Config& config)> g,
bool isEquality) bool isEquality)
@ -52,16 +52,11 @@ NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
const Key& key, const Key& key,
Matrix (*gradG)(const Config& config), Matrix (*gradG)(const Config& config),
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key, const LagrangeKey& lagrange_key,
bool isEquality) : bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
G_(boost::bind(gradG, _1)), key_(key) G_(boost::bind(gradG, _1)), key_(key)
{ {
// set a good lagrange key here
// TODO:should do something smart to find a unique one
// if (lagrange_key == "")
// this->lagrange_key_ = "L0"
// this->keys_.push_front(key);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -71,16 +66,11 @@ NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
const Key& key, const Key& key,
boost::function<Matrix(const Config& config)> gradG, boost::function<Matrix(const Config& config)> gradG,
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key, const LagrangeKey& lagrange_key,
bool isEquality) : bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
G_(gradG), key_(key) G_(gradG), key_(key)
{ {
// set a good lagrange key here
// TODO:should do something smart to find a unique one
// if (lagrange_key == "")
// this->lagrange_key_ = "L_" + key;
// this->keys_.push_front(key);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -102,17 +92,19 @@ bool NonlinearConstraint1<Config, Key, X>::equals(const Factor<Config>& f, doubl
const NonlinearConstraint1<Config, Key, X>* p = dynamic_cast<const NonlinearConstraint1<Config, Key, X>*> (&f); const NonlinearConstraint1<Config, Key, X>* p = dynamic_cast<const NonlinearConstraint1<Config, Key, X>*> (&f);
if (p == NULL) return false; if (p == NULL) return false;
if (!(key_ == p->key_)) return false; if (!(key_ == p->key_)) return false;
if (this->lagrange_key_ != p->lagrange_key_) return false; if (!(this->lagrange_key_.equals(p->lagrange_key_))) return false;
if (this->isEquality_ != p->isEquality_) return false; if (this->isEquality_ != p->isEquality_) return false;
return this->p_ == p->p_; return this->p_ == p->p_;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <class Config, class Key, class X> template <class Config, class Key, class X>
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> GaussianFactor::shared_ptr
NonlinearConstraint1<Config, Key, X>::linearize(const Config& config, const VectorConfig& lagrange) const { NonlinearConstraint1<Config, Key, X>::linearize(const Config& config) const {
const size_t p = this->p_;
// extract lagrange multiplier // extract lagrange multiplier
Vector lambda = lagrange[this->lagrange_key_]; Vector lambda = config[this->lagrange_key_];
// find the error // find the error
Vector g = g_(config); Vector g = g_(config);
@ -120,17 +112,25 @@ NonlinearConstraint1<Config, Key, X>::linearize(const Config& config, const Vect
// construct the gradient // construct the gradient
Matrix grad = G_(config); Matrix grad = G_(config);
// construct probabilistic factor // construct combined factor
Matrix A1 = vector_scale(lambda, grad); Matrix Ax = zeros(grad.size1()*2, grad.size2());
SharedDiagonal probModel = sharedSigma(this->p_,1.0); insertSub(Ax, vector_scale(lambda, grad), 0, 0);
insertSub(Ax, grad, grad.size1(), 0);
Matrix AL = eye(p*2, p);
Vector rhs = zero(p*2);
subInsert(rhs, -1*g, p);
// construct a mixed constraint model
Vector sigmas = zero(p*2);
subInsert(sigmas, ones(p), 0);
SharedDiagonal mixedConstraint = noiseModel::Constrained::MixedSigmas(sigmas);
GaussianFactor::shared_ptr factor(new GaussianFactor::shared_ptr factor(new
GaussianFactor(key_, A1, this->lagrange_key_, eye(this->p_), zero(this->p_), probModel)); GaussianFactor(key_, Ax, this->lagrange_key_, AL, rhs, mixedConstraint));
// construct the constraint return factor;
SharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_);
GaussianFactor::shared_ptr constraint(new GaussianFactor(key_, grad, -1*g, constraintModel));
return std::make_pair(factor, constraint);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -146,18 +146,12 @@ NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
const Key2& key2, const Key2& key2,
Matrix (*G2)(const Config& config), Matrix (*G2)(const Config& config),
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key, const LagrangeKey& lagrange_key,
bool isEquality) : bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)), G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)),
key1_(key1), key2_(key2) key1_(key1), key2_(key2)
{ {
// set a good lagrange key here
// TODO:should do something smart to find a unique one
// if (lagrange_key == "")
// this->lagrange_key_ = "L_" + key1 + key2;
// this->keys_.push_front(key1);
// this->keys_.push_back(key2);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -169,18 +163,12 @@ NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
const Key2& key2, const Key2& key2,
boost::function<Matrix(const Config& config)> G2, boost::function<Matrix(const Config& config)> G2,
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key, const LagrangeKey& lagrange_key,
bool isEquality) : bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
G1_(G1), G2_(G2), G1_(G1), G2_(G2),
key1_(key1), key2_(key2) key1_(key1), key2_(key2)
{ {
// set a good lagrange key here
// TODO:should do something smart to find a unique one
// if (lagrange_key == "")
// this->lagrange_key_ = "L_" + key1 + key2;
// this->keys_.push_front(key1);
// this->keys_.push_back(key2);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -200,17 +188,17 @@ bool NonlinearConstraint2<Config, Key1, X1, Key2, X2>::equals(const Factor<Confi
if (p == NULL) return false; if (p == NULL) return false;
if (!(key1_ == p->key1_)) return false; if (!(key1_ == p->key1_)) return false;
if (!(key2_ == p->key2_)) return false; if (!(key2_ == p->key2_)) return false;
if (this->lagrange_key_ != p->lagrange_key_) return false; if (!(this->lagrange_key_.equals(p->lagrange_key_))) return false;
if (this->isEquality_ != p->isEquality_) return false; if (this->isEquality_ != p->isEquality_) return false;
return this->p_ == p->p_; return this->p_ == p->p_;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Config, class Key1, class X1, class Key2, class X2> template<class Config, class Key1, class X1, class Key2, class X2>
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> NonlinearConstraint2< GaussianFactor::shared_ptr
Config, Key1, X1, Key2, X2>::linearize(const Config& config, const VectorConfig& lagrange) const { NonlinearConstraint2<Config, Key1, X1, Key2, X2>::linearize(const Config& config) const {
// extract lagrange multiplier // extract lagrange multiplier
Vector lambda = lagrange[this->lagrange_key_]; Vector lambda = config[this->lagrange_key_];
// find the error // find the error
Vector g = g_(config); Vector g = g_(config);
@ -231,7 +219,7 @@ std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> NonlinearConst
GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1, GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1,
key2_, grad2, -1.0 * g, constraintModel)); key2_, grad2, -1.0 * g, constraintModel));
return std::make_pair(factor, constraint); return factor;
} }
} }

57
cpp/NonlinearConstraint.h
View File

@ -13,6 +13,9 @@
namespace gtsam { namespace gtsam {
/** Typedef for Lagrange key type - must be present in factors and config */
typedef TypedSymbol<Vector, 'L'> LagrangeKey;
/** /**
* Base class for nonlinear constraints * Base class for nonlinear constraints
* This allows for both equality and inequality constraints, * This allows for both equality and inequality constraints,
@ -29,7 +32,7 @@ class NonlinearConstraint : public NonlinearFactor<Config> {
protected: protected:
/** key for the lagrange multipliers */ /** key for the lagrange multipliers */
std::string lagrange_key_; LagrangeKey lagrange_key_;
/** number of lagrange multipliers */ /** number of lagrange multipliers */
size_t p_; size_t p_;
@ -53,7 +56,7 @@ public:
* @param isEquality is true if the constraint is an equality constraint * @param isEquality is true if the constraint is an equality constraint
* @param g is the cost function for the constraint * @param g is the cost function for the constraint
*/ */
NonlinearConstraint(const std::string& lagrange_key, NonlinearConstraint(const LagrangeKey& lagrange_key,
size_t dim_lagrange, size_t dim_lagrange,
Vector (*g)(const Config& config), Vector (*g)(const Config& config),
bool isEquality=true); bool isEquality=true);
@ -64,13 +67,13 @@ public:
* @param g is the cost function for the constraint * @param g is the cost function for the constraint
* @param isEquality is true if the constraint is an equality constraint * @param isEquality is true if the constraint is an equality constraint
*/ */
NonlinearConstraint(const std::string& lagrange_key, NonlinearConstraint(const LagrangeKey& lagrange_key,
size_t dim_lagrange, size_t dim_lagrange,
boost::function<Vector(const Config& config)> g, boost::function<Vector(const Config& config)> g,
bool isEquality=true); bool isEquality=true);
/** returns the key used for the Lagrange multipliers */ /** returns the key used for the Lagrange multipliers */
std::string lagrangeKey() const { return lagrange_key_; } LagrangeKey lagrangeKey() const { return lagrange_key_; }
/** returns the number of lagrange multipliers */ /** returns the number of lagrange multipliers */
size_t nrConstraints() const { return p_; } size_t nrConstraints() const { return p_; }
@ -95,23 +98,11 @@ public:
bool active(const Config& config) const; bool active(const Config& config) const;
/** /**
* Linearize using a real Config and a VectorConfig of Lagrange multipliers * Real linearize, given a config that includes Lagrange multipliers
* Returns the two separate Gaussian factors to solve * @param config is the configuration (with lagrange multipliers)
* @param config is the real Config of the real variables * @return a combined linear factor containing both the constraint and the constraint factor
* @param lagrange is the VectorConfig of lagrange multipliers
* @return a pair GaussianFactor (probabilistic) and GaussianFactor (constraint)
*/ */
virtual std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const=0;
linearize(const Config& config, const VectorConfig& lagrange) const=0;
/**
* linearize with only Config, which is not currently implemented
* This will be implemented later for other constrained optimization
* algorithms
*/
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
throw std::invalid_argument("No current constraint linearization for a single Config!");
}
}; };
@ -151,7 +142,7 @@ public:
const Key& key, const Key& key,
Matrix (*G)(const Config& config), Matrix (*G)(const Config& config),
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key="", const LagrangeKey& lagrange_key,
bool isEquality=true); bool isEquality=true);
/** /**
@ -168,7 +159,7 @@ public:
const Key& key, const Key& key,
boost::function<Matrix(const Config& config)> G, boost::function<Matrix(const Config& config)> G,
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key="", const LagrangeKey& lagrange_key,
bool isEquality=true); bool isEquality=true);
/** Print */ /** Print */
@ -178,14 +169,9 @@ public:
bool equals(const Factor<Config>& f, double tol=1e-9) const; bool equals(const Factor<Config>& f, double tol=1e-9) const;
/** /**
* Linearize using a real Config and a VectorConfig of Lagrange multipliers * Linearize from config - must have Lagrange multipliers
* Returns the two separate Gaussian factors to solve
* @param config is the real Config of the real variables
* @param lagrange is the VectorConfig of lagrange multipliers
* @return a pair GaussianFactor (probabilistic) and GaussianFactor (constraint)
*/ */
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const;
linearize(const Config& config, const VectorConfig& lagrange) const;
}; };
/** /**
@ -228,7 +214,7 @@ public:
const Key2& key2, const Key2& key2,
Matrix (*G2)(const Config& config), Matrix (*G2)(const Config& config),
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key="", const LagrangeKey& lagrange_key,
bool isEquality=true); bool isEquality=true);
/** /**
@ -248,7 +234,7 @@ public:
const Key2& key2, const Key2& key2,
boost::function<Matrix(const Config& config)> G2, boost::function<Matrix(const Config& config)> G2,
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key="", const LagrangeKey& lagrange_key,
bool isEquality=true); bool isEquality=true);
/** Print */ /** Print */
@ -258,14 +244,9 @@ public:
bool equals(const Factor<Config>& f, double tol=1e-9) const; bool equals(const Factor<Config>& f, double tol=1e-9) const;
/** /**
* Linearize using a real Config and a VectorConfig of Lagrange multipliers * Linearize from config - must have Lagrange multipliers
* Returns the two separate Gaussian factors to solve
* @param config is the real Config of the real variables
* @param lagrange is the VectorConfig of lagrange multipliers
* @return a pair GaussianFactor (probabilistic) and GaussianFactor (constraint)
*/ */
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const;
linearize(const Config& config, const VectorConfig& lagrange) const;
}; };
} }

437
cpp/testNonlinearConstraint.cpp
View File

@ -23,7 +23,6 @@ typedef TypedSymbol<Vector, 'x'> Key;
typedef NonlinearConstraint1<VectorConfig, Key, Vector> NLC1; typedef NonlinearConstraint1<VectorConfig, Key, Vector> NLC1;
typedef NonlinearConstraint2<VectorConfig, Key, Vector, Key, Vector> NLC2; typedef NonlinearConstraint2<VectorConfig, Key, Vector, Key, Vector> NLC2;
/* ************************************************************************* */ /* ************************************************************************* */
// unary functions with scalar variables // unary functions with scalar variables
/* ************************************************************************* */ /* ************************************************************************* */
@ -51,9 +50,10 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
size_t p = 1; size_t p = 1;
list<Symbol> keys; keys += "x1"; list<Symbol> keys; keys += "x1";
Key x1(1); Key x1(1);
LagrangeKey L1(1);
NLC1 c1(boost::bind(test1::g, _1, keys), NLC1 c1(boost::bind(test1::g, _1, keys),
x1, boost::bind(test1::G, _1, keys), x1, boost::bind(test1::G, _1, keys),
p, "L1"); p, L1);
// get a configuration to use for finding the error // get a configuration to use for finding the error
VectorConfig config; VectorConfig config;
@ -70,40 +70,41 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
size_t p = 1; size_t p = 1;
list<Symbol> keys; keys += "x1"; list<Symbol> keys; keys += "x1";
Key x1(1); Key x1(1);
LagrangeKey L1(1);
NLC1 c1(boost::bind(test1::g, _1, keys), NLC1 c1(boost::bind(test1::g, _1, keys),
x1, boost::bind(test1::G, _1, keys), x1, boost::bind(test1::G, _1, keys),
p, "L1"); p, L1);
// get a configuration to use for linearization // get a configuration to use for linearization (with lagrange multipliers)
VectorConfig realconfig; VectorConfig realconfig;
realconfig.insert(x1, Vector_(1, 1.0)); realconfig.insert(x1, Vector_(1, 1.0));
realconfig.insert(L1, Vector_(1, 3.0));
// get a configuration of Lagrange multipliers
VectorConfig lagrangeConfig;
lagrangeConfig.insert("L1", Vector_(1, 3.0));
// linearize the system // linearize the system
GaussianFactor::shared_ptr actualFactor, actualConstraint; GaussianFactor::shared_ptr linfactor = c1.linearize(realconfig);
boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig);
// verify // verify - probabilistic component goes on top
SharedDiagonal probModel = sharedSigma(p,1.0); Vector sigmas = Vector_(2, 1.0, 0.0);
GaussianFactor expectedFactor(x1, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), probModel); SharedDiagonal mixedModel = noiseModel::Constrained::MixedSigmas(sigmas);
SharedDiagonal constraintModel = noiseModel::Constrained::All(p); // stack the matrices to combine
GaussianFactor expectedConstraint(x1, Matrix_(1,1, 2.0), Vector_(1, 4.0), constraintModel); Matrix Ax1 = Matrix_(2,1, 6.0, 2.0),
CHECK(assert_equal(*actualFactor, expectedFactor)); AL1 = Matrix_(2,1, 1.0, 0.0);
CHECK(assert_equal(*actualConstraint, expectedConstraint)); Vector rhs = Vector_(2, 0.0, 4.0);
GaussianFactor expectedFactor(x1, Ax1, L1, AL1, rhs, mixedModel);
CHECK(assert_equal(*linfactor, expectedFactor));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_equal ) { TEST( NonlinearConstraint1, unary_scalar_equal ) {
list<Symbol> keys1, keys2; keys1 += "x0"; keys2 += "x1"; list<Symbol> keys1, keys2; keys1 += "x0"; keys2 += "x1";
Key x(0), y(1); Key x(0), y(1);
LagrangeKey L1(1);
NLC1 NLC1
c1(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, "L_x1", true), c1(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, L1, true),
c2(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, "L_x1"), c2(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, L1),
c3(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 2, "L_x1"), c3(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 2, L1),
c4(boost::bind(test1::g, _1, keys2), y, boost::bind(test1::G, _1, keys2), 1, "L_x1"); c4(boost::bind(test1::g, _1, keys2), y, boost::bind(test1::G, _1, keys2), 1, L1);
CHECK(assert_equal(c1, c2)); CHECK(assert_equal(c1, c2));
CHECK(assert_equal(c2, c1)); CHECK(assert_equal(c2, c1));
@ -145,11 +146,12 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
size_t p = 1; size_t p = 1;
list<Symbol> keys; keys += "x0", "x1"; list<Symbol> keys; keys += "x0", "x1";
Key x0(0), x1(1); Key x0(0), x1(1);
LagrangeKey L1(1);
NLC2 c1( NLC2 c1(
boost::bind(test2::g, _1, keys), boost::bind(test2::g, _1, keys),
x0, boost::bind(test2::G1, _1, keys), x0, boost::bind(test2::G1, _1, keys),
x1, boost::bind(test2::G1, _1, keys), x1, boost::bind(test2::G1, _1, keys),
p, "L12"); p, L1);
// get a configuration to use for finding the error // get a configuration to use for finding the error
VectorConfig config; VectorConfig config;
@ -168,36 +170,40 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
size_t p = 1; size_t p = 1;
list<Symbol> keys; keys += "x0", "x1"; list<Symbol> keys; keys += "x0", "x1";
Key x0(0), x1(1); Key x0(0), x1(1);
LagrangeKey L1(1);
NLC2 c1( NLC2 c1(
boost::bind(test2::g, _1, keys), boost::bind(test2::g, _1, keys),
x0, boost::bind(test2::G1, _1, keys), x0, boost::bind(test2::G1, _1, keys),
x1, boost::bind(test2::G2, _1, keys), x1, boost::bind(test2::G2, _1, keys),
p, "L12"); p, L1);
// get a configuration to use for finding the error // get a configuration to use for finding the error
VectorConfig realconfig; VectorConfig realconfig;
realconfig.insert(x0, Vector_(1, 1.0)); realconfig.insert(x0, Vector_(1, 1.0));
realconfig.insert(x1, Vector_(1, 2.0)); realconfig.insert(x1, Vector_(1, 2.0));
realconfig.insert(L1, Vector_(1, 3.0));
// get a configuration of Lagrange multipliers
VectorConfig lagrangeConfig;
lagrangeConfig.insert("L12", Vector_(1, 3.0));
// linearize the system // linearize the system
GaussianFactor::shared_ptr actualFactor, actualConstraint; GaussianFactor::shared_ptr actualFactor = c1.linearize(realconfig);
boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig);
// verify // verify
SharedDiagonal probModel = sharedSigma(p,1.0); Matrix Ax0 = Matrix_(2,1, 6.0, 2.0),
GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), Ax1 = Matrix_(2,1,-3.0,-1.0),
x1, Matrix_(1,1, -3.0), AL = Matrix_(2,1, 1.0, 0.0);
"L12", eye(1), zero(1), probModel); Vector rhs = Vector_(2, 0, 6.0),
SharedDiagonal constraintModel = noiseModel::Constrained::All(p); sigmas = Vector_(2, 1.0, 0.0);
GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), SharedDiagonal expModel = noiseModel::Constrained::MixedSigmas(sigmas);
x1, Matrix_(1,1, -1.0),
Vector_(1, 6.0), constraintModel); // SharedDiagonal probModel = sharedSigma(p,1.0);
CHECK(assert_equal(*actualFactor, expectedFactor)); // GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0),
CHECK(assert_equal(*actualConstraint, expectedConstraint)); // x1, Matrix_(1,1, -3.0),
// L1, eye(1), zero(1), probModel);
// SharedDiagonal constraintModel = noiseModel::Constrained::All(p);
// GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0),
// x1, Matrix_(1,1, -1.0),
// Vector_(1, 6.0), constraintModel);
// CHECK(assert_equal(*actualFactor, expectedFactor));
// CHECK(assert_equal(*actualConstraint, expectedConstraint));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -205,11 +211,12 @@ TEST( NonlinearConstraint2, binary_scalar_equal ) {
list<Symbol> keys1, keys2, keys3; list<Symbol> keys1, keys2, keys3;
keys1 += "x0", "x1"; keys2 += "x1", "x0"; keys3 += "x0", "z"; keys1 += "x0", "x1"; keys2 += "x1", "x0"; keys3 += "x0", "z";
Key x0(0), x1(1), x2(2); Key x0(0), x1(1), x2(2);
LagrangeKey L1(1);
NLC2 NLC2
c1(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, "L_xy"), c1(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, L1),
c2(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, "L_xy"), c2(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, L1),
c3(boost::bind(test2::g, _1, keys2), x1, boost::bind(test2::G1, _1, keys2), x0, boost::bind(test2::G2, _1, keys2), 1, "L_xy"), c3(boost::bind(test2::g, _1, keys2), x1, boost::bind(test2::G1, _1, keys2), x0, boost::bind(test2::G2, _1, keys2), 1, L1),
c4(boost::bind(test2::g, _1, keys3), x0, boost::bind(test2::G1, _1, keys3), x2, boost::bind(test2::G2, _1, keys3), 3, "L_xy"); c4(boost::bind(test2::g, _1, keys3), x0, boost::bind(test2::G1, _1, keys3), x2, boost::bind(test2::G2, _1, keys3), 3, L1);
CHECK(assert_equal(c1, c2)); CHECK(assert_equal(c1, c2));
CHECK(assert_equal(c2, c1)); CHECK(assert_equal(c2, c1));
@ -217,178 +224,178 @@ TEST( NonlinearConstraint2, binary_scalar_equal ) {
CHECK(!c1.equals(c4)); CHECK(!c1.equals(c4));
} }
/* ************************************************************************* */ ///* ************************************************************************* */
// Inequality tests //// Inequality tests
/* ************************************************************************* */ ///* ************************************************************************* */
namespace inequality1 { //namespace inequality1 {
//
/** p = 1, g(x) x^2 - 5 > 0 */ // /** p = 1, g(x) x^2 - 5 > 0 */
Vector g(const VectorConfig& config, const Key& key) { // Vector g(const VectorConfig& config, const Key& key) {
double x = config[key](0); // double x = config[key](0);
double g = x * x - 5; // double g = x * x - 5;
return Vector_(1, g); // return the actual cost // return Vector_(1, g); // return the actual cost
} // }
//
/** p = 1, jacobianG(x) = 2*x */ // /** p = 1, jacobianG(x) = 2*x */
Matrix G(const VectorConfig& config, const Key& key) { // Matrix G(const VectorConfig& config, const Key& key) {
double x = config[key](0); // double x = config[key](0);
return Matrix_(1, 1, 2 * x); // return Matrix_(1, 1, 2 * x);
} // }
//
} // \namespace inequality1 //} // \namespace inequality1
//
/* ************************************************************************* */ ///* ************************************************************************* */
TEST( NonlinearConstraint1, unary_inequality ) { //TEST( NonlinearConstraint1, unary_inequality ) {
size_t p = 1; // size_t p = 1;
Key x0(0); // Key x0(0);
NLC1 c1(boost::bind(inequality1::g, _1, x0), // NLC1 c1(boost::bind(inequality1::g, _1, x0),
x0, boost::bind(inequality1::G, _1, x0), // x0, boost::bind(inequality1::G, _1, x0),
p, "L1", // p, "L1",
false); // inequality constraint // false); // inequality constraint
//
// get configurations to use for evaluation // // get configurations to use for evaluation
VectorConfig config1, config2; // VectorConfig config1, config2;
config1.insert(x0, Vector_(1, 10.0)); // should be inactive // config1.insert(x0, Vector_(1, 10.0)); // should be inactive
config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error // config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
//
// check error // // check error
CHECK(!c1.active(config1)); // CHECK(!c1.active(config1));
Vector actualError2 = c1.unwhitenedError(config2); // Vector actualError2 = c1.unwhitenedError(config2);
CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9))); // CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
CHECK(c1.active(config2)); // CHECK(c1.active(config2));
} //}
//
/* ************************************************************************* */ ///* ************************************************************************* */
TEST( NonlinearConstraint1, unary_inequality_linearize ) { //TEST( NonlinearConstraint1, unary_inequality_linearize ) {
size_t p = 1; // size_t p = 1;
Key x0(0); // Key x0(0);
NLC1 c1(boost::bind(inequality1::g, _1, x0), // NLC1 c1(boost::bind(inequality1::g, _1, x0),
x0, boost::bind(inequality1::G, _1, x0), // x0, boost::bind(inequality1::G, _1, x0),
p, "L1", // p, "L1",
false); // inequality constraint // false); // inequality constraint
//
// get configurations to use for linearization // // get configurations to use for linearization
VectorConfig config1, config2; // VectorConfig config1, config2;
config1.insert(x0, Vector_(1, 10.0)); // should have zero error // config1.insert(x0, Vector_(1, 10.0)); // should have zero error
config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error // config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
//
// get a configuration of Lagrange multipliers // // get a configuration of Lagrange multipliers
VectorConfig lagrangeConfig; // VectorConfig lagrangeConfig;
lagrangeConfig.insert("L1", Vector_(1, 3.0)); // lagrangeConfig.insert("L1", Vector_(1, 3.0));
//
// linearize for inactive constraint // // linearize for inactive constraint
GaussianFactor::shared_ptr actualFactor1, actualConstraint1; // GaussianFactor::shared_ptr actualFactor1, actualConstraint1;
boost::tie(actualFactor1, actualConstraint1) = c1.linearize(config1, lagrangeConfig); // boost::tie(actualFactor1, actualConstraint1) = c1.linearize(config1, lagrangeConfig);
//
// check if the factor is active // // check if the factor is active
CHECK(!c1.active(config1)); // CHECK(!c1.active(config1));
//
// linearize for active constraint // // linearize for active constraint
GaussianFactor::shared_ptr actualFactor2, actualConstraint2; // GaussianFactor::shared_ptr actualFactor2, actualConstraint2;
boost::tie(actualFactor2, actualConstraint2) = c1.linearize(config2, lagrangeConfig); // boost::tie(actualFactor2, actualConstraint2) = c1.linearize(config2, lagrangeConfig);
CHECK(c1.active(config2)); // CHECK(c1.active(config2));
//
// verify // // verify
SharedDiagonal probModel = sharedSigma(p,1.0); // SharedDiagonal probModel = sharedSigma(p,1.0);
GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), probModel); // GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), probModel);
SharedDiagonal constraintModel = noiseModel::Constrained::All(p); // SharedDiagonal constraintModel = noiseModel::Constrained::All(p);
GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), Vector_(1, 4.0), constraintModel); // GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), Vector_(1, 4.0), constraintModel);
CHECK(assert_equal(*actualFactor2, expectedFactor)); // CHECK(assert_equal(*actualFactor2, expectedFactor));
CHECK(assert_equal(*actualConstraint2, expectedConstraint)); // CHECK(assert_equal(*actualConstraint2, expectedConstraint));
} //}
//
/* ************************************************************************* */ ///* ************************************************************************* */
// Binding arbitrary functions //// Binding arbitrary functions
/* ************************************************************************* */ ///* ************************************************************************* */
namespace binding1 { //namespace binding1 {
//
/** p = 1, g(x) x^2 - r > 0 */ // /** p = 1, g(x) x^2 - r > 0 */
Vector g(double r, const VectorConfig& config, const Key& key) { // Vector g(double r, const VectorConfig& config, const Key& key) {
double x = config[key](0); // double x = config[key](0);
double g = x * x - r; // double g = x * x - r;
return Vector_(1, g); // return the actual cost // return Vector_(1, g); // return the actual cost
} // }
//
/** p = 1, jacobianG(x) = 2*x */ // /** p = 1, jacobianG(x) = 2*x */
Matrix G(double coeff, const VectorConfig& config, // Matrix G(double coeff, const VectorConfig& config,
const Key& key) { // const Key& key) {
double x = config[key](0); // double x = config[key](0);
return Matrix_(1, 1, coeff * x); // return Matrix_(1, 1, coeff * x);
} // }
//
} // \namespace binding1 //} // \namespace binding1
//
/* ************************************************************************* */ ///* ************************************************************************* */
TEST( NonlinearConstraint1, unary_binding ) { //TEST( NonlinearConstraint1, unary_binding ) {
size_t p = 1; // size_t p = 1;
double coeff = 2; // double coeff = 2;
double radius = 5; // double radius = 5;
Key x0(0); // Key x0(0);
NLC1 c1(boost::bind(binding1::g, radius, _1, x0), // NLC1 c1(boost::bind(binding1::g, radius, _1, x0),
x0, boost::bind(binding1::G, coeff, _1, x0), // x0, boost::bind(binding1::G, coeff, _1, x0),
p, "L1", // p, "L1",
false); // inequality constraint // false); // inequality constraint
//
// get configurations to use for evaluation // // get configurations to use for evaluation
VectorConfig config1, config2; // VectorConfig config1, config2;
config1.insert(x0, Vector_(1, 10.0)); // should have zero error // config1.insert(x0, Vector_(1, 10.0)); // should have zero error
config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error // config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
//
// check error // // check error
CHECK(!c1.active(config1)); // CHECK(!c1.active(config1));
Vector actualError2 = c1.unwhitenedError(config2); // Vector actualError2 = c1.unwhitenedError(config2);
CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9))); // CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
CHECK(c1.active(config2)); // CHECK(c1.active(config2));
} //}
//
/* ************************************************************************* */ ///* ************************************************************************* */
namespace binding2 { //namespace binding2 {
//
/** p = 1, g(x) = x^2-5 -y = 0 */ // /** p = 1, g(x) = x^2-5 -y = 0 */
Vector g(double r, const VectorConfig& config, const Key& k1, const Key& k2) { // Vector g(double r, const VectorConfig& config, const Key& k1, const Key& k2) {
double x = config[k1](0); // double x = config[k1](0);
double y = config[k2](0); // double y = config[k2](0);
return Vector_(1, x * x - r - y); // return Vector_(1, x * x - r - y);
} // }
//
/** jacobian for x, jacobianG(x,y) in x: 2x*/ // /** jacobian for x, jacobianG(x,y) in x: 2x*/
Matrix G1(double c, const VectorConfig& config, const Key& key) { // Matrix G1(double c, const VectorConfig& config, const Key& key) {
double x = config[key](0); // double x = config[key](0);
return Matrix_(1, 1, c * x); // return Matrix_(1, 1, c * x);
} // }
//
/** jacobian for y, jacobianG(x,y) in y: -1 */ // /** jacobian for y, jacobianG(x,y) in y: -1 */
Matrix G2(double c, const VectorConfig& config) { // Matrix G2(double c, const VectorConfig& config) {
return Matrix_(1, 1, -1.0 * c); // return Matrix_(1, 1, -1.0 * c);
} // }
//
} // \namespace binding2 //} // \namespace binding2
//
/* ************************************************************************* */ ///* ************************************************************************* */
TEST( NonlinearConstraint2, binary_binding ) { //TEST( NonlinearConstraint2, binary_binding ) {
// construct a constraint on x and y // // construct a constraint on x and y
// the lagrange multipliers will be expected on L_xy // // the lagrange multipliers will be expected on L_xy
// and there is only one multiplier // // and there is only one multiplier
size_t p = 1; // size_t p = 1;
double a = 2.0; // double a = 2.0;
double b = 1.0; // double b = 1.0;
double r = 5.0; // double r = 5.0;
Key x0(0), x1(1); // Key x0(0), x1(1);
NLC2 c1(boost::bind(binding2::g, r, _1, x0, x1), // NLC2 c1(boost::bind(binding2::g, r, _1, x0, x1),
x0, boost::bind(binding2::G1, a, _1, x0), // x0, boost::bind(binding2::G1, a, _1, x0),
x1, boost::bind(binding2::G2, b, _1), // x1, boost::bind(binding2::G2, b, _1),
p, "L1"); // p, "L1");
//
// get a configuration to use for finding the error // // get a configuration to use for finding the error
VectorConfig config; // VectorConfig config;
config.insert(x0, Vector_(1, 1.0)); // config.insert(x0, Vector_(1, 1.0));
config.insert(x1, Vector_(1, 2.0)); // config.insert(x1, Vector_(1, 2.0));
//
// calculate the error // // calculate the error
Vector actual = c1.unwhitenedError(config); // Vector actual = c1.unwhitenedError(config);
Vector expected = Vector_(1.0, -6.0); // Vector expected = Vector_(1.0, -6.0);
CHECK(assert_equal(actual, expected, 1e-5)); // CHECK(assert_equal(actual, expected, 1e-5));
} //}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }