Some more refactoring

release/4.3a0
dellaert 2014-10-18 16:45:04 +02:00
parent 66b3081603
commit 9c97b1d8a0
1 changed files with 58 additions and 23 deletions

View File

@ -323,23 +323,39 @@ struct SnavelyReprojectionError {
};
/* ************************************************************************* */
// manifold_traits prototype
// Same style as Boost.TypeTraits
/**
* A manifold defines a space in which there is a notion of a linear tangent space
* that can be centered around a given point on the manifold. These nonlinear
* spaces may have such properties as wrapping around (as is the case with rotations),
* which might make linear operations on parameters not return a viable element of
* the manifold.
*
* We perform optimization by computing a linear delta in the tangent space of the
* current estimate, and then apply this change using a retraction operation, which
* maps the change in tangent space back to the manifold itself.
*
* There may be multiple possible retractions for a given manifold, which can be chosen
* between depending on the computational complexity. The important criteria for
* the creation for the retract and localCoordinates functions is that they be
* inverse operations.
*
*/
// Traits, same style as Boost.TypeTraits
// All meta-functions below ever only declare a single type
// or a type/value/value_type
#include <boost/static_assert.hpp>
#include <type_traits>
// is manifold
// is manifold, by default this is false
template<typename T>
struct is_manifold: public false_type {
struct is_manifold: public std::false_type {
};
// dimension
// dimension, can return Eigen::Dynamic (-1) if not known at compile time
template<typename T>
struct dimension;
struct dimension: public std::integral_constant<int, T::dimension> {
};
// TangentVector is eigen::Matrix type in tangent space
// TangentVector is Eigen::Matrix type in tangent space, can be Dynamic...
template<typename T>
struct TangentVector {
BOOST_STATIC_ASSERT(is_manifold<T>::value);
@ -348,7 +364,7 @@ struct TangentVector {
// default localCoordinates
template<typename T>
struct localCoordinates {
struct LocalCoordinates {
typename TangentVector<T>::type operator()(const T& t1, const T& t2) {
return t1.localCoordinates(t2);
}
@ -356,7 +372,7 @@ struct localCoordinates {
// default retract
template<typename T>
struct retract {
struct Retract {
T operator()(const T& t, const typename TangentVector<T>::type& d) {
return t.retract(d);
}
@ -375,7 +391,7 @@ struct dimension<Eigen::Matrix<double, M, N, Options> > : public integral_consta
};
template<int M, int N, int Options>
struct localCoordinates<Eigen::Matrix<double, M, N, Options> > {
struct LocalCoordinates<Eigen::Matrix<double, M, N, Options> > {
typedef Eigen::Matrix<double, M, N, Options> T;
typedef typename TangentVector<T>::type result_type;
result_type operator()(const T& t1, const T& t2) {
@ -385,7 +401,7 @@ struct localCoordinates<Eigen::Matrix<double, M, N, Options> > {
};
template<int M, int N, int Options>
struct retract<Eigen::Matrix<double, M, N, Options> > {
struct Retract<Eigen::Matrix<double, M, N, Options> > {
typedef Eigen::Matrix<double, M, N, Options> T;
T operator()(const T& t, const typename TangentVector<T>::type& d) {
return t + Eigen::Map<const T>(d.data());
@ -417,8 +433,14 @@ TEST(Expression, dimension) {
// localCoordinates
TEST(Expression, localCoordinates) {
EXPECT(localCoordinates<Point2>()(Point2(0,0),Point2(1,0))==Vector2(1,0));
EXPECT(localCoordinates<Vector2>()(Vector2(0,0),Vector2(1,0))==Vector2(1,0));
EXPECT(LocalCoordinates<Point2>()(Point2(0,0),Point2(1,0))==Vector2(1,0));
EXPECT(LocalCoordinates<Vector2>()(Vector2(0,0),Vector2(1,0))==Vector2(1,0));
}
// retract
TEST(Expression, retract) {
EXPECT(Retract<Point2>()(Point2(0,0),Vector2(1,0))==Point2(1,0));
EXPECT(Retract<Vector2>()(Vector2(0,0),Vector2(1,0))==Vector2(1,0));
}
/* ************************************************************************* */
@ -426,21 +448,34 @@ TEST(Expression, localCoordinates) {
template<typename Y, typename X>
Matrix numericalDerivative(boost::function<Y(const X&)> h, const X& x,
double delta = 1e-5) {
BOOST_STATIC_ASSERT(is_manifold<Y>::value);
BOOST_STATIC_ASSERT(is_manifold<X>::value);
Y hx = h(x);
double factor = 1.0 / (2.0 * delta);
static const size_t M = dimension<Y>::value;
typedef typename TangentVector<Y>::type TangentY;
LocalCoordinates<Y> localCoordinates;
BOOST_STATIC_ASSERT(is_manifold<X>::value);
static const size_t N = dimension<X>::value;
Eigen::Matrix<double, N, 1> d;
typedef typename TangentVector<X>::type TangentX;
Retract<X> retract;
// get value at x
Y hx = h(x);
// Prepare a tangent vector to perturb x with
TangentX d;
d.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++) {
d.setZero();
d(j) = delta;
Vector hxplus = localCoordinates<Y>()(hx, h(retract<X>()(x, d)));
TangentY hxplus = localCoordinates(hx, h(retract(x, d)));
d(j) = -delta;
Vector hxmin = localCoordinates<Y>()(hx, h(retract<X>()(x, d)));
TangentY hxmin = localCoordinates(hx, h(retract(x, d)));
H.block<M, 1>(0, j) << (hxplus - hxmin) * factor;
d(j) = 0;
}
return H;
}