Updated how the graph is constructed.

Now the graph is constructed much faster. No longer n^2 and alot of the
work is offloaded to pdist2. Takes ~11 secs on map5