From ff57bbc2c4fd0a1b7669e414a15738fd42e0aaee Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 22 Sep 2020 15:25:05 -0400 Subject: [PATCH] update numericalDerivative functions to take in optional dimension template parameter and added corresponding tests --- gtsam/base/numericalDerivative.h | 104 +++++++++++-------- gtsam/base/tests/testNumericalDerivative.cpp | 54 ++++++++++ 2 files changed, 116 insertions(+), 42 deletions(-) diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 34ff0327e..fc5531cdc 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -109,7 +109,7 @@ typename Eigen::Matrix numericalGradient( * @param delta increment for numerical derivative * Class Y is the output argument * Class X is the input argument - * int N is the dimension of the X input value if variable dimension type but known at test time + * @tparam int N is the dimension of the X input value if variable dimension type but known at test time * @return m*n Jacobian computed via central differencing */ @@ -167,15 +167,16 @@ typename internal::FixedSizeMatrix::type numericalDerivative11(Y (*h)(const * @param x2 second argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative21(const boost::function& h, const X1& x1, const X2& x2, 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, boost::cref(x2)), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2)), x1, delta); } /** use a raw C++ function pointer */ @@ -192,15 +193,16 @@ typename internal::FixedSizeMatrix::type numericalDerivative21(Y (*h)(cons * @param x2 n-dimensional second argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative22(boost::function h, const X1& x1, const X2& x2, double delta = 1e-5) { // BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), // "Template argument X1 must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1), x2, delta); } /** use a raw C++ function pointer */ @@ -219,8 +221,9 @@ typename internal::FixedSizeMatrix::type numericalDerivative22(Y (*h)(cons * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing * All classes Y,X1,X2,X3 need dim, expmap, logmap + * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative31( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { @@ -228,7 +231,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative31( "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, boost::cref(x2), boost::cref(x3)), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta); } template @@ -247,8 +250,9 @@ typename internal::FixedSizeMatrix::type numericalDerivative31(Y (*h)(cons * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing * All classes Y,X1,X2,X3 need dim, expmap, logmap + * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative32( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { @@ -256,7 +260,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative32( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta); } template @@ -275,8 +279,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative32(Y (* * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing * All classes Y,X1,X2,X3 need dim, expmap, logmap + * @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative33( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { @@ -284,7 +289,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative33( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X3 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta); } template @@ -303,8 +308,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative33(Y (* * @param x4 fourth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative41( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { @@ -312,7 +318,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative41( "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, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta); } template @@ -330,8 +336,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative41(Y (* * @param x4 fourth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative42( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { @@ -339,7 +346,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative42( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta); } template @@ -357,8 +364,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative42(Y (* * @param x4 fourth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative43( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { @@ -366,7 +374,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative43( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X3 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta); } template @@ -384,8 +392,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative43(Y (* * @param x4 n-dimensional fourth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X4 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative44( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { @@ -393,7 +402,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative44( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X4 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta); } template @@ -412,8 +421,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative44(Y (* * @param x5 fifth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative51( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { @@ -421,7 +431,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative51( "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, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta); } template @@ -440,8 +450,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative51(Y (* * @param x5 fifth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative52( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { @@ -449,7 +460,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative52( "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, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta); } template @@ -468,8 +479,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative52(Y (* * @param x5 fifth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative53( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { @@ -477,7 +489,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative53( "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, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta); } template @@ -496,8 +508,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative53(Y (* * @param x5 fifth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X4 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative54( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { @@ -505,7 +518,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative54( "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, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta); } template @@ -524,8 +537,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative54(Y (* * @param x5 fifth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X5 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative55( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { @@ -533,7 +547,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative55( "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, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta); } template @@ -553,8 +567,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative55(Y (* * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative61( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { @@ -562,7 +577,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative61( "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, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta); } template @@ -582,8 +597,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative61(Y (* * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative62( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { @@ -591,7 +607,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative62( "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, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta); } template @@ -608,11 +624,12 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative62(Y (* * @param x3 third argument value * @param x4 fourth argument value * @param x5 fifth argument value - * @param x6 sixth argument value + * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative63( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { @@ -620,7 +637,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative63( "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, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta); } template @@ -640,8 +657,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative63(Y (* * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X4 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative64( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { @@ -649,7 +667,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative64( "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, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5), boost::cref(x6)), x4, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5), boost::cref(x6)), x4, delta); } template @@ -669,8 +687,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative64(Y (* * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X5 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative65( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { @@ -678,7 +697,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative65( "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, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1, boost::cref(x6)), x5, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1, boost::cref(x6)), x5, delta); } template @@ -698,8 +717,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative65(Y (* * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X6 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative66( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, @@ -708,7 +728,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative66( "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, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta); } template diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index d27e43040..6d6f41fdd 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -143,6 +143,13 @@ Vector6 f6(const double x1, const double x2, const double x3, const double x4, return result; } +Vector g6(const double x1, const double x2, const double x3, const double x4, + const double x5, const double x6) { + Vector result(6); + result << sin(x1), cos(x2), x3 * x3, x4 * x4 * x4, sqrt(x5), sin(x6) - cos(x6); + return result; +} + /* ************************************************************************* */ // TEST(testNumericalDerivative, numeriDerivative61) { @@ -153,6 +160,14 @@ TEST(testNumericalDerivative, numeriDerivative61) { double, double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected61, actual61, 1e-5)); + + Matrix expected61Dynamic = Matrix::Zero(6, 1); + expected61Dynamic(0, 0) = cos(x1); + Matrix actual61Dynamic = + numericalDerivative61(g6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected61Dynamic, actual61Dynamic, 1e-5)); } /* ************************************************************************* */ @@ -165,6 +180,13 @@ TEST(testNumericalDerivative, numeriDerivative62) { double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected62, actual62, 1e-5)); + + Matrix expected62Dynamic = Matrix::Zero(6, 1); + expected62Dynamic(1, 0) = -sin(x2); + Matrix61 actual62Dynamic = numericalDerivative62(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected62Dynamic, actual62Dynamic, 1e-5)); } /* ************************************************************************* */ @@ -177,6 +199,14 @@ TEST(testNumericalDerivative, numeriDerivative63) { double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected63, actual63, 1e-5)); + + Matrix expected63Dynamic = Matrix::Zero(6, 1); + expected63Dynamic(2, 0) = 2 * x3; + Matrix61 actual63Dynamic = + numericalDerivative63(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected63Dynamic, actual63Dynamic, 1e-5)); } /* ************************************************************************* */ @@ -189,6 +219,14 @@ TEST(testNumericalDerivative, numeriDerivative64) { double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected64, actual64, 1e-5)); + + Matrix expected64Dynamic = Matrix::Zero(6, 1); + expected64Dynamic(3, 0) = 3 * x4 * x4; + Matrix61 actual64Dynamic = + numericalDerivative64(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected64Dynamic, actual64Dynamic, 1e-5)); } /* ************************************************************************* */ @@ -201,6 +239,14 @@ TEST(testNumericalDerivative, numeriDerivative65) { double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected65, actual65, 1e-5)); + + Matrix expected65Dynamic = Matrix::Zero(6, 1); + expected65Dynamic(4, 0) = 0.5 / sqrt(x5); + Matrix61 actual65Dynamic = + numericalDerivative65(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected65Dynamic, actual65Dynamic, 1e-5)); } /* ************************************************************************* */ @@ -213,6 +259,14 @@ TEST(testNumericalDerivative, numeriDerivative66) { double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected66, actual66, 1e-5)); + + Matrix expected66Dynamic = Matrix::Zero(6, 1); + expected66Dynamic(5, 0) = cos(x6) + sin(x6); + Matrix61 actual66Dynamic = + numericalDerivative66(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected66Dynamic, actual66Dynamic, 1e-5)); } /* ************************************************************************* */