First version of corner detection. Builds ok, tested with real data , but too many corners appera... still debugging