From cb999dba58f5b794513818ea5a16ba92b1d48cc3 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 8 Apr 2011 22:18:45 +0000
Subject: [PATCH] Added numericalHessian function for computing the Hessian
 matrix of a Lie->scalar function

---
 gtsam/base/Makefile.am                       |  1 +
 gtsam/base/numericalDerivative.h             | 27 ++++++--
 gtsam/base/tests/testNumericalDerivative.cpp | 65 ++++++++++++++++++++
 3 files changed, 89 insertions(+), 4 deletions(-)
 create mode 100644 gtsam/base/tests/testNumericalDerivative.cpp

diff --git a/gtsam/base/Makefile.am b/gtsam/base/Makefile.am
index 2e0a0a65d..b9da4a7f5 100644
--- a/gtsam/base/Makefile.am
+++ b/gtsam/base/Makefile.am
@@ -16,6 +16,7 @@ check_PROGRAMS =
 headers += FixedVector.h types.h blockMatrices.h Matrix-inl.h lapack.h
 sources += Vector.cpp Matrix.cpp cholesky.cpp
 check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix tests/testCholesky
+check_PROGRAMS += tests/testNumericalDerivative
 
 if USE_LAPACK
 sources += DenseQR.cpp DenseQRUtil.cpp
diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h
index e699f7cb8..96e1da05b 100644
--- a/gtsam/base/numericalDerivative.h
+++ b/gtsam/base/numericalDerivative.h
@@ -49,12 +49,12 @@ namespace gtsam {
 	 * 	    boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1)
 	 *
 	 * For additional details, see the documentation:
-	 * 		http://www.boost.org/doc/libs/1_43_0/libs/bind/bind.html
+	 * 		http://www.boost.org/doc/libs/release/libs/bind/bind.html
 	 */
 
 
 	/** global functions for converting to a LieVector for use with numericalDerivative */
-        inline LieVector makeLieVector(const Vector& v) { return LieVector(v); }
+  inline LieVector makeLieVector(const Vector& v) { return LieVector(v); }
 	inline LieVector makeLieVectorD(double d) { return LieVector(Vector_(1, d)); }
 
 	/**
@@ -68,8 +68,8 @@ namespace gtsam {
 		const size_t n = x.dim();
 		Vector d(n,0.0), g(n,0.0);
 		for (size_t j=0;j<n;j++) {
-			d(j) +=   delta; double hxplus = h(expmap(x,d));
-			d(j) -= 2*delta; double hxmin  = h(expmap(x,d));
+			d(j) +=   delta; double hxplus = h(x.expmap(d));
+			d(j) -= 2*delta; double hxmin  = h(x.expmap(d));
 			d(j) +=   delta; g(j) = (hxplus-hxmin)*factor;
 		}
 		return g;
@@ -456,4 +456,23 @@ namespace gtsam {
 				boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
 	}
 
+  /**
+   * Compute numerical Hessian matrix.  Requires a single-argument Lie->scalar
+   * function.  This is implemented simply as the derivative of the gradient.
+   * @param f A function taking a Lie object as input and returning a scalar
+   * @param x The center point for computing the Hessian
+   * @param delta The numerical derivative step size
+   * @return n*n Hessian matrix computed via central differencing
+   */
+	template<class X>
+	inline Matrix numericalHessian(const boost::function<double(const X&)>& f, const X& x, double delta=1e-5) {
+	  return numericalDerivative11<X>(
+	      boost::function<LieVector(const X&)>(boost::bind(numericalGradient<X>, f, _1, delta)), x, delta);
+	}
+
+	template<class X>
+	inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta=1e-5) {
+	  return numericalHessian(boost::function<double(const X&)>(f), x, delta);
+	}
+
 }
diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp
new file mode 100644
index 000000000..7cdabbbe9
--- /dev/null
+++ b/gtsam/base/tests/testNumericalDerivative.cpp
@@ -0,0 +1,65 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testNumericalDerivative.cpp
+ * @brief   
+ * @author  Richard Roberts
+ * @created Apr 8, 2011
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <gtsam/base/numericalDerivative.h>
+
+using namespace gtsam;
+
+/* ************************************************************************* */
+double f(const LieVector& x) {
+  assert(x.size() == 2);
+  return sin(x(0)) + cos(x(1));
+}
+
+/* ************************************************************************* */
+TEST_UNSAFE(testNumericalDerivative, numericalHessian) {
+  LieVector center(2, 1.0, 1.0);
+
+  Matrix expected = Matrix_(2,2,
+      -sin(center(0)), 0.0,
+      0.0, -cos(center(1)));
+
+  Matrix actual = numericalHessian(f, center);
+
+  EXPECT(assert_equal(expected, actual, 1e-5));
+}
+
+/* ************************************************************************* */
+double f2(const LieVector& x) {
+  assert(x.size() == 2);
+  return sin(x(0)) * cos(x(1));
+}
+
+/* ************************************************************************* */
+TEST_UNSAFE(testNumericalDerivative, numericalHessian2) {
+  LieVector center(2, 0.5, 1.0);
+
+  Matrix expected = Matrix_(2,2,
+      -cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)),
+      -cos(center(0))*sin(center(1)), -sin(center(0))*cos(center(1)));
+
+  Matrix actual = numericalHessian(f2, center);
+
+  EXPECT(assert_equal(expected, actual, 1e-5));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */