Filter points: written a very slow algorithm, but hopefully good idea