Some re-naming and re-formatting only
Frank Dellaert
parent
94f986bbe7
commit
9efac7b3fb
|
|
@ -204,9 +204,9 @@ bool NonlinearConstraint2<Config>::equals(const Factor<Config>& f, double tol) c
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr>
|
||||
NonlinearConstraint2<Config>::linearize(const Config& config, const VectorConfig& lagrange) const {
|
||||
template<class Config>
|
||||
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> NonlinearConstraint2<
|
||||
Config>::linearize(const Config& config, const VectorConfig& lagrange) const {
|
||||
// extract lagrange multiplier
|
||||
Vector lambda = lagrange[this->lagrange_key_];
|
||||
|
||||
|
|
@ -220,12 +220,12 @@ NonlinearConstraint2<Config>::linearize(const Config& config, const VectorConfig
|
|||
// construct probabilistic factor
|
||||
Matrix A1 = vector_scale(lambda, grad1);
|
||||
Matrix A2 = vector_scale(lambda, grad2);
|
||||
GaussianFactor::shared_ptr factor(new
|
||||
GaussianFactor(key1_, A1, key2_, A2,
|
||||
GaussianFactor::shared_ptr factor(new GaussianFactor(key1_, A1, key2_, A2,
|
||||
this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0));
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -17,17 +17,19 @@ using namespace gtsam;
|
|||
// unary functions with scalar variables
|
||||
/* ************************************************************************* */
|
||||
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 */
|
||||
Vector g_func(const VectorConfig& config, const list<string>& keys) {
|
||||
/** p = 1, g(x) = x^2-5 = 0 */
|
||||
Vector g(const VectorConfig& config, const list<string>& keys) {
|
||||
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
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -36,7 +38,7 @@ 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("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
|
||||
VectorConfig config;
|
||||
|
|
@ -44,14 +46,14 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
|
|||
|
||||
// calculate the error
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint1, unary_scalar_linearize ) {
|
||||
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
|
||||
VectorConfig realconfig;
|
||||
|
|
@ -62,23 +64,23 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
|
|||
lagrangeConfig.insert("L_x1", Vector_(1, 3.0));
|
||||
|
||||
// linearize the system
|
||||
GaussianFactor::shared_ptr actFactor, actConstraint;
|
||||
boost::tie(actFactor, actConstraint) = c1.linearize(realconfig, lagrangeConfig);
|
||||
GaussianFactor::shared_ptr actualFactor, actualConstraint;
|
||||
boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig);
|
||||
|
||||
// verify
|
||||
GaussianFactor expFactor("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);
|
||||
CHECK(assert_equal(*actFactor, expFactor));
|
||||
CHECK(assert_equal(*actConstraint, expConstraint));
|
||||
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(*actualFactor, expectedFactor));
|
||||
CHECK(assert_equal(*actualConstraint, expectedConstraint));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint1, unary_scalar_equal ) {
|
||||
NonlinearConstraint1<VectorConfig>
|
||||
c1("x", *test1::grad_g, *test1::g_func, 1, "L_x1", true),
|
||||
c2("x", *test1::grad_g, *test1::g_func, 1, "L_x1"),
|
||||
c3("x", *test1::grad_g, *test1::g_func, 2, "L_x1"),
|
||||
c4("y", *test1::grad_g, *test1::g_func, 1, "L_x1");
|
||||
c1("x", *test1::G, *test1::g, 1, "L_x1", true),
|
||||
c2("x", *test1::G, *test1::g, 1, "L_x1"),
|
||||
c3("x", *test1::G, *test1::g, 2, "L_x1"),
|
||||
c4("y", *test1::G, *test1::g, 1, "L_x1");
|
||||
|
||||
CHECK(assert_equal(c1, c2));
|
||||
CHECK(assert_equal(c2, c1));
|
||||
|
|
@ -90,24 +92,26 @@ TEST( NonlinearConstraint1, unary_scalar_equal ) {
|
|||
// binary functions with scalar variables
|
||||
/* ************************************************************************* */
|
||||
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 */
|
||||
Matrix grad_g2(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) {
|
||||
/** p = 1, g(x) = x^2-5 -y = 0 */
|
||||
Vector g(const VectorConfig& config, const list<string>& keys) {
|
||||
double x = config[keys.front()](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
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -117,9 +121,9 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
|
|||
// and there is only one multiplier
|
||||
size_t p = 1;
|
||||
NonlinearConstraint2<VectorConfig> c1(
|
||||
"x", *test2::grad_g1,
|
||||
"y", *test2::grad_g2,
|
||||
*test2::g_func, p, "L_xy");
|
||||
"x", *test2::G1,
|
||||
"y", *test2::G2,
|
||||
*test2::g, p, "L_xy");
|
||||
|
||||
// get a configuration to use for finding the error
|
||||
VectorConfig config;
|
||||
|
|
@ -137,9 +141,9 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
|
|||
// create a constraint
|
||||
size_t p = 1;
|
||||
NonlinearConstraint2<VectorConfig> c1(
|
||||
"x", *test2::grad_g1,
|
||||
"y", *test2::grad_g2,
|
||||
*test2::g_func, p, "L_xy");
|
||||
"x", *test2::G1,
|
||||
"y", *test2::G2,
|
||||
*test2::g, p, "L_xy");
|
||||
|
||||
// get a configuration to use for finding the error
|
||||
VectorConfig realconfig;
|
||||
|
|
@ -151,27 +155,27 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
|
|||
lagrangeConfig.insert("L_xy", Vector_(1, 3.0));
|
||||
|
||||
// linearize the system
|
||||
GaussianFactor::shared_ptr actFactor, actConstraint;
|
||||
boost::tie(actFactor, actConstraint) = c1.linearize(realconfig, lagrangeConfig);
|
||||
GaussianFactor::shared_ptr actualFactor, actualConstraint;
|
||||
boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig);
|
||||
|
||||
// verify
|
||||
GaussianFactor expFactor("x", Matrix_(1,1, 6.0),
|
||||
GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0),
|
||||
"y", Matrix_(1,1, -3.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),
|
||||
Vector_(1, 6.0), 0.0);
|
||||
CHECK(assert_equal(*actFactor, expFactor));
|
||||
CHECK(assert_equal(*actConstraint, expConstraint));
|
||||
CHECK(assert_equal(*actualFactor, expectedFactor));
|
||||
CHECK(assert_equal(*actualConstraint, expectedConstraint));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint2, binary_scalar_equal ) {
|
||||
NonlinearConstraint2<VectorConfig>
|
||||
c1("x", *test2::grad_g1, "y", *test2::grad_g2,*test2::g_func, 1, "L_xy"),
|
||||
c2("x", *test2::grad_g1, "y", *test2::grad_g2,*test2::g_func, 1, "L_xy"),
|
||||
c3("y", *test2::grad_g1, "x", *test2::grad_g2,*test2::g_func, 1, "L_xy"),
|
||||
c4("x", *test2::grad_g1, "z", *test2::grad_g2,*test2::g_func, 3, "L_xy");
|
||||
c1("x", *test2::G1, "y", *test2::G2,*test2::g, 1, "L_xy"),
|
||||
c2("x", *test2::G1, "y", *test2::G2,*test2::g, 1, "L_xy"),
|
||||
c3("y", *test2::G1, "x", *test2::G2,*test2::g, 1, "L_xy"),
|
||||
c4("x", *test2::G1, "z", *test2::G2,*test2::g, 3, "L_xy");
|
||||
|
||||
CHECK(assert_equal(c1, c2));
|
||||
CHECK(assert_equal(c2, c1));
|
||||
|
|
@ -182,26 +186,28 @@ TEST( NonlinearConstraint2, binary_scalar_equal ) {
|
|||
/* ************************************************************************* */
|
||||
// Inequality tests
|
||||
/* ************************************************************************* */
|
||||
namespace inequality_unary {
|
||||
/** 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);
|
||||
}
|
||||
namespace inequality1 {
|
||||
|
||||
/** p = 1, g(x) x^2 - 5 > 0 */
|
||||
Vector g_func(const VectorConfig& config, const list<string>& keys) {
|
||||
/** p = 1, g(x) x^2 - 5 > 0 */
|
||||
Vector g(const VectorConfig& config, const list<string>& keys) {
|
||||
double x = config[keys.front()](0);
|
||||
double g = x*x-5;
|
||||
double g = x * x - 5;
|
||||
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 ) {
|
||||
size_t p = 1;
|
||||
NonlinearConstraint1<VectorConfig> c1("x", *inequality_unary::grad_g,
|
||||
*inequality_unary::g_func, p, "L_x1",
|
||||
NonlinearConstraint1<VectorConfig> c1("x", *inequality1::G,
|
||||
*inequality1::g, p, "L_x1",
|
||||
false); // inequality constraint
|
||||
|
||||
// get configurations to use for evaluation
|
||||
|
|
@ -211,16 +217,16 @@ TEST( NonlinearConstraint1, unary_inequality ) {
|
|||
|
||||
// check error
|
||||
CHECK(!c1.active(config1));
|
||||
Vector actError2 = c1.error_vector(config2);
|
||||
CHECK(assert_equal(actError2, Vector_(1, -4.0, 1e-9)));
|
||||
Vector actualError2 = c1.error_vector(config2);
|
||||
CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
|
||||
CHECK(c1.active(config2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint1, unary_inequality_linearize ) {
|
||||
size_t p = 1;
|
||||
NonlinearConstraint1<VectorConfig> c1("x", *inequality_unary::grad_g,
|
||||
*inequality_unary::g_func, p, "L_x",
|
||||
NonlinearConstraint1<VectorConfig> c1("x", *inequality1::G,
|
||||
*inequality1::g, p, "L_x",
|
||||
false); // inequality constraint
|
||||
|
||||
// get configurations to use for linearization
|
||||
|
|
@ -233,41 +239,44 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) {
|
|||
lagrangeConfig.insert("L_x", Vector_(1, 3.0));
|
||||
|
||||
// linearize for inactive constraint
|
||||
GaussianFactor::shared_ptr actFactor1, actConstraint1;
|
||||
boost::tie(actFactor1, actConstraint1) = c1.linearize(config1, lagrangeConfig);
|
||||
GaussianFactor::shared_ptr actualFactor1, actualConstraint1;
|
||||
boost::tie(actualFactor1, actualConstraint1) = c1.linearize(config1, lagrangeConfig);
|
||||
|
||||
// check if the factor is active
|
||||
// check if the factualor is active
|
||||
CHECK(!c1.active(config1));
|
||||
|
||||
// linearize for active constraint
|
||||
GaussianFactor::shared_ptr actFactor2, actConstraint2;
|
||||
boost::tie(actFactor2, actConstraint2) = c1.linearize(config2, lagrangeConfig);
|
||||
GaussianFactor::shared_ptr actualFactor2, actualConstraint2;
|
||||
boost::tie(actualFactor2, actualConstraint2) = c1.linearize(config2, lagrangeConfig);
|
||||
CHECK(c1.active(config2));
|
||||
|
||||
// verify
|
||||
GaussianFactor expFactor("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);
|
||||
CHECK(assert_equal(*actFactor2, expFactor));
|
||||
CHECK(assert_equal(*actConstraint2, expConstraint));
|
||||
GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0), "L_x", 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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Binding arbitrary functions
|
||||
/* ************************************************************************* */
|
||||
namespace unary_binding_functions {
|
||||
/** 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);
|
||||
}
|
||||
namespace binding1 {
|
||||
|
||||
/** p = 1, g(x) x^2 - r > 0 */
|
||||
Vector g_func(double r, const VectorConfig& config, const list<string>& keys) {
|
||||
/** p = 1, g(x) x^2 - r > 0 */
|
||||
Vector g(double r, const VectorConfig& config, const list<string>& keys) {
|
||||
double x = config[keys.front()](0);
|
||||
double g = x*x-r;
|
||||
double g = x * x - r;
|
||||
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 ) {
|
||||
|
|
@ -275,8 +284,8 @@ TEST( NonlinearConstraint1, unary_binding ) {
|
|||
double coeff = 2;
|
||||
double radius = 5;
|
||||
NonlinearConstraint1<VectorConfig> c1("x",
|
||||
boost::bind(unary_binding_functions::grad_g, coeff, _1, _2),
|
||||
boost::bind(unary_binding_functions::g_func, radius, _1, _2),
|
||||
boost::bind(binding1::G, coeff, _1, _2),
|
||||
boost::bind(binding1::g, radius, _1, _2),
|
||||
p, "L_x1",
|
||||
false); // inequality constraint
|
||||
|
||||
|
|
@ -287,31 +296,34 @@ TEST( NonlinearConstraint1, unary_binding ) {
|
|||
|
||||
// check error
|
||||
CHECK(!c1.active(config1));
|
||||
Vector actError2 = c1.error_vector(config2);
|
||||
CHECK(assert_equal(actError2, Vector_(1, -4.0, 1e-9)));
|
||||
Vector actualError2 = c1.error_vector(config2);
|
||||
CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
|
||||
CHECK(c1.active(config2));
|
||||
}
|
||||
|
||||
namespace binary_binding_functions {
|
||||
/** gradient for x, gradG(x,y) in x: 2x*/
|
||||
Matrix grad_g1(double c, const VectorConfig& config, const list<string>& keys) {
|
||||
double x = config[keys.front()](0);
|
||||
return Matrix_(1,1, c*x);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
namespace binding2 {
|
||||
|
||||
/** gradient for y, gradG(x,y) in y: -1 */
|
||||
Matrix grad_g2(double c, 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) {
|
||||
/** p = 1, g(x) = x^2-5 -y = 0 */
|
||||
Vector g(double r, const VectorConfig& config, const list<string>& keys) {
|
||||
double x = config[keys.front()](0);
|
||||
double y = config[keys.back()](0);
|
||||
return Vector_(1, x*x-r-y);
|
||||
}
|
||||
} // \namespace test2
|
||||
return Vector_(1, x * x - r - y);
|
||||
}
|
||||
|
||||
/** 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 ) {
|
||||
|
|
@ -323,9 +335,9 @@ TEST( NonlinearConstraint2, binary_binding ) {
|
|||
double b = 1.0;
|
||||
double r = 5.0;
|
||||
NonlinearConstraint2<VectorConfig> c1(
|
||||
"x", boost::bind(binary_binding_functions::grad_g1, a, _1, _2),
|
||||
"y", boost::bind(binary_binding_functions::grad_g2, b, _1, _2),
|
||||
boost::bind(binary_binding_functions::g_func, r, _1, _2), p, "L_xy");
|
||||
"x", boost::bind(binding2::G1, a, _1, _2),
|
||||
"y", boost::bind(binding2::G2, b, _1, _2),
|
||||
boost::bind(binding2::g, r, _1, _2), p, "L_xy");
|
||||
|
||||
// get a configuration to use for finding the error
|
||||
VectorConfig config;
|
||||
|
|
|
|||
188
cpp/testSQP.cpp
188
cpp/testSQP.cpp
|
|
@ -37,27 +37,27 @@ using namespace boost::assign;
|
|||
// trick from some reading group
|
||||
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
|
||||
|
||||
/**
|
||||
/* *********************************************************************
|
||||
* This example uses a nonlinear objective function and
|
||||
* 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.
|
||||
*
|
||||
* Note: the update equation used here has a fixed step size
|
||||
* and gain that is rather arbitrarily chosen, and as such,
|
||||
* will take a silly number of iterations.
|
||||
*/
|
||||
TEST (SQP, problem1_choleski ) {
|
||||
TEST (SQP, problem1_cholesky ) {
|
||||
bool verbose = false;
|
||||
// use a nonlinear function of f(x) = x^2+y^2
|
||||
// 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;
|
||||
init.insert("x", 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;
|
||||
|
||||
if (verbose) init.print("Initial State");
|
||||
|
|
@ -68,10 +68,10 @@ TEST (SQP, problem1_choleski ) {
|
|||
if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl;
|
||||
|
||||
// extract the states
|
||||
double x, y, lam;
|
||||
double x, y, lambda;
|
||||
x = state["x"](0);
|
||||
y = state["y"](0);
|
||||
lam = state["lam"](0);
|
||||
lambda = state["lambda"](0);
|
||||
|
||||
// calculate the components
|
||||
Matrix H1, H2, gradG;
|
||||
|
|
@ -79,7 +79,7 @@ TEST (SQP, problem1_choleski ) {
|
|||
|
||||
// hessian of lagrangian function, in two columns:
|
||||
H1 = Matrix_(2,1,
|
||||
2.0+2.0*lam,
|
||||
2.0+2.0*lambda,
|
||||
0.0);
|
||||
H2 = Matrix_(2,1,
|
||||
0.0,
|
||||
|
|
@ -87,8 +87,8 @@ TEST (SQP, problem1_choleski ) {
|
|||
|
||||
// deriviative of lagrangian function
|
||||
gradL = Vector_(2,
|
||||
2.0*x*(1+lam),
|
||||
2.0*y-lam);
|
||||
2.0*x*(1+lambda),
|
||||
2.0*y-lambda);
|
||||
|
||||
// constraint derivatives
|
||||
gradG = Matrix_(2,1,
|
||||
|
|
@ -101,7 +101,7 @@ TEST (SQP, problem1_choleski ) {
|
|||
|
||||
// create a factor for the states
|
||||
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
|
||||
GaussianFactor::shared_ptr f2(new
|
||||
|
|
@ -116,7 +116,7 @@ TEST (SQP, problem1_choleski ) {
|
|||
|
||||
// solve
|
||||
Ordering ord;
|
||||
ord += "x", "y", "lam";
|
||||
ord += "x", "y", "lambda";
|
||||
VectorConfig delta = fg.optimize(ord).scale(-1.0);
|
||||
if (verbose) delta.print("Delta");
|
||||
|
||||
|
|
@ -129,13 +129,13 @@ TEST (SQP, problem1_choleski ) {
|
|||
|
||||
// verify that it converges to the nearest optimal point
|
||||
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("y", Vector_(1, -0.5));
|
||||
CHECK(assert_equal(expected,state, 1e-2));
|
||||
}
|
||||
|
||||
/**
|
||||
/* *********************************************************************
|
||||
* This example uses a nonlinear objective function and
|
||||
* nonlinear equality constraint. This formulation splits
|
||||
* the constraint into a factor and a linear constraint.
|
||||
|
|
@ -147,13 +147,13 @@ TEST (SQP, problem1_sqp ) {
|
|||
bool verbose = false;
|
||||
// use a nonlinear function of f(x) = x^2+y^2
|
||||
// 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;
|
||||
init.insert("x", 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;
|
||||
|
||||
if (verbose) init.print("Initial State");
|
||||
|
|
@ -164,18 +164,10 @@ TEST (SQP, problem1_sqp ) {
|
|||
if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl;
|
||||
|
||||
// extract the states
|
||||
double x, y, lam;
|
||||
double x, y, lambda;
|
||||
x = state["x"](0);
|
||||
y = state["y"](0);
|
||||
lam = state["lam"](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);
|
||||
lambda = state["lambda"](0);
|
||||
|
||||
/** create the linear factor
|
||||
* ||h(x)-z||^2 => ||Ax-b||^2
|
||||
|
|
@ -185,6 +177,8 @@ TEST (SQP, problem1_sqp ) {
|
|||
* A identity
|
||||
* b linearization point
|
||||
*/
|
||||
Matrix A = eye(2);
|
||||
Vector b = Vector_(2, x, y);
|
||||
GaussianFactor::shared_ptr f1(
|
||||
new GaussianFactor("x", sub(A, 0,2, 0,1), // A(:,1)
|
||||
"y", sub(A, 0,2, 1,2), // A(:,2)
|
||||
|
|
@ -193,20 +187,21 @@ TEST (SQP, problem1_sqp ) {
|
|||
|
||||
/** create the constraint-linear factor
|
||||
* Provides a mechanism to use variable gain to force the constraint
|
||||
* to zero
|
||||
* lam*gradG*dx + dlam + lam
|
||||
* \lambda*gradG*dx + d\lambda = zero
|
||||
* 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(
|
||||
new GaussianFactor("x", lam*sub(gradG, 0,1, 0,1), // scaled gradG(:,1)
|
||||
"y", lam*sub(gradG, 0,1, 1,2), // scaled gradG(:,2)
|
||||
"lam", eye(1), // dlam term
|
||||
new GaussianFactor("x", lambda*sub(gradG, 0,1, 0,1), // scaled gradG(:,1)
|
||||
"y", lambda*sub(gradG, 0,1, 1,2), // scaled gradG(:,2)
|
||||
"lambda", eye(1), // dlambda term
|
||||
Vector_(1, 0.0), // rhs is zero
|
||||
1.0)); // arbitrary sigma
|
||||
|
||||
// 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(
|
||||
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
|
||||
|
|
@ -222,7 +217,7 @@ TEST (SQP, problem1_sqp ) {
|
|||
|
||||
// solve
|
||||
Ordering ord;
|
||||
ord += "x", "y", "lam";
|
||||
ord += "x", "y", "lambda";
|
||||
VectorConfig delta = fg.optimize(ord);
|
||||
if (verbose) delta.print("Delta");
|
||||
|
||||
|
|
@ -243,6 +238,7 @@ TEST (SQP, problem1_sqp ) {
|
|||
CHECK(assert_equal(state["y"], expected["y"], 1e-2));
|
||||
}
|
||||
|
||||
/* ********************************************************************* */
|
||||
// components for nonlinear factor graphs
|
||||
bool vector_compare(const std::string& key,
|
||||
const VectorConfig& feasible,
|
||||
|
|
@ -252,13 +248,15 @@ bool vector_compare(const std::string& key,
|
|||
lin = input[key];
|
||||
return equal_with_abs_tol(lin, feas, 1e-5);
|
||||
}
|
||||
|
||||
typedef NonlinearFactorGraph<VectorConfig> NLGraph;
|
||||
typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared;
|
||||
typedef boost::shared_ptr<NonlinearConstraint<VectorConfig> > shared_c;
|
||||
typedef boost::shared_ptr<NonlinearEquality<VectorConfig> > shared_eq;
|
||||
typedef boost::shared_ptr<VectorConfig> shared_cfg;
|
||||
typedef NonlinearOptimizer<NLGraph,VectorConfig> Optimizer;
|
||||
/**
|
||||
|
||||
/* *********************************************************************
|
||||
* Determining a ground truth linear system
|
||||
* with two poses seeing one landmark, with each pose
|
||||
* constrained to a particular value
|
||||
|
|
@ -317,38 +315,43 @@ TEST (SQP, two_pose_truth ) {
|
|||
CHECK(assert_equal(expected, *actual, 1e-5));
|
||||
}
|
||||
|
||||
/* ********************************************************************* */
|
||||
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 */
|
||||
Matrix grad_g1(const VectorConfig& config, const list<string>& keys) {
|
||||
// binary constraint between landmarks
|
||||
/** 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);
|
||||
}
|
||||
}
|
||||
|
||||
/** 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_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 */
|
||||
Matrix grad_g(const VectorConfig& config, const list<string>& keys) {
|
||||
// Unary Constraint on x1
|
||||
/** 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);
|
||||
}
|
||||
}
|
||||
|
||||
} // \namespace sqp_test2
|
||||
|
||||
/**
|
||||
/* *********************************************************************
|
||||
* Version that actually uses nonlinear equality constraints
|
||||
* to to perform optimization. Same as above, but no
|
||||
* equality constraint on x2, and two landmarks that
|
||||
|
|
@ -364,8 +367,8 @@ TEST (SQP, two_pose ) {
|
|||
// constant constraint on x1
|
||||
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
|
||||
new NonlinearConstraint1<VectorConfig>(
|
||||
"x1", *sqp_test2::grad_g,
|
||||
*sqp_test2::g_func, 2, "L_x1"));
|
||||
"x1", *sqp_test2::G,
|
||||
*sqp_test2::g, 2, "L_x1"));
|
||||
|
||||
// measurement from x1 to l1
|
||||
Vector z1 = Vector_(2, 0.0, 5.0);
|
||||
|
|
@ -380,9 +383,9 @@ TEST (SQP, two_pose ) {
|
|||
// equality constraint between l1 and l2
|
||||
boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2(
|
||||
new NonlinearConstraint2<VectorConfig>(
|
||||
"l1", *sqp_test1::grad_g1,
|
||||
"l2", *sqp_test1::grad_g2,
|
||||
*sqp_test1::g_func, 2, "L_l1l2"));
|
||||
"l1", *sqp_test1::G1,
|
||||
"l2", *sqp_test1::G2,
|
||||
*sqp_test1::g, 2, "L_l1l2"));
|
||||
|
||||
// construct the graph
|
||||
NLGraph graph;
|
||||
|
|
@ -403,7 +406,7 @@ TEST (SQP, two_pose ) {
|
|||
initLagrange->insert("L_x1", Vector_(2, 1.0, 1.0));
|
||||
|
||||
// create state config variables and initialize them
|
||||
VectorConfig state(*initialEstimate), state_lam(*initLagrange);
|
||||
VectorConfig state(*initialEstimate), state_lambda(*initLagrange);
|
||||
|
||||
// optimization loop
|
||||
int maxIt = 1;
|
||||
|
|
@ -422,7 +425,7 @@ TEST (SQP, two_pose ) {
|
|||
} else {
|
||||
// if a constraint, linearize using the constraint method (2 configs)
|
||||
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(c);
|
||||
}
|
||||
|
|
@ -440,8 +443,8 @@ TEST (SQP, two_pose ) {
|
|||
// update both state variables
|
||||
state = state.exmap(delta);
|
||||
if (verbose) state.print("newState");
|
||||
state_lam = state_lam.exmap(delta);
|
||||
if (verbose) state_lam.print("newStateLam");
|
||||
state_lambda = state_lambda.exmap(delta);
|
||||
if (verbose) state_lambda.print("newStateLam");
|
||||
}
|
||||
|
||||
// verify
|
||||
|
|
@ -467,7 +470,7 @@ typedef NonlinearOptimizer<VSLAMGraph,VSLAMConfig> VOptimizer;
|
|||
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 ) {
|
||||
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
|
||||
*/
|
||||
TEST (SQP, stereo_truth_noisy ) {
|
||||
|
|
@ -610,29 +613,35 @@ TEST (SQP, stereo_truth_noisy ) {
|
|||
CHECK(assert_equal(*truthConfig,*(optimizer.config())));
|
||||
}
|
||||
|
||||
/* ********************************************************************* */
|
||||
// Utility function to strip out a landmark number from a string key
|
||||
int getNum(const string& key) {
|
||||
return atoi(key.substr(1, key.size()-1).c_str());
|
||||
}
|
||||
|
||||
/* ********************************************************************* */
|
||||
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 */
|
||||
Matrix grad_g1(const VSLAMConfig& config, const list<string>& keys) {
|
||||
// binary constraint between landmarks
|
||||
/** 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);
|
||||
}
|
||||
}
|
||||
|
||||
/** gradient at l2 */
|
||||
Matrix grad_g2(const VSLAMConfig& config, const list<string>& keys) {
|
||||
return -1.0*eye(3);
|
||||
}
|
||||
} // \namespace sqp_test1
|
||||
/** gradient at l2 */
|
||||
Matrix G2(const VSLAMConfig& config, const list<string>& keys) {
|
||||
return -1.0 * eye(3);
|
||||
}
|
||||
|
||||
} // \namespace sqp_stereo
|
||||
|
||||
/* ********************************************************************* */
|
||||
VSLAMGraph stereoExampleGraph() {
|
||||
// create initial estimates
|
||||
Rot3 faceDownY(Matrix_(3,3,
|
||||
|
|
@ -666,14 +675,15 @@ VSLAMGraph stereoExampleGraph() {
|
|||
// as the previous examples
|
||||
boost::shared_ptr<NonlinearConstraint2<VSLAMConfig> > c2(
|
||||
new NonlinearConstraint2<VSLAMConfig>(
|
||||
"l1", *sqp_stereo::grad_g1,
|
||||
"l2", *sqp_stereo::grad_g2,
|
||||
*sqp_stereo::g_func, 3, "L_l1l2"));
|
||||
"l1", *sqp_stereo::G1,
|
||||
"l2", *sqp_stereo::G2,
|
||||
*sqp_stereo::g, 3, "L_l1l2"));
|
||||
graph.push_back(c2);
|
||||
|
||||
return graph;
|
||||
}
|
||||
|
||||
/* ********************************************************************* */
|
||||
boost::shared_ptr<VSLAMConfig> stereoExampleTruthConfig() {
|
||||
// create initial estimates
|
||||
Rot3 faceDownY(Matrix_(3,3,
|
||||
|
|
@ -697,7 +707,7 @@ boost::shared_ptr<VSLAMConfig> stereoExampleTruthConfig() {
|
|||
return truthConfig;
|
||||
}
|
||||
|
||||
/**
|
||||
/* *********************************************************************
|
||||
* SQP version of the above stereo example,
|
||||
* with the initial case as the ground truth
|
||||
*/
|
||||
|
|
@ -725,7 +735,7 @@ TEST (SQP, stereo_sqp ) {
|
|||
CHECK(assert_equal(*truthConfig,*(afterOneIteration.config())));
|
||||
}
|
||||
|
||||
/**
|
||||
/* *********************************************************************
|
||||
* SQP version of the above stereo example,
|
||||
* with noise in the initial estimate
|
||||
*/
|
||||
|
|
@ -789,7 +799,7 @@ TEST (SQP, stereo_sqp_noisy ) {
|
|||
CHECK(assert_equal(*truthConfig,*(optimizer.config())));
|
||||
}
|
||||
|
||||
/**
|
||||
/* *********************************************************************
|
||||
* SQP version of the above stereo example,
|
||||
* with noise in the initial estimate and manually specified
|
||||
* lagrange multipliers
|
||||
|
|
|
|||
Loading…
Reference in New Issue