From b19132e0044c3e7de2ed383cebb7ecb3a8ec26bc Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 18 Sep 2014 17:10:39 -0500
Subject: [PATCH] First version of test, with vector of Matrices

---
 .cproject                             |  16 +++-
 gtsam_unstable/base/tests/testBAD.cpp | 124 ++++++++++++++++++++++++++
 2 files changed, 136 insertions(+), 4 deletions(-)
 create mode 100644 gtsam_unstable/base/tests/testBAD.cpp

diff --git a/.cproject b/.cproject
index 21ac9d913..42dbff570 100644
--- a/.cproject
+++ b/.cproject
@@ -784,18 +784,18 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testGaussianFactorGraphUnordered.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
-				<buildTarget>testGaussianFactorGraph.run</buildTarget>
+				<buildTarget>testGaussianFactorGraphUnordered.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGaussianBayesNet.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testGaussianBayesNetUnordered.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
-				<buildTarget>testGaussianBayesNet.run</buildTarget>
+				<buildTarget>testGaussianBayesNetUnordered.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -1415,6 +1415,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testBAD.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testBAD.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
diff --git a/gtsam_unstable/base/tests/testBAD.cpp b/gtsam_unstable/base/tests/testBAD.cpp
new file mode 100644
index 000000000..c8b9c4b9c
--- /dev/null
+++ b/gtsam_unstable/base/tests/testBAD.cpp
@@ -0,0 +1,124 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testBAD.cpp
+ * @date September 18, 2014
+ * @author Frank Dellaert
+ * @brief unit tests for Block Automatic Differentiation
+ */
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/inference/Key.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+/// This class might have to become a class hierarchy ?
+template<class T>
+class Expression {
+
+public:
+
+  /// Constructor with a single key
+  Expression(Key key) {
+  }
+
+  /// Constructor with a value, yielding a constant
+  Expression(const T& t) {
+  }
+};
+
+/// Expression version of transform
+Expression<Point3> transformTo(const Expression<Pose3>& x,
+    const Expression<Point3>& p) {
+  return Expression<Point3>(0);
+}
+
+/// Expression version of project
+Expression<Point2> project(const Expression<Point3>& p) {
+  return Expression<Point2>(0);
+}
+
+/// Expression version of uncalibrate
+Expression<Point2> uncalibrate(const Expression<Cal3_S2>& K,
+    const Expression<Point2>& p) {
+  return Expression<Point2>(0);
+}
+
+/// Expression version of Point2.sub
+Expression<Point2> operator -(const Expression<Point2>& p,
+    const Expression<Point2>& q) {
+  return Expression<Point2>(0);
+}
+
+/// AD Factor
+template<class T>
+class BADFactor: NoiseModelFactor {
+
+public:
+
+  /// Constructor
+  BADFactor(const Expression<T>& t) {
+  }
+
+  Vector unwhitenedError(const Values& x,
+      boost::optional<std::vector<Matrix>&> H = boost::none) const {
+    if (H) H->push_back(zeros(2,2));
+    return Vector();
+  }
+
+};
+
+/* ************************************************************************* */
+
+TEST(BAD, test) {
+
+  // Create leaves
+  Expression<Pose3> x(1);
+  Expression<Point3> p(2);
+  Expression<Cal3_S2> K(3);
+  Expression<Point2> uv(Point2(300, 62));
+
+  // Create expression tree
+  Expression<Point3> p_cam = transformTo(x, p);
+  Expression<Point2> projection = project(p_cam);
+  Expression<Point2> uv_hat = uncalibrate(K, projection);
+  Expression<Point2> e = uv - uv_hat;
+
+  // Create factor
+  BADFactor<Point2> f(e);
+
+  // evaluate, with derivatives
+  Values values;
+  vector<Matrix> jacobians;
+  Vector actual = f.unwhitenedError(values, jacobians);
+
+  // Check value
+  Vector expected = (Vector(2) << 0, 0);
+  EXPECT(assert_equal(expected, actual));
+
+  // Check derivatives
+  Matrix expectedHx = zeros(2,3);
+  EXPECT(assert_equal(expectedHx, jacobians[0]));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+