diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 5587cfd69..aaeaeba3c 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -40,7 +40,7 @@ clean_external_libs: # we specify the library in levels # basic Math -sources = Vector.cpp svdcmp.cpp Matrix.cpp numericalDerivative.cpp +sources = Vector.cpp svdcmp.cpp Matrix.cpp check_PROGRAMS = testVector testMatrix testVector_SOURCES = testVector.cpp testVector_LDADD = libgtsam.la diff --git a/cpp/numericalDerivative.cpp b/cpp/numericalDerivative.cpp deleted file mode 100644 index ffcaaf34f..000000000 --- a/cpp/numericalDerivative.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/** - * @file numericalDerivative.cpp - * @brief Some functions to compute numerical derivatives - * @author Frank Dellaert - */ - -#include "numericalDerivative.h" - -namespace gtsam { - -/* ************************************************************************* */ - Matrix NumericalDerivative11 - (Vector (*h)(const Vector&), const Vector& x_, double delta) { - Vector x(x_), hx = h(x); - double factor = 1.0/(2.0*delta); - const size_t m = hx.size(), n = x.size(); - Matrix H = zeros(m,n); - for (size_t j=0;j Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) { @@ -38,16 +38,10 @@ namespace gtsam { * @param h unary function yielding m-vector * @param x n-dimensional value at which to evaluate h * @param delta increment for numerical derivative + * Class Y is the output argument + * Class X is the input argument * @return m*n Jacobian computed via central differencing - */ - Matrix NumericalDerivative11 - (Vector (*h)(const Vector&), const Vector& x, double delta=1e-5); - - /** - * Templated version (starts with LOWERCASE n) - * Class Y is the output argument - * Class X is the input argument - * Both classes X,Y need dim, expmap, vector + * Both classes X,Y need dim, expmap, logmap */ template Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) { @@ -77,13 +71,7 @@ namespace gtsam { * @param x2 second argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing - */ - Matrix NumericalDerivative21(Vector (*h)(const Vector&, const Vector&), - const Vector& x1, const Vector& x2, double delta=1e-5); - - /** - * Templated version (starts with LOWERCASE n) - * All classes Y,X1,X2 need dim, expmap, vector + * All classes Y,X1,X2 need dim, expmap, logmap */ template Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), @@ -114,18 +102,12 @@ namespace gtsam { * @param x2 n-dimensional second argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing - */ - Matrix NumericalDerivative22 - (Vector (*h)(const Vector&, const Vector&), const Vector& x1, const Vector& x2, double delta=1e-5); - - /** - * Templated version (starts with LOWERCASE n) - * All classes Y,X1,X2 need dim, expmap, vector + * All classes Y,X1,X2 need dim, expmap, logmap */ template - Matrix numericalDerivative22 - (Y (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) + Matrix numericalDerivative22 + (Y (*h)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta=1e-5) { Y hx = h(x1,x2); double factor = 1.0/(2.0*delta); @@ -153,18 +135,12 @@ namespace gtsam { * @param x2 second argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing - */ - Matrix NumericalDerivative31 - (Vector (*h)(const Vector&, const Vector&, const Vector&), const Vector& x1, const Vector& x2, const Vector& x3, double delta=1e-5); - - /** - * Templated version (starts with LOWERCASE n) - * All classes Y,X1,X2,X3 need dim, expmap, vector + * All classes Y,X1,X2,X3 need dim, expmap, logmap */ template - Matrix numericalDerivative31 - (Y (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) + Matrix numericalDerivative31 + (Y (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { Y hx = h(x1,x2,x3); double factor = 1.0/(2.0*delta); diff --git a/cpp/testSimulated2D.cpp b/cpp/testSimulated2D.cpp index d248c6197..3229acba1 100644 --- a/cpp/testSimulated2D.cpp +++ b/cpp/testSimulated2D.cpp @@ -18,7 +18,7 @@ using namespace std; TEST( simulated2D, Dprior ) { Vector x(2);x(0)=1;x(1)=-9; - Matrix numerical = NumericalDerivative11(prior,x); + Matrix numerical = numericalDerivative11(prior,x); Matrix computed = Dprior(x); CHECK(assert_equal(numerical,computed,1e-9)); } @@ -28,7 +28,7 @@ TEST( simulated2D, Dprior ) { Vector x1(2);x1(0)= 1;x1(1)=-9; Vector x2(2);x2(0)=-5;x2(1)= 6; - Matrix numerical = NumericalDerivative21(odo,x1,x2); + Matrix numerical = numericalDerivative21(odo,x1,x2); Matrix computed = Dodo1(x1,x2); CHECK(assert_equal(numerical,computed,1e-9)); } @@ -38,7 +38,7 @@ TEST( simulated2D, Dprior ) { Vector x1(2);x1(0)= 1;x1(1)=-9; Vector x2(2);x2(0)=-5;x2(1)= 6; - Matrix numerical = NumericalDerivative22(odo,x1,x2); + Matrix numerical = numericalDerivative22(odo,x1,x2); Matrix computed = Dodo2(x1,x2); CHECK(assert_equal(numerical,computed,1e-9)); } diff --git a/cpp/testSimulated3D.cpp b/cpp/testSimulated3D.cpp index cccdf8aae..ef62a722c 100644 --- a/cpp/testSimulated3D.cpp +++ b/cpp/testSimulated3D.cpp @@ -18,7 +18,7 @@ TEST( simulated3D, Dprior_3D ) { Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0)); Vector v = logmap(x1); - Matrix numerical = NumericalDerivative11(prior_3D,v); + Matrix numerical = numericalDerivative11(prior_3D,v); Matrix computed = Dprior_3D(v); CHECK(assert_equal(numerical,computed,1e-9)); } @@ -30,7 +30,7 @@ TEST( simulated3D, DOdo1 ) Vector v1 = logmap(x1); Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0)); Vector v2 = logmap(x2); - Matrix numerical = NumericalDerivative21(odo_3D,v1,v2); + Matrix numerical = numericalDerivative21(odo_3D,v1,v2); Matrix computed = Dodo1_3D(v1,v2); CHECK(assert_equal(numerical,computed,1e-9)); } @@ -42,7 +42,7 @@ TEST( simulated3D, DOdo2 ) Vector v1 = logmap(x1); Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0)); Vector v2 = logmap(x2); - Matrix numerical = NumericalDerivative22(odo_3D,v1,v2); + Matrix numerical = numericalDerivative22(odo_3D,v1,v2); Matrix computed = Dodo2_3D(v1,v2); CHECK(assert_equal(numerical,computed,1e-9)); }