First work on Graph SLAM that inherits SLAM interface. Still some work to do. Have to discuss with Dennis
issueID #10