Simultaneous Localization and Mapping (SLAM) in 3D space is a key research area in modern robotics. Notable RGB-D 3D reconstruction systems include KinectFusion™, which enables dense volumetric modeling of a static scene in real-time at sub-centimeter resolution. Current solutions to adapt this technology over extended areas suffer from problems of unbounded drift due to an open-loop process. The Inventors present a method to address loop closures in a volume shifting-based mapping systems that take advantage of camera pose graph optimization and non-rigid space deformation. The result is a visual SLAM system that captures high fidelity dense maps in real-time with the local reconstruction quality of KinectFusion™, and the advantages of local consistency given by camera pose graph optimization.