Changed code so that orientation angle in graph slam is normalized between -pi and pi as it is in EKF Slam code.
issueID #7