Implemented RANSAC to remove outliers, much better results, still improvements required