From c120ee93f600376b17839a53734d34a2cbe4ade2 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Wed, 26 Oct 2011 02:07:35 +0000
Subject: [PATCH] Added unit tests for higher-ary NonlinearFactors

---
 tests/testNonlinearFactor.cpp | 157 ++++++++++++++++++++++++++++++++++
 1 file changed, 157 insertions(+)

diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index 396e325de..2eda663c6 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -30,10 +30,12 @@
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/LieVector.h>
 #include <gtsam/slam/smallExample.h>
 #include <gtsam/slam/simulated2D.h>
 #include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
+#include <gtsam/nonlinear/LieValues-inl.h>
 
 using namespace std;
 using namespace gtsam;
@@ -232,6 +234,161 @@ TEST( NonlinearFactor, linearize_constraint2 )
 	CHECK(assert_equal((const GaussianFactor&)expected, *actual));
 }
 
+/* ************************************************************************* */
+typedef TypedSymbol<LieVector, 'x'> TestKey;
+typedef LieValues<TestKey> TestValues;
+
+/* ************************************************************************* */
+class TestFactor4 : public NonlinearFactor4<TestValues, TestKey, TestKey, TestKey, TestKey> {
+public:
+  typedef NonlinearFactor4<TestValues, TestKey, TestKey, TestKey, TestKey> Base;
+  TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4) {}
+
+  virtual Vector
+    evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4,
+        boost::optional<Matrix&> H1 = boost::none,
+        boost::optional<Matrix&> H2 = boost::none,
+        boost::optional<Matrix&> H3 = boost::none,
+        boost::optional<Matrix&> H4 = boost::none) const {
+    if(H1) {
+      *H1 = Matrix_(1,1, 1.0);
+      *H2 = Matrix_(1,1, 2.0);
+      *H3 = Matrix_(1,1, 3.0);
+      *H4 = Matrix_(1,1, 4.0);
+    }
+    return (Vector(1) << x1 + x2 + x3 + x4).finished();
+  }
+};
+
+/* ************************************ */
+TEST(NonlinearFactor, NonlinearFactor4) {
+  TestFactor4 tf;
+  TestValues tv;
+  tv.insert(1, LieVector(1, 1.0));
+  tv.insert(2, LieVector(1, 2.0));
+  tv.insert(3, LieVector(1, 3.0));
+  tv.insert(4, LieVector(1, 4.0));
+  EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv)));
+  DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
+  Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4);
+  JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
+  LONGS_EQUAL(jf.keys()[0], 0);
+  LONGS_EQUAL(jf.keys()[1], 1);
+  LONGS_EQUAL(jf.keys()[2], 2);
+  LONGS_EQUAL(jf.keys()[3], 3);
+  EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin())));
+  EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1)));
+  EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2)));
+  EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3)));
+  EXPECT(assert_equal(Vector_(1, -5.0), jf.getb()));
+}
+
+/* ************************************************************************* */
+class TestFactor5 : public NonlinearFactor5<TestValues, TestKey, TestKey, TestKey, TestKey, TestKey> {
+public:
+  typedef NonlinearFactor5<TestValues, TestKey, TestKey, TestKey, TestKey, TestKey> Base;
+  TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5) {}
+
+  virtual Vector
+    evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5,
+        boost::optional<Matrix&> H1 = boost::none,
+        boost::optional<Matrix&> H2 = boost::none,
+        boost::optional<Matrix&> H3 = boost::none,
+        boost::optional<Matrix&> H4 = boost::none,
+        boost::optional<Matrix&> H5 = boost::none) const {
+    if(H1) {
+      *H1 = Matrix_(1,1, 1.0);
+      *H2 = Matrix_(1,1, 2.0);
+      *H3 = Matrix_(1,1, 3.0);
+      *H4 = Matrix_(1,1, 4.0);
+      *H5 = Matrix_(1,1, 5.0);
+    }
+    return (Vector(1) << x1 + x2 + x3 + x4 + x5).finished();
+  }
+};
+
+/* ************************************ */
+TEST(NonlinearFactor, NonlinearFactor5) {
+  TestFactor5 tf;
+  TestValues tv;
+  tv.insert(1, LieVector(1, 1.0));
+  tv.insert(2, LieVector(1, 2.0));
+  tv.insert(3, LieVector(1, 3.0));
+  tv.insert(4, LieVector(1, 4.0));
+  tv.insert(5, LieVector(1, 5.0));
+  EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv)));
+  DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
+  Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5);
+  JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
+  LONGS_EQUAL(jf.keys()[0], 0);
+  LONGS_EQUAL(jf.keys()[1], 1);
+  LONGS_EQUAL(jf.keys()[2], 2);
+  LONGS_EQUAL(jf.keys()[3], 3);
+  LONGS_EQUAL(jf.keys()[4], 4);
+  EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin())));
+  EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1)));
+  EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2)));
+  EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3)));
+  EXPECT(assert_equal(Matrix_(1,1, 2.5), jf.getA(jf.begin()+4)));
+  EXPECT(assert_equal(Vector_(1, -7.5), jf.getb()));
+}
+
+/* ************************************************************************* */
+class TestFactor6 : public NonlinearFactor6<TestValues, TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> {
+public:
+  typedef NonlinearFactor6<TestValues, TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> Base;
+  TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5, 6) {}
+
+  virtual Vector
+    evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
+        boost::optional<Matrix&> H1 = boost::none,
+        boost::optional<Matrix&> H2 = boost::none,
+        boost::optional<Matrix&> H3 = boost::none,
+        boost::optional<Matrix&> H4 = boost::none,
+        boost::optional<Matrix&> H5 = boost::none,
+        boost::optional<Matrix&> H6 = boost::none) const {
+    if(H1) {
+      *H1 = Matrix_(1,1, 1.0);
+      *H2 = Matrix_(1,1, 2.0);
+      *H3 = Matrix_(1,1, 3.0);
+      *H4 = Matrix_(1,1, 4.0);
+      *H5 = Matrix_(1,1, 5.0);
+      *H6 = Matrix_(1,1, 6.0);
+    }
+    return (Vector(1) << x1 + x2 + x3 + x4 + x5 + x6).finished();
+  }
+};
+
+/* ************************************ */
+TEST(NonlinearFactor, NonlinearFactor6) {
+  TestFactor6 tf;
+  TestValues tv;
+  tv.insert(1, LieVector(1, 1.0));
+  tv.insert(2, LieVector(1, 2.0));
+  tv.insert(3, LieVector(1, 3.0));
+  tv.insert(4, LieVector(1, 4.0));
+  tv.insert(5, LieVector(1, 5.0));
+  tv.insert(6, LieVector(1, 6.0));
+  EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv)));
+  DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
+  Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5), TestKey(6);
+  JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
+  LONGS_EQUAL(jf.keys()[0], 0);
+  LONGS_EQUAL(jf.keys()[1], 1);
+  LONGS_EQUAL(jf.keys()[2], 2);
+  LONGS_EQUAL(jf.keys()[3], 3);
+  LONGS_EQUAL(jf.keys()[4], 4);
+  LONGS_EQUAL(jf.keys()[5], 5);
+  EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin())));
+  EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1)));
+  EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2)));
+  EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3)));
+  EXPECT(assert_equal(Matrix_(1,1, 2.5), jf.getA(jf.begin()+4)));
+  EXPECT(assert_equal(Matrix_(1,1, 3.0), jf.getA(jf.begin()+5)));
+  EXPECT(assert_equal(Vector_(1, -10.5), jf.getb()));
+
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */