I have wanted to use Graph SLAM for a long time now but got bogged down trying to detect features to supply to Graph SLAM.

I've tried Hough transforms to find lines, also wrote various versions if my own corner detection algorithms.  I even used deep nets to detect corners and achieved 95% accuracy.

But all that still didn't amount to a viable set of features.

I then hit on the idea that a feature could actually be an entire map, which is what has lead to the current technique of the robot creating sub maps which it localizes in and moves between.

I also had problems getting my implementation of Graph SLAM working with an X Y THETA pose.

Ultimately I resorted to a watered down implementation of Graph SLAM implemented without using matrices. It doesn't attempt to model the position of the robot, but rather just the relationships between the features.

Hopefully in the next few weeks I'll be able to integrate my Graph SLAM into the build and finally see a map that improves as the robot traverses the map.