From ba9faa68b6efd79763e61fa88d371b8b5c47e91c Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 8 Oct 2014 11:06:42 +0200
Subject: [PATCH] New Leaf/noise tests

---
 .../nonlinear/tests/testBADFactor.cpp         | 50 +++++++++++++++++++
 1 file changed, 50 insertions(+)

diff --git a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp
index 44a7eab3f..635a19235 100644
--- a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp
+++ b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp
@@ -33,6 +33,56 @@ using namespace gtsam;
 Point2 measured(-17, 30);
 SharedNoiseModel model = noiseModel::Unit::Create(2);
 
+/* ************************************************************************* */
+// Leaf
+TEST(BADFactor, leaf) {
+
+  // Create some values
+  Values values;
+  values.insert(2, Point2(3, 5));
+
+  JacobianFactor expected( //
+      2, (Matrix(2, 2) << 1, 0, 0, 1), //
+      (Vector(2) << -3, -5));
+
+  // Create leaves
+  Point2_ p(2);
+
+  // Try concise version
+  BADFactor<Point2> f(model, Point2(0, 0), p);
+  EXPECT_LONGS_EQUAL(2, f.dim());
+  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
+  boost::shared_ptr<JacobianFactor> jf = //
+      boost::dynamic_pointer_cast<JacobianFactor>(gf);
+  EXPECT( assert_equal(expected, *jf, 1e-9));
+}
+
+/* ************************************************************************* */
+// non-zero noise model
+TEST(BADFactor, model) {
+
+  // Create some values
+  Values values;
+  values.insert(2, Point2(3, 5));
+
+  JacobianFactor expected( //
+      2, (Matrix(2, 2) << 10, 0, 0, 100), //
+      (Vector(2) << -30, -500));
+
+  // Create leaves
+  Point2_ p(2);
+
+  // Try concise version
+  SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
+
+  BADFactor<Point2> f(model, Point2(0, 0), p);
+  EXPECT_LONGS_EQUAL(2, f.dim());
+  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
+  boost::shared_ptr<JacobianFactor> jf = //
+      boost::dynamic_pointer_cast<JacobianFactor>(gf);
+  EXPECT( assert_equal(expected, *jf, 1e-9));
+}
+
 /* ************************************************************************* */
 // Unary(Leaf))
 TEST(BADFactor, test) {