Switched to using LieVectors for vector types. Still some problems with numericalDerivative that have been worked around, but other functionality is intact.
Alex Cunningham
parent
77eda5ab8c
commit
eed13e48d2
|
|
@ -6,22 +6,69 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
|
||||
namespace gtsam {
|
||||
/** temporarily using the old system */
|
||||
|
||||
// The rest of the file makes double and Vector behave as a Lie type (with + as compose)
|
||||
/**
|
||||
* LieVector is a wrapper around vector to allow it to be a Lie type
|
||||
*/
|
||||
struct LieVector : public Vector, public Lie<LieVector>, Testable<LieVector> {
|
||||
|
||||
// Vector group operations
|
||||
inline Vector compose(const Vector& p1,const Vector& p2) { return p1+p2;}
|
||||
inline Vector inverse(const Vector& p) { return -p;}
|
||||
inline Vector between(const Vector& p1,const Vector& p2) { return p2-p1;}
|
||||
/** default constructor - should be unnecessary */
|
||||
LieVector() {}
|
||||
|
||||
// Vector is a trivial Lie group
|
||||
template<> inline Vector expmap(const Vector& d) { return d;}
|
||||
template<> inline Vector expmap(const Vector& p,const Vector& d) { return p+d;}
|
||||
template<> inline Vector logmap(const Vector& p) { return p;}
|
||||
template<> inline Vector logmap(const Vector& p1,const Vector& p2) { return p2-p1;}
|
||||
/** initialize from a normal vector */
|
||||
LieVector(const Vector& v) : Vector(v) {}
|
||||
|
||||
/** get the underlying vector */
|
||||
inline Vector vector() const {
|
||||
return static_cast<Vector>(*this);
|
||||
}
|
||||
|
||||
/** print @param s optional string naming the object */
|
||||
inline void print(const std::string& name="") const {
|
||||
gtsam::print(vector(), name);
|
||||
}
|
||||
|
||||
/** equality up to tolerance */
|
||||
inline bool equals(const LieVector& expected, double tol=1e-5) const {
|
||||
return gtsam::equal(vector(), expected.vector(), tol);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns dimensionality of the tangent space
|
||||
*/
|
||||
inline size_t dim() const { return this->size(); }
|
||||
|
||||
/**
|
||||
* Returns Exponential map update of T
|
||||
* Default implementation calls global binary function
|
||||
*/
|
||||
LieVector expmap(const Vector& v) const { return LieVector(vector() + v); }
|
||||
|
||||
/** expmap around identity */
|
||||
static LieVector Expmap(const Vector& v) { return LieVector(v); }
|
||||
|
||||
/**
|
||||
* Returns Log map
|
||||
* Default Implementation calls global binary function
|
||||
*/
|
||||
Vector logmap(const LieVector& lp) const { return *this - lp; }
|
||||
|
||||
/** Logmap around identity - just returns with default cast back */
|
||||
static Vector Logmap(const LieVector& p) { return p; }
|
||||
|
||||
/** compose with another object */
|
||||
inline LieVector compose(const LieVector& p) const {
|
||||
return LieVector(vector() + p);
|
||||
}
|
||||
|
||||
/** invert the object and yield a new one */
|
||||
inline LieVector inverse() const {
|
||||
return LieVector(-1.0 * vector());
|
||||
}
|
||||
};
|
||||
} // \namespace gtsam
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@ headers += Testable.h TestableAssertions.h numericalDerivative.h
|
|||
|
||||
# Lie Groups
|
||||
headers += Lie.h Lie-inl.h LieVector.h
|
||||
check_PROGRAMS += tests/testLieVector
|
||||
|
||||
# Data structures
|
||||
headers += BTree.h DSF.h
|
||||
|
|
|
|||
|
|
@ -12,6 +12,7 @@
|
|||
#include <boost/bind.hpp>
|
||||
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/base/LieVector.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
||||
//#define LINEARIZE_AT_IDENTITY
|
||||
|
|
@ -40,6 +41,11 @@ namespace gtsam {
|
|||
* http://www.boost.org/doc/libs/1_43_0/libs/bind/bind.html
|
||||
*/
|
||||
|
||||
|
||||
/** global functions for converting to a LieVector for use with numericalDerivative */
|
||||
LieVector makeLieVector(const Vector& v) { return LieVector(v); }
|
||||
LieVector makeLieVectorD(double d) { return LieVector(Vector_(1, d)); }
|
||||
|
||||
/**
|
||||
* Numerically compute gradient of scalar function
|
||||
* Class X is the input argument
|
||||
|
|
@ -89,31 +95,32 @@ namespace gtsam {
|
|||
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) {
|
||||
return numericalDerivative11<Y,X>(boost::bind(h, _1), x, delta);
|
||||
}
|
||||
|
||||
/** pseudo-template specialization for double Y values */
|
||||
/** remapping for double valued functions */
|
||||
template<class X>
|
||||
Matrix numericalDerivative11(boost::function<double(const X&)> h, const X& x, double delta=1e-5) {
|
||||
double hx = h(x);
|
||||
double factor = 1.0/(2.0*delta);
|
||||
const size_t m = 1, n = dim(x);
|
||||
Vector d(n,0.0);
|
||||
Matrix H = zeros(m,n);
|
||||
for (size_t j=0;j<n;j++) {
|
||||
d(j) += delta; Vector hxplus = Vector_(1, h(expmap(x,d))-hx);
|
||||
d(j) -= 2*delta; Vector hxmin = Vector_(1, h(expmap(x,d))-hx);
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
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<X>(boost::bind(h, _1), x, delta);
|
||||
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);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -142,6 +149,7 @@ namespace gtsam {
|
|||
return H;
|
||||
}
|
||||
|
||||
/** 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) {
|
||||
|
|
@ -152,24 +160,30 @@ namespace gtsam {
|
|||
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) {
|
||||
double hx = h(x1,x2);
|
||||
double factor = 1.0/(2.0*delta);
|
||||
const size_t m = 1, n = dim(x1);
|
||||
Vector d(n,0.0);
|
||||
Matrix H = zeros(m,n);
|
||||
for (size_t j=0;j<n;j++) {
|
||||
d(j) += delta; Vector hxplus = Vector_(1, h(expmap(x1,d),x2)-hx);
|
||||
d(j) -= 2*delta; Vector hxmin = Vector_(1, h(expmap(x1,d),x2)-hx);
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
return numericalDerivative21<LieVector,X1,X2>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta);
|
||||
}
|
||||
|
||||
template<class X1, class X2>
|
||||
inline Matrix numericalDerivative21(double (*h)(const X1&, const X2&),
|
||||
Matrix numericalDerivative21(double (*h)(const X1&, const X2&),
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative21<X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
|
||||
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);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -199,33 +213,42 @@ namespace gtsam {
|
|||
}
|
||||
return H;
|
||||
}
|
||||
|
||||
/** 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) {
|
||||
return numericalDerivative22<Y,X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
|
||||
}
|
||||
|
||||
/** pseudo-specialization for double Y values */
|
||||
/** 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) {
|
||||
double hx = h(x1,x2);
|
||||
double factor = 1.0/(2.0*delta);
|
||||
const size_t m = 1, n = dim(x2);
|
||||
Vector d(n,0.0);
|
||||
Matrix H = zeros(m,n);
|
||||
for (size_t j=0;j<n;j++) {
|
||||
d(j) += delta; Vector hxplus = Vector_(1,h(x1,expmap(x2,d))-hx);
|
||||
d(j) -= 2*delta; Vector hxmin = Vector_(1,h(x1,expmap(x2,d))-hx);
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
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&),
|
||||
inline Matrix numericalDerivative22(double (*h)(const X1&, const X2&),
|
||||
const X1& x1, const X2& x2, double delta=1e-5) {
|
||||
return numericalDerivative22<X1,X2>(boost::bind(h, _1, _2), x1, x2, delta);
|
||||
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);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -263,7 +286,46 @@ namespace gtsam {
|
|||
return numericalDerivative31<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
// arg 2
|
||||
/** 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);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical derivative in argument 2 of ternary function
|
||||
* @param h ternary function yielding m-vector
|
||||
* @param x1 n-dimensional first argument value
|
||||
* @param x2 second argument value
|
||||
* @param x3 third argument value
|
||||
* @param delta increment for numerical derivative
|
||||
* @return m*n Jacobian computed via central differencing
|
||||
* 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,
|
||||
|
|
@ -289,7 +351,46 @@ namespace gtsam {
|
|||
return numericalDerivative32<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
// arg 3
|
||||
/** 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);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute numerical derivative in argument 3 of ternary function
|
||||
* @param h ternary function yielding m-vector
|
||||
* @param x1 n-dimensional first argument value
|
||||
* @param x2 second argument value
|
||||
* @param x3 third argument value
|
||||
* @param delta increment for numerical derivative
|
||||
* @return m*n Jacobian computed via central differencing
|
||||
* 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,
|
||||
|
|
@ -315,82 +416,34 @@ namespace gtsam {
|
|||
return numericalDerivative33<Y,X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* specializations for double outputs
|
||||
*/
|
||||
/** 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,
|
||||
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) {
|
||||
double hx = h(x1,x2,x3);
|
||||
double factor = 1.0/(2.0*delta);
|
||||
const size_t m = 1, n = dim(x1);
|
||||
Vector d(n,0.0);
|
||||
Matrix H = zeros(m,n);
|
||||
for (size_t j=0;j<n;j++) {
|
||||
d(j) += delta; Vector hxplus = Vector_(1,h(expmap(x1,d),x2,x3)-hx);
|
||||
d(j) -= 2*delta; Vector hxmin = Vector_(1,h(expmap(x1,d),x2,x3)-hx);
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
}
|
||||
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<X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
|
||||
return numericalDerivative33<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
// arg 2
|
||||
template<class X1, class X2, class X3>
|
||||
Matrix numericalDerivative32
|
||||
(boost::function<double(const X1&, const X2&, const X3&)> h,
|
||||
inline Matrix numericalDerivative33(double (*h)(const X1&, const X2&, const X3&),
|
||||
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) {
|
||||
double hx = h(x1,x2,x3);
|
||||
double factor = 1.0/(2.0*delta);
|
||||
const size_t m = 1, n = dim(x2);
|
||||
Vector d(n,0.0);
|
||||
Matrix H = zeros(m,n);
|
||||
for (size_t j=0;j<n;j++) {
|
||||
d(j) += delta; Vector hxplus = Vector_(1,h(x1, expmap(x2,d),x3)-hx);
|
||||
d(j) -= 2*delta; Vector hxmin = Vector_(1,h(x1, expmap(x2,d),x3)-hx);
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
}
|
||||
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<X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
|
||||
return numericalDerivative33<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
// arg 3
|
||||
/** pseudo-partial template specialization for vector return values */
|
||||
template<class X1, class X2, class X3>
|
||||
Matrix numericalDerivative33
|
||||
(boost::function<double(const X1&, const X2&, const X3&)> h,
|
||||
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) {
|
||||
double hx = h(x1,x2,x3);
|
||||
double factor = 1.0/(2.0*delta);
|
||||
const size_t m = 1, n = dim(x3);
|
||||
Vector d(n,0.0);
|
||||
Matrix H = zeros(m,n);
|
||||
for (size_t j=0;j<n;j++) {
|
||||
d(j) += delta; Vector hxplus = Vector_(1,h(x1, x2, expmap(x3,d))-hx);
|
||||
d(j) -= 2*delta; Vector hxmin = Vector_(1,h(x1, x2, expmap(x3,d))-hx);
|
||||
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
|
||||
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
|
||||
}
|
||||
return H;
|
||||
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
|
||||
(double (*h)(const X1&, const X2&, const 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<X1,X2, X3>(boost::bind(h, _1, _2, _3), x1, x2, x3, delta);
|
||||
return numericalDerivative33<LieVector,X1,X2,X3>(
|
||||
boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,26 @@
|
|||
/**
|
||||
* @file testLieVector.cpp
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <gtsam/CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/base/LieVector.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testLieVector, construction ) {
|
||||
Vector v = Vector_(3, 1.0, 2.0, 3.0);
|
||||
LieVector lie1(v), lie2(v);
|
||||
|
||||
EXPECT(lie1.dim() == 3);
|
||||
EXPECT(assert_equal(v, lie1.vector()));
|
||||
EXPECT(assert_equal(lie1, lie2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
|
|
@ -439,8 +439,8 @@ TEST( Pose2, bearing )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double range_proxy(const Pose2& pose, const Point2& point) {
|
||||
return pose.range(point);
|
||||
LieVector range_proxy(const Pose2& pose, const Point2& point) {
|
||||
return LieVector(Vector_(1, pose.range(point)));
|
||||
}
|
||||
TEST( Pose2, range )
|
||||
{
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@ using namespace gtsam;
|
|||
using namespace std;
|
||||
static double inf = std::numeric_limits<double>::infinity();
|
||||
|
||||
typedef TypedSymbol<Vector, 'v'> VecKey;
|
||||
typedef TypedSymbol<LieVector, 'v'> VecKey;
|
||||
typedef LieConfig<VecKey> Config;
|
||||
|
||||
VecKey key1(1), key2(2), key3(3), key4(4);
|
||||
|
|
@ -211,7 +211,7 @@ TEST(LieConfig, exists_)
|
|||
config0.insert(key1, Vector_(1, 1.));
|
||||
config0.insert(key2, Vector_(1, 2.));
|
||||
|
||||
boost::optional<Vector> v = config0.exists_(key1);
|
||||
boost::optional<LieVector> v = config0.exists_(key1);
|
||||
CHECK(assert_equal(Vector_(1, 1.),*v));
|
||||
}
|
||||
|
||||
|
|
@ -237,7 +237,7 @@ TEST(LieConfig, update)
|
|||
/* ************************************************************************* */
|
||||
TEST(LieConfig, dummy_initialization)
|
||||
{
|
||||
typedef TypedSymbol<Vector, 'z'> Key;
|
||||
typedef TypedSymbol<LieVector, 'z'> Key;
|
||||
typedef LieConfig<Key> Config1;
|
||||
|
||||
Config1 init1;
|
||||
|
|
|
|||
|
|
@ -5,54 +5,35 @@
|
|||
**/
|
||||
|
||||
#include <gtsam/slam/Simulated3D.h>
|
||||
#include <gtsam/nonlinear/LieConfig-inl.h>
|
||||
#include <gtsam/nonlinear/TupleConfig-inl.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace simulated3D;
|
||||
INSTANTIATE_LIE_CONFIG(PointKey)
|
||||
INSTANTIATE_LIE_CONFIG(PoseKey)
|
||||
INSTANTIATE_TUPLE_CONFIG2(PoseConfig, PointConfig)
|
||||
|
||||
namespace simulated3D {
|
||||
|
||||
Vector prior (const Vector& x)
|
||||
{
|
||||
Point3 prior (const Point3& x, boost::optional<Matrix&> H) {
|
||||
if (H) *H = eye(3);
|
||||
return x;
|
||||
}
|
||||
|
||||
Matrix Dprior(const Vector& x)
|
||||
{
|
||||
Matrix H = eye((int) x.size());
|
||||
return H;
|
||||
}
|
||||
|
||||
Vector odo(const Vector& x1, const Vector& x2)
|
||||
{
|
||||
Point3 odo(const Point3& x1, const Point3& x2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
|
||||
if (H1) *H1 = -1 * eye(3);
|
||||
if (H2) *H2 = eye(3);
|
||||
return x2 - x1;
|
||||
}
|
||||
|
||||
Matrix Dodo1(const Vector& x1, const Vector& x2)
|
||||
{
|
||||
Matrix H = -1 * eye((int) x1.size());
|
||||
return H;
|
||||
}
|
||||
|
||||
Matrix Dodo2(const Vector& x1, const Vector& x2)
|
||||
{
|
||||
Matrix H = eye((int) x1.size());
|
||||
return H;
|
||||
}
|
||||
|
||||
|
||||
Vector mea(const Vector& x, const Vector& l)
|
||||
{
|
||||
Point3 mea(const Point3& x, const Point3& l,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
|
||||
if (H1) *H1 = -1 * eye(3);
|
||||
if (H2) *H2 = eye(3);
|
||||
return l - x;
|
||||
}
|
||||
|
||||
Matrix Dmea1(const Vector& x, const Vector& l)
|
||||
{
|
||||
Matrix H = -1 * eye((int) x.size());
|
||||
return H;
|
||||
}
|
||||
|
||||
Matrix Dmea2(const Vector& x, const Vector& l)
|
||||
{
|
||||
Matrix H = eye((int) x.size());
|
||||
return H;
|
||||
}
|
||||
|
||||
}} // namespace gtsam::simulated3D
|
||||
|
|
|
|||
|
|
@ -9,73 +9,72 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/LieVector.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/linear/VectorConfig.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/nonlinear/TupleConfig.h>
|
||||
|
||||
// \namespace
|
||||
|
||||
namespace gtsam {
|
||||
namespace simulated3D {
|
||||
|
||||
typedef VectorConfig VectorConfig;
|
||||
typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
|
||||
typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
|
||||
|
||||
typedef gtsam::TypedSymbol<Vector, 'x'> PoseKey;
|
||||
typedef gtsam::TypedSymbol<Vector, 'l'> PointKey;
|
||||
typedef LieConfig<PoseKey> PoseConfig;
|
||||
typedef LieConfig<PointKey> PointConfig;
|
||||
typedef TupleConfig2<PoseConfig, PointConfig> Config;
|
||||
|
||||
/**
|
||||
* Prior on a single pose
|
||||
*/
|
||||
Vector prior(const Vector& x);
|
||||
Matrix Dprior(const Vector& x);
|
||||
Point3 prior(const Point3& x, boost::optional<Matrix&> H = boost::none);
|
||||
|
||||
/**
|
||||
* odometry between two poses
|
||||
*/
|
||||
Vector odo(const Vector& x1, const Vector& x2);
|
||||
Matrix Dodo1(const Vector& x1, const Vector& x2);
|
||||
Matrix Dodo2(const Vector& x1, const Vector& x2);
|
||||
Point3 odo(const Point3& x1, const Point3& x2,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none);
|
||||
|
||||
/**
|
||||
* measurement between landmark and pose
|
||||
*/
|
||||
Vector mea(const Vector& x, const Vector& l);
|
||||
Matrix Dmea1(const Vector& x, const Vector& l);
|
||||
Matrix Dmea2(const Vector& x, const Vector& l);
|
||||
Point3 mea(const Point3& x, const Point3& l,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none);
|
||||
|
||||
struct Point2Prior3D: public NonlinearFactor1<VectorConfig, PoseKey> {
|
||||
struct PointPrior3D: public NonlinearFactor1<Config, PoseKey> {
|
||||
|
||||
Vector z_;
|
||||
Point3 z_;
|
||||
|
||||
Point2Prior3D(const Vector& z,
|
||||
PointPrior3D(const Point3& z,
|
||||
const SharedGaussian& model, const PoseKey& j) :
|
||||
NonlinearFactor1<VectorConfig, PoseKey> (model, j), z_(z) {
|
||||
NonlinearFactor1<Config, PoseKey> (model, j), z_(z) {
|
||||
}
|
||||
|
||||
Vector evaluateError(const Vector& x, boost::optional<Matrix&> H =
|
||||
Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
|
||||
boost::none) {
|
||||
if (H) *H = Dprior(x);
|
||||
return prior(x) - z_;
|
||||
return (prior(x, H) - z_).vector();
|
||||
}
|
||||
};
|
||||
|
||||
struct Simulated3DMeasurement: public NonlinearFactor2<VectorConfig,
|
||||
struct Simulated3DMeasurement: public NonlinearFactor2<Config,
|
||||
PoseKey, PointKey> {
|
||||
|
||||
Vector z_;
|
||||
Point3 z_;
|
||||
|
||||
Simulated3DMeasurement(const Vector& z,
|
||||
Simulated3DMeasurement(const Point3& z,
|
||||
const SharedGaussian& model, PoseKey& j1, PointKey j2) :
|
||||
NonlinearFactor2<VectorConfig, PoseKey, PointKey> (
|
||||
NonlinearFactor2<Config, PoseKey, PointKey> (
|
||||
model, j1, j2), z_(z) {
|
||||
}
|
||||
|
||||
Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
|
||||
Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional<
|
||||
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) {
|
||||
if (H1) *H1 = Dmea1(x1, x2);
|
||||
if (H2) *H2 = Dmea2(x1, x2);
|
||||
return mea(x1, x2) - z_;
|
||||
return (mea(x1, x2, H1, H2) - z_).vector();
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -49,21 +49,12 @@ public:
|
|||
boost::optional<Matrix&> Dforeign = boost::none,
|
||||
boost::optional<Matrix&> Dtrans = boost::none,
|
||||
boost::optional<Matrix&> Dlocal = boost::none) const {
|
||||
if (Dforeign) {
|
||||
Matrix Af;
|
||||
trans.transform_from(foreign, boost::none, Af);
|
||||
*Dforeign = Af;
|
||||
}
|
||||
if (Dtrans) {
|
||||
Matrix At;
|
||||
trans.transform_from(foreign, At, boost::none);
|
||||
*Dtrans = At;
|
||||
}
|
||||
Point newlocal = trans.transform_from(foreign, Dtrans, Dforeign);
|
||||
if (Dlocal) {
|
||||
Point dummy;
|
||||
*Dlocal = -1* eye(dummy.dim());
|
||||
}
|
||||
return gtsam::logmap(gtsam::between<Point>(local, trans.transform_from(foreign)));
|
||||
return gtsam::logmap(gtsam::between<Point>(local, newlocal));
|
||||
}
|
||||
};
|
||||
} // \namespace gtsam
|
||||
|
|
|
|||
|
|
@ -8,6 +8,7 @@
|
|||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/base/LieVector.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
|
||||
|
|
@ -87,8 +88,8 @@ TEST( Pose2Factor, rhs )
|
|||
/* ************************************************************************* */
|
||||
// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
|
||||
// Hence i.e., b = approximates z-h(x0) = error_vector(x0)
|
||||
Vector h(const Pose2& p1,const Pose2& p2) {
|
||||
return covariance->whiten(factor.evaluateError(p1,p2));
|
||||
LieVector h(const Pose2& p1,const Pose2& p2) {
|
||||
return LieVector(covariance->whiten(factor.evaluateError(p1,p2)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -8,6 +8,7 @@
|
|||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/base/LieVector.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
|
||||
|
|
@ -56,8 +57,8 @@ static Pose2Prior factor(1,prior, sigmas);
|
|||
/* ************************************************************************* */
|
||||
// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
|
||||
// Hence i.e., b = approximates z-h(x0) = error_vector(x0)
|
||||
Vector h(const Pose2& p1) {
|
||||
return sigmas->whiten(factor.evaluateError(p1));
|
||||
LieVector h(const Pose2& p1) {
|
||||
return LieVector(sigmas->whiten(factor.evaluateError(p1)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -14,38 +14,35 @@
|
|||
using namespace gtsam;
|
||||
using namespace simulated3D;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( simulated3D, Config )
|
||||
{
|
||||
Config actual;
|
||||
actual.insert(PointKey(1),Point3(1,1,1));
|
||||
actual.insert(PoseKey(2),Point3(2,2,2));
|
||||
EXPECT(assert_equal(actual,actual,1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( simulated3D, Dprior )
|
||||
{
|
||||
Pose3 x1(Rot3::rodriguez(0, 0, 1.57), Point3(1, 5, 0));
|
||||
Vector v = logmap(x1);
|
||||
Matrix numerical = numericalDerivative11(prior,v);
|
||||
Matrix computed = Dprior(v);
|
||||
CHECK(assert_equal(numerical,computed,1e-9));
|
||||
Point3 x(1,-9, 7);
|
||||
Matrix numerical = numericalDerivative11<Point3, Point3>(boost::bind(prior, _1, boost::none),x);
|
||||
Matrix computed;
|
||||
prior(x,computed);
|
||||
EXPECT(assert_equal(numerical,computed,1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( simulated3D, DOdo1 )
|
||||
TEST( simulated3D, DOdo )
|
||||
{
|
||||
Pose3 x1(Rot3::rodriguez(0, 0, 1.57), Point3(1, 5, 0));
|
||||
Vector v1 = logmap(x1);
|
||||
Pose3 x2(Rot3::rodriguez(0, 0, 0), Point3(2, 3, 0));
|
||||
Vector v2 = logmap(x2);
|
||||
Matrix numerical = numericalDerivative21(odo,v1,v2);
|
||||
Matrix computed = Dodo1(v1,v2);
|
||||
CHECK(assert_equal(numerical,computed,1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( simulated3D, DOdo2 )
|
||||
{
|
||||
Pose3 x1(Rot3::rodriguez(0, 0, 1.57), Point3(1, 5, 0));
|
||||
Vector v1 = logmap(x1);
|
||||
Pose3 x2(Rot3::rodriguez(0, 0, 0), Point3(2, 3, 0));
|
||||
Vector v2 = logmap(x2);
|
||||
Matrix numerical = numericalDerivative22(odo,v1,v2);
|
||||
Matrix computed = Dodo2(v1,v2);
|
||||
CHECK(assert_equal(numerical,computed,1e-9));
|
||||
Point3 x1(1,-9,7),x2(-5,6,7);
|
||||
Matrix H1,H2;
|
||||
odo(x1,x2,H1,H2);
|
||||
Matrix A1 = numericalDerivative21<Point3, Point3, Point3>(boost::bind(odo, _1, _2, boost::none, boost::none),x1,x2);
|
||||
EXPECT(assert_equal(A1,H1,1e-9));
|
||||
Matrix A2 = numericalDerivative22<Point3, Point3, Point3>(boost::bind(odo, _1, _2, boost::none, boost::none),x1,x2);
|
||||
EXPECT(assert_equal(A2,H2,1e-9));
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@ check_PROGRAMS += testNonlinearEqualityConstraint testBoundingConstraint
|
|||
check_PROGRAMS += testTransformConstraint testLinearApproxFactor
|
||||
|
||||
# experimental
|
||||
#check_PROGRAMS += testFusionTupleConfig
|
||||
#check_PROGRAMS += testFusionTupleConfig # Doesn't work on Macs
|
||||
|
||||
if USE_LDL
|
||||
check_PROGRAMS += testConstraintOptimizer
|
||||
|
|
|
|||
|
|
@ -62,6 +62,10 @@ TEST( TransformConstraint, equals ) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LieVector evaluateError_(const PointTransformConstraint& c,
|
||||
const Point2& global, const Pose2& trans, const Point2& local) {
|
||||
return LieVector(c.evaluateError(global, trans, local));
|
||||
}
|
||||
TEST( TransformConstraint, jacobians ) {
|
||||
|
||||
// from examples below
|
||||
|
|
@ -73,14 +77,14 @@ TEST( TransformConstraint, jacobians ) {
|
|||
tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
|
||||
|
||||
Matrix numericalDT, numericalDL, numericalDF;
|
||||
numericalDF = numericalDerivative31<Vector, Point2, Pose2, Point2>(
|
||||
boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
|
||||
numericalDF = numericalDerivative31<LieVector,Point2,Pose2,Point2>(
|
||||
boost::bind(evaluateError_, tc, _1, _2, _3),
|
||||
global, trans, local, 1e-5);
|
||||
numericalDT = numericalDerivative32<Vector, Point2, Pose2, Point2>(
|
||||
boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
|
||||
numericalDT = numericalDerivative32<LieVector,Point2,Pose2,Point2>(
|
||||
boost::bind(evaluateError_, tc, _1, _2, _3),
|
||||
global, trans, local, 1e-5);
|
||||
numericalDL = numericalDerivative33<Vector, Point2, Pose2, Point2>(
|
||||
boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
|
||||
numericalDL = numericalDerivative33<LieVector,Point2,Pose2,Point2>(
|
||||
boost::bind(evaluateError_, tc, _1, _2, _3),
|
||||
global, trans, local, 1e-5);
|
||||
|
||||
CHECK(assert_equal(numericalDF, actualDF));
|
||||
|
|
@ -105,14 +109,14 @@ TEST( TransformConstraint, jacobians_zero ) {
|
|||
tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
|
||||
|
||||
Matrix numericalDT, numericalDL, numericalDF;
|
||||
numericalDF = numericalDerivative31<Vector, Point2, Pose2, Point2>(
|
||||
boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
|
||||
numericalDF = numericalDerivative31<LieVector,Point2,Pose2,Point2>(
|
||||
boost::bind(evaluateError_, tc, _1, _2, _3),
|
||||
global, trans, local, 1e-5);
|
||||
numericalDT = numericalDerivative32<Vector, Point2, Pose2, Point2>(
|
||||
boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
|
||||
numericalDT = numericalDerivative32<LieVector,Point2,Pose2,Point2>(
|
||||
boost::bind(evaluateError_, tc, _1, _2, _3),
|
||||
global, trans, local, 1e-5);
|
||||
numericalDL = numericalDerivative33<Vector, Point2, Pose2, Point2>(
|
||||
boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none),
|
||||
numericalDL = numericalDerivative33<LieVector,Point2,Pose2,Point2>(
|
||||
boost::bind(evaluateError_, tc, _1, _2, _3),
|
||||
global, trans, local, 1e-5);
|
||||
|
||||
CHECK(assert_equal(numericalDF, actualDF));
|
||||
|
|
|
|||
Loading…
Reference in New Issue