prm.py 文件源码

def find_k_nearest(self, k, s_query):
        '''
        Gets KNN using heap sort
        '''
        neighbors = []
        for n in self.nodes:
            dist = np.linalg.norm(s_query - n.state)
            if dist > 0:
                hn = HeapNode(n, dist)
                heapq.heappush(neighbors, hn)

        #Check if we have <= k to choose from
        #in which case, simply return that list
        ret_list = []
        if len(neighbors) <= k:
            for k in neighbors:
                ret_list.append(k.node)
            return ret_list

        #get the k nearest neighbors
        for i in xrange(k):
            t = heapq.heappop(neighbors)
            ret_list.append(t.node)

        return ret_list