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

Make it clear that argument types must be fixed-size (for now).

release/4.3a0
dellaert 2014-10-22 00:40:28 +02:00
parent 4b3e0dbcc0
commit 3b0d2a5f47
1 changed files with 16 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
gtsam/base/numericalDerivative.h
View File

@ -28,7 +28,6 @@
#pragma GCC diagnostic pop #pragma GCC diagnostic pop
#endif #endif
#include <gtsam/base/LieVector.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/Manifold.h> #include <gtsam/base/Manifold.h>
@ -56,14 +55,6 @@ namespace gtsam {
* http://www.boost.org/doc/libs/release/libs/bind/bind.html * http://www.boost.org/doc/libs/release/libs/bind/bind.html
*/ */
/** global functions for converting to a LieVector for use with numericalDerivative */
inline LieVector makeLieVector(const Vector& v) {
return LieVector(v);
}
inline LieVector makeLieVectorD(double d) {
return LieVector((Vector) (Vector(1) << d));
}
/** /**
* Numerically compute gradient of scalar function * Numerically compute gradient of scalar function
* Class X is the input argument * Class X is the input argument
@ -76,20 +67,20 @@ Vector numericalGradient(boost::function<double(const X&)> h, const X& x,
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X>::value, BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X>::value,
"Template argument X must be a manifold type."); "Template argument X must be a manifold type.");
static const int N = traits::dimension<X>::value;
BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type.");
typedef DefaultChart<X> ChartX; typedef DefaultChart<X> ChartX;
typedef typename ChartX::vector TangentX; typedef typename ChartX::vector TangentX;
// get chart at x // get chart at x
ChartX chartX(x); ChartX chartX(x);
TangentX zeroX = chartX.apply(x);
size_t n = zeroX.size(); // hack to get size if dynamic type
// Prepare a tangent vector to perturb x with, only works for fixed size // Prepare a tangent vector to perturb x with, only works for fixed size
TangentX d; TangentX d;
d.setZero(); d.setZero();
Vector g = zero(n); Vector g = zero(N); // Can be fixed size
for (int j = 0; j < n; j++) { for (int j = 0; j < N; j++) {
d(j) = delta; d(j) = delta;
double hxplus = h(chartX.retract(d)); double hxplus = h(chartX.retract(d));
d(j) = -delta; d(j) = -delta;
@ -123,28 +114,30 @@ Matrix numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X>::value, BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X>::value,
"Template argument X must be a manifold type."); "Template argument X must be a manifold type.");
static const int N = traits::dimension<X>::value;
BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type.");
typedef DefaultChart<X> ChartX; typedef DefaultChart<X> ChartX;
typedef typename ChartX::vector TangentX; typedef typename ChartX::vector TangentX;
// get value at x, and corresponding chart // get value at x, and corresponding chart
Y hx = h(x); Y hx = h(x);
ChartY chartY(hx); ChartY chartY(hx);
// Bit of a hack for now to find number of rows
TangentY zeroY = chartY.apply(hx); TangentY zeroY = chartY.apply(hx);
size_t m = zeroY.size(); size_t m = zeroY.size();
// get chart at x // get chart at x
ChartX chartX(x); ChartX chartX(x);
TangentX zeroX = chartX.apply(x);
size_t n = zeroX.size();
// Prepare a tangent vector to perturb x with, only works for fixed size // Prepare a tangent vector to perturb x with, only works for fixed size
TangentX dx; TangentX dx;
dx.setZero(); dx.setZero();
// Fill in Jacobian H // Fill in Jacobian H
Matrix H = zeros(m,n); Matrix H = zeros(m, N);
double factor = 1.0 / (2.0 * delta); double factor = 1.0 / (2.0 * delta);
for (int j = 0; j < n; j++) { for (int j = 0; j < N; j++) {
dx(j) = delta; dx(j) = delta;
TangentY dy1 = chartY.apply(h(chartX.retract(dx))); TangentY dy1 = chartY.apply(h(chartX.retract(dx)));
dx(j) = -delta; dx(j) = -delta;