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.