NavState between constraint

[tdallapiazza]

Feature

As the NavState is just a 9D manifold, it doesn't inherit the LieGroup's convenient methods like Between. In working only with Navstate (mainly around imuFactor2) having a between for NavState would be cool for loop closures.

Motivation

Actually introducing a BetweenFactor<NavState> raise an error as traits<NaveState> has no Between method.

Pitch

Adding the method in traits<NavState> so that BetweenFactor<NavState> or if using expression between(NavStat_(key1), NavStat_(key2)) could be used in the graph.

[dellaert]

Interestingly, there are a few papers that define a Lie group "SE+(2)", which is just NavState plus a group operation I think. We could look into upgrading NavState and connecting to those papers...

[varunagrawal]

Barrau20icra provides a good way to deal with this.