Point to plane (with outlier rejection) implemented [untested]