61 lines
2.0 KiB
C++
61 lines
2.0 KiB
C++
|
/**
|
||
|
|
* @file numericalDerivative.cpp
|
||
|
|
* @brief Some functions to compute numerical derivatives
|
||
|
|
* @author Frank Dellaert
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "numericalDerivative.h"
|
||
|
|
|
||
|
|
namespace gtsam {
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
Matrix NumericalDerivative11
|
||
|
|
(Vector (*h)(const Vector&), const Vector& x_, double delta) {
|
||
|
|
Vector x(x_), hx = h(x);
|
||
|
|
double factor = 1.0/(2.0*delta);
|
||
|
|
const size_t m = hx.size(), n = x.size();
|
||
|
|
Matrix H = zeros(m,n);
|
||
|
|
for (size_t j=0;j<n;j++) {
|
||
|
|
x(j) += delta; Vector hxplus = h(x);
|
||
|
|
x(j) -= 2*delta; Vector hxmin = h(x);
|
||
|
|
x(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||
|
|
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||
|
|
}
|
||
|
|
return H;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
Matrix NumericalDerivative21
|
||
|
|
(Vector (*h)(const Vector&, const Vector&), const Vector& x1_, const Vector& x2, double delta) {
|
||
|
|
Vector x1(x1_), hx = h(x1,x2);
|
||
|
|
double factor = 1.0/(2.0*delta);
|
||
|
|
const size_t m = hx.size(), n = x1.size();
|
||
|
|
Matrix H = zeros(m,n);
|
||
|
|
for (size_t j=0;j<n;j++) {
|
||
|
|
x1(j) += delta; Vector hxplus = h(x1,x2);
|
||
|
|
x1(j) -= 2*delta; Vector hxmin = h(x1,x2);
|
||
|
|
x1(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||
|
|
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||
|
|
}
|
||
|
|
return H;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
Matrix NumericalDerivative22
|
||
|
|
(Vector (*h)(const Vector&, const Vector&), const Vector& x1, const Vector& x2_, double delta) {
|
||
|
|
Vector x2(x2_), hx = h(x1,x2);
|
||
|
|
double factor = 1.0/(2.0*delta);
|
||
|
|
const size_t m = hx.size(), n = x2.size();
|
||
|
|
Matrix H = zeros(m,n);
|
||
|
|
for (size_t j=0;j<n;j++) {
|
||
|
|
x2(j) += delta; Vector hxplus = h(x1,x2);
|
||
|
|
x2(j) -= 2*delta; Vector hxmin = h(x1,x2);
|
||
|
|
x2(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||
|
|
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||
|
|
}
|
||
|
|
return H;
|
||
|
|
}
|
||
|
|
/* ************************************************************************* */
|
||
|
|
|
||
|
|
}
|