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

add numericalHessian for tenary functions

release/4.3a0
Duy-Nguyen Ta 2011-10-03 02:45:11 +00:00
parent 3271743ca4
commit 469f4614dc
2 changed files with 117 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
gtsam/base/numericalDerivative.h
View File

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

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

@ -83,6 +83,41 @@ TEST_UNSAFE(testNumericalDerivative, numericalHessian211) {
EXPECT(assert_equal(expected22, actual22, 1e-5));
}
/* ************************************************************************* */
double f4(const LieVector& x, const LieVector& y, const LieVector& z) {
assert(x.size() == 1 && y.size() == 1 && z.size() == 1);
return sin(x(0)) * cos(y(0)) * z(0)*z(0);
}
/* ************************************************************************* */
TEST_UNSAFE(testNumericalDerivative, numericalHessian311) {
LieVector center1(1, 1.0), center2(1, 2.0), center3(1, 3.0);
double x = center1(0), y = center2(0), z = center3(0);
Matrix expected11 = Matrix_(1,1,-sin(x)*cos(y)*z*z);
Matrix actual11 = numericalHessian311(f4, center1, center2, center3);
EXPECT(assert_equal(expected11, actual11, 1e-5));
Matrix expected12 = Matrix_(1,1,-cos(x)*sin(y)*z*z);
Matrix actual12 = numericalHessian312(f4, center1, center2, center3);
EXPECT(assert_equal(expected12, actual12, 1e-5));
Matrix expected13 = Matrix_(1,1,cos(x)*cos(y)*2*z);
Matrix actual13 = numericalHessian313(f4, center1, center2, center3);
EXPECT(assert_equal(expected13, actual13, 1e-5));
Matrix expected22 = Matrix_(1,1,-sin(x)*cos(y)*z*z);
Matrix actual22 = numericalHessian322(f4, center1, center2, center3);
EXPECT(assert_equal(expected22, actual22, 1e-5));
Matrix expected23 = Matrix_(1,1,-sin(x)*sin(y)*2*z);
Matrix actual23 = numericalHessian323(f4, center1, center2, center3);
EXPECT(assert_equal(expected23, actual23, 1e-5));
Matrix expected33 = Matrix_(1,1,sin(x)*cos(y)*2);
Matrix actual33 = numericalHessian333(f4, center1, center2, center3);
EXPECT(assert_equal(expected33, actual33, 1e-5));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */