A Pose Graph based Visual SLAM Algorithm for Robot Pose Estimation

This paper presents a pose graph based visual SLAM (Simultaneous Localization and Mapping) method for 6-DOF robot pose estimation. The method uses a fast ICP (Iterative Closest Point) algorithm to enhance a visual odometry for estimating the pose change of a 3D camera in a feature-sparse environment. It then constructs a graph using the pose changes computed by the improved visual odometry and employ a pose optimization process to obtain the optimal estimates of the camera poses. The proposed method is compared with an Extended Kalman Filter (EKF) based pose estimation method in both feature-rich environments and feature-sparse environments. The experimental results show that the graph based SLAM method has a more consistent performance than the EKF based method in visual feature-rich environments and it outperforms the EKF counterpart in feature-sparse environments.