Add Martin Brossard's Implementation of SE_2(3)

[ProfFan]

Expect more in this series. Basically what I did is copying from Martin's GTSAM fork https://github.com/mbrossar/gtsam , fixing merge conflicts, and style fixes.

[dellaert]

Hi @ProfFan @mbrossar ,
could you perhaps elaborate in the PR comment on how this differs from NavState? It seems very similar.
If it is only the absence of "Lie-group" traits, maybe these could simply be added by adding (in a separate file) the traits to "complete" NavState into a Lie group.
Frank

[varunagrawal]

I agree with Frank. Mathematically, NavState is equivalent to $SE_2(3)$ but needs all the Lie traits implemented, something which has been on my TODO list for a while now.

[ProfFan]

Yup, I am just testing waters :) Let's find a slot to get that done.