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

New numericalDerivatives with traits an Charts - still some segfaults, *and* there should be no need for (a) multiple prototypes to match against c++ pointers, (b) the use of explicit template arguments. A task for someone...

release/4.3a0
dellaert 2014-10-21 18:50:52 +02:00
parent f46aa7cd8c
commit 1eb5e185e5
24 changed files with 667 additions and 790 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -2329,6 +2329,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testNumericalDerivative.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testNumericalDerivative.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>

831
gtsam/base/numericalDerivative.h
View File

@ -16,7 +16,6 @@
*/ */
// \callgraph // \callgraph
#pragma once #pragma once
#include <boost/function.hpp> #include <boost/function.hpp>
@ -31,10 +30,11 @@
#include <gtsam/base/LieVector.h> #include <gtsam/base/LieVector.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/Manifold.h>
namespace gtsam { namespace gtsam {
/* /*
* Note that all of these functions have two versions, a boost.function version and a * Note that all of these functions have two versions, a boost.function version and a
* standard C++ function pointer version. This allows reformulating the arguments of * standard C++ function pointer version. This allows reformulating the arguments of
* a function to fit the correct structure, which is useful for situations like * a function to fit the correct structure, which is useful for situations like
@ -56,216 +56,163 @@ 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));
}
/** 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
* The class X needs to have dim, expmap, logmap * The class X needs to have dim, expmap, logmap
*/ */
template<class X> template<class X>
Vector numericalGradient(boost::function<double(const X&)> h, const X& x, double delta=1e-5) { Vector numericalGradient(boost::function<double(const X&)> h, const X& x,
double factor = 1.0/(2.0*delta); double delta = 1e-5) {
const size_t n = x.dim(); double factor = 1.0 / (2.0 * delta);
Vector d = zero(n), g = zero(n);
for (size_t j=0;j<n;j++) { BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X>::value,
d(j) += delta; double hxplus = h(x.retract(d)); "Template argument X must be a manifold type.");
d(j) -= 2*delta; double hxmin = h(x.retract(d)); typedef DefaultChart<X> ChartX;
d(j) += delta; g(j) = (hxplus-hxmin)*factor; typedef typename ChartX::vector TangentX;
// get chart at 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
TangentX d;
d.setZero();
Vector g = zero(n);
for (int j = 0; j < n; j++) {
d(j) = delta;
double hxplus = h(chartX.retract(d));
d(j) = -delta;
double hxmin = h(chartX.retract(d));
d(j) = 0;
g(j) = (hxplus - hxmin) * factor;
} }
return g; return g;
} }
template<class X> /**
Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) { * @brief New-style numerical derivatives using manifold_traits
return numericalGradient<X>(boost::bind(h, _1), x, delta); * @brief Computes numerical derivative in argument 1 of unary function
}
/**
* Compute numerical derivative in argument 1 of unary function
* @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 Y is the output argument
* Class X is the input 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
*/ */
template<class Y, class X> template<class Y, class X>
Matrix numericalDerivative11(boost::function<Y(const X&)> h, const X& x, double delta=1e-5) { // TODO Should compute fixed-size matrix
Matrix numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
double delta = 1e-5) {
using namespace traits;
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<Y>::value,
"Template argument Y must be a manifold type.");
typedef DefaultChart<Y> ChartY;
typedef typename ChartY::vector TangentY;
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X>::value,
"Template argument X must be a manifold type.");
typedef DefaultChart<X> ChartX;
typedef typename ChartX::vector TangentX;
// get value at x, and corresponding chart
Y hx = h(x); Y hx = h(x);
double factor = 1.0/(2.0*delta); ChartY chartY(hx);
const size_t m = hx.dim(), n = x.dim(); TangentY zeroY = chartY.apply(hx);
Vector d = zero(n); size_t m = zeroY.size();
// get chart at 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
TangentX dx;
dx.setZero();
// Fill in Jacobian H
Matrix H = zeros(m,n); Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) { double factor = 1.0 / (2.0 * delta);
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x.retract(d))); for (int j = 0; j < n; j++) {
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x.retract(d))); dx(j) = delta;
d(j) += delta; TangentY dy1 = chartY.apply(h(chartX.retract(dx)));
H.col(j) << (hxplus-hxmin)*factor; dx(j) = -delta;
TangentY dy2 = chartY.apply(h(chartX.retract(dx)));
dx(j) = 0;
H.col(j) << (dy1 - dy2) * factor;
} }
return H; return H;
} }
/** use a raw C++ function pointer */ /** use a raw C++ function pointer */
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,
return numericalDerivative11<Y,X>(boost::bind(h, _1), x, delta); double delta = 1e-5) {
} return numericalDerivative11<Y, X>(boost::bind(h, _1), x, delta);
}
// /** remapping for double valued functions */ /**
// template<class X>
// Matrix numericalDerivative11(boost::function<double(const X&)> h, const X& x, double delta=1e-5) {
// return numericalDerivative11<LieVector, X>(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta);
// }
template<class X>
Matrix numericalDerivative11(double (*h)(const X&), const X& x, double delta=1e-5) {
return numericalDerivative11<LieVector, X>(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta);
}
/** remapping for vector valued functions */
template<class X>
Matrix numericalDerivative11(boost::function<Vector(const X&)> h, const X& x, double delta=1e-5) {
return numericalDerivative11<LieVector, X>(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta);
}
template<class X>
Matrix numericalDerivative11(Vector (*h)(const X&), const X& x, double delta=1e-5) {
return numericalDerivative11<LieVector, X>(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta);
}
/**
* Compute numerical derivative in argument 1 of binary function * Compute numerical derivative in argument 1 of binary function
* @param h binary function yielding m-vector * @param h binary function yielding m-vector
* @param x1 n-dimensional first argument value * @param x1 n-dimensional first argument value
* @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
*/ */
template<class Y, class X1, class X2> template<class Y, class X1, class X2>
Matrix numericalDerivative21(boost::function<Y(const X1&, const X2&)> h, Matrix numericalDerivative21(const boost::function<Y(const X1&, const X2&)>& h,
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); BOOST_STATIC_ASSERT_MSG(traits::is_manifold<Y>::value,
double factor = 1.0/(2.0*delta); "Template argument Y must be a manifold type.");
const size_t m = hx.dim(), n = x1.dim(); BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X1>::value,
Vector d = zero(n); "Template argument X1 must be a manifold type.");
Matrix H = zeros(m,n); return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2), x1, delta);
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;
H.col(j) << (hxplus-hxmin)*factor;
}
return H;
}
/** use a raw C++ function pointer */ /** use a raw C++ function pointer */
template<class Y, class X1, class X2> template<class Y, class X1, class X2>
inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
const X1& x1, const X2& x2, double delta=1e-5) { const X2& x2, double delta = 1e-5) {
return numericalDerivative21<Y,X1,X2>(boost::bind(h, _1, _2), x1, x2, delta); return numericalDerivative21<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
} }
/** pseudo-partial template specialization for double return values */ /**
template<class X1, class X2>
Matrix numericalDerivative21(boost::function<double(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta=1e-5) {
return numericalDerivative21<LieVector,X1,X2>(
boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
}
template<class X1, class X2>
Matrix numericalDerivative21(double (*h)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta=1e-5) {
return numericalDerivative21<LieVector,X1,X2>(
boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
}
/** pseudo-partial template specialization for vector return values */
template<class X1, class X2>
Matrix numericalDerivative21(boost::function<Vector(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta=1e-5) {
return numericalDerivative21<LieVector,X1,X2>(
boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
}
template<class X1, class X2>
inline Matrix numericalDerivative21(Vector (*h)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta=1e-5) {
return numericalDerivative21<LieVector,X1,X2>(
boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
}
/**
* Compute numerical derivative in argument 2 of binary function * Compute numerical derivative in argument 2 of binary function
* @param h binary function yielding m-vector * @param h binary function yielding m-vector
* @param x1 first argument value * @param x1 first argument value
* @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
*/ */
template<class Y, class X1, class X2> template<class Y, class X1, class X2>
Matrix numericalDerivative22 Matrix numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
(boost::function<Y(const X1&, const X2&)> h, const X1& x1, const X2& x2, double delta = 1e-5) {
const X1& x1, const X2& x2, double delta=1e-5) { BOOST_STATIC_ASSERT_MSG(traits::is_manifold<Y>::value,
Y hx = h(x1,x2); "Template argument Y must be a manifold type.");
double factor = 1.0/(2.0*delta); BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X2>::value,
const size_t m = hx.dim(), n = x2.dim(); "Template argument X2 must be a manifold type.");
Vector d = zero(n); return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1), x2, delta);
Matrix H = zeros(m,n); }
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;
H.col(j) << (hxplus-hxmin)*factor;
}
return H;
}
/** use a raw C++ function pointer */ /** use a raw C++ function pointer */
template<class Y, class X1, class X2> template<class Y, class X1, class X2>
inline Matrix numericalDerivative22 inline Matrix numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { const X2& x2, double delta = 1e-5) {
return numericalDerivative22<Y,X1,X2>(boost::bind(h, _1, _2), x1, x2, delta); return numericalDerivative22<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
} }
/** pseudo-partial template specialization for double return values */ /**
template<class X1, class X2>
Matrix numericalDerivative22(boost::function<double(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta=1e-5) {
return numericalDerivative22<LieVector,X1,X2>(
boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
}
template<class X1, class X2>
inline Matrix numericalDerivative22(double (*h)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta=1e-5) {
return numericalDerivative22<LieVector,X1,X2>(
boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
}
/** pseudo-partial template specialization for vector return values */
template<class X1, class X2>
Matrix numericalDerivative22(boost::function<Vector(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta=1e-5) {
return numericalDerivative22<LieVector,X1,X2>(
boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
}
template<class X1, class X2>
inline Matrix numericalDerivative22(Vector (*h)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta=1e-5) {
return numericalDerivative22<LieVector,X1,X2>(
boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta);
}
/**
* Compute numerical derivative in argument 1 of ternary function * Compute numerical derivative in argument 1 of ternary function
* @param h ternary function yielding m-vector * @param h ternary function yielding m-vector
* @param x1 n-dimensional first argument value * @param x1 n-dimensional first argument value
@ -275,62 +222,25 @@ namespace gtsam {
* @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 * All classes Y,X1,X2,X3 need dim, expmap, logmap
*/ */
template<class Y, class X1, class X2, class X3> template<class Y, class X1, class X2, class X3>
Matrix numericalDerivative31 Matrix numericalDerivative31(
(boost::function<Y(const X1&, const X2&, const X3&)> h, boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) const X2& x2, const X3& x3, double delta = 1e-5) {
{ BOOST_STATIC_ASSERT_MSG(traits::is_manifold<Y>::value,
Y hx = h(x1,x2,x3); "Template argument Y must be a manifold type.");
double factor = 1.0/(2.0*delta); BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X1>::value,
const size_t m = hx.dim(), n = x1.dim(); "Template argument X1 must be a manifold type.");
Vector d = zero(n); return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3), x1, delta);
Matrix H = zeros(m,n); }
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;
H.col(j) << (hxplus-hxmin)*factor;
}
return H;
}
template<class Y, class X1, class X2, class X3>
inline Matrix 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);
}
/** pseudo-partial template specialization for double return values */ template<class Y, class X1, class X2, class X3>
template<class X1, class X2, class X3> inline Matrix numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&),
Matrix numericalDerivative31(boost::function<double(const X1&, const X2&, const X3&)> h, 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) { return numericalDerivative31<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
return numericalDerivative31<LieVector,X1,X2,X3>( x2, x3, delta);
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); }
}
template<class X1, class X2, class X3> /**
inline Matrix numericalDerivative31(double (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
return numericalDerivative31<LieVector,X1,X2,X3>(
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
}
/** pseudo-partial template specialization for vector return values */
template<class X1, class X2, class X3>
Matrix numericalDerivative31(boost::function<Vector(const X1&, const X2&, const X3&)> h,
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
return numericalDerivative31<LieVector,X1,X2,X3>(
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
}
template<class X1, class X2, class X3>
inline Matrix numericalDerivative31(Vector (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
return numericalDerivative31<LieVector,X1,X2,X3>(
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
}
/**
* Compute numerical derivative in argument 2 of ternary function * Compute numerical derivative in argument 2 of ternary function
* @param h ternary function yielding m-vector * @param h ternary function yielding m-vector
* @param x1 n-dimensional first argument value * @param x1 n-dimensional first argument value
@ -340,62 +250,25 @@ namespace gtsam {
* @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 * All classes Y,X1,X2,X3 need dim, expmap, logmap
*/ */
template<class Y, class X1, class X2, class X3> template<class Y, class X1, class X2, class X3>
Matrix numericalDerivative32 Matrix numericalDerivative32(
(boost::function<Y(const X1&, const X2&, const X3&)> h, boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) const X2& x2, const X3& x3, double delta = 1e-5) {
{ BOOST_STATIC_ASSERT_MSG(traits::is_manifold<Y>::value,
Y hx = h(x1,x2,x3); "Template argument Y must be a manifold type.");
double factor = 1.0/(2.0*delta); BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X2>::value,
const size_t m = hx.dim(), n = x2.dim(); "Template argument X2 must be a manifold type.");
Vector d = zero(n); return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3), x2, delta);
Matrix H = zeros(m,n); }
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;
H.col(j) << (hxplus-hxmin)*factor;
}
return H;
}
template<class Y, class X1, class X2, class X3>
inline Matrix 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);
}
/** pseudo-partial template specialization for double return values */ template<class Y, class X1, class X2, class X3>
template<class X1, class X2, class X3> inline Matrix numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&),
Matrix numericalDerivative32(boost::function<double(const X1&, const X2&, const X3&)> h, 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) { return numericalDerivative32<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
return numericalDerivative32<LieVector,X1,X2,X3>( x2, x3, delta);
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); }
}
template<class X1, class X2, class X3> /**
inline Matrix numericalDerivative32(double (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
return numericalDerivative32<LieVector,X1,X2,X3>(
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
}
/** pseudo-partial template specialization for vector return values */
template<class X1, class X2, class X3>
Matrix numericalDerivative32(boost::function<Vector(const X1&, const X2&, const X3&)> h,
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
return numericalDerivative32<LieVector,X1,X2,X3>(
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
}
template<class X1, class X2, class X3>
inline Matrix numericalDerivative32(Vector (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
return numericalDerivative32<LieVector,X1,X2,X3>(
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
}
/**
* Compute numerical derivative in argument 3 of ternary function * Compute numerical derivative in argument 3 of ternary function
* @param h ternary function yielding m-vector * @param h ternary function yielding m-vector
* @param x1 n-dimensional first argument value * @param x1 n-dimensional first argument value
@ -405,62 +278,25 @@ namespace gtsam {
* @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 * All classes Y,X1,X2,X3 need dim, expmap, logmap
*/ */
template<class Y, class X1, class X2, class X3> template<class Y, class X1, class X2, class X3>
Matrix numericalDerivative33 Matrix numericalDerivative33(
(boost::function<Y(const X1&, const X2&, const X3&)> h, boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) const X2& x2, const X3& x3, double delta = 1e-5) {
{ BOOST_STATIC_ASSERT_MSG(traits::is_manifold<Y>::value,
Y hx = h(x1,x2,x3); "Template argument Y must be a manifold type.");
double factor = 1.0/(2.0*delta); BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X3>::value,
const size_t m = hx.dim(), n = x3.dim(); "Template argument X3 must be a manifold type.");
Vector d = zero(n); return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1), x3, delta);
Matrix H = zeros(m,n); }
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;
H.col(j) << (hxplus-hxmin)*factor;
}
return H;
}
template<class Y, class X1, class X2, class X3>
inline Matrix 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);
}
/** pseudo-partial template specialization for double return values */ template<class Y, class X1, class X2, class X3>
template<class X1, class X2, class X3> inline Matrix numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&),
Matrix numericalDerivative33(boost::function<double(const X1&, const X2&, const X3&)> h, 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) { return numericalDerivative33<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
return numericalDerivative33<LieVector,X1,X2,X3>( x2, x3, delta);
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); }
}
template<class X1, class X2, class X3> /**
inline Matrix numericalDerivative33(double (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
return numericalDerivative33<LieVector,X1,X2,X3>(
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
}
/** pseudo-partial template specialization for vector return values */
template<class X1, class X2, class X3>
Matrix numericalDerivative33(boost::function<Vector(const X1&, const X2&, const X3&)> h,
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
return numericalDerivative33<LieVector,X1,X2,X3>(
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
}
template<class X1, class X2, class X3>
inline Matrix numericalDerivative33(Vector (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
return numericalDerivative33<LieVector,X1,X2,X3>(
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
}
/**
* Compute numerical Hessian matrix. Requires a single-argument Lie->scalar * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar
* function. This is implemented simply as the derivative of the gradient. * function. This is implemented simply as the derivative of the gradient.
* @param f A function taking a Lie object as input and returning a scalar * @param f A function taking a Lie object as input and returning a scalar
@ -468,158 +304,223 @@ namespace gtsam {
* @param delta The numerical derivative step size * @param delta The numerical derivative step size
* @return n*n Hessian matrix computed via central differencing * @return n*n Hessian matrix computed via central differencing
*/ */
template<class X> template<class X>
inline Matrix numericalHessian(boost::function<double(const X&)> f, const X& x, double delta=1e-5) { inline Matrix numericalHessian(boost::function<double(const X&)> f, const X& x,
return numericalDerivative11<X>(boost::function<Vector(const X&)>(boost::bind( double delta = 1e-5) {
static_cast<Vector (*)(boost::function<double(const X&)>,const X&, double)>(&numericalGradient<X>), BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X>::value,
f, _1, delta)), x, delta); "Template argument X must be a manifold type.");
} typedef boost::function<double(const X&)> F;
typedef boost::function<Vector(F, const X&, double)> G;
G ng = static_cast<G>(numericalGradient<X> );
return numericalDerivative11<Vector, X>(boost::bind(ng, f, _1, delta), x,
delta);
}
template<class X> template<class X>
inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta=1e-5) { inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta =
1e-5) {
return numericalHessian(boost::function<double(const X&)>(f), x, delta); return numericalHessian(boost::function<double(const X&)>(f), x, delta);
} }
/** Helper class that computes the derivative of f w.r.t. x1, centered about
/** Helper class that computes the derivative of f w.r.t. x1, centered about
* x1_, as a function of x2 * x1_, as a function of x2
*/ */
template<class X1, class X2> template<class X1, class X2>
class G_x1 { class G_x1 {
const boost::function<double(const X1&, const X2&)>& f_; const boost::function<double(const X1&, const X2&)>& f_;
const X1& x1_; X1 x1_;
double delta_; double delta_;
public: public:
G_x1(const boost::function<double(const X1&, const X2&)>& f, const X1& x1, double delta) : f_(f), x1_(x1), delta_(delta) {} G_x1(const boost::function<double(const X1&, const X2&)>& f, const X1& x1,
double delta) :
f_(f), x1_(x1), delta_(delta) {
}
Vector operator()(const X2& x2) { Vector operator()(const X2& x2) {
return numericalGradient<X1>(boost::function<double (const X1&)>(boost::bind(f_, _1, x2)), x1_, delta_); return numericalGradient<X1>(boost::bind(f_, _1, x2), x1_, delta_);
} }
}; };
template<class X1, class X2> template<class X1, class X2>
inline Matrix numericalHessian212(boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2, double delta=1e-5) { inline Matrix numericalHessian212(
G_x1<X1,X2> g_x1(f, x1, delta); boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
return numericalDerivative11<X2>(boost::function<Vector (const X2&)>(boost::bind<Vector>(boost::ref(g_x1), _1)), x2, delta); double delta = 1e-5) {
} G_x1<X1, X2> g_x1(f, x1, delta);
return numericalDerivative11<Vector,X2>(
boost::function<Vector(const X2&)>(
boost::bind<Vector>(boost::ref(g_x1), _1)), x2, delta);
}
template<class X1, class X2>
inline Matrix 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> template<class X1, class X2>
inline Matrix numericalHessian212(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { inline Matrix numericalHessian211(
return numericalHessian212(boost::function<double (const X1&, const X2&)>(f), x1, x2, delta); boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
} double delta = 1e-5) {
Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
double) = &numericalGradient<X1>;
boost::function<double(const X1&)> f2(boost::bind(f, _1, x2));
template<class X1, class X2> return numericalDerivative11<Vector,X1>(
inline Matrix numericalHessian211(boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2, double delta=1e-5) { boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
x1, delta);
}
Vector (*numGrad)(boost::function<double(const X1&)>, const X1&, double) = &numericalGradient<X1>; template<class X1, class X2>
boost::function<double (const X1&)> f2(boost::bind(f, _1, x2)); inline Matrix 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);
}
return numericalDerivative11<X1>(boost::function<Vector (const X1&)>(boost::bind(numGrad, f2, _1, delta)), x1, delta); template<class X1, class X2>
} inline Matrix numericalHessian222(
boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
double delta = 1e-5) {
Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
double) = &numericalGradient<X2>;
boost::function<double(const X2&)> f2(boost::bind(f, x1, _1));
template<class X1, class X2> return numericalDerivative11<Vector,X2>(
inline Matrix numericalHessian211(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
return numericalHessian211(boost::function<double (const X1&, const X2&)>(f), x1, x2, delta); x2, delta);
} }
template<class X1, class X2>
inline Matrix 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);
}
template<class X1, class X2> /**
inline Matrix numericalHessian222(boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2, double delta=1e-5) {
Vector (*numGrad)(boost::function<double(const X2&)>, const X2&, double) = &numericalGradient<X2>;
boost::function<double (const X2&)> f2(boost::bind(f, x1, _1));
return numericalDerivative11<X2>(boost::function<Vector (const X2&)>(boost::bind(numGrad, f2, _1, delta)), x2, delta);
}
template<class X1, class X2>
inline Matrix 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);
}
/**
* Numerical Hessian for tenary functions * Numerical Hessian for tenary functions
*/ */
/* **************************************************************** */ /* **************************************************************** */
template<class X1, class X2, class X3> template<class X1, class X2, class X3>
inline Matrix numericalHessian311(boost::function<double(const X1&, const X2&, const X3&)> f, inline Matrix numericalHessian311(
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
Vector (*numGrad)(boost::function<double(const X1&)>, const X1&, double) = &numericalGradient<X1>; Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
boost::function<double (const X1&)> f2(boost::bind(f, _1, x2, x3)); double) = &numericalGradient<X1>;
boost::function<double(const X1&)> f2(boost::bind(f, _1, x2, x3));
return numericalDerivative11<X1>(boost::function<Vector (const X1&)>(boost::bind(numGrad, f2, _1, delta)), x1, delta); return numericalDerivative11<Vector,X1>(
} boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
x1, delta);
template<class X1, class X2, class X3> }
inline Matrix 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, delta); template<class X1, class X2, class X3>
} inline Matrix numericalHessian311(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
/* **************************************************************** */ return numericalHessian311(
template<class X1, class X2, class X3> boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
inline Matrix numericalHessian322(boost::function<double(const X1&, const X2&, const X3&)> f, delta);
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { }
Vector (*numGrad)(boost::function<double(const X2&)>, const X2&, double) = &numericalGradient<X2>; /* **************************************************************** */
boost::function<double (const X2&)> f2(boost::bind(f, x1, _1, x3)); template<class X1, class X2, class X3>
inline Matrix numericalHessian322(
return numericalDerivative11<X2>(boost::function<Vector (const X2&)>(boost::bind(numGrad, f2, _1, delta)), x2, delta); boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
} const X2& x2, const X3& x3, double delta = 1e-5) {
template<class X1, class X2, class X3> Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { double) = &numericalGradient<X2>;
return numericalHessian322(boost::function<double (const X1&, const X2&, const X3&)>(f), x1, x2, x3, delta); boost::function<double(const X2&)> f2(boost::bind(f, x1, _1, x3));
}
return numericalDerivative11<Vector,X2>(
/* **************************************************************** */ boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
template<class X1, class X2, class X3> x2, delta);
inline Matrix numericalHessian333(boost::function<double(const X1&, const X2&, const X3&)> f, }
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
template<class X1, class X2, class X3>
Vector (*numGrad)(boost::function<double(const X3&)>, const X3&, double) = &numericalGradient<X3>; inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&),
boost::function<double (const X3&)> f2(boost::bind(f, x1, x2, _1)); const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian322(
return numericalDerivative11<X3>(boost::function<Vector (const X3&)>(boost::bind(numGrad, f2, _1, delta)), x3, delta); boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
} delta);
}
template<class X1, class X2, class X3>
inline Matrix 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, delta); template<class X1, class X2, class X3>
} inline Matrix numericalHessian333(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
/* **************************************************************** */ const X2& x2, const X3& x3, double delta = 1e-5) {
template<class X1, class X2, class X3>
inline Matrix numericalHessian312(boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { Vector (*numGrad)(boost::function<double(const X3&)>, const X3&,
return numericalHessian212<X1,X2>(boost::function<double (const X1&, const X2&)>(boost::bind(f, _1, _2, x3)), x1, x2, delta ); double) = &numericalGradient<X3>;
} boost::function<double(const X3&)> f2(boost::bind(f, x1, x2, _1));
template<class X1, class X2, class X3> return numericalDerivative11<Vector,X3>(
inline Matrix numericalHessian313(boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { boost::function<Vector(const X3&)>(boost::bind(numGrad, f2, _1, delta)),
return numericalHessian212<X1,X3>(boost::function<double (const X1&, const X3&)>(boost::bind(f, _1, x2, _2)), x1, x3, delta ); x3, delta);
} }
template<class X1, class X2, class X3> template<class X1, class X2, class X3>
inline Matrix numericalHessian323(boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { inline Matrix numericalHessian333(double (*f)(const X1&, const X2&, const X3&),
return numericalHessian212<X2,X3>(boost::function<double (const X2&, const X3&)>(boost::bind(f, x1, _1, _2)), x2, x3, delta ); 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,
/* **************************************************************** */ delta);
template<class X1, class X2, class X3> }
inline Matrix 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, delta); /* **************************************************************** */
} template<class X1, class X2, class X3>
inline Matrix numericalHessian312(
template<class X1, class X2, class X3> boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
inline Matrix numericalHessian313(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { 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, delta); return numericalHessian212<X1, X2>(
} boost::function<double(const X1&, const X2&)>(boost::bind(f, _1, _2, x3)),
x1, x2, delta);
template<class X1, class X2, class X3> }
inline Matrix 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, delta); template<class X1, class X2, class X3>
} inline Matrix 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>(
boost::function<double(const X1&, const X3&)>(boost::bind(f, _1, x2, _2)),
x1, x3, delta);
}
template<class X1, class X2, class X3>
inline Matrix 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>(
boost::function<double(const X2&, const X3&)>(boost::bind(f, x1, _1, _2)),
x2, x3, delta);
}
/* **************************************************************** */
template<class X1, class X2, class X3>
inline Matrix 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,
delta);
}
template<class X1, class X2, class X3>
inline Matrix 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,
delta);
}
template<class X1, class X2, class X3>
inline Matrix 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,
delta);
}
} }

108
gtsam/base/tests/testNumericalDerivative.cpp
View File

@ -15,115 +15,123 @@
* @date Apr 8, 2011 * @date Apr 8, 2011
*/ */
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h> using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
double f(const LieVector& x) { double f(const Vector2& x) {
assert(x.size() == 2); assert(x.size() == 2);
return sin(x(0)) + cos(x(1)); return sin(x(0)) + cos(x(1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(testNumericalDerivative, numericalHessian) { //
LieVector center = ones(2); TEST(testNumericalDerivative, numericalGradient) {
Vector2 x(1, 1);
Matrix expected = (Matrix(2,2) << Vector expected(2);
-sin(center(0)), 0.0, expected << cos(x(1)), -sin(x(0));
0.0, -cos(center(1)));
Matrix actual = numericalHessian(f, center); Vector actual = numericalGradient<Vector2>(f, x);
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
double f2(const LieVector& x) { TEST(testNumericalDerivative, numericalHessian) {
Vector2 x(1, 1);
Matrix expected(2, 2);
expected << -sin(x(0)), 0.0, 0.0, -cos(x(1));
Matrix actual = numericalHessian<Vector2>(f, x);
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
double f2(const Vector2& x) {
assert(x.size() == 2); assert(x.size() == 2);
return sin(x(0)) * cos(x(1)); return sin(x(0)) * cos(x(1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
//
TEST(testNumericalDerivative, numericalHessian2) { TEST(testNumericalDerivative, numericalHessian2) {
Vector v_center = (Vector(2) << 0.5, 1.0); Vector2 v(0.5, 1.0);
LieVector center(v_center); Vector2 x(v);
Matrix expected = (Matrix(2,2) << Matrix expected = (Matrix(2, 2) << -cos(x(1)) * sin(x(0)), -sin(x(1))
-cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)), * cos(x(0)), -cos(x(0)) * sin(x(1)), -sin(x(0)) * cos(x(1)));
-cos(center(0))*sin(center(1)), -sin(center(0))*cos(center(1)));
Matrix actual = numericalHessian(f2, center); Matrix actual = numericalHessian(f2, x);
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
double f3(const LieVector& x1, const LieVector& x2) { double f3(double x1, double x2) {
assert(x1.size() == 1 && x2.size() == 1); return sin(x1) * cos(x2);
return sin(x1(0)) * cos(x2(0));
} }
/* ************************************************************************* */ /* ************************************************************************* */
//
TEST(testNumericalDerivative, numericalHessian211) { TEST(testNumericalDerivative, numericalHessian211) {
Vector v_center1 = (Vector(1) << 1.0); double x1 = 1, x2 = 5;
Vector v_center2 = (Vector(1) << 5.0);
LieVector center1(v_center1), center2(v_center2);
Matrix expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0))); Matrix expected11 = (Matrix(1, 1) << -sin(x1) * cos(x2));
Matrix actual11 = numericalHessian211(f3, center1, center2); Matrix actual11 = numericalHessian211<double, double>(f3, x1, x2);
EXPECT(assert_equal(expected11, actual11, 1e-5)); EXPECT(assert_equal(expected11, actual11, 1e-5));
Matrix expected12 = (Matrix(1, 1) <<-cos(center1(0))*sin(center2(0))); Matrix expected12 = (Matrix(1, 1) << -cos(x1) * sin(x2));
Matrix actual12 = numericalHessian212(f3, center1, center2); Matrix actual12 = numericalHessian212<double, double>(f3, x1, x2);
EXPECT(assert_equal(expected12, actual12, 1e-5)); EXPECT(assert_equal(expected12, actual12, 1e-5));
Matrix expected22 = (Matrix(1, 1) <<-sin(center1(0))*cos(center2(0))); Matrix expected22 = (Matrix(1, 1) << -sin(x1) * cos(x2));
Matrix actual22 = numericalHessian222(f3, center1, center2); Matrix actual22 = numericalHessian222<double, double>(f3, x1, x2);
EXPECT(assert_equal(expected22, actual22, 1e-5)); EXPECT(assert_equal(expected22, actual22, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
double f4(const LieVector& x, const LieVector& y, const LieVector& z) { double f4(double x, double y, double z) {
assert(x.size() == 1 && y.size() == 1 && z.size() == 1); return sin(x) * cos(y) * z * z;
return sin(x(0)) * cos(y(0)) * z(0)*z(0);
} }
/* ************************************************************************* */ /* ************************************************************************* */
//
TEST(testNumericalDerivative, numericalHessian311) { TEST(testNumericalDerivative, numericalHessian311) {
Vector v_center1 = (Vector(1) << 1.0); double x = 1, y = 2, z = 3;
Vector v_center2 = (Vector(1) << 2.0); Matrix expected11 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z);
Vector v_center3 = (Vector(1) << 3.0); Matrix actual11 = numericalHessian311<double, double, double>(f4, x, y, z);
LieVector center1(v_center1), center2(v_center2), center3(v_center3);
double x = center1(0), y = center2(0), z = center3(0);
Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z);
Matrix actual11 = numericalHessian311(f4, center1, center2, center3);
EXPECT(assert_equal(expected11, actual11, 1e-5)); EXPECT(assert_equal(expected11, actual11, 1e-5));
Matrix expected12 = (Matrix(1, 1) << -cos(x)*sin(y)*z*z); Matrix expected12 = (Matrix(1, 1) << -cos(x) * sin(y) * z * z);
Matrix actual12 = numericalHessian312(f4, center1, center2, center3); Matrix actual12 = numericalHessian312<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected12, actual12, 1e-5)); EXPECT(assert_equal(expected12, actual12, 1e-5));
Matrix expected13 = (Matrix(1, 1) << cos(x)*cos(y)*2*z); Matrix expected13 = (Matrix(1, 1) << cos(x) * cos(y) * 2 * z);
Matrix actual13 = numericalHessian313(f4, center1, center2, center3); Matrix actual13 = numericalHessian313<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected13, actual13, 1e-5)); EXPECT(assert_equal(expected13, actual13, 1e-5));
Matrix expected22 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z); Matrix expected22 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z);
Matrix actual22 = numericalHessian322(f4, center1, center2, center3); Matrix actual22 = numericalHessian322<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected22, actual22, 1e-5)); EXPECT(assert_equal(expected22, actual22, 1e-5));
Matrix expected23 = (Matrix(1, 1) << -sin(x)*sin(y)*2*z); Matrix expected23 = (Matrix(1, 1) << -sin(x) * sin(y) * 2 * z);
Matrix actual23 = numericalHessian323(f4, center1, center2, center3); Matrix actual23 = numericalHessian323<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected23, actual23, 1e-5)); EXPECT(assert_equal(expected23, actual23, 1e-5));
Matrix expected33 = (Matrix(1, 1) << sin(x)*cos(y)*2); Matrix expected33 = (Matrix(1, 1) << sin(x) * cos(y) * 2);
Matrix actual33 = numericalHessian333(f4, center1, center2, center3); Matrix actual33 = numericalHessian333<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected33, actual33, 1e-5)); EXPECT(assert_equal(expected33, actual33, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */

18
gtsam/geometry/tests/testRot3M.cpp
View File

@ -229,7 +229,8 @@ TEST( Rot3, rightJacobianExpMapSO3inverse )
Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias
Vector3 deltatheta; deltatheta << 0, 0, 0; Vector3 deltatheta; deltatheta << 0, 0, 0;
Matrix expectedJacobian = numericalDerivative11<LieVector>(boost::bind(&evaluateLogRotation, thetahat, _1), LieVector(deltatheta)); Matrix expectedJacobian = numericalDerivative11<Vector3,Vector3>(
boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta);
Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat); Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat);
EXPECT(assert_equal(expectedJacobian, actualJacobian)); EXPECT(assert_equal(expectedJacobian, actualJacobian));
} }
@ -439,19 +440,18 @@ TEST( Rot3, between )
/* ************************************************************************* */ /* ************************************************************************* */
Vector w = (Vector(3) << 0.1, 0.27, -0.2); Vector w = (Vector(3) << 0.1, 0.27, -0.2);
// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes? // Left trivialization Derivative of exp(w) wrpt w:
// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 // How does exp(w) change when w changes?
// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
// => y = log (exp(-w) * exp(w+dw)) // => y = log (exp(-w) * exp(w+dw))
Vector testDexpL(const LieVector& dw) { Vector3 testDexpL(const Vector3& dw) {
Vector y = Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw));
return y;
} }
TEST( Rot3, dexpL) { TEST( Rot3, dexpL) {
Matrix actualDexpL = Rot3::dexpL(w); Matrix actualDexpL = Rot3::dexpL(w);
Matrix expectedDexpL = numericalDerivative11( Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
boost::function<Vector(const LieVector&)>( Vector3::Zero(), 1e-2);
boost::bind(testDexpL, _1)), LieVector(zero(3)), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
Matrix actualDexpInvL = Rot3::dexpInvL(w); Matrix actualDexpInvL = Rot3::dexpInvL(w);

6
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -274,11 +274,7 @@ TEST( triangulation, TriangulationFactor ) {
Matrix HActual; Matrix HActual;
factor.evaluateError(landmark, HActual); factor.evaluateError(landmark, HActual);
// Matrix expectedH1 = numericalDerivative11<Pose3>( Matrix HExpected = numericalDerivative11<Vector,Point3>(
// boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
// boost::none, boost::none), pose1);
// The expected Jacobian
Matrix HExpected = numericalDerivative11<Point3>(
boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);
// Verify the Jacobians are correct // Verify the Jacobians are correct

4
gtsam/geometry/tests/testUnit3.cpp
View File

@ -115,13 +115,13 @@ TEST(Unit3, error) {
Matrix actual, expected; Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
{ {
expected = numericalDerivative11<Unit3>( expected = numericalDerivative11<Vector,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), q); boost::bind(&Unit3::error, &p, _1, boost::none), q);
p.error(q, actual); p.error(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
} }
{ {
expected = numericalDerivative11<Unit3>( expected = numericalDerivative11<Vector,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), r); boost::bind(&Unit3::error, &p, _1, boost::none), r);
p.error(r, actual); p.error(r, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); EXPECT(assert_equal(expected.transpose(), actual, 1e-9));

13
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <boost/assign/list_of.hpp> #include <boost/assign/list_of.hpp>
@ -150,7 +149,7 @@ TEST( GaussianBayesNet, DeterminantTest )
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
double computeError(const GaussianBayesNet& gbn, const LieVector& values) double computeError(const GaussianBayesNet& gbn, const Vector& values)
{ {
pair<Matrix,Vector> Rd = GaussianFactorGraph(gbn).jacobian(); pair<Matrix,Vector> Rd = GaussianFactorGraph(gbn).jacobian();
return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); return 0.5 * (Rd.first * values - Rd.second).squaredNorm();
@ -180,14 +179,12 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0))); 4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0)));
// Compute the Hessian numerically // Compute the Hessian numerically
Matrix hessian = numericalHessian( Matrix hessian = numericalHessian<Vector>(boost::bind(&computeError, gbn, _1),
boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)), Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols()));
LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())));
// Compute the gradient numerically // Compute the gradient numerically
Vector gradient = numericalGradient( Vector gradient = numericalGradient<Vector>(boost::bind(&computeError, gbn, _1),
boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)), Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols()));
LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())));
// Compute the gradient using dense matrices // Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();

4
gtsam/navigation/tests/testAttitudeFactor.cpp
View File

@ -45,7 +45,7 @@ TEST( Rot3AttitudeFactor, Constructor ) {
EXPECT(assert_equal(zero(2),factor.evaluateError(nRb),1e-5)); EXPECT(assert_equal(zero(2),factor.evaluateError(nRb),1e-5));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Rot3>( Matrix expectedH = numericalDerivative11<Vector,Rot3>(
boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none), boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none),
nRb); nRb);
@ -78,7 +78,7 @@ TEST( Pose3AttitudeFactor, Constructor ) {
EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5)); EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Pose3>( Matrix expectedH = numericalDerivative11<Vector,Pose3>(
boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1, boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1,
boost::none), T); boost::none), T);

4
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -239,12 +239,12 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelPdelBias = numericalDerivative11<imuBias::ConstantBias>( Matrix expectedDelPdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
Matrix expectedDelVdelBias = numericalDerivative11<imuBias::ConstantBias>( Matrix expectedDelVdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);

2
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -56,7 +56,7 @@ TEST( GPSFactor, Constructors ) {
EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5)); EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Pose3>( Matrix expectedH = numericalDerivative11<Vector,Pose3>(
boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative

36
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -192,15 +192,15 @@ TEST( ImuFactor, Error )
EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians // Expected Jacobians
Matrix H1e = numericalDerivative11<Pose3>( Matrix H1e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
Matrix H2e = numericalDerivative11<LieVector>( Matrix H2e = numericalDerivative11<Vector,LieVector>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
Matrix H3e = numericalDerivative11<Pose3>( Matrix H3e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
Matrix H4e = numericalDerivative11<LieVector>( Matrix H4e = numericalDerivative11<Vector,LieVector>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
Matrix H5e = numericalDerivative11<imuBias::ConstantBias>( Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
// Check rotation Jacobians // Check rotation Jacobians
@ -276,15 +276,15 @@ TEST( ImuFactor, ErrorWithBiases )
// EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians // Expected Jacobians
Matrix H1e = numericalDerivative11<Pose3>( Matrix H1e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
Matrix H2e = numericalDerivative11<LieVector>( Matrix H2e = numericalDerivative11<Vector,LieVector>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
Matrix H3e = numericalDerivative11<Pose3>( Matrix H3e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
Matrix H4e = numericalDerivative11<LieVector>( Matrix H4e = numericalDerivative11<Vector,LieVector>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
Matrix H5e = numericalDerivative11<imuBias::ConstantBias>( Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
// Check rotation Jacobians // Check rotation Jacobians
@ -341,7 +341,7 @@ TEST( ImuFactor, PartialDerivativeLogmap )
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelFdeltheta = numericalDerivative11<LieVector>(boost::bind( Matrix expectedDelFdeltheta = numericalDerivative11<Vector,LieVector>(boost::bind(
&evaluateLogRotation, thetahat, _1), LieVector(deltatheta)); &evaluateLogRotation, thetahat, _1), LieVector(deltatheta));
const Vector3 x = thetahat; // parametrization of so(3) const Vector3 x = thetahat; // parametrization of so(3)
@ -417,12 +417,12 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelPdelBias = numericalDerivative11<imuBias::ConstantBias>( Matrix expectedDelPdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
Matrix expectedDelVdelBias = numericalDerivative11<imuBias::ConstantBias>( Matrix expectedDelVdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
@ -531,15 +531,15 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
// Expected Jacobians // Expected Jacobians
Matrix H1e = numericalDerivative11<Pose3>( Matrix H1e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
Matrix H2e = numericalDerivative11<LieVector>( Matrix H2e = numericalDerivative11<Vector,LieVector>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
Matrix H3e = numericalDerivative11<Pose3>( Matrix H3e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
Matrix H4e = numericalDerivative11<LieVector>( Matrix H4e = numericalDerivative11<Vector,LieVector>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
Matrix H5e = numericalDerivative11<imuBias::ConstantBias>( Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
// Check rotation Jacobians // Check rotation Jacobians

14
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -72,35 +72,35 @@ TEST( MagFactor, Factors ) {
// MagFactor // MagFactor
MagFactor f(1, measured, s, dir, bias, model); MagFactor f(1, measured, s, dir, bias, model);
EXPECT( assert_equal(zero(3),f.evaluateError(theta,H1),1e-5)); EXPECT( assert_equal(zero(3),f.evaluateError(theta,H1),1e-5));
EXPECT( assert_equal(numericalDerivative11<Rot2> // EXPECT( assert_equal(numericalDerivative11<Vector,Rot2> //
(boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7));
// MagFactor1 // MagFactor1
MagFactor1 f1(1, measured, s, dir, bias, model); MagFactor1 f1(1, measured, s, dir, bias, model);
EXPECT( assert_equal(zero(3),f1.evaluateError(nRb,H1),1e-5)); EXPECT( assert_equal(zero(3),f1.evaluateError(nRb,H1),1e-5));
EXPECT( assert_equal(numericalDerivative11<Rot3> // EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
(boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7));
// MagFactor2 // MagFactor2
MagFactor2 f2(1, 2, measured, nRb, model); MagFactor2 f2(1, 2, measured, nRb, model);
EXPECT( assert_equal(zero(3),f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT( assert_equal(zero(3),f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT( assert_equal(numericalDerivative11<Point3> // EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
(boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),//
H1, 1e-7)); H1, 1e-7));
EXPECT( assert_equal(numericalDerivative11<Point3> // EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
(boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// (boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),//
H2, 1e-7)); H2, 1e-7));
// MagFactor2 // MagFactor2
MagFactor3 f3(1, 2, 3, measured, nRb, model); MagFactor3 f3(1, 2, 3, measured, nRb, model);
EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
EXPECT(assert_equal(numericalDerivative11<LieScalar> // EXPECT(assert_equal(numericalDerivative11<Vector,LieScalar> //
(boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
H1, 1e-7)); H1, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Unit3> // EXPECT(assert_equal(numericalDerivative11<Vector,Unit3> //
(boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// (boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),//
H2, 1e-7)); H2, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Point3> // EXPECT(assert_equal(numericalDerivative11<Vector,Point3> //
(boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// (boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),//
H3, 1e-7)); H3, 1e-7));
} }

4
gtsam/slam/tests/testEssentialMatrixConstraint.cpp
View File

@ -50,10 +50,10 @@ TEST( EssentialMatrixConstraint, test ) {
CHECK(assert_equal(expected, actual, 1e-8)); CHECK(assert_equal(expected, actual, 1e-8));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>( Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
boost::none, boost::none), pose1); boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Pose3>( Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1, boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1,
boost::none, boost::none), pose2); boost::none, boost::none), pose2);

18
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -96,7 +96,7 @@ TEST (EssentialMatrixFactor, factor) {
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected; Matrix Hexpected;
Hexpected = numericalDerivative11<EssentialMatrix>( Hexpected = numericalDerivative11<Vector,EssentialMatrix>(
boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1,
boost::none), trueE); boost::none), trueE);
@ -173,8 +173,8 @@ TEST (EssentialMatrixFactor2, factor) {
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f = boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2,
boost::none, boost::none); boost::none, boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, trueE, d); Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, trueE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, trueE, d); Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, trueE, d);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@ -247,8 +247,8 @@ TEST (EssentialMatrixFactor3, factor) {
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f = boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2,
boost::none, boost::none); boost::none, boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, bodyE, d); Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, bodyE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, bodyE, d); Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, bodyE, d);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@ -389,8 +389,8 @@ TEST (EssentialMatrixFactor2, extraTest) {
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f = boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2,
boost::none, boost::none); boost::none, boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, trueE, d); Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, trueE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, trueE, d); Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, trueE, d);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
@ -458,8 +458,8 @@ TEST (EssentialMatrixFactor3, extraTest) {
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f = boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2,
boost::none, boost::none); boost::none, boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, bodyE, d); Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, bodyE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, bodyE, d); Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, bodyE, d);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));

8
gtsam/slam/tests/testRotateFactor.cpp
View File

@ -69,13 +69,13 @@ TEST (RotateFactor, test) {
Matrix actual, expected; Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
{ {
expected = numericalDerivative11<Rot3>( expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc); boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc);
f.evaluateError(iRc, actual); f.evaluateError(iRc, actual);
EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(expected, actual, 1e-9));
} }
{ {
expected = numericalDerivative11<Rot3>( expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R); boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R);
f.evaluateError(R, actual); f.evaluateError(R, actual);
EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(expected, actual, 1e-9));
@ -141,14 +141,14 @@ TEST (RotateDirectionsFactor, test) {
Matrix actual, expected; Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
{ {
expected = numericalDerivative11<Rot3>( expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
boost::none), iRc); boost::none), iRc);
f.evaluateError(iRc, actual); f.evaluateError(iRc, actual);
EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(expected, actual, 1e-9));
} }
{ {
expected = numericalDerivative11<Rot3>( expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
boost::none), R); boost::none), R);
f.evaluateError(R, actual); f.evaluateError(R, actual);

20
gtsam_unstable/geometry/Pose3Upright.h
View File

@ -126,11 +126,9 @@ public:
/// Log map at identity - return the canonical coordinates of this rotation /// Log map at identity - return the canonical coordinates of this rotation
static Vector Logmap(const Pose3Upright& p); static Vector Logmap(const Pose3Upright& p);
private:
/// @} /// @}
/// @name Advanced Interface
/// @{ private:
// Serialization function // Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;
@ -142,4 +140,18 @@ private:
}; // \class Pose3Upright }; // \class Pose3Upright
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<Pose3Upright> : public std::true_type {
};
template<>
struct dimension<Pose3Upright> : public std::integral_constant<int, 4> {
};
}
} // \namespace gtsam } // \namespace gtsam

6
gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
View File

@ -90,7 +90,7 @@ TEST( InvDepthFactor, Dproject_pose)
{ {
LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4); LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative31<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual; Matrix actual;
Point2 uv = inv_camera.project(landmark, inv_depth, actual, boost::none, boost::none); Point2 uv = inv_camera.project(landmark, inv_depth, actual, boost::none, boost::none);
@ -102,7 +102,7 @@ TEST( InvDepthFactor, Dproject_landmark)
{ {
LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4); LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative32<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual; Matrix actual;
Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, actual, boost::none); Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, actual, boost::none);
@ -114,7 +114,7 @@ TEST( InvDepthFactor, Dproject_inv_depth)
{ {
LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4); LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative33<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual; Matrix actual;
Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, boost::none, actual); Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, boost::none, actual);

55
gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp
View File

@ -22,6 +22,7 @@
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam_unstable/nonlinear/Expression.h> #include <gtsam_unstable/nonlinear/Expression.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/LieScalar.h> #include <gtsam/base/LieScalar.h>
@ -135,60 +136,6 @@ struct SnavelyProjection {
}; };
/* ************************************************************************* */
// New-style numerical derivatives using manifold_traits
template<typename Y, typename X>
Matrix numericalDerivative(boost::function<Y(const X&)> h, const X& x,
double delta = 1e-5) {
using namespace traits;
BOOST_STATIC_ASSERT(is_manifold<Y>::value);
static const size_t M = dimension<Y>::value;
typedef DefaultChart<Y> ChartY;
typedef typename ChartY::vector TangentY;
BOOST_STATIC_ASSERT(is_manifold<X>::value);
static const size_t N = dimension<X>::value;
typedef DefaultChart<X> ChartX;
typedef typename ChartX::vector TangentX;
// get chart at x
ChartX chartX(x);
// get value at x, and corresponding chart
Y hx = h(x);
ChartY chartY(hx);
// Prepare a tangent vector to perturb x with
TangentX dx;
dx.setZero();
// Fill in Jacobian H
Matrix H = zeros(M, N);
double factor = 1.0 / (2.0 * delta);
for (size_t j = 0; j < N; j++) {
dx(j) = delta;
TangentY dy1 = chartY.apply(h(chartX.retract(dx)));
dx(j) = -delta;
TangentY dy2 = chartY.apply(h(chartX.retract(dx)));
H.block<M, 1>(0, j) << (dy1 - dy2) * factor;
dx(j) = 0;
}
return H;
}
template<typename Y, typename X1, typename X2>
Matrix numericalDerivative21(boost::function<Y(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta = 1e-5) {
return numericalDerivative<Y, X1>(boost::bind(h, _1, x2), x1, delta);
}
template<typename Y, typename X1, typename X2>
Matrix numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta = 1e-5) {
return numericalDerivative<Y, X2>(boost::bind(h, x1, _1), x2, delta);
}
/* ************************************************************************* */ /* ************************************************************************* */
// Test Ceres AutoDiff // Test Ceres AutoDiff
TEST(Expression, AutoDiff) { TEST(Expression, AutoDiff) {

12
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -104,11 +104,15 @@ public:
boost::optional<gtsam::Matrix&> H1=boost::none, boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const { boost::optional<gtsam::Matrix&> H2=boost::none) const {
if(H1) { if (H1) {
(*H1) = numericalDerivative11<Pose3>(boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1, landmark), pose); (*H1) = numericalDerivative11<Vector, Pose3>(
boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1,
landmark), pose);
} }
if(H2) { if (H2) {
(*H2) = numericalDerivative11<LieVector>(boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, _1), landmark); (*H2) = numericalDerivative11<Vector, LieVector>(
boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose,
_1), landmark);
} }
return inverseDepthError(pose, landmark); return inverseDepthError(pose, landmark);

12
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -107,11 +107,15 @@ public:
boost::optional<gtsam::Matrix&> H1=boost::none, boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const { boost::optional<gtsam::Matrix&> H2=boost::none) const {
if(H1) { if (H1) {
(*H1) = numericalDerivative11<Pose3>(boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1, landmark), pose); (*H1) = numericalDerivative11<Vector, Pose3>(
boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1,
landmark), pose);
} }
if(H2) { if (H2) {
(*H2) = numericalDerivative11<LieVector>(boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, _1), landmark); (*H2) = numericalDerivative11<Vector, LieVector>(
boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose,
_1), landmark);
} }
return inverseDepthError(pose, landmark); return inverseDepthError(pose, landmark);

10
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -108,10 +108,10 @@ public:
boost::optional<gtsam::Matrix&> H2=boost::none) const { boost::optional<gtsam::Matrix&> H2=boost::none) const {
if(H1) { if(H1) {
(*H1) = numericalDerivative11<Pose3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); (*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose);
} }
if(H2) { if(H2) {
(*H2) = numericalDerivative11<LieVector>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark); (*H2) = numericalDerivative11<Vector,LieVector>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark);
} }
return inverseDepthError(pose, landmark); return inverseDepthError(pose, landmark);
@ -228,13 +228,13 @@ public:
boost::optional<gtsam::Matrix&> H3=boost::none) const { boost::optional<gtsam::Matrix&> H3=boost::none) const {
if(H1) { if(H1) {
(*H1) = numericalDerivative11<Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); (*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1);
} }
if(H2) { if(H2) {
(*H2) = numericalDerivative11<Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2); (*H2) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2);
} }
if(H3) { if(H3) {
(*H3) = numericalDerivative11<LieVector>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); (*H3) = numericalDerivative11<Vector,LieVector>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark);
} }
return inverseDepthError(pose1, pose2, landmark); return inverseDepthError(pose1, pose2, landmark);

8
gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp
View File

@ -176,8 +176,8 @@ TEST( PoseBetweenFactor, Jacobian ) {
Point3(-3.37493895, 6.14660244, -8.93650986)); Point3(-3.37493895, 6.14660244, -8.93650986));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative
Matrix actualH1; Matrix actualH1;
@ -205,8 +205,8 @@ TEST( PoseBetweenFactor, JacobianWithTransform ) {
Point3(-3.5257579, 6.02637531, -8.98382384)); Point3(-3.5257579, 6.02637531, -8.98382384));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative
Matrix actualH1; Matrix actualH1;

4
gtsam_unstable/slam/tests/testPosePriorFactor.cpp
View File

@ -166,7 +166,7 @@ TEST( PosePriorFactor, Jacobian ) {
Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative
Matrix actualH1; Matrix actualH1;
@ -190,7 +190,7 @@ TEST( PosePriorFactor, JacobianWithTransform ) {
Point3(-4.74767676, 7.67044942, -11.00985)); Point3(-4.74767676, 7.67044942, -11.00985));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative
Matrix actualH1; Matrix actualH1;

4
gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp
View File

@ -170,7 +170,7 @@ TEST( ProjectionFactor, Jacobian ) {
CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
// Verify H2 with numerical derivative // Verify H2 with numerical derivative
Matrix H2Expected = numericalDerivative32<Pose3, Pose3, Point3>( Matrix H2Expected = numericalDerivative32<Vector,Pose3, Pose3, Point3>(
boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>( boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3, boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
boost::none, boost::none, boost::none)), pose, Pose3(), point); boost::none, boost::none, boost::none)), pose, Pose3(), point);
@ -205,7 +205,7 @@ TEST( ProjectionFactor, JacobianWithTransform ) {
CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
// Verify H2 with numerical derivative // Verify H2 with numerical derivative
Matrix H2Expected = numericalDerivative32<Pose3, Pose3, Point3>( Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>( boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3, boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
boost::none, boost::none, boost::none)), pose, body_P_sensor, point); boost::none, boost::none, boost::none)), pose, body_P_sensor, point);