From 4367a245bd258e1139bc9a65dc0dcfdd1753450b Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Thu, 20 Jan 2011 15:42:11 +0000
Subject: [PATCH] Added unit test for NonlinearISAM with a simple markov chain
 demo

---
 tests/Makefile.am           |  1 +
 tests/testNonlinearISAM.cpp | 62 +++++++++++++++++++++++++++++++++++++
 2 files changed, 63 insertions(+)
 create mode 100644 tests/testNonlinearISAM.cpp

diff --git a/tests/Makefile.am b/tests/Makefile.am
index c15da1a6a..a29b2ad69 100644
--- a/tests/Makefile.am
+++ b/tests/Makefile.am
@@ -19,6 +19,7 @@ check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorG
 check_PROGRAMS += testNonlinearOptimizer
 check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph
 check_PROGRAMS += testTupleValues
+check_PROGRAMS += testNonlinearISAM
 
 # only if serialization is available
 if ENABLE_SERIALIZATION
diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp
new file mode 100644
index 000000000..dcdc8dabf
--- /dev/null
+++ b/tests/testNonlinearISAM.cpp
@@ -0,0 +1,62 @@
+/**
+ * @file testNonlinearISAM
+ * @author Alex Cunningham
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <gtsam/linear/Sampler.h>
+#include <gtsam/nonlinear/NonlinearISAM-inl.h>
+#include <gtsam/slam/planarSLAM.h>
+
+using namespace gtsam;
+using namespace planarSLAM;
+
+typedef NonlinearISAM<Values> PlanarISAM;
+
+const double tol=1e-5;
+
+/* ************************************************************************* */
+TEST(testNonlinearISAM, markov_chain ) {
+	int reorder_interval = 2;
+	PlanarISAM isam(reorder_interval);
+
+	SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 3.0, 3.0, 0.5));
+	Sampler sampler(model, 42u);
+
+	// create initial graph
+	PoseKey key(0);
+	Pose2 cur_pose; // start at origin
+	Graph start_factors;
+	start_factors.addPoseConstraint(key, cur_pose);
+	Values init;
+	Values expected;
+	init.insert(key, cur_pose);
+	expected.insert(key, cur_pose);
+	isam.update(start_factors, init);
+
+	// loop for a period of time to verify memory usage
+	size_t nrPoses = 21;
+	Pose2 z(1.0, 2.0, 0.1);
+	for (size_t i=1; i<=nrPoses; ++i) {
+		Graph new_factors;
+		PoseKey key1(i-1), key2(i);
+		new_factors.addOdometry(key1, key2, z, model);
+		Values new_init;
+		cur_pose = cur_pose.compose(z);
+		new_init.insert(key2, cur_pose.expmap(sampler.sample()));
+		expected.insert(key2, cur_pose);
+		isam.update(new_factors, new_init);
+	}
+
+	// verify values - all but the last one should be very close
+	Values actual = isam.estimate();
+	for (size_t i=0; i<21; ++i) {
+		PoseKey cur_key(i);
+		EXPECT(assert_equal(expected[cur_key], actual[cur_key], tol));
+	}
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */