Some more refactoring
parent
66b3081603
commit
9c97b1d8a0
|
@ -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;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue