inital implementation of slam optimizer