From 9b402e478c2544d5316ed98c328225969b9297a3 Mon Sep 17 00:00:00 2001
From: Cong Li <cli@nuro.ai>
Date: Mon, 25 Nov 2019 14:49:40 -0800
Subject: [PATCH] compute numerical derivative for 6-argument function and add
 test code for this, also fix bugs in computing numerical derivative in
 5-argument function

---
 gtsam/base/numericalDerivative.h             | 180 ++++++++++++++++++-
 gtsam/base/tests/testNumericalDerivative.cpp |  80 +++++++++
 2 files changed, 257 insertions(+), 3 deletions(-)

diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h
index a9a088108..831bb6bcc 100644
--- a/gtsam/base/numericalDerivative.h
+++ b/gtsam/base/numericalDerivative.h
@@ -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.
diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp
index 1cbf71e1f..d27e43040 100644
--- a/gtsam/base/tests/testNumericalDerivative.cpp
+++ b/gtsam/base/tests/testNumericalDerivative.cpp
@@ -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;