From e789de2353feb54af38b32e00f66aa722c5ce5ad Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Mon, 29 Sep 2014 12:14:59 +0200
Subject: [PATCH] Check derivatives of compose

---
 gtsam_unstable/base/tests/testBAD.cpp | 40 +++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)

diff --git a/gtsam_unstable/base/tests/testBAD.cpp b/gtsam_unstable/base/tests/testBAD.cpp
index c0088d4a2..dac1d7ece 100644
--- a/gtsam_unstable/base/tests/testBAD.cpp
+++ b/gtsam_unstable/base/tests/testBAD.cpp
@@ -94,8 +94,48 @@ TEST(BAD, test) {
 /* ************************************************************************* */
 
 TEST(BAD, compose) {
+
+  // Create expression
   Expression<Rot3> R1(1), R2(2);
   Expression<Rot3> R3 = R1 * R2;
+
+  // Create factor
+  BADFactor<Rot3> f(Rot3(), R3);
+
+  // Create some values
+  Values values;
+  values.insert(1, Rot3());
+  values.insert(2, Rot3());
+
+  // Check linearization
+  JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
+  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));
+}
+
+/* ************************************************************************* */
+// Test compose with arguments referring to the same rotation
+TEST(BAD, compose2) {
+
+  // Create expression
+  Expression<Rot3> R1(1), R2(1);
+  Expression<Rot3> R3 = R1 * R2;
+
+  // Create factor
+  BADFactor<Rot3> f(Rot3(), R3);
+
+  // Create some values
+  Values values;
+  values.insert(1, Rot3());
+
+  // Check linearization
+  JacobianFactor expected(1, 2*eye(3), zero(3));
+  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));
 }
 
 /* ************************************************************************* */