diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index e5cfbdd3b..a7dc37d55 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -296,6 +296,48 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative33(Y (* x2, x3, delta); } +/** + * Compute numerical derivative in argument 1 of 4-argument 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 x4 fourth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative41( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, _1, x2, x3, x4), x1, delta); +} + +/** + * Compute numerical derivative in argument 2 of 4-argument 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 x4 fourth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative42( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, _1, x3, x4), x2, delta); +} + /** * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar * function. This is implemented simply as the derivative of the gradient. diff --git a/gtsam/base/testLie.h b/gtsam/base/testLie.h index 4bbca85ca..791d5c04d 100644 --- a/gtsam/base/testLie.h +++ b/gtsam/base/testLie.h @@ -34,23 +34,25 @@ void testLieGroupDerivatives(TestResult& result_, const std::string& name_, Matrix H1, H2; typedef traits T; + typedef OptionalJacobian OJ; // Inverse + OJ none; EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1))); - EXPECT(assert_equal(numericalDerivative11(T::Inverse, t1),H1)); + EXPECT(assert_equal(numericalDerivative21(T::Inverse, t1, none),H1)); EXPECT(assert_equal(t2.inverse(),T::Inverse(t2, H1))); - EXPECT(assert_equal(numericalDerivative11(T::Inverse, t2),H1)); + EXPECT(assert_equal(numericalDerivative21(T::Inverse, t2, none),H1)); // Compose EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Compose, t1, t2), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Compose, t1, t2), H2)); + EXPECT(assert_equal(numericalDerivative41(T::Compose, t1, t2, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Compose, t1, t2, none, none), H2)); // Between EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Between, t1, t2), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Between, t1, t2), H2)); + EXPECT(assert_equal(numericalDerivative41(T::Between, t1, t2, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Between, t1, t2, none, none), H2)); } // Do a comprehensive test of Lie Group Chart derivatives template @@ -59,17 +61,20 @@ void testChartDerivatives(TestResult& result_, const std::string& name_, Matrix H1, H2; typedef traits T; + typedef typename G::TangentVector V; + typedef OptionalJacobian OJ; // Retract - typename G::TangentVector w12 = T::Local(t1, t2); + OJ none; + V w12 = T::Local(t1, t2); EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Retract, t1, w12), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Retract, t1, w12), H2)); + EXPECT(assert_equal(numericalDerivative41(T::Retract, t1, w12, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Retract, t1, w12, none, none), H2)); // Local EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Local, t1, t2), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Local, t1, t2), H2)); + EXPECT(assert_equal(numericalDerivative41(T::Local, t1, t2, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Local, t1, t2, none, none), H2)); } } // namespace gtsam