diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index ded96a520..076a5fc76 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -476,4 +476,64 @@ namespace gtsam { return numericalHessian(boost::function(f), x, delta); } + + /** Helper class that computes the derivative of f w.r.t. x1, centered about + * x1_, as a function of x2 + */ + template + class G_x1 { + const boost::function& f_; + const X1& x1_; + double delta_; + public: + G_x1(const boost::function& f, const X1& x1, double delta) : f_(f), x1_(x1), delta_(delta) {} + Vector operator()(const X2& x2) { + return numericalGradient(boost::function(boost::bind(f_, _1, x2)), x1_, delta_); + } + }; + + template + inline Matrix numericalHessian212(boost::function f, const X1& x1, const X2& x2, double delta=1e-5) { + G_x1 g_x1(f, x1, delta); + return numericalDerivative11(boost::function(boost::bind(boost::ref(g_x1), _1)), x2, delta); + } + + + template + inline Matrix numericalHessian212(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { + return numericalHessian212(boost::function(f), x1, x2, delta); + } + + + template + inline Matrix numericalHessian211(boost::function f, const X1& x1, const X2& x2, double delta=1e-5) { + + Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; + boost::function f2(boost::bind(f, _1, x2)); + + return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x1, delta); + } + + + template + inline Matrix numericalHessian211(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { + return numericalHessian211(boost::function(f), x1, x2, delta); + } + + + template + inline Matrix numericalHessian222(boost::function f, const X1& x1, const X2& x2, double delta=1e-5) { + + Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; + boost::function f2(boost::bind(f, x1, _1)); + + return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x2, delta); + } + + + template + inline Matrix numericalHessian222(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { + return numericalHessian222(boost::function(f), x1, x2, delta); + } + } diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index 7cdabbbe9..fae6d06c7 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -60,6 +60,31 @@ TEST_UNSAFE(testNumericalDerivative, numericalHessian2) { EXPECT(assert_equal(expected, actual, 1e-5)); } +/* ************************************************************************* */ +double f3(const LieVector& x1, const LieVector& x2) { + assert(x1.size() == 1 && x2.size() == 1); + return sin(x1(0)) * cos(x2(0)); +} + +/* ************************************************************************* */ +TEST_UNSAFE(testNumericalDerivative, numericalHessian211) { + LieVector center1(1, 1.0), center2(1, 1.0); + + Matrix expected12 = Matrix_(1,1,-sin(center1(0))*cos(center2(0))); + Matrix actual12 = numericalHessian212(f3, center1, center2); + EXPECT(assert_equal(expected12, actual12, 1e-5)); + + + Matrix expected11 = Matrix_(1,1,-cos(center1(0))*sin(center2(0))); + Matrix actual11 = numericalHessian211(f3, center1, center2); + EXPECT(assert_equal(expected11, actual11, 1e-5)); + + + Matrix expected22 = Matrix_(1,1,-sin(center1(0))*cos(center2(0))); + Matrix actual22 = numericalHessian222(f3, center1, center2); + EXPECT(assert_equal(expected22, actual22, 1e-5)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */