626 lines
27 KiB
C++
626 lines
27 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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 numericalDerivative.h
|
|
* @brief Some functions to compute numerical derivatives
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
// \callgraph
|
|
|
|
#pragma once
|
|
|
|
#include <boost/function.hpp>
|
|
#ifdef __GNUC__
|
|
#pragma GCC diagnostic push
|
|
#pragma GCC diagnostic ignored "-Wunused-variable"
|
|
#endif
|
|
#include <boost/bind.hpp>
|
|
#ifdef __GNUC__
|
|
#pragma GCC diagnostic pop
|
|
#endif
|
|
|
|
#include <gtsam/base/LieVector.h>
|
|
#include <gtsam/base/Matrix.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/*
|
|
* Note that all of these functions have two versions, a boost.function version and a
|
|
* standard C++ function pointer version. This allows reformulating the arguments of
|
|
* a function to fit the correct structure, which is useful for situations like
|
|
* member functions and functions with arguments not involved in the derivative:
|
|
*
|
|
* Usage of the boost bind version to rearrange arguments:
|
|
* for a function with one relevant param and an optional derivative:
|
|
* Foo bar(const Obj& a, boost::optional<Matrix&> H1)
|
|
* Use boost.bind to restructure:
|
|
* boost::bind(bar, _1, boost::none)
|
|
* This syntax will fix the optional argument to boost::none, while using the first argument provided
|
|
*
|
|
* For member functions, such as below, with an instantiated copy instanceOfSomeClass
|
|
* Foo SomeClass::bar(const Obj& a)
|
|
* Use boost bind as follows to create a function pointer that uses the member function:
|
|
* boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1)
|
|
*
|
|
* For additional details, see the documentation:
|
|
* 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 makeLieVectorD(double d) { return LieVector((Vector) (Vector(1) << d)); }
|
|
|
|
/**
|
|
* Numerically compute gradient of scalar function
|
|
* Class X is the input argument
|
|
* The class X needs to have dim, expmap, logmap
|
|
*/
|
|
template<class X>
|
|
Vector numericalGradient(boost::function<double(const X&)> h, const X& x, double delta=1e-5) {
|
|
double factor = 1.0/(2.0*delta);
|
|
const size_t n = x.dim();
|
|
Vector d = zero(n), g = zero(n);
|
|
for (size_t j=0;j<n;j++) {
|
|
d(j) += delta; double hxplus = h(x.retract(d));
|
|
d(j) -= 2*delta; double hxmin = h(x.retract(d));
|
|
d(j) += delta; g(j) = (hxplus-hxmin)*factor;
|
|
}
|
|
return g;
|
|
}
|
|
|
|
template<class X>
|
|
Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) {
|
|
return numericalGradient<X>(boost::bind(h, _1), x, delta);
|
|
}
|
|
|
|
/**
|
|
* Compute numerical derivative in argument 1 of unary function
|
|
* @param h unary function yielding m-vector
|
|
* @param x n-dimensional value at which to evaluate h
|
|
* @param delta increment for numerical derivative
|
|
* Class Y is the output argument
|
|
* Class X is the input argument
|
|
* @return m*n Jacobian computed via central differencing
|
|
* Both classes X,Y need dim, expmap, logmap
|
|
*/
|
|
template<class Y, class X>
|
|
Matrix numericalDerivative11(boost::function<Y(const X&)> h, const X& x, double delta=1e-5) {
|
|
Y hx = h(x);
|
|
double factor = 1.0/(2.0*delta);
|
|
const size_t m = hx.dim(), n = x.dim();
|
|
Vector d = zero(n);
|
|
Matrix H = zeros(m,n);
|
|
for (size_t j=0;j<n;j++) {
|
|
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x.retract(d)));
|
|
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x.retract(d)));
|
|
d(j) += delta;
|
|
H.col(j) << (hxplus-hxmin)*factor;
|
|
}
|
|
return H;
|
|
}
|
|
|
|
/** use a raw C++ function pointer */
|
|
template<class Y, class X>
|
|
Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) {
|
|
return numericalDerivative11<Y,X>(boost::bind(h, _1), x, delta);
|
|
}
|
|
|
|
// /** remapping for double valued functions */
|
|
// template<class X>
|
|
// Matrix numericalDerivative11(boost::function<double(const X&)> h, const X& x, double delta=1e-5) {
|
|
// return numericalDerivative11<LieVector, X>(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta);
|
|
// }
|
|
|
|
template<class X>
|
|
Matrix numericalDerivative11(double (*h)(const X&), const X& x, double delta=1e-5) {
|
|
return numericalDerivative11<LieVector, X>(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta);
|
|
}
|
|
|
|
/** remapping for vector valued functions */
|
|
template<class X>
|
|
Matrix numericalDerivative11(boost::function<Vector(const X&)> h, const X& x, double delta=1e-5) {
|
|
return numericalDerivative11<LieVector, X>(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta);
|
|
}
|
|
|
|
template<class X>
|
|
Matrix numericalDerivative11(Vector (*h)(const X&), const X& x, double delta=1e-5) {
|
|
return numericalDerivative11<LieVector, X>(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta);
|
|
}
|
|
|
|
/**
|
|
* Compute numerical derivative in argument 1 of binary function
|
|
* @param h binary function yielding m-vector
|
|
* @param x1 n-dimensional first argument value
|
|
* @param x2 second argument value
|
|
* @param delta increment for numerical derivative
|
|
* @return m*n Jacobian computed via central differencing
|
|
* All classes Y,X1,X2 need dim, expmap, logmap
|
|
*/
|
|
template<class Y, class X1, class X2>
|
|
Matrix numericalDerivative21(boost::function<Y(const X1&, const X2&)> h,
|
|
const X1& x1, const X2& x2, double delta=1e-5) {
|
|
Y hx = h(x1,x2);
|
|
double factor = 1.0/(2.0*delta);
|
|
const size_t m = hx.dim(), n = x1.dim();
|
|
Vector d = zero(n);
|
|
Matrix H = zeros(m,n);
|
|
for (size_t j=0;j<n;j++) {
|
|
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1.retract(d),x2));
|
|
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1.retract(d),x2));
|
|
d(j) += delta;
|
|
H.col(j) << (hxplus-hxmin)*factor;
|
|
}
|
|
return H;
|
|
}
|
|
|
|
/** use a raw C++ function pointer */
|
|
template<class Y, class X1, class X2>
|
|
inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&),
|
|
const X1& x1, const X2& x2, double delta=1e-5) {
|
|
return numericalDerivative21<Y,X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
|
|
}
|
|
|
|
/** pseudo-partial template specialization for double return values */
|
|
template<class X1, class X2>
|
|
Matrix numericalDerivative21(boost::function<double(const X1&, const X2&)> h,
|
|
const X1& x1, const X2& x2, double delta=1e-5) {
|
|
return numericalDerivative21<LieVector,X1,X2>(
|
|
boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
|
|
}
|
|
|
|
template<class X1, class X2>
|
|
Matrix numericalDerivative21(double (*h)(const X1&, const X2&),
|
|
const X1& x1, const X2& x2, double delta=1e-5) {
|
|
return numericalDerivative21<LieVector,X1,X2>(
|
|
boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
|
|
}
|
|
|
|
/** pseudo-partial template specialization for vector return values */
|
|
template<class X1, class X2>
|
|
Matrix numericalDerivative21(boost::function<Vector(const X1&, const X2&)> h,
|
|
const X1& x1, const X2& x2, double delta=1e-5) {
|
|
return numericalDerivative21<LieVector,X1,X2>(
|
|
boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
|
|
}
|
|
|
|
template<class X1, class X2>
|
|
inline Matrix numericalDerivative21(Vector (*h)(const X1&, const X2&),
|
|
const X1& x1, const X2& x2, double delta=1e-5) {
|
|
return numericalDerivative21<LieVector,X1,X2>(
|
|
boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
|
|
}
|
|
|
|
/**
|
|
* Compute numerical derivative in argument 2 of binary function
|
|
* @param h binary function yielding m-vector
|
|
* @param x1 first argument value
|
|
* @param x2 n-dimensional second argument value
|
|
* @param delta increment for numerical derivative
|
|
* @return m*n Jacobian computed via central differencing
|
|
* All classes Y,X1,X2 need dim, expmap, logmap
|
|
*/
|
|
template<class Y, class X1, class X2>
|
|
Matrix numericalDerivative22
|
|
(boost::function<Y(const X1&, const X2&)> h,
|
|
const X1& x1, const X2& x2, double delta=1e-5) {
|
|
Y hx = h(x1,x2);
|
|
double factor = 1.0/(2.0*delta);
|
|
const size_t m = hx.dim(), n = x2.dim();
|
|
Vector d = zero(n);
|
|
Matrix H = zeros(m,n);
|
|
for (size_t j=0;j<n;j++) {
|
|
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1,x2.retract(d)));
|
|
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1,x2.retract(d)));
|
|
d(j) += delta;
|
|
H.col(j) << (hxplus-hxmin)*factor;
|
|
}
|
|
return H;
|
|
}
|
|
|
|
/** use a raw C++ function pointer */
|
|
template<class Y, class X1, class X2>
|
|
inline Matrix numericalDerivative22
|
|
(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) {
|
|
return numericalDerivative22<Y,X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
|
|
}
|
|
|
|
/** pseudo-partial template specialization for double return values */
|
|
template<class X1, class X2>
|
|
Matrix numericalDerivative22(boost::function<double(const X1&, const X2&)> h,
|
|
const X1& x1, const X2& x2, double delta=1e-5) {
|
|
return numericalDerivative22<LieVector,X1,X2>(
|
|
boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
|
|
}
|
|
|
|
template<class X1, class X2>
|
|
inline Matrix numericalDerivative22(double (*h)(const X1&, const X2&),
|
|
const X1& x1, const X2& x2, double delta=1e-5) {
|
|
return numericalDerivative22<LieVector,X1,X2>(
|
|
boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
|
|
}
|
|
|
|
/** pseudo-partial template specialization for vector return values */
|
|
template<class X1, class X2>
|
|
Matrix numericalDerivative22(boost::function<Vector(const X1&, const X2&)> h,
|
|
const X1& x1, const X2& x2, double delta=1e-5) {
|
|
return numericalDerivative22<LieVector,X1,X2>(
|
|
boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
|
|
}
|
|
|
|
template<class X1, class X2>
|
|
inline Matrix numericalDerivative22(Vector (*h)(const X1&, const X2&),
|
|
const X1& x1, const X2& x2, double delta=1e-5) {
|
|
return numericalDerivative22<LieVector,X1,X2>(
|
|
boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
|
|
}
|
|
|
|
/**
|
|
* Compute numerical derivative in argument 1 of ternary function
|
|
* @param h ternary function yielding m-vector
|
|
* @param x1 n-dimensional first argument value
|
|
* @param x2 second argument value
|
|
* @param x3 third argument value
|
|
* @param delta increment for numerical derivative
|
|
* @return m*n Jacobian computed via central differencing
|
|
* All classes Y,X1,X2,X3 need dim, expmap, logmap
|
|
*/
|
|
template<class Y, class X1, class X2, class X3>
|
|
Matrix numericalDerivative31
|
|
(boost::function<Y(const X1&, const X2&, const X3&)> h,
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5)
|
|
{
|
|
Y hx = h(x1,x2,x3);
|
|
double factor = 1.0/(2.0*delta);
|
|
const size_t m = hx.dim(), n = x1.dim();
|
|
Vector d = zero(n);
|
|
Matrix H = zeros(m,n);
|
|
for (size_t j=0;j<n;j++) {
|
|
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1.retract(d),x2,x3));
|
|
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1.retract(d),x2,x3));
|
|
d(j) += delta;
|
|
H.col(j) << (hxplus-hxmin)*factor;
|
|
}
|
|
return H;
|
|
}
|
|
template<class Y, class X1, class X2, class X3>
|
|
inline Matrix numericalDerivative31
|
|
(Y (*h)(const X1&, const X2&, const X3&),
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalDerivative31<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
|
|
}
|
|
|
|
/** pseudo-partial template specialization for double return values */
|
|
template<class X1, class X2, class X3>
|
|
Matrix numericalDerivative31(boost::function<double(const X1&, const X2&, const X3&)> h,
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalDerivative31<LieVector,X1,X2,X3>(
|
|
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
|
}
|
|
|
|
template<class X1, class X2, class X3>
|
|
inline Matrix numericalDerivative31(double (*h)(const X1&, const X2&, const X3&),
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalDerivative31<LieVector,X1,X2,X3>(
|
|
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
|
}
|
|
|
|
/** pseudo-partial template specialization for vector return values */
|
|
template<class X1, class X2, class X3>
|
|
Matrix numericalDerivative31(boost::function<Vector(const X1&, const X2&, const X3&)> h,
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalDerivative31<LieVector,X1,X2,X3>(
|
|
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
|
}
|
|
|
|
template<class X1, class X2, class X3>
|
|
inline Matrix numericalDerivative31(Vector (*h)(const X1&, const X2&, const X3&),
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalDerivative31<LieVector,X1,X2,X3>(
|
|
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
|
}
|
|
|
|
/**
|
|
* Compute numerical derivative in argument 2 of ternary function
|
|
* @param h ternary function yielding m-vector
|
|
* @param x1 n-dimensional first argument value
|
|
* @param x2 second argument value
|
|
* @param x3 third argument value
|
|
* @param delta increment for numerical derivative
|
|
* @return m*n Jacobian computed via central differencing
|
|
* All classes Y,X1,X2,X3 need dim, expmap, logmap
|
|
*/
|
|
template<class Y, class X1, class X2, class X3>
|
|
Matrix numericalDerivative32
|
|
(boost::function<Y(const X1&, const X2&, const X3&)> h,
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5)
|
|
{
|
|
Y hx = h(x1,x2,x3);
|
|
double factor = 1.0/(2.0*delta);
|
|
const size_t m = hx.dim(), n = x2.dim();
|
|
Vector d = zero(n);
|
|
Matrix H = zeros(m,n);
|
|
for (size_t j=0;j<n;j++) {
|
|
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1, x2.retract(d),x3));
|
|
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1, x2.retract(d),x3));
|
|
d(j) += delta;
|
|
H.col(j) << (hxplus-hxmin)*factor;
|
|
}
|
|
return H;
|
|
}
|
|
template<class Y, class X1, class X2, class X3>
|
|
inline Matrix numericalDerivative32
|
|
(Y (*h)(const X1&, const X2&, const X3&),
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalDerivative32<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
|
|
}
|
|
|
|
/** pseudo-partial template specialization for double return values */
|
|
template<class X1, class X2, class X3>
|
|
Matrix numericalDerivative32(boost::function<double(const X1&, const X2&, const X3&)> h,
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalDerivative32<LieVector,X1,X2,X3>(
|
|
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
|
}
|
|
|
|
template<class X1, class X2, class X3>
|
|
inline Matrix numericalDerivative32(double (*h)(const X1&, const X2&, const X3&),
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalDerivative32<LieVector,X1,X2,X3>(
|
|
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
|
}
|
|
|
|
/** pseudo-partial template specialization for vector return values */
|
|
template<class X1, class X2, class X3>
|
|
Matrix numericalDerivative32(boost::function<Vector(const X1&, const X2&, const X3&)> h,
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalDerivative32<LieVector,X1,X2,X3>(
|
|
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
|
}
|
|
|
|
template<class X1, class X2, class X3>
|
|
inline Matrix numericalDerivative32(Vector (*h)(const X1&, const X2&, const X3&),
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalDerivative32<LieVector,X1,X2,X3>(
|
|
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
|
}
|
|
|
|
/**
|
|
* Compute numerical derivative in argument 3 of ternary function
|
|
* @param h ternary function yielding m-vector
|
|
* @param x1 n-dimensional first argument value
|
|
* @param x2 second argument value
|
|
* @param x3 third argument value
|
|
* @param delta increment for numerical derivative
|
|
* @return m*n Jacobian computed via central differencing
|
|
* All classes Y,X1,X2,X3 need dim, expmap, logmap
|
|
*/
|
|
template<class Y, class X1, class X2, class X3>
|
|
Matrix numericalDerivative33
|
|
(boost::function<Y(const X1&, const X2&, const X3&)> h,
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5)
|
|
{
|
|
Y hx = h(x1,x2,x3);
|
|
double factor = 1.0/(2.0*delta);
|
|
const size_t m = hx.dim(), n = x3.dim();
|
|
Vector d = zero(n);
|
|
Matrix H = zeros(m,n);
|
|
for (size_t j=0;j<n;j++) {
|
|
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1, x2, x3.retract(d)));
|
|
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1, x2, x3.retract(d)));
|
|
d(j) += delta;
|
|
H.col(j) << (hxplus-hxmin)*factor;
|
|
}
|
|
return H;
|
|
}
|
|
template<class Y, class X1, class X2, class X3>
|
|
inline Matrix numericalDerivative33
|
|
(Y (*h)(const X1&, const X2&, const X3&),
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalDerivative33<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
|
|
}
|
|
|
|
/** pseudo-partial template specialization for double return values */
|
|
template<class X1, class X2, class X3>
|
|
Matrix numericalDerivative33(boost::function<double(const X1&, const X2&, const X3&)> h,
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalDerivative33<LieVector,X1,X2,X3>(
|
|
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
|
}
|
|
|
|
template<class X1, class X2, class X3>
|
|
inline Matrix numericalDerivative33(double (*h)(const X1&, const X2&, const X3&),
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalDerivative33<LieVector,X1,X2,X3>(
|
|
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
|
}
|
|
|
|
/** pseudo-partial template specialization for vector return values */
|
|
template<class X1, class X2, class X3>
|
|
Matrix numericalDerivative33(boost::function<Vector(const X1&, const X2&, const X3&)> h,
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalDerivative33<LieVector,X1,X2,X3>(
|
|
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
|
}
|
|
|
|
template<class X1, class X2, class X3>
|
|
inline Matrix numericalDerivative33(Vector (*h)(const X1&, const X2&, const X3&),
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalDerivative33<LieVector,X1,X2,X3>(
|
|
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(boost::function<double(const X&)> f, const X& x, double delta=1e-5) {
|
|
return numericalDerivative11<X>(boost::function<Vector(const X&)>(boost::bind(
|
|
static_cast<Vector (*)(boost::function<double(const X&)>,const X&, double)>(&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);
|
|
}
|
|
|
|
|
|
/** Helper class that computes the derivative of f w.r.t. x1, centered about
|
|
* x1_, as a function of x2
|
|
*/
|
|
template<class X1, class X2>
|
|
class G_x1 {
|
|
const boost::function<double(const X1&, const X2&)>& f_;
|
|
const X1& x1_;
|
|
double delta_;
|
|
public:
|
|
G_x1(const boost::function<double(const X1&, const X2&)>& f, const X1& x1, double delta) : f_(f), x1_(x1), delta_(delta) {}
|
|
Vector operator()(const X2& x2) {
|
|
return numericalGradient<X1>(boost::function<double (const X1&)>(boost::bind(f_, _1, x2)), x1_, delta_);
|
|
}
|
|
};
|
|
|
|
template<class X1, class X2>
|
|
inline Matrix numericalHessian212(boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2, double delta=1e-5) {
|
|
G_x1<X1,X2> g_x1(f, x1, delta);
|
|
return numericalDerivative11<X2>(boost::function<Vector (const X2&)>(boost::bind<Vector>(boost::ref(g_x1), _1)), x2, delta);
|
|
}
|
|
|
|
|
|
template<class X1, class X2>
|
|
inline Matrix numericalHessian212(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) {
|
|
return numericalHessian212(boost::function<double (const X1&, const X2&)>(f), x1, x2, delta);
|
|
}
|
|
|
|
|
|
template<class X1, class X2>
|
|
inline Matrix numericalHessian211(boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2, double delta=1e-5) {
|
|
|
|
Vector (*numGrad)(boost::function<double(const X1&)>, const X1&, double) = &numericalGradient<X1>;
|
|
boost::function<double (const X1&)> f2(boost::bind(f, _1, x2));
|
|
|
|
return numericalDerivative11<X1>(boost::function<Vector (const X1&)>(boost::bind(numGrad, f2, _1, delta)), x1, delta);
|
|
}
|
|
|
|
|
|
template<class X1, class X2>
|
|
inline Matrix numericalHessian211(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) {
|
|
return numericalHessian211(boost::function<double (const X1&, const X2&)>(f), x1, x2, delta);
|
|
}
|
|
|
|
|
|
template<class X1, class X2>
|
|
inline Matrix numericalHessian222(boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2, double delta=1e-5) {
|
|
|
|
Vector (*numGrad)(boost::function<double(const X2&)>, const X2&, double) = &numericalGradient<X2>;
|
|
boost::function<double (const X2&)> f2(boost::bind(f, x1, _1));
|
|
|
|
return numericalDerivative11<X2>(boost::function<Vector (const X2&)>(boost::bind(numGrad, f2, _1, delta)), x2, delta);
|
|
}
|
|
|
|
|
|
template<class X1, class X2>
|
|
inline Matrix numericalHessian222(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) {
|
|
return numericalHessian222(boost::function<double (const X1&, const X2&)>(f), x1, x2, delta);
|
|
}
|
|
|
|
/**
|
|
* Numerical Hessian for tenary functions
|
|
*/
|
|
/* **************************************************************** */
|
|
template<class X1, class X2, class X3>
|
|
inline Matrix numericalHessian311(boost::function<double(const X1&, const X2&, const X3&)> f,
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
|
|
Vector (*numGrad)(boost::function<double(const X1&)>, const X1&, double) = &numericalGradient<X1>;
|
|
boost::function<double (const X1&)> f2(boost::bind(f, _1, x2, x3));
|
|
|
|
return numericalDerivative11<X1>(boost::function<Vector (const X1&)>(boost::bind(numGrad, f2, _1, delta)), x1, delta);
|
|
}
|
|
|
|
template<class X1, class X2, class X3>
|
|
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<double (const X1&, const X2&, const X3&)>(f), x1, x2, x3, delta);
|
|
}
|
|
|
|
/* **************************************************************** */
|
|
template<class X1, class X2, class X3>
|
|
inline Matrix numericalHessian322(boost::function<double(const X1&, const X2&, const X3&)> f,
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
|
|
Vector (*numGrad)(boost::function<double(const X2&)>, const X2&, double) = &numericalGradient<X2>;
|
|
boost::function<double (const X2&)> f2(boost::bind(f, x1, _1, x3));
|
|
|
|
return numericalDerivative11<X2>(boost::function<Vector (const X2&)>(boost::bind(numGrad, f2, _1, delta)), x2, delta);
|
|
}
|
|
|
|
template<class X1, class X2, class X3>
|
|
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<double (const X1&, const X2&, const X3&)>(f), x1, x2, x3, delta);
|
|
}
|
|
|
|
/* **************************************************************** */
|
|
template<class X1, class X2, class X3>
|
|
inline Matrix numericalHessian333(boost::function<double(const X1&, const X2&, const X3&)> f,
|
|
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
|
|
Vector (*numGrad)(boost::function<double(const X3&)>, const X3&, double) = &numericalGradient<X3>;
|
|
boost::function<double (const X3&)> f2(boost::bind(f, x1, x2, _1));
|
|
|
|
return numericalDerivative11<X3>(boost::function<Vector (const X3&)>(boost::bind(numGrad, f2, _1, delta)), x3, delta);
|
|
}
|
|
|
|
template<class X1, class X2, class X3>
|
|
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<double (const X1&, const X2&, const X3&)>(f), x1, x2, x3, delta);
|
|
}
|
|
|
|
/* **************************************************************** */
|
|
template<class X1, class X2, class X3>
|
|
inline Matrix numericalHessian312(boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalHessian212<X1,X2>(boost::function<double (const X1&, const X2&)>(boost::bind(f, _1, _2, x3)), x1, x2, delta );
|
|
}
|
|
|
|
template<class X1, class X2, class X3>
|
|
inline Matrix numericalHessian313(boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalHessian212<X1,X3>(boost::function<double (const X1&, const X3&)>(boost::bind(f, _1, x2, _2)), x1, x3, delta );
|
|
}
|
|
|
|
template<class X1, class X2, class X3>
|
|
inline Matrix numericalHessian323(boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
|
return numericalHessian212<X2,X3>(boost::function<double (const X2&, const X3&)>(boost::bind(f, x1, _1, _2)), x2, x3, delta );
|
|
}
|
|
|
|
/* **************************************************************** */
|
|
template<class X1, class X2, class X3>
|
|
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<double (const X1&, const X2&, const X3&)>(f), x1, x2, x3, delta);
|
|
}
|
|
|
|
template<class X1, class X2, class X3>
|
|
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<double (const X1&, const X2&, const X3&)>(f), x1, x2, x3, delta);
|
|
}
|
|
|
|
template<class X1, class X2, class X3>
|
|
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<double (const X1&, const X2&, const X3&)>(f), x1, x2, x3, delta);
|
|
}
|
|
}
|