Custom factors and degrees of freedom.

[biggibig]

Hello. I was wondering if there were any restriction on creating custom factors.

Let's say we are dealing with a 6DOF pose graph SLAM Rx Ry Rz, Tx Ty Tz. Can I make custom factors that are less than 6DOF? I know there is a GPS factor Tx Ty Tz that's only 3DOF. But For example can I make custom a factor consisting of 4DOF something like Rx Ry Rz Tz? Or some other reduced combination of the 6 degrees of freedom, maybe even a single DOF, for example if I know that my heading is really good?

[mikesheffler]

Yes, you can! GPS is a good example of affecting only a subset of the full 6DOF pose. Another is the Relative Elevation factor, where z, if you will, is being affected, but rotations and other changes in translation (x, and y) are not. Take a look at the evaluateError method for that class.

The only bit of warning is that it is very easy to be wrong when calculating the Jacobians for your custom factor. Take a look at an overview of some relevant math for hints on how to do your calculations. Check your work by putting together a unit test that uses GTSAM's numerical derivative capabilities; for example, here's the unit test for the Relative Elevation factor.

Finally, consider expression factors. If you are clever about chaining together existing functions, you can sometimes write expression factors that are quite complicated without having to calculate Jacobians yourself at all. Look in examples for inspiration.

Good luck! Consider the Google Group as a resource well.

[biggibig]

That's great, lots of good resources to look at. Is it possible to make custom relative factors or only absolute? Relative elevation factor sounds like a relative factor just from it's name. Thanks for the reply!

[mikesheffler]

Is it possible to make custom relative factors or only absolute?

Did you mean "is it possible to make absolute factors, or only relative"? I wasn't sure from the sentence that came after it.

Anyway, both are possible!

Relative stuff can be handy because there might be more than one thing that you are not sure about. So, RelativeElevationFactor, for example, is a binary factor, because it deals with two things -- a Pose3 and a Point3. You want to reason about the elevation of that Pose3 with respect to that Point3, but the Point3 is also a variable (you measured it, that measurement had some error, etc.).

Absolute stuff is handy because ... maybe it's not relative. If you are thinking about just one variable, you're probably going to be using a unary factor (one thing). The simplest example of a unary factor is a prior factor. We use the term prior when we're thinking about the a priori distribution of a variable, but the mechanics of using the prior factors available to us aren't fundamentally different than the mechanics of unary factors -- they're the same thing, we just don't have two names for it. That's a liiiittle bit of a conceptual overload on that one word, but it's basically true.

Consider this example of setting a prior from the Pose2 SLAM Example.

If what you mean by absolute isn't the unary part, but rather something closer to this-quantity-isn't-up-for-debate (it's not a 'variable' at all), you could consider Nonlinear Equality, but realistically, you're usually inviting numerical problems if you go down that route, and you're better off using a traditional unary factor that has a very small uncertainty in the 'measurement'.

Be sure to check out Frank's crash course guide to GTSAM. 🙂

[biggibig]

Yes by absolute I meant priors. For some reason I assumed that gtsam only allowed custom unary factors. But if it allows custom relative as well that's even better. I'm going to have to review all the materials and update my gtsam version.