From 469f4614dc893af24ec0cf7bd9781b3cbd001217 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Mon, 3 Oct 2011 02:45:11 +0000 Subject: [PATCH] add numericalHessian for tenary functions --- gtsam/base/numericalDerivative.h | 82 ++++++++++++++++++++ gtsam/base/tests/testNumericalDerivative.cpp | 35 +++++++++ 2 files changed, 117 insertions(+) diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 21111881e..04bc3eb84 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -535,4 +535,86 @@ namespace gtsam { return numericalHessian222(boost::function(f), x1, x2, delta); } + /** + * Numerical Hessian for tenary functions + */ + /* **************************************************************** */ + template + inline Matrix numericalHessian311(boost::function f, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + + Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; + boost::function f2(boost::bind(f, _1, x2, x3)); + + return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x1, delta); + } + + template + inline Matrix numericalHessian311(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalHessian311(boost::function(f), x1, x2, x3, delta); + } + + /* **************************************************************** */ + template + inline Matrix numericalHessian322(boost::function f, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + + Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; + boost::function f2(boost::bind(f, x1, _1, x3)); + + return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x2, delta); + } + + template + inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalHessian322(boost::function(f), x1, x2, x3, delta); + } + + /* **************************************************************** */ + template + inline Matrix numericalHessian333(boost::function f, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + + Vector (*numGrad)(boost::function, const X3&, double) = &numericalGradient; + boost::function f2(boost::bind(f, x1, x2, _1)); + + return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x3, delta); + } + + template + inline Matrix numericalHessian333(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalHessian333(boost::function(f), x1, x2, x3, delta); + } + + /* **************************************************************** */ + template + inline Matrix numericalHessian312(boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalHessian212(boost::function(boost::bind(f, _1, _2, x3)), x1, x2, delta ); + } + + template + inline Matrix numericalHessian313(boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalHessian212(boost::function(boost::bind(f, _1, x2, _2)), x1, x3, delta ); + } + + template + inline Matrix numericalHessian323(boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalHessian212(boost::function(boost::bind(f, x1, _1, _2)), x2, x3, delta ); + } + + /* **************************************************************** */ + template + inline Matrix numericalHessian312(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalHessian312(boost::function(f), x1, x2, x3, delta); + } + + template + inline Matrix numericalHessian313(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalHessian313(boost::function(f), x1, x2, x3, delta); + } + + template + inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalHessian323(boost::function(f), x1, x2, x3, delta); + } } diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index 969163ad3..793310501 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -83,6 +83,41 @@ TEST_UNSAFE(testNumericalDerivative, numericalHessian211) { EXPECT(assert_equal(expected22, actual22, 1e-5)); } +/* ************************************************************************* */ +double f4(const LieVector& x, const LieVector& y, const LieVector& z) { + assert(x.size() == 1 && y.size() == 1 && z.size() == 1); + return sin(x(0)) * cos(y(0)) * z(0)*z(0); +} + +/* ************************************************************************* */ +TEST_UNSAFE(testNumericalDerivative, numericalHessian311) { + LieVector center1(1, 1.0), center2(1, 2.0), center3(1, 3.0); + double x = center1(0), y = center2(0), z = center3(0); + Matrix expected11 = Matrix_(1,1,-sin(x)*cos(y)*z*z); + Matrix actual11 = numericalHessian311(f4, center1, center2, center3); + EXPECT(assert_equal(expected11, actual11, 1e-5)); + + Matrix expected12 = Matrix_(1,1,-cos(x)*sin(y)*z*z); + Matrix actual12 = numericalHessian312(f4, center1, center2, center3); + EXPECT(assert_equal(expected12, actual12, 1e-5)); + + Matrix expected13 = Matrix_(1,1,cos(x)*cos(y)*2*z); + Matrix actual13 = numericalHessian313(f4, center1, center2, center3); + EXPECT(assert_equal(expected13, actual13, 1e-5)); + + Matrix expected22 = Matrix_(1,1,-sin(x)*cos(y)*z*z); + Matrix actual22 = numericalHessian322(f4, center1, center2, center3); + EXPECT(assert_equal(expected22, actual22, 1e-5)); + + Matrix expected23 = Matrix_(1,1,-sin(x)*sin(y)*2*z); + Matrix actual23 = numericalHessian323(f4, center1, center2, center3); + EXPECT(assert_equal(expected23, actual23, 1e-5)); + + Matrix expected33 = Matrix_(1,1,sin(x)*cos(y)*2); + Matrix actual33 = numericalHessian333(f4, center1, center2, center3); + EXPECT(assert_equal(expected33, actual33, 1e-5)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */