From 30b21503b336b5d773e628a00facbb4ba59615c7 Mon Sep 17 00:00:00 2001
From: Mike Bosse <mike.bosse@mavt.ethz.ch>
Date: Mon, 15 Dec 2014 16:11:28 +0100
Subject: [PATCH] fixed size matrices for numerical derivatives

---
 gtsam/base/Lie.h                 |  8 +--
 gtsam/base/numericalDerivative.h | 91 +++++++++++++++++++-------------
 2 files changed, 56 insertions(+), 43 deletions(-)

diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h
index 7a9d32249..7953bf511 100644
--- a/gtsam/base/Lie.h
+++ b/gtsam/base/Lie.h
@@ -147,11 +147,7 @@ T expm(const Vector& x, int K=7) {
  * the gtsam namespace to be more easily enforced as testable
  */
 #define GTSAM_CONCEPT_LIE_INST(T) \
-    template class gtsam::ManifoldConcept<T>; \
-    template class gtsam::GroupConcept<T>; \
-    template class gtsam::LieConcept<T>;
+    template class gtsam::IsLieGroup<T>;
 
 #define GTSAM_CONCEPT_LIE_TYPE(T) \
-    typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T; \
-    typedef gtsam::GroupConcept<T> _gtsam_GroupConcept_##T; \
-    typedef gtsam::LieConcept<T> _gtsam_LieConcept_##T;
+    typedef gtsam::IsLieGroup<T> _gtsam_LieConcept_##T;
diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h
index 0a783037a..f159d43ff 100644
--- a/gtsam/base/numericalDerivative.h
+++ b/gtsam/base/numericalDerivative.h
@@ -59,13 +59,22 @@ namespace gtsam {
  *     http://www.boost.org/doc/libs/release/libs/bind/bind.html
  */
 
+
+// a quick helper struct to get the appropriate fixed sized matrix from two value types
+namespace internal {
+template<class Y, class X=double>
+struct FixedSizeMatrix {
+  typedef Eigen::Matrix<double,traits_x<Y>::dimension, traits_x<X>::dimension> type;
+};
+}
+
 /**
  * Numerically compute gradient of scalar function
  * Class X is the input argument
  * The class X needs to have dim, expmap, logmap
  */
 template<class X>
-Vector numericalGradient(boost::function<double(const X&)> h, const X& x,
+typename internal::FixedSizeMatrix<X>::type numericalGradient(boost::function<double(const X&)> h, const X& x,
     double delta = 1e-5) {
   double factor = 1.0 / (2.0 * delta);
 
@@ -103,12 +112,15 @@ Vector numericalGradient(boost::function<double(const X&)> h, const X& x,
  * Class X is the input argument
  * @return m*n Jacobian computed via central differencing
  */
+
 template<class Y, class X>
 // TODO Should compute fixed-size matrix
-Matrix numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
+typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
     double delta = 1e-5) {
   using namespace traits;
 
+  typedef typename internal::FixedSizeMatrix<Y,X>::type Matrix;
+
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits_x<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   typedef traits_x<Y> TraitsY;
@@ -148,7 +160,7 @@ Matrix numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
 
 /** use a raw C++ function pointer */
 template<class Y, class X>
-Matrix numericalDerivative11(Y (*h)(const X&), const X& x,
+typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const X&), const X& x,
     double delta = 1e-5) {
   return numericalDerivative11<Y, X>(boost::bind(h, _1), x, delta);
 }
@@ -162,7 +174,7 @@ Matrix numericalDerivative11(Y (*h)(const X&), const X& x,
  * @return m*n Jacobian computed via central differencing
  */
 template<class Y, class X1, class X2>
-Matrix numericalDerivative21(const boost::function<Y(const X1&, const X2&)>& h,
+typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const boost::function<Y(const X1&, const X2&)>& h,
     const X1& x1, const X2& x2, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits_x<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
@@ -173,7 +185,7 @@ Matrix numericalDerivative21(const boost::function<Y(const X1&, const X2&)>& h,
 
 /** use a raw C++ function pointer */
 template<class Y, class X1, class X2>
-inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
+typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
     const X2& x2, double delta = 1e-5) {
   return numericalDerivative21<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
 }
@@ -187,7 +199,7 @@ inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
  * @return m*n Jacobian computed via central differencing
  */
 template<class Y, class X1, class X2>
-Matrix numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
+typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
     const X1& x1, const X2& x2, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits_x<X1>::structure_category>::value),
        "Template argument X1 must be a manifold type.");
@@ -198,7 +210,7 @@ Matrix numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
 
 /** use a raw C++ function pointer */
 template<class Y, class X1, class X2>
-inline Matrix numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
+typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
     const X2& x2, double delta = 1e-5) {
   return numericalDerivative22<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
 }
@@ -214,7 +226,7 @@ inline Matrix numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
  * All classes Y,X1,X2,X3 need dim, expmap, logmap
  */
 template<class Y, class X1, class X2, class X3>
-Matrix numericalDerivative31(
+typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
     boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits_x<Y>::structure_category>::value),
@@ -225,7 +237,7 @@ Matrix numericalDerivative31(
 }
 
 template<class Y, class X1, class X2, class X3>
-inline Matrix numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&),
+typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalDerivative31<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
       x2, x3, delta);
@@ -242,7 +254,7 @@ inline Matrix numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&),
  * All classes Y,X1,X2,X3 need dim, expmap, logmap
  */
 template<class Y, class X1, class X2, class X3>
-Matrix numericalDerivative32(
+typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
     boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits_x<Y>::structure_category>::value),
@@ -253,7 +265,7 @@ Matrix numericalDerivative32(
 }
 
 template<class Y, class X1, class X2, class X3>
-inline Matrix numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&),
+inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalDerivative32<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
       x2, x3, delta);
@@ -270,7 +282,7 @@ inline Matrix numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&),
  * All classes Y,X1,X2,X3 need dim, expmap, logmap
  */
 template<class Y, class X1, class X2, class X3>
-Matrix numericalDerivative33(
+typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
     boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits_x<Y>::structure_category>::value),
@@ -281,7 +293,7 @@ Matrix numericalDerivative33(
 }
 
 template<class Y, class X1, class X2, class X3>
-inline Matrix numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&),
+inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&),
     const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalDerivative33<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
       x2, x3, delta);
@@ -296,7 +308,7 @@ inline Matrix numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&),
  * @return n*n Hessian matrix computed via central differencing
  */
 template<class X>
-inline Matrix numericalHessian(boost::function<double(const X&)> f, const X& x,
+inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(boost::function<double(const X&)> f, const X& x,
     double delta = 1e-5) {
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits_x<X>::structure_category>::value),
       "Template argument X must be a manifold type.");
@@ -309,7 +321,7 @@ inline Matrix numericalHessian(boost::function<double(const X&)> f, const X& x,
 }
 
 template<class X>
-inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta =
+inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(double (*f)(const X&), const X& x, double delta =
     1e-5) {
   return numericalHessian(boost::function<double(const X&)>(f), x, delta);
 }
@@ -323,6 +335,8 @@ class G_x1 {
   X1 x1_;
   double delta_;
 public:
+  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
+
   G_x1(const boost::function<double(const X1&, const X2&)>& f, const X1& x1,
       double delta) :
       f_(f), x1_(x1), delta_(delta) {
@@ -333,9 +347,10 @@ public:
 };
 
 template<class X1, class X2>
-inline Matrix numericalHessian212(
+inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(
     boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
     double delta = 1e-5) {
+  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
   G_x1<X1, X2> g_x1(f, x1, delta);
   return numericalDerivative11<Vector, X2>(
       boost::function<Vector(const X2&)>(
@@ -343,17 +358,19 @@ inline Matrix numericalHessian212(
 }
 
 template<class X1, class X2>
-inline Matrix numericalHessian212(double (*f)(const X1&, const X2&),
+inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(double (*f)(const X1&, const X2&),
     const X1& x1, const X2& x2, double delta = 1e-5) {
   return numericalHessian212(boost::function<double(const X1&, const X2&)>(f),
       x1, x2, delta);
 }
 
 template<class X1, class X2>
-inline Matrix numericalHessian211(
+inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(
     boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
     double delta = 1e-5) {
 
+  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
+
   Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
       double) = &numericalGradient<X1>;
   boost::function<double(const X1&)> f2(boost::bind(f, _1, x2));
@@ -364,17 +381,17 @@ inline Matrix numericalHessian211(
 }
 
 template<class X1, class X2>
-inline Matrix numericalHessian211(double (*f)(const X1&, const X2&),
+inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(double (*f)(const X1&, const X2&),
     const X1& x1, const X2& x2, double delta = 1e-5) {
   return numericalHessian211(boost::function<double(const X1&, const X2&)>(f),
       x1, x2, delta);
 }
 
 template<class X1, class X2>
-inline Matrix numericalHessian222(
+inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(
     boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
     double delta = 1e-5) {
-
+  typedef typename internal::FixedSizeMatrix<X2>::type Vector;
   Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
       double) = &numericalGradient<X2>;
   boost::function<double(const X2&)> f2(boost::bind(f, x1, _1));
@@ -385,7 +402,7 @@ inline Matrix numericalHessian222(
 }
 
 template<class X1, class X2>
-inline Matrix numericalHessian222(double (*f)(const X1&, const X2&),
+inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(double (*f)(const X1&, const X2&),
     const X1& x1, const X2& x2, double delta = 1e-5) {
   return numericalHessian222(boost::function<double(const X1&, const X2&)>(f),
       x1, x2, delta);
@@ -396,10 +413,10 @@ inline Matrix numericalHessian222(double (*f)(const X1&, const X2&),
  */
 /* **************************************************************** */
 template<class X1, class X2, class X3>
-inline Matrix numericalHessian311(
+inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
-
+  typedef typename internal::FixedSizeMatrix<X1>::type Vector;
   Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
       double) = &numericalGradient<X1>;
   boost::function<double(const X1&)> f2(boost::bind(f, _1, x2, x3));
@@ -410,7 +427,7 @@ inline Matrix numericalHessian311(
 }
 
 template<class X1, class X2, class X3>
-inline Matrix numericalHessian311(double (*f)(const X1&, const X2&, const X3&),
+inline typename internal::FixedSizeMatrix<X1,X1>::type 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<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
@@ -419,10 +436,10 @@ inline Matrix numericalHessian311(double (*f)(const X1&, const X2&, const X3&),
 
 /* **************************************************************** */
 template<class X1, class X2, class X3>
-inline Matrix numericalHessian322(
+inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
-
+  typedef typename internal::FixedSizeMatrix<X2>::type Vector;
   Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
       double) = &numericalGradient<X2>;
   boost::function<double(const X2&)> f2(boost::bind(f, x1, _1, x3));
@@ -433,7 +450,7 @@ inline Matrix numericalHessian322(
 }
 
 template<class X1, class X2, class X3>
-inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&),
+inline typename internal::FixedSizeMatrix<X2,X2>::type 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<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
@@ -442,10 +459,10 @@ inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&),
 
 /* **************************************************************** */
 template<class X1, class X2, class X3>
-inline Matrix numericalHessian333(
+inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
-
+  typedef typename internal::FixedSizeMatrix<X3>::type Vector;
   Vector (*numGrad)(boost::function<double(const X3&)>, const X3&,
       double) = &numericalGradient<X3>;
   boost::function<double(const X3&)> f2(boost::bind(f, x1, x2, _1));
@@ -456,7 +473,7 @@ inline Matrix numericalHessian333(
 }
 
 template<class X1, class X2, class X3>
-inline Matrix numericalHessian333(double (*f)(const X1&, const X2&, const X3&),
+inline typename internal::FixedSizeMatrix<X3,X3>::type 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<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
@@ -465,7 +482,7 @@ inline Matrix numericalHessian333(double (*f)(const X1&, const X2&, const X3&),
 
 /* **************************************************************** */
 template<class X1, class X2, class X3>
-inline Matrix numericalHessian312(
+inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian212<X1, X2>(
@@ -474,7 +491,7 @@ inline Matrix numericalHessian312(
 }
 
 template<class X1, class X2, class X3>
-inline Matrix numericalHessian313(
+inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian212<X1, X3>(
@@ -483,7 +500,7 @@ inline Matrix numericalHessian313(
 }
 
 template<class X1, class X2, class X3>
-inline Matrix numericalHessian323(
+inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian212<X2, X3>(
@@ -493,7 +510,7 @@ inline Matrix numericalHessian323(
 
 /* **************************************************************** */
 template<class X1, class X2, class X3>
-inline Matrix numericalHessian312(double (*f)(const X1&, const X2&, const X3&),
+inline typename internal::FixedSizeMatrix<X1,X2>::type 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<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
@@ -501,7 +518,7 @@ inline Matrix numericalHessian312(double (*f)(const X1&, const X2&, const X3&),
 }
 
 template<class X1, class X2, class X3>
-inline Matrix numericalHessian313(double (*f)(const X1&, const X2&, const X3&),
+inline typename internal::FixedSizeMatrix<X1,X3>::type 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<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
@@ -509,7 +526,7 @@ inline Matrix numericalHessian313(double (*f)(const X1&, const X2&, const X3&),
 }
 
 template<class X1, class X2, class X3>
-inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&),
+inline typename internal::FixedSizeMatrix<X2,X3>::type 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<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,