def knn_search(root: Node, result_set: KNNResultSet, key): if root is None: return False # compare the root itself 第一步 query的点和root计算worst result_set.add_point(math.fabs(root.key - key), root.value) if result_set.worstDist( ) == 0: # A special case - if the worst distance is 0, no need to search anymore return True if root.key >= key: # query point < root.key, search left # iterate left branch first if knn_search(root.left, result_set, key): # if key!= query, need to go through one subtree. return True # ( knn_search root 里面root是none返回false, key == query时候 worstDist=0,返回true,不然就一直迭代) elif math.fabs(root.key - key) < result_set.worstDist( ): ## May not need to search for the other subtree, depends on worst distance. return knn_search(root.right, result_set, key) return False else: # iterate right branch first if knn_search(root.right, result_set, key): return True elif math.fabs(root.key - key) < result_set.worstDist(): return knn_search(root.left, result_set, key) return False
def kdtree_knn_search(root: Node, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray): if root is None: return False if root.is_leaf(): # 是叶子 不会被分割 ,直接丢到结果集里面 # compare the contents of a leaf leaf_points = db[root.point_indices, :] diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1) for i in range(diff.shape[0]): result_set.add_point(diff[i], root.point_indices[i]) return False if query[root.axis] <= root.value: ## axis维度上的查询点 在根节点的左边 kdtree_knn_search(root.left, db, result_set, query) # q[axis] inside the partition if math.fabs(query[root.axis] - root.value) < result_set.worstDist(): kdtree_knn_search(root.right, db, result_set, query) # |q[axis] - splitting_value| < w else: kdtree_knn_search(root.right, db, result_set, query) if math.fabs(query[root.axis] - root.value) < result_set.worstDist(): kdtree_knn_search(root.left, db, result_set, query) return False
def kdtree_knn_search(root: Node, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray): if root is None: return False if root.is_leaf(): # compare the contents of a leaf, put into the result set leaf_points = db[root.point_indices, :] diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1) for i in range(diff.shape[0]): result_set.add_point(diff[i], root.point_indices[i]) return False # 作业2 # 提示:仍通过递归的方式实现搜索 # 屏蔽开始 if query[root.axis] < root.value: # query[axis] inside the partition kdtree_knn_search(root.left, db, result_set, query) if math.fabs(query[root.axis] - root.value) < result_set.worstDist(): # |query[axis]-splitting_value| < w kdtree_knn_search(root.right, db, result_set, query) else: kdtree_knn_search(root.right, db, result_set, query) if math.fabs(query[root.axis] - root.value) < result_set.worstDist(): kdtree_knn_search(root.left, db, result_set, query) # 屏蔽结束 return False
def kdtree_knn_search(root: Node, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray): if root is None: return False if root.is_leaf(): # compare the contents of a leaf leaf_points = db[root.point_indices, :] diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1) for i in range(diff.shape[0]): result_set.add_point(diff[i], root.point_indices[i]) return False if query[root.axis] <= root.value: kdtree_knn_search(root.left, db, result_set, query) if math.fabs(query[root.axis] - root.value) < result_set.get_worst_dist(): kdtree_knn_search(root.right, db, result_set, query) else: kdtree_knn_search(root.right, db, result_set, query) if math.fabs(query[root.axis] - root.value) < result_set.get_worst_dist(): kdtree_knn_search(root.left, db, result_set, query) return False
def kdtree_knn_search(root: Node, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray): if root is None: return False if root.is_leaf(): # compare the contents of a leaf leaf_points = db[root.point_indices, :] diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1) for i in range(diff.shape[0]): result_set.add_point(diff[i], root.point_indices[i]) return False # 作业2 # 提示:仍通过递归的方式实现搜索 # 屏蔽开始 # search in the left or right by axis, and search in the circle, whose radius is fixed if query[root.axis] <= root.value: kdtree_knn_search(root.left, db, result_set, query) if math.fabs(query[root.axis] - root.value) < result_set.worstDist(): kdtree_knn_search(root.right, db, result_set, query) else: kdtree_knn_search(root.right, db, result_set, query) if math.fabs(query[root.axis] - root.value) < result_set.worstDist(): kdtree_knn_search(root.left, db, result_set, query) # 屏蔽结束 return False
def octree_knn_search(root: Octant, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray): if root is None: return False if root.is_leaf and len(root.point_indices) > 0: leaf_points = db[root.point_indices, :] diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1) for i in range(diff.shape[0]): result_set.add_point(diff[i], root.point_indices[i]) # check whether we can stop search now return inside(query, result_set.worstDist(), root) morton_code = 0 if query[0] > root.center[0]: morton_code = morton_code | 1 if query[1] > root.center[1]: morton_code = morton_code | 2 if query[2] > root.center[2]: morton_code = morton_code | 4 if octree_knn_search(root.children[morton_code], db, result_set, query): return True for c, child in enumerate(root.children): if c == morton_code or child is None: continue if False == overlaps(query, result_set.worstDist(), child): continue if octree_knn_search(child, db, result_set, query): return True return inside(query, result_set.worstDist(), root)
def kdtree_knn_search(root: Node, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray): #搜索query点的邻近点 if root is None: return False #1、节点不存在 if root.is_leaf(): # compare the contents of a leaf leaf_points = db[root.point_indices, :] #获取叶子节点的所有点 diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1) for i in range(diff.shape[0]): result_set.add_point(diff[i], root.point_indices[i]) return False #2、叶子节点 # 作业2 # 提示:仍通过递归的方式实现搜索 # 屏蔽开始 if root.value >= query[root.axis]: if kdtree_knn_search(root.left, db, result_set, query): return True elif math.fabs(root.value - query[root.axis]) < result_set.worst_dist: return kdtree_knn_search(root.right, db, result_set, query) return False #3、左右子树都不满足,就需要返回上一层树,所以为False else: if kdtree_knn_search(root.right, db, result_set, query): return True elif math.fabs(root.value - query[root.axis]) < result_set.worst_dist: return kdtree_knn_search(root.left, db, result_set, query) return False #3、左右子树都不满足,就需要返回上一层树,所以为False # 屏蔽结束 return False
def kdtree_knn_search(root: Node, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray): if root is None: return False if root.is_leaf(): # compare the contents of a leaf leaf_points = db[root.point_indices, :] diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1) for i in range(diff.shape[0]): result_set.add_point(diff[i], root.point_indices[i]) return False # 作业2 # 提示:仍通过递归的方式实现搜索 # 屏蔽开始 root.value = (middle_left_point_value + middle_right_point_value) * 0.5 root.left = kdtree_recursive_build(root.left, db, point_indices_sorted[0:middle_right_idx], axis_round_robin(axis, dim=db.shape[1]) leaf_size) # bi-divide, and get the right-son tree root.right = kdtree_recursive_build(root.right, db, point_indices_sorted[middle_right_idx:]), axis_round_robin(axis, dim=db.shape[1]), leaf_size)
def knn_search(root:Node,result_set:KNNResultSet,key): if root is None: return False # compare the root itself #计算worst_dist ,并把当前root.value(index二叉树)里的值加入到resut_set 中 result_set.add_point(math.fabs(root.key - key),root.value) # A special case – if the worst distance is 0, no need to search anymore if result_set.worstDist() == 0: return True # iterate left branch first if root.key >= key: # If key != query, need to go through one subtree if knn_search(root.left, result_set, key): return True # May not need to search for the other subtree, depends on worst distance elif math.fabs(root.key-key) < result_set.worstDist(): return knn_search(root.right, result_set, key) return False else: # iterate right branch first if knn_search(root.right, result_set, key): return True elif math.fabs(root.key-key) < result_set.worstDist(): return knn_search(root.left, result_set, key) return False
def octree_knn_search(root: Octant, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray): if root is None: return False if root.is_leaf and len(root.point_indices) > 0: # compare the contents of a leaf leaf_points = db[root.point_indices, :] diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1) for i in range(diff.shape[0]): result_set.add_point(diff[i], root.point_indices[i]) # check whether we can stop search now return inside(query, result_set.worstDist(), root) # 提前结束:核心-八叉树对3个维度有限制; # 结束条件:其实是两个方向:向下递归,向上回溯 # 1-当一个节点返回True时,表示找到knn了,维度限制导致不会有更优的了,不再向下,立即结束; # 2-最坏距离球如果 inside 当前节点,不需要再向上回溯检查其他节点,立即结束; # 跳过条件:在检查 # 1. search the first relevant child: # 找到最近的孩子,根据查询点的莫顿码 morton_code = 0 if query[0] > root.center[0]: morton_code = morton_code | 1 if query[1] > root.center[1]: morton_code = morton_code | 2 if query[2] > root.center[2]: morton_code = morton_code | 4 if octree_knn_search(root.children[morton_code], db, result_set, query): return True # 2. check other children for c, child in enumerate(root.children): if c == morton_code or child is None: continue if False == overlaps(query, result_set.worstDist(), child): continue if octree_knn_search(child, db, result_set, query): return True # final check of if we can stop search return inside(query, result_set.worstDist(), root)
def octree_knn_search(root: Octant, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray): if root is None: return False if root.is_leaf and len(root.point_indices) > 0: # compare the contents of a leaf leaf_points = db[root.point_indices, :] diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1) for i in range(diff.shape[0]): result_set.add_point(diff[i], root.point_indices[i]) # check whether we can stop search now #判断需要查询的点query,和半径为result_set.worstDist()构成的球,是否在octant内,该octant为叶子节点。 #如果在内则不需要去查询其他的Octant return inside(query, result_set.worstDist(), root) # 作业7 # 屏蔽开始 #如果不是叶子节点,先找到查询点属于哪个子Octant children_idx = 0 if query[0] > root.center[0]: # x轴 children_idx = children_idx | 1 if query[1] > root.center[1]: # y轴 children_idx = children_idx | 2 if query[2] > root.center[2]: # z轴 children_idx = children_idx | 4 #如果在这个子octant中发现,查询点在该子octant中,同时由worst_dist构成的球也被octant包含,所以直接返回 if octree_knn_search(root.children[children_idx], db, result_set, query): return True #如果不满足上边的情况则需要遍历其他子octant for c, child in enumerate(root.children): if c == children_idx or child == None: continue if overlaps(query, result_set.worstDist(), child) == False: continue if octree_knn_search(root.children[c], db, result_set, query): return True # 屏蔽结束 # final check of if we can stop search return inside(query, result_set.worstDist(), root)
def octree_knn_search(root: Octant, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray): if root is None: return False if root.is_leaf and len(root.point_indices) > 0: # compare the contents of a leaf leaf_points = db[root.point_indices, :] diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1) for i in range(diff.shape[0]): result_set.add_point(diff[i], root.point_indices[i]) # check whether we can stop search now return inside(query, result_set.worstDist(), root) # 作业7 # 屏蔽开始 child_idx = 0 # 判断所查询点在八叉树的位置 if query[0] > root.center[0]: child_idx = child_idx | 1 if query[1] > root.center[1]: child_idx = child_idx | 2 if query[2] > root.center[2]: child_idx = child_idx | 4 # 递归判断是否在该区域就可以找到足够多的点 if octree_knn_search(root.children[child_idx], db, result_set, query): return True # 没有在查询点区域找到,对其他区域进行搜索 for c, child in enumerate(root.children): if c == child_idx or child is None: # 搜索区域没有点或所搜索到查询点区域,skip continue if False == overlaps(query, result_set.worstDist(), child): # 搜索区域与查询点和最坏距离构成的球面没有交点,skip continue if octree_knn_search(child, db, result_set, query): # 其他情况可以进入搜索区域搜索(递归) return True # 屏蔽结束 # final check of if we can stop search return inside(query, result_set.worstDist(), root)
def octree_knn_search(root: Octant, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray): if root is None: return False if root.is_leaf and len(root.point_indices) > 0: # compare the contents of a leaf leaf_points = db[root.point_indices, :] diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1) for i in range(diff.shape[0]): result_set.add_point(diff[i], root.point_indices[i]) # check whether we can stop search now return inside(query, result_set.worstDist(), root) # 作业7 # 屏蔽开始 # Determine & search the most relevant child morton_code = 0 if query[0] > root.center[0]: morton_code = morton_code | 1 if query[1] > root.center[1]: morton_code = morton_code | 2 if query[2] > root.center[2]: morton_code = morton_code | 4 if octree_knn_search(root.children[morton_code], db, result_set, query): return True # check other children for c, child in enumerate(root.children): if c == morton_code or child is None: continue # if an octant is not overlapping with query ball, skip if overlaps(query, result_set.worstDist(), child) == False: continue if octree_knn_search(child, db, result_set, query): return True # 屏蔽结束 # final check of if we can stop search return inside(query, result_set.worstDist(), root) # if query ball is inside an octant, stop
def octree_knn_search(root: Octant, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray): if root is None: return False if root.is_leaf and len(root.point_indices) > 0: # compare the contents of a leaf leaf_points = db[root.point_indices, :] diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1) for i in range(diff.shape[0]): result_set.add_point(diff[i], root.point_indices[i]) # check whether we can stop search now return inside(query, result_set.worstDist(), root) # 作业7 # 屏蔽开始 # 屏蔽结束 # final check of if we can stop search return inside(query, result_set.worstDist(), root)
def kdtree_knn_search(root: Node, db: np.ndarray, result_set: KNNResultSet, query: np.ndarray): if root is None: return False if root.is_leaf(): # compare the contents of a leaf leaf_points = db[root.point_indices, :] diff = np.linalg.norm(np.expand_dims(query, 0) - leaf_points, axis=1) for i in range(diff.shape[0]): result_set.add_point(diff[i], root.point_indices[i]) return False # 作业2 # 提示:仍通过递归的方式实现搜索 # 屏蔽开始 # 屏蔽结束 return False
def knn_search(root: Node, result_set: KNNResultSet, key): if root is None: return False # compare the root itself result_set.add_point(math.fabs(root.key - key), root.value) if result_set.worstDist() == 0: return True if root.key >= key: # iterate left branch first if knn_search(root.left, result_set, key): return True elif math.fabs(root.key-key) < result_set.worstDist(): return knn_search(root.right, result_set, key) return False else: # iterate right branch first if knn_search(root.right, result_set, key): return True elif math.fabs(root.key-key) < result_set.worstDist(): return knn_search(root.left, result_set, key) return False
def main(): # configuration leaf_size = 32 min_extent = 0.0001 k = 8 radius = 1 #root_dir = '/Users/renqian/cloud_lesson/kitti' # 数据集路径 #cat = os.listdir(root_dir) #iteration_num = len(cat) # load date filename = '/home/ljn/SLAM/dateset/000000.bin' db_np = read_velodyne_bin(filename) iteration = 1 print("octree -----------------------------------") construction_time_sum = 0 knn_time_sum = 0 radius_time_sum = 0 brute_time_sum = 0 result_set_knn = KNNResultSet(capacity=k) query = db_np[95, :] begin_t = time.time() root = octree.octree_construction(db_np, leaf_size, min_extent) construction_time_sum += time.time() - begin_t begin_t = time.time() octree.octree_knn_search(root, db_np, result_set_knn, query) knn_time_sum += time.time() - begin_t print('knn search result\n', result_set_knn) begin_t = time.time() result_set_rnn = RadiusNNResultSet(radius=radius) octree.octree_radius_search_fast(root, db_np, result_set_rnn, query) radius_time_sum += time.time() - begin_t print('rnn search result\n', result_set_rnn) begin_t = time.time() diff = np.linalg.norm(np.expand_dims(query, 0) - db_np, axis=1) nn_idx = np.argsort(diff) nn_dist = diff[nn_idx] nn_dist_idx_pre = np.linspace(0, nn_dist.shape[0], nn_dist.shape[0]) nn_dist_idx = nn_dist_idx_pre[nn_idx] brute_time_sum += time.time() - begin_t #brute knn search result_set_knn_brute = KNNResultSet(capacity=k) for index in range(k): result_set_knn_brute.add_point(nn_dist[index], nn_dist_idx[index]) # brute radiusNN search result_set_rnn_brute = RadiusNNResultSet(radius=radius) for index in range(nn_dist.shape[0]): if nn_dist[index] < radius: result_set_rnn_brute.add_point(nn_dist[index], nn_dist_idx[index]) continue else: break print("Octree: build %.3f, knn %.3f, radius %.3f, brute %.3f" % (construction_time_sum * 1000, knn_time_sum * 1000, radius_time_sum * 1000, brute_time_sum * 1000)) print("kdtree -----------------------------------") construction_time_sum = 0 knn_time_sum = 0 radius_time_sum = 0 brute_time_sum = 0 for i in range(iteration): query = db_np[95, :] begin_t = time.time() root = kdtree.kdtree_construction(db_np, leaf_size) construction_time_sum += time.time() - begin_t begin_t = time.time() result_set_knn = KNNResultSet(capacity=k) kdtree.kdtree_knn_search(root, db_np, result_set_knn, query) knn_time_sum += time.time() - begin_t begin_t = time.time() result_set_rnn = RadiusNNResultSet(radius=radius) kdtree.kdtree_radius_search(root, db_np, result_set_rnn, query) radius_time_sum += time.time() - begin_t begin_t = time.time() diff = np.linalg.norm(np.expand_dims(query, 0) - db_np, axis=1) nn_idx = np.argsort(diff) nn_dist = diff[nn_idx] brute_time_sum += time.time() - begin_t nn_dist_idx_pre = np.linspace(0, nn_dist.shape[0] - 1, nn_dist.shape[0]) nn_dist_idx = nn_dist_idx_pre[nn_idx] # brute knn search result_set_knn_brute = KNNResultSet(capacity=k) for index in range(k): result_set_knn_brute.add_point(nn_dist[index], nn_dist_idx[index]) # brute radiusNN search result_set_rnn_brute = RadiusNNResultSet(radius=radius) for index in range(nn_dist.shape[0]): if nn_dist[index] < radius: result_set_rnn_brute.add_point(nn_dist[index], nn_dist_idx[index]) continue else: break print("Kdtree: build %.3f, knn %.3f, radius %.3f, brute %.3f" % (construction_time_sum * 1000, knn_time_sum * 1000, radius_time_sum * 1000, brute_time_sum * 1000)) print("scipy kdtree -----------------------------------") construction_time_sum = 0 knn_time_sum = 0 radius_time_sum = 0 query = db_np[95, :] begin_t = time.time() tree = spatial.KDTree(db_np, leaf_size) construction_time_sum += time.time() - begin_t #no knn begin_t = time.time() knn_time_sum += time.time() - begin_t begin_t = time.time() result_set_rnn = tree.query_ball_point(query, radius) radius_time_sum += time.time() - begin_t print('rnn search result\n', result_set_rnn) print("Octree: build %.3f, knn %.3f, radius %.3f" % (construction_time_sum * 1000, knn_time_sum * 1000, radius_time_sum * 1000))