12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Removed Vector-specific versions of NumericalDerivative, instead Vector is now a Lie object

release/4.3a0
Richard Roberts 2010-01-10 18:23:47 +00:00
parent 1a96ef41cf
commit 88ae5380a8
5 changed files with 22 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cpp/Makefile.am
View File

@ -40,7 +40,7 @@ clean_external_libs:
# we specify the library in levels # we specify the library in levels
# basic Math # basic Math
sources = Vector.cpp svdcmp.cpp Matrix.cpp numericalDerivative.cpp sources = Vector.cpp svdcmp.cpp Matrix.cpp
check_PROGRAMS = testVector testMatrix check_PROGRAMS = testVector testMatrix
testVector_SOURCES = testVector.cpp testVector_SOURCES = testVector.cpp
testVector_LDADD = libgtsam.la testVector_LDADD = libgtsam.la

60
cpp/numericalDerivative.cpp
View File

@ -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<n;j++) {
x(j) += delta; Vector hxplus = h(x);
x(j) -= 2*delta; Vector hxmin = h(x);
x(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
return H;
}
/* ************************************************************************* */
Matrix NumericalDerivative21
(Vector (*h)(const Vector&, const Vector&), const Vector& x1_, const Vector& x2, double delta) {
Vector x1(x1_), hx = h(x1,x2);
double factor = 1.0/(2.0*delta);
const size_t m = hx.size(), n = x1.size();
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
x1(j) += delta; Vector hxplus = h(x1,x2);
x1(j) -= 2*delta; Vector hxmin = h(x1,x2);
x1(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
return H;
}
/* ************************************************************************* */
Matrix NumericalDerivative22
(Vector (*h)(const Vector&, const Vector&), const Vector& x1, const Vector& x2_, double delta) {
Vector x2(x2_), hx = h(x1,x2);
double factor = 1.0/(2.0*delta);
const size_t m = hx.size(), n = x2.size();
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
x2(j) += delta; Vector hxplus = h(x1,x2);
x2(j) -= 2*delta; Vector hxmin = h(x1,x2);
x2(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
return H;
}
/* ************************************************************************* */
}

54
cpp/numericalDerivative.h
View File

@ -15,9 +15,9 @@
namespace gtsam { namespace gtsam {
/** /**
* Numerically compute gradient of scalar function * Numerically compute gradient of scalar function
* Class X is the input argument * Class X is the input argument
* The class X needs to have dim, expmap, vector * The class X needs to have dim, expmap, logmap
*/ */
template<class X> template<class X>
Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) { 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 h unary function yielding m-vector
* @param x n-dimensional value at which to evaluate h * @param x n-dimensional value at which to evaluate h
* @param delta increment for numerical derivative * @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 * @return m*n Jacobian computed via central differencing
*/ * Both classes X,Y need dim, expmap, logmap
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
*/ */
template<class Y, class X> template<class Y, class X>
Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) { 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 x2 second argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @return m*n Jacobian computed via central differencing
*/ * All classes Y,X1,X2 need dim, expmap, logmap
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
*/ */
template<class Y, class X1, class X2> template<class Y, class X1, class X2>
Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), Matrix numericalDerivative21(Y (*h)(const X1&, const X2&),
@ -114,18 +102,12 @@ namespace gtsam {
* @param x2 n-dimensional second argument value * @param x2 n-dimensional second argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @return m*n Jacobian computed via central differencing
*/ * All classes Y,X1,X2 need dim, expmap, logmap
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
*/ */
template<class Y, class X1, class X2> template<class Y, class X1, class X2>
Matrix numericalDerivative22 Matrix numericalDerivative22
(Y (*h)(const X1&, const X2&), (Y (*h)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta=1e-5) const X1& x1, const X2& x2, double delta=1e-5)
{ {
Y hx = h(x1,x2); Y hx = h(x1,x2);
double factor = 1.0/(2.0*delta); double factor = 1.0/(2.0*delta);
@ -153,18 +135,12 @@ namespace gtsam {
* @param x2 second argument value * @param x2 second argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @return m*n Jacobian computed via central differencing
*/ * All classes Y,X1,X2,X3 need dim, expmap, logmap
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
*/ */
template<class Y, class X1, class X2, class X3> template<class Y, class X1, class X2, class X3>
Matrix numericalDerivative31 Matrix numericalDerivative31
(Y (*h)(const X1&, const X2&, const X3&), (Y (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) const X1& x1, const X2& x2, const X3& x3, double delta=1e-5)
{ {
Y hx = h(x1,x2,x3); Y hx = h(x1,x2,x3);
double factor = 1.0/(2.0*delta); double factor = 1.0/(2.0*delta);

6
cpp/testSimulated2D.cpp
View File

@ -18,7 +18,7 @@ using namespace std;
TEST( simulated2D, Dprior ) TEST( simulated2D, Dprior )
{ {
Vector x(2);x(0)=1;x(1)=-9; Vector x(2);x(0)=1;x(1)=-9;
Matrix numerical = NumericalDerivative11(prior,x); Matrix numerical = numericalDerivative11(prior,x);
Matrix computed = Dprior(x); Matrix computed = Dprior(x);
CHECK(assert_equal(numerical,computed,1e-9)); CHECK(assert_equal(numerical,computed,1e-9));
} }
@ -28,7 +28,7 @@ TEST( simulated2D, Dprior )
{ {
Vector x1(2);x1(0)= 1;x1(1)=-9; Vector x1(2);x1(0)= 1;x1(1)=-9;
Vector x2(2);x2(0)=-5;x2(1)= 6; 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); Matrix computed = Dodo1(x1,x2);
CHECK(assert_equal(numerical,computed,1e-9)); CHECK(assert_equal(numerical,computed,1e-9));
} }
@ -38,7 +38,7 @@ TEST( simulated2D, Dprior )
{ {
Vector x1(2);x1(0)= 1;x1(1)=-9; Vector x1(2);x1(0)= 1;x1(1)=-9;
Vector x2(2);x2(0)=-5;x2(1)= 6; 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); Matrix computed = Dodo2(x1,x2);
CHECK(assert_equal(numerical,computed,1e-9)); CHECK(assert_equal(numerical,computed,1e-9));
} }

6
cpp/testSimulated3D.cpp
View File

@ -18,7 +18,7 @@ TEST( simulated3D, Dprior_3D )
{ {
Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0)); Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0));
Vector v = logmap(x1); Vector v = logmap(x1);
Matrix numerical = NumericalDerivative11(prior_3D,v); Matrix numerical = numericalDerivative11(prior_3D,v);
Matrix computed = Dprior_3D(v); Matrix computed = Dprior_3D(v);
CHECK(assert_equal(numerical,computed,1e-9)); CHECK(assert_equal(numerical,computed,1e-9));
} }
@ -30,7 +30,7 @@ TEST( simulated3D, DOdo1 )
Vector v1 = logmap(x1); Vector v1 = logmap(x1);
Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0)); Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0));
Vector v2 = logmap(x2); Vector v2 = logmap(x2);
Matrix numerical = NumericalDerivative21(odo_3D,v1,v2); Matrix numerical = numericalDerivative21(odo_3D,v1,v2);
Matrix computed = Dodo1_3D(v1,v2); Matrix computed = Dodo1_3D(v1,v2);
CHECK(assert_equal(numerical,computed,1e-9)); CHECK(assert_equal(numerical,computed,1e-9));
} }
@ -42,7 +42,7 @@ TEST( simulated3D, DOdo2 )
Vector v1 = logmap(x1); Vector v1 = logmap(x1);
Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0)); Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0));
Vector v2 = logmap(x2); Vector v2 = logmap(x2);
Matrix numerical = NumericalDerivative22(odo_3D,v1,v2); Matrix numerical = numericalDerivative22(odo_3D,v1,v2);
Matrix computed = Dodo2_3D(v1,v2); Matrix computed = Dodo2_3D(v1,v2);
CHECK(assert_equal(numerical,computed,1e-9)); CHECK(assert_equal(numerical,computed,1e-9));
} }