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>
NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key,
NonlinearConstraint<Config>::NonlinearConstraint(const LagrangeKey& lagrange_key,
size_t dim_lagrange,
Vector (*g)(const Config& config),
bool isEquality)
@ -28,7 +28,7 @@ NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key
/* ************************************************************************* */
template <class Config>
NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key,
NonlinearConstraint<Config>::NonlinearConstraint(const LagrangeKey& lagrange_key,
size_t dim_lagrange,
boost::function<Vector(const Config& config)> g,
bool isEquality)
@ -52,16 +52,11 @@ NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
const Key& key,
Matrix (*gradG)(const Config& config),
size_t dim_constraint,
const std::string& lagrange_key,
const LagrangeKey& lagrange_key,
bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
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,
boost::function<Matrix(const Config& config)> gradG,
size_t dim_constraint,
const std::string& lagrange_key,
const LagrangeKey& lagrange_key,
bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
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);
if (p == NULL) 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;
return this->p_ == p->p_;
}
/* ************************************************************************* */
template <class Config, class Key, class X>
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr>
NonlinearConstraint1<Config, Key, X>::linearize(const Config& config, const VectorConfig& lagrange) const {
GaussianFactor::shared_ptr
NonlinearConstraint1<Config, Key, X>::linearize(const Config& config) const {
const size_t p = this->p_;
// extract lagrange multiplier
Vector lambda = lagrange[this->lagrange_key_];
Vector lambda = config[this->lagrange_key_];
// find the error
Vector g = g_(config);
@ -120,17 +112,25 @@ NonlinearConstraint1<Config, Key, X>::linearize(const Config& config, const Vect
// construct the gradient
Matrix grad = G_(config);
// construct probabilistic factor
Matrix A1 = vector_scale(lambda, grad);
SharedDiagonal probModel = sharedSigma(this->p_,1.0);
// construct combined factor
Matrix Ax = zeros(grad.size1()*2, grad.size2());
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(key_, A1, this->lagrange_key_, eye(this->p_), zero(this->p_), probModel));
GaussianFactor(key_, Ax, this->lagrange_key_, AL, rhs, mixedConstraint));
// construct the constraint
SharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_);
GaussianFactor::shared_ptr constraint(new GaussianFactor(key_, grad, -1*g, constraintModel));
return std::make_pair(factor, constraint);
return factor;
}
/* ************************************************************************* */
@ -146,18 +146,12 @@ NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
const Key2& key2,
Matrix (*G2)(const Config& config),
size_t dim_constraint,
const std::string& lagrange_key,
const LagrangeKey& lagrange_key,
bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)),
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,
boost::function<Matrix(const Config& config)> G2,
size_t dim_constraint,
const std::string& lagrange_key,
const LagrangeKey& lagrange_key,
bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
G1_(G1), G2_(G2),
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 (!(key1_ == p->key1_)) 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;
return this->p_ == p->p_;
}
/* ************************************************************************* */
template<class Config, class Key1, class X1, class Key2, class X2>
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> NonlinearConstraint2<
Config, Key1, X1, Key2, X2>::linearize(const Config& config, const VectorConfig& lagrange) const {
GaussianFactor::shared_ptr
NonlinearConstraint2<Config, Key1, X1, Key2, X2>::linearize(const Config& config) const {
// extract lagrange multiplier
Vector lambda = lagrange[this->lagrange_key_];
Vector lambda = config[this->lagrange_key_];
// find the error
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,
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 {
/** Typedef for Lagrange key type - must be present in factors and config */
typedef TypedSymbol<Vector, 'L'> LagrangeKey;
/**
* Base class for nonlinear constraints
* This allows for both equality and inequality constraints,
@ -29,7 +32,7 @@ class NonlinearConstraint : public NonlinearFactor<Config> {
protected:
/** key for the lagrange multipliers */
std::string lagrange_key_;
LagrangeKey lagrange_key_;
/** number of lagrange multipliers */
size_t p_;
@ -53,7 +56,7 @@ public:
* @param isEquality is true if the constraint is an equality 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,
Vector (*g)(const Config& config),
bool isEquality=true);
@ -64,13 +67,13 @@ public:
* @param g is the cost function for the 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,
boost::function<Vector(const Config& config)> g,
bool isEquality=true);
/** 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 */
size_t nrConstraints() const { return p_; }
@ -95,23 +98,11 @@ public:
bool active(const Config& config) const;
/**
* Linearize using a real Config and a VectorConfig of 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)
* Real linearize, given a config that includes Lagrange multipliers
* @param config is the configuration (with lagrange multipliers)
* @return a combined linear factor containing both the constraint and the constraint factor
*/
virtual std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr>
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!");
}
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const=0;
};
@ -151,7 +142,7 @@ public:
const Key& key,
Matrix (*G)(const Config& config),
size_t dim_constraint,
const std::string& lagrange_key="",
const LagrangeKey& lagrange_key,
bool isEquality=true);
/**
@ -168,7 +159,7 @@ public:
const Key& key,
boost::function<Matrix(const Config& config)> G,
size_t dim_constraint,
const std::string& lagrange_key="",
const LagrangeKey& lagrange_key,
bool isEquality=true);
/** Print */
@ -178,14 +169,9 @@ public:
bool equals(const Factor<Config>& f, double tol=1e-9) const;
/**
* Linearize using a real Config and a VectorConfig of 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)
* Linearize from config - must have Lagrange multipliers
*/
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr>
linearize(const Config& config, const VectorConfig& lagrange) const;
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const;
};
/**
@ -228,7 +214,7 @@ public:
const Key2& key2,
Matrix (*G2)(const Config& config),
size_t dim_constraint,
const std::string& lagrange_key="",
const LagrangeKey& lagrange_key,
bool isEquality=true);
/**
@ -248,7 +234,7 @@ public:
const Key2& key2,
boost::function<Matrix(const Config& config)> G2,
size_t dim_constraint,
const std::string& lagrange_key="",
const LagrangeKey& lagrange_key,
bool isEquality=true);
/** Print */
@ -258,14 +244,9 @@ public:
bool equals(const Factor<Config>& f, double tol=1e-9) const;
/**
* Linearize using a real Config and a VectorConfig of 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)
* Linearize from config - must have Lagrange multipliers
*/
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr>
linearize(const Config& config, const VectorConfig& lagrange) const;
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const;
};
}

437
cpp/testNonlinearConstraint.cpp
View File

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