Robust Point Matching
Energy Function
