Pose SLAM Python examples using iSAM2

[jerredchen]

Two files to be added to Python examples:

• Pose2ISAM2Example.py
• Pose3ISAM2Example.py

Both files are Pose SLAM examples that utilize iSAM2 as opposed to a nonlinear optimizer. The examples both consist of a robot that takes in odometry measurements which are inserted into the factor graph and optimized using iSAM2. Instead of hardcoding an additional loop closure constraint, each pose performs simple loop closure detection. A plot is also generated to demonstrate the incremental nature of iSAM2.

Pose2ISAM2Example Plot:

Pose2SLAMPlot.mp4

Pose2ISAM2Example Output (final pose estimates):

Pose3ISAM2Example Plot:

Pose3SLAMPlot.mp4

Pose3ISAM2Example Output (final pose estimates):

[dellaert]

Did only do Pose2, pls generalize my comments to Pose3.

[dellaert]

Cool! Approve after addressing further comments.

[jerredchen]

@dellaert I know that you have already approved changes, but would like to request re-review for you to take one last look for good measure. This is what the Pose2ISAM2 visualization may look like, which we discussed is still the converged optimum as long as the initial estimate was within the basin of attraction. The converged optimum when using noisy measurements to create the odometry factors (which is realistic for practical implementation) of course results in an imperfect trajectory for each visualization plot.

new_plot.mp4

[dellaert]

Please merge after formatting and sort imports w Google style.