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>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>

597
gtsam/base/numericalDerivative.h
View File

@ -16,7 +16,6 @@
*/
// \callgraph
#pragma once
#include <boost/function.hpp>
@ -31,6 +30,7 @@
#include <gtsam/base/LieVector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Manifold.h>
namespace gtsam {
@ -56,10 +56,13 @@ namespace gtsam {
* 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)); }
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
@ -67,77 +70,98 @@ namespace gtsam {
* The class X needs to have dim, expmap, logmap
*/
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 delta = 1e-5) {
double factor = 1.0 / (2.0 * delta);
const size_t n = x.dim();
Vector d = zero(n), g = zero(n);
for (size_t j=0;j<n;j++) {
d(j) += delta; double hxplus = h(x.retract(d));
d(j) -= 2*delta; double hxmin = h(x.retract(d));
d(j) += delta; g(j) = (hxplus-hxmin)*factor;
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 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;
}
template<class X>
Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) {
return numericalGradient<X>(boost::bind(h, _1), x, delta);
}
/**
* Compute numerical derivative in argument 1 of unary function
* @brief New-style numerical derivatives using manifold_traits
* @brief Computes numerical derivative in argument 1 of unary function
* @param h unary function yielding m-vector
* @param x n-dimensional value at which to evaluate h
* @param delta increment for numerical derivative
* Class Y is the output argument
* Class X is the input argument
* @return m*n Jacobian computed via central differencing
* Both classes X,Y need dim, expmap, logmap
*/
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);
double factor = 1.0/(2.0*delta);
const size_t m = hx.dim(), n = x.dim();
Vector d = zero(n);
ChartY chartY(hx);
TangentY zeroY = chartY.apply(hx);
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);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x.retract(d)));
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x.retract(d)));
d(j) += delta;
H.col(j) << (hxplus-hxmin)*factor;
double factor = 1.0 / (2.0 * delta);
for (int 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)));
dx(j) = 0;
H.col(j) << (dy1 - dy2) * factor;
}
return H;
}
/** use a raw C++ function pointer */
template<class Y, class X>
Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) {
Matrix numericalDerivative11(Y (*h)(const X&), const X& x,
double delta = 1e-5) {
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
* @param h binary function yielding m-vector
@ -145,62 +169,24 @@ namespace gtsam {
* @param x2 second argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* All classes Y,X1,X2 need dim, expmap, logmap
*/
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) {
Y hx = h(x1,x2);
double factor = 1.0/(2.0*delta);
const size_t m = hx.dim(), n = x1.dim();
Vector d = zero(n);
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));
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1.retract(d),x2));
d(j) += delta;
H.col(j) << (hxplus-hxmin)*factor;
}
return H;
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<Y>::value,
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X1>::value,
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2), x1, delta);
}
/** use a raw C++ function pointer */
template<class Y, class X1, class X2>
inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta=1e-5) {
inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
const X2& x2, double delta = 1e-5) {
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
* @param h binary function yielding m-vector
@ -208,63 +194,24 @@ namespace gtsam {
* @param x2 n-dimensional second argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* All classes Y,X1,X2 need dim, expmap, logmap
*/
template<class Y, class X1, class X2>
Matrix numericalDerivative22
(boost::function<Y(const X1&, const X2&)> h,
Matrix numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta = 1e-5) {
Y hx = h(x1,x2);
double factor = 1.0/(2.0*delta);
const size_t m = hx.dim(), n = x2.dim();
Vector d = zero(n);
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;
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<Y>::value,
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X2>::value,
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1), x2, delta);
}
/** use a raw C++ function pointer */
template<class Y, class X1, class X2>
inline Matrix numericalDerivative22
(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) {
inline Matrix numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
const X2& x2, double delta = 1e-5) {
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
* @param h ternary function yielding m-vector
@ -276,58 +223,21 @@ namespace gtsam {
* All classes Y,X1,X2,X3 need dim, expmap, logmap
*/
template<class Y, class X1, class X2, class X3>
Matrix numericalDerivative31
(boost::function<Y(const X1&, const X2&, const X3&)> h,
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5)
{
Y hx = h(x1,x2,x3);
double factor = 1.0/(2.0*delta);
const size_t m = hx.dim(), n = x1.dim();
Vector d = zero(n);
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;
Matrix numericalDerivative31(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<Y>::value,
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X1>::value,
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3), x1, delta);
}
template<class Y, class X1, class X2, class X3>
inline Matrix numericalDerivative31
(Y (*h)(const X1&, const X2&, const 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 X1, class X2, class 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) {
return numericalDerivative31<LieVector,X1,X2,X3>(
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);
return numericalDerivative31<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
x2, x3, delta);
}
/**
@ -341,58 +251,21 @@ namespace gtsam {
* All classes Y,X1,X2,X3 need dim, expmap, logmap
*/
template<class Y, class X1, class X2, class X3>
Matrix numericalDerivative32
(boost::function<Y(const X1&, const X2&, const X3&)> h,
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5)
{
Y hx = h(x1,x2,x3);
double factor = 1.0/(2.0*delta);
const size_t m = hx.dim(), n = x2.dim();
Vector d = zero(n);
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;
Matrix numericalDerivative32(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<Y>::value,
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X2>::value,
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3), x2, delta);
}
template<class Y, class X1, class X2, class X3>
inline Matrix numericalDerivative32
(Y (*h)(const X1&, const X2&, const 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 X1, class X2, class 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) {
return numericalDerivative32<LieVector,X1,X2,X3>(
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);
return numericalDerivative32<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
x2, x3, delta);
}
/**
@ -406,58 +279,21 @@ namespace gtsam {
* All classes Y,X1,X2,X3 need dim, expmap, logmap
*/
template<class Y, class X1, class X2, class X3>
Matrix numericalDerivative33
(boost::function<Y(const X1&, const X2&, const X3&)> h,
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5)
{
Y hx = h(x1,x2,x3);
double factor = 1.0/(2.0*delta);
const size_t m = hx.dim(), n = x3.dim();
Vector d = zero(n);
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;
Matrix numericalDerivative33(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<Y>::value,
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X3>::value,
"Template argument X3 must be a manifold type.");
return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1), x3, delta);
}
template<class Y, class X1, class X2, class X3>
inline Matrix numericalDerivative33
(Y (*h)(const X1&, const X2&, const 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 X1, class X2, class 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) {
return numericalDerivative33<LieVector,X1,X2,X3>(
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);
return numericalDerivative33<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
x2, x3, delta);
}
/**
@ -469,75 +305,98 @@ namespace gtsam {
* @return n*n Hessian matrix computed via central differencing
*/
template<class X>
inline Matrix numericalHessian(boost::function<double(const X&)> f, const X& x, double delta=1e-5) {
return numericalDerivative11<X>(boost::function<Vector(const X&)>(boost::bind(
static_cast<Vector (*)(boost::function<double(const X&)>,const X&, double)>(&numericalGradient<X>),
f, _1, delta)), x, delta);
inline Matrix numericalHessian(boost::function<double(const X&)> f, const X& x,
double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X>::value,
"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>
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);
}
/** Helper class that computes the derivative of f w.r.t. x1, centered about
* x1_, as a function of x2
*/
template<class X1, class X2>
class G_x1 {
const boost::function<double(const X1&, const X2&)>& f_;
const X1& x1_;
X1 x1_;
double delta_;
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) {
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>
inline Matrix numericalHessian212(boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2, double delta=1e-5) {
inline Matrix numericalHessian212(
boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
double delta = 1e-5) {
G_x1<X1, X2> g_x1(f, x1, delta);
return numericalDerivative11<X2>(boost::function<Vector (const X2&)>(boost::bind<Vector>(boost::ref(g_x1), _1)), x2, 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);
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>
inline Matrix numericalHessian211(boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2, double delta=1e-5) {
inline Matrix numericalHessian211(
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>;
Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
double) = &numericalGradient<X1>;
boost::function<double(const X1&)> f2(boost::bind(f, _1, x2));
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>
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);
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);
}
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) {
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>;
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);
return numericalDerivative11<Vector,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);
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);
}
/**
@ -545,81 +404,123 @@ namespace gtsam {
*/
/* **************************************************************** */
template<class X1, class X2, class X3>
inline Matrix numericalHessian311(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 numericalHessian311(
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&,
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);
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 numericalHessian322(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 numericalHessian322(
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 X2&)>, const X2&, double) = &numericalGradient<X2>;
Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
double) = &numericalGradient<X2>;
boost::function<double(const X2&)> f2(boost::bind(f, x1, _1, x3));
return numericalDerivative11<X2>(boost::function<Vector (const X2&)>(boost::bind(numGrad, f2, _1, delta)), x2, delta);
return numericalDerivative11<Vector,X2>(
boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
x2, delta);
}
template<class X1, class X2, class X3>
inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
return numericalHessian322(boost::function<double (const X1&, const X2&, const X3&)>(f), x1, x2, x3, delta);
inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian322(
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) {
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) {
Vector (*numGrad)(boost::function<double(const X3&)>, const X3&, double) = &numericalGradient<X3>;
Vector (*numGrad)(boost::function<double(const X3&)>, const X3&,
double) = &numericalGradient<X3>;
boost::function<double(const X3&)> f2(boost::bind(f, x1, x2, _1));
return numericalDerivative11<X3>(boost::function<Vector (const X3&)>(boost::bind(numGrad, f2, _1, delta)), x3, delta);
return numericalDerivative11<Vector,X3>(
boost::function<Vector(const X3&)>(boost::bind(numGrad, f2, _1, delta)),
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);
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 numericalHessian312(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,X2>(boost::function<double (const X1&, const X2&)>(boost::bind(f, _1, _2, x3)), x1, x2, delta );
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) {
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 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 );
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 );
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);
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);
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);
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);
}
}

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

@ -15,115 +15,123 @@
* @date Apr 8, 2011
*/
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
double f(const LieVector& x) {
double f(const Vector2& x) {
assert(x.size() == 2);
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) <<
-sin(center(0)), 0.0,
0.0, -cos(center(1)));
Vector expected(2);
expected << cos(x(1)), -sin(x(0));
Matrix actual = numericalHessian(f, center);
Vector actual = numericalGradient<Vector2>(f, x);
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);
return sin(x(0)) * cos(x(1));
}
/* ************************************************************************* */
//
TEST(testNumericalDerivative, numericalHessian2) {
Vector v_center = (Vector(2) << 0.5, 1.0);
LieVector center(v_center);
Vector2 v(0.5, 1.0);
Vector2 x(v);
Matrix expected = (Matrix(2,2) <<
-cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)),
-cos(center(0))*sin(center(1)), -sin(center(0))*cos(center(1)));
Matrix expected = (Matrix(2, 2) << -cos(x(1)) * sin(x(0)), -sin(x(1))
* cos(x(0)), -cos(x(0)) * sin(x(1)), -sin(x(0)) * cos(x(1)));
Matrix actual = numericalHessian(f2, center);
Matrix actual = numericalHessian(f2, x);
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
double f3(const LieVector& x1, const LieVector& x2) {
assert(x1.size() == 1 && x2.size() == 1);
return sin(x1(0)) * cos(x2(0));
double f3(double x1, double x2) {
return sin(x1) * cos(x2);
}
/* ************************************************************************* */
//
TEST(testNumericalDerivative, numericalHessian211) {
Vector v_center1 = (Vector(1) << 1.0);
Vector v_center2 = (Vector(1) << 5.0);
LieVector center1(v_center1), center2(v_center2);
double x1 = 1, x2 = 5;
Matrix expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0)));
Matrix actual11 = numericalHessian211(f3, center1, center2);
Matrix expected11 = (Matrix(1, 1) << -sin(x1) * cos(x2));
Matrix actual11 = numericalHessian211<double, double>(f3, x1, x2);
EXPECT(assert_equal(expected11, actual11, 1e-5));
Matrix expected12 = (Matrix(1, 1) <<-cos(center1(0))*sin(center2(0)));
Matrix actual12 = numericalHessian212(f3, center1, center2);
Matrix expected12 = (Matrix(1, 1) << -cos(x1) * sin(x2));
Matrix actual12 = numericalHessian212<double, double>(f3, x1, x2);
EXPECT(assert_equal(expected12, actual12, 1e-5));
Matrix expected22 = (Matrix(1, 1) <<-sin(center1(0))*cos(center2(0)));
Matrix actual22 = numericalHessian222(f3, center1, center2);
Matrix expected22 = (Matrix(1, 1) << -sin(x1) * cos(x2));
Matrix actual22 = numericalHessian222<double, double>(f3, x1, x2);
EXPECT(assert_equal(expected22, actual22, 1e-5));
}
/* ************************************************************************* */
double f4(const LieVector& x, const LieVector& y, const LieVector& z) {
assert(x.size() == 1 && y.size() == 1 && z.size() == 1);
return sin(x(0)) * cos(y(0)) * z(0)*z(0);
double f4(double x, double y, double z) {
return sin(x) * cos(y) * z * z;
}
/* ************************************************************************* */
//
TEST(testNumericalDerivative, numericalHessian311) {
Vector v_center1 = (Vector(1) << 1.0);
Vector v_center2 = (Vector(1) << 2.0);
Vector v_center3 = (Vector(1) << 3.0);
LieVector center1(v_center1), center2(v_center2), center3(v_center3);
double x = center1(0), y = center2(0), z = center3(0);
double x = 1, y = 2, z = 3;
Matrix expected11 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z);
Matrix actual11 = numericalHessian311(f4, center1, center2, center3);
Matrix actual11 = numericalHessian311<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected11, actual11, 1e-5));
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));
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));
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));
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));
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));
}
/* ************************************************************************* */
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 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);
EXPECT(assert_equal(expectedJacobian, actualJacobian));
}
@ -439,19 +440,18 @@ TEST( Rot3, between )
/* ************************************************************************* */
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?
// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
// Left trivialization Derivative of exp(w) wrpt w:
// 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))
Vector testDexpL(const LieVector& dw) {
Vector y = Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw));
return y;
Vector3 testDexpL(const Vector3& dw) {
return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw));
}
TEST( Rot3, dexpL) {
Matrix actualDexpL = Rot3::dexpL(w);
Matrix expectedDexpL = numericalDerivative11(
boost::function<Vector(const LieVector&)>(
boost::bind(testDexpL, _1)), LieVector(zero(3)), 1e-2);
Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
Vector3::Zero(), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
Matrix actualDexpInvL = Rot3::dexpInvL(w);

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

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

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

@ -115,13 +115,13 @@ TEST(Unit3, error) {
Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Unit3>(
expected = numericalDerivative11<Vector,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), q);
p.error(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
}
{
expected = numericalDerivative11<Unit3>(
expected = numericalDerivative11<Vector,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), r);
p.error(r, actual);
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/GaussianFactorGraph.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/assign/list_of.hpp>
@ -150,7 +149,7 @@ TEST( GaussianBayesNet, DeterminantTest )
/* ************************************************************************* */
namespace {
double computeError(const GaussianBayesNet& gbn, const LieVector& values)
double computeError(const GaussianBayesNet& gbn, const Vector& values)
{
pair<Matrix,Vector> Rd = GaussianFactorGraph(gbn).jacobian();
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)));
// Compute the Hessian numerically
Matrix hessian = numericalHessian(
boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())));
Matrix hessian = numericalHessian<Vector>(boost::bind(&computeError, gbn, _1),
Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols()));
// Compute the gradient numerically
Vector gradient = numericalGradient(
boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())));
Vector gradient = numericalGradient<Vector>(boost::bind(&computeError, gbn, _1),
Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols()));
// Compute the gradient using dense matrices
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));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Rot3>(
Matrix expectedH = numericalDerivative11<Vector,Rot3>(
boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none),
nRb);
@ -78,7 +78,7 @@ TEST( Pose3AttitudeFactor, Constructor ) {
EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Pose3>(
Matrix expectedH = numericalDerivative11<Vector,Pose3>(
boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1,
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));
// 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);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(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);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(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));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Pose3>(
Matrix expectedH = numericalDerivative11<Vector,Pose3>(
boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T);
// 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));
// Expected Jacobians
Matrix H1e = numericalDerivative11<Pose3>(
Matrix H1e = numericalDerivative11<Vector,Pose3>(
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);
Matrix H3e = numericalDerivative11<Pose3>(
Matrix H3e = numericalDerivative11<Vector,Pose3>(
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);
Matrix H5e = numericalDerivative11<imuBias::ConstantBias>(
Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
// Check rotation Jacobians
@ -276,15 +276,15 @@ TEST( ImuFactor, ErrorWithBiases )
// EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians
Matrix H1e = numericalDerivative11<Pose3>(
Matrix H1e = numericalDerivative11<Vector,Pose3>(
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);
Matrix H3e = numericalDerivative11<Pose3>(
Matrix H3e = numericalDerivative11<Vector,Pose3>(
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);
Matrix H5e = numericalDerivative11<imuBias::ConstantBias>(
Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
// Check rotation Jacobians
@ -341,7 +341,7 @@ TEST( ImuFactor, PartialDerivativeLogmap )
// Compute numerical derivatives
Matrix expectedDelFdeltheta = numericalDerivative11<LieVector>(boost::bind(
Matrix expectedDelFdeltheta = numericalDerivative11<Vector,LieVector>(boost::bind(
&evaluateLogRotation, thetahat, _1), LieVector(deltatheta));
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));
// 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);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(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);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(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);
// Expected Jacobians
Matrix H1e = numericalDerivative11<Pose3>(
Matrix H1e = numericalDerivative11<Vector,Pose3>(
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);
Matrix H3e = numericalDerivative11<Pose3>(
Matrix H3e = numericalDerivative11<Vector,Pose3>(
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);
Matrix H5e = numericalDerivative11<imuBias::ConstantBias>(
Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
// Check rotation Jacobians

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

@ -72,35 +72,35 @@ TEST( MagFactor, Factors ) {
// MagFactor
MagFactor f(1, measured, s, dir, bias, model);
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));
// MagFactor1
MagFactor1 f1(1, measured, s, dir, bias, model);
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));
// MagFactor2
MagFactor2 f2(1, 2, measured, nRb, model);
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),//
H1, 1e-7));
EXPECT( assert_equal(numericalDerivative11<Point3> //
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
(boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),//
H2, 1e-7));
// MagFactor2
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(numericalDerivative11<LieScalar> //
EXPECT(assert_equal(numericalDerivative11<Vector,LieScalar> //
(boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
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),//
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),//
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));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>(
Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Pose3>(
Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1,
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
Matrix Hexpected;
Hexpected = numericalDerivative11<EssentialMatrix>(
Hexpected = numericalDerivative11<Vector,EssentialMatrix>(
boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1,
boost::none), trueE);
@ -173,8 +173,8 @@ TEST (EssentialMatrixFactor2, factor) {
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2,
boost::none, boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, trueE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, trueE, d);
Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, trueE, d);
Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, trueE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@ -247,8 +247,8 @@ TEST (EssentialMatrixFactor3, factor) {
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2,
boost::none, boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, bodyE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, bodyE, d);
Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, bodyE, d);
Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, bodyE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@ -389,8 +389,8 @@ TEST (EssentialMatrixFactor2, extraTest) {
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2,
boost::none, boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, trueE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, trueE, d);
Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, trueE, d);
Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, trueE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
@ -458,8 +458,8 @@ TEST (EssentialMatrixFactor3, extraTest) {
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2,
boost::none, boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, bodyE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, bodyE, d);
Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, bodyE, d);
Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, bodyE, d);
// Verify the Jacobian is correct
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;
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Rot3>(
expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc);
f.evaluateError(iRc, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}
{
expected = numericalDerivative11<Rot3>(
expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R);
f.evaluateError(R, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
@ -141,14 +141,14 @@ TEST (RotateDirectionsFactor, test) {
Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Rot3>(
expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
boost::none), iRc);
f.evaluateError(iRc, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}
{
expected = numericalDerivative11<Rot3>(
expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
boost::none), R);
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
static Vector Logmap(const Pose3Upright& p);
private:
/// @}
/// @name Advanced Interface
/// @{
private:
// Serialization function
friend class boost::serialization::access;
@ -142,4 +140,18 @@ private:
}; // \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

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));
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);
Matrix actual;
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));
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);
Matrix actual;
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));
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);
Matrix 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/Cal3Bundler.h>
#include <gtsam_unstable/nonlinear/Expression.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.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(Expression, AutoDiff) {

8
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -105,10 +105,14 @@ public:
boost::optional<gtsam::Matrix&> H2=boost::none) const {
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) {
(*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);

8
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -108,10 +108,14 @@ public:
boost::optional<gtsam::Matrix&> H2=boost::none) const {
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) {
(*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);

10
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -108,10 +108,10 @@ public:
boost::optional<gtsam::Matrix&> H2=boost::none) const {
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) {
(*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);
@ -228,13 +228,13 @@ public:
boost::optional<gtsam::Matrix&> H3=boost::none) const {
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) {
(*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) {
(*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);

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

@ -176,8 +176,8 @@ TEST( PoseBetweenFactor, Jacobian ) {
Point3(-3.37493895, 6.14660244, -8.93650986));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<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 expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
// Use the factor to calculate the derivative
Matrix actualH1;
@ -205,8 +205,8 @@ TEST( PoseBetweenFactor, JacobianWithTransform ) {
Point3(-3.5257579, 6.02637531, -8.98382384));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<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 expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
// Use the factor to calculate the derivative
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));
// 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
Matrix actualH1;
@ -190,7 +190,7 @@ TEST( PosePriorFactor, JacobianWithTransform ) {
Point3(-4.74767676, 7.67044942, -11.00985));
// 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
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));
// 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::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
boost::none, boost::none, boost::none)), pose, Pose3(), point);
@ -205,7 +205,7 @@ TEST( ProjectionFactor, JacobianWithTransform ) {
CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
// 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::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
boost::none, boost::none, boost::none)), pose, body_P_sensor, point);