Use of stl priority_queue instead of custom heap class

Got some performance improvements too by using a real Heap instead of CvPriorityQueueFloat.