diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 84a6b67f9..252d2c73c 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -23,9 +23,9 @@ #include #include #include -//#include -#include +#include "ceres/ceres.h" +#include "ceres/rotation.h" #undef CHECK #include @@ -272,7 +272,7 @@ struct Projective { }; /* ************************************************************************* */ - +// manifold_traits prototype #include template @@ -311,51 +311,43 @@ TEST(Expression, Traits) { EXPECT_LONGS_EQUAL(8, manifold_traits::dimension); } -template -Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta = 1e-5) { - Y hx = h(x1, x2); +/* ************************************************************************* */ +// New-style numerical derivatives using manifold_traits +template +Matrix numericalDerivative(boost::function h, const X& x, + double delta = 1e-5) { + Y hx = h(x); double factor = 1.0 / (2.0 * delta); static const size_t m = manifold_traits::dimension, n = - manifold_traits::dimension; + manifold_traits::dimension; Eigen::Matrix d; d.setZero(); Matrix H = zeros(m, n); for (size_t j = 0; j < n; j++) { d(j) += delta; Vector hxplus = manifold_traits::localCoordinates(hx, - h(manifold_traits::retract(x1, d), x2)); + h(manifold_traits::retract(x, d))); d(j) -= 2 * delta; Vector hxmin = manifold_traits::localCoordinates(hx, - h(manifold_traits::retract(x1, d), x2)); + h(manifold_traits::retract(x, d))); d(j) += delta; H.block(0, j) << (hxplus - hxmin) * factor; } return H; } -template +template +Matrix numericalDerivative21(boost::function h, + const X1& x1, const X2& x2, double delta = 1e-5) { + return numericalDerivative(boost::bind(h,_1,x2),x1,delta); +} + +template Matrix numericalDerivative22(boost::function h, const X1& x1, const X2& x2, double delta = 1e-5) { - Y hx = h(x1, x2); - double factor = 1.0 / (2.0 * delta); - static const size_t m = manifold_traits::dimension, n = - manifold_traits::dimension; - Eigen::Matrix d; - d.setZero(); - Matrix H = zeros(m, n); - for (size_t j = 0; j < n; j++) { - d(j) += delta; - Vector hxplus = manifold_traits::localCoordinates(hx, - h(x1, manifold_traits::retract(x2, d))); - d(j) -= 2 * delta; - Vector hxmin = manifold_traits::localCoordinates(hx, - h(x1, manifold_traits::retract(x2, d))); - d(j) += delta; - H.block(0, j) << (hxplus - hxmin) * factor; - } - return H; + return numericalDerivative(boost::bind(h,x1,_1),x2,delta); } + /* ************************************************************************* */ // Test Ceres AutoDiff TEST(Expression, AutoDiff) {