Corrected plane finding algorithm