Add sim_annealing_registration and rpm_em_step