From f98f49381a2d9cc2d668df0a8898281adcba23ac Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Wed, 18 Nov 2009 21:57:59 +0000
Subject: [PATCH] Added an example to testSQP to use for combining graphs

---
 cpp/Makefile.am |  2 +-
 cpp/testSQP.cpp | 80 +++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 81 insertions(+), 1 deletion(-)

diff --git a/cpp/Makefile.am b/cpp/Makefile.am
index 101a6834c..847243aee 100644
--- a/cpp/Makefile.am
+++ b/cpp/Makefile.am
@@ -124,7 +124,7 @@ testNonlinearOptimizer_SOURCES   = $(example) testNonlinearOptimizer.cpp
 testNonlinearOptimizer_LDADD     = libgtsam.la
 testNonlinearEquality_SOURCES    = testNonlinearEquality.cpp
 testNonlinearEquality_LDADD      = libgtsam.la
-testSQP_SOURCES                  = testSQP.cpp
+testSQP_SOURCES                  = $(example) testSQP.cpp
 testSQP_LDADD                    = libgtsam.la
 
 # geometry
diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp
index 1b7658ec8..c3ebe87c8 100644
--- a/cpp/testSQP.cpp
+++ b/cpp/testSQP.cpp
@@ -11,8 +11,17 @@
 #include <boost/foreach.hpp>
 #include <CppUnitLite/TestHarness.h>
 #include <GaussianFactorGraph.h>
+#include <NonlinearEquality.h>
+#include <NonlinearFactorGraph.h>
+#include <NonlinearOptimizer.h>
+#include <Simulated2DMeasurement.h>
+#include <simulated2D.h>
 #include <Ordering.h>
 
+// templated implementations
+#include <NonlinearFactorGraph-inl.h>
+#include "NonlinearOptimizer-inl.h"
+
 using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
@@ -238,6 +247,77 @@ 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,
+					const VectorConfig& input) {
+	Vector feas, lin;
+	feas = feasible[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<NonlinearEquality<VectorConfig> > shared_eq;
+typedef NonlinearOptimizer<NLGraph,VectorConfig> Optimizer;
+/**
+ * Determining a ground truth nonlinear system
+ * with two poses seeing one landmark, with each pose
+ * constrained to a particular value
+ */
+TEST (SQP, two_pose_truth ) {
+	// position (1, 1) constraint for x1
+	// position (5, 6) constraint for x2
+	VectorConfig feas;
+	feas.insert("x1", Vector_(2, 1.0, 1.0));
+	feas.insert("x2", Vector_(2, 5.0, 6.0));
+
+	// constant constraint on x1
+	shared_eq ef1(new NonlinearEquality<VectorConfig>("x1", feas, 2, *vector_compare));
+
+	// constant constraint on x2
+	shared_eq ef2(new NonlinearEquality<VectorConfig>("x2", feas, 2, *vector_compare));
+
+	// measurement from x1 to l1
+	Vector z1 = Vector_(2, 0.0, 5.0);
+	double sigma1 = 0.1;
+	shared f1(new Simulated2DMeasurement(z1, sigma1, "x1", "l1"));
+
+	// measurement from x2 to l1
+	Vector z2 = Vector_(2, -4.0, 0.0);
+	double sigma2 = 0.1;
+	shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l1"));
+
+	// construct the graph
+	NLGraph graph;
+	graph.push_back(ef1);
+	graph.push_back(ef2);
+	graph.push_back(f1);
+	graph.push_back(f2);
+
+	// create an initial estimate
+	boost::shared_ptr<VectorConfig> initialEstimate(new VectorConfig(feas)); // must start with feasible set
+	initialEstimate->insert("l1", Vector_(2, 1.0, 6.0)); // ground truth
+	//initialEstimate->insert("l1", Vector_(2, 1.2, 5.6)); // with small error
+
+	// optimize the graph
+	Ordering ordering;
+	ordering += "x1", "x2", "l1";
+	Optimizer optimizer(graph, ordering, initialEstimate, 1e-5);
+
+	// display solution
+	double relativeThreshold = 1e-5;
+	double absoluteThreshold = 1e-5;
+	Optimizer act_opt = optimizer.gaussNewton(relativeThreshold, absoluteThreshold);
+	boost::shared_ptr<const VectorConfig> actual = act_opt.config();
+	//actual->print("Configuration after optimization");
+
+	// verify
+	VectorConfig expected(feas);
+	expected.insert("l1", Vector_(2, 1.0, 6.0));
+	CHECK(assert_equal(expected, *actual, 1e-5));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */