3D Non-Rigid SLAM in Minimally Invasive Surgery