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

Some re-naming and re-formatting only

release/4.3a0
Frank Dellaert 2009-12-15 05:34:49 +00:00
parent 94f986bbe7
commit 9efac7b3fb
3 changed files with 244 additions and 222 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cpp/NonlinearConstraint-inl.h
View File

@ -204,9 +204,9 @@ bool NonlinearConstraint2<Config>::equals(const Factor<Config>& f, double tol) c
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <class Config> template<class Config>
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> NonlinearConstraint2<
NonlinearConstraint2<Config>::linearize(const Config& config, const VectorConfig& lagrange) const { Config>::linearize(const Config& config, const VectorConfig& lagrange) const {
// extract lagrange multiplier // extract lagrange multiplier
Vector lambda = lagrange[this->lagrange_key_]; Vector lambda = lagrange[this->lagrange_key_];
@ -220,12 +220,12 @@ NonlinearConstraint2<Config>::linearize(const Config& config, const VectorConfig
// construct probabilistic factor // construct probabilistic factor
Matrix A1 = vector_scale(lambda, grad1); Matrix A1 = vector_scale(lambda, grad1);
Matrix A2 = vector_scale(lambda, grad2); Matrix A2 = vector_scale(lambda, grad2);
GaussianFactor::shared_ptr factor(new GaussianFactor::shared_ptr factor(new GaussianFactor(key1_, A1, key2_, A2,
GaussianFactor(key1_, A1, key2_, A2,
this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0)); this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0));
// construct the constraint // construct the constraint
GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1, key2_, grad2, -1.0*g, 0.0)); GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1,
key2_, grad2, -1.0 * g, 0.0));
return std::make_pair(factor, constraint); return std::make_pair(factor, constraint);
} }

240
cpp/testNonlinearConstraint.cpp
View File

@ -17,17 +17,19 @@ using namespace gtsam;
// unary functions with scalar variables // unary functions with scalar variables
/* ************************************************************************* */ /* ************************************************************************* */
namespace test1 { namespace test1 {
/** p = 1, gradG(x) = 2x */
Matrix grad_g(const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](0);
return Matrix_(1,1, 2*x);
}
/** p = 1, g(x) = x^2-5 = 0 */ /** p = 1, g(x) = x^2-5 = 0 */
Vector g_func(const VectorConfig& config, const list<string>& keys) { Vector g(const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
return Vector_(1, x*x-5); return Vector_(1, x * x - 5);
} }
/** p = 1, gradG(x) = 2x */
Matrix G(const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](0);
return Matrix_(1, 1, 2 * x);
}
} // \namespace test1 } // \namespace test1
/* ************************************************************************* */ /* ************************************************************************* */
@ -36,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::grad_g, *test1::g_func, p, "L_x1"); NonlinearConstraint1<VectorConfig> c1("x", *test1::G, *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;
@ -44,14 +46,14 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
// calculate the error // calculate the error
Vector actual = c1.error_vector(config); Vector actual = c1.error_vector(config);
Vector expected = Vector_(1.0, -4.0); Vector expected = Vector_(1, -4.0);
CHECK(assert_equal(actual, expected, 1e-5)); CHECK(assert_equal(actual, expected, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_linearize ) { TEST( NonlinearConstraint1, unary_scalar_linearize ) {
size_t p = 1; size_t p = 1;
NonlinearConstraint1<VectorConfig> c1("x", *test1::grad_g, *test1::g_func, p, "L_x1"); NonlinearConstraint1<VectorConfig> c1("x", *test1::G, *test1::g, p, "L_x1");
// get a configuration to use for linearization // get a configuration to use for linearization
VectorConfig realconfig; VectorConfig realconfig;
@ -62,23 +64,23 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
lagrangeConfig.insert("L_x1", Vector_(1, 3.0)); lagrangeConfig.insert("L_x1", Vector_(1, 3.0));
// linearize the system // linearize the system
GaussianFactor::shared_ptr actFactor, actConstraint; GaussianFactor::shared_ptr actualFactor, actualConstraint;
boost::tie(actFactor, actConstraint) = c1.linearize(realconfig, lagrangeConfig); boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig);
// verify // verify
GaussianFactor expFactor("x", Matrix_(1,1, 6.0), "L_x1", eye(1), zero(1), 1.0); GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0), "L_x1", eye(1), zero(1), 1.0);
GaussianFactor expConstraint("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(*actFactor, expFactor)); CHECK(assert_equal(*actualFactor, expectedFactor));
CHECK(assert_equal(*actConstraint, expConstraint)); CHECK(assert_equal(*actualConstraint, expectedConstraint));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_equal ) { TEST( NonlinearConstraint1, unary_scalar_equal ) {
NonlinearConstraint1<VectorConfig> NonlinearConstraint1<VectorConfig>
c1("x", *test1::grad_g, *test1::g_func, 1, "L_x1", true), c1("x", *test1::G, *test1::g, 1, "L_x1", true),
c2("x", *test1::grad_g, *test1::g_func, 1, "L_x1"), c2("x", *test1::G, *test1::g, 1, "L_x1"),
c3("x", *test1::grad_g, *test1::g_func, 2, "L_x1"), c3("x", *test1::G, *test1::g, 2, "L_x1"),
c4("y", *test1::grad_g, *test1::g_func, 1, "L_x1"); c4("y", *test1::G, *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));
@ -90,24 +92,26 @@ TEST( NonlinearConstraint1, unary_scalar_equal ) {
// binary functions with scalar variables // binary functions with scalar variables
/* ************************************************************************* */ /* ************************************************************************* */
namespace test2 { namespace test2 {
/** gradient for x, gradG(x,y) in x: 2x*/
Matrix grad_g1(const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](0);
return Matrix_(1,1, 2.0*x);
}
/** gradient for y, gradG(x,y) in y: -1 */ /** p = 1, g(x) = x^2-5 -y = 0 */
Matrix grad_g2(const VectorConfig& config, const list<string>& keys) { Vector g(const VectorConfig& config, const list<string>& keys) {
double x = config[keys.back()](0);
return Matrix_(1,1, -1.0);
}
/** p = 1, g(x) = x^2-5 -y = 0 */
Vector g_func(const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
double y = config[keys.back()](0); double y = config[keys.back()](0);
return Vector_(1, x*x-5.0-y); return Vector_(1, x * x - 5.0 - y);
} }
/** gradient for x, gradG(x,y) in x: 2x*/
Matrix G1(const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](0);
return Matrix_(1, 1, 2.0 * x);
}
/** gradient for y, gradG(x,y) in y: -1 */
Matrix G2(const VectorConfig& config, const list<string>& keys) {
double x = config[keys.back()](0);
return Matrix_(1, 1, -1.0);
}
} // \namespace test2 } // \namespace test2
/* ************************************************************************* */ /* ************************************************************************* */
@ -117,9 +121,9 @@ 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(
"x", *test2::grad_g1, "x", *test2::G1,
"y", *test2::grad_g2, "y", *test2::G2,
*test2::g_func, p, "L_xy"); *test2::g, 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;
@ -137,9 +141,9 @@ 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(
"x", *test2::grad_g1, "x", *test2::G1,
"y", *test2::grad_g2, "y", *test2::G2,
*test2::g_func, p, "L_xy"); *test2::g, 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;
@ -151,27 +155,27 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
lagrangeConfig.insert("L_xy", Vector_(1, 3.0)); lagrangeConfig.insert("L_xy", Vector_(1, 3.0));
// linearize the system // linearize the system
GaussianFactor::shared_ptr actFactor, actConstraint; GaussianFactor::shared_ptr actualFactor, actualConstraint;
boost::tie(actFactor, actConstraint) = c1.linearize(realconfig, lagrangeConfig); boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig);
// verify // verify
GaussianFactor expFactor("x", Matrix_(1,1, 6.0), GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0),
"y", Matrix_(1,1, -3.0), "y", Matrix_(1,1, -3.0),
"L_xy", eye(1), zero(1), 1.0); "L_xy", eye(1), zero(1), 1.0);
GaussianFactor expConstraint("x", Matrix_(1,1, 2.0), GaussianFactor expectedConstraint("x", Matrix_(1,1, 2.0),
"y", Matrix_(1,1, -1.0), "y", Matrix_(1,1, -1.0),
Vector_(1, 6.0), 0.0); Vector_(1, 6.0), 0.0);
CHECK(assert_equal(*actFactor, expFactor)); CHECK(assert_equal(*actualFactor, expectedFactor));
CHECK(assert_equal(*actConstraint, expConstraint)); CHECK(assert_equal(*actualConstraint, expectedConstraint));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint2, binary_scalar_equal ) { TEST( NonlinearConstraint2, binary_scalar_equal ) {
NonlinearConstraint2<VectorConfig> NonlinearConstraint2<VectorConfig>
c1("x", *test2::grad_g1, "y", *test2::grad_g2,*test2::g_func, 1, "L_xy"), c1("x", *test2::G1, "y", *test2::G2,*test2::g, 1, "L_xy"),
c2("x", *test2::grad_g1, "y", *test2::grad_g2,*test2::g_func, 1, "L_xy"), c2("x", *test2::G1, "y", *test2::G2,*test2::g, 1, "L_xy"),
c3("y", *test2::grad_g1, "x", *test2::grad_g2,*test2::g_func, 1, "L_xy"), c3("y", *test2::G1, "x", *test2::G2,*test2::g, 1, "L_xy"),
c4("x", *test2::grad_g1, "z", *test2::grad_g2,*test2::g_func, 3, "L_xy"); c4("x", *test2::G1, "z", *test2::G2,*test2::g, 3, "L_xy");
CHECK(assert_equal(c1, c2)); CHECK(assert_equal(c1, c2));
CHECK(assert_equal(c2, c1)); CHECK(assert_equal(c2, c1));
@ -182,26 +186,28 @@ TEST( NonlinearConstraint2, binary_scalar_equal ) {
/* ************************************************************************* */ /* ************************************************************************* */
// Inequality tests // Inequality tests
/* ************************************************************************* */ /* ************************************************************************* */
namespace inequality_unary { namespace inequality1 {
/** p = 1, gradG(x) = 2*x */
Matrix grad_g(const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](0);
return Matrix_(1,1, 2*x);
}
/** p = 1, g(x) x^2 - 5 > 0 */ /** p = 1, g(x) x^2 - 5 > 0 */
Vector g_func(const VectorConfig& config, const list<string>& keys) { Vector g(const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
double g = x*x-5; double g = x * x - 5;
return Vector_(1, g); // return the actual cost return Vector_(1, g); // return the actual cost
} }
} // \namespace test1
/** p = 1, gradG(x) = 2*x */
Matrix G(const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](0);
return Matrix_(1, 1, 2 * x);
}
} // \namespace inequality1
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_inequality ) { TEST( NonlinearConstraint1, unary_inequality ) {
size_t p = 1; size_t p = 1;
NonlinearConstraint1<VectorConfig> c1("x", *inequality_unary::grad_g, NonlinearConstraint1<VectorConfig> c1("x", *inequality1::G,
*inequality_unary::g_func, p, "L_x1", *inequality1::g, p, "L_x1",
false); // inequality constraint false); // inequality constraint
// get configurations to use for evaluation // get configurations to use for evaluation
@ -211,16 +217,16 @@ TEST( NonlinearConstraint1, unary_inequality ) {
// check error // check error
CHECK(!c1.active(config1)); CHECK(!c1.active(config1));
Vector actError2 = c1.error_vector(config2); Vector actualError2 = c1.error_vector(config2);
CHECK(assert_equal(actError2, Vector_(1, -4.0, 1e-9))); CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
CHECK(c1.active(config2)); CHECK(c1.active(config2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_inequality_linearize ) { TEST( NonlinearConstraint1, unary_inequality_linearize ) {
size_t p = 1; size_t p = 1;
NonlinearConstraint1<VectorConfig> c1("x", *inequality_unary::grad_g, NonlinearConstraint1<VectorConfig> c1("x", *inequality1::G,
*inequality_unary::g_func, p, "L_x", *inequality1::g, p, "L_x",
false); // inequality constraint false); // inequality constraint
// get configurations to use for linearization // get configurations to use for linearization
@ -233,41 +239,44 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) {
lagrangeConfig.insert("L_x", Vector_(1, 3.0)); lagrangeConfig.insert("L_x", Vector_(1, 3.0));
// linearize for inactive constraint // linearize for inactive constraint
GaussianFactor::shared_ptr actFactor1, actConstraint1; GaussianFactor::shared_ptr actualFactor1, actualConstraint1;
boost::tie(actFactor1, actConstraint1) = c1.linearize(config1, lagrangeConfig); boost::tie(actualFactor1, actualConstraint1) = c1.linearize(config1, lagrangeConfig);
// check if the factor is active // check if the factualor is active
CHECK(!c1.active(config1)); CHECK(!c1.active(config1));
// linearize for active constraint // linearize for active constraint
GaussianFactor::shared_ptr actFactor2, actConstraint2; GaussianFactor::shared_ptr actualFactor2, actualConstraint2;
boost::tie(actFactor2, actConstraint2) = c1.linearize(config2, lagrangeConfig); boost::tie(actualFactor2, actualConstraint2) = c1.linearize(config2, lagrangeConfig);
CHECK(c1.active(config2)); CHECK(c1.active(config2));
// verify // verify
GaussianFactor expFactor("x", Matrix_(1,1, 6.0), "L_x", eye(1), zero(1), 1.0); GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0), "L_x", eye(1), zero(1), 1.0);
GaussianFactor expConstraint("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(*actFactor2, expFactor)); CHECK(assert_equal(*actualFactor2, expectedFactor));
CHECK(assert_equal(*actConstraint2, expConstraint)); CHECK(assert_equal(*actualConstraint2, expectedConstraint));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Binding arbitrary functions // Binding arbitrary functions
/* ************************************************************************* */ /* ************************************************************************* */
namespace unary_binding_functions { namespace binding1 {
/** p = 1, gradG(x) = 2*x */
Matrix grad_g(double coeff, const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](0);
return Matrix_(1,1, coeff*x);
}
/** p = 1, g(x) x^2 - r > 0 */ /** p = 1, g(x) x^2 - r > 0 */
Vector g_func(double r, const VectorConfig& config, const list<string>& keys) { Vector g(double r, const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
double g = x*x-r; double g = x * x - r;
return Vector_(1, g); // return the actual cost return Vector_(1, g); // return the actual cost
} }
} // \namespace unary_binding_functions
/** p = 1, gradG(x) = 2*x */
Matrix G(double coeff, const VectorConfig& config,
const list<string>& keys) {
double x = config[keys.front()](0);
return Matrix_(1, 1, coeff * x);
}
} // \namespace binding1
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_binding ) { TEST( NonlinearConstraint1, unary_binding ) {
@ -275,8 +284,8 @@ TEST( NonlinearConstraint1, unary_binding ) {
double coeff = 2; double coeff = 2;
double radius = 5; double radius = 5;
NonlinearConstraint1<VectorConfig> c1("x", NonlinearConstraint1<VectorConfig> c1("x",
boost::bind(unary_binding_functions::grad_g, coeff, _1, _2), boost::bind(binding1::G, coeff, _1, _2),
boost::bind(unary_binding_functions::g_func, radius, _1, _2), boost::bind(binding1::g, radius, _1, _2),
p, "L_x1", p, "L_x1",
false); // inequality constraint false); // inequality constraint
@ -287,31 +296,34 @@ TEST( NonlinearConstraint1, unary_binding ) {
// check error // check error
CHECK(!c1.active(config1)); CHECK(!c1.active(config1));
Vector actError2 = c1.error_vector(config2); Vector actualError2 = c1.error_vector(config2);
CHECK(assert_equal(actError2, Vector_(1, -4.0, 1e-9))); CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
CHECK(c1.active(config2)); CHECK(c1.active(config2));
} }
namespace binary_binding_functions { /* ************************************************************************* */
/** gradient for x, gradG(x,y) in x: 2x*/ namespace binding2 {
Matrix grad_g1(double c, const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](0);
return Matrix_(1,1, c*x);
}
/** gradient for y, gradG(x,y) in y: -1 */ /** p = 1, g(x) = x^2-5 -y = 0 */
Matrix grad_g2(double c, const VectorConfig& config, const list<string>& keys) { Vector g(double r, const VectorConfig& config, const list<string>& keys) {
double x = config[keys.back()](0);
return Matrix_(1,1, -1.0*c);
}
/** p = 1, g(x) = x^2-5 -y = 0 */
Vector g_func(double r, const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
double y = config[keys.back()](0); double y = config[keys.back()](0);
return Vector_(1, x*x-r-y); return Vector_(1, x * x - r - y);
} }
} // \namespace test2
/** gradient for x, gradG(x,y) in x: 2x*/
Matrix G1(double c, const VectorConfig& config, const list<string>& keys) {
double x = config[keys.front()](0);
return Matrix_(1, 1, c * x);
}
/** gradient for y, gradG(x,y) in y: -1 */
Matrix G2(double c, const VectorConfig& config, const list<string>& keys) {
double x = config[keys.back()](0);
return Matrix_(1, 1, -1.0 * c);
}
} // \namespace binding2
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint2, binary_binding ) { TEST( NonlinearConstraint2, binary_binding ) {
@ -323,9 +335,9 @@ 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(
"x", boost::bind(binary_binding_functions::grad_g1, a, _1, _2), "x", boost::bind(binding2::G1, a, _1, _2),
"y", boost::bind(binary_binding_functions::grad_g2, b, _1, _2), "y", boost::bind(binding2::G2, b, _1, _2),
boost::bind(binary_binding_functions::g_func, r, _1, _2), p, "L_xy"); boost::bind(binding2::g, r, _1, _2), 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;

188
cpp/testSQP.cpp
View File

@ -37,27 +37,27 @@ using namespace boost::assign;
// trick from some reading group // trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
/** /* *********************************************************************
* This example uses a nonlinear objective function and * This example uses a nonlinear objective function and
* nonlinear equality constraint. The formulation is actually * nonlinear equality constraint. The formulation is actually
* the Choleski form that creates the full Hessian explicitly, * the Cholesky form that creates the full Hessian explicitly,
* which should really be avoided with our QR-based machinery. * which should really be avoided with our QR-based machinery.
* *
* Note: the update equation used here has a fixed step size * Note: the update equation used here has a fixed step size
* and gain that is rather arbitrarily chosen, and as such, * and gain that is rather arbitrarily chosen, and as such,
* will take a silly number of iterations. * will take a silly number of iterations.
*/ */
TEST (SQP, problem1_choleski ) { TEST (SQP, problem1_cholesky ) {
bool verbose = false; bool verbose = false;
// use a nonlinear function of f(x) = x^2+y^2 // use a nonlinear function of f(x) = x^2+y^2
// nonlinear equality constraint: g(x) = x^2-5-y=0 // nonlinear equality constraint: g(x) = x^2-5-y=0
// Lagrangian: f(x) + lam*g(x) // Lagrangian: f(x) + \lambda*g(x)
// state structure: [x y lam] // state structure: [x y \lambda]
VectorConfig init, state; VectorConfig init, state;
init.insert("x", Vector_(1, 1.0)); init.insert("x", Vector_(1, 1.0));
init.insert("y", Vector_(1, 1.0)); init.insert("y", Vector_(1, 1.0));
init.insert("lam", Vector_(1, 1.0)); init.insert("lambda", Vector_(1, 1.0));
state = init; state = init;
if (verbose) init.print("Initial State"); if (verbose) init.print("Initial State");
@ -68,10 +68,10 @@ TEST (SQP, problem1_choleski ) {
if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl; if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl;
// extract the states // extract the states
double x, y, lam; double x, y, lambda;
x = state["x"](0); x = state["x"](0);
y = state["y"](0); y = state["y"](0);
lam = state["lam"](0); lambda = state["lambda"](0);
// calculate the components // calculate the components
Matrix H1, H2, gradG; Matrix H1, H2, gradG;
@ -79,7 +79,7 @@ TEST (SQP, problem1_choleski ) {
// hessian of lagrangian function, in two columns: // hessian of lagrangian function, in two columns:
H1 = Matrix_(2,1, H1 = Matrix_(2,1,
2.0+2.0*lam, 2.0+2.0*lambda,
0.0); 0.0);
H2 = Matrix_(2,1, H2 = Matrix_(2,1,
0.0, 0.0,
@ -87,8 +87,8 @@ TEST (SQP, problem1_choleski ) {
// deriviative of lagrangian function // deriviative of lagrangian function
gradL = Vector_(2, gradL = Vector_(2,
2.0*x*(1+lam), 2.0*x*(1+lambda),
2.0*y-lam); 2.0*y-lambda);
// constraint derivatives // constraint derivatives
gradG = Matrix_(2,1, gradG = Matrix_(2,1,
@ -101,7 +101,7 @@ TEST (SQP, problem1_choleski ) {
// create a factor for the states // create a factor for the states
GaussianFactor::shared_ptr f1(new GaussianFactor::shared_ptr f1(new
GaussianFactor("x", H1, "y", H2, "lam", gradG, gradL, 1.0)); GaussianFactor("x", H1, "y", H2, "lambda", gradG, gradL, 1.0));
// create a factor for the lagrange multiplier // create a factor for the lagrange multiplier
GaussianFactor::shared_ptr f2(new GaussianFactor::shared_ptr f2(new
@ -116,7 +116,7 @@ TEST (SQP, problem1_choleski ) {
// solve // solve
Ordering ord; Ordering ord;
ord += "x", "y", "lam"; ord += "x", "y", "lambda";
VectorConfig delta = fg.optimize(ord).scale(-1.0); VectorConfig delta = fg.optimize(ord).scale(-1.0);
if (verbose) delta.print("Delta"); if (verbose) delta.print("Delta");
@ -129,13 +129,13 @@ TEST (SQP, problem1_choleski ) {
// verify that it converges to the nearest optimal point // verify that it converges to the nearest optimal point
VectorConfig expected; VectorConfig expected;
expected.insert("lam", Vector_(1, -1.0)); expected.insert("lambda", Vector_(1, -1.0));
expected.insert("x", Vector_(1, 2.12)); expected.insert("x", Vector_(1, 2.12));
expected.insert("y", Vector_(1, -0.5)); expected.insert("y", Vector_(1, -0.5));
CHECK(assert_equal(expected,state, 1e-2)); CHECK(assert_equal(expected,state, 1e-2));
} }
/** /* *********************************************************************
* This example uses a nonlinear objective function and * This example uses a nonlinear objective function and
* nonlinear equality constraint. This formulation splits * nonlinear equality constraint. This formulation splits
* the constraint into a factor and a linear constraint. * the constraint into a factor and a linear constraint.
@ -147,13 +147,13 @@ TEST (SQP, problem1_sqp ) {
bool verbose = false; bool verbose = false;
// use a nonlinear function of f(x) = x^2+y^2 // use a nonlinear function of f(x) = x^2+y^2
// nonlinear equality constraint: g(x) = x^2-5-y=0 // nonlinear equality constraint: g(x) = x^2-5-y=0
// Lagrangian: f(x) + lam*g(x) // Lagrangian: f(x) + \lambda*g(x)
// state structure: [x y lam] // state structure: [x y \lambda]
VectorConfig init, state; VectorConfig init, state;
init.insert("x", Vector_(1, 1.0)); init.insert("x", Vector_(1, 1.0));
init.insert("y", Vector_(1, 1.0)); init.insert("y", Vector_(1, 1.0));
init.insert("lam", Vector_(1, 1.0)); init.insert("lambda", Vector_(1, 1.0));
state = init; state = init;
if (verbose) init.print("Initial State"); if (verbose) init.print("Initial State");
@ -164,18 +164,10 @@ TEST (SQP, problem1_sqp ) {
if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl; if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl;
// extract the states // extract the states
double x, y, lam; double x, y, lambda;
x = state["x"](0); x = state["x"](0);
y = state["y"](0); y = state["y"](0);
lam = state["lam"](0); lambda = state["lambda"](0);
// create components
Matrix A = eye(2);
Matrix gradG = Matrix_(1, 2,
2*x, -1.0);
Vector g = Vector_(1,
x*x-y-5);
Vector b = Vector_(2, x, y);
/** create the linear factor /** create the linear factor
* ||h(x)-z||^2 => ||Ax-b||^2 * ||h(x)-z||^2 => ||Ax-b||^2
@ -185,6 +177,8 @@ TEST (SQP, problem1_sqp ) {
* A identity * A identity
* b linearization point * b linearization point
*/ */
Matrix A = eye(2);
Vector b = Vector_(2, x, y);
GaussianFactor::shared_ptr f1( GaussianFactor::shared_ptr f1(
new GaussianFactor("x", sub(A, 0,2, 0,1), // A(:,1) new GaussianFactor("x", sub(A, 0,2, 0,1), // A(:,1)
"y", sub(A, 0,2, 1,2), // A(:,2) "y", sub(A, 0,2, 1,2), // A(:,2)
@ -193,20 +187,21 @@ TEST (SQP, problem1_sqp ) {
/** create the constraint-linear factor /** create the constraint-linear factor
* Provides a mechanism to use variable gain to force the constraint * Provides a mechanism to use variable gain to force the constraint
* to zero * \lambda*gradG*dx + d\lambda = zero
* lam*gradG*dx + dlam + lam
* formulated in matrix form as: * formulated in matrix form as:
* [lam*gradG eye(1)] [dx; dlam] = zero * [\lambda*gradG eye(1)] [dx; d\lambda] = zero
*/ */
Matrix gradG = Matrix_(1, 2,2*x, -1.0);
GaussianFactor::shared_ptr f2( GaussianFactor::shared_ptr f2(
new GaussianFactor("x", lam*sub(gradG, 0,1, 0,1), // scaled gradG(:,1) new GaussianFactor("x", lambda*sub(gradG, 0,1, 0,1), // scaled gradG(:,1)
"y", lam*sub(gradG, 0,1, 1,2), // scaled gradG(:,2) "y", lambda*sub(gradG, 0,1, 1,2), // scaled gradG(:,2)
"lam", eye(1), // dlam term "lambda", eye(1), // dlambda term
Vector_(1, 0.0), // rhs is zero Vector_(1, 0.0), // rhs is zero
1.0)); // arbitrary sigma 1.0)); // arbitrary sigma
// create the actual constraint // create the actual constraint
// [gradG] [x; y]- g = 0 // [gradG] [x; y] - g = 0
Vector g = Vector_(1,x*x-y-5);
GaussianFactor::shared_ptr c1( GaussianFactor::shared_ptr c1(
new GaussianFactor("x", sub(gradG, 0,1, 0,1), // slice first part of gradG new GaussianFactor("x", sub(gradG, 0,1, 0,1), // slice first part of gradG
"y", sub(gradG, 0,1, 1,2), // slice second part of gradG "y", sub(gradG, 0,1, 1,2), // slice second part of gradG
@ -222,7 +217,7 @@ TEST (SQP, problem1_sqp ) {
// solve // solve
Ordering ord; Ordering ord;
ord += "x", "y", "lam"; ord += "x", "y", "lambda";
VectorConfig delta = fg.optimize(ord); VectorConfig delta = fg.optimize(ord);
if (verbose) delta.print("Delta"); if (verbose) delta.print("Delta");
@ -243,6 +238,7 @@ TEST (SQP, problem1_sqp ) {
CHECK(assert_equal(state["y"], expected["y"], 1e-2)); CHECK(assert_equal(state["y"], expected["y"], 1e-2));
} }
/* ********************************************************************* */
// components for nonlinear factor graphs // components for nonlinear factor graphs
bool vector_compare(const std::string& key, bool vector_compare(const std::string& key,
const VectorConfig& feasible, const VectorConfig& feasible,
@ -252,13 +248,15 @@ bool vector_compare(const std::string& key,
lin = input[key]; lin = input[key];
return equal_with_abs_tol(lin, feas, 1e-5); return equal_with_abs_tol(lin, feas, 1e-5);
} }
typedef NonlinearFactorGraph<VectorConfig> NLGraph; typedef NonlinearFactorGraph<VectorConfig> NLGraph;
typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared; typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared;
typedef boost::shared_ptr<NonlinearConstraint<VectorConfig> > shared_c; typedef boost::shared_ptr<NonlinearConstraint<VectorConfig> > shared_c;
typedef boost::shared_ptr<NonlinearEquality<VectorConfig> > shared_eq; typedef boost::shared_ptr<NonlinearEquality<VectorConfig> > shared_eq;
typedef boost::shared_ptr<VectorConfig> shared_cfg; typedef boost::shared_ptr<VectorConfig> shared_cfg;
typedef NonlinearOptimizer<NLGraph,VectorConfig> Optimizer; typedef NonlinearOptimizer<NLGraph,VectorConfig> Optimizer;
/**
/* *********************************************************************
* Determining a ground truth linear system * Determining a ground truth linear system
* with two poses seeing one landmark, with each pose * with two poses seeing one landmark, with each pose
* constrained to a particular value * constrained to a particular value
@ -317,38 +315,43 @@ TEST (SQP, two_pose_truth ) {
CHECK(assert_equal(expected, *actual, 1e-5)); CHECK(assert_equal(expected, *actual, 1e-5));
} }
/* ********************************************************************* */
namespace sqp_test1 { namespace sqp_test1 {
// binary constraint between landmarks
/** g(x) = x-y = 0 */
Vector g_func(const VectorConfig& config, const list<string>& keys) {
return config[keys.front()]-config[keys.back()];
}
/** gradient at l1 */ // binary constraint between landmarks
Matrix grad_g1(const VectorConfig& config, const list<string>& keys) { /** g(x) = x-y = 0 */
Vector g(const VectorConfig& config, const list<string>& keys) {
return config[keys.front()] - config[keys.back()];
}
/** gradient at l1 */
Matrix G1(const VectorConfig& config, const list<string>& keys) {
return eye(2); return eye(2);
} }
/** gradient at l2 */
Matrix G2(const VectorConfig& config, const list<string>& keys) {
return -1 * eye(2);
}
/** gradient at l2 */
Matrix grad_g2(const VectorConfig& config, const list<string>& keys) {
return -1*eye(2);
}
} // \namespace sqp_test1 } // \namespace sqp_test1
namespace sqp_test2 { namespace sqp_test2 {
// Unary Constraint on x1
/** g(x) = x -[1;1] = 0 */
Vector g_func(const VectorConfig& config, const list<string>& keys) {
return config[keys.front()]-Vector_(2, 1.0, 1.0);
}
/** gradient at x1 */ // Unary Constraint on x1
Matrix grad_g(const VectorConfig& config, const list<string>& keys) { /** g(x) = x -[1;1] = 0 */
Vector g(const VectorConfig& config, const list<string>& keys) {
return config[keys.front()] - Vector_(2, 1.0, 1.0);
}
/** gradient at x1 */
Matrix G(const VectorConfig& config, const list<string>& keys) {
return eye(2); return eye(2);
} }
} // \namespace sqp_test2 } // \namespace sqp_test2
/** /* *********************************************************************
* Version that actually uses nonlinear equality constraints * Version that actually uses nonlinear equality constraints
* to to perform optimization. Same as above, but no * to to perform optimization. Same as above, but no
* equality constraint on x2, and two landmarks that * equality constraint on x2, and two landmarks that
@ -364,8 +367,8 @@ 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>(
"x1", *sqp_test2::grad_g, "x1", *sqp_test2::G,
*sqp_test2::g_func, 2, "L_x1")); *sqp_test2::g, 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);
@ -380,9 +383,9 @@ 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>(
"l1", *sqp_test1::grad_g1, "l1", *sqp_test1::G1,
"l2", *sqp_test1::grad_g2, "l2", *sqp_test1::G2,
*sqp_test1::g_func, 2, "L_l1l2")); *sqp_test1::g, 2, "L_l1l2"));
// construct the graph // construct the graph
NLGraph graph; NLGraph graph;
@ -403,7 +406,7 @@ TEST (SQP, two_pose ) {
initLagrange->insert("L_x1", Vector_(2, 1.0, 1.0)); initLagrange->insert("L_x1", Vector_(2, 1.0, 1.0));
// create state config variables and initialize them // create state config variables and initialize them
VectorConfig state(*initialEstimate), state_lam(*initLagrange); VectorConfig state(*initialEstimate), state_lambda(*initLagrange);
// optimization loop // optimization loop
int maxIt = 1; int maxIt = 1;
@ -422,7 +425,7 @@ TEST (SQP, two_pose ) {
} else { } else {
// if a constraint, linearize using the constraint method (2 configs) // if a constraint, linearize using the constraint method (2 configs)
GaussianFactor::shared_ptr f, c; GaussianFactor::shared_ptr f, c;
boost::tie(f,c) = constraint->linearize(state, state_lam); boost::tie(f,c) = constraint->linearize(state, state_lambda);
fg.push_back(f); fg.push_back(f);
fg.push_back(c); fg.push_back(c);
} }
@ -440,8 +443,8 @@ TEST (SQP, two_pose ) {
// update both state variables // update both state variables
state = state.exmap(delta); state = state.exmap(delta);
if (verbose) state.print("newState"); if (verbose) state.print("newState");
state_lam = state_lam.exmap(delta); state_lambda = state_lambda.exmap(delta);
if (verbose) state_lam.print("newStateLam"); if (verbose) state_lambda.print("newStateLam");
} }
// verify // verify
@ -467,7 +470,7 @@ typedef NonlinearOptimizer<VSLAMGraph,VSLAMConfig> VOptimizer;
typedef SQPOptimizer<VSLAMGraph, VSLAMConfig> SOptimizer; typedef SQPOptimizer<VSLAMGraph, VSLAMConfig> SOptimizer;
/** /**
* Ground truth for a visual slam example with stereo vision * Ground truth for a visual SLAM example with stereo vision
*/ */
TEST (SQP, stereo_truth ) { TEST (SQP, stereo_truth ) {
bool verbose = false; bool verbose = false;
@ -528,8 +531,8 @@ TEST (SQP, stereo_truth ) {
} }
/** /* *********************************************************************
* Ground truth for a visual slam example with stereo vision * Ground truth for a visual SLAM example with stereo vision
* with some noise injected into the initial config * with some noise injected into the initial config
*/ */
TEST (SQP, stereo_truth_noisy ) { TEST (SQP, stereo_truth_noisy ) {
@ -610,29 +613,35 @@ TEST (SQP, stereo_truth_noisy ) {
CHECK(assert_equal(*truthConfig,*(optimizer.config()))); CHECK(assert_equal(*truthConfig,*(optimizer.config())));
} }
/* ********************************************************************* */
// Utility function to strip out a landmark number from a string key // Utility function to strip out a landmark number from a string key
int getNum(const string& key) { int getNum(const string& key) {
return atoi(key.substr(1, key.size()-1).c_str()); return atoi(key.substr(1, key.size()-1).c_str());
} }
/* ********************************************************************* */
namespace sqp_stereo { namespace sqp_stereo {
// binary constraint between landmarks
/** g(x) = x-y = 0 */
Vector g_func(const VSLAMConfig& config, const list<string>& keys) {
return config.landmarkPoint(getNum(keys.front())).vector()-config.landmarkPoint(getNum(keys.back())).vector();
}
/** gradient at l1 */ // binary constraint between landmarks
Matrix grad_g1(const VSLAMConfig& config, const list<string>& keys) { /** g(x) = x-y = 0 */
Vector g(const VSLAMConfig& config, const list<string>& keys) {
return config.landmarkPoint(getNum(keys.front())).vector()
- config.landmarkPoint(getNum(keys.back())).vector();
}
/** gradient at l1 */
Matrix G1(const VSLAMConfig& config, const list<string>& keys) {
return eye(3); return eye(3);
} }
/** gradient at l2 */ /** gradient at l2 */
Matrix grad_g2(const VSLAMConfig& config, const list<string>& keys) { Matrix G2(const VSLAMConfig& config, const list<string>& keys) {
return -1.0*eye(3); return -1.0 * eye(3);
} }
} // \namespace sqp_test1
} // \namespace sqp_stereo
/* ********************************************************************* */
VSLAMGraph stereoExampleGraph() { VSLAMGraph stereoExampleGraph() {
// create initial estimates // create initial estimates
Rot3 faceDownY(Matrix_(3,3, Rot3 faceDownY(Matrix_(3,3,
@ -666,14 +675,15 @@ 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>(
"l1", *sqp_stereo::grad_g1, "l1", *sqp_stereo::G1,
"l2", *sqp_stereo::grad_g2, "l2", *sqp_stereo::G2,
*sqp_stereo::g_func, 3, "L_l1l2")); *sqp_stereo::g, 3, "L_l1l2"));
graph.push_back(c2); graph.push_back(c2);
return graph; return graph;
} }
/* ********************************************************************* */
boost::shared_ptr<VSLAMConfig> stereoExampleTruthConfig() { boost::shared_ptr<VSLAMConfig> stereoExampleTruthConfig() {
// create initial estimates // create initial estimates
Rot3 faceDownY(Matrix_(3,3, Rot3 faceDownY(Matrix_(3,3,
@ -697,7 +707,7 @@ boost::shared_ptr<VSLAMConfig> stereoExampleTruthConfig() {
return truthConfig; return truthConfig;
} }
/** /* *********************************************************************
* SQP version of the above stereo example, * SQP version of the above stereo example,
* with the initial case as the ground truth * with the initial case as the ground truth
*/ */
@ -725,7 +735,7 @@ TEST (SQP, stereo_sqp ) {
CHECK(assert_equal(*truthConfig,*(afterOneIteration.config()))); CHECK(assert_equal(*truthConfig,*(afterOneIteration.config())));
} }
/** /* *********************************************************************
* SQP version of the above stereo example, * SQP version of the above stereo example,
* with noise in the initial estimate * with noise in the initial estimate
*/ */
@ -789,7 +799,7 @@ TEST (SQP, stereo_sqp_noisy ) {
CHECK(assert_equal(*truthConfig,*(optimizer.config()))); CHECK(assert_equal(*truthConfig,*(optimizer.config())));
} }
/** /* *********************************************************************
* SQP version of the above stereo example, * SQP version of the above stereo example,
* with noise in the initial estimate and manually specified * with noise in the initial estimate and manually specified
* lagrange multipliers * lagrange multipliers