Refactor tps_pose_cost into another type of cost. Fix transpose and matrix dimension problems after enabling asserts.