12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

numericalHessian for binary functions

release/4.3a0
Duy-Nguyen Ta 2011-04-11 21:59:17 +00:00
parent 9f1bf3e475
commit 6d5dee0860
2 changed files with 85 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
gtsam/base/numericalDerivative.h
View File

@ -476,4 +476,64 @@ namespace gtsam {
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);
}
}

25
gtsam/base/tests/testNumericalDerivative.cpp
View File

@ -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); }
/* ************************************************************************* */