fixed most ransac_slam nan bugs