GTSAM Concepts

GTSAM Notes

As discussed in Generic Programming Techniques, concepts define

  • associated types
  • valid expressions, like functions and values
  • invariants
  • complexity guarantees

Below we discuss the most important concepts use in GTSAM, and after that we discuss how they are implemented/used/enforced.

Manifold

To optimize over continuous types, we assume they are manifolds. This is central to GTSAM and hence discussed in some more detail below.

Manifolds and charts are intimately linked concepts. We are only interested here in differentiable manifolds, continuous spaces that can be locally approximated at any point using a local vector space, called the tangent space. A chart is an invertible map from the manifold to that tangent space.

In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct gtsam::traits. Concept checks are used to check that all required functions are implemented. In detail, we ask that the following items are defined in the traits object (although, not all are needed for optimization):

  • values:
    • enum { dimension = D};, an enum that indicates the dimensionality n of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1.
  • types:
    • TangentVector, type that lives in tangent space. This will almost always be an Eigen::Matrix<double,n,1>.
    • ChartJacobian, a typedef for OptionalJacobian<dimension, dimension>.
    • ManifoldType, a pointer back to the type.
    • structure_category, a tag type that defines what requirements the type fulfills, and therefore what requirements this traits class must fulfill. It should be defined to be one of the following:
      • gtsam::traits::manifold_tag – Everything in this list is expected
      • gtsam::traits::group_tag – The functions defined under Groups below.
      • gtsam::traits::lie_group_tag – Everything in this list is expected, plus the functions defined under Groups, and Lie Groups below.
      • gtsam::traits::vector_space_tag – Everything in this list is expected, plus the functions defined under Groups, and Lie Groups below.
  • valid expressions:
    • size_t dim = traits<T>::GetDimension(p); static function should be defined. This is mostly useful if the size is not known at compile time.
    • v = traits<T>::Local(p,q), the chart, from manifold to tangent space, think of it as q (-) p, where p and q are elements of the manifold and the result, v is an element of the vector space.
    • p = traits<T>::Retract(p,v), the inverse chart, from tangent space to manifold, think of it as p (+) v, where p is an element of the manifold and the result, v is an element of the vector space.
  • invariants
    • Retract(p, Local(p,q)) == q
    • Local(p, Retract(p, v)) == v

Group

A group should be well known from grade school :-), and provides a type with a composition operation that is closed, associative, has an identity element, and an inverse for each element. The following should be added to the traits class for a group:

  • valid expressions:
    • r = traits<T>::Compose(p,q), where p, q, and r are elements of the manifold.
    • q = traits<T>::Inverse(p), where p and q are elements of the manifold.
    • r = traits<T>::Between(p,q), where p, q, and r are elements of the manifold.
  • static members:
    • traits<T>::Identity, a static const member that represents the group’s identity element.
  • invariants:
    • Compose(p,Inverse(p)) == Identity
    • Compose(p,Between(p,q)) == q
    • Between(p,q) == Compose(Inverse(p),q)

The gtsam::group::traits namespace defines the following:

  • values:
    • traits<T>::Identity – The identity element for this group stored as a static const.
    • traits<T>::group_flavor – the flavor of this group’s compose() operator, either:
      • gtsam::traits::group_multiplicative_tag for multiplicative operator syntax, or
      • gtsam::traits::group_additive_tag for additive operator syntax.

We do not at this time support more than one composition operator per type. Although mathematically possible, it is hardly ever needed, and the machinery to support it would be burdensome and counter-intuitive.

Also, a type should provide either multiplication or addition operators depending on the flavor of the operation. To distinguish between the two, we will use a tag (see below).

Lie Group

A Lie group is both a manifold and a group. Hence, a LIE_GROUP type should implements both MANIFOLD and GROUP concepts. However, we now also need to be able to evaluate the derivatives of compose and inverse. Hence, we have the following extra valid static functions defined in the struct gtsam::traits<T>:

  • r = traits<T>::Compose(p,q,Hq,Hp)
  • q = traits<T>::Inverse(p,Hp)
  • r = traits<T>::Between(p,q,Hq,H2p)

where above the H arguments stand for optional Jacobian arguments. That makes it possible to create factors implementing priors (PriorFactor) or relations between two instances of a Lie group type (BetweenFactor).

In addition, a Lie group has a Lie algebra, which affords two extra valid expressions:

  • v = traits<T>::Logmap(p,Hp), the log map, with optional Jacobian
  • p = traits<T>::Expmap(v,Hv), the exponential map, with optional Jacobian

Note that in the Lie group case, the usual valid expressions for Retract and Local can be generated automatically, e.g.

    T Retract(p,v,Hp,Hv) {
      T q = Expmap(v,Hqv);
      T r = Compose(p,q,Hrp,Hrq);
      Hv = Hrq * Hqv; // chain rule
      return r;
    }

For Lie groups, the exponential map above is the most obvious mapping: it associates straight lines in the tangent space with geodesics on the manifold (and it’s inverse, the log map). However, there are several cases in which we deviate from this:

However, the exponential map is unnecessarily expensive for use in optimization. Hence, in GTSAM there is the option to provide a cheaper chart by means of the ChartAtOrigin struct in a class. This is done for SE(2), SO(3) and SE(3) (see Pose2, Rot3, Pose3)

Most Lie groups we care about are Matrix groups, continuous sub-groups of GL(n), the group of n x n invertible matrices. In this case, a lot of the derivatives calculations needed can be standardized, and this is done by the LieGroup superclass. You only need to provide an AdjointMap method.

A CRTP helper class LieGroup is available that can take a class and create some of the Lie group methods automatically. The class needs:

  • operator* : implements group operator
  • inverse: implements group inverse
  • AdjointMap: maps tangent vectors according to group element
  • Expmap/Logmap: exponential map and its inverse
  • ChartAtOrigin: struct where you define Retract/Local at origin

To use, simply derive, but also say using LieGroup<Class,N>::inverse so you get an inverse with a derivative.

Finally, to create the traits automatically you can use internal::LieGroupTraits<Class>

Vector Space

While vector spaces are in principle also manifolds, it is overkill to think about charts etc. Really, we should simply think about vector addition and subtraction. I.e. where

  • Identity == 0
  • Inverse(p) == -p
  • Compose(p,q) == p+q
  • Between(p,q) == q-p
  • Local(q) == p-q
  • Retract(v) == p+v

This considerably simplifies certain operations. A VectorSpace superclass is available to implement the traits. Types that are vector space models include Matrix, Vector, any fixed or dynamic Eigen Matrix, Point2, and Point3.

Testable

Unit tests heavily depend on the following two functions being defined for all types that need to be tested:

  • valid expressions:
    • Print(p,s) where s is an optional string
    • Equals(p,q,tol) where tol is an optional (double) tolerance

Implementation

GTSAM Types start with Uppercase, e.g., gtsam::Point2, and are models of the TESTABLE, MANIFOLD, GROUP, LIE_GROUP, and VECTOR_SPACE concepts.

gtsam::traits is our way to associate these concepts with types, and we also define a limited number of gtsam::tags to select the correct implementation of certain functions at compile time (tag dispatching).

Traits

However, a base class is not a good way to implement/check the other concepts, as we would like these to apply equally well to types that are outside GTSAM control, e.g., Eigen::VectorXd. This is where traits come in.

We use Eigen-style or STL-style traits, that define many properties at once.

Note that not everything that makes a concept is defined by traits. Valid expressions such as traits<T>::Compose are defined simply as static functions within the traits class. Finally, for GTSAM types, it is perfectly acceptable (and even desired) to define associated types as internal types, rather than having to use traits internally.

Concept Checks

Boost provides a nice way to check whether a given type satisfies a concept. For example, the following

BOOST_CONCEPT_ASSERT(IsVectorSpace<Point2>)

asserts that Point2 indeed is a model for the VectorSpace concept.

Future Concepts

Group Action

Group actions are concepts in and of themselves that can be concept checked (see below). In particular, a group can act on another space. For example, the cyclic group of order 6 can rotate 2D vectors around the origin:

q = R(i)*p
where R(i) = R(60)^i, where R(60) rotates by 60 degrees

Hence, we formalize by the following extension of the concept:

  • valid expressions:
    • q = traits<T>::Act(g,p), for some instance, p, of a space S, that can be acted upon by the group element g to produce q in S.
    • q = traits<T>::Act(g,p,Hp), if the space acted upon is a continuous differentiable manifold.

In the latter case, if S is an n-dimensional manifold, Hp is an output argument that should be filled with the nxn Jacobian matrix of the action with respect to a change in p. It typically depends on the group element g, but in most common examples will not depend on the value of p. For example, in the cyclic group example above, we simply have

Hp = R(i)

Note there is no derivative of the action with respect to a change in g. That will only be defined for Lie groups, which we introduce now.

Lie Group Action

When a Lie group acts on a space, we have two derivatives to care about:

  • gtasm::manifold::traits<T>::act(g,p,Hg,Hp), if the space acted upon is a continuous differentiable manifold.

An example is a similarity transform in 3D, which can act on 3D space, like

q = s*R*p + t

Note that again the derivative in p, Hp is simply s R, which depends on g but not on p. The derivative in g, Hg, is in general more complex.

For now, we won’t care about Lie groups acting on non-manifolds.