cleaned up a bit; but not really working yet. Trouble with partial specialization of lie_group::expmap<Quaternion<> >()
Mike Bosse
parent
21b97ddedf
commit
376dec5103
|
|
@ -8,10 +8,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
//#include "manifold.h"
|
||||
//#include "chart.h"
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/OptionalJacobian.h>
|
||||
#include <boost/concept_check.hpp>
|
||||
#include <boost/concept/requires.hpp>
|
||||
#include <boost/static_assert.hpp>
|
||||
|
|
@ -64,6 +63,7 @@ template<typename T>
|
|||
BOOST_CONCEPT_REQUIRES(((Testable<T>)),(bool)) //
|
||||
check_invariants(const T& a, const T& b) {
|
||||
typedef typename traits::DefaultChart<T>::type Chart;
|
||||
// no invariants to check for manifolds, so always true if concept check compiles
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -80,32 +80,38 @@ template<TEMPLATE> struct DefaultChart < MANIFOLD > { typedef DEFAULT_CHART type
|
|||
/**
|
||||
* Chart concept
|
||||
*/
|
||||
template<typename T>
|
||||
template<typename C>
|
||||
class IsChart {
|
||||
public:
|
||||
typedef typename T::ManifoldType ManifoldType;
|
||||
typedef typename C::ManifoldType ManifoldType;
|
||||
typedef typename manifold::traits::TangentVector<ManifoldType>::type V;
|
||||
static const int dim = manifold::traits::dimension<ManifoldType>::value;
|
||||
typedef OptionalJacobian<dim, dim> OptionalJacobian;
|
||||
|
||||
BOOST_CONCEPT_USAGE(IsChart) {
|
||||
// make sure Derived methods in Chart are defined
|
||||
v = T::Local(p,q);
|
||||
q = T::Retract(p,v);
|
||||
// make sure methods in Chart are defined
|
||||
v = C::Local(p,q);
|
||||
q = C::Retract(p,v);
|
||||
// and the versions with Jacobians.
|
||||
v = C::Local(p,q,Hp,Hq);
|
||||
q = C::Retract(p,v,Hp,Hv);
|
||||
}
|
||||
private:
|
||||
ManifoldType p,q;
|
||||
OptionalJacobian Hp,Hq,Hv;
|
||||
V v;
|
||||
};
|
||||
|
||||
/**
|
||||
* Manifold concept
|
||||
*/
|
||||
template<typename T>
|
||||
template<typename M>
|
||||
class IsManifold {
|
||||
public:
|
||||
typedef typename traits::structure_category<T>::type structure_category_tag;
|
||||
static const size_t dim = manifold::traits::dimension<T>::value;
|
||||
typedef typename manifold::traits::TangentVector<T>::type TangentVector;
|
||||
typedef typename manifold::traits::DefaultChart<T>::type DefaultChart;
|
||||
typedef typename traits::structure_category<M>::type structure_category_tag;
|
||||
static const size_t dim = manifold::traits::dimension<M>::value;
|
||||
typedef typename manifold::traits::TangentVector<M>::type TangentVector;
|
||||
typedef typename manifold::traits::DefaultChart<M>::type DefaultChart;
|
||||
|
||||
BOOST_CONCEPT_USAGE(IsManifold) {
|
||||
BOOST_STATIC_ASSERT_MSG(
|
||||
|
|
@ -114,43 +120,39 @@ public:
|
|||
BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim);
|
||||
BOOST_CONCEPT_ASSERT((IsChart<DefaultChart >));
|
||||
}
|
||||
private:
|
||||
T p,q;
|
||||
TangentVector v;
|
||||
};
|
||||
|
||||
namespace group {
|
||||
|
||||
/** @name Free functions any Group needs to define */
|
||||
//@{
|
||||
template<typename T> T compose(const T&g, const T& h);
|
||||
template<typename T> T between(const T&g, const T& h);
|
||||
template<typename T> T inverse(const T&g);
|
||||
template<typename G> G compose(const G& g, const G& h);
|
||||
template<typename G> G between(const G& g, const G& h);
|
||||
template<typename G> G inverse(const G& g);
|
||||
template<typename G, typename S> S act(const G& g, const S& s);
|
||||
//@}
|
||||
|
||||
namespace traits {
|
||||
|
||||
/** @name Group Traits */
|
||||
//@{
|
||||
template<typename T> struct identity;
|
||||
template<typename T> struct flavor;
|
||||
template<typename G> struct identity;
|
||||
template<typename G> struct flavor;
|
||||
//@}
|
||||
|
||||
/** @name Group Flavor Tags */
|
||||
//@{
|
||||
struct additive_tag {
|
||||
};
|
||||
struct multiplicative_tag {
|
||||
};
|
||||
struct additive_tag {};
|
||||
struct multiplicative_tag {};
|
||||
//@}
|
||||
|
||||
}// \ namespace traits
|
||||
|
||||
/// Check invariants
|
||||
template<typename T>
|
||||
BOOST_CONCEPT_REQUIRES(((Testable<T>)),(bool)) //
|
||||
check_invariants(const T& a, const T& b, double tol = 1e-9) {
|
||||
T e = traits::identity<T>::value;
|
||||
template<typename G>
|
||||
BOOST_CONCEPT_REQUIRES(((Testable<G>)),(bool)) //
|
||||
check_invariants(const G& a, const G& b, double tol = 1e-9) {
|
||||
G e = traits::identity<G>::value;
|
||||
return compose(a, inverse(a)).equals(e, tol)
|
||||
&& between(a, b).equals(compose(inverse(a), b), tol)
|
||||
&& compose(a, between(a, b)).equals(b, tol);
|
||||
|
|
@ -190,47 +192,51 @@ template<TEMPLATE> struct flavor<GROUP > { typedef multiplicative_tag type;};\
|
|||
/**
|
||||
* Group Concept
|
||||
*/
|
||||
template<typename T>
|
||||
template<typename G>
|
||||
class IsGroup {
|
||||
public:
|
||||
|
||||
typedef typename traits::structure_category<T>::type structure_category_tag;
|
||||
typedef typename group::traits::identity<T>::value_type identity_value_type;
|
||||
typedef typename group::traits::flavor<T>::type flavor_tag;
|
||||
|
||||
void operator_usage(group::traits::multiplicative_tag) {
|
||||
g = g * h;
|
||||
}
|
||||
void operator_usage(group::traits::additive_tag) {
|
||||
g = g + h;
|
||||
g = h - g;
|
||||
g = -g;
|
||||
}
|
||||
typedef typename traits::structure_category<G>::type structure_category_tag;
|
||||
typedef typename group::traits::identity<G>::value_type identity_value_type;
|
||||
typedef typename group::traits::flavor<G>::type flavor_tag;
|
||||
|
||||
BOOST_CONCEPT_USAGE(IsGroup) {
|
||||
BOOST_STATIC_ASSERT_MSG(
|
||||
(boost::is_base_of<traits::group_tag, structure_category_tag>::value),
|
||||
"This type's structure_category trait does not assert it as a group (or derived)");
|
||||
e = group::traits::identity<T>::value;
|
||||
g = group::compose(g, h);
|
||||
g = group::between(g, h);
|
||||
g = group::inverse(g);
|
||||
e = group::traits::identity<G>::value;
|
||||
e = group::compose(g, h);
|
||||
e = group::between(g, h);
|
||||
e = group::inverse(g);
|
||||
operator_usage(flavor);
|
||||
// todo: how do we test the act concept? or do we even need to?
|
||||
}
|
||||
|
||||
private:
|
||||
void operator_usage(group::traits::multiplicative_tag) {
|
||||
e = g * h;
|
||||
//e = -g; // todo this should work, but it is failing for Quaternions
|
||||
}
|
||||
void operator_usage(group::traits::additive_tag) {
|
||||
e = g + h;
|
||||
e = h - g;
|
||||
e = -g;
|
||||
}
|
||||
|
||||
flavor_tag flavor;
|
||||
T e, g, h;
|
||||
G e, g, h;
|
||||
};
|
||||
|
||||
namespace lie_group {
|
||||
|
||||
/** @name Free functions any Group needs to define */
|
||||
/** @name Free functions any Lie Group needs to define */
|
||||
//@{
|
||||
// TODO need Jacobians
|
||||
//template<typename T> T compose(const T&g, const T& h);
|
||||
//template<typename T> T between(const T&g, const T& h);
|
||||
//template<typename T> T inverse(const T&g);
|
||||
template<typename LG, int dim> LG compose(const LG& g, const LG& h, OptionalJacobian<dim, dim> Hg, OptionalJacobian<dim, dim> Hh);
|
||||
template<typename LG, int dim> LG between(const LG& g, const LG& h, OptionalJacobian<dim, dim> Hg, OptionalJacobian<dim, dim> Hh);
|
||||
template<typename LG> LG inverse(const LG& g, OptionalJacobian<manifold::traits::dimension<LG>::value, manifold::traits::dimension<LG>::value > Hg);
|
||||
template<typename LG> typename manifold::traits::TangentVector<LG>::type logmap(const LG & g);
|
||||
//template<typename LG> LG expmap(const typename manifold::traits::TangentVector<LG>::type& v);
|
||||
template<typename LG> LG expmap(const Eigen::Ref<const typename manifold::traits::TangentVector<LG>::type>& v);
|
||||
//@}
|
||||
|
||||
namespace traits {
|
||||
|
|
@ -242,12 +248,12 @@ namespace traits {
|
|||
}// \ namespace traits
|
||||
|
||||
/// Check invariants
|
||||
//template<typename T>
|
||||
//BOOST_CONCEPT_REQUIRES(((Testable<T>)),(bool)) check_invariants(const T& a,
|
||||
// const T& b) {
|
||||
// bool check_invariants(const V& a, const V& b) {
|
||||
// return equal(Chart::retract(a, b), a + b)
|
||||
// && equal(Chart::local(a, b), b - a);
|
||||
//template<typename LG>
|
||||
//BOOST_CONCEPT_REQUIRES(((Testable<LG>)),(bool)) check_invariants(const LG& a,
|
||||
// const LG& b) {
|
||||
// bool check_invariants(const LG& a, const LG& b) {
|
||||
// return equal(Chart::Retract(a, b), a + b)
|
||||
// && equal(Chart::Local(a, b), b - a);
|
||||
// }
|
||||
//}
|
||||
}// \ namespace lie_group
|
||||
|
|
@ -255,57 +261,64 @@ namespace traits {
|
|||
/**
|
||||
* Lie Group Concept
|
||||
*/
|
||||
template<typename T>
|
||||
class IsLieGroup: public IsGroup<T>, public IsManifold<T> {
|
||||
template<typename LG>
|
||||
class IsLieGroup: public IsGroup<LG>, public IsManifold<LG> {
|
||||
public:
|
||||
|
||||
typedef typename traits::structure_category<T>::type structure_category_tag;
|
||||
|
||||
typedef typename traits::structure_category<LG>::type structure_category_tag;
|
||||
typedef OptionalJacobian<IsManifold<LG>::dim, IsManifold<LG>::dim> OptionalJacobian;
|
||||
typedef typename manifold::traits::TangentVector<LG>::type V;
|
||||
BOOST_CONCEPT_USAGE(IsLieGroup) {
|
||||
BOOST_STATIC_ASSERT_MSG(
|
||||
(boost::is_base_of<traits::lie_group_tag, structure_category_tag>::value),
|
||||
"This type's trait does not assert it as a Lie group (or derived)");
|
||||
"This type's trait does not assert it is a Lie group (or derived)");
|
||||
// TODO Check with Jacobian
|
||||
// using lie_group::compose;
|
||||
// using lie_group::between;
|
||||
// using lie_group::inverse;
|
||||
// g = compose(g, h);
|
||||
// g = between(g, h);
|
||||
// g = inverse(g);
|
||||
using lie_group::compose;
|
||||
using lie_group::between;
|
||||
using lie_group::inverse;
|
||||
g = compose(g, h, Hg, Hh);
|
||||
g = between(g, h, Hg, Hh);
|
||||
g = inverse(g, Hg);
|
||||
g = lie_group::expmap<LG>(v);
|
||||
v = lie_group::logmap<LG>(g);
|
||||
}
|
||||
private:
|
||||
|
||||
T g, h;
|
||||
LG g, h;
|
||||
V v;
|
||||
OptionalJacobian Hg, Hh;
|
||||
};
|
||||
|
||||
/**
|
||||
* A Lie Group Chart
|
||||
* Creates Local/Retract from exponential map and its inverse
|
||||
* Assumes Expmap and Logmap defined in Derived
|
||||
* TODO: Can we do this with a single Derived argument ?
|
||||
*/
|
||||
template<typename Derived, typename T, typename TangentVector>
|
||||
template<typename M>
|
||||
struct LieGroupChart {
|
||||
typedef M ManifoldType;
|
||||
typedef typename manifold::traits::TangentVector<ManifoldType>::type TangentVector;
|
||||
static const int dim = manifold::traits::dimension<ManifoldType>::value;
|
||||
typedef OptionalJacobian<dim, dim> OptionalJacobian;
|
||||
|
||||
/// retract, composes with Exmpap around identity
|
||||
static T Retract(const T& p, const TangentVector& omega) {
|
||||
// TODO needs to be manifold::compose, with derivatives
|
||||
return group::compose(p, Derived::Expmap(omega));
|
||||
/// retract, composes with Expmap around identity
|
||||
static ManifoldType Retract(const ManifoldType& p, const TangentVector& omega, OptionalJacobian Hp=boost::none, OptionalJacobian Hw=boost::none) {
|
||||
// todo: use the chain rule with Jacobian of Expmap
|
||||
return lie_group::compose(p, lie_group::expmap<ManifoldType>(omega), Hp, Hw);
|
||||
}
|
||||
|
||||
/// local is our own, as there is a slight bug in Eigen
|
||||
static TangentVector Local(const T& q1, const T& q2) {
|
||||
// TODO needs to be manifold::between, with derivatives
|
||||
return Derived::Logmap(group::between(q1, q2));
|
||||
static TangentVector Local(const ManifoldType& p, const ManifoldType& q, OptionalJacobian Hp=boost::none, OptionalJacobian Hq=boost::none) {
|
||||
// todo: use the chain rule with Jacobian of Logmap
|
||||
return lie_group::logmap<ManifoldType>(lie_group::between(p, q, Hp, Hq));
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
class IsVectorSpace: public IsLieGroup<T> {
|
||||
template<typename V>
|
||||
class IsVectorSpace: public IsLieGroup<V> {
|
||||
public:
|
||||
|
||||
typedef typename traits::structure_category<T>::type structure_category_tag;
|
||||
typedef typename traits::structure_category<V>::type structure_category_tag;
|
||||
|
||||
BOOST_CONCEPT_USAGE(IsVectorSpace) {
|
||||
BOOST_STATIC_ASSERT_MSG(
|
||||
|
|
@ -317,7 +330,7 @@ public:
|
|||
}
|
||||
|
||||
private:
|
||||
T p, q, r;
|
||||
V p, q, r;
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -17,75 +17,21 @@
|
|||
|
||||
#include <gtsam/base/concepts.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// Chart for Eigen Quaternions
|
||||
template<typename _Scalar, int _Options>
|
||||
struct MakeQuaternionChart: LieGroupChart<
|
||||
MakeQuaternionChart<_Scalar, _Options>,
|
||||
Eigen::Quaternion<_Scalar, _Options>,
|
||||
Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> > {
|
||||
|
||||
// required
|
||||
typedef Eigen::Quaternion<_Scalar, _Options> ManifoldType;
|
||||
|
||||
// internal
|
||||
typedef ManifoldType Q;
|
||||
typedef typename manifold::traits::TangentVector<Q>::type Omega;
|
||||
|
||||
/// Exponential map given axis/angle representation of Lie algebra
|
||||
/// TODO: obsolete? But see if not called now in Rot3Q
|
||||
static Q Expmap(const _Scalar& angle, const Eigen::Ref<const Omega>& axis) {
|
||||
return Q(Eigen::AngleAxis<_Scalar>(angle, axis));
|
||||
}
|
||||
|
||||
/// Exponential map, simply be converting omega to axis/angle representation
|
||||
static Q Expmap(const Eigen::Ref<const Omega>& omega) {
|
||||
if (omega.isZero())
|
||||
return Q::Identity();
|
||||
else {
|
||||
_Scalar angle = omega.norm();
|
||||
return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle));
|
||||
}
|
||||
}
|
||||
|
||||
/// We use our own Logmap, as there is a slight bug in Eigen
|
||||
static Omega Logmap(const Q& q) {
|
||||
using std::acos;
|
||||
using std::sqrt;
|
||||
static const double twoPi = 2.0 * M_PI,
|
||||
// define these compile time constants to avoid std::abs:
|
||||
NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10;
|
||||
|
||||
const double qw = q.w();
|
||||
if (qw > NearlyOne) {
|
||||
// Taylor expansion of (angle / s) at 1
|
||||
return (2 - 2 * (qw - 1) / 3) * q.vec();
|
||||
} else if (qw < NearlyNegativeOne) {
|
||||
// Angle is zero, return zero vector
|
||||
return Omega::Zero();
|
||||
} else {
|
||||
// Normal, away from zero case
|
||||
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
|
||||
// Important: convert to [-pi,pi] to keep error continuous
|
||||
if (angle > M_PI)
|
||||
angle -= twoPi;
|
||||
else if (angle < -M_PI)
|
||||
angle += twoPi;
|
||||
return (angle / s) * q.vec();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// Define group traits
|
||||
#define QUATERNION_TEMPLATE typename _Scalar, int _Options
|
||||
#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
|
||||
|
||||
template<QUATERNION_TEMPLATE>
|
||||
QUATERNION_TYPE operator-(const QUATERNION_TYPE &q) { return gtsam::group::inverse(q); }
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Define group traits
|
||||
GTSAM_GROUP_IDENTITY(QUATERNION_TEMPLATE, QUATERNION_TYPE)
|
||||
GTSAM_MULTIPLICATIVE_GROUP(QUATERNION_TEMPLATE, QUATERNION_TYPE)
|
||||
|
||||
// Define manifold traits
|
||||
#define QUATERNION_TANGENT Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1>
|
||||
#define QUATERNION_CHART MakeQuaternionChart<_Scalar,_Options>
|
||||
#define QUATERNION_CHART LieGroupChart<typename QUATERNION_TYPE>
|
||||
GTSAM_MANIFOLD(QUATERNION_TEMPLATE, QUATERNION_TYPE, 3, QUATERNION_TANGENT,
|
||||
QUATERNION_CHART)
|
||||
|
||||
|
|
@ -97,13 +43,58 @@ struct structure_category<Eigen::Quaternion<_Scalar,_Options> > {
|
|||
};
|
||||
}
|
||||
|
||||
/// lie_group for Eigen Quaternions
|
||||
namespace lie_group {
|
||||
|
||||
/// Exponential map, simply be converting omega to axis/angle representation
|
||||
template <typename _Scalar, int _Options>
|
||||
static QUATERNION_TYPE expmap<QUATERNION_TYPE>(const Eigen::Ref<const typename QUATERNION_TANGENT >& omega) {
|
||||
if (omega.isZero())
|
||||
return QUATERNION_TYPE::Identity();
|
||||
else {
|
||||
_Scalar angle = omega.norm();
|
||||
return QUATERNION_TYPE(Eigen::AngleAxis<_Scalar>(angle, omega / angle));
|
||||
}
|
||||
}
|
||||
|
||||
/// We use our own Logmap, as there is a slight bug in Eigen
|
||||
template <QUATERNION_TEMPLATE>
|
||||
static QUATERNION_TANGENT logmap<QUATERNION_TYPE>(const QUATERNION_TYPE& q) {
|
||||
using std::acos;
|
||||
using std::sqrt;
|
||||
static const double twoPi = 2.0 * M_PI,
|
||||
// define these compile time constants to avoid std::abs:
|
||||
NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10;
|
||||
|
||||
const double qw = q.w();
|
||||
if (qw > NearlyOne) {
|
||||
// Taylor expansion of (angle / s) at 1
|
||||
//return (2 + 2 * (1-qw) / 3) * q.vec();
|
||||
return (8./3. - 2./3. * qw) * q.vec();
|
||||
} else if (qw < NearlyNegativeOne) {
|
||||
// Taylor expansion of (angle / s) at -1
|
||||
//return (-2 - 2 * (1 + qw) / 3) * q.vec();
|
||||
return (-8./3 + 2./3 * qw) * q.vec();
|
||||
} else {
|
||||
// Normal, away from zero case
|
||||
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
|
||||
// Important: convert to [-pi,pi] to keep error continuous
|
||||
if (angle > M_PI)
|
||||
angle -= twoPi;
|
||||
else if (angle < -M_PI)
|
||||
angle += twoPi;
|
||||
return (angle / s) * q.vec();
|
||||
}
|
||||
}
|
||||
} // end namespace lie_group
|
||||
|
||||
/**
|
||||
* GSAM typedef to an Eigen::Quaternion<double>, we disable alignment because
|
||||
* geometry objects are stored in boost pool allocators, in Values containers,
|
||||
* and and these pool allocators do not support alignment.
|
||||
*/
|
||||
typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;
|
||||
typedef MakeQuaternionChart<double, Eigen::DontAlign> QuaternionChart;
|
||||
typedef LieGroupChart<Quaternion> QuaternionChart;
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -44,8 +44,11 @@ SO3 Rodrigues(const double& theta, const Vector3& axis) {
|
|||
return R;
|
||||
}
|
||||
|
||||
|
||||
namespace lie_group {
|
||||
/// simply convert omega to axis/angle representation
|
||||
SO3 SO3Chart::Expmap(const Eigen::Ref<const Vector3>& omega) {
|
||||
template <>
|
||||
SO3 expmap<SO3>(const Eigen::Ref<const Vector3>& omega) {
|
||||
if (omega.isZero())
|
||||
return SO3::Identity();
|
||||
else {
|
||||
|
|
@ -54,7 +57,8 @@ SO3 SO3Chart::Expmap(const Eigen::Ref<const Vector3>& omega) {
|
|||
}
|
||||
}
|
||||
|
||||
Vector3 SO3Chart::Logmap(const SO3& R) {
|
||||
template <>
|
||||
Vector3 logmap<SO3>(const SO3& R) {
|
||||
|
||||
// note switch to base 1
|
||||
const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
|
||||
|
|
@ -88,6 +92,6 @@ Vector3 SO3Chart::Logmap(const SO3& R) {
|
|||
return magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
|
||||
}
|
||||
}
|
||||
|
||||
} // end namespace lie_group
|
||||
} // end namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -48,24 +48,15 @@ public:
|
|||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Chart for SO3 comprising of exponential map and its inverse (log-map)
|
||||
*/
|
||||
struct SO3Chart: LieGroupChart<SO3Chart,SO3,Vector3> {
|
||||
|
||||
typedef SO3 ManifoldType;
|
||||
|
||||
/// Exponential map
|
||||
static SO3 Expmap(const Eigen::Ref<const Vector3>& omega);
|
||||
|
||||
/// We use our own Logmap, as there is a slight bug in Eigen
|
||||
static Vector3 Logmap(const SO3& R);
|
||||
};
|
||||
|
||||
#define SO3_TEMPLATE
|
||||
GTSAM_GROUP_IDENTITY0(SO3)
|
||||
GTSAM_MULTIPLICATIVE_GROUP(SO3_TEMPLATE, SO3)
|
||||
|
||||
/**
|
||||
* Chart for SO3 comprising of exponential map and its inverse (log-map)
|
||||
*/
|
||||
typedef LieGroupChart<SO3> SO3Chart;
|
||||
|
||||
#define SO3_TANGENT Vector3
|
||||
#define SO3_CHART SO3Chart
|
||||
GTSAM_MANIFOLD(SO3_TEMPLATE, SO3, 3, SO3_TANGENT, SO3_CHART)
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
|
||||
typedef Quaternion Q; // Typedef
|
||||
typedef OptionalJacobian<manifold::traits::dimension<Q>::value, manifold::traits::dimension<Q>::value> QuaternionJacobian;
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , Concept) {
|
||||
|
|
@ -48,8 +49,9 @@ TEST(Quaternion , Local) {
|
|||
Q q1(Eigen::AngleAxisd(0, z_axis));
|
||||
Q q2(Eigen::AngleAxisd(0.1, z_axis));
|
||||
typedef manifold::traits::DefaultChart<Q>::type Chart;
|
||||
QuaternionJacobian H1,H2;
|
||||
Vector3 expected(0, 0, 0.1);
|
||||
Vector3 actual = Chart::Local(q1, q2);
|
||||
Vector3 actual = Chart::Local(q1, q2, H1, H2);
|
||||
EXPECT(assert_equal((Vector)expected,actual));
|
||||
}
|
||||
|
||||
|
|
@ -60,20 +62,24 @@ TEST(Quaternion , Retract) {
|
|||
Q expected(Eigen::AngleAxisd(0.1, z_axis));
|
||||
typedef manifold::traits::DefaultChart<Q>::type Chart;
|
||||
Vector3 v(0, 0, 0.1);
|
||||
Q actual = Chart::Retract(q, v);
|
||||
QuaternionJacobian Hq,Hv;
|
||||
Q actual = Chart::Retract(q, v, Hq, Hv);
|
||||
EXPECT(actual.isApprox(expected));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , Compose) {
|
||||
EXPECT(false);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , Between) {
|
||||
EXPECT(false);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , Inverse) {
|
||||
EXPECT(false);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
|||
|
|
@ -21,6 +21,8 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
typedef OptionalJacobian<3,3> SO3Jacobian;
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3 , Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<SO3 >));
|
||||
|
|
@ -46,8 +48,9 @@ TEST(SO3 , Local) {
|
|||
SO3 q1(Eigen::AngleAxisd(0, z_axis));
|
||||
SO3 q2(Eigen::AngleAxisd(0.1, z_axis));
|
||||
typedef manifold::traits::DefaultChart<SO3>::type Chart;
|
||||
SO3Jacobian H1,H2;
|
||||
Vector3 expected(0, 0, 0.1);
|
||||
Vector3 actual = Chart::Local(q1, q2);
|
||||
Vector3 actual = Chart::Local(q1, q2, H1, H2);
|
||||
EXPECT(assert_equal((Vector)expected,actual));
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue