SQPOptimizer can now do its own initialization of the Lagrange multipliers.

Cleaned up NonlinearConstraint

cpp/NonlinearConstraint-inl.h

@@ -15,6 +15,21 @@ namespace gtsam {
 // Implementations of unary nonlinear constraints
 /* ************************************************************************* */
 
+template <class Config>
+NonlinearConstraint1<Config>::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<Config>(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 <class Config>
 void NonlinearConstraint1<Config>::print(const std::string& s) const {
@@ -64,6 +79,24 @@ NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig
 // Implementations of binary nonlinear constraints
 /* ************************************************************************* */
 
+/* ************************************************************************* */
+template <class Config>
+NonlinearConstraint2<Config>::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<Config>(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 <class Config>
 void NonlinearConstraint2<Config>::print(const std::string& s) const {

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="") :
+			const std::string& lagrange_key="");
-				NonlinearConstraint<Config>(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;
-	}
 
 	/** 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="") :
+			const std::string& lagrange_key="");
-				NonlinearConstraint<Config>(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;
-	}
 
 	/** Print */
 	void print(const std::string& s = "") const;

cpp/SQPOptimizer-inl.h

@@ -7,27 +7,48 @@
 #pragma once
 
 #include <boost/foreach.hpp>
+#include <boost/assign/std/list.hpp> // for operator +=
+#include <boost/assign/std/map.hpp> // for insert
 #include "GaussianFactorGraph.h"
 #include "SQPOptimizer.h"
 
 using namespace std;
+using namespace boost::assign;
+
 namespace gtsam {
 
 /* **************************************************************** */
 template <class G, class C>
 SQPOptimizer<G,C>::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<C> NLConstraint;
+	typedef boost::shared_ptr<NLConstraint > shared_c;
 
+	// find the constraints
+	for (const_iterator factor = graph_->begin(); factor < graph_->end(); factor++) {
+		const shared_c constraint = boost::shared_dynamic_cast<NLConstraint >(*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 <class G, class C>
 SQPOptimizer<G,C>::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<G, C> SQPOptimizer<G, C>::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<G, C> SQPOptimizer<G, C>::iterate(Verbosity v) const {
 	shared_vconfig newLamConfig(new VectorConfig(lagrange_config_->exmap(delta)));
 
 	// construct a new optimizer
-	return SQPOptimizer<G, C>(*graph_, *ordering_, newConfig, newLamConfig);
+	return SQPOptimizer<G, C>(*graph_, full_ordering_, newConfig, newLamConfig);
 }
 
 }

cpp/SQPOptimizer.h

@@ -27,16 +27,22 @@ public:
 
 	// useful for storing configurations
 	typedef boost::shared_ptr<const Config> shared_config;
-	typedef boost::shared_ptr<const VectorConfig> shared_vconfig;
+	typedef boost::shared_ptr<VectorConfig> 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_;
 

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<NLGraph, VectorConfig> Optimizer;
 
-/**
+NLGraph linearMapWarpGraph() {
- * 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));
-
 	// constant constraint on x1
 	boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
 			new NonlinearConstraint1<VectorConfig>(
-					"x1", *sqpOptimizer_test2::grad_g,
+					"x1", *sqp_LinearMapWarp1::grad_g,
-					*sqpOptimizer_test2::g_func, 2, "L_x1"));
+					*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<NonlinearConstraint2<VectorConfig> > c2(
 			new NonlinearConstraint2<VectorConfig>(
-					"l1", *sqpOptimizer_test1::grad_g1,
+					"l1", *sqp_LinearMapWarp2::grad_g1,
-					"l2", *sqpOptimizer_test1::grad_g2,
+					"l2", *sqp_LinearMapWarp2::grad_g2,
-					*sqpOptimizer_test1::g_func, 2, "L_l1l2"));
+					*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
+	shared_config initialEstimate(new VectorConfig);
-	initialEstimate->insert("l1", Vector_(2, 1.0, 6.0)); // ground truth
+	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); }
 /* ************************************************************************* */