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

Fixed more tests for NonlinearConstraints, inequality constraints now work.

release/4.3a0
Alex Cunningham 2010-02-07 01:18:17 +00:00
parent 814fb949ba
commit ea3b8083b0
5 changed files with 456 additions and 456 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
cpp/Matrix.cpp
View File

@ -375,6 +375,25 @@ inline void householder_update_manual(Matrix &A, int j, double beta, const Vecto
}
void householder_update(Matrix &A, int j, double beta, const Vector& vjm) {
// TODO: SWAP IN ATLAS VERSION OF THE SYSTEM
// // straight atlas version
// const size_t m = A.size1(), n = A.size2(), mj = m-j;
//
// // find pointers to the data
// const double * vptr = vjm.data().begin(); // mj long
// double * Aptr = A.data().begin() + n*j; // mj x n - note that this starts at row j
//
// // first step: get w = beta*trans(A(j:m,:))*vjm
// Vector w(n);
// double * wptr = w.data().begin();
//
// // execute w generation
// cblas_dgemv(CblasRowMajor, CblasTrans, mj, n, beta, Aptr, n, vptr, 1, 0.0, wptr, 1);
//
// // second step: rank 1 update A(j:m,:) = v(j:m)*w' + A(j:m,:)
// cblas_dger(CblasRowMajor, mj, n, 1.0, vptr, 1, wptr, 1, Aptr, n);
#ifdef GSL
#ifndef REVERTGSL
const size_t m = A.size1(), n = A.size2();

76
cpp/NonlinearConstraint-inl.h
View File

@ -20,21 +20,9 @@ namespace gtsam {
template <class Config>
NonlinearConstraint<Config>::NonlinearConstraint(const LagrangeKey& lagrange_key,
size_t dim_lagrange,
Vector (*g)(const Config& config),
bool isEquality)
: NonlinearFactor<Config>(1.0),
lagrange_key_(lagrange_key), p_(dim_lagrange),
isEquality_(isEquality), g_(boost::bind(g, _1)) {}
/* ************************************************************************* */
template <class Config>
NonlinearConstraint<Config>::NonlinearConstraint(const LagrangeKey& lagrange_key,
size_t dim_lagrange,
boost::function<Vector(const Config& config)> g,
bool isEquality)
: NonlinearFactor<Config>(noiseModel::Constrained::All(dim_lagrange)),
lagrange_key_(lagrange_key), p_(dim_lagrange),
g_(g), isEquality_(isEquality) {}
bool isEquality) :
NonlinearFactor<Config>(noiseModel::Constrained::All(dim_lagrange)),
lagrange_key_(lagrange_key), p_(dim_lagrange), isEquality_(isEquality) {}
/* ************************************************************************* */
template <class Config>
@ -54,8 +42,8 @@ NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
size_t dim_constraint,
const LagrangeKey& lagrange_key,
bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
G_(boost::bind(gradG, _1)), key_(key)
NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
g_(boost::bind(g, _1)), G_(boost::bind(gradG, _1)), key_(key)
{
}
@ -68,22 +56,21 @@ NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
size_t dim_constraint,
const LagrangeKey& lagrange_key,
bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
G_(gradG), key_(key)
NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
g_(g), G_(gradG), key_(key)
{
}
/* ************************************************************************* */
template <class Config, class Key, class X>
void NonlinearConstraint1<Config, Key, X>::print(const std::string& s) const {
std::cout << "NonlinearConstraint1 [" << s << "]:\n";
// << " key: " << key_ << "\n"
// << " p: " << this->p_ << "\n"
// << " lambda key: " << this->lagrange_key_ << "\n";
// if (this->isEquality_)
// std::cout << " Equality Factor" << std::endl;
// else
// std::cout << " Inequality Factor" << std::endl;
std::cout << "NonlinearConstraint1 [" << s << "]: Dim: " << this->p_ << "\n"
<< " Key : " << (std::string) this->key_ << "\n"
<< " Lagrange Key: " << (std::string) this->lagrange_key_ << "\n";
if (this->isEquality_)
std::cout << " Equality Factor" << std::endl;
else
std::cout << " Inequality Factor" << std::endl;
}
/* ************************************************************************* */
@ -148,8 +135,8 @@ NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
size_t dim_constraint,
const LagrangeKey& lagrange_key,
bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)),
NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
g_(boost::bind(g, _1)), G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)),
key1_(key1), key2_(key2)
{
}
@ -165,8 +152,8 @@ NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
size_t dim_constraint,
const LagrangeKey& lagrange_key,
bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
G1_(G1), G2_(G2),
NonlinearConstraint<Config>(lagrange_key, dim_constraint, isEquality),
g_(g), G1_(G1), G2_(G2),
key1_(key1), key2_(key2)
{
}
@ -174,11 +161,14 @@ NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
/* ************************************************************************* */
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 << "]:\n";
// << " key1: " << key1_ << "\n"
// << " key2: " << key2_ << "\n"
// << " p: " << this->p_ << "\n"
// << " lambda key: " << this->lagrange_key_ << std::endl;
std::cout << "NonlinearConstraint2 [" << s << "]: Dim: " << this->p_ << "\n"
<< " Key1 : " << (std::string) this->key1_ << "\n"
<< " Key2 : " << (std::string) this->key2_ << "\n"
<< " Lagrange Key: " << (std::string) this->lagrange_key_ << "\n";
if (this->isEquality_)
std::cout << " Equality Factor" << std::endl;
else
std::cout << " Inequality Factor" << std::endl;
}
/* ************************************************************************* */
@ -198,7 +188,6 @@ 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 {
const size_t p = this->p_;
// extract lagrange multiplier
Vector lambda = config[this->lagrange_key_];
@ -231,19 +220,6 @@ NonlinearConstraint2<Config, Key1, X1, Key2, X2>::linearize(const Config& config
GaussianFactor::shared_ptr factor(new
GaussianFactor(key1_, Ax1, key2_, Ax2, this->lagrange_key_, AL, rhs, mixedConstraint));
// // construct probabilistic factor
// Matrix A1 = vector_scale(lambda, grad1);
// Matrix A2 = vector_scale(lambda, grad2);
// SharedDiagonal probModel = sharedSigma(this->p_,1.0);
// GaussianFactor::shared_ptr factor(new GaussianFactor(key1_, A1, key2_, A2,
// this->lagrange_key_, eye(this->p_), zero(this->p_), probModel));
//
// // construct the constraint
// SharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_);
// GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1,
// key2_, grad2, -1.0 * g, constraintModel));
return factor;
}

47
cpp/NonlinearConstraint.h
View File

@ -40,36 +40,15 @@ protected:
/** type of constraint */
bool isEquality_;
/** 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_;
public:
/** Constructor - sets the cost function and the lagrange multipliers
* @param lagrange_key is the label for the associated lagrange multipliers
* @param dim_lagrange is the number of associated constraints
* @param isEquality is true if the constraint is an equality constraint
* @param g is the cost function for the constraint
*/
NonlinearConstraint(const LagrangeKey& lagrange_key,
size_t dim_lagrange,
Vector (*g)(const Config& config),
bool isEquality=true);
/** Constructor - sets a more general cost function using boost::bind directly
* @param lagrange_key is the label for the associated lagrange multipliers
* @param dim_lagrange is the number of associated constraints
* @param g is the cost function for the constraint
* @param isEquality is true if the constraint is an equality constraint
*/
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 */
@ -88,7 +67,7 @@ public:
virtual bool equals(const Factor<Config>& f, double tol=1e-9) const=0;
/** error function - returns the result of the constraint function */
inline Vector unwhitenedError(const Config& c) const { return g_(c); }
virtual Vector unwhitenedError(const Config& c) const=0;
/**
* Determines whether the constraint is active given a particular configuration
@ -108,6 +87,8 @@ 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
*/
template <class Config, class Key, class X>
class NonlinearConstraint1 : public NonlinearConstraint<Config> {
@ -123,6 +104,14 @@ private:
*/
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_;
/** key for the constrained variable */
Key key_;
@ -168,6 +157,9 @@ public:
/** 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); }
/**
* Linearize from config - must have Lagrange multipliers
*/
@ -192,6 +184,14 @@ private:
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_;
/** keys for the constrained variables */
Key1 key1_;
Key2 key2_;
@ -243,6 +243,9 @@ public:
/** 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); }
/**
* Linearize from config - must have Lagrange multipliers
*/

388
cpp/testNonlinearConstraint.cpp
View File

@ -14,14 +14,17 @@
#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 NonlinearConstraint1<VectorConfig, Key, Vector> NLC1;
typedef NonlinearConstraint2<VectorConfig, Key, Vector, Key, Vector> NLC2;
typedef TupleConfig2< LieConfig<LagrangeKey, Vector>,
LieConfig<Key, Vector> > VecConfig;
typedef NonlinearConstraint1<VecConfig, Key, Vector> NLC1;
typedef NonlinearConstraint2<VecConfig, Key, Vector, Key, Vector> NLC2;
/* ************************************************************************* */
// unary functions with scalar variables
@ -29,13 +32,13 @@ typedef NonlinearConstraint2<VectorConfig, Key, Vector, Key, Vector> NLC2;
namespace test1 {
/** p = 1, g(x) = x^2-5 = 0 */
Vector g(const VectorConfig& config, const list<Symbol>& keys) {
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 VectorConfig& config, const list<Symbol>& keys) {
Matrix G(const VecConfig& config, const list<Key>& keys) {
double x = config[keys.front()](0);
return Matrix_(1, 1, 2 * x);
}
@ -48,16 +51,16 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
// the lagrange multipliers will be expected on L_x1
// and there is only one multiplier
size_t p = 1;
list<Symbol> keys; keys += "x1";
Key x1(1);
list<Key> keys; keys += x1;
LagrangeKey L1(1);
NLC1 c1(boost::bind(test1::g, _1, keys),
x1, boost::bind(test1::G, _1, keys),
p, L1);
// get a configuration to use for finding the error
VectorConfig config;
config.insert("x1", Vector_(1, 1.0));
VecConfig config;
config.insert(x1, Vector_(1, 1.0));
// calculate the error
Vector actual = c1.unwhitenedError(config);
@ -68,15 +71,15 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
/* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_linearize ) {
size_t p = 1;
list<Symbol> keys; keys += "x1";
Key x1(1);
list<Key> keys; keys += x1;
LagrangeKey L1(1);
NLC1 c1(boost::bind(test1::g, _1, keys),
x1, boost::bind(test1::G, _1, keys),
p, L1);
// get a configuration to use for linearization (with lagrange multipliers)
VectorConfig realconfig;
VecConfig realconfig;
realconfig.insert(x1, Vector_(1, 1.0));
realconfig.insert(L1, Vector_(1, 3.0));
@ -97,8 +100,8 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
/* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_equal ) {
list<Symbol> keys1, keys2; keys1 += "x0"; keys2 += "x1";
Key x(0), y(1);
list<Key> keys1, keys2; keys1 += x; keys2 += y;
LagrangeKey L1(1);
NLC1
c1(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, L1, true),
@ -118,20 +121,20 @@ TEST( NonlinearConstraint1, unary_scalar_equal ) {
namespace test2 {
/** p = 1, g(x) = x^2-5 -y = 0 */
Vector g(const VectorConfig& config, const list<Symbol>& keys) {
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 VectorConfig& config, const list<Symbol>& keys) {
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 VectorConfig& config, const list<Symbol>& keys) {
Matrix G2(const VecConfig& config, const list<Key>& keys) {
double x = config[keys.back()](0);
return Matrix_(1, 1, -1.0);
}
@ -144,8 +147,8 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
// the lagrange multipliers will be expected on L_xy
// and there is only one multiplier
size_t p = 1;
list<Symbol> keys; keys += "x0", "x1";
Key x0(0), x1(1);
list<Key> keys; keys += x0, x1;
LagrangeKey L1(1);
NLC2 c1(
boost::bind(test2::g, _1, keys),
@ -154,9 +157,9 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
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));
VecConfig config;
config.insert(x0, Vector_(1, 1.0));
config.insert(x1, Vector_(1, 2.0));
// calculate the error
Vector actual = c1.unwhitenedError(config);
@ -168,8 +171,8 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
TEST( NonlinearConstraint2, binary_scalar_linearize ) {
// create a constraint
size_t p = 1;
list<Symbol> keys; keys += "x0", "x1";
Key x0(0), x1(1);
list<Key> keys; keys += x0, x1;
LagrangeKey L1(1);
NLC2 c1(
boost::bind(test2::g, _1, keys),
@ -178,7 +181,7 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
p, L1);
// get a configuration to use for finding the error
VectorConfig realconfig;
VecConfig realconfig;
realconfig.insert(x0, Vector_(1, 1.0));
realconfig.insert(x1, Vector_(1, 2.0));
realconfig.insert(L1, Vector_(1, 3.0));
@ -199,9 +202,9 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
/* ************************************************************************* */
TEST( NonlinearConstraint2, binary_scalar_equal ) {
list<Symbol> keys1, keys2, keys3;
keys1 += "x0", "x1"; keys2 += "x1", "x0"; keys3 += "x0", "z";
list<Key> keys1, keys2, keys3;
Key x0(0), x1(1), x2(2);
keys1 += x0, x1; keys2 += x1, x0; keys3 += x0;
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, L1),
@ -215,178 +218,177 @@ 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 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);
LagrangeKey L1(1);
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
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);
LagrangeKey L1(1);
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
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
config1.insert(L1, Vector_(1, 3.0));
config2.insert(L1, Vector_(1, 3.0));
// 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); LagrangeKey L1(1);
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
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
// 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); LagrangeKey L1(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
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); }

382
cpp/testSQP.cpp
View File

@ -755,119 +755,119 @@ TEST (SQP, stereo_sqp ) {
// create optimizer
VOptimizer optimizer(graph, truthConfig, solver);
// optimize
VOptimizer afterOneIteration = optimizer.iterate();
// check if correct
CHECK(assert_equal(*truthConfig,*(afterOneIteration.config())));
// // optimize
// VOptimizer afterOneIteration = optimizer.iterate();
//
// // check if correct
// CHECK(assert_equal(*truthConfig,*(afterOneIteration.config())));
}
/* *********************************************************************
* SQP version of the above stereo example,
* with noise in the initial estimate
*/
TEST (SQP, stereo_sqp_noisy ) {
bool verbose = false;
// get a graph
boost::shared_ptr<VGraph> graph = stereoExampleGraph();
// create initial data
Rot3 faceDownY(Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 0.0, 1.0,
0.0, 1.0, 0.0));
Pose3 pose1(faceDownY, Point3()); // origin, left camera
Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left
Point3 landmark1(0.5, 5.0, 0.0); //centered between the cameras, 5 units away
Point3 landmark2(1.5, 5.0, 0.0);
// noisy config
boost::shared_ptr<VConfig> initConfig(new VConfig);
initConfig->insert(Pose3Key(1), pose1);
initConfig->insert(Pose3Key(2), pose2);
initConfig->insert(Point3Key(1), landmark1);
initConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place
initConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0));
// create ordering
shared_ptr<Ordering> ord(new Ordering());
*ord += "x1", "x2", "l1", "l2", "L12";
VOptimizer::shared_solver solver(new VOptimizer::solver(ord));
// create optimizer
VOptimizer optimizer(graph, initConfig, solver);
// optimize
VOptimizer *pointer = new VOptimizer(optimizer);
for (int i=0;i<1;i++) {
VOptimizer* newOptimizer = new VOptimizer(pointer->iterateLM());
delete pointer;
pointer = newOptimizer;
}
VOptimizer::shared_config actual = pointer->config();
delete(pointer);
// get the truth config
boost::shared_ptr<VConfig> truthConfig = stereoExampleTruthConfig();
truthConfig->insert(LagrangeKey(12), Vector_(3, 0.0, 1.0, 1.0));
// check if correct
CHECK(assert_equal(*truthConfig,*actual, 1e-5));
}
static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1));
// typedefs
//typedef simulated2D::Config Config2D;
//typedef boost::shared_ptr<Config2D> shared_config;
//typedef NonlinearFactorGraph<Config2D> NLGraph;
//typedef boost::shared_ptr<NonlinearFactor<Config2D> > shared;
namespace map_warp_example {
typedef NonlinearConstraint1<
Config2D, simulated2D::PoseKey, Point2> NLC1;
//typedef NonlinearConstraint2<
// Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2;
} // \namespace map_warp_example
/* ********************************************************************* */
// Example that moves two separate maps into the same frame of reference
// Note that this is a linear example, so it should converge in one step
/* ********************************************************************* */
namespace sqp_LinearMapWarp2 {
// binary constraint between landmarks
/** g(x) = x-y = 0 */
Vector g_func(const Config2D& config, const simulated2D::PointKey& key1, const simulated2D::PointKey& key2) {
Point2 p = config[key1]-config[key2];
return Vector_(2, p.x(), p.y());
}
/** jacobian at l1 */
Matrix jac_g1(const Config2D& config) {
return eye(2);
}
/** jacobian at l2 */
Matrix jac_g2(const Config2D& config) {
return -1*eye(2);
}
} // \namespace sqp_LinearMapWarp2
namespace sqp_LinearMapWarp1 {
// Unary Constraint on x1
/** g(x) = x -[1;1] = 0 */
Vector g_func(const Config2D& config, const simulated2D::PoseKey& key) {
Point2 p = config[key]-Point2(1.0, 1.0);
return Vector_(2, p.x(), p.y());
}
/** jacobian at x1 */
Matrix jac_g(const Config2D& config) {
return eye(2);
}
} // \namespace sqp_LinearMapWarp12
///* *********************************************************************
// * SQP version of the above stereo example,
// * with noise in the initial estimate
// */
//TEST (SQP, stereo_sqp_noisy ) {
// bool verbose = false;
//
// // get a graph
// boost::shared_ptr<VGraph> graph = stereoExampleGraph();
//
// // create initial data
// Rot3 faceDownY(Matrix_(3,3,
// 1.0, 0.0, 0.0,
// 0.0, 0.0, 1.0,
// 0.0, 1.0, 0.0));
// Pose3 pose1(faceDownY, Point3()); // origin, left camera
// Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left
// Point3 landmark1(0.5, 5.0, 0.0); //centered between the cameras, 5 units away
// Point3 landmark2(1.5, 5.0, 0.0);
//
// // noisy config
// boost::shared_ptr<VConfig> initConfig(new VConfig);
// initConfig->insert(Pose3Key(1), pose1);
// initConfig->insert(Pose3Key(2), pose2);
// initConfig->insert(Point3Key(1), landmark1);
// initConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place
// initConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0));
//
// // create ordering
// shared_ptr<Ordering> ord(new Ordering());
// *ord += "x1", "x2", "l1", "l2", "L12";
// VOptimizer::shared_solver solver(new VOptimizer::solver(ord));
//
// // create optimizer
// VOptimizer optimizer(graph, initConfig, solver);
//
// // optimize
// VOptimizer *pointer = new VOptimizer(optimizer);
// for (int i=0;i<1;i++) {
// VOptimizer* newOptimizer = new VOptimizer(pointer->iterateLM());
// delete pointer;
// pointer = newOptimizer;
// }
// VOptimizer::shared_config actual = pointer->config();
// delete(pointer);
//
// // get the truth config
// boost::shared_ptr<VConfig> truthConfig = stereoExampleTruthConfig();
// truthConfig->insert(LagrangeKey(12), Vector_(3, 0.0, 1.0, 1.0));
//
// // check if correct
// CHECK(assert_equal(*truthConfig,*actual, 1e-5));
//}
//
//static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1));
//
//// typedefs
////typedef simulated2D::Config Config2D;
////typedef boost::shared_ptr<Config2D> shared_config;
////typedef NonlinearFactorGraph<Config2D> NLGraph;
////typedef boost::shared_ptr<NonlinearFactor<Config2D> > shared;
//
//namespace map_warp_example {
//typedef NonlinearConstraint1<
// Config2D, simulated2D::PoseKey, Point2> NLC1;
////typedef NonlinearConstraint2<
//// Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2;
//} // \namespace map_warp_example
//
///* ********************************************************************* */
//// Example that moves two separate maps into the same frame of reference
//// Note that this is a linear example, so it should converge in one step
///* ********************************************************************* */
//
//namespace sqp_LinearMapWarp2 {
//// binary constraint between landmarks
///** g(x) = x-y = 0 */
//Vector g_func(const Config2D& config, const simulated2D::PointKey& key1, const simulated2D::PointKey& key2) {
// Point2 p = config[key1]-config[key2];
// return Vector_(2, p.x(), p.y());
//}
//
///** jacobian at l1 */
//Matrix jac_g1(const Config2D& config) {
// return eye(2);
//}
//
///** jacobian at l2 */
//Matrix jac_g2(const Config2D& config) {
// return -1*eye(2);
//}
//} // \namespace sqp_LinearMapWarp2
//
//namespace sqp_LinearMapWarp1 {
//// Unary Constraint on x1
///** g(x) = x -[1;1] = 0 */
//Vector g_func(const Config2D& config, const simulated2D::PoseKey& key) {
// Point2 p = config[key]-Point2(1.0, 1.0);
// return Vector_(2, p.x(), p.y());
//}
//
///** jacobian at x1 */
//Matrix jac_g(const Config2D& config) {
// return eye(2);
//}
//} // \namespace sqp_LinearMapWarp12
//typedef NonlinearOptimizer<NLGraph, Config2D> Optimizer;
@ -875,86 +875,86 @@ Matrix jac_g(const Config2D& config) {
* Creates the graph with each robot seeing the landmark, and it is
* known that it is the same landmark
*/
boost::shared_ptr<Graph2D> linearMapWarpGraph() {
using namespace map_warp_example;
// keys
simulated2D::PoseKey x1(1), x2(2);
simulated2D::PointKey l1(1), l2(2);
// constant constraint on x1
LagrangeKey L1(1);
shared_ptr<NLC1> c1(new NLC1(boost::bind(sqp_LinearMapWarp1::g_func, _1, x1),
x1, boost::bind(sqp_LinearMapWarp1::jac_g, _1),
2, L1));
// measurement from x1 to l1
Point2 z1(0.0, 5.0);
shared f1(new simulated2D::GenericMeasurement<Config2D>(z1, sigma, x1,l1));
// measurement from x2 to l2
Point2 z2(-4.0, 0.0);
shared f2(new simulated2D::GenericMeasurement<Config2D>(z2, sigma, x2,l2));
// equality constraint between l1 and l2
LagrangeKey L12(12);
shared_ptr<NLC2> c2 (new NLC2(
boost::bind(sqp_LinearMapWarp2::g_func, _1, l1, l2),
l1, boost::bind(sqp_LinearMapWarp2::jac_g1, _1),
l2, boost::bind(sqp_LinearMapWarp2::jac_g2, _1),
2, L12));
// construct the graph
boost::shared_ptr<Graph2D> graph(new Graph2D());
graph->push_back(c1);
graph->push_back(c2);
graph->push_back(f1);
graph->push_back(f2);
return graph;
}
/* ********************************************************************* */
TEST ( SQPOptimizer, map_warp_initLam ) {
bool verbose = false;
// get a graph
boost::shared_ptr<Graph2D> graph = linearMapWarpGraph();
// keys
simulated2D::PoseKey x1(1), x2(2);
simulated2D::PointKey l1(1), l2(2);
LagrangeKey L1(1), L12(12);
// create an initial estimate
shared_ptr<Config2D> initialEstimate(new Config2D);
initialEstimate->insert(x1, Point2(1.0, 1.0));
initialEstimate->insert(l1, Point2(1.0, 6.0));
initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
initialEstimate->insert(x2, Point2(0.0, 0.0)); // other pose starts at origin
initialEstimate->insert(L12, Vector_(2, 1.0, 1.0));
initialEstimate->insert(L1, Vector_(2, 1.0, 1.0));
// create an ordering
shared_ptr<Ordering> ordering(new Ordering());
*ordering += "x1", "x2", "l1", "l2", "L12", "L1";
// create an optimizer
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
Optimizer optimizer(graph, initialEstimate, solver);
// perform an iteration of optimization
Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT);
// get the config back out and verify
Config2D actual = *(oneIteration.config());
Config2D expected;
expected.insert(x1, Point2(1.0, 1.0));
expected.insert(l1, Point2(1.0, 6.0));
expected.insert(l2, Point2(1.0, 6.0));
expected.insert(x2, Point2(5.0, 6.0));
expected.insert(L1, Vector_(2, 1.0, 1.0));
expected.insert(L12, Vector_(2, 6.0, 7.0));
CHECK(assert_equal(expected, actual));
}
//boost::shared_ptr<Graph2D> linearMapWarpGraph() {
// using namespace map_warp_example;
// // keys
// simulated2D::PoseKey x1(1), x2(2);
// simulated2D::PointKey l1(1), l2(2);
//
// // constant constraint on x1
// LagrangeKey L1(1);
// shared_ptr<NLC1> c1(new NLC1(boost::bind(sqp_LinearMapWarp1::g_func, _1, x1),
// x1, boost::bind(sqp_LinearMapWarp1::jac_g, _1),
// 2, L1));
//
// // measurement from x1 to l1
// Point2 z1(0.0, 5.0);
// shared f1(new simulated2D::GenericMeasurement<Config2D>(z1, sigma, x1,l1));
//
// // measurement from x2 to l2
// Point2 z2(-4.0, 0.0);
// shared f2(new simulated2D::GenericMeasurement<Config2D>(z2, sigma, x2,l2));
//
// // equality constraint between l1 and l2
// LagrangeKey L12(12);
// shared_ptr<NLC2> c2 (new NLC2(
// boost::bind(sqp_LinearMapWarp2::g_func, _1, l1, l2),
// l1, boost::bind(sqp_LinearMapWarp2::jac_g1, _1),
// l2, boost::bind(sqp_LinearMapWarp2::jac_g2, _1),
// 2, L12));
//
// // construct the graph
// boost::shared_ptr<Graph2D> graph(new Graph2D());
// graph->push_back(c1);
// graph->push_back(c2);
// graph->push_back(f1);
// graph->push_back(f2);
//
// return graph;
//}
//
///* ********************************************************************* */
//TEST ( SQPOptimizer, map_warp_initLam ) {
// bool verbose = false;
// // get a graph
// boost::shared_ptr<Graph2D> graph = linearMapWarpGraph();
//
// // keys
// simulated2D::PoseKey x1(1), x2(2);
// simulated2D::PointKey l1(1), l2(2);
// LagrangeKey L1(1), L12(12);
//
// // create an initial estimate
// shared_ptr<Config2D> initialEstimate(new Config2D);
// initialEstimate->insert(x1, Point2(1.0, 1.0));
// initialEstimate->insert(l1, Point2(1.0, 6.0));
// initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
// initialEstimate->insert(x2, Point2(0.0, 0.0)); // other pose starts at origin
// initialEstimate->insert(L12, Vector_(2, 1.0, 1.0));
// initialEstimate->insert(L1, Vector_(2, 1.0, 1.0));
//
// // create an ordering
// shared_ptr<Ordering> ordering(new Ordering());
// *ordering += "x1", "x2", "l1", "l2", "L12", "L1";
//
// // create an optimizer
// Optimizer::shared_solver solver(new Optimizer::solver(ordering));
// Optimizer optimizer(graph, initialEstimate, solver);
//
// // perform an iteration of optimization
// Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT);
//
// // get the config back out and verify
// Config2D actual = *(oneIteration.config());
// Config2D expected;
// expected.insert(x1, Point2(1.0, 1.0));
// expected.insert(l1, Point2(1.0, 6.0));
// expected.insert(l2, Point2(1.0, 6.0));
// expected.insert(x2, Point2(5.0, 6.0));
// expected.insert(L1, Vector_(2, 1.0, 1.0));
// expected.insert(L12, Vector_(2, 6.0, 7.0));
// CHECK(assert_equal(expected, actual));
//}
///* ********************************************************************* */
//// This is an obstacle avoidance demo, where there is a trajectory of