1) robustified detection using mutual model-cluster fitness score and 2) robustifed ICP using huberKernel