diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h index d77407eb5..3fb3d1915 100644 --- a/cpp/NonlinearConstraint-inl.h +++ b/cpp/NonlinearConstraint-inl.h @@ -15,6 +15,21 @@ namespace gtsam { // Implementations of unary nonlinear constraints /* ************************************************************************* */ +template +NonlinearConstraint1::NonlinearConstraint1( + const std::string& key, + Matrix (*gradG)(const Config& config, const std::string& key), + Vector (*g)(const Config& config, const std::string& key), + size_t dim_constraint, + const std::string& lagrange_key) : + NonlinearConstraint(lagrange_key, dim_constraint), + g_(g), gradG_(gradG), key_(key) { + // set a good lagrange key here + // TODO:should do something smart to find a unique one + if (lagrange_key == "") + this->lagrange_key_ = "L_" + key; + } + /* ************************************************************************* */ template void NonlinearConstraint1::print(const std::string& s) const { @@ -64,6 +79,24 @@ NonlinearConstraint1::linearize(const Config& config, const VectorConfig // Implementations of binary nonlinear constraints /* ************************************************************************* */ +/* ************************************************************************* */ +template +NonlinearConstraint2::NonlinearConstraint2( + const std::string& key1, + Matrix (*gradG1)(const Config& config, const std::string& key), + const std::string& key2, + Matrix (*gradG2)(const Config& config, const std::string& key), + Vector (*g)(const Config& config, const std::string& key1, const std::string& key2), + size_t dim_constraint, + const std::string& lagrange_key) : + NonlinearConstraint(lagrange_key, dim_constraint), + g_(g), gradG1_(gradG1), gradG2_(gradG2), key1_(key1), key2_(key2) { + // set a good lagrange key here + // TODO:should do something smart to find a unique one + if (lagrange_key == "") + this->lagrange_key_ = "L_" + key1 + key2; +} + /* ************************************************************************* */ template void NonlinearConstraint2::print(const std::string& s) const { diff --git a/cpp/NonlinearConstraint.h b/cpp/NonlinearConstraint.h index 591b7b80a..a0db1cbc0 100644 --- a/cpp/NonlinearConstraint.h +++ b/cpp/NonlinearConstraint.h @@ -43,7 +43,7 @@ public: lagrange_key_(lagrange_key), p_(dim_lagrange) {} /** returns the key used for the Lagrange multipliers */ - std::string& lagrangeKey() const { return lagrange_key_; } + std::string lagrangeKey() const { return lagrange_key_; } /** returns the number of lagrange multipliers */ size_t nrConstraints() const { return p_; } @@ -120,14 +120,7 @@ public: Matrix (*gradG)(const Config& config, const std::string& key), Vector (*g)(const Config& config, const std::string& key), size_t dim_constraint, - const std::string& lagrange_key="") : - NonlinearConstraint(lagrange_key, dim_constraint), - g_(g), gradG_(gradG), key_(key) { - // set a good lagrange key here - // TODO:should do something smart to find a unique one - if (lagrange_key == "") - this->lagrange_key_ = "L_" + key; - } + const std::string& lagrange_key=""); /** Print */ void print(const std::string& s = "") const; @@ -199,14 +192,7 @@ public: Matrix (*gradG2)(const Config& config, const std::string& key), Vector (*g)(const Config& config, const std::string& key1, const std::string& key2), size_t dim_constraint, - const std::string& lagrange_key="") : - NonlinearConstraint(lagrange_key, dim_constraint), - g_(g), gradG1_(gradG1), gradG2_(gradG2), key1_(key1), key2_(key2) { - // set a good lagrange key here - // TODO:should do something smart to find a unique one - if (lagrange_key == "") - this->lagrange_key_ = "L_" + key1 + key2; - } + const std::string& lagrange_key=""); /** Print */ void print(const std::string& s = "") const; diff --git a/cpp/SQPOptimizer-inl.h b/cpp/SQPOptimizer-inl.h index c222cda4c..bb4edd34f 100644 --- a/cpp/SQPOptimizer-inl.h +++ b/cpp/SQPOptimizer-inl.h @@ -7,27 +7,48 @@ #pragma once #include +#include // for operator += +#include // for insert #include "GaussianFactorGraph.h" #include "SQPOptimizer.h" using namespace std; +using namespace boost::assign; + namespace gtsam { /* **************************************************************** */ template SQPOptimizer::SQPOptimizer(const G& graph, const Ordering& ordering, shared_config config) -: graph_(&graph), ordering_(&ordering), config_(config) +: graph_(&graph), ordering_(&ordering), full_ordering_(ordering), + config_(config), lagrange_config_(new VectorConfig) { - // TODO: assign a value to the lagrange config + // local typedefs + typedef typename G::const_iterator const_iterator; + typedef NonlinearConstraint NLConstraint; + typedef boost::shared_ptr shared_c; + // find the constraints + for (const_iterator factor = graph_->begin(); factor < graph_->end(); factor++) { + const shared_c constraint = boost::shared_dynamic_cast(*factor); + if (constraint != NULL) { + size_t p = constraint->nrConstraints(); + // update ordering + string key = constraint->lagrangeKey(); + full_ordering_ += key; + // initialize lagrange multipliers + lagrange_config_->insert(key, ones(p)); + } + } } /* **************************************************************** */ template SQPOptimizer::SQPOptimizer(const G& graph, const Ordering& ordering, shared_config config, shared_vconfig lagrange) -: graph_(&graph), ordering_(&ordering), config_(config), lagrange_config_(lagrange) +: graph_(&graph), ordering_(&ordering), full_ordering_(ordering), + config_(config), lagrange_config_(lagrange) { } @@ -66,7 +87,7 @@ SQPOptimizer SQPOptimizer::iterate(Verbosity v) const { if (verbose) fg.print("Before Optimization"); // optimize linear graph to get full delta config - VectorConfig delta = fg.optimize(*ordering_).scale(-1.0); + VectorConfig delta = fg.optimize(full_ordering_).scale(-1.0); if (verbose) delta.print("Delta Config"); @@ -75,7 +96,7 @@ SQPOptimizer SQPOptimizer::iterate(Verbosity v) const { shared_vconfig newLamConfig(new VectorConfig(lagrange_config_->exmap(delta))); // construct a new optimizer - return SQPOptimizer(*graph_, *ordering_, newConfig, newLamConfig); + return SQPOptimizer(*graph_, full_ordering_, newConfig, newLamConfig); } } diff --git a/cpp/SQPOptimizer.h b/cpp/SQPOptimizer.h index cd1f81cda..902eca5f2 100644 --- a/cpp/SQPOptimizer.h +++ b/cpp/SQPOptimizer.h @@ -27,16 +27,22 @@ public: // useful for storing configurations typedef boost::shared_ptr shared_config; - typedef boost::shared_ptr shared_vconfig; + typedef boost::shared_ptr shared_vconfig; private: - // keep const references to the graph and the original ordering + // keep const references to the graph and initial ordering const FactorGraph* graph_; const Ordering* ordering_; // keep configurations shared_config config_; shared_vconfig lagrange_config_; + + // keep a configuration that has been updated to include the lagrange multipliers + Ordering full_ordering_; + + + // keep a set of errors for the overall system and just the constraints double error_; double constraint_error_; diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp index ad9059415..6c6bb92d5 100644 --- a/cpp/testSQPOptimizer.cpp +++ b/cpp/testSQPOptimizer.cpp @@ -43,7 +43,7 @@ TEST ( SQPOptimizer, basic ) { CHECK(assert_equal(*config, *(optimizer.config()))); } -namespace sqpOptimizer_test1 { +namespace sqp_LinearMapWarp2 { // binary constraint between landmarks /** g(x) = x-y = 0 */ Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) { @@ -59,9 +59,9 @@ Matrix grad_g1(const VectorConfig& config, const std::string& key) { Matrix grad_g2(const VectorConfig& config, const std::string& key) { return -1*eye(2); } -} // \namespace sqpOptimizer_test1 +} // \namespace sqp_LinearMapWarp2 -namespace sqpOptimizer_test2 { +namespace sqp_LinearMapWarp1 { // Unary Constraint on x1 /** g(x) = x -[1;1] = 0 */ Vector g_func(const VectorConfig& config, const std::string& key) { @@ -72,24 +72,16 @@ Vector g_func(const VectorConfig& config, const std::string& key) { Matrix grad_g(const VectorConfig& config, const std::string& key) { return eye(2); } -} // \namespace sqpOptimizer_test2 +} // \namespace sqp_LinearMapWarp12 typedef SQPOptimizer Optimizer; -/** - * Test pulled from testSQP, with - */ -TEST ( SQPOptimizer, simple_case ) { - bool verbose = false; - // position (1, 1) constraint for x1 - VectorConfig feas; - feas.insert("x1", Vector_(2, 1.0, 1.0)); - +NLGraph linearMapWarpGraph() { // constant constraint on x1 boost::shared_ptr > c1( new NonlinearConstraint1( - "x1", *sqpOptimizer_test2::grad_g, - *sqpOptimizer_test2::g_func, 2, "L_x1")); + "x1", *sqp_LinearMapWarp1::grad_g, + *sqp_LinearMapWarp1::g_func, 2, "L_x1")); // measurement from x1 to l1 Vector z1 = Vector_(2, 0.0, 5.0); @@ -104,9 +96,9 @@ TEST ( SQPOptimizer, simple_case ) { // equality constraint between l1 and l2 boost::shared_ptr > c2( new NonlinearConstraint2( - "l1", *sqpOptimizer_test1::grad_g1, - "l2", *sqpOptimizer_test1::grad_g2, - *sqpOptimizer_test1::g_func, 2, "L_l1l2")); + "l1", *sqp_LinearMapWarp2::grad_g1, + "l2", *sqp_LinearMapWarp2::grad_g2, + *sqp_LinearMapWarp2::g_func, 2, "L_l1l2")); // construct the graph NLGraph graph; @@ -115,9 +107,19 @@ TEST ( SQPOptimizer, simple_case ) { graph.push_back(f1); graph.push_back(f2); + return graph; +} + +/* ********************************************************************* */ +TEST ( SQPOptimizer, map_warp_initLam ) { + bool verbose = false; + // get a graph + NLGraph graph = linearMapWarpGraph(); + // create an initial estimate - shared_config initialEstimate(new VectorConfig(feas)); // must start with feasible set - initialEstimate->insert("l1", Vector_(2, 1.0, 6.0)); // ground truth + shared_config initialEstimate(new VectorConfig); + initialEstimate->insert("x1", Vector_(2, 1.0, 1.0)); + initialEstimate->insert("l1", Vector_(2, 1.0, 6.0)); initialEstimate->insert("l2", Vector_(2, -4.0, 0.0)); // starting with a separate reference frame initialEstimate->insert("x2", Vector_(2, 0.0, 0.0)); // other pose starts at origin @@ -146,6 +148,38 @@ TEST ( SQPOptimizer, simple_case ) { CHECK(assert_equal(actual, expected)); } +/* ********************************************************************* */ +TEST ( SQPOptimizer, map_warp ) { + // get a graph + NLGraph graph = linearMapWarpGraph(); + + // create an initial estimate + shared_config initialEstimate(new VectorConfig); + initialEstimate->insert("x1", Vector_(2, 1.0, 1.0)); + initialEstimate->insert("l1", Vector_(2, 1.0, 6.0)); + initialEstimate->insert("l2", Vector_(2, -4.0, 0.0)); // starting with a separate reference frame + initialEstimate->insert("x2", Vector_(2, 0.0, 0.0)); // other pose starts at origin + + // create an ordering + Ordering ordering; + ordering += "x1", "x2", "l1", "l2"; + + // create an optimizer + Optimizer optimizer(graph, ordering, initialEstimate); + + // perform an iteration of optimization + Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT); + + // get the config back out and verify + VectorConfig actual = *(oneIteration.config()); + VectorConfig expected; + expected.insert("x1", Vector_(2, 1.0, 1.0)); + expected.insert("l1", Vector_(2, 1.0, 6.0)); + expected.insert("l2", Vector_(2, 1.0, 6.0)); + expected.insert("x2", Vector_(2, 5.0, 6.0)); + CHECK(assert_equal(actual, expected)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */