diff --git a/.cproject b/.cproject index 38c4c66f4..700b82ce6 100644 --- a/.cproject +++ b/.cproject @@ -2329,6 +2329,14 @@ true true + + make + -j5 + testNumericalDerivative.run + true + true + true + make -j5 diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 036f23f68..01ed3f09a 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -16,7 +16,6 @@ */ // \callgraph - #pragma once #include @@ -31,595 +30,497 @@ #include #include +#include namespace gtsam { - /* - * Note that all of these functions have two versions, a boost.function version and a - * standard C++ function pointer version. This allows reformulating the arguments of - * a function to fit the correct structure, which is useful for situations like - * member functions and functions with arguments not involved in the derivative: - * - * Usage of the boost bind version to rearrange arguments: - * for a function with one relevant param and an optional derivative: - * Foo bar(const Obj& a, boost::optional H1) - * Use boost.bind to restructure: - * boost::bind(bar, _1, boost::none) - * This syntax will fix the optional argument to boost::none, while using the first argument provided - * - * For member functions, such as below, with an instantiated copy instanceOfSomeClass - * Foo SomeClass::bar(const Obj& a) - * 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/release/libs/bind/bind.html - */ - - - /** global functions for converting to a LieVector for use with numericalDerivative */ - inline LieVector makeLieVector(const Vector& v) { return LieVector(v); } - inline LieVector makeLieVectorD(double d) { return LieVector((Vector) (Vector(1) << d)); } - - /** - * Numerically compute gradient of scalar function - * Class X is the input argument - * The class X needs to have dim, expmap, logmap - */ - template - Vector numericalGradient(boost::function h, const X& x, double delta=1e-5) { - double factor = 1.0/(2.0*delta); - const size_t n = x.dim(); - Vector d = zero(n), g = zero(n); - for (size_t j=0;j - Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) { - return numericalGradient(boost::bind(h, _1), x, delta); - } - - /** - * Compute numerical derivative in argument 1 of unary function - * @param h unary function yielding m-vector - * @param x n-dimensional value at which to evaluate h - * @param delta increment for numerical derivative - * Class Y is the output argument - * Class X is the input argument - * @return m*n Jacobian computed via central differencing - * Both classes X,Y need dim, expmap, logmap - */ - template - Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { - Y hx = h(x); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(h, _1), x, delta); - } - -// /** remapping for double valued functions */ -// template -// Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { -// return numericalDerivative11(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); -// } - - template - Matrix numericalDerivative11(double (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); - } - - /** remapping for vector valued functions */ - template - Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); - } - - template - Matrix numericalDerivative11(Vector (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); - } - - /** - * Compute numerical derivative in argument 1 of binary function - * @param h binary function yielding m-vector - * @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 - Matrix numericalDerivative21(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); - const size_t m = hx.dim(), n = x1.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21(boost::bind(h, _1, _2), x1, x2, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } - - template - Matrix numericalDerivative21(double (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } - - template - inline Matrix numericalDerivative21(Vector (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } - - /** - * Compute numerical derivative in argument 2 of binary function - * @param h binary function yielding m-vector - * @param x1 first argument value - * @param x2 n-dimensional 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 - 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); - const size_t m = hx.dim(), n = x2.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative22 - (Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22(boost::bind(h, _1, _2), x1, x2, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative22(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } - - template - inline Matrix numericalDerivative22(double (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative22(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } - - template - inline Matrix numericalDerivative22(Vector (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVector, 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 - Matrix numericalDerivative31 - (boost::function 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 = hx.dim(), n = x1.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline 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(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative31(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative31(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative31(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative31(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** - * Compute numerical derivative in argument 2 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 - Matrix numericalDerivative32 - (boost::function 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 = hx.dim(), n = x2.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline 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(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative32(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative32(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative32(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative32(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** - * Compute numerical derivative in argument 3 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 - Matrix numericalDerivative33 - (boost::function 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 = hx.dim(), n = x3.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline 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(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative33(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative33(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative33(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative33(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** - * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar - * function. This is implemented simply as the derivative of the gradient. - * @param f A function taking a Lie object as input and returning a scalar - * @param x The center point for computing the Hessian - * @param delta The numerical derivative step size - * @return n*n Hessian matrix computed via central differencing - */ - template - inline Matrix numericalHessian(boost::function f, const X& x, double delta=1e-5) { - return numericalDerivative11(boost::function(boost::bind( - static_cast,const X&, double)>(&numericalGradient), - f, _1, delta)), x, delta); - } - - template - inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta=1e-5) { - return numericalHessian(boost::function(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 G_x1 { - const boost::function& f_; - const X1& x1_; - double delta_; - public: - G_x1(const boost::function& f, const X1& x1, double delta) : f_(f), x1_(x1), delta_(delta) {} - Vector operator()(const X2& x2) { - return numericalGradient(boost::function(boost::bind(f_, _1, x2)), x1_, delta_); - } - }; - - template - inline Matrix numericalHessian212(boost::function f, const X1& x1, const X2& x2, double delta=1e-5) { - G_x1 g_x1(f, x1, delta); - return numericalDerivative11(boost::function(boost::bind(boost::ref(g_x1), _1)), x2, delta); - } - - - template - inline Matrix numericalHessian212(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalHessian212(boost::function(f), x1, x2, delta); - } - - - template - inline Matrix numericalHessian211(boost::function f, const X1& x1, const X2& x2, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; - boost::function f2(boost::bind(f, _1, x2)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x1, delta); - } - - - template - inline Matrix numericalHessian211(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalHessian211(boost::function(f), x1, x2, delta); - } - - - template - inline Matrix numericalHessian222(boost::function f, const X1& x1, const X2& x2, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; - boost::function f2(boost::bind(f, x1, _1)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x2, delta); - } - - - template - inline Matrix numericalHessian222(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalHessian222(boost::function(f), x1, x2, delta); - } - - /** - * Numerical Hessian for tenary functions - */ - /* **************************************************************** */ - template - inline Matrix numericalHessian311(boost::function f, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; - boost::function f2(boost::bind(f, _1, x2, x3)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x1, delta); - } - - template - 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(f), x1, x2, x3, delta); - } - - /* **************************************************************** */ - template - inline Matrix numericalHessian322(boost::function f, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; - boost::function f2(boost::bind(f, x1, _1, x3)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x2, delta); - } - - template - 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(f), x1, x2, x3, delta); - } - - /* **************************************************************** */ - template - inline Matrix numericalHessian333(boost::function f, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X3&, double) = &numericalGradient; - boost::function f2(boost::bind(f, x1, x2, _1)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x3, delta); - } - - template - 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(f), x1, x2, x3, delta); - } - - /* **************************************************************** */ - template - inline Matrix numericalHessian312(boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian212(boost::function(boost::bind(f, _1, _2, x3)), x1, x2, delta ); - } - - template - inline Matrix numericalHessian313(boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian212(boost::function(boost::bind(f, _1, x2, _2)), x1, x3, delta ); - } - - template - inline Matrix numericalHessian323(boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian212(boost::function(boost::bind(f, x1, _1, _2)), x2, x3, delta ); - } - - /* **************************************************************** */ - template - 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(f), x1, x2, x3, delta); - } - - template - 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(f), x1, x2, x3, delta); - } - - template - 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(f), x1, x2, x3, delta); - } +/* + * Note that all of these functions have two versions, a boost.function version and a + * standard C++ function pointer version. This allows reformulating the arguments of + * a function to fit the correct structure, which is useful for situations like + * member functions and functions with arguments not involved in the derivative: + * + * Usage of the boost bind version to rearrange arguments: + * for a function with one relevant param and an optional derivative: + * Foo bar(const Obj& a, boost::optional H1) + * Use boost.bind to restructure: + * boost::bind(bar, _1, boost::none) + * This syntax will fix the optional argument to boost::none, while using the first argument provided + * + * For member functions, such as below, with an instantiated copy instanceOfSomeClass + * Foo SomeClass::bar(const Obj& a) + * 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/release/libs/bind/bind.html + */ + +/** global functions for converting to a LieVector for use with numericalDerivative */ +inline LieVector makeLieVector(const Vector& v) { + return LieVector(v); +} +inline LieVector makeLieVectorD(double d) { + return LieVector((Vector) (Vector(1) << d)); +} + +/** + * Numerically compute gradient of scalar function + * Class X is the input argument + * The class X needs to have dim, expmap, logmap + */ +template +Vector numericalGradient(boost::function h, const X& x, + double delta = 1e-5) { + double factor = 1.0 / (2.0 * delta); + + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X must be a manifold type."); + typedef DefaultChart ChartX; + typedef typename ChartX::vector TangentX; + + // get chart at x + ChartX chartX(x); + TangentX zeroX = chartX.apply(x); + size_t n = zeroX.size(); // hack to get size if dynamic type + + // Prepare a tangent vector to perturb x with, only works for fixed size + TangentX d; + d.setZero(); + + Vector g = zero(n); + for (int j = 0; j < n; j++) { + d(j) = delta; + double hxplus = h(chartX.retract(d)); + d(j) = -delta; + double hxmin = h(chartX.retract(d)); + d(j) = 0; + g(j) = (hxplus - hxmin) * factor; + } + return g; +} + +/** + * @brief New-style numerical derivatives using manifold_traits + * @brief Computes numerical derivative in argument 1 of unary function + * @param h unary function yielding m-vector + * @param x n-dimensional value at which to evaluate h + * @param delta increment for numerical derivative + * Class Y is the output argument + * Class X is the input argument + * @return m*n Jacobian computed via central differencing + */ +template +// TODO Should compute fixed-size matrix +Matrix numericalDerivative11(boost::function h, const X& x, + double delta = 1e-5) { + using namespace traits; + + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + typedef DefaultChart ChartY; + typedef typename ChartY::vector TangentY; + + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X must be a manifold type."); + typedef DefaultChart ChartX; + typedef typename ChartX::vector TangentX; + + // get value at x, and corresponding chart + Y hx = h(x); + ChartY chartY(hx); + TangentY zeroY = chartY.apply(hx); + size_t m = zeroY.size(); + + // get chart at x + ChartX chartX(x); + TangentX zeroX = chartX.apply(x); + size_t n = zeroX.size(); + + // Prepare a tangent vector to perturb x with, only works for fixed size + TangentX dx; + dx.setZero(); + + // Fill in Jacobian H + Matrix H = zeros(m,n); + double factor = 1.0 / (2.0 * delta); + for (int j = 0; j < n; j++) { + dx(j) = delta; + TangentY dy1 = chartY.apply(h(chartX.retract(dx))); + dx(j) = -delta; + TangentY dy2 = chartY.apply(h(chartX.retract(dx))); + dx(j) = 0; + H.col(j) << (dy1 - dy2) * factor; + } + return H; +} + +/** use a raw C++ function pointer */ +template +Matrix numericalDerivative11(Y (*h)(const X&), const X& x, + double delta = 1e-5) { + return numericalDerivative11(boost::bind(h, _1), x, delta); +} + +/** + * Compute numerical derivative in argument 1 of binary function + * @param h binary function yielding m-vector + * @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 + */ +template +Matrix numericalDerivative21(const boost::function& h, + const X1& x1, const X2& x2, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, _1, x2), x1, delta); +} + +/** use a raw C++ function pointer */ +template +inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1, + const X2& x2, double delta = 1e-5) { + return numericalDerivative21(boost::bind(h, _1, _2), x1, x2, delta); +} + +/** + * Compute numerical derivative in argument 2 of binary function + * @param h binary function yielding m-vector + * @param x1 first argument value + * @param x2 n-dimensional second argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +Matrix numericalDerivative22(boost::function h, + const X1& x1, const X2& x2, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X2 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, _1), x2, delta); +} + +/** use a raw C++ function pointer */ +template +inline Matrix numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1, + const X2& x2, double delta = 1e-5) { + return numericalDerivative22(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 +Matrix numericalDerivative31( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, _1, x2, x3), x1, delta); +} + +template +inline 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(boost::bind(h, _1, _2, _3), x1, + x2, x3, delta); +} + +/** + * Compute numerical derivative in argument 2 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 +Matrix numericalDerivative32( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X2 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, _1, x3), x2, delta); +} + +template +inline 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(boost::bind(h, _1, _2, _3), x1, + x2, x3, delta); +} + +/** + * Compute numerical derivative in argument 3 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 +Matrix numericalDerivative33( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X3 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, x2, _1), x3, delta); +} + +template +inline 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(boost::bind(h, _1, _2, _3), x1, + x2, x3, delta); +} + +/** + * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar + * function. This is implemented simply as the derivative of the gradient. + * @param f A function taking a Lie object as input and returning a scalar + * @param x The center point for computing the Hessian + * @param delta The numerical derivative step size + * @return n*n Hessian matrix computed via central differencing + */ +template +inline Matrix numericalHessian(boost::function f, const X& x, + double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X must be a manifold type."); + typedef boost::function F; + typedef boost::function G; + G ng = static_cast(numericalGradient ); + return numericalDerivative11(boost::bind(ng, f, _1, delta), x, + delta); +} + +template +inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta = + 1e-5) { + return numericalHessian(boost::function(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 G_x1 { + const boost::function& f_; + X1 x1_; + double delta_; +public: + G_x1(const boost::function& f, const X1& x1, + double delta) : + f_(f), x1_(x1), delta_(delta) { + } + Vector operator()(const X2& x2) { + return numericalGradient(boost::bind(f_, _1, x2), x1_, delta_); + } +}; + +template +inline Matrix numericalHessian212( + boost::function f, const X1& x1, const X2& x2, + double delta = 1e-5) { + G_x1 g_x1(f, x1, delta); + return numericalDerivative11( + boost::function( + boost::bind(boost::ref(g_x1), _1)), x2, delta); +} + +template +inline Matrix numericalHessian212(double (*f)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta = 1e-5) { + return numericalHessian212(boost::function(f), + x1, x2, delta); +} + +template +inline Matrix numericalHessian211( + boost::function f, const X1& x1, const X2& x2, + double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X1&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, _1, x2)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x1, delta); +} + +template +inline Matrix numericalHessian211(double (*f)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta = 1e-5) { + return numericalHessian211(boost::function(f), + x1, x2, delta); +} + +template +inline Matrix numericalHessian222( + boost::function f, const X1& x1, const X2& x2, + double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X2&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, x1, _1)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x2, delta); +} + +template +inline Matrix numericalHessian222(double (*f)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta = 1e-5) { + return numericalHessian222(boost::function(f), + x1, x2, delta); +} + +/** + * Numerical Hessian for tenary functions + */ +/* **************************************************************** */ +template +inline Matrix numericalHessian311( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X1&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, _1, x2, x3)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x1, delta); +} + +template +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(f), x1, x2, x3, + delta); +} + +/* **************************************************************** */ +template +inline Matrix numericalHessian322( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X2&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, x1, _1, x3)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x2, delta); +} + +template +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(f), x1, x2, x3, + delta); +} + +/* **************************************************************** */ +template +inline Matrix numericalHessian333( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X3&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, x1, x2, _1)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x3, delta); +} + +template +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(f), x1, x2, x3, + delta); +} + +/* **************************************************************** */ +template +inline Matrix numericalHessian312( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian212( + boost::function(boost::bind(f, _1, _2, x3)), + x1, x2, delta); +} + +template +inline Matrix numericalHessian313( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian212( + boost::function(boost::bind(f, _1, x2, _2)), + x1, x3, delta); +} + +template +inline Matrix numericalHessian323( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian212( + boost::function(boost::bind(f, x1, _1, _2)), + x2, x3, delta); +} + +/* **************************************************************** */ +template +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(f), x1, x2, x3, + delta); +} + +template +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(f), x1, x2, x3, + delta); +} + +template +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(f), x1, x2, x3, + delta); +} } diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index f7e4d3baa..b4a9b91d6 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -15,115 +15,123 @@ * @date Apr 8, 2011 */ +#include #include -#include - +using namespace std; using namespace gtsam; /* ************************************************************************* */ -double f(const LieVector& x) { +double f(const Vector2& x) { assert(x.size() == 2); return sin(x(0)) + cos(x(1)); } /* ************************************************************************* */ -TEST(testNumericalDerivative, numericalHessian) { - LieVector center = ones(2); +// +TEST(testNumericalDerivative, numericalGradient) { + Vector2 x(1, 1); - Matrix expected = (Matrix(2,2) << - -sin(center(0)), 0.0, - 0.0, -cos(center(1))); + Vector expected(2); + expected << cos(x(1)), -sin(x(0)); - Matrix actual = numericalHessian(f, center); + Vector actual = numericalGradient(f, x); EXPECT(assert_equal(expected, actual, 1e-5)); } /* ************************************************************************* */ -double f2(const LieVector& x) { +TEST(testNumericalDerivative, numericalHessian) { + Vector2 x(1, 1); + + Matrix expected(2, 2); + expected << -sin(x(0)), 0.0, 0.0, -cos(x(1)); + + Matrix actual = numericalHessian(f, x); + + EXPECT(assert_equal(expected, actual, 1e-5)); +} + +/* ************************************************************************* */ +double f2(const Vector2& x) { assert(x.size() == 2); return sin(x(0)) * cos(x(1)); } /* ************************************************************************* */ +// TEST(testNumericalDerivative, numericalHessian2) { - Vector v_center = (Vector(2) << 0.5, 1.0); - LieVector center(v_center); + Vector2 v(0.5, 1.0); + Vector2 x(v); - Matrix expected = (Matrix(2,2) << - -cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)), - -cos(center(0))*sin(center(1)), -sin(center(0))*cos(center(1))); + Matrix expected = (Matrix(2, 2) << -cos(x(1)) * sin(x(0)), -sin(x(1)) + * cos(x(0)), -cos(x(0)) * sin(x(1)), -sin(x(0)) * cos(x(1))); - Matrix actual = numericalHessian(f2, center); + Matrix actual = numericalHessian(f2, x); 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)); +double f3(double x1, double x2) { + return sin(x1) * cos(x2); } /* ************************************************************************* */ +// TEST(testNumericalDerivative, numericalHessian211) { - Vector v_center1 = (Vector(1) << 1.0); - Vector v_center2 = (Vector(1) << 5.0); - LieVector center1(v_center1), center2(v_center2); + double x1 = 1, x2 = 5; - Matrix expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0))); - Matrix actual11 = numericalHessian211(f3, center1, center2); + Matrix expected11 = (Matrix(1, 1) << -sin(x1) * cos(x2)); + Matrix actual11 = numericalHessian211(f3, x1, x2); EXPECT(assert_equal(expected11, actual11, 1e-5)); - Matrix expected12 = (Matrix(1, 1) <<-cos(center1(0))*sin(center2(0))); - Matrix actual12 = numericalHessian212(f3, center1, center2); + Matrix expected12 = (Matrix(1, 1) << -cos(x1) * sin(x2)); + Matrix actual12 = numericalHessian212(f3, x1, x2); EXPECT(assert_equal(expected12, actual12, 1e-5)); - Matrix expected22 = (Matrix(1, 1) <<-sin(center1(0))*cos(center2(0))); - Matrix actual22 = numericalHessian222(f3, center1, center2); + Matrix expected22 = (Matrix(1, 1) << -sin(x1) * cos(x2)); + Matrix actual22 = numericalHessian222(f3, x1, x2); 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); +double f4(double x, double y, double z) { + return sin(x) * cos(y) * z * z; } /* ************************************************************************* */ +// TEST(testNumericalDerivative, numericalHessian311) { - Vector v_center1 = (Vector(1) << 1.0); - Vector v_center2 = (Vector(1) << 2.0); - Vector v_center3 = (Vector(1) << 3.0); - LieVector center1(v_center1), center2(v_center2), center3(v_center3); - - 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); + double x = 1, y = 2, z = 3; + Matrix expected11 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z); + Matrix actual11 = numericalHessian311(f4, x, y, z); 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); + Matrix expected12 = (Matrix(1, 1) << -cos(x) * sin(y) * z * z); + Matrix actual12 = numericalHessian312(f4, x, y, z); 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); + Matrix expected13 = (Matrix(1, 1) << cos(x) * cos(y) * 2 * z); + Matrix actual13 = numericalHessian313(f4, x, y, z); 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); + Matrix expected22 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z); + Matrix actual22 = numericalHessian322(f4, x, y, z); 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); + Matrix expected23 = (Matrix(1, 1) << -sin(x) * sin(y) * 2 * z); + Matrix actual23 = numericalHessian323(f4, x, y, z); EXPECT(assert_equal(expected23, actual23, 1e-5)); - Matrix expected33 = (Matrix(1, 1) << sin(x)*cos(y)*2); - Matrix actual33 = numericalHessian333(f4, center1, center2, center3); + Matrix expected33 = (Matrix(1, 1) << sin(x) * cos(y) * 2); + Matrix actual33 = numericalHessian333(f4, x, y, z); EXPECT(assert_equal(expected33, actual33, 1e-5)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index b0890057e..7707e9161 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -229,7 +229,8 @@ TEST( Rot3, rightJacobianExpMapSO3inverse ) Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias Vector3 deltatheta; deltatheta << 0, 0, 0; - Matrix expectedJacobian = numericalDerivative11(boost::bind(&evaluateLogRotation, thetahat, _1), LieVector(deltatheta)); + Matrix expectedJacobian = numericalDerivative11( + boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta); Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat); EXPECT(assert_equal(expectedJacobian, actualJacobian)); } @@ -439,19 +440,18 @@ TEST( Rot3, between ) /* ************************************************************************* */ Vector w = (Vector(3) << 0.1, 0.27, -0.2); -// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes? -// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 +// Left trivialization Derivative of exp(w) wrpt w: +// How does exp(w) change when w changes? +// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 // => y = log (exp(-w) * exp(w+dw)) -Vector testDexpL(const LieVector& dw) { - Vector y = Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); - return y; +Vector3 testDexpL(const Vector3& dw) { + return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); } TEST( Rot3, dexpL) { Matrix actualDexpL = Rot3::dexpL(w); - Matrix expectedDexpL = numericalDerivative11( - boost::function( - boost::bind(testDexpL, _1)), LieVector(zero(3)), 1e-2); + Matrix expectedDexpL = numericalDerivative11(testDexpL, + Vector3::Zero(), 1e-2); EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); Matrix actualDexpInvL = Rot3::dexpInvL(w); diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 51c195d32..0bd553a40 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -274,11 +274,7 @@ TEST( triangulation, TriangulationFactor ) { Matrix HActual; factor.evaluateError(landmark, HActual); -// Matrix expectedH1 = numericalDerivative11( -// boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, -// boost::none, boost::none), pose1); - // The expected Jacobian - Matrix HExpected = numericalDerivative11( + Matrix HExpected = numericalDerivative11( boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); // Verify the Jacobians are correct diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 4aa553df2..53ae2fc58 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -115,13 +115,13 @@ TEST(Unit3, error) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), q); p.error(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), r); p.error(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index d65f96f7f..7726f2280 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include @@ -150,7 +149,7 @@ TEST( GaussianBayesNet, DeterminantTest ) /* ************************************************************************* */ namespace { - double computeError(const GaussianBayesNet& gbn, const LieVector& values) + double computeError(const GaussianBayesNet& gbn, const Vector& values) { pair Rd = GaussianFactorGraph(gbn).jacobian(); return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); @@ -180,14 +179,12 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) { 4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0))); // Compute the Hessian numerically - Matrix hessian = numericalHessian( - boost::function(boost::bind(&computeError, gbn, _1)), - LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols()))); + Matrix hessian = numericalHessian(boost::bind(&computeError, gbn, _1), + Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())); // Compute the gradient numerically - Vector gradient = numericalGradient( - boost::function(boost::bind(&computeError, gbn, _1)), - LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols()))); + Vector gradient = numericalGradient(boost::bind(&computeError, gbn, _1), + Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index c4155ea16..790080556 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -45,7 +45,7 @@ TEST( Rot3AttitudeFactor, Constructor ) { EXPECT(assert_equal(zero(2),factor.evaluateError(nRb),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none), nRb); @@ -78,7 +78,7 @@ TEST( Pose3AttitudeFactor, Constructor ) { EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1, boost::none), T); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 1a8b6160d..279c20e69 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -239,12 +239,12 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( + Matrix expectedDelPdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - Matrix expectedDelVdelBias = numericalDerivative11( + Matrix expectedDelVdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index f02047aa9..8c93020c9 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -56,7 +56,7 @@ TEST( GPSFactor, Constructors ) { EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); // Use the factor to calculate the derivative diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index a6894898b..ad97d9433 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -192,15 +192,15 @@ TEST( ImuFactor, Error ) EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Expected Jacobians - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); // Check rotation Jacobians @@ -276,15 +276,15 @@ TEST( ImuFactor, ErrorWithBiases ) // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Expected Jacobians - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); // Check rotation Jacobians @@ -341,7 +341,7 @@ TEST( ImuFactor, PartialDerivativeLogmap ) // Compute numerical derivatives - Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( + Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( &evaluateLogRotation, thetahat, _1), LieVector(deltatheta)); const Vector3 x = thetahat; // parametrization of so(3) @@ -417,12 +417,12 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( + Matrix expectedDelPdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - Matrix expectedDelVdelBias = numericalDerivative11( + Matrix expectedDelVdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); @@ -531,15 +531,15 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); // Expected Jacobians - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); // Check rotation Jacobians diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 1875e4f1c..5599c93d6 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -72,35 +72,35 @@ TEST( MagFactor, Factors ) { // MagFactor MagFactor f(1, measured, s, dir, bias, model); EXPECT( assert_equal(zero(3),f.evaluateError(theta,H1),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); EXPECT( assert_equal(zero(3),f1.evaluateError(nRb,H1),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); EXPECT( assert_equal(zero(3),f2.evaluateError(scaled,bias,H1,H2),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// H1, 1e-7)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// H2, 1e-7)); // MagFactor2 MagFactor3 f3(1, 2, 3, measured, nRb, model); EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); - EXPECT(assert_equal(numericalDerivative11 // + EXPECT(assert_equal(numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7)); - EXPECT(assert_equal(numericalDerivative11 // + EXPECT(assert_equal(numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// H2, 1e-7)); - EXPECT(assert_equal(numericalDerivative11 // + EXPECT(assert_equal(numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// H3, 1e-7)); } diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 5184393ac..4c0edf5c9 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -50,10 +50,10 @@ TEST( EssentialMatrixConstraint, test ) { CHECK(assert_equal(expected, actual, 1e-8)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11( + Matrix expectedH1 = numericalDerivative11( boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11( + Matrix expectedH2 = numericalDerivative11( boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 1e5674599..c889fb1e9 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -96,7 +96,7 @@ TEST (EssentialMatrixFactor, factor) { // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected; - Hexpected = numericalDerivative11( + Hexpected = numericalDerivative11( boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, boost::none), trueE); @@ -173,8 +173,8 @@ TEST (EssentialMatrixFactor2, factor) { boost::function f = boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); + Hexpected1 = numericalDerivative21(f, trueE, d); + Hexpected2 = numericalDerivative22(f, trueE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); @@ -247,8 +247,8 @@ TEST (EssentialMatrixFactor3, factor) { boost::function f = boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + Hexpected1 = numericalDerivative21(f, bodyE, d); + Hexpected2 = numericalDerivative22(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); @@ -389,8 +389,8 @@ TEST (EssentialMatrixFactor2, extraTest) { boost::function f = boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); + Hexpected1 = numericalDerivative21(f, trueE, d); + Hexpected2 = numericalDerivative22(f, trueE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); @@ -458,8 +458,8 @@ TEST (EssentialMatrixFactor3, extraTest) { boost::function f = boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + Hexpected1 = numericalDerivative21(f, bodyE, d); + Hexpected2 = numericalDerivative22(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index f36405318..15e8cf984 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -69,13 +69,13 @@ TEST (RotateFactor, test) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc); f.evaluateError(iRc, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R); f.evaluateError(R, actual); EXPECT(assert_equal(expected, actual, 1e-9)); @@ -141,14 +141,14 @@ TEST (RotateDirectionsFactor, test) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, boost::none), iRc); f.evaluateError(iRc, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, boost::none), R); f.evaluateError(R, actual); diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index 2a814b915..c1e12b4a7 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -126,11 +126,9 @@ public: /// Log map at identity - return the canonical coordinates of this rotation static Vector Logmap(const Pose3Upright& p); -private: - /// @} - /// @name Advanced Interface - /// @{ + +private: // Serialization function friend class boost::serialization::access; @@ -142,4 +140,18 @@ private: }; // \class Pose3Upright +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +} + + } // \namespace gtsam diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index b477d3e44..6e0b92fd7 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -90,7 +90,7 @@ TEST( InvDepthFactor, Dproject_pose) { LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); - Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); + Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); Matrix actual; Point2 uv = inv_camera.project(landmark, inv_depth, actual, boost::none, boost::none); @@ -102,7 +102,7 @@ TEST( InvDepthFactor, Dproject_landmark) { LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); - Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); + Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); Matrix actual; Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, actual, boost::none); @@ -114,7 +114,7 @@ TEST( InvDepthFactor, Dproject_inv_depth) { LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); - Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); + Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); Matrix actual; Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, boost::none, actual); diff --git a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp index 397c91c5f..053acdd34 100644 --- a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -135,60 +136,6 @@ struct SnavelyProjection { }; -/* ************************************************************************* */ -// New-style numerical derivatives using manifold_traits -template -Matrix numericalDerivative(boost::function h, const X& x, - double delta = 1e-5) { - using namespace traits; - - BOOST_STATIC_ASSERT(is_manifold::value); - static const size_t M = dimension::value; - typedef DefaultChart ChartY; - typedef typename ChartY::vector TangentY; - - BOOST_STATIC_ASSERT(is_manifold::value); - static const size_t N = dimension::value; - typedef DefaultChart ChartX; - typedef typename ChartX::vector TangentX; - - // get chart at x - ChartX chartX(x); - - // get value at x, and corresponding chart - Y hx = h(x); - ChartY chartY(hx); - - // Prepare a tangent vector to perturb x with - TangentX dx; - dx.setZero(); - - // Fill in Jacobian H - Matrix H = zeros(M, N); - double factor = 1.0 / (2.0 * delta); - for (size_t j = 0; j < N; j++) { - dx(j) = delta; - TangentY dy1 = chartY.apply(h(chartX.retract(dx))); - dx(j) = -delta; - TangentY dy2 = chartY.apply(h(chartX.retract(dx))); - H.block(0, j) << (dy1 - dy2) * factor; - dx(j) = 0; - } - return H; -} - -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) { - return numericalDerivative(boost::bind(h, x1, _1), x2, delta); -} - /* ************************************************************************* */ // Test Ceres AutoDiff TEST(Expression, AutoDiff) { diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index d61787358..6a8c297b7 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -104,11 +104,15 @@ public: boost::optional H1=boost::none, boost::optional H2=boost::none) const { - if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1, landmark), pose); + if (H1) { + (*H1) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1, + landmark), pose); } - if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, _1), landmark); + if (H2) { + (*H2) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, + _1), landmark); } return inverseDepthError(pose, landmark); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 9d4113431..8d55d1ce5 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -107,11 +107,15 @@ public: boost::optional H1=boost::none, boost::optional H2=boost::none) const { - if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1, landmark), pose); + if (H1) { + (*H1) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1, + landmark), pose); } - if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, _1), landmark); + if (H2) { + (*H2) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, + _1), landmark); } return inverseDepthError(pose, landmark); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index aef15638f..e4b282550 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -108,10 +108,10 @@ public: boost::optional H2=boost::none) const { if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); + (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); } if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark); + (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark); } return inverseDepthError(pose, landmark); @@ -228,13 +228,13 @@ public: boost::optional H3=boost::none) const { if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); + (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); } if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2); + (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2); } if(H3) { - (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); + (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); } return inverseDepthError(pose1, pose2, landmark); diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index bf9dc0e8e..d0af4af62 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -176,8 +176,8 @@ TEST( PoseBetweenFactor, Jacobian ) { Point3(-3.37493895, 6.14660244, -8.93650986)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); + Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); // Use the factor to calculate the derivative Matrix actualH1; @@ -205,8 +205,8 @@ TEST( PoseBetweenFactor, JacobianWithTransform ) { Point3(-3.5257579, 6.02637531, -8.98382384)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); + Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); // Use the factor to calculate the derivative Matrix actualH1; diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp index cbfa45431..a01cfedba 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -166,7 +166,7 @@ TEST( PosePriorFactor, Jacobian ) { Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); // Use the factor to calculate the derivative Matrix actualH1; @@ -190,7 +190,7 @@ TEST( PosePriorFactor, JacobianWithTransform ) { Point3(-4.74767676, 7.67044942, -11.00985)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); // Use the factor to calculate the derivative Matrix actualH1; diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index 3a7bf0cd0..aacca18ea 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp @@ -170,7 +170,7 @@ TEST( ProjectionFactor, Jacobian ) { CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); // Verify H2 with numerical derivative - Matrix H2Expected = numericalDerivative32( + Matrix H2Expected = numericalDerivative32( boost::function( boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3, boost::none, boost::none, boost::none)), pose, Pose3(), point); @@ -205,7 +205,7 @@ TEST( ProjectionFactor, JacobianWithTransform ) { CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); // Verify H2 with numerical derivative - Matrix H2Expected = numericalDerivative32( + Matrix H2Expected = numericalDerivative32( boost::function( boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3, boost::none, boost::none, boost::none)), pose, body_P_sensor, point);