Added boost.function versions of numericalDerivative functions to allow for remapping of function arguments and testing of member functions using boost.bind. See note in numericalDerivative.h for details and examples.
Alex Cunningham
parent
55e4e791cb
commit
195fa64178
|
|
@ -8,6 +8,9 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <boost/function.hpp>
|
||||||
|
#include <boost/bind.hpp>
|
||||||
|
|
||||||
#include "Lie.h"
|
#include "Lie.h"
|
||||||
#include "Matrix.h"
|
#include "Matrix.h"
|
||||||
|
|
||||||
|
|
@ -15,169 +18,232 @@
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/**
|
/*
|
||||||
* Numerically compute gradient of scalar function
|
* Note that all of these functions have two versions, a boost.function version and a
|
||||||
* Class X is the input argument
|
* standard C++ function pointer version. This allows reformulating the arguments of
|
||||||
* The class X needs to have dim, expmap, logmap
|
* a function to fit the correct structure, which is useful for situations like
|
||||||
*/
|
* member functions and functions with arguments not involved in the derivative:
|
||||||
template<class X>
|
*
|
||||||
Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) {
|
* Usage of the boost bind version to rearrange arguments:
|
||||||
double factor = 1.0/(2.0*delta);
|
* for a function with one relevant param and an optional derivative:
|
||||||
const size_t n = x.dim();
|
* Foo bar(const Obj& a, boost::optional<Matrix&> H1)
|
||||||
Vector d(n,0.0), g(n,0.0);
|
* Use boost.bind to restructure:
|
||||||
for (size_t j=0;j<n;j++) {
|
* boost::bind(bar, _1, boost::none)
|
||||||
d(j) += delta; double hxplus = h(expmap(x,d));
|
* This syntax will fix the optional argument to boost::none, while using the first argument provided
|
||||||
d(j) -= 2*delta; double hxmin = h(expmap(x,d));
|
*
|
||||||
d(j) += delta; g(j) = (hxplus-hxmin)*factor;
|
* For member functions, such as below, with an instantiated copy instanceOfSomeClass
|
||||||
}
|
* Foo SomeClass::bar(const Obj& a)
|
||||||
return g;
|
* 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/1_43_0/libs/bind/bind.html
|
||||||
|
*/
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Compute numerical derivative in argument 1 of unary function
|
* Numerically compute gradient of scalar function
|
||||||
* @param h unary function yielding m-vector
|
* Class X is the input argument
|
||||||
* @param x n-dimensional value at which to evaluate h
|
* The class X needs to have dim, expmap, logmap
|
||||||
* @param delta increment for numerical derivative
|
*/
|
||||||
* Class Y is the output argument
|
template<class X>
|
||||||
* Class X is the input argument
|
Vector numericalGradient(boost::function<double(const X&)> h, const X& x, double delta=1e-5) {
|
||||||
* @return m*n Jacobian computed via central differencing
|
double factor = 1.0/(2.0*delta);
|
||||||
* Both classes X,Y need dim, expmap, logmap
|
const size_t n = x.dim();
|
||||||
*/
|
Vector d(n,0.0), g(n,0.0);
|
||||||
template<class Y, class X>
|
for (size_t j=0;j<n;j++) {
|
||||||
Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) {
|
d(j) += delta; double hxplus = h(expmap(x,d));
|
||||||
Y hx = h(x);
|
d(j) -= 2*delta; double hxmin = h(expmap(x,d));
|
||||||
double factor = 1.0/(2.0*delta);
|
d(j) += delta; g(j) = (hxplus-hxmin)*factor;
|
||||||
const size_t m = dim(hx), n = dim(x);
|
}
|
||||||
Vector d(n,0.0);
|
return g;
|
||||||
Matrix H = zeros(m,n);
|
}
|
||||||
for (size_t j=0;j<n;j++) {
|
|
||||||
d(j) += delta; Vector hxplus = logmap(hx, h(expmap(x,d)));
|
|
||||||
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(expmap(x,d)));
|
|
||||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
|
||||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
|
||||||
}
|
|
||||||
return H;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
template<class X>
|
||||||
* Compute numerical derivative in argument 1 of binary function
|
Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) {
|
||||||
* @param h binary function yielding m-vector
|
return numericalGradient<X>(boost::bind(h, _1), x, delta);
|
||||||
* @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(Y (*h)(const X1&, const X2&),
|
|
||||||
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 = dim(hx), n = dim(x1);
|
|
||||||
Vector d(n,0.0);
|
|
||||||
Matrix H = zeros(m,n);
|
|
||||||
for (size_t j=0;j<n;j++) {
|
|
||||||
d(j) += delta; Vector hxplus = logmap(hx, h(expmap(x1,d),x2));
|
|
||||||
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(expmap(x1,d),x2));
|
|
||||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
|
||||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
|
||||||
}
|
|
||||||
return H;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Compute numerical derivative in argument 2 of binary function
|
* Compute numerical derivative in argument 1 of unary function
|
||||||
* @param h binary function yielding m-vector
|
* @param h unary function yielding m-vector
|
||||||
* @param x1 first argument value
|
* @param x n-dimensional value at which to evaluate h
|
||||||
* @param x2 n-dimensional second argument value
|
* @param delta increment for numerical derivative
|
||||||
* @param delta increment for numerical derivative
|
* Class Y is the output argument
|
||||||
* @return m*n Jacobian computed via central differencing
|
* Class X is the input argument
|
||||||
* All classes Y,X1,X2 need dim, expmap, logmap
|
* @return m*n Jacobian computed via central differencing
|
||||||
*/
|
* Both classes X,Y need dim, expmap, logmap
|
||||||
template<class Y, class X1, class X2>
|
*/
|
||||||
Matrix numericalDerivative22
|
template<class Y, class X>
|
||||||
(Y (*h)(const X1&, const X2&),
|
Matrix numericalDerivative11(boost::function<Y(const X&)> h, const X& x, double delta=1e-5) {
|
||||||
const X1& x1, const X2& x2, double delta=1e-5)
|
Y hx = h(x);
|
||||||
{
|
double factor = 1.0/(2.0*delta);
|
||||||
Y hx = h(x1,x2);
|
const size_t m = dim(hx), n = dim(x);
|
||||||
double factor = 1.0/(2.0*delta);
|
Vector d(n,0.0);
|
||||||
const size_t m = dim(hx), n = dim(x2);
|
Matrix H = zeros(m,n);
|
||||||
Vector d(n,0.0);
|
for (size_t j=0;j<n;j++) {
|
||||||
Matrix H = zeros(m,n);
|
d(j) += delta; Vector hxplus = logmap(hx, h(expmap(x,d)));
|
||||||
for (size_t j=0;j<n;j++) {
|
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(expmap(x,d)));
|
||||||
d(j) += delta; Vector hxplus = logmap(hx, h(x1,expmap(x2,d)));
|
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||||
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(x1,expmap(x2,d)));
|
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
}
|
||||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
return H;
|
||||||
}
|
}
|
||||||
return H;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
template<class Y, class X>
|
||||||
* Compute numerical derivative in argument 1 of binary function
|
Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) {
|
||||||
* @param h binary function yielding m-vector
|
return numericalDerivative11<Y,X>(boost::bind(h, _1), x, delta);
|
||||||
* @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,X3 need dim, expmap, logmap
|
|
||||||
*/
|
|
||||||
template<class Y, class X1, class X2, class X3>
|
|
||||||
Matrix numericalDerivative31
|
|
||||||
(Y (*h)(const X1&, const X2&, const X3&),
|
|
||||||
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 = dim(hx), n = dim(x1);
|
|
||||||
Vector d(n,0.0);
|
|
||||||
Matrix H = zeros(m,n);
|
|
||||||
for (size_t j=0;j<n;j++) {
|
|
||||||
d(j) += delta; Vector hxplus = logmap(hx, h(expmap(x1,d),x2,x3));
|
|
||||||
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(expmap(x1,d),x2,x3));
|
|
||||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
|
||||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
|
||||||
}
|
|
||||||
return H;
|
|
||||||
}
|
|
||||||
|
|
||||||
// arg 2
|
/**
|
||||||
template<class Y, class X1, class X2, class X3>
|
* Compute numerical derivative in argument 1 of binary function
|
||||||
Matrix numericalDerivative32
|
* @param h binary function yielding m-vector
|
||||||
(Y (*h)(const X1&, const X2&, const X3&),
|
* @param x1 n-dimensional first argument value
|
||||||
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5)
|
* @param x2 second argument value
|
||||||
{
|
* @param delta increment for numerical derivative
|
||||||
Y hx = h(x1,x2,x3);
|
* @return m*n Jacobian computed via central differencing
|
||||||
double factor = 1.0/(2.0*delta);
|
* All classes Y,X1,X2 need dim, expmap, logmap
|
||||||
const size_t m = dim(hx), n = dim(x2);
|
*/
|
||||||
Vector d(n,0.0);
|
template<class Y, class X1, class X2>
|
||||||
Matrix H = zeros(m,n);
|
Matrix numericalDerivative21(boost::function<Y(const X1&, const X2&)> h,
|
||||||
for (size_t j=0;j<n;j++) {
|
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||||
d(j) += delta; Vector hxplus = logmap(hx, h(x1, expmap(x2,d),x3));
|
Y hx = h(x1,x2);
|
||||||
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(x1, expmap(x2,d),x3));
|
double factor = 1.0/(2.0*delta);
|
||||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
const size_t m = dim(hx), n = dim(x1);
|
||||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
Vector d(n,0.0);
|
||||||
}
|
Matrix H = zeros(m,n);
|
||||||
return H;
|
for (size_t j=0;j<n;j++) {
|
||||||
}
|
d(j) += delta; Vector hxplus = logmap(hx, h(expmap(x1,d),x2));
|
||||||
|
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(expmap(x1,d),x2));
|
||||||
|
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||||
|
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||||
|
}
|
||||||
|
return H;
|
||||||
|
}
|
||||||
|
|
||||||
// arg 3
|
template<class Y, class X1, class X2>
|
||||||
template<class Y, class X1, class X2, class X3>
|
Matrix numericalDerivative21(Y (*h)(const X1&, const X2&),
|
||||||
Matrix numericalDerivative33
|
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||||
(Y (*h)(const X1&, const X2&, const X3&),
|
return numericalDerivative21<Y,X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
|
||||||
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);
|
* Compute numerical derivative in argument 2 of binary function
|
||||||
const size_t m = dim(hx), n = dim(x3);
|
* @param h binary function yielding m-vector
|
||||||
Vector d(n,0.0);
|
* @param x1 first argument value
|
||||||
Matrix H = zeros(m,n);
|
* @param x2 n-dimensional second argument value
|
||||||
for (size_t j=0;j<n;j++) {
|
* @param delta increment for numerical derivative
|
||||||
d(j) += delta; Vector hxplus = logmap(hx, h(x1, x2, expmap(x3,d)));
|
* @return m*n Jacobian computed via central differencing
|
||||||
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(x1, x2, expmap(x3,d)));
|
* All classes Y,X1,X2 need dim, expmap, logmap
|
||||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
*/
|
||||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
template<class Y, class X1, class X2>
|
||||||
}
|
Matrix numericalDerivative22
|
||||||
return H;
|
(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 = dim(hx), n = dim(x2);
|
||||||
|
Vector d(n,0.0);
|
||||||
|
Matrix H = zeros(m,n);
|
||||||
|
for (size_t j=0;j<n;j++) {
|
||||||
|
d(j) += delta; Vector hxplus = logmap(hx, h(x1,expmap(x2,d)));
|
||||||
|
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(x1,expmap(x2,d)));
|
||||||
|
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||||
|
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||||
|
}
|
||||||
|
return H;
|
||||||
|
}
|
||||||
|
template<class Y, class X1, class X2>
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* 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 = dim(hx), n = dim(x1);
|
||||||
|
Vector d(n,0.0);
|
||||||
|
Matrix H = zeros(m,n);
|
||||||
|
for (size_t j=0;j<n;j++) {
|
||||||
|
d(j) += delta; Vector hxplus = logmap(hx, h(expmap(x1,d),x2,x3));
|
||||||
|
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(expmap(x1,d),x2,x3));
|
||||||
|
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||||
|
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||||
|
}
|
||||||
|
return H;
|
||||||
|
}
|
||||||
|
template<class Y, class X1, class X2, class X3>
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
// arg 2
|
||||||
|
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 = dim(hx), n = dim(x2);
|
||||||
|
Vector d(n,0.0);
|
||||||
|
Matrix H = zeros(m,n);
|
||||||
|
for (size_t j=0;j<n;j++) {
|
||||||
|
d(j) += delta; Vector hxplus = logmap(hx, h(x1, expmap(x2,d),x3));
|
||||||
|
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(x1, expmap(x2,d),x3));
|
||||||
|
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||||
|
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||||
|
}
|
||||||
|
return H;
|
||||||
|
}
|
||||||
|
template<class Y, class X1, class X2, class X3>
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
// arg 3
|
||||||
|
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 = dim(hx), n = dim(x3);
|
||||||
|
Vector d(n,0.0);
|
||||||
|
Matrix H = zeros(m,n);
|
||||||
|
for (size_t j=0;j<n;j++) {
|
||||||
|
d(j) += delta; Vector hxplus = logmap(hx, h(x1, x2, expmap(x3,d)));
|
||||||
|
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(x1, x2, expmap(x3,d)));
|
||||||
|
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||||
|
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||||
|
}
|
||||||
|
return H;
|
||||||
|
}
|
||||||
|
template<class Y, class X1, class X2, class X3>
|
||||||
|
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);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue