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

Changed argument ordering for constraints

release/4.3a0
Alex Cunningham 2009-12-18 01:40:29 +00:00
parent 7dfc67860c
commit 7d0de77fc6
5 changed files with 50 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cpp/NonlinearConstraint-inl.h
View File

@ -48,9 +48,9 @@ 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),
const std::string& key, const std::string& key,
Matrix (*gradG)(const Config& config, const std::list<std::string>& keys), Matrix (*gradG)(const Config& config, const std::list<std::string>& keys),
Vector (*g)(const Config& config, const std::list<std::string>& keys),
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key, const std::string& lagrange_key,
bool isEquality) : bool isEquality) :
@ -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,
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, const std::list<std::string>& keys)> gradG,
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key, const std::string& lagrange_key,
bool isEquality) : bool isEquality) :
@ -138,11 +138,11 @@ 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),
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::list<std::string>& keys),
const std::string& key2, const std::string& key2,
Matrix (*gradG2)(const Config& config, const std::list<std::string>& keys), Matrix (*gradG2)(const Config& config, const std::list<std::string>& keys),
Vector (*g)(const Config& config, const std::list<std::string>& keys),
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key, const std::string& lagrange_key,
bool isEquality) : bool isEquality) :
@ -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,
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, const std::list<std::string>& keys)> 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, const std::list<std::string>& keys)> gradG2,
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key, const std::string& lagrange_key,
bool isEquality) : bool isEquality) :

8
cpp/NonlinearConstraint.h
View File

@ -148,9 +148,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),
const std::string& key, const std::string& key,
Matrix (*gradG)(const Config& config, const std::list<std::string>& keys), Matrix (*gradG)(const Config& config, const std::list<std::string>& keys),
Vector (*g)(const Config& config, const std::list<std::string>& keys),
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 +165,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,
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, const std::list<std::string>& keys)> gradG,
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key="", const std::string& lagrange_key="",
bool isEquality=true); bool isEquality=true);
@ -224,11 +224,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),
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::list<std::string>& keys),
const std::string& key2, const std::string& key2,
Matrix (*gradG2)(const Config& config, const std::list<std::string>& keys), Matrix (*gradG2)(const Config& config, const std::list<std::string>& keys),
Vector (*g)(const Config& config, const std::list<std::string>& keys),
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 +244,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,
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, const std::list<std::string>& keys)> 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, const std::list<std::string>& keys)> gradG2,
boost::function<Vector(const Config& config, const std::list<std::string>& keys)> g,
size_t dim_constraint, size_t dim_constraint,
const std::string& lagrange_key="", const std::string& lagrange_key="",
bool isEquality=true); bool isEquality=true);

43
cpp/testNonlinearConstraint.cpp
View File

@ -38,7 +38,7 @@ 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("x", *test1::G, *test1::g, p, "L_x1"); NonlinearConstraint1<VectorConfig> c1(*test1::g, "x", *test1::G, 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 +53,7 @@ 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("x", *test1::G, *test1::g, p, "L_x1"); NonlinearConstraint1<VectorConfig> c1(*test1::g, "x", *test1::G, p, "L_x1");
// get a configuration to use for linearization // get a configuration to use for linearization
VectorConfig realconfig; VectorConfig realconfig;
@ -77,10 +77,10 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_equal ) { TEST( NonlinearConstraint1, unary_scalar_equal ) {
NonlinearConstraint1<VectorConfig> NonlinearConstraint1<VectorConfig>
c1("x", *test1::G, *test1::g, 1, "L_x1", true), c1(*test1::g, "x", *test1::G, 1, "L_x1", true),
c2("x", *test1::G, *test1::g, 1, "L_x1"), c2(*test1::g, "x", *test1::G, 1, "L_x1"),
c3("x", *test1::G, *test1::g, 2, "L_x1"), c3(*test1::g, "x", *test1::G, 2, "L_x1"),
c4("y", *test1::G, *test1::g, 1, "L_x1"); c4(*test1::g, "y", *test1::G, 1, "L_x1");
CHECK(assert_equal(c1, c2)); CHECK(assert_equal(c1, c2));
CHECK(assert_equal(c2, c1)); CHECK(assert_equal(c2, c1));
@ -121,9 +121,10 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
// and there is only one multiplier // and there is only one multiplier
size_t p = 1; size_t p = 1;
NonlinearConstraint2<VectorConfig> c1( NonlinearConstraint2<VectorConfig> c1(
*test2::g,
"x", *test2::G1, "x", *test2::G1,
"y", *test2::G2, "y", *test2::G2,
*test2::g, p, "L_xy"); p, "L_xy");
// get a configuration to use for finding the error // get a configuration to use for finding the error
VectorConfig config; VectorConfig config;
@ -141,9 +142,10 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
// create a constraint // create a constraint
size_t p = 1; size_t p = 1;
NonlinearConstraint2<VectorConfig> c1( NonlinearConstraint2<VectorConfig> c1(
*test2::g,
"x", *test2::G1, "x", *test2::G1,
"y", *test2::G2, "y", *test2::G2,
*test2::g, p, "L_xy"); p, "L_xy");
// get a configuration to use for finding the error // get a configuration to use for finding the error
VectorConfig realconfig; VectorConfig realconfig;
@ -172,10 +174,10 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint2, binary_scalar_equal ) { TEST( NonlinearConstraint2, binary_scalar_equal ) {
NonlinearConstraint2<VectorConfig> NonlinearConstraint2<VectorConfig>
c1("x", *test2::G1, "y", *test2::G2,*test2::g, 1, "L_xy"), c1(*test2::g, "x", *test2::G1, "y", *test2::G2, 1, "L_xy"),
c2("x", *test2::G1, "y", *test2::G2,*test2::g, 1, "L_xy"), c2(*test2::g, "x", *test2::G1, "y", *test2::G2, 1, "L_xy"),
c3("y", *test2::G1, "x", *test2::G2,*test2::g, 1, "L_xy"), c3(*test2::g, "y", *test2::G1, "x", *test2::G2, 1, "L_xy"),
c4("x", *test2::G1, "z", *test2::G2,*test2::g, 3, "L_xy"); c4(*test2::g, "x", *test2::G1, "z", *test2::G2, 3, "L_xy");
CHECK(assert_equal(c1, c2)); CHECK(assert_equal(c1, c2));
CHECK(assert_equal(c2, c1)); CHECK(assert_equal(c2, c1));
@ -206,8 +208,9 @@ namespace inequality1 {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_inequality ) { TEST( NonlinearConstraint1, unary_inequality ) {
size_t p = 1; size_t p = 1;
NonlinearConstraint1<VectorConfig> c1("x", *inequality1::G, NonlinearConstraint1<VectorConfig> c1(*inequality1::g,
*inequality1::g, p, "L_x1", "x", *inequality1::G,
p, "L_x1",
false); // inequality constraint false); // inequality constraint
// get configurations to use for evaluation // get configurations to use for evaluation
@ -225,8 +228,9 @@ 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("x", *inequality1::G, NonlinearConstraint1<VectorConfig> c1(*inequality1::g,
*inequality1::g, p, "L_x", "x", *inequality1::G,
p, "L_x",
false); // inequality constraint false); // inequality constraint
// get configurations to use for linearization // get configurations to use for linearization
@ -283,9 +287,9 @@ TEST( NonlinearConstraint1, unary_binding ) {
size_t p = 1; size_t p = 1;
double coeff = 2; double coeff = 2;
double radius = 5; double radius = 5;
NonlinearConstraint1<VectorConfig> c1("x", NonlinearConstraint1<VectorConfig> c1(
boost::bind(binding1::G, coeff, _1, _2),
boost::bind(binding1::g, radius, _1, _2), boost::bind(binding1::g, radius, _1, _2),
"x", boost::bind(binding1::G, coeff, _1, _2),
p, "L_x1", p, "L_x1",
false); // inequality constraint false); // inequality constraint
@ -335,9 +339,10 @@ TEST( NonlinearConstraint2, binary_binding ) {
double b = 1.0; double b = 1.0;
double r = 5.0; double r = 5.0;
NonlinearConstraint2<VectorConfig> c1( NonlinearConstraint2<VectorConfig> c1(
boost::bind(binding2::g, r, _1, _2),
"x", boost::bind(binding2::G1, a, _1, _2), "x", boost::bind(binding2::G1, a, _1, _2),
"y", boost::bind(binding2::G2, b, _1, _2), "y", boost::bind(binding2::G2, b, _1, _2),
boost::bind(binding2::g, r, _1, _2), p, "L_xy"); p, "L_xy");
// get a configuration to use for finding the error // get a configuration to use for finding the error
VectorConfig config; VectorConfig config;

9
cpp/testSQP.cpp
View File

@ -367,8 +367,9 @@ TEST (SQP, two_pose ) {
// constant constraint on x1 // constant constraint on x1
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1( boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
new NonlinearConstraint1<VectorConfig>( new NonlinearConstraint1<VectorConfig>(
*sqp_test2::g,
"x1", *sqp_test2::G, "x1", *sqp_test2::G,
*sqp_test2::g, 2, "L_x1")); 2, "L_x1"));
// measurement from x1 to l1 // measurement from x1 to l1
Vector z1 = Vector_(2, 0.0, 5.0); Vector z1 = Vector_(2, 0.0, 5.0);
@ -383,9 +384,10 @@ TEST (SQP, two_pose ) {
// equality constraint between l1 and l2 // equality constraint between l1 and l2
boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2( boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2(
new NonlinearConstraint2<VectorConfig>( new NonlinearConstraint2<VectorConfig>(
*sqp_test1::g,
"l1", *sqp_test1::G1, "l1", *sqp_test1::G1,
"l2", *sqp_test1::G2, "l2", *sqp_test1::G2,
*sqp_test1::g, 2, "L_l1l2")); 2, "L_l1l2"));
// construct the graph // construct the graph
NLGraph graph; NLGraph graph;
@ -675,9 +677,10 @@ VSLAMGraph stereoExampleGraph() {
// as the previous examples // as the previous examples
boost::shared_ptr<NonlinearConstraint2<VSLAMConfig> > c2( boost::shared_ptr<NonlinearConstraint2<VSLAMConfig> > c2(
new NonlinearConstraint2<VSLAMConfig>( new NonlinearConstraint2<VSLAMConfig>(
*sqp_stereo::g,
"l1", *sqp_stereo::G1, "l1", *sqp_stereo::G1,
"l2", *sqp_stereo::G2, "l2", *sqp_stereo::G2,
*sqp_stereo::g, 3, "L_l1l2")); 3, "L_l1l2"));
graph.push_back(c2); graph.push_back(c2);
return graph; return graph;

16
cpp/testSQPOptimizer.cpp
View File

@ -94,8 +94,9 @@ NLGraph linearMapWarpGraph() {
// constant constraint on x1 // constant constraint on x1
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1( boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
new NonlinearConstraint1<VectorConfig>( new NonlinearConstraint1<VectorConfig>(
*sqp_LinearMapWarp1::g_func,
"x1", *sqp_LinearMapWarp1::grad_g, "x1", *sqp_LinearMapWarp1::grad_g,
*sqp_LinearMapWarp1::g_func, 2, "L_x1")); 2, "L_x1"));
// measurement from x1 to l1 // measurement from x1 to l1
Vector z1 = Vector_(2, 0.0, 5.0); Vector z1 = Vector_(2, 0.0, 5.0);
@ -110,9 +111,10 @@ NLGraph linearMapWarpGraph() {
// equality constraint between l1 and l2 // equality constraint between l1 and l2
boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2( boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2(
new NonlinearConstraint2<VectorConfig>( new NonlinearConstraint2<VectorConfig>(
*sqp_LinearMapWarp2::g_func,
"l1", *sqp_LinearMapWarp2::grad_g1, "l1", *sqp_LinearMapWarp2::grad_g1,
"l2", *sqp_LinearMapWarp2::grad_g2, "l2", *sqp_LinearMapWarp2::grad_g2,
*sqp_LinearMapWarp2::g_func, 2, "L_l1l2")); 2, "L_l1l2"));
// construct the graph // construct the graph
NLGraph graph; NLGraph graph;
@ -270,9 +272,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("x2", *sqp_avoid1::grad_g1, shared_NLC2 c1(new NLC2(*sqp_avoid1::g_func,
"x2", *sqp_avoid1::grad_g1,
"obs", *sqp_avoid1::grad_g2, "obs", *sqp_avoid1::grad_g2,
*sqp_avoid1::g_func, 1, "L_x2obs", false)); 1, "L_x2obs", false));
// construct the graph // construct the graph
NLGraph graph; NLGraph graph;
@ -400,9 +403,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("x2", boost::bind(sqp_avoid2::grad_g1, _1, _2), 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), "obs", boost::bind(sqp_avoid2::grad_g2, _1, _2),
boost::bind(sqp_avoid2::g_func, radius, _1, _2), 1, "L_x2obs", false)); 1, "L_x2obs", false));
// construct the graph // construct the graph
NLGraph graph; NLGraph graph;