Add a MagPoseFactor for using magnetometer measurements with Pose2/Pose3

[miloknowles]

This PR adds a new MagPoseFactor<POSE> which extends the existing MagFactor to work with Pose2 and Pose3. The factor also includes an optional body_P_sensor in case the magnetometer does not coincide with the body frame. I implemented this factor so that I could use magnetometer measurements within a VIO system, and thought it might be useful to others. It doesn't seem like the MagFactor gets much use, so no worries if you don't think this factor should be added.

[varunagrawal]

Needs some comment and minor syntax updates.

[varunagrawal]

LGTM

[varunagrawal]

LGTM

[varunagrawal]

LGTM

[varunagrawal]

LGTM

[varunagrawal]

LGTM

[varunagrawal]

LGTM

[varunagrawal]

LGTM

[varunagrawal]

LGTM

[varunagrawal]

Sorry for the ridiculous amounts of reviews. Github was unresponsive for a while and I have no idea what happened.

[varunagrawal]

@dellaert should we consider moving both MagFactor and MagPoseFactor from gtsam_unstable to gtsam?

[miloknowles]

Thanks for the review, @varunagrawal! Made the changes you requested and will wait on the decision about moving out of gtsam_unstable.

[varunagrawal]

LGTM

[varunagrawal]

@miloknowles we can always consider moving it out of gtsam_unstable in another PR. Let's merge this for now.

[tmcg0]

Sorry to comment on a closed PR, but glad this got merged! I had actually built a custom factor to do this in my own work and this factor is way more robust, tested, and flexible than the one I had made, so thanks Milo!

I just wanted to throw in my 2 cents that this would be great to move out of gtsam_unstable. I'm not sure what determines what goes in unstable vs. not, but I can't think of any major deficiencies with this w.r.t. manifold discontinuities, numerical conditioning, or instability of any subcomponent.

[dellaert]

Awesome. Fork, move, PR :-)

[miloknowles]

@tmcg0 feel free to open the gtsam_unstable --> gtsam PR if you're already working on it! Otherwise I'm happy to take care of that.

[tmcg0]

@tmcg0 feel free to open the gtsam_unstable --> gtsam PR if you're already working on it! Otherwise I'm happy to take care of that.

You can go ahead and take care of it! I haven't started it and I'm sure I'd mess up something with the build scripts or python bindings and have to spend a lot of time backtracking fixes.