Added benchmark for 40 frames for icp, saved as (rms, time)