Merge pull request #174 from baladeer/cli/numericalDerivativeForSixArgumentFunction
compute numerical derivative for 6-argument functionrelease/4.3a0
commit
e90e1f1dd2
|
@ -453,7 +453,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
|
|||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5>
|
||||
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
|
||||
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
|
||||
return numericalDerivative52<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
|
||||
}
|
||||
|
@ -509,7 +509,7 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
|
|||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5>
|
||||
inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
|
||||
inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
|
||||
return numericalDerivative54<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
|
||||
}
|
||||
|
@ -537,11 +537,185 @@ typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
|
|||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5>
|
||||
inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
|
||||
inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
|
||||
return numericalDerivative55<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical derivative in argument 1 of 6-argument function
|
||||
* @param h quintic 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 x5 fifth argument value
|
||||
* @param x6 sixth argument value
|
||||
* @param delta increment for numerical derivative
|
||||
* @return m*n Jacobian computed via central differencing
|
||||
*/
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
|
||||
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(
|
||||
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
|
||||
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
|
||||
"Template argument Y must be a manifold type.");
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
|
||||
"Template argument X1 must be a manifold type.");
|
||||
return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta);
|
||||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
|
||||
inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
|
||||
return numericalDerivative61<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical derivative in argument 2 of 6-argument function
|
||||
* @param h quintic 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 x5 fifth argument value
|
||||
* @param x6 sixth argument value
|
||||
* @param delta increment for numerical derivative
|
||||
* @return m*n Jacobian computed via central differencing
|
||||
*/
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
|
||||
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(
|
||||
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
|
||||
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
|
||||
"Template argument Y must be a manifold type.");
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
|
||||
"Template argument X1 must be a manifold type.");
|
||||
return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta);
|
||||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
|
||||
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
|
||||
return numericalDerivative62<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical derivative in argument 3 of 6-argument function
|
||||
* @param h quintic 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 x5 fifth argument value
|
||||
* @param x6 sixth argument value
|
||||
* @param delta increment for numerical derivative
|
||||
* @return m*n Jacobian computed via central differencing
|
||||
*/
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
|
||||
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(
|
||||
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
|
||||
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
|
||||
"Template argument Y must be a manifold type.");
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
|
||||
"Template argument X1 must be a manifold type.");
|
||||
return numericalDerivative11<Y, X3>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta);
|
||||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
|
||||
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
|
||||
return numericalDerivative63<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical derivative in argument 4 of 6-argument function
|
||||
* @param h quintic 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 x5 fifth argument value
|
||||
* @param x6 sixth argument value
|
||||
* @param delta increment for numerical derivative
|
||||
* @return m*n Jacobian computed via central differencing
|
||||
*/
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
|
||||
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(
|
||||
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
|
||||
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
|
||||
"Template argument Y must be a manifold type.");
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
|
||||
"Template argument X1 must be a manifold type.");
|
||||
return numericalDerivative11<Y, X4>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5), boost::cref(x6)), x4, delta);
|
||||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
|
||||
inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
|
||||
return numericalDerivative64<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical derivative in argument 5 of 6-argument function
|
||||
* @param h quintic 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 x5 fifth argument value
|
||||
* @param x6 sixth argument value
|
||||
* @param delta increment for numerical derivative
|
||||
* @return m*n Jacobian computed via central differencing
|
||||
*/
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
|
||||
typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(
|
||||
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
|
||||
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
|
||||
"Template argument Y must be a manifold type.");
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
|
||||
"Template argument X1 must be a manifold type.");
|
||||
return numericalDerivative11<Y, X5>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1, boost::cref(x6)), x5, delta);
|
||||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
|
||||
inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
|
||||
return numericalDerivative65<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical derivative in argument 6 of 6-argument function
|
||||
* @param h quintic 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 x5 fifth argument value
|
||||
* @param x6 sixth argument value
|
||||
* @param delta increment for numerical derivative
|
||||
* @return m*n Jacobian computed via central differencing
|
||||
*/
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
|
||||
typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative66(
|
||||
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
|
||||
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
|
||||
"Template argument Y must be a manifold type.");
|
||||
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
|
||||
"Template argument X1 must be a manifold type.");
|
||||
return numericalDerivative11<Y, X6>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta);
|
||||
}
|
||||
|
||||
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
|
||||
inline typename internal::FixedSizeMatrix<Y,X6>::type numericalDerivative66(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
|
||||
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
|
||||
return numericalDerivative66<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, _1, _2, _3, _4, _5, _6), x1, x2, x3, x4, x5, x6);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical Hessian matrix. Requires a single-argument Lie->scalar
|
||||
* function. This is implemented simply as the derivative of the gradient.
|
||||
|
|
|
@ -135,6 +135,86 @@ TEST(testNumericalDerivative, numericalHessian311) {
|
|||
EXPECT(assert_equal(expected33, actual33, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector6 f6(const double x1, const double x2, const double x3, const double x4,
|
||||
const double x5, const double x6) {
|
||||
Vector6 result;
|
||||
result << sin(x1), cos(x2), x3 * x3, x4 * x4 * x4, sqrt(x5), sin(x6) - cos(x6);
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//
|
||||
TEST(testNumericalDerivative, numeriDerivative61) {
|
||||
double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
|
||||
|
||||
Matrix expected61 = (Matrix(6, 1) << cos(x1), 0, 0, 0, 0, 0).finished();
|
||||
Matrix61 actual61 = numericalDerivative61<Vector6, double, double,
|
||||
double, double, double, double>(f6, x1, x2, x3, x4, x5, x6);
|
||||
|
||||
EXPECT(assert_equal(expected61, actual61, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//
|
||||
TEST(testNumericalDerivative, numeriDerivative62) {
|
||||
double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
|
||||
|
||||
Matrix expected62 = (Matrix(6, 1) << 0, -sin(x2), 0, 0, 0, 0).finished();
|
||||
Matrix61 actual62 = numericalDerivative62<Vector6, double, double, double,
|
||||
double, double, double>(f6, x1, x2, x3, x4, x5, x6);
|
||||
|
||||
EXPECT(assert_equal(expected62, actual62, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//
|
||||
TEST(testNumericalDerivative, numeriDerivative63) {
|
||||
double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
|
||||
|
||||
Matrix expected63 = (Matrix(6, 1) << 0, 0, 2 * x3, 0, 0, 0).finished();
|
||||
Matrix61 actual63 = numericalDerivative63<Vector6, double, double, double,
|
||||
double, double, double>(f6, x1, x2, x3, x4, x5, x6);
|
||||
|
||||
EXPECT(assert_equal(expected63, actual63, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//
|
||||
TEST(testNumericalDerivative, numeriDerivative64) {
|
||||
double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
|
||||
|
||||
Matrix expected64 = (Matrix(6, 1) << 0, 0, 0, 3 * x4 * x4, 0, 0).finished();
|
||||
Matrix61 actual64 = numericalDerivative64<Vector6, double, double, double,
|
||||
double, double, double>(f6, x1, x2, x3, x4, x5, x6);
|
||||
|
||||
EXPECT(assert_equal(expected64, actual64, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//
|
||||
TEST(testNumericalDerivative, numeriDerivative65) {
|
||||
double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
|
||||
|
||||
Matrix expected65 = (Matrix(6, 1) << 0, 0, 0, 0, 0.5 / sqrt(x5), 0).finished();
|
||||
Matrix61 actual65 = numericalDerivative65<Vector6, double, double, double,
|
||||
double, double, double>(f6, x1, x2, x3, x4, x5, x6);
|
||||
|
||||
EXPECT(assert_equal(expected65, actual65, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//
|
||||
TEST(testNumericalDerivative, numeriDerivative66) {
|
||||
double x1 = 1, x2 = 2, x3 = 3 , x4 = 4, x5 = 5, x6 = 6;
|
||||
|
||||
Matrix expected66 = (Matrix(6, 1) << 0, 0, 0, 0, 0, cos(x6) + sin(x6)).finished();
|
||||
Matrix61 actual66 = numericalDerivative66<Vector6, double, double, double,
|
||||
double, double, double>(f6, x1, x2, x3, x4, x5, x6);
|
||||
|
||||
EXPECT(assert_equal(expected66, actual66, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
Loading…
Reference in New Issue