From 0d580032c575aa9acb21f60147915e50e4473121 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Thu, 22 Oct 2009 15:52:39 +0000
Subject: [PATCH] Added unit test for creating an n-ary LinearFactor

---
 cpp/testLinearFactor.cpp | 78 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 78 insertions(+)

diff --git a/cpp/testLinearFactor.cpp b/cpp/testLinearFactor.cpp
index af5c3192d..5bfd4dd23 100644
--- a/cpp/testLinearFactor.cpp
+++ b/cpp/testLinearFactor.cpp
@@ -109,6 +109,84 @@ TEST( LinearFactor, linearFactor2 )
 }
 
 /* ************************************************************************* */
+TEST( LinearFactor, linearFactorN){
+  vector<LinearFactor::shared_ptr> f;
+  f.push_back(LinearFactor::shared_ptr(new LinearFactor("x1", Matrix_(2,2,
+      1.0, 0.0,
+      0.0, 1.0),
+      Vector_(2,
+      10.0, 5.0))));
+  f.push_back(LinearFactor::shared_ptr(new LinearFactor("x1", Matrix_(2,2,
+      -10.0, 0.0,
+      0.0, -10.0),
+      "x2", Matrix_(2,2,
+      10.0, 0.0,
+      0.0, 10.0),
+      Vector_(2,
+      1.0, -2.0))));
+  f.push_back(LinearFactor::shared_ptr(new LinearFactor("x2", Matrix_(2,2,
+      -10.0, 0.0,
+      0.0, -10.0),
+      "x3", Matrix_(2,2,
+      10.0, 0.0,
+      0.0, 10.0),
+      Vector_(2,
+      1.5, -1.5))));
+  f.push_back(LinearFactor::shared_ptr(new LinearFactor("x3", Matrix_(2,2,
+      -10.0, 0.0,
+      0.0, -10.0),
+      "x4", Matrix_(2,2,
+      10.0, 0.0,
+      0.0, 10.0),
+      Vector_(2,
+      2.0, -1.0))));
+
+  MutableLinearFactor combined(f);
+
+  vector<pair<string, Matrix> > meas;
+  meas.push_back(make_pair("x1", Matrix_(8,2,
+      1.0, 0.0,
+      0.0, 1.0,
+      -10.0, 0.0,
+      0.0, -10.0,
+      0.0, 0.0,
+      0.0, 0.0,
+      0.0, 0.0,
+      0.0, 0.0)));
+  meas.push_back(make_pair("x2", Matrix_(8,2,
+      0.0, 0.0,
+      0.0, 0.0,
+      10.0, 0.0,
+      0.0, 10.0,
+      -10.0, 0.0,
+      0.0, -10.0,
+      0.0, 0.0,
+      0.0, 0.0)));
+  meas.push_back(make_pair("x3", Matrix_(8,2,
+      0.0, 0.0,
+      0.0, 0.0,
+      0.0, 0.0,
+      0.0, 0.0,
+      10.0, 0.0,
+      0.0, 10.0,
+      -10.0, 0.0,
+      0.0, -10.0)));
+  meas.push_back(make_pair("x4", Matrix_(8,2,
+      0.0, 0.0,
+      0.0, 0.0,
+      0.0, 0.0,
+      0.0, 0.0,
+      0.0, 0.0,
+      0.0, 0.0,
+      10.0, 0.0,
+      0.0, 10.0)));
+  Vector b = Vector_(8,
+      10.0, 5.0, 1.0, -2.0, 1.5, -1.5, 2.0, -1.0);
+
+  LinearFactor expected(meas, b);
+  CHECK(combined.equals(expected));
+}
+
 TEST( NonlinearFactorGraph, linearFactor3){
 
 	Matrix A11(2,2);