PoseGraph

implement SLAM algorithm from scratch using pose graph optimization framework