-
Notifications
You must be signed in to change notification settings - Fork 28
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Design Discussion #45
Comments
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 |
What is the magic underneath?To build expressions, GTSAM relies on two things:
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?
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 |
What should we do now that we know all of that?@sanderson77 @rdube @gawela @mikebosse What the above suggests to me is:
Any other comments or suggestions? |
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!).
|
Getting rid of the coefficient class should simplify the design. One question on point 2, if we completely get rid of |
There you call |
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,
|
Hi Sean, Let me try to explain things a little better later today. |
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:
where 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: 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 Does that make sense or what it a bit fast? |
I think this makes sense :) |
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:
The text was updated successfully, but these errors were encountered: