Implement final cost function for marker to joint problem