Finished rigid CPD implementation