Assorted minor improvements.
Changed KDTree to use unique_ptr instead of shared_ptr, for speed/memory boost.
  Decreased runtime by 20%, memory footprint by 40%.
Changed frontier check to an unordered_set, for speed boost.