Local optimized and scalable frame-to-model SLAM