From 709d18a83dc4e1dbe1f11de54a737e9cf5cd494f Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 6 Jun 2013 02:09:52 +0000
Subject: [PATCH] A more modern (and presumably faster) Eigen syntax

---
 gtsam/base/numericalDerivative.h | 26 ++++++++++++--------------
 1 file changed, 12 insertions(+), 14 deletions(-)

diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h
index 429920e1c..053a89da8 100644
--- a/gtsam/base/numericalDerivative.h
+++ b/gtsam/base/numericalDerivative.h
@@ -32,8 +32,6 @@
 #include <gtsam/base/LieVector.h>
 #include <gtsam/base/Matrix.h>
 
-
-
 namespace gtsam {
 
   /*
@@ -106,8 +104,8 @@ namespace gtsam {
     for (size_t j=0;j<n;j++) {
       d(j) +=   delta; Vector hxplus = hx.localCoordinates(h(x.retract(d)));
       d(j) -= 2*delta; Vector hxmin  = hx.localCoordinates(h(x.retract(d)));
-      d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
-      for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+      d(j) +=   delta;
+      H.col(j) << (hxplus-hxmin)*factor;
     }
     return H;
   }
@@ -160,8 +158,8 @@ namespace gtsam {
     for (size_t j=0;j<n;j++) {
       d(j) +=   delta; Vector hxplus = hx.localCoordinates(h(x1.retract(d),x2));
       d(j) -= 2*delta; Vector hxmin  = hx.localCoordinates(h(x1.retract(d),x2));
-      d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
-      for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+      d(j) +=   delta;
+      H.col(j) << (hxplus-hxmin)*factor;
     }
     return H;
   }
@@ -224,8 +222,8 @@ namespace gtsam {
     for (size_t j=0;j<n;j++) {
       d(j) +=   delta; Vector hxplus = hx.localCoordinates(h(x1,x2.retract(d)));
       d(j) -= 2*delta; Vector hxmin  = hx.localCoordinates(h(x1,x2.retract(d)));
-      d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
-      for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+      d(j) +=   delta;
+      H.col(j) << (hxplus-hxmin)*factor;
     }
     return H;
   }
@@ -290,8 +288,8 @@ namespace gtsam {
     for (size_t j=0;j<n;j++) {
       d(j) +=   delta; Vector hxplus = hx.localCoordinates(h(x1.retract(d),x2,x3));
       d(j) -= 2*delta; Vector hxmin  = hx.localCoordinates(h(x1.retract(d),x2,x3));
-      d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
-      for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+      d(j) +=   delta;
+      H.col(j) << (hxplus-hxmin)*factor;
     }
     return H;
   }
@@ -355,8 +353,8 @@ namespace gtsam {
     for (size_t j=0;j<n;j++) {
       d(j) +=   delta; Vector hxplus = hx.localCoordinates(h(x1, x2.retract(d),x3));
       d(j) -= 2*delta; Vector hxmin  = hx.localCoordinates(h(x1, x2.retract(d),x3));
-      d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
-      for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+      d(j) +=   delta;
+      H.col(j) << (hxplus-hxmin)*factor;
     }
     return H;
   }
@@ -420,8 +418,8 @@ namespace gtsam {
     for (size_t j=0;j<n;j++) {
       d(j) +=   delta; Vector hxplus = hx.localCoordinates(h(x1, x2, x3.retract(d)));
       d(j) -= 2*delta; Vector hxmin  = hx.localCoordinates(h(x1, x2, x3.retract(d)));
-      d(j) +=   delta; Vector dh = (hxplus-hxmin)*factor;
-      for (size_t i=0;i<m;i++) H(i,j) = dh(i);
+      d(j) +=   delta;
+      H.col(j) << (hxplus-hxmin)*factor;
     }
     return H;
   }