Design Discussion

[furgalep]

I'd like to converge on a design that tightly integrates with GTSAM as soon as possible so that we can come up with first implementations of everything using the new "Traits" design and GTSAM expressions. To understand this discussion it is useful to review how the GTSAM trait classes work, and how block automatic differentiation (BAD) works in GTSAM. The short summary of these two topics is:

1. The GTSAM traits design will allow us to wrap external types. We will start by representing 6DOF transformations using the minkindr library, then switch to kindr after it has been refactored.
2. We will also use traits to wrap custom coefficient types needed by different curve types.
3. We want to use the GTSAM BAD framework. This framework is most efficient when fixed-size types are used throughout. It can get slow when variable-sized types are used.

[furgalep]

So, first thing first...here is a brief sketch of how curves and GTSAM should work together.

What does it look like from a user perspective?

Let's say that I have a curve representing elements of SE3. The normal (non gtsam usage) would be to use the curve like this:

SE3Curve curve;

// initialize the curve somehow.

// Evaluate the curve at time t_k.
SE3 Tk = curve.evaluate( t_k );
// Evaluate the velocity at time t_k.
Eigen::Vector6d v = curve.evaluateDerivative( t_k, 1 );

Integration with GTSAM is a little more complicated because of its functional design. But, because we use the traits classes and BAD framework, from the outside, the usage is not that different:

SE3Curve curve;

// initialize the curve somehow.

// Get a gtsam expression for the curve's value at time t_k.
gtsam::Expression<SE3> exp_Tk = curve.getExpression( t_k );
// Get a gtsam expression for the curve's derivative at time t_k.
gtsam::Expression<Eigen::Vector3d> exp_v = curve.getDerivativeExpression( t_k );

// Get the coefficient values used by the curve.
// These coefficient values can be optimized by gtsam.
gtsam::Values values;
curve.appendValues( &values );

// Evaluate the expression with the given values.
SE3 T_k = exp_Tk.value( values );
Eigen::Vector6d v =  exp_v.value( values );

Nice, right? Using the Values object and the expression allows gtsam to evaluate the curve using different expression values. The expressions are also able to compute their Jacobians with respect to small changes in the values, but that will only be used by GTSAM underneath.

[furgalep]

What is the magic underneath?

To build expressions, GTSAM relies on two things:

1. at the lowest level, every type used (every type stored in the values object, as well as every expression value type) has an associated traits class, and
2. each expression is built from a function that provides a value and the Jacobian of that value with respect to small changes in the argument.

GTSAM comes with a traits class for all Eigen matrices and vectors, and we will implement traits classes for the kindr types. This leaves the coefficients.

The implementation of this statement,

gtsam::Expression<SE3> exp_Tk = curve.getExpression( t_k );

must be completely curve specific. Here is an example for a Hermite spline (with details omitted).

// First, we need a functor that computes the curve value
// and the Jacobians with respect to the arguments
struct HermiteFunctor {
  int64_t coefficientZeroTimeNsec;
  int64_t coefficientOneTimeNsec;
  int64_t evaluationTime;

  HermiteFunctor( int64_t coefficientZeroTimeNsec, int64_t coefficientOneTimeNsec, int64_t evaluationTime ) : 
    coefficientZeroTimeNsec_(coefficientZeroTimeNsec), coefficientOneTimeNsec_(coefficientOneTimeNsec), evaluationTime_(evaluationTime) {}

  SE3 operator()( const HermiteCoefficient& c1, const HermiteCoefficient& c2,
                 gtsam::OptionalJacobian<6,12> Jc1, gtsam::OptionalJacobian<6,12> Jc2) {
    SE3 result = /* the magic hermite formula */;
    if(Jc1) {
      // Fill in Jc1.
    }
    if(Jc2) {
      // Fill in Jc2.
    }
    return result;
  }
};

// Next we can fill in the member function:
Expression<SE3> SE3Curve::evaluateExpression( int64_t time ) {
  // Look up the coefficients (I'm just guessing at this syntax).
  KeyCoefficientTime * kct1 = nullptr;
  KeyCoefficientTime * kct2 = nullptr;
  manager_.getCoeficientsAt(time, &kct1, &kct2);

  // These are the "Leaf" expressions. They are hooked up
  // directly to keys. When expression.value( values ) is called
  // these expressions look up their value in the values object.
  Expression<HermiteCoefficient> c1(kct1->key);
  Expression<HermiteCoefficient> c2(kct2->key);

  // This expression constructor takes two input expressions and a binary
  // functor.
  return Expression<SE3>( c1, c2, HermiteFunctor(kct1->time, kct2->time, time) );
}

// lastly, implement the function that packs the coefficients into the Values object
void SE3Curve::appendValues( gtsam::Values* values ) const {
  // TODO(ptf) check that values is not null.
  // Then pack values with the coefficients
  HermiteCoefficientManager::const_iterator it = manager_.begin();
  for( ; it != manager_.end(); ++it ) {
    // It is very important that the type of it->value matches the type required
    // by the HermiteFunctor defined above. In this example, the type must be
    // HermiteCoefficient, not a generic coefficient class.
    values->insert( it->key, it->value );
  }
}

Please note that I'm not sure if any of this syntax is implemented like this. I'm pretty sure that our current design does not support at least some of this.

What is important in the example above?

1. HermiteCoefficient should be a fixed size and it should have an associated gtsam::Traits<HermiteCoefficient> definition that clearly states the size available at compile time.
2. The HermiteCoefficient type (not a parent class type) must be used by the functor and the expressions.
3. The HermiteCoefficient type (not a parent class type) must be pushed into the Values object.

These two points seem to suggest that the design for the coefficients that we have right now won't work for interfacing with GTSAM. It also means that the design that @sanderson77 came up with for the coefficient manager (where the extra data is stored in the KeyCoefficientTime struct, will also not work because that extra data won't be available when invoking the functor. Instead, the extra data needs to be stored with the coefficient.

[furgalep]

What should we do now that we know all of that?

@sanderson77 @rdube @gawela @mikebosse

What the above suggests to me is:

1. We need a proper hierarchy for Coefficients. Top level functions that want to access coefficients will probably have to access them by containers of boost::shared_ptr<Coefficient>. We can still have snazzy things like Eigen::VectorXd v = coefficient->asVector().
2. The Support2CoefficientManager should be templated on value type. The coefficients should themselves store all the extra stuff that is needed for evaluation of a particular curve type. Said more strongly, instead of storing any extra data needed for evaluation in a KeyCoefficientTime derived class (as proposed in #44), we should store this extra information in the coefficient itself so that it is available for evaluation.

Any other comments or suggestions?

[furgalep]

Suggestions based on careful consideration of all evidence.

Here are the results of @furgalep and @mikebosse having a discussion of the Design Discussion #45 (oop...that is this issue!).

1. We believe that coefficients should be strongly typed and pushed way down into the derived classes. Therefore, we should get rid of the type-erasure-style coefficient class.
2. We don't believe that the base class should expose the coefficients. CurveBase can go away, the coefficient getting functions can be removed, and we can move the remaining functions into Curve.
3. Coefficient and related classes can go away
4. Every curve type should implement its own coefficient type(s). This will also involve implementing a gtsam::Traits class for each coefficient type.
5. HermiteCoefficientManager --> LocalSupport2CoefficientManager should be templated on coefficient type.

[rdube]

Getting rid of the coefficient class should simplify the design. One question on point 2, if we completely get rid of getCoefficients() how would we generate initial values for performing a GTSAM optimization of the curve? We can create factors based on expressions which will internally make the link to the coefficients. However, we still need to initialize these values for calling the optimization. Am I missing something?

[furgalep]

There you call fitCurve()

[furgalep]

or extend()

https://github.com/ethz-asl/curves/blob/master/curves/include/curves/Curve.hpp#L52-L69

[sanderson77]

This all sounds good. I've looked through the documentation on traits, although I am still a little fuzzy, the expressions implementation is SlerpSE3Curve was fairly clear. I should be able to make the kernel interpolation work with this setup..

A few questions,

1) It is my understanding that our Coefficients will then be the `values' that can be used to evaluate expressions.. Paul then made a comment above that "The expressions are also able to compute their Jacobians with respect to small changes in the values"... so I guess my question is: if I add additional variables to my curve-specific Coefficients, do these necessarily become part of the optimized state? My guess is that it will depend on how I setup the chart/jacobians?

2) Another thing that will come up eventually is that I have some static values that are relative (stored between coeff0 and coeff1).. and presumably with this setup I will have to store them either inside coeff0 OR coeff1.. which is a little messy for synchronization. As long as nothing has direct access to the coeff manager, my curve implementation should be able to bookkeep this?

[furgalep]

Hi Sean, Good questions.

Let me try to explain things a little better later today.

[furgalep]

Okay, let me spend a few minutes sketching things out.

Every curve should have some set of coefficients that have values that we will estimate in gtsam. Every curve can decide if it uses a uniform coefficient type, or several different coefficient types. Most of the curves that we implement right now will have uniform coefficient types.

To work with GTSAM we have a few requirements:

• Traits: Every coefficient type has a gtsam Traits class that specifies the size, retract and local functions, and a handful of other things.
• Evaluation Functions: We implement the evaluation functions for our curves as functions that may manipulated into the form:

ValueType evaluate( const Coeff1Type& coeff1, ..., const CoeffNType& coeffN,
                                  OptionalJacobian<V,D1> J1, ..., OptionalJacobian<V,DN> JN);

where V = gtsam::traits<ValueType>::dimension and Di = gtsam::traits<CoeffiType>::dimension.

For most of our use cases, all coefficients will have the same type.

I say "manipulated into the form of" because you are free to use boost::bind to build a function that looks like this from a function that needs more information. An example is available here: https://github.com/ethz-asl/curves/pull/48/files#diff-50ae2f23510fead4b936f72ccab83966R156

In one way, this requirement is a headache as it means that we need to have pure functional evaluation functions. This means that if you have any other data that you need (covariance matrices, etc), you need to either bind them to these functions or store them in the coefficients.

Originally I suggested that you should store the extra data with the coefficients. I'm not sure about that anymore. The problem is that GTSAM likes to copy coefficients around a lot and so any extra data would be copied. Also, your requirement to keep data in between pairs of coefficients is a bit tricky.

At the highest level, I think that you should store your extra data with coefficients somehow in the curve. If you use the LocalSupport2Manager, you can store the extra data in the template type. But...this type doesn't have to be the coefficient type. You can store the coefficient in there with a bunch of other stuff. Then, when building Expressions for GTSAM, you could pass the other data into the functor using boost::bind(). This way, the Value that is passed to GTSAM is just the coefficient type and the extra data is just stored in the Expression.

Does that make sense or what it a bit fast?

[sanderson77]

I think this makes sense :)