Changed NonlinearConstraints to take cost and gradient functions that do not take the list of keys. Tests have been reconstructed using boost::bind
Alex Cunningham
parent
b02582b43d
commit
a1918056a5
|
|
@ -20,17 +20,17 @@ namespace gtsam {
|
|||
template <class Config>
|
||||
NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key,
|
||||
size_t dim_lagrange,
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
Vector (*g)(const Config& config),
|
||||
bool isEquality)
|
||||
: NonlinearFactor<Config>(zero(dim_lagrange), 1.0),
|
||||
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>
|
||||
NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key,
|
||||
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)
|
||||
: NonlinearFactor<Config>(zero(dim_lagrange), 1.0),
|
||||
lagrange_key_(lagrange_key), p_(dim_lagrange),
|
||||
|
|
@ -48,14 +48,14 @@ bool NonlinearConstraint<Config>::active(const Config& config) const {
|
|||
|
||||
template <class Config>
|
||||
NonlinearConstraint1<Config>::NonlinearConstraint1(
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
Vector (*g)(const Config& config),
|
||||
const std::string& key,
|
||||
Matrix (*gradG)(const Config& config, const std::list<std::string>& keys),
|
||||
Matrix (*gradG)(const Config& config),
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key,
|
||||
bool 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
|
||||
// TODO:should do something smart to find a unique one
|
||||
|
|
@ -67,9 +67,9 @@ NonlinearConstraint1<Config>::NonlinearConstraint1(
|
|||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
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,
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG,
|
||||
boost::function<Matrix(const Config& config)> gradG,
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key,
|
||||
bool isEquality) :
|
||||
|
|
@ -115,10 +115,10 @@ NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig
|
|||
Vector lambda = lagrange[this->lagrange_key_];
|
||||
|
||||
// find the error
|
||||
Vector g = g_(config, this->keys());
|
||||
Vector g = g_(config);
|
||||
|
||||
// construct the gradient
|
||||
Matrix grad = gradG_(config, this->keys());
|
||||
Matrix grad = gradG_(config);
|
||||
|
||||
// construct probabilistic factor
|
||||
Matrix A1 = vector_scale(lambda, grad);
|
||||
|
|
@ -138,16 +138,16 @@ NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig
|
|||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
NonlinearConstraint2<Config>::NonlinearConstraint2(
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
Vector (*g)(const Config& config),
|
||||
const std::string& key1,
|
||||
Matrix (*gradG1)(const Config& config, const std::list<std::string>& keys),
|
||||
Matrix (*gradG1)(const Config& config),
|
||||
const std::string& key2,
|
||||
Matrix (*gradG2)(const Config& config, const std::list<std::string>& keys),
|
||||
Matrix (*gradG2)(const Config& config),
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key,
|
||||
bool 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)
|
||||
{
|
||||
// set a good lagrange key here
|
||||
|
|
@ -161,11 +161,11 @@ NonlinearConstraint2<Config>::NonlinearConstraint2(
|
|||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
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,
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG1,
|
||||
boost::function<Matrix(const Config& config)> gradG1,
|
||||
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,
|
||||
const std::string& lagrange_key,
|
||||
bool isEquality) :
|
||||
|
|
@ -211,11 +211,11 @@ std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> NonlinearConst
|
|||
Vector lambda = lagrange[this->lagrange_key_];
|
||||
|
||||
// find the error
|
||||
Vector g = g_(config, this->keys());
|
||||
Vector g = g_(config);
|
||||
|
||||
// construct the gradients
|
||||
Matrix grad1 = gradG1_(config, this->keys());
|
||||
Matrix grad2 = gradG2_(config, this->keys());
|
||||
Matrix grad1 = gradG1_(config);
|
||||
Matrix grad2 = gradG2_(config);
|
||||
|
||||
// construct probabilistic factor
|
||||
Matrix A1 = vector_scale(lambda, grad1);
|
||||
|
|
|
|||
|
|
@ -40,10 +40,9 @@ protected:
|
|||
* 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
|
||||
* @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
|
||||
*/
|
||||
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g_;
|
||||
boost::function<Vector(const Config& config)> g_;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -55,7 +54,7 @@ public:
|
|||
*/
|
||||
NonlinearConstraint(const std::string& lagrange_key,
|
||||
size_t dim_lagrange,
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
Vector (*g)(const Config& config),
|
||||
bool isEquality=true);
|
||||
|
||||
/** Constructor - sets a more general cost function using boost::bind directly
|
||||
|
|
@ -66,7 +65,7 @@ public:
|
|||
*/
|
||||
NonlinearConstraint(const std::string& lagrange_key,
|
||||
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);
|
||||
|
||||
/** 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;
|
||||
|
||||
/** 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
|
||||
|
|
@ -128,10 +127,9 @@ private:
|
|||
* returns a pxn matrix
|
||||
* Use boost.bind to create the function object
|
||||
* @param config to use for linearization
|
||||
* @param key of selected variable
|
||||
* @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 */
|
||||
std::string key_;
|
||||
|
|
@ -148,9 +146,9 @@ public:
|
|||
* @param isEquality is true if the constraint is an equality constraint
|
||||
*/
|
||||
NonlinearConstraint1(
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
Vector (*g)(const Config& config),
|
||||
const std::string& key,
|
||||
Matrix (*gradG)(const Config& config, const std::list<std::string>& keys),
|
||||
Matrix (*gradG)(const Config& config),
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key="",
|
||||
bool isEquality=true);
|
||||
|
|
@ -165,9 +163,9 @@ public:
|
|||
* @param isEquality is true if the constraint is an equality constraint
|
||||
*/
|
||||
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,
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG,
|
||||
boost::function<Matrix(const Config& config)> gradG,
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key="",
|
||||
bool isEquality=true);
|
||||
|
|
@ -202,11 +200,10 @@ private:
|
|||
* the first and second variables
|
||||
* returns a pxn matrix
|
||||
* @param config to use for linearization
|
||||
* @param key of selected variable
|
||||
* @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, const std::list<std::string>& keys)> gradG2_;
|
||||
boost::function<Matrix(const Config& config)> gradG1_;
|
||||
boost::function<Matrix(const Config& config)> gradG2_;
|
||||
|
||||
/** keys for the constrained variables */
|
||||
std::string key1_;
|
||||
|
|
@ -224,11 +221,11 @@ public:
|
|||
* @param isEquality is true if the constraint is an equality constraint
|
||||
*/
|
||||
NonlinearConstraint2(
|
||||
Vector (*g)(const Config& config, const std::list<std::string>& keys),
|
||||
Vector (*g)(const Config& config),
|
||||
const std::string& key1,
|
||||
Matrix (*gradG1)(const Config& config, const std::list<std::string>& keys),
|
||||
Matrix (*gradG1)(const Config& config),
|
||||
const std::string& key2,
|
||||
Matrix (*gradG2)(const Config& config, const std::list<std::string>& keys),
|
||||
Matrix (*gradG2)(const Config& config),
|
||||
size_t dim_constraint,
|
||||
const std::string& lagrange_key="",
|
||||
bool isEquality=true);
|
||||
|
|
@ -244,11 +241,11 @@ public:
|
|||
* @param isEquality is true if the constraint is an equality constraint
|
||||
*/
|
||||
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,
|
||||
boost::function<Matrix(const Config& config, const std::list<std::string>& keys)> gradG1,
|
||||
boost::function<Matrix(const Config& config)> gradG1,
|
||||
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,
|
||||
const std::string& lagrange_key="",
|
||||
bool isEquality=true);
|
||||
|
|
|
|||
|
|
@ -4,14 +4,17 @@
|
|||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <list>
|
||||
#include <boost/bind.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
#include <VectorConfig.h>
|
||||
#include <NonlinearConstraint.h>
|
||||
#include <NonlinearConstraint-inl.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace boost::assign;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// unary functions with scalar variables
|
||||
|
|
@ -38,7 +41,10 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
|
|||
// the lagrange multipliers will be expected on L_x1
|
||||
// and there is only one multiplier
|
||||
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
|
||||
VectorConfig config;
|
||||
|
|
@ -53,7 +59,10 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
|
|||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint1, unary_scalar_linearize ) {
|
||||
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
|
||||
VectorConfig realconfig;
|
||||
|
|
@ -76,11 +85,12 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint1, unary_scalar_equal ) {
|
||||
list<string> keys1, keys2; keys1 += "x"; keys2 += "y";
|
||||
NonlinearConstraint1<VectorConfig>
|
||||
c1(*test1::g, "x", *test1::G, 1, "L_x1", true),
|
||||
c2(*test1::g, "x", *test1::G, 1, "L_x1"),
|
||||
c3(*test1::g, "x", *test1::G, 2, "L_x1"),
|
||||
c4(*test1::g, "y", *test1::G, 1, "L_x1");
|
||||
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");
|
||||
|
||||
CHECK(assert_equal(c1, c2));
|
||||
CHECK(assert_equal(c2, c1));
|
||||
|
|
@ -120,10 +130,11 @@ 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<string> keys; keys += "x", "y";
|
||||
NonlinearConstraint2<VectorConfig> c1(
|
||||
*test2::g,
|
||||
"x", *test2::G1,
|
||||
"y", *test2::G2,
|
||||
boost::bind(test2::g, _1, keys),
|
||||
"x", boost::bind(test2::G1, _1, keys),
|
||||
"y", boost::bind(test2::G1, _1, keys),
|
||||
p, "L_xy");
|
||||
|
||||
// get a configuration to use for finding the error
|
||||
|
|
@ -141,10 +152,11 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
|
|||
TEST( NonlinearConstraint2, binary_scalar_linearize ) {
|
||||
// create a constraint
|
||||
size_t p = 1;
|
||||
list<string> keys; keys += "x", "y";
|
||||
NonlinearConstraint2<VectorConfig> c1(
|
||||
*test2::g,
|
||||
"x", *test2::G1,
|
||||
"y", *test2::G2,
|
||||
boost::bind(test2::g, _1, keys),
|
||||
"x", boost::bind(test2::G1, _1, keys),
|
||||
"y", boost::bind(test2::G2, _1, keys),
|
||||
p, "L_xy");
|
||||
|
||||
// get a configuration to use for finding the error
|
||||
|
|
@ -173,11 +185,13 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint2, binary_scalar_equal ) {
|
||||
list<string> keys1, keys2, keys3;
|
||||
keys1 += "x", "y"; keys2 += "y", "x"; keys3 += "x", "z";
|
||||
NonlinearConstraint2<VectorConfig>
|
||||
c1(*test2::g, "x", *test2::G1, "y", *test2::G2, 1, "L_xy"),
|
||||
c2(*test2::g, "x", *test2::G1, "y", *test2::G2, 1, "L_xy"),
|
||||
c3(*test2::g, "y", *test2::G1, "x", *test2::G2, 1, "L_xy"),
|
||||
c4(*test2::g, "x", *test2::G1, "z", *test2::G2, 3, "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(boost::bind(test2::g, _1, keys1), "x", boost::bind(test2::G1, _1, keys1), "y", boost::bind(test2::G2, _1, keys1), 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(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(c2, c1));
|
||||
|
|
@ -208,8 +222,9 @@ namespace inequality1 {
|
|||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint1, unary_inequality ) {
|
||||
size_t p = 1;
|
||||
NonlinearConstraint1<VectorConfig> c1(*inequality1::g,
|
||||
"x", *inequality1::G,
|
||||
list<string> keys; keys += "x";
|
||||
NonlinearConstraint1<VectorConfig> c1(boost::bind(inequality1::g, _1, keys),
|
||||
"x", boost::bind(inequality1::G, _1, keys),
|
||||
p, "L_x1",
|
||||
false); // inequality constraint
|
||||
|
||||
|
|
@ -228,9 +243,10 @@ TEST( NonlinearConstraint1, unary_inequality ) {
|
|||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint1, unary_inequality_linearize ) {
|
||||
size_t p = 1;
|
||||
NonlinearConstraint1<VectorConfig> c1(*inequality1::g,
|
||||
"x", *inequality1::G,
|
||||
p, "L_x",
|
||||
list<string> keys; keys += "x";
|
||||
NonlinearConstraint1<VectorConfig> c1(boost::bind(inequality1::g, _1, keys),
|
||||
"x", boost::bind(inequality1::G, _1, keys),
|
||||
p, "L_x1",
|
||||
false); // inequality constraint
|
||||
|
||||
// get configurations to use for linearization
|
||||
|
|
@ -240,13 +256,13 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) {
|
|||
|
||||
// get a configuration of Lagrange multipliers
|
||||
VectorConfig lagrangeConfig;
|
||||
lagrangeConfig.insert("L_x", Vector_(1, 3.0));
|
||||
lagrangeConfig.insert("L_x1", Vector_(1, 3.0));
|
||||
|
||||
// linearize for inactive constraint
|
||||
GaussianFactor::shared_ptr actualFactor1, actualConstraint1;
|
||||
boost::tie(actualFactor1, actualConstraint1) = c1.linearize(config1, lagrangeConfig);
|
||||
|
||||
// check if the factualor is active
|
||||
// check if the factor is active
|
||||
CHECK(!c1.active(config1));
|
||||
|
||||
// linearize for active constraint
|
||||
|
|
@ -255,7 +271,7 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) {
|
|||
CHECK(c1.active(config2));
|
||||
|
||||
// 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);
|
||||
CHECK(assert_equal(*actualFactor2, expectedFactor));
|
||||
CHECK(assert_equal(*actualConstraint2, expectedConstraint));
|
||||
|
|
@ -287,9 +303,10 @@ TEST( NonlinearConstraint1, unary_binding ) {
|
|||
size_t p = 1;
|
||||
double coeff = 2;
|
||||
double radius = 5;
|
||||
list<string> keys; keys += "x";
|
||||
NonlinearConstraint1<VectorConfig> c1(
|
||||
boost::bind(binding1::g, radius, _1, _2),
|
||||
"x", boost::bind(binding1::G, coeff, _1, _2),
|
||||
boost::bind(binding1::g, radius, _1, keys),
|
||||
"x", boost::bind(binding1::G, coeff, _1, keys),
|
||||
p, "L_x1",
|
||||
false); // inequality constraint
|
||||
|
||||
|
|
@ -338,10 +355,11 @@ TEST( NonlinearConstraint2, binary_binding ) {
|
|||
double a = 2.0;
|
||||
double b = 1.0;
|
||||
double r = 5.0;
|
||||
list<string> keys; keys += "x", "y";
|
||||
NonlinearConstraint2<VectorConfig> c1(
|
||||
boost::bind(binding2::g, r, _1, _2),
|
||||
"x", boost::bind(binding2::G1, a, _1, _2),
|
||||
"y", boost::bind(binding2::G2, b, _1, _2),
|
||||
boost::bind(binding2::g, r, _1, keys),
|
||||
"x", boost::bind(binding2::G1, a, _1, keys),
|
||||
"y", boost::bind(binding2::G2, b, _1, keys),
|
||||
p, "L_xy");
|
||||
|
||||
// get a configuration to use for finding the error
|
||||
|
|
|
|||
|
|
@ -365,10 +365,11 @@ TEST (SQP, two_pose ) {
|
|||
feas.insert("x1", Vector_(2, 1.0, 1.0));
|
||||
|
||||
// constant constraint on x1
|
||||
list<string> keys1; keys1 += "x1";
|
||||
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
|
||||
new NonlinearConstraint1<VectorConfig>(
|
||||
*sqp_test2::g,
|
||||
"x1", *sqp_test2::G,
|
||||
boost::bind(sqp_test2::g, _1, keys1),
|
||||
"x1", boost::bind(sqp_test2::G, _1, keys1),
|
||||
2, "L_x1"));
|
||||
|
||||
// measurement from x1 to l1
|
||||
|
|
@ -382,11 +383,12 @@ TEST (SQP, two_pose ) {
|
|||
shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l2"));
|
||||
|
||||
// equality constraint between l1 and l2
|
||||
list<string> keys2; keys2 += "l1", "l2";
|
||||
boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2(
|
||||
new NonlinearConstraint2<VectorConfig>(
|
||||
*sqp_test1::g,
|
||||
"l1", *sqp_test1::G1,
|
||||
"l2", *sqp_test1::G2,
|
||||
boost::bind(sqp_test1::g, _1, keys2),
|
||||
"l1", boost::bind(sqp_test1::G1, _1, keys2),
|
||||
"l2", boost::bind(sqp_test1::G2, _1, keys2),
|
||||
2, "L_l1l2"));
|
||||
|
||||
// construct the graph
|
||||
|
|
@ -675,11 +677,12 @@ VSLAMGraph stereoExampleGraph() {
|
|||
// create the binary equality constraint between the landmarks
|
||||
// NOTE: this is really just a linear constraint that is exactly the same
|
||||
// as the previous examples
|
||||
list<string> keys; keys += "l1", "l2";
|
||||
boost::shared_ptr<NonlinearConstraint2<VSLAMConfig> > c2(
|
||||
new NonlinearConstraint2<VSLAMConfig>(
|
||||
*sqp_stereo::g,
|
||||
"l1", *sqp_stereo::G1,
|
||||
"l2", *sqp_stereo::G2,
|
||||
boost::bind(sqp_stereo::g, _1, keys),
|
||||
"l1", boost::bind(sqp_stereo::G1, _1, keys),
|
||||
"l2", boost::bind(sqp_stereo::G2, _1, keys),
|
||||
3, "L_l1l2"));
|
||||
graph.push_back(c2);
|
||||
|
||||
|
|
|
|||
|
|
@ -92,10 +92,11 @@ typedef SQPOptimizer<NLGraph, VectorConfig> Optimizer;
|
|||
*/
|
||||
NLGraph linearMapWarpGraph() {
|
||||
// constant constraint on x1
|
||||
list<string> keyx; keyx += "x1";
|
||||
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
|
||||
new NonlinearConstraint1<VectorConfig>(
|
||||
*sqp_LinearMapWarp1::g_func,
|
||||
"x1", *sqp_LinearMapWarp1::grad_g,
|
||||
boost::bind(sqp_LinearMapWarp1::g_func, _1, keyx),
|
||||
"x1", boost::bind(sqp_LinearMapWarp1::grad_g, _1, keyx),
|
||||
2, "L_x1"));
|
||||
|
||||
// measurement from x1 to l1
|
||||
|
|
@ -109,11 +110,12 @@ NLGraph linearMapWarpGraph() {
|
|||
shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l2"));
|
||||
|
||||
// equality constraint between l1 and l2
|
||||
list<string> keys; keys += "l1", "l2";
|
||||
boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2(
|
||||
new NonlinearConstraint2<VectorConfig>(
|
||||
*sqp_LinearMapWarp2::g_func,
|
||||
"l1", *sqp_LinearMapWarp2::grad_g1,
|
||||
"l2", *sqp_LinearMapWarp2::grad_g2,
|
||||
boost::bind(sqp_LinearMapWarp2::g_func, _1, keys),
|
||||
"l1", boost::bind(sqp_LinearMapWarp2::grad_g1, _1, keys),
|
||||
"l2", boost::bind(sqp_LinearMapWarp2::grad_g2, _1, keys),
|
||||
2, "L_l1l2"));
|
||||
|
||||
// construct the graph
|
||||
|
|
@ -272,9 +274,10 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraph() {
|
|||
|
||||
// create a binary inequality constraint that forces the middle point away from
|
||||
// the obstacle
|
||||
shared_NLC2 c1(new NLC2(*sqp_avoid1::g_func,
|
||||
"x2", *sqp_avoid1::grad_g1,
|
||||
"obs", *sqp_avoid1::grad_g2,
|
||||
list<string> keys; keys += "x2", "obs";
|
||||
shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid1::g_func, _1, keys),
|
||||
"x2", boost::bind(sqp_avoid1::grad_g1, _1, keys),
|
||||
"obs",boost::bind(sqp_avoid1::grad_g2, _1, keys),
|
||||
1, "L_x2obs", false));
|
||||
|
||||
// construct the graph
|
||||
|
|
@ -403,9 +406,10 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraphGeneral() {
|
|||
|
||||
// create a binary inequality constraint that forces the middle point away from
|
||||
// the obstacle
|
||||
shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid2::g_func, radius, _1, _2),
|
||||
"x2", boost::bind(sqp_avoid2::grad_g1, _1, _2),
|
||||
"obs", boost::bind(sqp_avoid2::grad_g2, _1, _2),
|
||||
list<string> keys; keys += "x2", "obs";
|
||||
shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid2::g_func, radius, _1, keys),
|
||||
"x2", boost::bind(sqp_avoid2::grad_g1, _1, keys),
|
||||
"obs", boost::bind(sqp_avoid2::grad_g2, _1, keys),
|
||||
1, "L_x2obs", false));
|
||||
|
||||
// construct the graph
|
||||
|
|
|
|||
Loading…
Reference in New Issue