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

Changed NonlinearConstraints to take cost and gradient functions that do not take the list of keys. Tests have been reconstructed using boost::bind

release/4.3a0
Alex Cunningham 2009-12-18 02:39:02 +00:00
parent b02582b43d
commit a1918056a5
5 changed files with 110 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
cpp/NonlinearConstraint-inl.h
View File

@ -20,17 +20,17 @@ namespace gtsam {
template <class Config> template <class Config>
NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key, NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key,
size_t dim_lagrange, size_t dim_lagrange,
Vector (*g)(const Config& config, const std::list<std::string>& keys), Vector (*g)(const Config& config),
bool isEquality) bool isEquality)
: NonlinearFactor<Config>(zero(dim_lagrange), 1.0), : NonlinearFactor<Config>(zero(dim_lagrange), 1.0),
lagrange_key_(lagrange_key), p_(dim_lagrange), lagrange_key_(lagrange_key), p_(dim_lagrange),
isEquality_(isEquality), g_(boost::bind(g, _1, _2)) {} isEquality_(isEquality), g_(boost::bind(g, _1)) {}
/* ************************************************************************* */ /* ************************************************************************* */
template <class Config> template <class Config>
NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key, NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key,
size_t dim_lagrange, size_t dim_lagrange,
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g, boost::function<Vector(const Config& config)> g,
bool isEquality) bool isEquality)
: NonlinearFactor<Config>(zero(dim_lagrange), 1.0), : NonlinearFactor<Config>(zero(dim_lagrange), 1.0),
lagrange_key_(lagrange_key), p_(dim_lagrange), lagrange_key_(lagrange_key), p_(dim_lagrange),
@ -48,14 +48,14 @@ bool NonlinearConstraint<Config>::active(const Config& config) const {
template <class Config> template <class Config>
NonlinearConstraint1<Config>::NonlinearConstraint1( NonlinearConstraint1<Config>::NonlinearConstraint1(
Vector (*g)(const Config& config, const std::list<std::string>& keys), Vector (*g)(const Config& config),
const std::string& key, const std::string& key,
Matrix (*gradG)(const Config& config, const std::list<std::string>& keys), Matrix (*gradG)(const Config& config),
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key, const std::string& lagrange_key,
bool isEquality) : bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
gradG_(boost::bind(gradG, _1, _2)), key_(key) gradG_(boost::bind(gradG, _1)), key_(key)
{ {
// set a good lagrange key here // set a good lagrange key here
// TODO:should do something smart to find a unique one // TODO:should do something smart to find a unique one
@ -67,9 +67,9 @@ NonlinearConstraint1<Config>::NonlinearConstraint1(
/* ************************************************************************* */ /* ************************************************************************* */
template <class Config> template <class Config>
NonlinearConstraint1<Config>::NonlinearConstraint1( NonlinearConstraint1<Config>::NonlinearConstraint1(
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g, boost::function<Vector(const Config& config)> g,
const std::string& key, const std::string& key,
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG, boost::function<Matrix(const Config& config)> gradG,
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key, const std::string& lagrange_key,
bool isEquality) : bool isEquality) :
@ -115,10 +115,10 @@ NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig
Vector lambda = lagrange[this->lagrange_key_]; Vector lambda = lagrange[this->lagrange_key_];
// find the error // find the error
Vector g = g_(config, this->keys()); Vector g = g_(config);
// construct the gradient // construct the gradient
Matrix grad = gradG_(config, this->keys()); Matrix grad = gradG_(config);
// construct probabilistic factor // construct probabilistic factor
Matrix A1 = vector_scale(lambda, grad); Matrix A1 = vector_scale(lambda, grad);
@ -138,16 +138,16 @@ NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig
/* ************************************************************************* */ /* ************************************************************************* */
template <class Config> template <class Config>
NonlinearConstraint2<Config>::NonlinearConstraint2( NonlinearConstraint2<Config>::NonlinearConstraint2(
Vector (*g)(const Config& config, const std::list<std::string>& keys), Vector (*g)(const Config& config),
const std::string& key1, const std::string& key1,
Matrix (*gradG1)(const Config& config, const std::list<std::string>& keys), Matrix (*gradG1)(const Config& config),
const std::string& key2, const std::string& key2,
Matrix (*gradG2)(const Config& config, const std::list<std::string>& keys), Matrix (*gradG2)(const Config& config),
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key, const std::string& lagrange_key,
bool isEquality) : bool isEquality) :
NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality), NonlinearConstraint<Config>(lagrange_key, dim_constraint, g, isEquality),
gradG1_(boost::bind(gradG1, _1, _2)), gradG2_(boost::bind(gradG2, _1, _2)), gradG1_(boost::bind(gradG1, _1)), gradG2_(boost::bind(gradG2, _1)),
key1_(key1), key2_(key2) key1_(key1), key2_(key2)
{ {
// set a good lagrange key here // set a good lagrange key here
@ -161,11 +161,11 @@ NonlinearConstraint2<Config>::NonlinearConstraint2(
/* ************************************************************************* */ /* ************************************************************************* */
template <class Config> template <class Config>
NonlinearConstraint2<Config>::NonlinearConstraint2( NonlinearConstraint2<Config>::NonlinearConstraint2(
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g, boost::function<Vector(const Config& config)> g,
const std::string& key1, const std::string& key1,
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG1, boost::function<Matrix(const Config& config)> gradG1,
const std::string& key2, const std::string& key2,
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG2, boost::function<Matrix(const Config& config)> gradG2,
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key, const std::string& lagrange_key,
bool isEquality) : bool isEquality) :
@ -211,11 +211,11 @@ std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> NonlinearConst
Vector lambda = lagrange[this->lagrange_key_]; Vector lambda = lagrange[this->lagrange_key_];
// find the error // find the error
Vector g = g_(config, this->keys()); Vector g = g_(config);
// construct the gradients // construct the gradients
Matrix grad1 = gradG1_(config, this->keys()); Matrix grad1 = gradG1_(config);
Matrix grad2 = gradG2_(config, this->keys()); Matrix grad2 = gradG2_(config);
// construct probabilistic factor // construct probabilistic factor
Matrix A1 = vector_scale(lambda, grad1); Matrix A1 = vector_scale(lambda, grad1);

37
cpp/NonlinearConstraint.h
View File

@ -40,10 +40,9 @@ protected:
* If the value is zero, the constraint is not active * If the value is zero, the constraint is not active
* Use boost.bind to create the function object * Use boost.bind to create the function object
* @param config is a configuration of all the variables * @param config is a configuration of all the variables
* @param keys is the set of keys - assumed that the function knows how to use
* @return the cost for each of p constraints, arranged in a vector * @return the cost for each of p constraints, arranged in a vector
*/ */
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g_; boost::function<Vector(const Config& config)> g_;
public: public:
@ -55,7 +54,7 @@ public:
*/ */
NonlinearConstraint(const std::string& lagrange_key, NonlinearConstraint(const std::string& lagrange_key,
size_t dim_lagrange, size_t dim_lagrange,
Vector (*g)(const Config& config, const std::list<std::string>& keys), Vector (*g)(const Config& config),
bool isEquality=true); bool isEquality=true);
/** Constructor - sets a more general cost function using boost::bind directly /** Constructor - sets a more general cost function using boost::bind directly
@ -66,7 +65,7 @@ public:
*/ */
NonlinearConstraint(const std::string& lagrange_key, NonlinearConstraint(const std::string& lagrange_key,
size_t dim_lagrange, size_t dim_lagrange,
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> 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 */
@ -85,7 +84,7 @@ public:
virtual bool equals(const Factor<Config>& f, double tol=1e-9) const=0; virtual bool equals(const Factor<Config>& f, double tol=1e-9) const=0;
/** error function - returns the result of the constraint function */ /** error function - returns the result of the constraint function */
inline Vector error_vector(const Config& c) const { return g_(c, this->keys()); } inline Vector error_vector(const Config& c) const { return g_(c); }
/** /**
* Determines whether the constraint is active given a particular configuration * Determines whether the constraint is active given a particular configuration
@ -128,10 +127,9 @@ private:
* returns a pxn matrix * returns a pxn matrix
* Use boost.bind to create the function object * Use boost.bind to create the function object
* @param config to use for linearization * @param config to use for linearization
* @param key of selected variable
* @return the jacobian of the constraint in terms of key * @return the jacobian of the constraint in terms of key
*/ */
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG_; boost::function<Matrix(const Config& config)> gradG_;
/** key for the constrained variable */ /** key for the constrained variable */
std::string key_; std::string key_;
@ -148,9 +146,9 @@ public:
* @param isEquality is true if the constraint is an equality constraint * @param isEquality is true if the constraint is an equality constraint
*/ */
NonlinearConstraint1( NonlinearConstraint1(
Vector (*g)(const Config& config, const std::list<std::string>& keys), Vector (*g)(const Config& config),
const std::string& key, const std::string& key,
Matrix (*gradG)(const Config& config, const std::list<std::string>& keys), Matrix (*gradG)(const Config& config),
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key="", const std::string& lagrange_key="",
bool isEquality=true); bool isEquality=true);
@ -165,9 +163,9 @@ public:
* @param isEquality is true if the constraint is an equality constraint * @param isEquality is true if the constraint is an equality constraint
*/ */
NonlinearConstraint1( NonlinearConstraint1(
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g, boost::function<Vector(const Config& config)> g,
const std::string& key, const std::string& key,
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG, boost::function<Matrix(const Config& config)> gradG,
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key="", const std::string& lagrange_key="",
bool isEquality=true); bool isEquality=true);
@ -202,11 +200,10 @@ private:
* the first and second variables * the first and second variables
* returns a pxn matrix * returns a pxn matrix
* @param config to use for linearization * @param config to use for linearization
* @param key of selected variable
* @return the jacobian of the constraint in terms of key * @return the jacobian of the constraint in terms of key
*/ */
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG1_; boost::function<Matrix(const Config& config)> gradG1_;
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG2_; boost::function<Matrix(const Config& config)> gradG2_;
/** keys for the constrained variables */ /** keys for the constrained variables */
std::string key1_; std::string key1_;
@ -224,11 +221,11 @@ public:
* @param isEquality is true if the constraint is an equality constraint * @param isEquality is true if the constraint is an equality constraint
*/ */
NonlinearConstraint2( NonlinearConstraint2(
Vector (*g)(const Config& config, const std::list<std::string>& keys), Vector (*g)(const Config& config),
const std::string& key1, const std::string& key1,
Matrix (*gradG1)(const Config& config, const std::list<std::string>& keys), Matrix (*gradG1)(const Config& config),
const std::string& key2, const std::string& key2,
Matrix (*gradG2)(const Config& config, const std::list<std::string>& keys), Matrix (*gradG2)(const Config& config),
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key="", const std::string& lagrange_key="",
bool isEquality=true); bool isEquality=true);
@ -244,11 +241,11 @@ public:
* @param isEquality is true if the constraint is an equality constraint * @param isEquality is true if the constraint is an equality constraint
*/ */
NonlinearConstraint2( NonlinearConstraint2(
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g, boost::function<Vector(const Config& config)> g,
const std::string& key1, const std::string& key1,
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG1, boost::function<Matrix(const Config& config)> gradG1,
const std::string& key2, const std::string& key2,
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG2, boost::function<Matrix(const Config& config)> gradG2,
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key="", const std::string& lagrange_key="",
bool isEquality=true); bool isEquality=true);

76
cpp/testNonlinearConstraint.cpp
View File

@ -4,14 +4,17 @@
* @author Alex Cunningham * @author Alex Cunningham
*/ */
#include <list>
#include <boost/bind.hpp> #include <boost/bind.hpp>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/list.hpp> // for operator +=
#include <VectorConfig.h> #include <VectorConfig.h>
#include <NonlinearConstraint.h> #include <NonlinearConstraint.h>
#include <NonlinearConstraint-inl.h> #include <NonlinearConstraint-inl.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace boost::assign;
/* ************************************************************************* */ /* ************************************************************************* */
// unary functions with scalar variables // unary functions with scalar variables
@ -38,7 +41,10 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
// the lagrange multipliers will be expected on L_x1 // the lagrange multipliers will be expected on L_x1
// and there is only one multiplier // and there is only one multiplier
size_t p = 1; size_t p = 1;
NonlinearConstraint1<VectorConfig> c1(*test1::g, "x", *test1::G, p, "L_x1"); list<string> keys; keys += "x";
NonlinearConstraint1<VectorConfig> c1(boost::bind(test1::g, _1, keys),
"x", boost::bind(test1::G, _1, keys),
p, "L_x1");
// get a configuration to use for finding the error // get a configuration to use for finding the error
VectorConfig config; VectorConfig config;
@ -53,7 +59,10 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_linearize ) { TEST( NonlinearConstraint1, unary_scalar_linearize ) {
size_t p = 1; size_t p = 1;
NonlinearConstraint1<VectorConfig> c1(*test1::g, "x", *test1::G, p, "L_x1"); list<string> keys; keys += "x";
NonlinearConstraint1<VectorConfig> c1(boost::bind(test1::g, _1, keys),
"x", boost::bind(test1::G, _1, keys),
p, "L_x1");
// get a configuration to use for linearization // get a configuration to use for linearization
VectorConfig realconfig; VectorConfig realconfig;
@ -76,11 +85,12 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_equal ) { TEST( NonlinearConstraint1, unary_scalar_equal ) {
list<string> keys1, keys2; keys1 += "x"; keys2 += "y";
NonlinearConstraint1<VectorConfig> NonlinearConstraint1<VectorConfig>
c1(*test1::g, "x", *test1::G, 1, "L_x1", true), c1(boost::bind(test1::g, _1, keys1), "x", boost::bind(test1::G, _1, keys1), 1, "L_x1", true),
c2(*test1::g, "x", *test1::G, 1, "L_x1"), c2(boost::bind(test1::g, _1, keys1), "x", boost::bind(test1::G, _1, keys1), 1, "L_x1"),
c3(*test1::g, "x", *test1::G, 2, "L_x1"), c3(boost::bind(test1::g, _1, keys1), "x", boost::bind(test1::G, _1, keys1), 2, "L_x1"),
c4(*test1::g, "y", *test1::G, 1, "L_x1"); c4(boost::bind(test1::g, _1, keys2), "y", boost::bind(test1::G, _1, keys2), 1, "L_x1");
CHECK(assert_equal(c1, c2)); CHECK(assert_equal(c1, c2));
CHECK(assert_equal(c2, c1)); CHECK(assert_equal(c2, c1));
@ -120,10 +130,11 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
// 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;
list<string> keys; keys += "x", "y";
NonlinearConstraint2<VectorConfig> c1( NonlinearConstraint2<VectorConfig> c1(
*test2::g, boost::bind(test2::g, _1, keys),
"x", *test2::G1, "x", boost::bind(test2::G1, _1, keys),
"y", *test2::G2, "y", boost::bind(test2::G1, _1, keys),
p, "L_xy"); p, "L_xy");
// get a configuration to use for finding the error // get a configuration to use for finding the error
@ -141,10 +152,11 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
TEST( NonlinearConstraint2, binary_scalar_linearize ) { TEST( NonlinearConstraint2, binary_scalar_linearize ) {
// create a constraint // create a constraint
size_t p = 1; size_t p = 1;
list<string> keys; keys += "x", "y";
NonlinearConstraint2<VectorConfig> c1( NonlinearConstraint2<VectorConfig> c1(
*test2::g, boost::bind(test2::g, _1, keys),
"x", *test2::G1, "x", boost::bind(test2::G1, _1, keys),
"y", *test2::G2, "y", boost::bind(test2::G2, _1, keys),
p, "L_xy"); p, "L_xy");
// get a configuration to use for finding the error // get a configuration to use for finding the error
@ -173,11 +185,13 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint2, binary_scalar_equal ) { TEST( NonlinearConstraint2, binary_scalar_equal ) {
list<string> keys1, keys2, keys3;
keys1 += "x", "y"; keys2 += "y", "x"; keys3 += "x", "z";
NonlinearConstraint2<VectorConfig> NonlinearConstraint2<VectorConfig>
c1(*test2::g, "x", *test2::G1, "y", *test2::G2, 1, "L_xy"), c1(boost::bind(test2::g, _1, keys1), "x", boost::bind(test2::G1, _1, keys1), "y", boost::bind(test2::G2, _1, keys1), 1, "L_xy"),
c2(*test2::g, "x", *test2::G1, "y", *test2::G2, 1, "L_xy"), c2(boost::bind(test2::g, _1, keys1), "x", boost::bind(test2::G1, _1, keys1), "y", boost::bind(test2::G2, _1, keys1), 1, "L_xy"),
c3(*test2::g, "y", *test2::G1, "x", *test2::G2, 1, "L_xy"), c3(boost::bind(test2::g, _1, keys2), "y", boost::bind(test2::G1, _1, keys2), "x", boost::bind(test2::G2, _1, keys2), 1, "L_xy"),
c4(*test2::g, "x", *test2::G1, "z", *test2::G2, 3, "L_xy"); c4(boost::bind(test2::g, _1, keys3), "x", boost::bind(test2::G1, _1, keys3), "z", boost::bind(test2::G2, _1, keys3), 3, "L_xy");
CHECK(assert_equal(c1, c2)); CHECK(assert_equal(c1, c2));
CHECK(assert_equal(c2, c1)); CHECK(assert_equal(c2, c1));
@ -208,8 +222,9 @@ namespace inequality1 {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_inequality ) { TEST( NonlinearConstraint1, unary_inequality ) {
size_t p = 1; size_t p = 1;
NonlinearConstraint1<VectorConfig> c1(*inequality1::g, list<string> keys; keys += "x";
"x", *inequality1::G, NonlinearConstraint1<VectorConfig> c1(boost::bind(inequality1::g, _1, keys),
"x", boost::bind(inequality1::G, _1, keys),
p, "L_x1", p, "L_x1",
false); // inequality constraint false); // inequality constraint
@ -228,9 +243,10 @@ TEST( NonlinearConstraint1, unary_inequality ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_inequality_linearize ) { TEST( NonlinearConstraint1, unary_inequality_linearize ) {
size_t p = 1; size_t p = 1;
NonlinearConstraint1<VectorConfig> c1(*inequality1::g, list<string> keys; keys += "x";
"x", *inequality1::G, NonlinearConstraint1<VectorConfig> c1(boost::bind(inequality1::g, _1, keys),
p, "L_x", "x", boost::bind(inequality1::G, _1, keys),
p, "L_x1",
false); // inequality constraint false); // inequality constraint
// get configurations to use for linearization // get configurations to use for linearization
@ -240,13 +256,13 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) {
// get a configuration of Lagrange multipliers // get a configuration of Lagrange multipliers
VectorConfig lagrangeConfig; VectorConfig lagrangeConfig;
lagrangeConfig.insert("L_x", Vector_(1, 3.0)); lagrangeConfig.insert("L_x1", 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 factualor is active // check if the factor is active
CHECK(!c1.active(config1)); CHECK(!c1.active(config1));
// linearize for active constraint // linearize for active constraint
@ -255,7 +271,7 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) {
CHECK(c1.active(config2)); CHECK(c1.active(config2));
// verify // verify
GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0), "L_x", eye(1), zero(1), 1.0); GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0), "L_x1", eye(1), zero(1), 1.0);
GaussianFactor expectedConstraint("x", Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0); GaussianFactor expectedConstraint("x", Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0);
CHECK(assert_equal(*actualFactor2, expectedFactor)); CHECK(assert_equal(*actualFactor2, expectedFactor));
CHECK(assert_equal(*actualConstraint2, expectedConstraint)); CHECK(assert_equal(*actualConstraint2, expectedConstraint));
@ -287,9 +303,10 @@ TEST( NonlinearConstraint1, unary_binding ) {
size_t p = 1; size_t p = 1;
double coeff = 2; double coeff = 2;
double radius = 5; double radius = 5;
list<string> keys; keys += "x";
NonlinearConstraint1<VectorConfig> c1( NonlinearConstraint1<VectorConfig> c1(
boost::bind(binding1::g, radius, _1, _2), boost::bind(binding1::g, radius, _1, keys),
"x", boost::bind(binding1::G, coeff, _1, _2), "x", boost::bind(binding1::G, coeff, _1, keys),
p, "L_x1", p, "L_x1",
false); // inequality constraint false); // inequality constraint
@ -338,10 +355,11 @@ TEST( NonlinearConstraint2, binary_binding ) {
double a = 2.0; double a = 2.0;
double b = 1.0; double b = 1.0;
double r = 5.0; double r = 5.0;
list<string> keys; keys += "x", "y";
NonlinearConstraint2<VectorConfig> c1( NonlinearConstraint2<VectorConfig> c1(
boost::bind(binding2::g, r, _1, _2), boost::bind(binding2::g, r, _1, keys),
"x", boost::bind(binding2::G1, a, _1, _2), "x", boost::bind(binding2::G1, a, _1, keys),
"y", boost::bind(binding2::G2, b, _1, _2), "y", boost::bind(binding2::G2, b, _1, keys),
p, "L_xy"); p, "L_xy");
// get a configuration to use for finding the error // get a configuration to use for finding the error

19
cpp/testSQP.cpp
View File

@ -365,10 +365,11 @@ TEST (SQP, two_pose ) {
feas.insert("x1", Vector_(2, 1.0, 1.0)); feas.insert("x1", Vector_(2, 1.0, 1.0));
// constant constraint on x1 // constant constraint on x1
list<string> keys1; keys1 += "x1";
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1( boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
new NonlinearConstraint1<VectorConfig>( new NonlinearConstraint1<VectorConfig>(
*sqp_test2::g, boost::bind(sqp_test2::g, _1, keys1),
"x1", *sqp_test2::G, "x1", boost::bind(sqp_test2::G, _1, keys1),
2, "L_x1")); 2, "L_x1"));
// measurement from x1 to l1 // measurement from x1 to l1
@ -382,11 +383,12 @@ TEST (SQP, two_pose ) {
shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l2")); shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l2"));
// equality constraint between l1 and l2 // equality constraint between l1 and l2
list<string> keys2; keys2 += "l1", "l2";
boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2( boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2(
new NonlinearConstraint2<VectorConfig>( new NonlinearConstraint2<VectorConfig>(
*sqp_test1::g, boost::bind(sqp_test1::g, _1, keys2),
"l1", *sqp_test1::G1, "l1", boost::bind(sqp_test1::G1, _1, keys2),
"l2", *sqp_test1::G2, "l2", boost::bind(sqp_test1::G2, _1, keys2),
2, "L_l1l2")); 2, "L_l1l2"));
// construct the graph // construct the graph
@ -675,11 +677,12 @@ VSLAMGraph stereoExampleGraph() {
// create the binary equality constraint between the landmarks // create the binary equality constraint between the landmarks
// NOTE: this is really just a linear constraint that is exactly the same // NOTE: this is really just a linear constraint that is exactly the same
// as the previous examples // as the previous examples
list<string> keys; keys += "l1", "l2";
boost::shared_ptr<NonlinearConstraint2<VSLAMConfig> > c2( boost::shared_ptr<NonlinearConstraint2<VSLAMConfig> > c2(
new NonlinearConstraint2<VSLAMConfig>( new NonlinearConstraint2<VSLAMConfig>(
*sqp_stereo::g, boost::bind(sqp_stereo::g, _1, keys),
"l1", *sqp_stereo::G1, "l1", boost::bind(sqp_stereo::G1, _1, keys),
"l2", *sqp_stereo::G2, "l2", boost::bind(sqp_stereo::G2, _1, keys),
3, "L_l1l2")); 3, "L_l1l2"));
graph.push_back(c2); graph.push_back(c2);

26
cpp/testSQPOptimizer.cpp
View File

@ -92,10 +92,11 @@ typedef SQPOptimizer<NLGraph, VectorConfig> Optimizer;
*/ */
NLGraph linearMapWarpGraph() { NLGraph linearMapWarpGraph() {
// constant constraint on x1 // constant constraint on x1
list<string> keyx; keyx += "x1";
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1( boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
new NonlinearConstraint1<VectorConfig>( new NonlinearConstraint1<VectorConfig>(
*sqp_LinearMapWarp1::g_func, boost::bind(sqp_LinearMapWarp1::g_func, _1, keyx),
"x1", *sqp_LinearMapWarp1::grad_g, "x1", boost::bind(sqp_LinearMapWarp1::grad_g, _1, keyx),
2, "L_x1")); 2, "L_x1"));
// measurement from x1 to l1 // measurement from x1 to l1
@ -109,11 +110,12 @@ NLGraph linearMapWarpGraph() {
shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l2")); shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l2"));
// equality constraint between l1 and l2 // equality constraint between l1 and l2
list<string> keys; keys += "l1", "l2";
boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2( boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2(
new NonlinearConstraint2<VectorConfig>( new NonlinearConstraint2<VectorConfig>(
*sqp_LinearMapWarp2::g_func, boost::bind(sqp_LinearMapWarp2::g_func, _1, keys),
"l1", *sqp_LinearMapWarp2::grad_g1, "l1", boost::bind(sqp_LinearMapWarp2::grad_g1, _1, keys),
"l2", *sqp_LinearMapWarp2::grad_g2, "l2", boost::bind(sqp_LinearMapWarp2::grad_g2, _1, keys),
2, "L_l1l2")); 2, "L_l1l2"));
// construct the graph // construct the graph
@ -272,9 +274,10 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraph() {
// create a binary inequality constraint that forces the middle point away from // create a binary inequality constraint that forces the middle point away from
// the obstacle // the obstacle
shared_NLC2 c1(new NLC2(*sqp_avoid1::g_func, list<string> keys; keys += "x2", "obs";
"x2", *sqp_avoid1::grad_g1, shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid1::g_func, _1, keys),
"obs", *sqp_avoid1::grad_g2, "x2", boost::bind(sqp_avoid1::grad_g1, _1, keys),
"obs",boost::bind(sqp_avoid1::grad_g2, _1, keys),
1, "L_x2obs", false)); 1, "L_x2obs", false));
// construct the graph // construct the graph
@ -403,9 +406,10 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraphGeneral() {
// create a binary inequality constraint that forces the middle point away from // create a binary inequality constraint that forces the middle point away from
// the obstacle // the obstacle
shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid2::g_func, radius, _1, _2), list<string> keys; keys += "x2", "obs";
"x2", boost::bind(sqp_avoid2::grad_g1, _1, _2), shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid2::g_func, radius, _1, keys),
"obs", boost::bind(sqp_avoid2::grad_g2, _1, _2), "x2", boost::bind(sqp_avoid2::grad_g1, _1, keys),
"obs", boost::bind(sqp_avoid2::grad_g2, _1, keys),
1, "L_x2obs", false)); 1, "L_x2obs", false));
// construct the graph // construct the graph