Debugging Pose Graphs, Part 2

It turns out that my bundle adjustment step had a few bugs. All fixed now, which we verify by running the bundle adjustment step on all matched points (not just a few) and seeing that it gives the same result as only bundle adjustment.
Ok, but now new problems. If I run bundle adjustment after the pose graph optimization, I get a twisty world. Need to see what's going on...
This one's definitely a bug in the pose graph code (bundle adjustment now turned off), but thought it looked kinda cool
And the associated pose graph viewed from above:
Another fun one:
Wound up fixing the issue because I was allocating memory within a for loop and running the optimizer outside that loop, so it didn't have access to the memory. Whoops. But a few cool visualizations, nonetheless.