The great Lie group heist: LieGroup CRTP class that defines both Manifold and Lie Group methods for a class, using AdjointMap as its main weapon. For the retract/localCoordinates, a ChartAtOrigin struct needs to be defined that can be switched at compile time (example Pose2). Rot3, Pose3 soon to follow.
dellaert
parent
6ddd37a0e2
commit
f22c922600
|
|
@ -24,6 +24,67 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/// A CRTP helper class that implements Lie group methods
|
||||
/// Prerequisites: methods operator*, inverse, and AdjointMap, as well as a
|
||||
/// ChartAtOrigin struct that will be used to define the manifold Chart
|
||||
/// To use, simply derive, but also say "using LieGroup<Class,N>::inverse"
|
||||
/// For derivative math, see doc/math.pdf
|
||||
template <class Class, int N>
|
||||
struct LieGroup {
|
||||
|
||||
BOOST_STATIC_ASSERT_MSG(N != Eigen::Dynamic,
|
||||
"LieGroup not yet specialized for dynamically sized types.");
|
||||
|
||||
enum { dimension = N };
|
||||
typedef OptionalJacobian<N, N> ChartJacobian;
|
||||
typedef Eigen::Matrix<double, N, 1> TangentVector;
|
||||
|
||||
const Class & derived() const {
|
||||
return static_cast<const Class&>(*this);
|
||||
}
|
||||
|
||||
Class compose(const Class& g) const {
|
||||
return derived() * g;
|
||||
}
|
||||
|
||||
Class between(const Class& g) const {
|
||||
return derived().inverse() * g;
|
||||
}
|
||||
|
||||
Class compose(const Class& g, ChartJacobian H1,
|
||||
ChartJacobian H2 = boost::none) const {
|
||||
if (H1) *H1 = g.inverse().AdjointMap();
|
||||
if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
|
||||
return derived() * g;
|
||||
}
|
||||
|
||||
Class between(const Class& g, ChartJacobian H1,
|
||||
ChartJacobian H2 = boost::none) const {
|
||||
Class result = derived().inverse() * g;
|
||||
if (H1) *H1 = - result.inverse().AdjointMap();
|
||||
if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
|
||||
return result;
|
||||
}
|
||||
|
||||
Class inverse(ChartJacobian H) const {
|
||||
if (H) *H = - derived().AdjointMap();
|
||||
return derived().inverse();
|
||||
}
|
||||
|
||||
Class retract(const TangentVector& v, //
|
||||
ChartJacobian H1 = boost::none, //
|
||||
ChartJacobian H2 = boost::none) const {
|
||||
return compose(Class::ChartAtOrigin::Retract(v),H1,H2);
|
||||
}
|
||||
|
||||
TangentVector localCoordinates(const Class& g, //
|
||||
ChartJacobian H1 = boost::none, //
|
||||
ChartJacobian H2 = boost::none) const {
|
||||
return Class::ChartAtOrigin::Local(between(g,H1,H2));
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/// tag to assert a type is a Lie group
|
||||
struct lie_group_tag: public manifold_tag, public group_tag {};
|
||||
|
||||
|
|
|
|||
|
|
@ -92,25 +92,20 @@ Vector3 Pose2::Logmap(const Pose2& p, OptionalJacobian<3, 3> H) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose2 Pose2::retract(const Vector& v, OptionalJacobian<3, 3> Hthis,
|
||||
OptionalJacobian<3, 3> Hv) const {
|
||||
if (Hthis || Hv) CONCEPT_NOT_IMPLEMENTED;
|
||||
Pose2 Pose2::ChartAtOrigin::Retract(const Vector3& v, ChartJacobian H) {
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
return compose(Expmap(v));
|
||||
return Expmap(v, H);
|
||||
#else
|
||||
assert(v.size() == 3);
|
||||
return compose(Pose2(v[0], v[1], v[2]));
|
||||
if (H) *H = Matrix3::Identity();
|
||||
return Pose2(v[0], v[1], v[2]);
|
||||
#endif
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose2::localCoordinates(const Pose2& p2, OptionalJacobian<3, 3> Hthis,
|
||||
OptionalJacobian<3, 3> Hother) const {
|
||||
if (Hthis || Hother) CONCEPT_NOT_IMPLEMENTED;
|
||||
Vector3 Pose2::ChartAtOrigin::Local(const Pose2& r, ChartJacobian H) {
|
||||
#ifdef SLOW_BUT_CORRECT_EXPMAP
|
||||
return Logmap(between(p2));
|
||||
return Logmap(r, H);
|
||||
#else
|
||||
Pose2 r = between(p2);
|
||||
if (H) *H = Matrix3::Identity();
|
||||
return Vector3(r.x(), r.y(), r.theta());
|
||||
#endif
|
||||
}
|
||||
|
|
@ -189,8 +184,7 @@ Matrix3 Pose2::dexpInvL(const Vector3& v) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose2 Pose2::inverse(OptionalJacobian<3,3> H1) const {
|
||||
if (H1) *H1 = -AdjointMap();
|
||||
Pose2 Pose2::inverse() const {
|
||||
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
|
||||
}
|
||||
|
||||
|
|
@ -208,16 +202,6 @@ Point2 Pose2::transform_to(const Point2& point,
|
|||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SE(2) section
|
||||
Pose2 Pose2::compose(const Pose2& p2, OptionalJacobian<3,3> H1,
|
||||
OptionalJacobian<3,3> H2) const {
|
||||
// TODO: inline and reuse?
|
||||
if(H1) *H1 = p2.inverse().AdjointMap();
|
||||
if(H2) *H2 = I_3x3;
|
||||
return (*this)*p2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SE(2) section
|
||||
Point2 Pose2::transform_from(const Point2& p,
|
||||
|
|
@ -233,40 +217,6 @@ Point2 Pose2::transform_from(const Point2& p,
|
|||
return q + t_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1,
|
||||
OptionalJacobian<3,3> H2) const {
|
||||
// get cosines and sines from rotation matrices
|
||||
const Rot2& R1 = r_, R2 = p2.r();
|
||||
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
|
||||
|
||||
// Assert that R1 and R2 are normalized
|
||||
assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
|
||||
|
||||
// Calculate delta rotation = between(R1,R2)
|
||||
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
|
||||
Rot2 R(Rot2::atan2(s,c)); // normalizes
|
||||
|
||||
// Calculate delta translation = unrotate(R1, dt);
|
||||
Point2 dt = p2.t() - t_;
|
||||
double x = dt.x(), y = dt.y();
|
||||
// t = R1' * (t2-t1)
|
||||
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
|
||||
|
||||
// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
|
||||
if (H1) {
|
||||
double dt1 = -s2 * x + c2 * y;
|
||||
double dt2 = -c2 * x - s2 * y;
|
||||
*H1 <<
|
||||
-c, -s, dt1,
|
||||
s, -c, dt2,
|
||||
0.0, 0.0,-1.0;
|
||||
}
|
||||
if (H2) *H2 = I_3x3;
|
||||
|
||||
return Pose2(R,t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot2 Pose2::bearing(const Point2& point,
|
||||
OptionalJacobian<1, 3> H1, OptionalJacobian<1, 2> H2) const {
|
||||
|
|
|
|||
|
|
@ -31,7 +31,7 @@ namespace gtsam {
|
|||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Pose2 {
|
||||
class GTSAM_EXPORT Pose2: public LieGroup<Pose2, 3> {
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -40,13 +40,12 @@ public:
|
|||
typedef Point2 Translation;
|
||||
|
||||
private:
|
||||
|
||||
Rot2 r_;
|
||||
Point2 t_;
|
||||
|
||||
public:
|
||||
|
||||
enum { dimension = 3 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
|
|
@ -106,52 +105,23 @@ public:
|
|||
/// identity for group operation
|
||||
inline static Pose2 identity() { return Pose2(); }
|
||||
|
||||
/// inverse transformation with derivatives
|
||||
Pose2 inverse(OptionalJacobian<3, 3> H1=boost::none) const;
|
||||
|
||||
/// compose this transformation onto another (first *this and then p2)
|
||||
Pose2 compose(const Pose2& p2,
|
||||
OptionalJacobian<3, 3> H1 = boost::none,
|
||||
OptionalJacobian<3, 3> H2 = boost::none) const;
|
||||
/// inverse
|
||||
Pose2 inverse() const;
|
||||
|
||||
/// compose syntactic sugar
|
||||
inline Pose2 operator*(const Pose2& p2) const {
|
||||
return Pose2(r_*p2.r(), t_ + r_*p2.t());
|
||||
}
|
||||
|
||||
/**
|
||||
* Return relative pose between p1 and p2, in p1 coordinate frame
|
||||
*/
|
||||
Pose2 between(const Pose2& p2, OptionalJacobian<3,3> H1 = boost::none,
|
||||
OptionalJacobian<3,3> H = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// Dimensionality of tangent space = 3 DOF - used to autodetect sizes
|
||||
inline static size_t Dim() { return 3; }
|
||||
|
||||
/// Dimensionality of tangent space = 3 DOF
|
||||
inline size_t dim() const { return 3; }
|
||||
|
||||
/// Retraction from R^3 \f$ [T_x,T_y,\theta] \f$ to Pose2 manifold neighborhood around current pose
|
||||
Pose2 retract(const Vector& v, OptionalJacobian<3, 3> Hthis =
|
||||
boost::none, OptionalJacobian<3, 3> Hv = boost::none) const;
|
||||
|
||||
/// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose
|
||||
Vector localCoordinates(const Pose2& p2, OptionalJacobian<3, 3> Hthis =
|
||||
boost::none, OptionalJacobian<3, 3> Hother = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
|
||||
///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$
|
||||
static Pose2 Expmap(const Vector& xi, OptionalJacobian<3, 3> H = boost::none);
|
||||
static Pose2 Expmap(const Vector& xi, ChartJacobian H = boost::none);
|
||||
|
||||
///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
|
||||
static Vector3 Logmap(const Pose2& p, OptionalJacobian<3, 3> H = boost::none);
|
||||
static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
|
||||
|
||||
/**
|
||||
* Calculate Adjoint map
|
||||
|
|
@ -189,6 +159,13 @@ public:
|
|||
/// Left-trivialized derivative inverse of the exponential map
|
||||
static Matrix3 dexpInvL(const Vector3& v);
|
||||
|
||||
// Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
|
||||
struct ChartAtOrigin {
|
||||
static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none);
|
||||
static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none);
|
||||
};
|
||||
|
||||
using LieGroup<Pose2, 3>::inverse; // version with derivative
|
||||
|
||||
/// @}
|
||||
/// @name Group Action on Point2
|
||||
|
|
|
|||
|
|
@ -60,47 +60,7 @@ Rot2& Rot2::normalize() {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot2 Rot2::compose(const Rot2& R, OptionalJacobian<1, 1> H1,
|
||||
OptionalJacobian<1, 1> H2) const {
|
||||
if (H1)
|
||||
*H1 = I_1x1;
|
||||
if (H2)
|
||||
*H2 = I_1x1;
|
||||
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot2 Rot2::between(const Rot2& R, OptionalJacobian<1, 1> H1,
|
||||
OptionalJacobian<1, 1> H2) const {
|
||||
if (H1)
|
||||
*H1 = -I_1x1;
|
||||
if (H2)
|
||||
*H2 = I_1x1;
|
||||
return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot2 Rot2::retract(const Vector& v, OptionalJacobian<1, 1> H1,
|
||||
OptionalJacobian<1, 1> H2) const {
|
||||
if (H1)
|
||||
*H1 = I_1x1;
|
||||
if (H2)
|
||||
*H2 = I_1x1;
|
||||
return *this * Expmap(v);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector1 Rot2::localCoordinates(const Rot2& t2, OptionalJacobian<1, 1> H1,
|
||||
OptionalJacobian<1, 1> H2) const {
|
||||
if (H1)
|
||||
*H1 = -I_1x1;
|
||||
if (H2)
|
||||
*H2 = I_1x1;
|
||||
return Logmap(between(t2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot2 Rot2::Expmap(const Vector& v, OptionalJacobian<1, 1> H) {
|
||||
Rot2 Rot2::Expmap(const Vector1& v, OptionalJacobian<1, 1> H) {
|
||||
if (H)
|
||||
*H = I_1x1;
|
||||
if (zero(v))
|
||||
|
|
|
|||
|
|
@ -30,25 +30,16 @@ namespace gtsam {
|
|||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Rot2 {
|
||||
|
||||
public:
|
||||
/** get the dimension by the type */
|
||||
static const size_t dimension = 1;
|
||||
|
||||
private:
|
||||
class GTSAM_EXPORT Rot2 : public LieGroup<Rot2, 1> {
|
||||
|
||||
/** we store cos(theta) and sin(theta) */
|
||||
double c_, s_;
|
||||
|
||||
|
||||
/** normalize to make sure cos and sin form unit vector */
|
||||
Rot2& normalize();
|
||||
|
||||
/** private constructor from cos/sin */
|
||||
inline Rot2(double c, double s) :
|
||||
c_(c), s_(s) {
|
||||
}
|
||||
inline Rot2(double c, double s) : c_(c), s_(s) {}
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -56,14 +47,10 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/** default constructor, zero rotation */
|
||||
Rot2() :
|
||||
c_(1.0), s_(0.0) {
|
||||
}
|
||||
Rot2() : c_(1.0), s_(0.0) {}
|
||||
|
||||
/// Constructor from angle in radians == exponential map at identity
|
||||
Rot2(double theta) :
|
||||
c_(cos(theta)), s_(sin(theta)) {
|
||||
}
|
||||
Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
|
||||
|
||||
/// Named constructor from angle in radians
|
||||
static Rot2 fromAngle(double theta) {
|
||||
|
|
@ -110,59 +97,26 @@ namespace gtsam {
|
|||
inline static Rot2 identity() { return Rot2(); }
|
||||
|
||||
/** The inverse rotation - negative angle */
|
||||
Rot2 inverse(OptionalJacobian<1,1> H = boost::none) const {
|
||||
if (H) *H = -I_1x1;
|
||||
return Rot2(c_, -s_);
|
||||
}
|
||||
Rot2 inverse() const { return Rot2(c_, -s_);}
|
||||
|
||||
/** Compose - make a new rotation by adding angles */
|
||||
Rot2 operator*(const Rot2& R) const {
|
||||
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
|
||||
}
|
||||
|
||||
/** Compose - make a new rotation by adding angles */
|
||||
Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 =
|
||||
boost::none, OptionalJacobian<1,1> H2 = boost::none) const;
|
||||
|
||||
/** Between using the default implementation */
|
||||
Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 =
|
||||
boost::none, OptionalJacobian<1,1> H2 = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// dimension of the variable - used to autodetect sizes
|
||||
inline static size_t Dim() {
|
||||
return dimension;
|
||||
}
|
||||
|
||||
/// Dimensionality of the tangent space, DOF = 1
|
||||
inline size_t dim() const {
|
||||
return dimension;
|
||||
}
|
||||
|
||||
/// Updates a with tangent space delta
|
||||
Rot2 retract(const Vector& v, OptionalJacobian<1, 1> H1 = boost::none,
|
||||
OptionalJacobian<1, 1> H2 = boost::none) const;
|
||||
|
||||
/// Returns inverse retraction
|
||||
Vector1 localCoordinates(const Rot2& t2, OptionalJacobian<1, 1> H1 =
|
||||
boost::none, OptionalJacobian<1, 1> H2 = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
|
||||
/// Exponential map at identity - create a rotation from canonical coordinates
|
||||
static Rot2 Expmap(const Vector1& v, ChartJacobian H = boost::none);
|
||||
|
||||
/// Log map at identity - return the canonical coordinates of this rotation
|
||||
static Vector1 Logmap(const Rot2& r, ChartJacobian H = boost::none);
|
||||
|
||||
/** Calculate Adjoint map */
|
||||
Matrix1 AdjointMap() const { return I_1x1; }
|
||||
|
||||
///Exponential map at identity - create a rotation from canonical coordinates
|
||||
static Rot2 Expmap(const Vector& v, OptionalJacobian<1, 1> H = boost::none);
|
||||
|
||||
///Log map at identity - return the canonical coordinates of this rotation
|
||||
static Vector1 Logmap(const Rot2& r, OptionalJacobian<1, 1> H = boost::none);
|
||||
|
||||
/// Left-trivialized derivative of the exponential map
|
||||
static Matrix dexpL(const Vector& v) {
|
||||
return ones(1);
|
||||
|
|
@ -173,6 +127,18 @@ namespace gtsam {
|
|||
return ones(1);
|
||||
}
|
||||
|
||||
// Chart at origin simply uses exponential map and its inverse
|
||||
struct ChartAtOrigin {
|
||||
static Rot2 Retract(const Vector1& v, ChartJacobian H = boost::none) {
|
||||
return Expmap(v, H);
|
||||
}
|
||||
static Vector1 Local(const Rot2& r, ChartJacobian H = boost::none) {
|
||||
return Logmap(r, H);
|
||||
}
|
||||
};
|
||||
|
||||
using LieGroup<Rot2, 1>::inverse; // version with derivative
|
||||
|
||||
/// @}
|
||||
/// @name Group Action on Point2
|
||||
/// @{
|
||||
|
|
|
|||
Loading…
Reference in New Issue