Add kd-tree
Add priority queue backed by CFBinaryHeap