First version of detecting using ground plane candidates
Some debugs to see if I am getting the correct features