Incorporating GTSAM Pose3 Class

[mattking-smith]

Issue Purpose

I have gotten interest/requests from colleagues to incorporate the Pose3 class from GTSAM for the purpose of motion planning for single body systems such as quadcopters, UAVs, etc...

This would allow for both position collision avoidances and attitude constraints

Outstanding Goals

• [ ] Pose3 class implementation
• [ ] Attitude constrained example

  Verification In:

• [ ] C++
• [ ] MATLAB
• [ ] Python